Sync Bullet to upstream

This syncs Bullet to the latest upstream git version as of writing this.
(commit 47b0259b9700455022b5cf79b651cc1dc71dd59e).

442 files changed, 76878 insertions, 53302 deletions

diff --git a/extern/bullet2/CMakeLists.txt b/extern/bullet2/CMakeLists.txt
index fd043bb9048..648442e1bd9 100644
--- a/extern/bullet2/CMakeLists.txt
+++ b/extern/bullet2/CMakeLists.txt
@@ -47,7 +47,6 @@ set(SRC
   src/BulletCollision/BroadphaseCollision/btDbvt.cpp
   src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
   src/BulletCollision/BroadphaseCollision/btDispatcher.cpp
-  src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
   src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
   src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
   src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
@@ -95,12 +94,14 @@ set(SRC
   src/BulletCollision/CollisionShapes/btCylinderShape.cpp
   src/BulletCollision/CollisionShapes/btEmptyShape.cpp
   src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
+  src/BulletCollision/CollisionShapes/btMiniSDF.cpp
   src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp
   src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
   src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp
   src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
   src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
   src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp
+  src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp
   src/BulletCollision/CollisionShapes/btShapeHull.cpp
   src/BulletCollision/CollisionShapes/btSphereShape.cpp
   src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp
@@ -154,7 +155,6 @@ set(SRC
   src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
   src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
   src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
-  src/BulletDynamics/Dynamics/Bullet-C-API.cpp
   src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
   src/BulletDynamics/Dynamics/btRigidBody.cpp
   src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
@@ -187,6 +187,7 @@ set(SRC
   src/LinearMath/btPolarDecomposition.cpp
   src/LinearMath/btQuickprof.cpp
   src/LinearMath/btSerializer.cpp
+  src/LinearMath/btSerializer64.cpp
   src/LinearMath/btVector3.cpp
 
   src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
@@ -196,7 +197,6 @@ set(SRC
   src/BulletCollision/BroadphaseCollision/btDbvt.h
   src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
   src/BulletCollision/BroadphaseCollision/btDispatcher.h
-  src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
   src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
   src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h
   src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h
@@ -250,12 +250,14 @@ set(SRC
   src/BulletCollision/CollisionShapes/btEmptyShape.h
   src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
   src/BulletCollision/CollisionShapes/btMaterial.h
+  src/BulletCollision/CollisionShapes/btMiniSDF.h
   src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h
   src/BulletCollision/CollisionShapes/btMultiSphereShape.h
   src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h
   src/BulletCollision/CollisionShapes/btOptimizedBvh.h
   src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
   src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h
+  src/BulletCollision/CollisionShapes/btSdfCollisionShape.h
   src/BulletCollision/CollisionShapes/btShapeHull.h
   src/BulletCollision/CollisionShapes/btSphereShape.h
   src/BulletCollision/CollisionShapes/btStaticPlaneShape.h
@@ -417,7 +419,6 @@ set(SRC
 
   src/btBulletCollisionCommon.h
   src/btBulletDynamicsCommon.h
-  src/Bullet-C-Api.h
 )
 
 set(LIB
diff --git a/extern/bullet2/patches/blender.patch b/extern/bullet2/patches/blender.patch
deleted file mode 100644
index b4b24fdc220..00000000000
--- a/extern/bullet2/patches/blender.patch
+++ /dev/null
@@ -1,368 +0,0 @@
-diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h
-index 839b19c..3058195 100644
---- a/extern/bullet2/src/LinearMath/btVector3.h
-+++ b/extern/bullet2/src/LinearMath/btVector3.h
-@@ -39,7 +39,7 @@ subject to the following restrictions:
- #endif
- 
- 
--#define BT_SHUFFLE(x,y,z,w) ((w)<<6 | (z)<<4 | (y)<<2 | (x))
-+#define BT_SHUFFLE(x,y,z,w) (((w) << 6 | (z) << 4 | (y) << 2 | (x)) & 0xff)
- //#define bt_pshufd_ps( _a, _mask ) (__m128) _mm_shuffle_epi32((__m128i)(_a), (_mask) )
- #define bt_pshufd_ps( _a, _mask ) _mm_shuffle_ps((_a), (_a), (_mask) )
- #define bt_splat3_ps( _a, _i ) bt_pshufd_ps((_a), BT_SHUFFLE(_i,_i,_i, 3) )
-diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h
---- a/extern/bullet2/src/LinearMath/btScalar.h
-+++ b/extern/bullet2/src/LinearMath/btScalar.h
-@@ -16,6 +16,9 @@
- 
- #ifndef BT_SCALAR_H
- #define BT_SCALAR_H
-+#if defined(_MSC_VER) && defined(__clang__) /* clang supplies it's own overloads already */
-+#define BT_NO_SIMD_OPERATOR_OVERLOADS
-+#endif
- 
- #ifdef BT_MANAGED_CODE
- //Aligned data types not supported in managed code
-@@ -83,7 +86,7 @@
- 			#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
-+			//#define BT_ALLOW_SSE4 //disable this cause blender targets sse2 
- #endif //(_MSC_FULL_VER >= 160040219)
- 
- 			//BT_USE_SSE_IN_API is disabled under Windows by default, because 
-@@ -102,7 +105,7 @@
- 		#endif //__MINGW32__
- 
- #ifdef BT_DEBUG
--	#ifdef _MSC_VER
-+	#if defined(_MSC_VER) && !defined(__clang__)
- 		#include <stdio.h>
- 		#define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);__debugbreak();	}}
- 	#else//_MSC_VER
-diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
-index be9eca6..ec40c96 100644
---- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
-+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
-@@ -15,7 +15,7 @@ subject to the following restrictions:
- 
- 
- /**
-- * @mainpage Bullet Documentation
-+ * @page Bullet Documentation
-  *
-  * @section intro_sec Introduction
-  * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ).
-diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp
-index 36dd043..57eb817 100644
---- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp
-+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp
-@@ -579,14 +579,10 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape(  btCollisionS
- 				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);
-diff --git a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp
-index 57fc119..31faf1d 100644
---- a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp
-+++ b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp
-@@ -29,14 +29,11 @@ subject to the following restrictions:
- static btVector3
- getNormalizedVector(const btVector3& v)
- {
--	btScalar l = v.length();
--	btVector3 n = v;
--	if (l < SIMD_EPSILON) {
--		n.setValue(0,0,0);
--	} else {
--		n /= l;
--	}
-+	btVector3 n(0, 0, 0);
- 
-+	if (v.length() > SIMD_EPSILON) {
-+		n = v.normalized();
-+	}
- 	return n;
- }
- 
-diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
-index 27ccefe..8e4456e 100644
---- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
-+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
-@@ -37,8 +37,13 @@ struct	btSimdScalar
- 	{
- 
- 	}
--
-+/* workaround for clang 3.4 ( == apple clang 5.1 ) issue, friction would fail with forced inlining */
-+#if (defined(__clang__) && defined(__apple_build_version__) &&  (__clang_major__ == 5) && (__clang_minor__ == 1)) \
-+|| (defined(__clang__) && !defined(__apple_build_version__) && (__clang_major__ == 3) && (__clang_minor__ == 4))
-+	inline __attribute__ ((noinline)) btSimdScalar(float	fl)
-+#else
- 	SIMD_FORCE_INLINE	btSimdScalar(float	fl)
-+#endif
- 	:m_vec128 (_mm_set1_ps(fl))
- 	{
- 	}
-diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp
-index 5d62da7..fcd312e 100644
---- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp
-+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp
-@@ -28,7 +28,6 @@
- #include "btMultiBodyJointFeedback.h"
- #include "LinearMath/btTransformUtil.h"
- #include "LinearMath/btSerializer.h"
--#include "Bullet3Common/b3Logging.h"
- // #define INCLUDE_GYRO_TERM 
- 
- ///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals
-@@ -1732,7 +1731,6 @@ void btMultiBody::goToSleep()
- 
- void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
- {
--	int num_links = getNumLinks();
- 	extern bool gDisableDeactivation;
-     if (!m_canSleep || gDisableDeactivation) 
- 	{
-diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
-index 8a034b3..4f66b20 100644
---- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
-+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
-@@ -809,7 +809,6 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS
- }
- #endif
- 
--#include "Bullet3Common/b3Logging.h"
- void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
- {
- #if 1 
-diff --git a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h
-index bcf0c79..8992ddb 100644
---- a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h
-+++ b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h
-@@ -185,7 +185,6 @@ struct	btSparseSdf
- 		{
- 			++nprobes;		
- 			++ncells;
--			int sz = sizeof(Cell);
- 			if (ncells>m_clampCells)
- 			{
- 				static int numResets=0;
-diff --git a/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp b/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp
-index d58ac95..3fd77df 100644
---- a/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp
-+++ b/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp
-@@ -2665,6 +2665,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in
- 	}
- 
- 	vertices.resize(0);
-+	original_vertex_index.resize(0);
- 	edges.resize(0);
- 	faces.resize(0);
- 
-@@ -2675,6 +2676,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in
- 	{
- 		btConvexHullInternal::Vertex* v = oldVertices[copied];
- 		vertices.push_back(hull.getCoordinates(v));
-+		original_vertex_index.push_back(v->point.index);
- 		btConvexHullInternal::Edge* firstEdge = v->edges;
- 		if (firstEdge)
- 		{
-diff --git a/extern/bullet2/src/LinearMath/btConvexHullComputer.h b/extern/bullet2/src/LinearMath/btConvexHullComputer.h
-index 7240ac4..6871ce8 100644
---- a/extern/bullet2/src/LinearMath/btConvexHullComputer.h
-+++ b/extern/bullet2/src/LinearMath/btConvexHullComputer.h
-@@ -67,6 +67,7 @@ class btConvexHullComputer
- 
- 		// Vertices of the output hull
- 		btAlignedObjectArray<btVector3> vertices;
-+		btAlignedObjectArray<int> original_vertex_index;
- 
- 		// Edges of the output hull
- 		btAlignedObjectArray<Edge> edges;
-diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
-index 0623e35..02ea503 100644
---- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
-+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
-@@ -13,9 +13,9 @@ subject to the following restrictions:
- 3. This notice may not be removed or altered from any source distribution.
- */
- 
--#if defined (_WIN32) || defined (__i386__)
--#define BT_USE_SSE_IN_API
--#endif
-+//#if defined (_WIN32) || defined (__i386__)
-+//#define BT_USE_SSE_IN_API
-+//#endif
- 
- #include "btConvexHullShape.h"
- #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
-diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp
-index b56d729..88018b4 100644
---- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp
-+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp
-@@ -13,9 +13,9 @@ subject to the following restrictions:
- 3. This notice may not be removed or altered from any source distribution.
- */
- 
--#if defined (_WIN32) || defined (__i386__)
--#define BT_USE_SSE_IN_API
--#endif
-+//#if defined (_WIN32) || defined (__i386__)
-+//#define BT_USE_SSE_IN_API
-+//#endif
- 
- #include "btConvexShape.h"
- #include "btTriangleShape.h"
-diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
-index a7362ea..6abfdff 100644
---- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
-+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
-@@ -13,9 +13,9 @@ subject to the following restrictions:
- 3. This notice may not be removed or altered from any source distribution.
- */
- 
--#if defined (_WIN32) || defined (__i386__)
--#define BT_USE_SSE_IN_API
--#endif
-+//#if defined (_WIN32) || defined (__i386__)
-+//#define BT_USE_SSE_IN_API
-+//#endif
- 
- #include "btMultiSphereShape.h"
- #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
-diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
-index 4854f37..9095c59 100644
---- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
-+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
-@@ -12,9 +12,9 @@ subject to the following restrictions:
- 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.
- */
--#if defined (_WIN32) || defined (__i386__)
--#define BT_USE_SSE_IN_API
--#endif
-+//#if defined (_WIN32) || defined (__i386__)
-+//#define BT_USE_SSE_IN_API
-+//#endif
- 
- #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
- #include "btConvexPolyhedron.h"
-diff --git a/extern/bullet2/src/LinearMath/btVector3.cpp b/extern/bullet2/src/LinearMath/btVector3.cpp
-index e05bdcc..dbcf2b6 100644
---- a/extern/bullet2/src/LinearMath/btVector3.cpp
-+++ b/extern/bullet2/src/LinearMath/btVector3.cpp
-@@ -15,9 +15,9 @@
-  This source version has been altered.
-  */
- 
--#if defined (_WIN32) || defined (__i386__)
--#define BT_USE_SSE_IN_API
--#endif
-+//#if defined (_WIN32) || defined (__i386__)
-+//#define BT_USE_SSE_IN_API
-+//#endif
- 
- 
- #include "btVector3.h"
-diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
-index e0e8bc7..a788268 100644
---- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
-+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
-@@ -425,50 +425,38 @@ 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);
--		btCollisionObject* colObjA = &c->getRigidBodyA();
--		btCollisionObject* colObjB = &c->getRigidBodyB();
--		if (colObjA == this)
--		{
--			colObjA->setIgnoreCollisionCheck(colObjB, true);
--		}
--		else
--		{
--			colObjB->setIgnoreCollisionCheck(colObjA, true);
--		}
--	} 
-+		m_constraintRefs.push_back(c); 
-+
-+	m_checkCollideWith = true;
- }
- 
- void btRigidBody::removeConstraintRef(btTypedConstraint* c)
- {
--	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);
--        }
--    }
-+	m_constraintRefs.remove(c);
-+	m_checkCollideWith = m_constraintRefs.size() > 0;
- }
- 
- int	btRigidBody::calculateSerializeBufferSize()	const
-diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
-index 1d177db..c2f8c5d 100644
---- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
-+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
-@@ -509,6 +509,8 @@ public:
- 		return (getBroadphaseProxy() != 0);
- 	}
- 
-+	virtual bool checkCollideWithOverride(const  btCollisionObject* co) const;
-+
- 	void addConstraintRef(btTypedConstraint* c);
- 	void removeConstraintRef(btTypedConstraint* c);
- 
diff --git a/extern/bullet2/patches/btPolyhedralConvexShape_Inertia_fix.patch b/extern/bullet2/patches/btPolyhedralConvexShape_Inertia_fix.patch
deleted file mode 100644
index abafb5855dd..00000000000
--- a/extern/bullet2/patches/btPolyhedralConvexShape_Inertia_fix.patch
+++ /dev/null
@@ -1,41 +0,0 @@
-diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
-index 9095c592d87..b831e20c2f9 100644
---- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
-+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
-@@ -406,17 +406,17 @@ void	btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& ine
- #ifndef __SPU__
- 	//not yet, return box inertia
- 
--	btScalar margin = getMargin();
-+	//btScalar margin = getMargin();
- 
- 	btTransform ident;
- 	ident.setIdentity();
- 	btVector3 aabbMin,aabbMax;
--	getAabb(ident,aabbMin,aabbMax);
-+	getAabb(ident,aabbMin,aabbMax); // This already contains the margin
- 	btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
- 
--	btScalar lx=btScalar(2.)*(halfExtents.x()+margin);
--	btScalar ly=btScalar(2.)*(halfExtents.y()+margin);
--	btScalar lz=btScalar(2.)*(halfExtents.z()+margin);
-+	btScalar lx=btScalar(2.)*(halfExtents.x());
-+	btScalar ly=btScalar(2.)*(halfExtents.y());
-+	btScalar lz=btScalar(2.)*(halfExtents.z());
- 	const btScalar x2 = lx*lx;
- 	const btScalar y2 = ly*ly;
- 	const btScalar z2 = lz*lz;
-@@ -476,10 +476,10 @@ void	btPolyhedralConvexAabbCachingShape::recalcLocalAabb()
- 	
- 	for ( int i = 0; i < 3; ++i )
- 	{
--		m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin;
--		m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin;
-+		m_localAabbMax[i] = _supporting[i][i];
-+		m_localAabbMin[i] = _supporting[i + 3][i];
- 	}
--	
-+
- 	#else
- 
- 	for (int i=0;i<3;i++)
diff --git a/extern/bullet2/patches/inertia.patch b/extern/bullet2/patches/inertia.patch
new file mode 100644
index 00000000000..731772dd220
--- /dev/null
+++ b/extern/bullet2/patches/inertia.patch
@@ -0,0 +1,113 @@
+From 1b4c1687748bafd3c521f454bfdfc89b3857b65e Mon Sep 17 00:00:00 2001
+From: David Vogel <Dadido3@aol.com>
+Date: Mon, 30 Mar 2020 19:45:23 +0200
+Subject: [PATCH 1/2] Fix inertia and margin calculation for
+ btPolyhedralConvexShape
+
+---
+ .../CollisionShapes/btPolyhedralConvexShape.cpp    | 14 +++++++-------
+ 1 file changed, 7 insertions(+), 7 deletions(-)
+
+diff --git a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
+index 521ecfc760..e4bd7bb4d5 100644
+--- a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
++++ b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
+@@ -463,17 +463,17 @@ void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass, btVector3& in
+ #ifndef __SPU__
+ 	//not yet, return box inertia
+ 
+-	btScalar margin = getMargin();
++	//btScalar margin = getMargin();
+ 
+ 	btTransform ident;
+ 	ident.setIdentity();
+ 	btVector3 aabbMin, aabbMax;
+-	getAabb(ident, aabbMin, aabbMax);
++	getAabb(ident, aabbMin, aabbMax);  // This already contains the margin
+ 	btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
+ 
+-	btScalar lx = btScalar(2.) * (halfExtents.x() + margin);
+-	btScalar ly = btScalar(2.) * (halfExtents.y() + margin);
+-	btScalar lz = btScalar(2.) * (halfExtents.z() + margin);
++	btScalar lx = btScalar(2.) * (halfExtents.x());
++	btScalar ly = btScalar(2.) * (halfExtents.y());
++	btScalar lz = btScalar(2.) * (halfExtents.z());
+ 	const btScalar x2 = lx * lx;
+ 	const btScalar y2 = ly * ly;
+ 	const btScalar z2 = lz * lz;
+@@ -529,8 +529,8 @@ void btPolyhedralConvexAabbCachingShape::recalcLocalAabb()
+ 
+ 	for (int i = 0; i < 3; ++i)
+ 	{
+-		m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin;
+-		m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin;
++		m_localAabbMax[i] = _supporting[i][i];
++		m_localAabbMin[i] = _supporting[i + 3][i];
+ 	}
+ 
+ #else
+
+From 4b9a201d4c1b8cacbcdd68f9cdb55745caa6adc4 Mon Sep 17 00:00:00 2001
+From: David Vogel <Dadido3@aol.com>
+Date: Mon, 30 Mar 2020 20:43:55 +0200
+Subject: [PATCH 2/2]  Fix margins
+
+- Margin in ineratia calculation of btConeShape is already contained in the AABB
+- Remove margin from the cached AABB in btConvexInternalShape, as it is added on getAabb()
+---
+ src/BulletCollision/CollisionShapes/btConeShape.h      | 10 ++++------
+ .../CollisionShapes/btConvexInternalShape.cpp          |  8 ++++----
+ 2 files changed, 8 insertions(+), 10 deletions(-)
+
+diff --git a/src/BulletCollision/CollisionShapes/btConeShape.h b/src/BulletCollision/CollisionShapes/btConeShape.h
+index 49f26bc4e5..ee6786c807 100644
+--- a/src/BulletCollision/CollisionShapes/btConeShape.h
++++ b/src/BulletCollision/CollisionShapes/btConeShape.h
+@@ -56,15 +56,13 @@ btConeShape : public btConvexInternalShape
+ 		btTransform identity;
+ 		identity.setIdentity();
+ 		btVector3 aabbMin, aabbMax;
+-		getAabb(identity, aabbMin, aabbMax);
+ 
++		getAabb(identity, aabbMin, aabbMax);  // This already contains the margin
+ 		btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
+ 
+-		btScalar margin = getMargin();
+-
+-		btScalar lx = btScalar(2.) * (halfExtents.x() + margin);
+-		btScalar ly = btScalar(2.) * (halfExtents.y() + margin);
+-		btScalar lz = btScalar(2.) * (halfExtents.z() + margin);
++		btScalar lx = btScalar(2.) * (halfExtents.x());
++		btScalar ly = btScalar(2.) * (halfExtents.y());
++		btScalar lz = btScalar(2.) * (halfExtents.z());
+ 		const btScalar x2 = lx * lx;
+ 		const btScalar y2 = ly * ly;
+ 		const btScalar z2 = lz * lz;
+diff --git a/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp b/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp
+index 4d598b1aa2..b847f8f40f 100644
+--- a/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp
++++ b/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp
+@@ -117,8 +117,8 @@ void btConvexInternalAabbCachingShape::recalcLocalAabb()
+ 
+ 	for (int i = 0; i < 3; ++i)
+ 	{
+-		m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin;
+-		m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin;
++		m_localAabbMax[i] = _supporting[i][i];
++		m_localAabbMin[i] = _supporting[i + 3][i];
+ 	}
+ 
+ #else
+@@ -128,10 +128,10 @@ void btConvexInternalAabbCachingShape::recalcLocalAabb()
+ 		btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.));
+ 		vec[i] = btScalar(1.);
+ 		btVector3 tmp = localGetSupportingVertex(vec);
+-		m_localAabbMax[i] = tmp[i] + m_collisionMargin;
++		m_localAabbMax[i] = tmp[i];
+ 		vec[i] = btScalar(-1.);
+ 		tmp = localGetSupportingVertex(vec);
+-		m_localAabbMin[i] = tmp[i] - m_collisionMargin;
++		m_localAabbMin[i] = tmp[i];
+ 	}
+ #endif
+ }
diff --git a/extern/bullet2/src/Bullet-C-Api.h b/extern/bullet2/src/Bullet-C-Api.h
deleted file mode 100644
index 5d00f7e3ac3..00000000000
--- a/extern/bullet2/src/Bullet-C-Api.h
+++ /dev/null
@@ -1,187 +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]);
-
-
-	/* Convex Hull */
-	PL_DECLARE_HANDLE(plConvexHull);
-	plConvexHull plConvexHullCompute(float (*coords)[3], int count);
-	void plConvexHullDelete(plConvexHull hull);
-	int plConvexHullNumVertices(plConvexHull hull);
-	int plConvexHullNumFaces(plConvexHull hull);
-	void plConvexHullGetVertex(plConvexHull hull, int n, float coords[3], int *original_index);
-	int plConvexHullGetFaceSize(plConvexHull hull, int n);
-	void plConvexHullGetFaceVertices(plConvexHull hull, int n, int *vertices);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif //BULLET_C_API_H
-
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
index 77763305b1b..ec6fe9f4d8a 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
@@ -2,7 +2,6 @@
 //Bullet Continuous Collision Detection and Physics Library
 //Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
-
 //
 // btAxisSweep3
 //
@@ -19,18 +18,15 @@
 // 3. This notice may not be removed or altered from any source distribution.
 #include "btAxisSweep3.h"
 
-
-btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
-:btAxisSweep3Internal<unsigned short int>(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache,disableRaycastAccelerator)
+btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin, const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
+	: btAxisSweep3Internal<unsigned short int>(worldAabbMin, worldAabbMax, 0xfffe, 0xffff, maxHandles, pairCache, disableRaycastAccelerator)
 {
 	// 1 handle is reserved as sentinel
 	btAssert(maxHandles > 1 && maxHandles < 32767);
-
 }
 
-
-bt32BitAxisSweep3::bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache , bool disableRaycastAccelerator)
-:btAxisSweep3Internal<unsigned int>(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache,disableRaycastAccelerator)
+bt32BitAxisSweep3::bt32BitAxisSweep3(const btVector3& worldAabbMin, const btVector3& worldAabbMax, unsigned int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
+	: btAxisSweep3Internal<unsigned int>(worldAabbMin, worldAabbMax, 0xfffffffe, 0x7fffffff, maxHandles, pairCache, disableRaycastAccelerator)
 {
 	// 1 handle is reserved as sentinel
 	btAssert(maxHandles > 1 && maxHandles < 2147483647);
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
index cd6e1a8929e..1e42f25f3bd 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
@@ -25,1005 +25,7 @@
 #include "btBroadphaseProxy.h"
 #include "btOverlappingPairCallback.h"
 #include "btDbvtBroadphase.h"
-
-//#define DEBUG_BROADPHASE 1
-#define USE_OVERLAP_TEST_ON_REMOVES 1
-
-/// The internal templace class btAxisSweep3Internal implements the sweep and prune broadphase.
-/// It uses quantized integers to represent the begin and end points for each of the 3 axis.
-/// Dont use this class directly, use btAxisSweep3 or bt32BitAxisSweep3 instead.
-template <typename BP_FP_INT_TYPE>
-class btAxisSweep3Internal : public btBroadphaseInterface
-{
-protected:
-
-	BP_FP_INT_TYPE	m_bpHandleMask;
-	BP_FP_INT_TYPE	m_handleSentinel;
-
-public:
-	
- BT_DECLARE_ALIGNED_ALLOCATOR();
-
-	class Edge
-	{
-	public:
-		BP_FP_INT_TYPE m_pos;			// low bit is min/max
-		BP_FP_INT_TYPE m_handle;
-
-		BP_FP_INT_TYPE IsMax() const {return static_cast<BP_FP_INT_TYPE>(m_pos & 1);}
-	};
-
-public:
-	class	Handle : public btBroadphaseProxy
-	{
-	public:
-	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-		// indexes into the edge arrays
-		BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3];		// 6 * 2 = 12
-//		BP_FP_INT_TYPE m_uniqueId;
-		btBroadphaseProxy*	m_dbvtProxy;//for faster raycast
-		//void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
-	
-		SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;}
-		SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];}
-	};		// 24 bytes + 24 for Edge structures = 44 bytes total per entry
-
-	
-protected:
-	btVector3 m_worldAabbMin;						// overall system bounds
-	btVector3 m_worldAabbMax;						// overall system bounds
-
-	btVector3 m_quantize;						// scaling factor for quantization
-
-	BP_FP_INT_TYPE m_numHandles;						// number of active handles
-	BP_FP_INT_TYPE m_maxHandles;						// max number of handles
-	Handle* m_pHandles;						// handles pool
-	
-	BP_FP_INT_TYPE m_firstFreeHandle;		// free handles list
-
-	Edge* m_pEdges[3];						// edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries)
-	void* m_pEdgesRawPtr[3];
-
-	btOverlappingPairCache* m_pairCache;
-
-	///btOverlappingPairCallback is an additional optional user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache.
-	btOverlappingPairCallback* m_userPairCallback;
-	
-	bool	m_ownsPairCache;
-
-	int	m_invalidPair;
-
-	///additional dynamic aabb structure, used to accelerate ray cast queries.
-	///can be disabled using a optional argument in the constructor
-	btDbvtBroadphase*	m_raycastAccelerator;
-	btOverlappingPairCache*	m_nullPairCache;
-
-
-	// allocation/deallocation
-	BP_FP_INT_TYPE allocHandle();
-	void freeHandle(BP_FP_INT_TYPE handle);
-	
-
-	bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1);
-
-#ifdef DEBUG_BROADPHASE
-	void debugPrintAxis(int axis,bool checkCardinality=true);
-#endif //DEBUG_BROADPHASE
-
-	//Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
-	//void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
-
-	
-
-	void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
-	void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
-	void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
-	void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
-
-public:
-
-	btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0,bool disableRaycastAccelerator = false);
-
-	virtual	~btAxisSweep3Internal();
-
-	BP_FP_INT_TYPE getNumHandles() const
-	{
-		return m_numHandles;
-	}
-
-	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
-	
-	BP_FP_INT_TYPE addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
-	void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
-	void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
-	SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
-
-	virtual void resetPool(btDispatcher* dispatcher);
-
-	void	processAllOverlappingPairs(btOverlapCallback* callback);
-
-	//Broadphase Interface
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
-	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
-	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
-	virtual void  getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
-	
-	virtual void	rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0));
-	virtual void	aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
-
-	
-	void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const;
-	///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result
-	void unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
-	
-	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
-
-	btOverlappingPairCache*	getOverlappingPairCache()
-	{
-		return m_pairCache;
-	}
-	const btOverlappingPairCache*	getOverlappingPairCache() const
-	{
-		return m_pairCache;
-	}
-
-	void	setOverlappingPairUserCallback(btOverlappingPairCallback* pairCallback)
-	{
-		m_userPairCallback = pairCallback;
-	}
-	const btOverlappingPairCallback*	getOverlappingPairUserCallback() const
-	{
-		return m_userPairCallback;
-	}
-
-	///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
-	///will add some transform later
-	virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
-	{
-		aabbMin = m_worldAabbMin;
-		aabbMax = m_worldAabbMax;
-	}
-
-	virtual void	printStats()
-	{
-/*		printf("btAxisSweep3.h\n");
-		printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
-		printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(),
-			m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ());
-			*/
-
-	}
-
-};
-
-////////////////////////////////////////////////////////////////////
-
-
-
-
-#ifdef DEBUG_BROADPHASE
-#include <stdio.h>
-
-template <typename BP_FP_INT_TYPE>
-void btAxisSweep3<BP_FP_INT_TYPE>::debugPrintAxis(int axis, bool checkCardinality)
-{
-	int numEdges = m_pHandles[0].m_maxEdges[axis];
-	printf("SAP Axis %d, numEdges=%d\n",axis,numEdges);
-
-	int i;
-	for (i=0;i<numEdges+1;i++)
-	{
-		Edge* pEdge = m_pEdges[axis] + i;
-		Handle* pHandlePrev = getHandle(pEdge->m_handle);
-		int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis];
-		char beginOrEnd;
-		beginOrEnd=pEdge->IsMax()?'E':'B';
-		printf("	[%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex);
-	}
-
-	if (checkCardinality)
-		btAssert(numEdges == m_numHandles*2+1);
-}
-#endif //DEBUG_BROADPHASE
-
-template <typename BP_FP_INT_TYPE>
-btBroadphaseProxy*	btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy)
-{
-		(void)shapeType;
-		BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,multiSapProxy);
-		
-		Handle* handle = getHandle(handleId);
-		
-		if (m_raycastAccelerator)
-		{
-			btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,0);
-			handle->m_dbvtProxy = rayProxy;
-		}
-		return handle;
-}
-
-
-
-template <typename BP_FP_INT_TYPE>
-void	btAxisSweep3Internal<BP_FP_INT_TYPE>::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
-{
-	Handle* handle = static_cast<Handle*>(proxy);
-	if (m_raycastAccelerator)
-		m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher);
-	removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
-}
-
-template <typename BP_FP_INT_TYPE>
-void	btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
-{
-	Handle* handle = static_cast<Handle*>(proxy);
-	handle->m_aabbMin = aabbMin;
-	handle->m_aabbMax = aabbMax;
-	updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax,dispatcher);
-	if (m_raycastAccelerator)
-		m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher);
-
-}
-
-template <typename BP_FP_INT_TYPE>
-void	btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
-{
-	if (m_raycastAccelerator)
-	{
-		m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback,aabbMin,aabbMax);
-	} else
-	{
-		//choose axis?
-		BP_FP_INT_TYPE axis = 0;
-		//for each proxy
-		for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++)
-		{
-			if (m_pEdges[axis][i].IsMax())
-			{
-				rayCallback.process(getHandle(m_pEdges[axis][i].m_handle));
-			}
-		}
-	}
-}
-
-template <typename BP_FP_INT_TYPE>
-void	btAxisSweep3Internal<BP_FP_INT_TYPE>::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
-{
-	if (m_raycastAccelerator)
-	{
-		m_raycastAccelerator->aabbTest(aabbMin,aabbMax,callback);
-	} else
-	{
-		//choose axis?
-		BP_FP_INT_TYPE axis = 0;
-		//for each proxy
-		for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++)
-		{
-			if (m_pEdges[axis][i].IsMax())
-			{
-				Handle* handle = getHandle(m_pEdges[axis][i].m_handle);
-				if (TestAabbAgainstAabb2(aabbMin,aabbMax,handle->m_aabbMin,handle->m_aabbMax))
-				{
-					callback.process(handle);
-				}
-			}
-		}
-	}
-}
-
-
-
-template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
-{
-	Handle* pHandle = static_cast<Handle*>(proxy);
-	aabbMin = pHandle->m_aabbMin;
-	aabbMax = pHandle->m_aabbMax;
-}
-
-
-template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
-{
-	Handle* pHandle = static_cast<Handle*>(proxy);
-
-	unsigned short vecInMin[3];
-	unsigned short vecInMax[3];
-
-	vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos ;
-	vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos +1 ;
-	vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos ;
-	vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos +1 ;
-	vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos ;
-	vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos +1 ;
-	
-	aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()),(btScalar)(vecInMin[1]) / (m_quantize.getY()),(btScalar)(vecInMin[2]) / (m_quantize.getZ()));
-	aabbMin += m_worldAabbMin;
-	
-	aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()),(btScalar)(vecInMax[1]) / (m_quantize.getY()),(btScalar)(vecInMax[2]) / (m_quantize.getZ()));
-	aabbMax += m_worldAabbMin;
-}
-
-
-
-
-template <typename BP_FP_INT_TYPE>
-btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache , bool disableRaycastAccelerator)
-:m_bpHandleMask(handleMask),
-m_handleSentinel(handleSentinel),
-m_pairCache(pairCache),
-m_userPairCallback(0),
-m_ownsPairCache(false),
-m_invalidPair(0),
-m_raycastAccelerator(0)
-{
-	BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles+1);//need to add one sentinel handle
-
-	if (!m_pairCache)
-	{
-		void* ptr = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16);
-		m_pairCache = new(ptr) btHashedOverlappingPairCache();
-		m_ownsPairCache = true;
-	}
-
-	if (!disableRaycastAccelerator)
-	{
-		m_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache),16)) btNullPairCache();
-		m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase),16)) btDbvtBroadphase(m_nullPairCache);//m_pairCache);
-		m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs
-	}
-
-	//btAssert(bounds.HasVolume());
-
-	// init bounds
-	m_worldAabbMin = worldAabbMin;
-	m_worldAabbMax = worldAabbMax;
-
-	btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin;
-
-	BP_FP_INT_TYPE	maxInt = m_handleSentinel;
-
-	m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize;
-
-	// allocate handles buffer, using btAlignedAlloc, and put all handles on free list
-	m_pHandles = new Handle[maxHandles];
-	
-	m_maxHandles = maxHandles;
-	m_numHandles = 0;
-
-	// handle 0 is reserved as the null index, and is also used as the sentinel
-	m_firstFreeHandle = 1;
-	{
-		for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++)
-			m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
-		m_pHandles[maxHandles - 1].SetNextFree(0);
-	}
-
-	{
-		// allocate edge buffers
-		for (int i = 0; i < 3; i++)
-		{
-			m_pEdgesRawPtr[i] = btAlignedAlloc(sizeof(Edge)*maxHandles*2,16);
-			m_pEdges[i] = new(m_pEdgesRawPtr[i]) Edge[maxHandles * 2];
-		}
-	}
-	//removed overlap management
-
-	// make boundary sentinels
-	
-	m_pHandles[0].m_clientObject = 0;
-
-	for (int axis = 0; axis < 3; axis++)
-	{
-		m_pHandles[0].m_minEdges[axis] = 0;
-		m_pHandles[0].m_maxEdges[axis] = 1;
-
-		m_pEdges[axis][0].m_pos = 0;
-		m_pEdges[axis][0].m_handle = 0;
-		m_pEdges[axis][1].m_pos = m_handleSentinel;
-		m_pEdges[axis][1].m_handle = 0;
-#ifdef DEBUG_BROADPHASE
-		debugPrintAxis(axis);
-#endif //DEBUG_BROADPHASE
-
-	}
-
-}
-
-template <typename BP_FP_INT_TYPE>
-btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal()
-{
-	if (m_raycastAccelerator)
-	{
-		m_nullPairCache->~btOverlappingPairCache();
-		btAlignedFree(m_nullPairCache);
-		m_raycastAccelerator->~btDbvtBroadphase();
-		btAlignedFree (m_raycastAccelerator);
-	}
-
-	for (int i = 2; i >= 0; i--)
-	{
-		btAlignedFree(m_pEdgesRawPtr[i]);
-	}
-	delete [] m_pHandles;
-
-	if (m_ownsPairCache)
-	{
-		m_pairCache->~btOverlappingPairCache();
-		btAlignedFree(m_pairCache);
-	}
-}
-
-template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const
-{
-#ifdef OLD_CLAMPING_METHOD
-	///problem with this clamping method is that the floating point during quantization might still go outside the range [(0|isMax) .. (m_handleSentinel&m_bpHandleMask]|isMax]
-	///see http://code.google.com/p/bullet/issues/detail?id=87
-	btVector3 clampedPoint(point);
-	clampedPoint.setMax(m_worldAabbMin);
-	clampedPoint.setMin(m_worldAabbMax);
-	btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
-	out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & m_bpHandleMask) | isMax);
-	out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & m_bpHandleMask) | isMax);
-	out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax);
-#else
-	btVector3 v = (point - m_worldAabbMin) * m_quantize;
-	out[0]=(v[0]<=0)?(BP_FP_INT_TYPE)isMax:(v[0]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0]&m_bpHandleMask)|isMax);
-	out[1]=(v[1]<=0)?(BP_FP_INT_TYPE)isMax:(v[1]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1]&m_bpHandleMask)|isMax);
-	out[2]=(v[2]<=0)?(BP_FP_INT_TYPE)isMax:(v[2]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2]&m_bpHandleMask)|isMax);
-#endif //OLD_CLAMPING_METHOD
-}
-
-
-template <typename BP_FP_INT_TYPE>
-BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle()
-{
-	btAssert(m_firstFreeHandle);
-
-	BP_FP_INT_TYPE handle = m_firstFreeHandle;
-	m_firstFreeHandle = getHandle(handle)->GetNextFree();
-	m_numHandles++;
-
-	return handle;
-}
-
-template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
-{
-	btAssert(handle > 0 && handle < m_maxHandles);
-
-	getHandle(handle)->SetNextFree(m_firstFreeHandle);
-	m_firstFreeHandle = handle;
-
-	m_numHandles--;
-}
-
-
-template <typename BP_FP_INT_TYPE>
-BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy)
-{
-	// quantize the bounds
-	BP_FP_INT_TYPE min[3], max[3];
-	quantize(min, aabbMin, 0);
-	quantize(max, aabbMax, 1);
-
-	// allocate a handle
-	BP_FP_INT_TYPE handle = allocHandle();
-	
-
-	Handle* pHandle = getHandle(handle);
-	
-	pHandle->m_uniqueId = static_cast<int>(handle);
-	//pHandle->m_pOverlaps = 0;
-	pHandle->m_clientObject = pOwner;
-	pHandle->m_collisionFilterGroup = collisionFilterGroup;
-	pHandle->m_collisionFilterMask = collisionFilterMask;
-	pHandle->m_multiSapParentProxy = multiSapProxy;
-
-	// compute current limit of edge arrays
-	BP_FP_INT_TYPE limit = static_cast<BP_FP_INT_TYPE>(m_numHandles * 2);
-
-	
-	// insert new edges just inside the max boundary edge
-	for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++)
-	{
-
-		m_pHandles[0].m_maxEdges[axis] += 2;
-
-		m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1];
-
-		m_pEdges[axis][limit - 1].m_pos = min[axis];
-		m_pEdges[axis][limit - 1].m_handle = handle;
-
-		m_pEdges[axis][limit].m_pos = max[axis];
-		m_pEdges[axis][limit].m_handle = handle;
-
-		pHandle->m_minEdges[axis] = static_cast<BP_FP_INT_TYPE>(limit - 1);
-		pHandle->m_maxEdges[axis] = limit;
-	}
-
-	// now sort the new edges to their correct position
-	sortMinDown(0, pHandle->m_minEdges[0], dispatcher,false);
-	sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher,false);
-	sortMinDown(1, pHandle->m_minEdges[1], dispatcher,false);
-	sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher,false);
-	sortMinDown(2, pHandle->m_minEdges[2], dispatcher,true);
-	sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher,true);
-
-
-	return handle;
-}
-
-
-template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher)
-{
-
-	Handle* pHandle = getHandle(handle);
-
-	//explicitly remove the pairs containing the proxy
-	//we could do it also in the sortMinUp (passing true)
-	///@todo: compare performance
-	if (!m_pairCache->hasDeferredRemoval())
-	{
-		m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher);
-	}
-
-	// compute current limit of edge arrays
-	int limit = static_cast<int>(m_numHandles * 2);
-	
-	int axis;
-
-	for (axis = 0;axis<3;axis++)
-	{
-		m_pHandles[0].m_maxEdges[axis] -= 2;
-	}
-
-	// remove the edges by sorting them up to the end of the list
-	for ( axis = 0; axis < 3; axis++)
-	{
-		Edge* pEdges = m_pEdges[axis];
-		BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
-		pEdges[max].m_pos = m_handleSentinel;
-
-		sortMaxUp(axis,max,dispatcher,false);
-
-
-		BP_FP_INT_TYPE i = pHandle->m_minEdges[axis];
-		pEdges[i].m_pos = m_handleSentinel;
-
-
-		sortMinUp(axis,i,dispatcher,false);
-
-		pEdges[limit-1].m_handle = 0;
-		pEdges[limit-1].m_pos = m_handleSentinel;
-		
-#ifdef DEBUG_BROADPHASE
-			debugPrintAxis(axis,false);
-#endif //DEBUG_BROADPHASE
-
-
-	}
-
-
-	// free the handle
-	freeHandle(handle);
-
-	
-}
-
-template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool(btDispatcher* /*dispatcher*/)
-{
-	if (m_numHandles == 0)
-	{
-		m_firstFreeHandle = 1;
-		{
-			for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++)
-				m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
-			m_pHandles[m_maxHandles - 1].SetNextFree(0);
-		}
-	}
-}       
-
-
-extern int gOverlappingPairs;
-//#include <stdio.h>
-
-template <typename BP_FP_INT_TYPE>
-void	btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatcher* dispatcher)
-{
-
-	if (m_pairCache->hasDeferredRemoval())
-	{
-	
-		btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();
-
-		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
-		overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
-
-		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
-		m_invalidPair = 0;
-
-		
-		int i;
-
-		btBroadphasePair previousPair;
-		previousPair.m_pProxy0 = 0;
-		previousPair.m_pProxy1 = 0;
-		previousPair.m_algorithm = 0;
-		
-		
-		for (i=0;i<overlappingPairArray.size();i++)
-		{
-		
-			btBroadphasePair& pair = overlappingPairArray[i];
-
-			bool isDuplicate = (pair == previousPair);
-
-			previousPair = pair;
-
-			bool needsRemoval = false;
-
-			if (!isDuplicate)
-			{
-				///important to use an AABB test that is consistent with the broadphase
-				bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
-
-				if (hasOverlap)
-				{
-					needsRemoval = false;//callback->processOverlap(pair);
-				} else
-				{
-					needsRemoval = true;
-				}
-			} else
-			{
-				//remove duplicate
-				needsRemoval = true;
-				//should have no algorithm
-				btAssert(!pair.m_algorithm);
-			}
-			
-			if (needsRemoval)
-			{
-				m_pairCache->cleanOverlappingPair(pair,dispatcher);
-
-		//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
-		//		m_overlappingPairArray.pop_back();
-				pair.m_pProxy0 = 0;
-				pair.m_pProxy1 = 0;
-				m_invalidPair++;
-				gOverlappingPairs--;
-			} 
-			
-		}
-
-	///if you don't like to skip the invalid pairs in the array, execute following code:
-	#define CLEAN_INVALID_PAIRS 1
-	#ifdef CLEAN_INVALID_PAIRS
-
-		//perform a sort, to sort 'invalid' pairs to the end
-		overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
-
-		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
-		m_invalidPair = 0;
-	#endif//CLEAN_INVALID_PAIRS
-		
-		//printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
-	}
-
-}
-
-
-template <typename BP_FP_INT_TYPE>
-bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
-{
-	const Handle* pHandleA = static_cast<Handle*>(proxy0);
-	const Handle* pHandleB = static_cast<Handle*>(proxy1);
-	
-	//optimization 1: check the array index (memory address), instead of the m_pos
-
-	for (int axis = 0; axis < 3; axis++)
-	{ 
-		if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || 
-			pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) 
-		{ 
-			return false; 
-		} 
-	} 
-	return true;
-}
-
-template <typename BP_FP_INT_TYPE>
-bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1)
-{
-	//optimization 1: check the array index (memory address), instead of the m_pos
-
-	if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] || 
-		pHandleB->m_maxEdges[axis0] < pHandleA->m_minEdges[axis0] ||
-		pHandleA->m_maxEdges[axis1] < pHandleB->m_minEdges[axis1] ||
-		pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1]) 
-	{ 
-		return false; 
-	} 
-	return true;
-}
-
-template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
-{
-//	btAssert(bounds.IsFinite());
-	//btAssert(bounds.HasVolume());
-
-	Handle* pHandle = getHandle(handle);
-
-	// quantize the new bounds
-	BP_FP_INT_TYPE min[3], max[3];
-	quantize(min, aabbMin, 0);
-	quantize(max, aabbMax, 1);
-
-	// update changed edges
-	for (int axis = 0; axis < 3; axis++)
-	{
-		BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis];
-		BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis];
-
-		int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos;
-		int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos;
-
-		m_pEdges[axis][emin].m_pos = min[axis];
-		m_pEdges[axis][emax].m_pos = max[axis];
-
-		// expand (only adds overlaps)
-		if (dmin < 0)
-			sortMinDown(axis, emin,dispatcher,true);
-
-		if (dmax > 0)
-			sortMaxUp(axis, emax,dispatcher,true);
-
-		// shrink (only removes overlaps)
-		if (dmin > 0)
-			sortMinUp(axis, emin,dispatcher,true);
-
-		if (dmax < 0)
-			sortMaxDown(axis, emax,dispatcher,true);
-
-#ifdef DEBUG_BROADPHASE
-	debugPrintAxis(axis);
-#endif //DEBUG_BROADPHASE
-	}
-
-	
-}
-
-
-
-
-// sorting a min edge downwards can only ever *add* overlaps
-template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
-{
-
-	Edge* pEdge = m_pEdges[axis] + edge;
-	Edge* pPrev = pEdge - 1;
-	Handle* pHandleEdge = getHandle(pEdge->m_handle);
-
-	while (pEdge->m_pos < pPrev->m_pos)
-	{
-		Handle* pHandlePrev = getHandle(pPrev->m_handle);
-
-		if (pPrev->IsMax())
-		{
-			// if previous edge is a maximum check the bounds and add an overlap if necessary
-			const int axis1 = (1  << axis) & 3;
-			const int axis2 = (1  << axis1) & 3;
-			if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev,axis1,axis2))
-			{
-				m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev);
-				if (m_userPairCallback)
-					m_userPairCallback->addOverlappingPair(pHandleEdge,pHandlePrev);
-
-				//AddOverlap(pEdge->m_handle, pPrev->m_handle);
-
-			}
-
-			// update edge reference in other handle
-			pHandlePrev->m_maxEdges[axis]++;
-		}
-		else
-			pHandlePrev->m_minEdges[axis]++;
-
-		pHandleEdge->m_minEdges[axis]--;
-
-		// swap the edges
-		Edge swap = *pEdge;
-		*pEdge = *pPrev;
-		*pPrev = swap;
-
-		// decrement
-		pEdge--;
-		pPrev--;
-	}
-
-#ifdef DEBUG_BROADPHASE
-	debugPrintAxis(axis);
-#endif //DEBUG_BROADPHASE
-
-}
-
-// sorting a min edge upwards can only ever *remove* overlaps
-template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
-{
-	Edge* pEdge = m_pEdges[axis] + edge;
-	Edge* pNext = pEdge + 1;
-	Handle* pHandleEdge = getHandle(pEdge->m_handle);
-
-	while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
-	{
-		Handle* pHandleNext = getHandle(pNext->m_handle);
-
-		if (pNext->IsMax())
-		{
-			Handle* handle0 = getHandle(pEdge->m_handle);
-			Handle* handle1 = getHandle(pNext->m_handle);
-			const int axis1 = (1  << axis) & 3;
-			const int axis2 = (1  << axis1) & 3;
-			
-			// if next edge is maximum remove any overlap between the two handles
-			if (updateOverlaps 
-#ifdef USE_OVERLAP_TEST_ON_REMOVES
-				&& testOverlap2D(handle0,handle1,axis1,axis2)
-#endif //USE_OVERLAP_TEST_ON_REMOVES
-				)
-			{
-				
-
-				m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);	
-				if (m_userPairCallback)
-					m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher);
-				
-			}
-
-
-			// update edge reference in other handle
-			pHandleNext->m_maxEdges[axis]--;
-		}
-		else
-			pHandleNext->m_minEdges[axis]--;
-
-		pHandleEdge->m_minEdges[axis]++;
-
-		// swap the edges
-		Edge swap = *pEdge;
-		*pEdge = *pNext;
-		*pNext = swap;
-
-		// increment
-		pEdge++;
-		pNext++;
-	}
-
-
-}
-
-// sorting a max edge downwards can only ever *remove* overlaps
-template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
-{
-
-	Edge* pEdge = m_pEdges[axis] + edge;
-	Edge* pPrev = pEdge - 1;
-	Handle* pHandleEdge = getHandle(pEdge->m_handle);
-
-	while (pEdge->m_pos < pPrev->m_pos)
-	{
-		Handle* pHandlePrev = getHandle(pPrev->m_handle);
-
-		if (!pPrev->IsMax())
-		{
-			// if previous edge was a minimum remove any overlap between the two handles
-			Handle* handle0 = getHandle(pEdge->m_handle);
-			Handle* handle1 = getHandle(pPrev->m_handle);
-			const int axis1 = (1  << axis) & 3;
-			const int axis2 = (1  << axis1) & 3;
-
-			if (updateOverlaps  
-#ifdef USE_OVERLAP_TEST_ON_REMOVES
-				&& testOverlap2D(handle0,handle1,axis1,axis2)
-#endif //USE_OVERLAP_TEST_ON_REMOVES
-				)
-			{
-				//this is done during the overlappingpairarray iteration/narrowphase collision
-
-				
-				m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);
-				if (m_userPairCallback)
-					m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher);
-			
-
-
-			}
-
-			// update edge reference in other handle
-			pHandlePrev->m_minEdges[axis]++;;
-		}
-		else
-			pHandlePrev->m_maxEdges[axis]++;
-
-		pHandleEdge->m_maxEdges[axis]--;
-
-		// swap the edges
-		Edge swap = *pEdge;
-		*pEdge = *pPrev;
-		*pPrev = swap;
-
-		// decrement
-		pEdge--;
-		pPrev--;
-	}
-
-	
-#ifdef DEBUG_BROADPHASE
-	debugPrintAxis(axis);
-#endif //DEBUG_BROADPHASE
-
-}
-
-// sorting a max edge upwards can only ever *add* overlaps
-template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
-{
-	Edge* pEdge = m_pEdges[axis] + edge;
-	Edge* pNext = pEdge + 1;
-	Handle* pHandleEdge = getHandle(pEdge->m_handle);
-
-	while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
-	{
-		Handle* pHandleNext = getHandle(pNext->m_handle);
-
-		const int axis1 = (1  << axis) & 3;
-		const int axis2 = (1  << axis1) & 3;
-
-		if (!pNext->IsMax())
-		{
-			// if next edge is a minimum check the bounds and add an overlap if necessary
-			if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext,axis1,axis2))
-			{
-				Handle* handle0 = getHandle(pEdge->m_handle);
-				Handle* handle1 = getHandle(pNext->m_handle);
-				m_pairCache->addOverlappingPair(handle0,handle1);
-				if (m_userPairCallback)
-					m_userPairCallback->addOverlappingPair(handle0,handle1);
-			}
-
-			// update edge reference in other handle
-			pHandleNext->m_minEdges[axis]--;
-		}
-		else
-			pHandleNext->m_maxEdges[axis]--;
-
-		pHandleEdge->m_maxEdges[axis]++;
-
-		// swap the edges
-		Edge swap = *pEdge;
-		*pEdge = *pNext;
-		*pNext = swap;
-
-		// increment
-		pEdge++;
-		pNext++;
-	}
-	
-}
-
-
-
-////////////////////////////////////////////////////////////////////
-
+#include "btAxisSweep3Internal.h"
 
 /// The btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase.
 /// It uses arrays rather then lists for storage of the 3 axis. Also it operates using 16 bit integer coordinates instead of floats.
@@ -1031,9 +33,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE ed
 class btAxisSweep3 : public btAxisSweep3Internal<unsigned short int>
 {
 public:
-
-	btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
-
+	btAxisSweep3(const btVector3& worldAabbMin, const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
 };
 
 /// The bt32BitAxisSweep3 allows higher precision quantization and more objects compared to the btAxisSweep3 sweep and prune.
@@ -1042,10 +42,7 @@ public:
 class bt32BitAxisSweep3 : public btAxisSweep3Internal<unsigned int>
 {
 public:
-
-	bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
-
+	bt32BitAxisSweep3(const btVector3& worldAabbMin, const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
 };
 
 #endif
-
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h
new file mode 100644
index 00000000000..2ee35528fd9
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h
@@ -0,0 +1,954 @@
+//Bullet Continuous Collision Detection and Physics Library
+//Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+//
+// btAxisSweep3.h
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// 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_AXIS_SWEEP_3_INTERNAL_H
+#define BT_AXIS_SWEEP_3_INTERNAL_H
+
+#include "LinearMath/btVector3.h"
+#include "btOverlappingPairCache.h"
+#include "btBroadphaseInterface.h"
+#include "btBroadphaseProxy.h"
+#include "btOverlappingPairCallback.h"
+#include "btDbvtBroadphase.h"
+
+//#define DEBUG_BROADPHASE 1
+#define USE_OVERLAP_TEST_ON_REMOVES 1
+
+/// The internal templace class btAxisSweep3Internal implements the sweep and prune broadphase.
+/// It uses quantized integers to represent the begin and end points for each of the 3 axis.
+/// Dont use this class directly, use btAxisSweep3 or bt32BitAxisSweep3 instead.
+template <typename BP_FP_INT_TYPE>
+class btAxisSweep3Internal : public btBroadphaseInterface
+{
+protected:
+	BP_FP_INT_TYPE m_bpHandleMask;
+	BP_FP_INT_TYPE m_handleSentinel;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	class Edge
+	{
+	public:
+		BP_FP_INT_TYPE m_pos;  // low bit is min/max
+		BP_FP_INT_TYPE m_handle;
+
+		BP_FP_INT_TYPE IsMax() const { return static_cast<BP_FP_INT_TYPE>(m_pos & 1); }
+	};
+
+public:
+	class Handle : public btBroadphaseProxy
+	{
+	public:
+		BT_DECLARE_ALIGNED_ALLOCATOR();
+
+		// indexes into the edge arrays
+		BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3];  // 6 * 2 = 12
+													  //		BP_FP_INT_TYPE m_uniqueId;
+		btBroadphaseProxy* m_dbvtProxy;               //for faster raycast
+		//void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
+
+		SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) { m_minEdges[0] = next; }
+		SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const { return m_minEdges[0]; }
+	};  // 24 bytes + 24 for Edge structures = 44 bytes total per entry
+
+protected:
+	btVector3 m_worldAabbMin;  // overall system bounds
+	btVector3 m_worldAabbMax;  // overall system bounds
+
+	btVector3 m_quantize;  // scaling factor for quantization
+
+	BP_FP_INT_TYPE m_numHandles;  // number of active handles
+	BP_FP_INT_TYPE m_maxHandles;  // max number of handles
+	Handle* m_pHandles;           // handles pool
+
+	BP_FP_INT_TYPE m_firstFreeHandle;  // free handles list
+
+	Edge* m_pEdges[3];  // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries)
+	void* m_pEdgesRawPtr[3];
+
+	btOverlappingPairCache* m_pairCache;
+
+	///btOverlappingPairCallback is an additional optional user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache.
+	btOverlappingPairCallback* m_userPairCallback;
+
+	bool m_ownsPairCache;
+
+	int m_invalidPair;
+
+	///additional dynamic aabb structure, used to accelerate ray cast queries.
+	///can be disabled using a optional argument in the constructor
+	btDbvtBroadphase* m_raycastAccelerator;
+	btOverlappingPairCache* m_nullPairCache;
+
+	// allocation/deallocation
+	BP_FP_INT_TYPE allocHandle();
+	void freeHandle(BP_FP_INT_TYPE handle);
+
+	bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB, int axis0, int axis1);
+
+#ifdef DEBUG_BROADPHASE
+	void debugPrintAxis(int axis, bool checkCardinality = true);
+#endif  //DEBUG_BROADPHASE
+
+	//Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
+	//void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
+
+	void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps);
+	void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps);
+	void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps);
+	void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps);
+
+public:
+	btAxisSweep3Internal(const btVector3& worldAabbMin, const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
+
+	virtual ~btAxisSweep3Internal();
+
+	BP_FP_INT_TYPE getNumHandles() const
+	{
+		return m_numHandles;
+	}
+
+	virtual void calculateOverlappingPairs(btDispatcher* dispatcher);
+
+	BP_FP_INT_TYPE addHandle(const btVector3& aabbMin, const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher);
+	void removeHandle(BP_FP_INT_TYPE handle, btDispatcher* dispatcher);
+	void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher);
+	SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const { return m_pHandles + index; }
+
+	virtual void resetPool(btDispatcher* dispatcher);
+
+	void processAllOverlappingPairs(btOverlapCallback* callback);
+
+	//Broadphase Interface
+	virtual btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher);
+	virtual void destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher);
+	virtual void setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher);
+	virtual void getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const;
+
+	virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0));
+	virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
+
+	void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const;
+	///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result
+	void unQuantize(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const;
+
+	bool testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
+
+	btOverlappingPairCache* getOverlappingPairCache()
+	{
+		return m_pairCache;
+	}
+	const btOverlappingPairCache* getOverlappingPairCache() const
+	{
+		return m_pairCache;
+	}
+
+	void setOverlappingPairUserCallback(btOverlappingPairCallback* pairCallback)
+	{
+		m_userPairCallback = pairCallback;
+	}
+	const btOverlappingPairCallback* getOverlappingPairUserCallback() const
+	{
+		return m_userPairCallback;
+	}
+
+	///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
+	///will add some transform later
+	virtual void getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const
+	{
+		aabbMin = m_worldAabbMin;
+		aabbMax = m_worldAabbMax;
+	}
+
+	virtual void printStats()
+	{
+		/*		printf("btAxisSweep3.h\n");
+		printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
+		printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(),
+			m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ());
+			*/
+	}
+};
+
+////////////////////////////////////////////////////////////////////
+
+#ifdef DEBUG_BROADPHASE
+#include <stdio.h>
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3<BP_FP_INT_TYPE>::debugPrintAxis(int axis, bool checkCardinality)
+{
+	int numEdges = m_pHandles[0].m_maxEdges[axis];
+	printf("SAP Axis %d, numEdges=%d\n", axis, numEdges);
+
+	int i;
+	for (i = 0; i < numEdges + 1; i++)
+	{
+		Edge* pEdge = m_pEdges[axis] + i;
+		Handle* pHandlePrev = getHandle(pEdge->m_handle);
+		int handleIndex = pEdge->IsMax() ? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis];
+		char beginOrEnd;
+		beginOrEnd = pEdge->IsMax() ? 'E' : 'B';
+		printf("	[%c,h=%d,p=%x,i=%d]\n", beginOrEnd, pEdge->m_handle, pEdge->m_pos, handleIndex);
+	}
+
+	if (checkCardinality)
+		btAssert(numEdges == m_numHandles * 2 + 1);
+}
+#endif  //DEBUG_BROADPHASE
+
+template <typename BP_FP_INT_TYPE>
+btBroadphaseProxy* btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher)
+{
+	(void)shapeType;
+	BP_FP_INT_TYPE handleId = addHandle(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher);
+
+	Handle* handle = getHandle(handleId);
+
+	if (m_raycastAccelerator)
+	{
+		btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher);
+		handle->m_dbvtProxy = rayProxy;
+	}
+	return handle;
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
+{
+	Handle* handle = static_cast<Handle*>(proxy);
+	if (m_raycastAccelerator)
+		m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy, dispatcher);
+	removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher)
+{
+	Handle* handle = static_cast<Handle*>(proxy);
+	handle->m_aabbMin = aabbMin;
+	handle->m_aabbMax = aabbMax;
+	updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax, dispatcher);
+	if (m_raycastAccelerator)
+		m_raycastAccelerator->setAabb(handle->m_dbvtProxy, aabbMin, aabbMax, dispatcher);
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin, const btVector3& aabbMax)
+{
+	if (m_raycastAccelerator)
+	{
+		m_raycastAccelerator->rayTest(rayFrom, rayTo, rayCallback, aabbMin, aabbMax);
+	}
+	else
+	{
+		//choose axis?
+		BP_FP_INT_TYPE axis = 0;
+		//for each proxy
+		for (BP_FP_INT_TYPE i = 1; i < m_numHandles * 2 + 1; i++)
+		{
+			if (m_pEdges[axis][i].IsMax())
+			{
+				rayCallback.process(getHandle(m_pEdges[axis][i].m_handle));
+			}
+		}
+	}
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
+{
+	if (m_raycastAccelerator)
+	{
+		m_raycastAccelerator->aabbTest(aabbMin, aabbMax, callback);
+	}
+	else
+	{
+		//choose axis?
+		BP_FP_INT_TYPE axis = 0;
+		//for each proxy
+		for (BP_FP_INT_TYPE i = 1; i < m_numHandles * 2 + 1; i++)
+		{
+			if (m_pEdges[axis][i].IsMax())
+			{
+				Handle* handle = getHandle(m_pEdges[axis][i].m_handle);
+				if (TestAabbAgainstAabb2(aabbMin, aabbMax, handle->m_aabbMin, handle->m_aabbMax))
+				{
+					callback.process(handle);
+				}
+			}
+		}
+	}
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const
+{
+	Handle* pHandle = static_cast<Handle*>(proxy);
+	aabbMin = pHandle->m_aabbMin;
+	aabbMax = pHandle->m_aabbMax;
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::unQuantize(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const
+{
+	Handle* pHandle = static_cast<Handle*>(proxy);
+
+	unsigned short vecInMin[3];
+	unsigned short vecInMax[3];
+
+	vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos;
+	vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos + 1;
+	vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos;
+	vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos + 1;
+	vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos;
+	vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos + 1;
+
+	aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()), (btScalar)(vecInMin[1]) / (m_quantize.getY()), (btScalar)(vecInMin[2]) / (m_quantize.getZ()));
+	aabbMin += m_worldAabbMin;
+
+	aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()), (btScalar)(vecInMax[1]) / (m_quantize.getY()), (btScalar)(vecInMax[2]) / (m_quantize.getZ()));
+	aabbMax += m_worldAabbMin;
+}
+
+template <typename BP_FP_INT_TYPE>
+btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btVector3& worldAabbMin, const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
+	: m_bpHandleMask(handleMask),
+	  m_handleSentinel(handleSentinel),
+	  m_pairCache(pairCache),
+	  m_userPairCallback(0),
+	  m_ownsPairCache(false),
+	  m_invalidPair(0),
+	  m_raycastAccelerator(0)
+{
+	BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles + 1);  //need to add one sentinel handle
+
+	if (!m_pairCache)
+	{
+		void* ptr = btAlignedAlloc(sizeof(btHashedOverlappingPairCache), 16);
+		m_pairCache = new (ptr) btHashedOverlappingPairCache();
+		m_ownsPairCache = true;
+	}
+
+	if (!disableRaycastAccelerator)
+	{
+		m_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache), 16)) btNullPairCache();
+		m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase), 16)) btDbvtBroadphase(m_nullPairCache);  //m_pairCache);
+		m_raycastAccelerator->m_deferedcollide = true;                                                                //don't add/remove pairs
+	}
+
+	//btAssert(bounds.HasVolume());
+
+	// init bounds
+	m_worldAabbMin = worldAabbMin;
+	m_worldAabbMax = worldAabbMax;
+
+	btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin;
+
+	BP_FP_INT_TYPE maxInt = m_handleSentinel;
+
+	m_quantize = btVector3(btScalar(maxInt), btScalar(maxInt), btScalar(maxInt)) / aabbSize;
+
+	// allocate handles buffer, using btAlignedAlloc, and put all handles on free list
+	m_pHandles = new Handle[maxHandles];
+
+	m_maxHandles = maxHandles;
+	m_numHandles = 0;
+
+	// handle 0 is reserved as the null index, and is also used as the sentinel
+	m_firstFreeHandle = 1;
+	{
+		for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++)
+			m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
+		m_pHandles[maxHandles - 1].SetNextFree(0);
+	}
+
+	{
+		// allocate edge buffers
+		for (int i = 0; i < 3; i++)
+		{
+			m_pEdgesRawPtr[i] = btAlignedAlloc(sizeof(Edge) * maxHandles * 2, 16);
+			m_pEdges[i] = new (m_pEdgesRawPtr[i]) Edge[maxHandles * 2];
+		}
+	}
+	//removed overlap management
+
+	// make boundary sentinels
+
+	m_pHandles[0].m_clientObject = 0;
+
+	for (int axis = 0; axis < 3; axis++)
+	{
+		m_pHandles[0].m_minEdges[axis] = 0;
+		m_pHandles[0].m_maxEdges[axis] = 1;
+
+		m_pEdges[axis][0].m_pos = 0;
+		m_pEdges[axis][0].m_handle = 0;
+		m_pEdges[axis][1].m_pos = m_handleSentinel;
+		m_pEdges[axis][1].m_handle = 0;
+#ifdef DEBUG_BROADPHASE
+		debugPrintAxis(axis);
+#endif  //DEBUG_BROADPHASE
+	}
+}
+
+template <typename BP_FP_INT_TYPE>
+btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal()
+{
+	if (m_raycastAccelerator)
+	{
+		m_nullPairCache->~btOverlappingPairCache();
+		btAlignedFree(m_nullPairCache);
+		m_raycastAccelerator->~btDbvtBroadphase();
+		btAlignedFree(m_raycastAccelerator);
+	}
+
+	for (int i = 2; i >= 0; i--)
+	{
+		btAlignedFree(m_pEdgesRawPtr[i]);
+	}
+	delete[] m_pHandles;
+
+	if (m_ownsPairCache)
+	{
+		m_pairCache->~btOverlappingPairCache();
+		btAlignedFree(m_pairCache);
+	}
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const
+{
+#ifdef OLD_CLAMPING_METHOD
+	///problem with this clamping method is that the floating point during quantization might still go outside the range [(0|isMax) .. (m_handleSentinel&m_bpHandleMask]|isMax]
+	///see http://code.google.com/p/bullet/issues/detail?id=87
+	btVector3 clampedPoint(point);
+	clampedPoint.setMax(m_worldAabbMin);
+	clampedPoint.setMin(m_worldAabbMax);
+	btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
+	out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & m_bpHandleMask) | isMax);
+	out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & m_bpHandleMask) | isMax);
+	out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax);
+#else
+	btVector3 v = (point - m_worldAabbMin) * m_quantize;
+	out[0] = (v[0] <= 0) ? (BP_FP_INT_TYPE)isMax : (v[0] >= m_handleSentinel) ? (BP_FP_INT_TYPE)((m_handleSentinel & m_bpHandleMask) | isMax) : (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0] & m_bpHandleMask) | isMax);
+	out[1] = (v[1] <= 0) ? (BP_FP_INT_TYPE)isMax : (v[1] >= m_handleSentinel) ? (BP_FP_INT_TYPE)((m_handleSentinel & m_bpHandleMask) | isMax) : (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1] & m_bpHandleMask) | isMax);
+	out[2] = (v[2] <= 0) ? (BP_FP_INT_TYPE)isMax : (v[2] >= m_handleSentinel) ? (BP_FP_INT_TYPE)((m_handleSentinel & m_bpHandleMask) | isMax) : (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2] & m_bpHandleMask) | isMax);
+#endif  //OLD_CLAMPING_METHOD
+}
+
+template <typename BP_FP_INT_TYPE>
+BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle()
+{
+	btAssert(m_firstFreeHandle);
+
+	BP_FP_INT_TYPE handle = m_firstFreeHandle;
+	m_firstFreeHandle = getHandle(handle)->GetNextFree();
+	m_numHandles++;
+
+	return handle;
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
+{
+	btAssert(handle > 0 && handle < m_maxHandles);
+
+	getHandle(handle)->SetNextFree(m_firstFreeHandle);
+	m_firstFreeHandle = handle;
+
+	m_numHandles--;
+}
+
+template <typename BP_FP_INT_TYPE>
+BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btVector3& aabbMin, const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher)
+{
+	// quantize the bounds
+	BP_FP_INT_TYPE min[3], max[3];
+	quantize(min, aabbMin, 0);
+	quantize(max, aabbMax, 1);
+
+	// allocate a handle
+	BP_FP_INT_TYPE handle = allocHandle();
+
+	Handle* pHandle = getHandle(handle);
+
+	pHandle->m_uniqueId = static_cast<int>(handle);
+	//pHandle->m_pOverlaps = 0;
+	pHandle->m_clientObject = pOwner;
+	pHandle->m_collisionFilterGroup = collisionFilterGroup;
+	pHandle->m_collisionFilterMask = collisionFilterMask;
+
+	// compute current limit of edge arrays
+	BP_FP_INT_TYPE limit = static_cast<BP_FP_INT_TYPE>(m_numHandles * 2);
+
+	// insert new edges just inside the max boundary edge
+	for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++)
+	{
+		m_pHandles[0].m_maxEdges[axis] += 2;
+
+		m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1];
+
+		m_pEdges[axis][limit - 1].m_pos = min[axis];
+		m_pEdges[axis][limit - 1].m_handle = handle;
+
+		m_pEdges[axis][limit].m_pos = max[axis];
+		m_pEdges[axis][limit].m_handle = handle;
+
+		pHandle->m_minEdges[axis] = static_cast<BP_FP_INT_TYPE>(limit - 1);
+		pHandle->m_maxEdges[axis] = limit;
+	}
+
+	// now sort the new edges to their correct position
+	sortMinDown(0, pHandle->m_minEdges[0], dispatcher, false);
+	sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher, false);
+	sortMinDown(1, pHandle->m_minEdges[1], dispatcher, false);
+	sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher, false);
+	sortMinDown(2, pHandle->m_minEdges[2], dispatcher, true);
+	sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher, true);
+
+	return handle;
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle, btDispatcher* dispatcher)
+{
+	Handle* pHandle = getHandle(handle);
+
+	//explicitly remove the pairs containing the proxy
+	//we could do it also in the sortMinUp (passing true)
+	///@todo: compare performance
+	if (!m_pairCache->hasDeferredRemoval())
+	{
+		m_pairCache->removeOverlappingPairsContainingProxy(pHandle, dispatcher);
+	}
+
+	// compute current limit of edge arrays
+	int limit = static_cast<int>(m_numHandles * 2);
+
+	int axis;
+
+	for (axis = 0; axis < 3; axis++)
+	{
+		m_pHandles[0].m_maxEdges[axis] -= 2;
+	}
+
+	// remove the edges by sorting them up to the end of the list
+	for (axis = 0; axis < 3; axis++)
+	{
+		Edge* pEdges = m_pEdges[axis];
+		BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
+		pEdges[max].m_pos = m_handleSentinel;
+
+		sortMaxUp(axis, max, dispatcher, false);
+
+		BP_FP_INT_TYPE i = pHandle->m_minEdges[axis];
+		pEdges[i].m_pos = m_handleSentinel;
+
+		sortMinUp(axis, i, dispatcher, false);
+
+		pEdges[limit - 1].m_handle = 0;
+		pEdges[limit - 1].m_pos = m_handleSentinel;
+
+#ifdef DEBUG_BROADPHASE
+		debugPrintAxis(axis, false);
+#endif  //DEBUG_BROADPHASE
+	}
+
+	// free the handle
+	freeHandle(handle);
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool(btDispatcher* /*dispatcher*/)
+{
+	if (m_numHandles == 0)
+	{
+		m_firstFreeHandle = 1;
+		{
+			for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++)
+				m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
+			m_pHandles[m_maxHandles - 1].SetNextFree(0);
+		}
+	}
+}
+
+//#include <stdio.h>
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatcher* dispatcher)
+{
+	if (m_pairCache->hasDeferredRemoval())
+	{
+		btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray();
+
+		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+		m_invalidPair = 0;
+
+		int i;
+
+		btBroadphasePair previousPair;
+		previousPair.m_pProxy0 = 0;
+		previousPair.m_pProxy1 = 0;
+		previousPair.m_algorithm = 0;
+
+		for (i = 0; i < overlappingPairArray.size(); i++)
+		{
+			btBroadphasePair& pair = overlappingPairArray[i];
+
+			bool isDuplicate = (pair == previousPair);
+
+			previousPair = pair;
+
+			bool needsRemoval = false;
+
+			if (!isDuplicate)
+			{
+				///important to use an AABB test that is consistent with the broadphase
+				bool hasOverlap = testAabbOverlap(pair.m_pProxy0, pair.m_pProxy1);
+
+				if (hasOverlap)
+				{
+					needsRemoval = false;  //callback->processOverlap(pair);
+				}
+				else
+				{
+					needsRemoval = true;
+				}
+			}
+			else
+			{
+				//remove duplicate
+				needsRemoval = true;
+				//should have no algorithm
+				btAssert(!pair.m_algorithm);
+			}
+
+			if (needsRemoval)
+			{
+				m_pairCache->cleanOverlappingPair(pair, dispatcher);
+
+				//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+				//		m_overlappingPairArray.pop_back();
+				pair.m_pProxy0 = 0;
+				pair.m_pProxy1 = 0;
+				m_invalidPair++;
+			}
+		}
+
+///if you don't like to skip the invalid pairs in the array, execute following code:
+#define CLEAN_INVALID_PAIRS 1
+#ifdef CLEAN_INVALID_PAIRS
+
+		//perform a sort, to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+		m_invalidPair = 0;
+#endif  //CLEAN_INVALID_PAIRS
+
+		//printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
+	}
+}
+
+template <typename BP_FP_INT_TYPE>
+bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
+{
+	const Handle* pHandleA = static_cast<Handle*>(proxy0);
+	const Handle* pHandleB = static_cast<Handle*>(proxy1);
+
+	//optimization 1: check the array index (memory address), instead of the m_pos
+
+	for (int axis = 0; axis < 3; axis++)
+	{
+		if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] ||
+			pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis])
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+template <typename BP_FP_INT_TYPE>
+bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap2D(const Handle* pHandleA, const Handle* pHandleB, int axis0, int axis1)
+{
+	//optimization 1: check the array index (memory address), instead of the m_pos
+
+	if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] ||
+		pHandleB->m_maxEdges[axis0] < pHandleA->m_minEdges[axis0] ||
+		pHandleA->m_maxEdges[axis1] < pHandleB->m_minEdges[axis1] ||
+		pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1])
+	{
+		return false;
+	}
+	return true;
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher)
+{
+	//	btAssert(bounds.IsFinite());
+	//btAssert(bounds.HasVolume());
+
+	Handle* pHandle = getHandle(handle);
+
+	// quantize the new bounds
+	BP_FP_INT_TYPE min[3], max[3];
+	quantize(min, aabbMin, 0);
+	quantize(max, aabbMax, 1);
+
+	// update changed edges
+	for (int axis = 0; axis < 3; axis++)
+	{
+		BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis];
+		BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis];
+
+		int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos;
+		int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos;
+
+		m_pEdges[axis][emin].m_pos = min[axis];
+		m_pEdges[axis][emax].m_pos = max[axis];
+
+		// expand (only adds overlaps)
+		if (dmin < 0)
+			sortMinDown(axis, emin, dispatcher, true);
+
+		if (dmax > 0)
+			sortMaxUp(axis, emax, dispatcher, true);
+
+		// shrink (only removes overlaps)
+		if (dmin > 0)
+			sortMinUp(axis, emin, dispatcher, true);
+
+		if (dmax < 0)
+			sortMaxDown(axis, emax, dispatcher, true);
+
+#ifdef DEBUG_BROADPHASE
+		debugPrintAxis(axis);
+#endif  //DEBUG_BROADPHASE
+	}
+}
+
+// sorting a min edge downwards can only ever *add* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
+{
+	Edge* pEdge = m_pEdges[axis] + edge;
+	Edge* pPrev = pEdge - 1;
+	Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+	while (pEdge->m_pos < pPrev->m_pos)
+	{
+		Handle* pHandlePrev = getHandle(pPrev->m_handle);
+
+		if (pPrev->IsMax())
+		{
+			// if previous edge is a maximum check the bounds and add an overlap if necessary
+			const int axis1 = (1 << axis) & 3;
+			const int axis2 = (1 << axis1) & 3;
+			if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev, axis1, axis2))
+			{
+				m_pairCache->addOverlappingPair(pHandleEdge, pHandlePrev);
+				if (m_userPairCallback)
+					m_userPairCallback->addOverlappingPair(pHandleEdge, pHandlePrev);
+
+				//AddOverlap(pEdge->m_handle, pPrev->m_handle);
+			}
+
+			// update edge reference in other handle
+			pHandlePrev->m_maxEdges[axis]++;
+		}
+		else
+			pHandlePrev->m_minEdges[axis]++;
+
+		pHandleEdge->m_minEdges[axis]--;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pPrev;
+		*pPrev = swap;
+
+		// decrement
+		pEdge--;
+		pPrev--;
+	}
+
+#ifdef DEBUG_BROADPHASE
+	debugPrintAxis(axis);
+#endif  //DEBUG_BROADPHASE
+}
+
+// sorting a min edge upwards can only ever *remove* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
+{
+	Edge* pEdge = m_pEdges[axis] + edge;
+	Edge* pNext = pEdge + 1;
+	Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+	while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
+	{
+		Handle* pHandleNext = getHandle(pNext->m_handle);
+
+		if (pNext->IsMax())
+		{
+			Handle* handle0 = getHandle(pEdge->m_handle);
+			Handle* handle1 = getHandle(pNext->m_handle);
+			const int axis1 = (1 << axis) & 3;
+			const int axis2 = (1 << axis1) & 3;
+
+			// if next edge is maximum remove any overlap between the two handles
+			if (updateOverlaps
+#ifdef USE_OVERLAP_TEST_ON_REMOVES
+				&& testOverlap2D(handle0, handle1, axis1, axis2)
+#endif  //USE_OVERLAP_TEST_ON_REMOVES
+			)
+			{
+				m_pairCache->removeOverlappingPair(handle0, handle1, dispatcher);
+				if (m_userPairCallback)
+					m_userPairCallback->removeOverlappingPair(handle0, handle1, dispatcher);
+			}
+
+			// update edge reference in other handle
+			pHandleNext->m_maxEdges[axis]--;
+		}
+		else
+			pHandleNext->m_minEdges[axis]--;
+
+		pHandleEdge->m_minEdges[axis]++;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pNext;
+		*pNext = swap;
+
+		// increment
+		pEdge++;
+		pNext++;
+	}
+}
+
+// sorting a max edge downwards can only ever *remove* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
+{
+	Edge* pEdge = m_pEdges[axis] + edge;
+	Edge* pPrev = pEdge - 1;
+	Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+	while (pEdge->m_pos < pPrev->m_pos)
+	{
+		Handle* pHandlePrev = getHandle(pPrev->m_handle);
+
+		if (!pPrev->IsMax())
+		{
+			// if previous edge was a minimum remove any overlap between the two handles
+			Handle* handle0 = getHandle(pEdge->m_handle);
+			Handle* handle1 = getHandle(pPrev->m_handle);
+			const int axis1 = (1 << axis) & 3;
+			const int axis2 = (1 << axis1) & 3;
+
+			if (updateOverlaps
+#ifdef USE_OVERLAP_TEST_ON_REMOVES
+				&& testOverlap2D(handle0, handle1, axis1, axis2)
+#endif  //USE_OVERLAP_TEST_ON_REMOVES
+			)
+			{
+				//this is done during the overlappingpairarray iteration/narrowphase collision
+
+				m_pairCache->removeOverlappingPair(handle0, handle1, dispatcher);
+				if (m_userPairCallback)
+					m_userPairCallback->removeOverlappingPair(handle0, handle1, dispatcher);
+			}
+
+			// update edge reference in other handle
+			pHandlePrev->m_minEdges[axis]++;
+			;
+		}
+		else
+			pHandlePrev->m_maxEdges[axis]++;
+
+		pHandleEdge->m_maxEdges[axis]--;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pPrev;
+		*pPrev = swap;
+
+		// decrement
+		pEdge--;
+		pPrev--;
+	}
+
+#ifdef DEBUG_BROADPHASE
+	debugPrintAxis(axis);
+#endif  //DEBUG_BROADPHASE
+}
+
+// sorting a max edge upwards can only ever *add* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
+{
+	Edge* pEdge = m_pEdges[axis] + edge;
+	Edge* pNext = pEdge + 1;
+	Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+	while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
+	{
+		Handle* pHandleNext = getHandle(pNext->m_handle);
+
+		const int axis1 = (1 << axis) & 3;
+		const int axis2 = (1 << axis1) & 3;
+
+		if (!pNext->IsMax())
+		{
+			// if next edge is a minimum check the bounds and add an overlap if necessary
+			if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext, axis1, axis2))
+			{
+				Handle* handle0 = getHandle(pEdge->m_handle);
+				Handle* handle1 = getHandle(pNext->m_handle);
+				m_pairCache->addOverlappingPair(handle0, handle1);
+				if (m_userPairCallback)
+					m_userPairCallback->addOverlappingPair(handle0, handle1);
+			}
+
+			// update edge reference in other handle
+			pHandleNext->m_minEdges[axis]--;
+		}
+		else
+			pHandleNext->m_maxEdges[axis]--;
+
+		pHandleEdge->m_maxEdges[axis]++;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pNext;
+		*pNext = swap;
+
+		// increment
+		pEdge++;
+		pNext++;
+	}
+}
+
+#endif
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
index f1bf00594d3..b097eca5f5b 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
@@ -13,10 +13,8 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-#ifndef		BT_BROADPHASE_INTERFACE_H
-#define 	BT_BROADPHASE_INTERFACE_H
-
-
+#ifndef BT_BROADPHASE_INTERFACE_H
+#define BT_BROADPHASE_INTERFACE_H
 
 struct btDispatcherInfo;
 class btDispatcher;
@@ -24,23 +22,23 @@ class btDispatcher;
 
 class btOverlappingPairCache;
 
-
-
-struct	btBroadphaseAabbCallback
+struct btBroadphaseAabbCallback
 {
 	virtual ~btBroadphaseAabbCallback() {}
-	virtual bool	process(const btBroadphaseProxy* proxy) = 0;
+	virtual bool process(const btBroadphaseProxy* proxy) = 0;
 };
 
-
-struct	btBroadphaseRayCallback : public btBroadphaseAabbCallback
+struct btBroadphaseRayCallback : public btBroadphaseAabbCallback
 {
 	///added some cached data to accelerate ray-AABB tests
-	btVector3		m_rayDirectionInverse;
-	unsigned int	m_signs[3];
-	btScalar		m_lambda_max;
+	btVector3 m_rayDirectionInverse;
+	unsigned int m_signs[3];
+	btScalar m_lambda_max;
 
 	virtual ~btBroadphaseRayCallback() {}
+
+protected:
+	btBroadphaseRayCallback() {}
 };
 
 #include "LinearMath/btVector3.h"
@@ -53,30 +51,29 @@ class btBroadphaseInterface
 public:
 	virtual ~btBroadphaseInterface() {}
 
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy) =0;
-	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0;
-	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0;
-	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const =0;
+	virtual btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher) = 0;
+	virtual void destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher) = 0;
+	virtual void setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher) = 0;
+	virtual void getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const = 0;
 
-	virtual void	rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)) = 0;
+	virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0)) = 0;
 
-	virtual void	aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) = 0;
+	virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) = 0;
 
 	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
-	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher)=0;
+	virtual void calculateOverlappingPairs(btDispatcher* dispatcher) = 0;
 
-	virtual	btOverlappingPairCache*	getOverlappingPairCache()=0;
-	virtual	const btOverlappingPairCache*	getOverlappingPairCache() const =0;
+	virtual btOverlappingPairCache* getOverlappingPairCache() = 0;
+	virtual const btOverlappingPairCache* getOverlappingPairCache() const = 0;
 
 	///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
 	///will add some transform later
-	virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const =0;
+	virtual void getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const = 0;
 
 	///reset broadphase internal structures, to ensure determinism/reproducability
-	virtual void resetPool(btDispatcher* dispatcher) { (void) dispatcher; };
-
-	virtual void	printStats() = 0;
+	virtual void resetPool(btDispatcher* dispatcher) { (void)dispatcher; };
 
+	virtual void printStats() = 0;
 };
 
-#endif //BT_BROADPHASE_INTERFACE_H
+#endif  //BT_BROADPHASE_INTERFACE_H
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp
index f4d7341f8dd..7ee065aac32 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp
@@ -15,3 +15,4 @@ subject to the following restrictions:
 
 #include "btBroadphaseProxy.h"
 
+BT_NOT_EMPTY_FILE  // fix warning LNK4221: This object file does not define any previously undefined public symbols, so it will not be used by any link operation that consumes this library
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
index bb58b828936..bc0742ad623 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
@@ -16,11 +16,10 @@ subject to the following restrictions:
 #ifndef BT_BROADPHASE_PROXY_H
 #define BT_BROADPHASE_PROXY_H
 
-#include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE
+#include "LinearMath/btScalar.h"  //for SIMD_FORCE_INLINE
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btAlignedAllocator.h"
 
-
 /// btDispatcher uses these types
 /// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave
 /// to facilitate type checking
@@ -35,8 +34,8 @@ enum BroadphaseNativeTypes
 	CONVEX_HULL_SHAPE_PROXYTYPE,
 	CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE,
 	CUSTOM_POLYHEDRAL_SHAPE_TYPE,
-//implicit convex shapes
-IMPLICIT_CONVEX_SHAPES_START_HERE,
+	//implicit convex shapes
+	IMPLICIT_CONVEX_SHAPES_START_HERE,
 	SPHERE_SHAPE_PROXYTYPE,
 	MULTI_SPHERE_SHAPE_PROXYTYPE,
 	CAPSULE_SHAPE_PROXYTYPE,
@@ -49,8 +48,8 @@ IMPLICIT_CONVEX_SHAPES_START_HERE,
 	BOX_2D_SHAPE_PROXYTYPE,
 	CONVEX_2D_SHAPE_PROXYTYPE,
 	CUSTOM_CONVEX_SHAPE_TYPE,
-//concave shapes
-CONCAVE_SHAPES_START_HERE,
+	//concave shapes
+	CONCAVE_SHAPES_START_HERE,
 	//keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
 	TRIANGLE_MESH_SHAPE_PROXYTYPE,
 	SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE,
@@ -58,15 +57,16 @@ CONCAVE_SHAPES_START_HERE,
 	FAST_CONCAVE_MESH_PROXYTYPE,
 	//terrain
 	TERRAIN_SHAPE_PROXYTYPE,
-///Used for GIMPACT Trimesh integration
+	///Used for GIMPACT Trimesh integration
 	GIMPACT_SHAPE_PROXYTYPE,
-///Multimaterial mesh
-    MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE,
-	
+	///Multimaterial mesh
+	MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE,
+
 	EMPTY_SHAPE_PROXYTYPE,
 	STATIC_PLANE_PROXYTYPE,
 	CUSTOM_CONCAVE_SHAPE_TYPE,
-CONCAVE_SHAPES_END_HERE,
+	SDF_SHAPE_PROXYTYPE = CUSTOM_CONCAVE_SHAPE_TYPE,
+	CONCAVE_SHAPES_END_HERE,
 
 	COMPOUND_SHAPE_PROXYTYPE,
 
@@ -76,38 +76,37 @@ CONCAVE_SHAPES_END_HERE,
 	INVALID_SHAPE_PROXYTYPE,
 
 	MAX_BROADPHASE_COLLISION_TYPES
-	
-};
 
+};
 
-///The btBroadphaseProxy is the main class that can be used with the Bullet broadphases. 
+///The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
 ///It stores collision shape type information, collision filter information and a client object, typically a btCollisionObject or btRigidBody.
-ATTRIBUTE_ALIGNED16(struct) btBroadphaseProxy
+ATTRIBUTE_ALIGNED16(struct)
+btBroadphaseProxy
 {
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-BT_DECLARE_ALIGNED_ALLOCATOR();
-	
 	///optional filtering to cull potential collisions
 	enum CollisionFilterGroups
 	{
-	        DefaultFilter = 1,
-	        StaticFilter = 2,
-	        KinematicFilter = 4,
-	        DebrisFilter = 8,
-			SensorTrigger = 16,
-			CharacterFilter = 32,
-	        AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
+		DefaultFilter = 1,
+		StaticFilter = 2,
+		KinematicFilter = 4,
+		DebrisFilter = 8,
+		SensorTrigger = 16,
+		CharacterFilter = 32,
+		AllFilter = -1  //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
 	};
 
 	//Usually the client btCollisionObject or Rigidbody class
-	void*	m_clientObject;
-	short int m_collisionFilterGroup;
-	short int m_collisionFilterMask;
-	void*	m_multiSapParentProxy;		
-	int			m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
+	void* m_clientObject;
+	int m_collisionFilterGroup;
+	int m_collisionFilterMask;
 
-	btVector3	m_aabbMin;
-	btVector3	m_aabbMax;
+	int m_uniqueId;  //m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
+
+	btVector3 m_aabbMin;
+	btVector3 m_aabbMax;
 
 	SIMD_FORCE_INLINE int getUid() const
 	{
@@ -115,48 +114,45 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 	}
 
 	//used for memory pools
-	btBroadphaseProxy() :m_clientObject(0),m_multiSapParentProxy(0)
+	btBroadphaseProxy() : m_clientObject(0)
 	{
 	}
 
-	btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0)
-		:m_clientObject(userPtr),
-		m_collisionFilterGroup(collisionFilterGroup),
-		m_collisionFilterMask(collisionFilterMask),
-		m_aabbMin(aabbMin),
-		m_aabbMax(aabbMax)
+	btBroadphaseProxy(const btVector3& aabbMin, const btVector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask)
+		: m_clientObject(userPtr),
+		  m_collisionFilterGroup(collisionFilterGroup),
+		  m_collisionFilterMask(collisionFilterMask),
+		  m_aabbMin(aabbMin),
+		  m_aabbMax(aabbMax)
 	{
-		m_multiSapParentProxy = multiSapParentProxy;
 	}
 
-	
-
 	static SIMD_FORCE_INLINE bool isPolyhedral(int proxyType)
 	{
-		return (proxyType  < IMPLICIT_CONVEX_SHAPES_START_HERE);
+		return (proxyType < IMPLICIT_CONVEX_SHAPES_START_HERE);
 	}
 
-	static SIMD_FORCE_INLINE bool	isConvex(int proxyType)
+	static SIMD_FORCE_INLINE bool isConvex(int proxyType)
 	{
 		return (proxyType < CONCAVE_SHAPES_START_HERE);
 	}
 
-	static SIMD_FORCE_INLINE bool	isNonMoving(int proxyType)
+	static SIMD_FORCE_INLINE bool isNonMoving(int proxyType)
 	{
-		return (isConcave(proxyType) && !(proxyType==GIMPACT_SHAPE_PROXYTYPE));
+		return (isConcave(proxyType) && !(proxyType == GIMPACT_SHAPE_PROXYTYPE));
 	}
 
-	static SIMD_FORCE_INLINE bool	isConcave(int proxyType)
+	static SIMD_FORCE_INLINE bool isConcave(int proxyType)
 	{
 		return ((proxyType > CONCAVE_SHAPES_START_HERE) &&
-			(proxyType < CONCAVE_SHAPES_END_HERE));
+				(proxyType < CONCAVE_SHAPES_END_HERE));
 	}
-	static SIMD_FORCE_INLINE bool	isCompound(int proxyType)
+	static SIMD_FORCE_INLINE bool isCompound(int proxyType)
 	{
 		return (proxyType == COMPOUND_SHAPE_PROXYTYPE);
 	}
 
-	static SIMD_FORCE_INLINE bool	isSoftBody(int proxyType)
+	static SIMD_FORCE_INLINE bool isSoftBody(int proxyType)
 	{
 		return (proxyType == SOFTBODY_SHAPE_PROXYTYPE);
 	}
@@ -168,67 +164,55 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	static SIMD_FORCE_INLINE bool isConvex2d(int proxyType)
 	{
-		return (proxyType == BOX_2D_SHAPE_PROXYTYPE) ||	(proxyType == CONVEX_2D_SHAPE_PROXYTYPE);
+		return (proxyType == BOX_2D_SHAPE_PROXYTYPE) || (proxyType == CONVEX_2D_SHAPE_PROXYTYPE);
 	}
-
-	
-}
-;
+};
 
 class btCollisionAlgorithm;
 
 struct btBroadphaseProxy;
 
-
-
 ///The btBroadphasePair class contains a pair of aabb-overlapping objects.
 ///A btDispatcher can search a btCollisionAlgorithm that performs exact/narrowphase collision detection on the actual collision shapes.
-ATTRIBUTE_ALIGNED16(struct) btBroadphasePair
+ATTRIBUTE_ALIGNED16(struct)
+btBroadphasePair
 {
-	btBroadphasePair ()
-		:
-	m_pProxy0(0),
-		m_pProxy1(0),
-		m_algorithm(0),
-		m_internalInfo1(0)
+	btBroadphasePair()
+		: m_pProxy0(0),
+		  m_pProxy1(0),
+		  m_algorithm(0),
+		  m_internalInfo1(0)
 	{
 	}
 
-BT_DECLARE_ALIGNED_ALLOCATOR();
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btBroadphasePair(const btBroadphasePair& other)
-		:		m_pProxy0(other.m_pProxy0),
-				m_pProxy1(other.m_pProxy1),
-				m_algorithm(other.m_algorithm),
-				m_internalInfo1(other.m_internalInfo1)
-	{
-	}
-	btBroadphasePair(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
+	btBroadphasePair(btBroadphaseProxy & proxy0, btBroadphaseProxy & proxy1)
 	{
-
 		//keep them sorted, so the std::set operations work
 		if (proxy0.m_uniqueId < proxy1.m_uniqueId)
-        { 
-            m_pProxy0 = &proxy0; 
-            m_pProxy1 = &proxy1; 
-        }
-        else 
-        { 
-			m_pProxy0 = &proxy1; 
-            m_pProxy1 = &proxy0; 
-        }
+		{
+			m_pProxy0 = &proxy0;
+			m_pProxy1 = &proxy1;
+		}
+		else
+		{
+			m_pProxy0 = &proxy1;
+			m_pProxy1 = &proxy0;
+		}
 
 		m_algorithm = 0;
 		m_internalInfo1 = 0;
-
 	}
-	
+
 	btBroadphaseProxy* m_pProxy0;
 	btBroadphaseProxy* m_pProxy1;
-	
-	mutable btCollisionAlgorithm* m_algorithm;
-	union { void* m_internalInfo1; int m_internalTmpValue;};//don't use this data, it will be removed in future version.
 
+	mutable btCollisionAlgorithm* m_algorithm;
+	union {
+		void* m_internalInfo1;
+		int m_internalTmpValue;
+	};  //don't use this data, it will be removed in future version.
 };
 
 /*
@@ -240,31 +224,25 @@ SIMD_FORCE_INLINE bool operator<(const btBroadphasePair& a, const btBroadphasePa
 }
 */
 
-
-
 class btBroadphasePairSortPredicate
 {
-	public:
-
-		bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b ) const
-		{
-			const int uidA0 = a.m_pProxy0 ? a.m_pProxy0->m_uniqueId : -1;
-			const int uidB0 = b.m_pProxy0 ? b.m_pProxy0->m_uniqueId : -1;
-			const int uidA1 = a.m_pProxy1 ? a.m_pProxy1->m_uniqueId : -1;
-			const int uidB1 = b.m_pProxy1 ? b.m_pProxy1->m_uniqueId : -1;
-
-			 return uidA0 > uidB0 || 
-				(a.m_pProxy0 == b.m_pProxy0 && uidA1 > uidB1) ||
-				(a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm); 
-		}
+public:
+	bool operator()(const btBroadphasePair& a, const btBroadphasePair& b) const
+	{
+		const int uidA0 = a.m_pProxy0 ? a.m_pProxy0->m_uniqueId : -1;
+		const int uidB0 = b.m_pProxy0 ? b.m_pProxy0->m_uniqueId : -1;
+		const int uidA1 = a.m_pProxy1 ? a.m_pProxy1->m_uniqueId : -1;
+		const int uidB1 = b.m_pProxy1 ? b.m_pProxy1->m_uniqueId : -1;
+
+		return uidA0 > uidB0 ||
+			   (a.m_pProxy0 == b.m_pProxy0 && uidA1 > uidB1) ||
+			   (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm);
+	}
 };
 
-
-SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b) 
+SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b)
 {
-	 return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1);
+	return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1);
 }
 
-
-#endif //BT_BROADPHASE_PROXY_H
-
+#endif  //BT_BROADPHASE_PROXY_H
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp
index c95d1be0f2c..6e36d3bd730 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp
@@ -20,4 +20,3 @@ btCollisionAlgorithm::btCollisionAlgorithm(const btCollisionAlgorithmConstructio
 {
 	m_dispatcher = ci.m_dispatcher1;
 }
-
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h
index 405656236bd..b00c0b1b4bb 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h
@@ -25,57 +25,51 @@ class btManifoldResult;
 class btCollisionObject;
 struct btCollisionObjectWrapper;
 struct btDispatcherInfo;
-class	btPersistentManifold;
+class btPersistentManifold;
 
-typedef btAlignedObjectArray<btPersistentManifold*>	btManifoldArray;
+typedef btAlignedObjectArray<btPersistentManifold*> btManifoldArray;
 
 struct btCollisionAlgorithmConstructionInfo
 {
 	btCollisionAlgorithmConstructionInfo()
-		:m_dispatcher1(0),
-		m_manifold(0)
+		: m_dispatcher1(0),
+		  m_manifold(0)
 	{
 	}
-	btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp)
-		:m_dispatcher1(dispatcher)
+	btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher, int temp)
+		: m_dispatcher1(dispatcher)
 	{
 		(void)temp;
 	}
 
-	btDispatcher*	m_dispatcher1;
-	btPersistentManifold*	m_manifold;
-
-//	int	getDispatcherId();
+	btDispatcher* m_dispatcher1;
+	btPersistentManifold* m_manifold;
 
+	//	int	getDispatcherId();
 };
 
-
 ///btCollisionAlgorithm is an collision interface that is compatible with the Broadphase and btDispatcher.
 ///It is persistent over frames
 class btCollisionAlgorithm
 {
-
 protected:
-
-	btDispatcher*	m_dispatcher;
+	btDispatcher* m_dispatcher;
 
 protected:
-//	int	getDispatcherId();
-	
-public:
+	//	int	getDispatcherId();
 
-	btCollisionAlgorithm() {};
+public:
+	btCollisionAlgorithm(){};
 
 	btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci);
 
-	virtual ~btCollisionAlgorithm() {};
+	virtual ~btCollisionAlgorithm(){};
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0;
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) = 0;
 
-	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0;
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) = 0;
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray) = 0;
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray) = 0;
 };
 
-
-#endif //BT_COLLISION_ALGORITHM_H
+#endif  //BT_COLLISION_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp
index 2ca20cdd8b8..166cb04c0b0 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp
@@ -17,210 +17,226 @@ subject to the following restrictions:
 #include "btDbvt.h"
 
 //
-typedef btAlignedObjectArray<btDbvtNode*>			tNodeArray;
-typedef btAlignedObjectArray<const btDbvtNode*>	tConstNodeArray;
+typedef btAlignedObjectArray<btDbvtNode*> tNodeArray;
+typedef btAlignedObjectArray<const btDbvtNode*> tConstNodeArray;
 
 //
 struct btDbvtNodeEnumerator : btDbvt::ICollide
 {
-	tConstNodeArray	nodes;
+	tConstNodeArray nodes;
 	void Process(const btDbvtNode* n) { nodes.push_back(n); }
 };
 
 //
-static DBVT_INLINE int			indexof(const btDbvtNode* node)
+static DBVT_INLINE int indexof(const btDbvtNode* node)
 {
-	return(node->parent->childs[1]==node);
+	return (node->parent->childs[1] == node);
 }
 
 //
-static DBVT_INLINE btDbvtVolume	merge(	const btDbvtVolume& a,
+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* ptr = (btDbvtVolume*) locals;
-	btDbvtVolume&	res=*ptr;
+#ifdef BT_USE_SSE
+	ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]);
+	btDbvtVolume* ptr = (btDbvtVolume*)locals;
+	btDbvtVolume& res = *ptr;
 #else
-		btDbvtVolume	res;
+	btDbvtVolume res;
 #endif
-	Merge(a,b,res);
-	return(res);
+	Merge(a, b, res);
+	return (res);
 }
 
 // volume+edge lengths
-static DBVT_INLINE btScalar		size(const btDbvtVolume& a)
+static DBVT_INLINE btScalar size(const btDbvtVolume& a)
 {
-	const btVector3	edges=a.Lengths();
-	return(	edges.x()*edges.y()*edges.z()+
-		edges.x()+edges.y()+edges.z());
+	const btVector3 edges = a.Lengths();
+	return (edges.x() * edges.y() * edges.z() +
+			edges.x() + edges.y() + edges.z());
 }
 
 //
-static void						getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth)
+static void getmaxdepth(const btDbvtNode* node, int depth, int& maxdepth)
 {
-	if(node->isinternal())
+	if (node->isinternal())
 	{
-		getmaxdepth(node->childs[0],depth+1,maxdepth);
-		getmaxdepth(node->childs[1],depth+1,maxdepth);
-	} else maxdepth=btMax(maxdepth,depth);
+		getmaxdepth(node->childs[0], depth + 1, maxdepth);
+		getmaxdepth(node->childs[1], depth + 1, maxdepth);
+	}
+	else
+		maxdepth = btMax(maxdepth, depth);
 }
 
 //
-static DBVT_INLINE void			deletenode(	btDbvt* pdbvt,
-										   btDbvtNode* node)
+static DBVT_INLINE void deletenode(btDbvt* pdbvt,
+								   btDbvtNode* node)
 {
 	btAlignedFree(pdbvt->m_free);
-	pdbvt->m_free=node;
+	pdbvt->m_free = node;
 }
 
 //
-static void						recursedeletenode(	btDbvt* pdbvt,
-												  btDbvtNode* node)
+static void recursedeletenode(btDbvt* pdbvt,
+							  btDbvtNode* node)
 {
-	if(!node->isleaf())
+	if (node == 0) return;
+	if (!node->isleaf())
 	{
-		recursedeletenode(pdbvt,node->childs[0]);
-		recursedeletenode(pdbvt,node->childs[1]);
+		recursedeletenode(pdbvt, node->childs[0]);
+		recursedeletenode(pdbvt, node->childs[1]);
 	}
-	if(node==pdbvt->m_root) pdbvt->m_root=0;
-	deletenode(pdbvt,node);
+	if (node == pdbvt->m_root) pdbvt->m_root = 0;
+	deletenode(pdbvt, node);
 }
 
 //
-static DBVT_INLINE btDbvtNode*	createnode(	btDbvt* pdbvt,
-										   btDbvtNode* parent,
-										   void* data)
+static DBVT_INLINE btDbvtNode* createnode(btDbvt* pdbvt,
+										  btDbvtNode* parent,
+										  void* data)
 {
-	btDbvtNode*	node;
-	if(pdbvt->m_free)
-	{ node=pdbvt->m_free;pdbvt->m_free=0; }
+	btDbvtNode* node;
+	if (pdbvt->m_free)
+	{
+		node = pdbvt->m_free;
+		pdbvt->m_free = 0;
+	}
 	else
-	{ node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); }
-	node->parent	=	parent;
-	node->data		=	data;
-	node->childs[1]	=	0;
-	return(node);
+	{
+		node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
+	}
+	node->parent = parent;
+	node->data = data;
+	node->childs[1] = 0;
+	return (node);
 }
 
 //
-static DBVT_INLINE btDbvtNode*	createnode(	btDbvt* pdbvt,
-										   btDbvtNode* parent,
-										   const btDbvtVolume& volume,
-										   void* data)
+static DBVT_INLINE btDbvtNode* createnode(btDbvt* pdbvt,
+										  btDbvtNode* parent,
+										  const btDbvtVolume& volume,
+										  void* data)
 {
-	btDbvtNode*	node=createnode(pdbvt,parent,data);
-	node->volume=volume;
-	return(node);
+	btDbvtNode* node = createnode(pdbvt, parent, data);
+	node->volume = volume;
+	return (node);
 }
 
 //
-static DBVT_INLINE btDbvtNode*	createnode(	btDbvt* pdbvt,
-										   btDbvtNode* parent,
-										   const btDbvtVolume& volume0,
-										   const btDbvtVolume& volume1,
-										   void* data)
+static DBVT_INLINE btDbvtNode* createnode(btDbvt* pdbvt,
+										  btDbvtNode* parent,
+										  const btDbvtVolume& volume0,
+										  const btDbvtVolume& volume1,
+										  void* data)
 {
-	btDbvtNode*	node=createnode(pdbvt,parent,data);
-	Merge(volume0,volume1,node->volume);
-	return(node);
+	btDbvtNode* node = createnode(pdbvt, parent, data);
+	Merge(volume0, volume1, node->volume);
+	return (node);
 }
 
 //
-static void						insertleaf(	btDbvt* pdbvt,
-										   btDbvtNode* root,
-										   btDbvtNode* leaf)
+static void insertleaf(btDbvt* pdbvt,
+					   btDbvtNode* root,
+					   btDbvtNode* leaf)
 {
-	if(!pdbvt->m_root)
+	if (!pdbvt->m_root)
 	{
-		pdbvt->m_root	=	leaf;
-		leaf->parent	=	0;
+		pdbvt->m_root = leaf;
+		leaf->parent = 0;
 	}
 	else
 	{
-		if(!root->isleaf())
+		if (!root->isleaf())
 		{
-			do	{
-				root=root->childs[Select(	leaf->volume,
-					root->childs[0]->volume,
-					root->childs[1]->volume)];
-			} while(!root->isleaf());
+			do
+			{
+				root = root->childs[Select(leaf->volume,
+										   root->childs[0]->volume,
+										   root->childs[1]->volume)];
+			} while (!root->isleaf());
 		}
-		btDbvtNode*	prev=root->parent;
-		btDbvtNode*	node=createnode(pdbvt,prev,leaf->volume,root->volume,0);
-		if(prev)
+		btDbvtNode* prev = root->parent;
+		btDbvtNode* node = createnode(pdbvt, prev, leaf->volume, root->volume, 0);
+		if (prev)
 		{
-			prev->childs[indexof(root)]	=	node;
-			node->childs[0]				=	root;root->parent=node;
-			node->childs[1]				=	leaf;leaf->parent=node;
-			do	{
-				if(!prev->volume.Contain(node->volume))
-					Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
+			prev->childs[indexof(root)] = node;
+			node->childs[0] = root;
+			root->parent = node;
+			node->childs[1] = leaf;
+			leaf->parent = node;
+			do
+			{
+				if (!prev->volume.Contain(node->volume))
+					Merge(prev->childs[0]->volume, prev->childs[1]->volume, prev->volume);
 				else
 					break;
-				node=prev;
-			} while(0!=(prev=node->parent));
+				node = prev;
+			} while (0 != (prev = node->parent));
 		}
 		else
 		{
-			node->childs[0]	=	root;root->parent=node;
-			node->childs[1]	=	leaf;leaf->parent=node;
-			pdbvt->m_root	=	node;
+			node->childs[0] = root;
+			root->parent = node;
+			node->childs[1] = leaf;
+			leaf->parent = node;
+			pdbvt->m_root = node;
 		}
 	}
 }
 
 //
-static btDbvtNode*				removeleaf(	btDbvt* pdbvt,
-										   btDbvtNode* leaf)
+static btDbvtNode* removeleaf(btDbvt* pdbvt,
+							  btDbvtNode* leaf)
 {
-	if(leaf==pdbvt->m_root)
+	if (leaf == pdbvt->m_root)
 	{
-		pdbvt->m_root=0;
-		return(0);
+		pdbvt->m_root = 0;
+		return (0);
 	}
 	else
 	{
-		btDbvtNode*	parent=leaf->parent;
-		btDbvtNode*	prev=parent->parent;
-		btDbvtNode*	sibling=parent->childs[1-indexof(leaf)];			
-		if(prev)
+		btDbvtNode* parent = leaf->parent;
+		btDbvtNode* prev = parent->parent;
+		btDbvtNode* sibling = parent->childs[1 - indexof(leaf)];
+		if (prev)
 		{
-			prev->childs[indexof(parent)]=sibling;
-			sibling->parent=prev;
-			deletenode(pdbvt,parent);
-			while(prev)
+			prev->childs[indexof(parent)] = sibling;
+			sibling->parent = prev;
+			deletenode(pdbvt, parent);
+			while (prev)
 			{
-				const btDbvtVolume	pb=prev->volume;
-				Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
-				if(NotEqual(pb,prev->volume))
+				const btDbvtVolume pb = prev->volume;
+				Merge(prev->childs[0]->volume, prev->childs[1]->volume, prev->volume);
+				if (NotEqual(pb, prev->volume))
 				{
-					prev=prev->parent;
-				} else break;
+					prev = prev->parent;
+				}
+				else
+					break;
 			}
-			return(prev?prev:pdbvt->m_root);
+			return (prev ? prev : pdbvt->m_root);
 		}
 		else
-		{								
-			pdbvt->m_root=sibling;
-			sibling->parent=0;
-			deletenode(pdbvt,parent);
-			return(pdbvt->m_root);
-		}			
+		{
+			pdbvt->m_root = sibling;
+			sibling->parent = 0;
+			deletenode(pdbvt, parent);
+			return (pdbvt->m_root);
+		}
 	}
 }
 
 //
-static void						fetchleaves(btDbvt* pdbvt,
-											btDbvtNode* root,
-											tNodeArray& leaves,
-											int depth=-1)
+static void fetchleaves(btDbvt* pdbvt,
+						btDbvtNode* root,
+						tNodeArray& leaves,
+						int depth = -1)
 {
-	if(root->isinternal()&&depth)
+	if (root->isinternal() && depth)
 	{
-		fetchleaves(pdbvt,root->childs[0],leaves,depth-1);
-		fetchleaves(pdbvt,root->childs[1],leaves,depth-1);
-		deletenode(pdbvt,root);
+		fetchleaves(pdbvt, root->childs[0], leaves, depth - 1);
+		fetchleaves(pdbvt, root->childs[1], leaves, depth - 1);
+		deletenode(pdbvt, root);
 	}
 	else
 	{
@@ -229,170 +245,204 @@ static void						fetchleaves(btDbvt* pdbvt,
 }
 
 //
-static void						split(	const tNodeArray& leaves,
-									  tNodeArray& left,
-									  tNodeArray& right,
-									  const btVector3& org,
-									  const btVector3& axis)
+static bool leftOfAxis(const btDbvtNode* node,
+					   const btVector3& org,
+					   const btVector3& axis)
 {
-	left.resize(0);
-	right.resize(0);
-	for(int i=0,ni=leaves.size();i<ni;++i)
+	return btDot(axis, node->volume.Center() - org) <= 0;
+}
+
+// Partitions leaves such that leaves[0, n) are on the
+// left of axis, and leaves[n, count) are on the right
+// of axis. returns N.
+static int split(btDbvtNode** leaves,
+				 int count,
+				 const btVector3& org,
+				 const btVector3& axis)
+{
+	int begin = 0;
+	int end = count;
+	for (;;)
 	{
-		if(btDot(axis,leaves[i]->volume.Center()-org)<0)
-			left.push_back(leaves[i]);
-		else
-			right.push_back(leaves[i]);
+		while (begin != end && leftOfAxis(leaves[begin], org, axis))
+		{
+			++begin;
+		}
+
+		if (begin == end)
+		{
+			break;
+		}
+
+		while (begin != end && !leftOfAxis(leaves[end - 1], org, axis))
+		{
+			--end;
+		}
+
+		if (begin == end)
+		{
+			break;
+		}
+
+		// swap out of place nodes
+		--end;
+		btDbvtNode* temp = leaves[begin];
+		leaves[begin] = leaves[end];
+		leaves[end] = temp;
+		++begin;
 	}
+
+	return begin;
 }
 
 //
-static btDbvtVolume				bounds(	const tNodeArray& leaves)
+static btDbvtVolume bounds(btDbvtNode** leaves,
+						   int count)
 {
-#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
-	ATTRIBUTE_ALIGNED16(char	locals[sizeof(btDbvtVolume)]);
-	btDbvtVolume* ptr = (btDbvtVolume*) locals;
-	btDbvtVolume&	volume=*ptr;
-	volume=leaves[0]->volume;
+#ifdef BT_USE_SSE
+	ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]);
+	btDbvtVolume* ptr = (btDbvtVolume*)locals;
+	btDbvtVolume& volume = *ptr;
+	volume = leaves[0]->volume;
 #else
-	btDbvtVolume volume=leaves[0]->volume;
+	btDbvtVolume volume = leaves[0]->volume;
 #endif
-	for(int i=1,ni=leaves.size();i<ni;++i)
+	for (int i = 1, ni = count; i < ni; ++i)
 	{
-		Merge(volume,leaves[i]->volume,volume);
+		Merge(volume, leaves[i]->volume, volume);
 	}
-	return(volume);
+	return (volume);
 }
 
 //
-static void						bottomup(	btDbvt* pdbvt,
-										 tNodeArray& leaves)
+static void bottomup(btDbvt* pdbvt,
+					 btDbvtNode** leaves,
+					 int count)
 {
-	while(leaves.size()>1)
+	while (count > 1)
 	{
-		btScalar	minsize=SIMD_INFINITY;
-		int			minidx[2]={-1,-1};
-		for(int i=0;i<leaves.size();++i)
+		btScalar minsize = SIMD_INFINITY;
+		int minidx[2] = {-1, -1};
+		for (int i = 0; i < count; ++i)
 		{
-			for(int j=i+1;j<leaves.size();++j)
+			for (int j = i + 1; j < count; ++j)
 			{
-				const btScalar	sz=size(merge(leaves[i]->volume,leaves[j]->volume));
-				if(sz<minsize)
+				const btScalar sz = size(merge(leaves[i]->volume, leaves[j]->volume));
+				if (sz < minsize)
 				{
-					minsize		=	sz;
-					minidx[0]	=	i;
-					minidx[1]	=	j;
+					minsize = sz;
+					minidx[0] = i;
+					minidx[1] = j;
 				}
 			}
 		}
-		btDbvtNode*	n[]	=	{leaves[minidx[0]],leaves[minidx[1]]};
-		btDbvtNode*	p	=	createnode(pdbvt,0,n[0]->volume,n[1]->volume,0);
-		p->childs[0]		=	n[0];
-		p->childs[1]		=	n[1];
-		n[0]->parent		=	p;
-		n[1]->parent		=	p;
-		leaves[minidx[0]]	=	p;
-		leaves.swap(minidx[1],leaves.size()-1);
-		leaves.pop_back();
+		btDbvtNode* n[] = {leaves[minidx[0]], leaves[minidx[1]]};
+		btDbvtNode* p = createnode(pdbvt, 0, n[0]->volume, n[1]->volume, 0);
+		p->childs[0] = n[0];
+		p->childs[1] = n[1];
+		n[0]->parent = p;
+		n[1]->parent = p;
+		leaves[minidx[0]] = p;
+		leaves[minidx[1]] = leaves[count - 1];
+		--count;
 	}
 }
 
 //
-static btDbvtNode*			topdown(btDbvt* pdbvt,
-									tNodeArray& leaves,
-									int bu_treshold)
+static btDbvtNode* topdown(btDbvt* pdbvt,
+						   btDbvtNode** leaves,
+						   int count,
+						   int bu_treshold)
 {
-	static const btVector3	axis[]={btVector3(1,0,0),
-		btVector3(0,1,0),
-		btVector3(0,0,1)};
-	if(leaves.size()>1)
+	static const btVector3 axis[] = {btVector3(1, 0, 0),
+									 btVector3(0, 1, 0),
+									 btVector3(0, 0, 1)};
+	btAssert(bu_treshold > 2);
+	if (count > 1)
 	{
-		if(leaves.size()>bu_treshold)
+		if (count > bu_treshold)
 		{
-			const btDbvtVolume	vol=bounds(leaves);
-			const btVector3			org=vol.Center();
-			tNodeArray				sets[2];
-			int						bestaxis=-1;
-			int						bestmidp=leaves.size();
-			int						splitcount[3][2]={{0,0},{0,0},{0,0}};
+			const btDbvtVolume vol = bounds(leaves, count);
+			const btVector3 org = vol.Center();
+			int partition;
+			int bestaxis = -1;
+			int bestmidp = count;
+			int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
 			int i;
-			for( i=0;i<leaves.size();++i)
+			for (i = 0; i < count; ++i)
 			{
-				const btVector3	x=leaves[i]->volume.Center()-org;
-				for(int j=0;j<3;++j)
+				const btVector3 x = leaves[i]->volume.Center() - org;
+				for (int j = 0; j < 3; ++j)
 				{
-					++splitcount[j][btDot(x,axis[j])>0?1:0];
+					++splitcount[j][btDot(x, axis[j]) > 0 ? 1 : 0];
 				}
 			}
-			for( i=0;i<3;++i)
+			for (i = 0; i < 3; ++i)
 			{
-				if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
+				if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0))
 				{
-					const int	midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1]));
-					if(midp<bestmidp)
+					const int midp = (int)btFabs(btScalar(splitcount[i][0] - splitcount[i][1]));
+					if (midp < bestmidp)
 					{
-						bestaxis=i;
-						bestmidp=midp;
+						bestaxis = i;
+						bestmidp = midp;
 					}
 				}
 			}
-			if(bestaxis>=0)
+			if (bestaxis >= 0)
 			{
-				sets[0].reserve(splitcount[bestaxis][0]);
-				sets[1].reserve(splitcount[bestaxis][1]);
-				split(leaves,sets[0],sets[1],org,axis[bestaxis]);
+				partition = split(leaves, count, org, axis[bestaxis]);
+				btAssert(partition != 0 && partition != count);
 			}
 			else
 			{
-				sets[0].reserve(leaves.size()/2+1);
-				sets[1].reserve(leaves.size()/2);
-				for(int i=0,ni=leaves.size();i<ni;++i)
-				{
-					sets[i&1].push_back(leaves[i]);
-				}
+				partition = count / 2 + 1;
 			}
-			btDbvtNode*	node=createnode(pdbvt,0,vol,0);
-			node->childs[0]=topdown(pdbvt,sets[0],bu_treshold);
-			node->childs[1]=topdown(pdbvt,sets[1],bu_treshold);
-			node->childs[0]->parent=node;
-			node->childs[1]->parent=node;
-			return(node);
+			btDbvtNode* node = createnode(pdbvt, 0, vol, 0);
+			node->childs[0] = topdown(pdbvt, &leaves[0], partition, bu_treshold);
+			node->childs[1] = topdown(pdbvt, &leaves[partition], count - partition, bu_treshold);
+			node->childs[0]->parent = node;
+			node->childs[1]->parent = node;
+			return (node);
 		}
 		else
 		{
-			bottomup(pdbvt,leaves);
-			return(leaves[0]);
+			bottomup(pdbvt, leaves, count);
+			return (leaves[0]);
 		}
 	}
-	return(leaves[0]);
+	return (leaves[0]);
 }
 
 //
-static DBVT_INLINE btDbvtNode*	sort(btDbvtNode* n,btDbvtNode*& r)
+static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n, btDbvtNode*& r)
 {
-	btDbvtNode*	p=n->parent;
+	btDbvtNode* p = n->parent;
 	btAssert(n->isinternal());
-	if(p>n)
+	if (p > n)
 	{
-		const int		i=indexof(n);
-		const int		j=1-i;
-		btDbvtNode*	s=p->childs[j];
-		btDbvtNode*	q=p->parent;
-		btAssert(n==p->childs[i]);
-		if(q) q->childs[indexof(p)]=n; else r=n;
-		s->parent=n;
-		p->parent=n;
-		n->parent=q;
-		p->childs[0]=n->childs[0];
-		p->childs[1]=n->childs[1];
-		n->childs[0]->parent=p;
-		n->childs[1]->parent=p;
-		n->childs[i]=p;
-		n->childs[j]=s;
-		btSwap(p->volume,n->volume);
-		return(p);
+		const int i = indexof(n);
+		const int j = 1 - i;
+		btDbvtNode* s = p->childs[j];
+		btDbvtNode* q = p->parent;
+		btAssert(n == p->childs[i]);
+		if (q)
+			q->childs[indexof(p)] = n;
+		else
+			r = n;
+		s->parent = n;
+		p->parent = n;
+		n->parent = q;
+		p->childs[0] = n->childs[0];
+		p->childs[1] = n->childs[1];
+		n->childs[0]->parent = p;
+		n->childs[1]->parent = p;
+		n->childs[i] = p;
+		n->childs[j] = s;
+		btSwap(p->volume, n->volume);
+		return (p);
 	}
-	return(n);
+	return (n);
 }
 
 #if 0
@@ -410,11 +460,11 @@ static DBVT_INLINE btDbvtNode*	walkup(btDbvtNode* n,int count)
 //
 btDbvt::btDbvt()
 {
-	m_root		=	0;
-	m_free		=	0;
-	m_lkhd		=	-1;
-	m_leaves	=	0;
-	m_opath		=	0;
+	m_root = 0;
+	m_free = 0;
+	m_lkhd = -1;
+	m_leaves = 0;
+	m_opath = 0;
 }
 
 //
@@ -424,228 +474,233 @@ btDbvt::~btDbvt()
 }
 
 //
-void			btDbvt::clear()
+void btDbvt::clear()
 {
-	if(m_root)	
-		recursedeletenode(this,m_root);
+	if (m_root)
+		recursedeletenode(this, m_root);
 	btAlignedFree(m_free);
-	m_free=0;
-	m_lkhd		=	-1;
+	m_free = 0;
+	m_lkhd = -1;
 	m_stkStack.clear();
-	m_opath		=	0;
-	
+	m_opath = 0;
 }
 
 //
-void			btDbvt::optimizeBottomUp()
+void btDbvt::optimizeBottomUp()
 {
-	if(m_root)
+	if (m_root)
 	{
 		tNodeArray leaves;
 		leaves.reserve(m_leaves);
-		fetchleaves(this,m_root,leaves);
-		bottomup(this,leaves);
-		m_root=leaves[0];
+		fetchleaves(this, m_root, leaves);
+		bottomup(this, &leaves[0], leaves.size());
+		m_root = leaves[0];
 	}
 }
 
 //
-void			btDbvt::optimizeTopDown(int bu_treshold)
+void btDbvt::optimizeTopDown(int bu_treshold)
 {
-	if(m_root)
+	if (m_root)
 	{
-		tNodeArray	leaves;
+		tNodeArray leaves;
 		leaves.reserve(m_leaves);
-		fetchleaves(this,m_root,leaves);
-		m_root=topdown(this,leaves,bu_treshold);
+		fetchleaves(this, m_root, leaves);
+		m_root = topdown(this, &leaves[0], leaves.size(), bu_treshold);
 	}
 }
 
 //
-void			btDbvt::optimizeIncremental(int passes)
+void btDbvt::optimizeIncremental(int passes)
 {
-	if(passes<0) passes=m_leaves;
-	if(m_root&&(passes>0))
+	if (passes < 0) passes = m_leaves;
+	if (m_root && (passes > 0))
 	{
-		do	{
-			btDbvtNode*		node=m_root;
-			unsigned	bit=0;
-			while(node->isinternal())
+		do
+		{
+			btDbvtNode* node = m_root;
+			unsigned bit = 0;
+			while (node->isinternal())
 			{
-				node=sort(node,m_root)->childs[(m_opath>>bit)&1];
-				bit=(bit+1)&(sizeof(unsigned)*8-1);
+				node = sort(node, m_root)->childs[(m_opath >> bit) & 1];
+				bit = (bit + 1) & (sizeof(unsigned) * 8 - 1);
 			}
 			update(node);
 			++m_opath;
-		} while(--passes);
+		} while (--passes);
 	}
 }
 
 //
-btDbvtNode*	btDbvt::insert(const btDbvtVolume& volume,void* data)
+btDbvtNode* btDbvt::insert(const btDbvtVolume& volume, void* data)
 {
-	btDbvtNode*	leaf=createnode(this,0,volume,data);
-	insertleaf(this,m_root,leaf);
+	btDbvtNode* leaf = createnode(this, 0, volume, data);
+	insertleaf(this, m_root, leaf);
 	++m_leaves;
-	return(leaf);
+	return (leaf);
 }
 
 //
-void			btDbvt::update(btDbvtNode* leaf,int lookahead)
+void btDbvt::update(btDbvtNode* leaf, int lookahead)
 {
-	btDbvtNode*	root=removeleaf(this,leaf);
-	if(root)
+	btDbvtNode* root = removeleaf(this, leaf);
+	if (root)
 	{
-		if(lookahead>=0)
+		if (lookahead >= 0)
 		{
-			for(int i=0;(i<lookahead)&&root->parent;++i)
+			for (int i = 0; (i < lookahead) && root->parent; ++i)
 			{
-				root=root->parent;
+				root = root->parent;
 			}
-		} else root=m_root;
+		}
+		else
+			root = m_root;
 	}
-	insertleaf(this,root,leaf);
+	insertleaf(this, root, leaf);
 }
 
 //
-void			btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume)
+void btDbvt::update(btDbvtNode* leaf, btDbvtVolume& volume)
 {
-	btDbvtNode*	root=removeleaf(this,leaf);
-	if(root)
+	btDbvtNode* root = removeleaf(this, leaf);
+	if (root)
 	{
-		if(m_lkhd>=0)
+		if (m_lkhd >= 0)
 		{
-			for(int i=0;(i<m_lkhd)&&root->parent;++i)
+			for (int i = 0; (i < m_lkhd) && root->parent; ++i)
 			{
-				root=root->parent;
+				root = root->parent;
 			}
-		} else root=m_root;
+		}
+		else
+			root = m_root;
 	}
-	leaf->volume=volume;
-	insertleaf(this,root,leaf);
+	leaf->volume = volume;
+	insertleaf(this, root, leaf);
 }
 
 //
-bool			btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin)
+bool btDbvt::update(btDbvtNode* leaf, btDbvtVolume& volume, const btVector3& velocity, btScalar margin)
 {
-	if(leaf->volume.Contain(volume)) return(false);
-	volume.Expand(btVector3(margin,margin,margin));
+	if (leaf->volume.Contain(volume)) return (false);
+	volume.Expand(btVector3(margin, margin, margin));
 	volume.SignedExpand(velocity);
-	update(leaf,volume);
-	return(true);
+	update(leaf, volume);
+	return (true);
 }
 
 //
-bool			btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity)
+bool btDbvt::update(btDbvtNode* leaf, btDbvtVolume& volume, const btVector3& velocity)
 {
-	if(leaf->volume.Contain(volume)) return(false);
+	if (leaf->volume.Contain(volume)) return (false);
 	volume.SignedExpand(velocity);
-	update(leaf,volume);
-	return(true);
+	update(leaf, volume);
+	return (true);
 }
 
 //
-bool			btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin)
+bool btDbvt::update(btDbvtNode* leaf, btDbvtVolume& volume, btScalar margin)
 {
-	if(leaf->volume.Contain(volume)) return(false);
-	volume.Expand(btVector3(margin,margin,margin));
-	update(leaf,volume);
-	return(true);
+	if (leaf->volume.Contain(volume)) return (false);
+	volume.Expand(btVector3(margin, margin, margin));
+	update(leaf, volume);
+	return (true);
 }
 
 //
-void			btDbvt::remove(btDbvtNode* leaf)
+void btDbvt::remove(btDbvtNode* leaf)
 {
-	removeleaf(this,leaf);
-	deletenode(this,leaf);
+	removeleaf(this, leaf);
+	deletenode(this, leaf);
 	--m_leaves;
 }
 
 //
-void			btDbvt::write(IWriter* iwriter) const
+void btDbvt::write(IWriter* iwriter) const
 {
-	btDbvtNodeEnumerator	nodes;
-	nodes.nodes.reserve(m_leaves*2);
-	enumNodes(m_root,nodes);
-	iwriter->Prepare(m_root,nodes.nodes.size());
-	for(int i=0;i<nodes.nodes.size();++i)
+	btDbvtNodeEnumerator nodes;
+	nodes.nodes.reserve(m_leaves * 2);
+	enumNodes(m_root, nodes);
+	iwriter->Prepare(m_root, nodes.nodes.size());
+	for (int i = 0; i < nodes.nodes.size(); ++i)
 	{
-		const btDbvtNode* n=nodes.nodes[i];
-		int			p=-1;
-		if(n->parent) p=nodes.nodes.findLinearSearch(n->parent);
-		if(n->isinternal())
+		const btDbvtNode* n = nodes.nodes[i];
+		int p = -1;
+		if (n->parent) p = nodes.nodes.findLinearSearch(n->parent);
+		if (n->isinternal())
 		{
-			const int	c0=nodes.nodes.findLinearSearch(n->childs[0]);
-			const int	c1=nodes.nodes.findLinearSearch(n->childs[1]);
-			iwriter->WriteNode(n,i,p,c0,c1);
+			const int c0 = nodes.nodes.findLinearSearch(n->childs[0]);
+			const int c1 = nodes.nodes.findLinearSearch(n->childs[1]);
+			iwriter->WriteNode(n, i, p, c0, c1);
 		}
 		else
 		{
-			iwriter->WriteLeaf(n,i,p);
-		}	
+			iwriter->WriteLeaf(n, i, p);
+		}
 	}
 }
 
 //
-void			btDbvt::clone(btDbvt& dest,IClone* iclone) const
+void btDbvt::clone(btDbvt& dest, IClone* iclone) const
 {
 	dest.clear();
-	if(m_root!=0)
-	{	
-		btAlignedObjectArray<sStkCLN>	stack;
+	if (m_root != 0)
+	{
+		btAlignedObjectArray<sStkCLN> stack;
 		stack.reserve(m_leaves);
-		stack.push_back(sStkCLN(m_root,0));
-		do	{
-			const int		i=stack.size()-1;
-			const sStkCLN	e=stack[i];
-			btDbvtNode*			n=createnode(&dest,e.parent,e.node->volume,e.node->data);
+		stack.push_back(sStkCLN(m_root, 0));
+		do
+		{
+			const int i = stack.size() - 1;
+			const sStkCLN e = stack[i];
+			btDbvtNode* n = createnode(&dest, e.parent, e.node->volume, e.node->data);
 			stack.pop_back();
-			if(e.parent!=0)
-				e.parent->childs[i&1]=n;
+			if (e.parent != 0)
+				e.parent->childs[i & 1] = n;
 			else
-				dest.m_root=n;
-			if(e.node->isinternal())
+				dest.m_root = n;
+			if (e.node->isinternal())
 			{
-				stack.push_back(sStkCLN(e.node->childs[0],n));
-				stack.push_back(sStkCLN(e.node->childs[1],n));
+				stack.push_back(sStkCLN(e.node->childs[0], n));
+				stack.push_back(sStkCLN(e.node->childs[1], n));
 			}
 			else
 			{
 				iclone->CloneLeaf(n);
 			}
-		} while(stack.size()>0);
+		} while (stack.size() > 0);
 	}
 }
 
 //
-int				btDbvt::maxdepth(const btDbvtNode* node)
+int btDbvt::maxdepth(const btDbvtNode* node)
 {
-	int	depth=0;
-	if(node) getmaxdepth(node,1,depth);
-	return(depth);
+	int depth = 0;
+	if (node) getmaxdepth(node, 1, depth);
+	return (depth);
 }
 
 //
-int				btDbvt::countLeaves(const btDbvtNode* node)
+int btDbvt::countLeaves(const btDbvtNode* node)
 {
-	if(node->isinternal())
-		return(countLeaves(node->childs[0])+countLeaves(node->childs[1]));
+	if (node->isinternal())
+		return (countLeaves(node->childs[0]) + countLeaves(node->childs[1]));
 	else
-		return(1);
+		return (1);
 }
 
 //
-void			btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves)
+void btDbvt::extractLeaves(const btDbvtNode* node, btAlignedObjectArray<const btDbvtNode*>& leaves)
 {
-	if(node->isinternal())
+	if (node->isinternal())
 	{
-		extractLeaves(node->childs[0],leaves);
-		extractLeaves(node->childs[1],leaves);
+		extractLeaves(node->childs[0], leaves);
+		extractLeaves(node->childs[1], leaves);
 	}
 	else
 	{
 		leaves.push_back(node);
-	}	
+	}
 }
 
 //
@@ -694,603 +749,608 @@ struct btDbvtBenchmark
 {
 	struct NilPolicy : btDbvt::ICollide
 	{
-		NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true)		{}
-		void	Process(const btDbvtNode*,const btDbvtNode*)				{ ++m_pcount; }
-		void	Process(const btDbvtNode*)									{ ++m_pcount; }
-		void	Process(const btDbvtNode*,btScalar depth)
+		NilPolicy() : m_pcount(0), m_depth(-SIMD_INFINITY), m_checksort(true) {}
+		void Process(const btDbvtNode*, const btDbvtNode*) { ++m_pcount; }
+		void Process(const btDbvtNode*) { ++m_pcount; }
+		void Process(const btDbvtNode*, btScalar depth)
 		{
 			++m_pcount;
-			if(m_checksort)
-			{ if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); }
+			if (m_checksort)
+			{
+				if (depth >= m_depth)
+					m_depth = depth;
+				else
+					printf("wrong depth: %f (should be >= %f)\r\n", depth, m_depth);
+			}
 		}
-		int			m_pcount;
-		btScalar	m_depth;
-		bool		m_checksort;
+		int m_pcount;
+		btScalar m_depth;
+		bool m_checksort;
 	};
 	struct P14 : btDbvt::ICollide
 	{
 		struct Node
 		{
-			const btDbvtNode*	leaf;
-			btScalar			depth;
+			const btDbvtNode* leaf;
+			btScalar depth;
 		};
-		void Process(const btDbvtNode* leaf,btScalar depth)
+		void Process(const btDbvtNode* leaf, btScalar depth)
 		{
-			Node	n;
-			n.leaf	=	leaf;
-			n.depth	=	depth;
+			Node n;
+			n.leaf = leaf;
+			n.depth = depth;
 		}
-		static int sortfnc(const Node& a,const Node& b)
+		static int sortfnc(const Node& a, const Node& b)
 		{
-			if(a.depth<b.depth) return(+1);
-			if(a.depth>b.depth) return(-1);
-			return(0);
+			if (a.depth < b.depth) return (+1);
+			if (a.depth > b.depth) return (-1);
+			return (0);
 		}
-		btAlignedObjectArray<Node>		m_nodes;
+		btAlignedObjectArray<Node> m_nodes;
 	};
 	struct P15 : btDbvt::ICollide
 	{
 		struct Node
 		{
-			const btDbvtNode*	leaf;
-			btScalar			depth;
+			const btDbvtNode* leaf;
+			btScalar depth;
 		};
 		void Process(const btDbvtNode* leaf)
 		{
-			Node	n;
-			n.leaf	=	leaf;
-			n.depth	=	dot(leaf->volume.Center(),m_axis);
+			Node n;
+			n.leaf = leaf;
+			n.depth = dot(leaf->volume.Center(), m_axis);
 		}
-		static int sortfnc(const Node& a,const Node& b)
+		static int sortfnc(const Node& a, const Node& b)
 		{
-			if(a.depth<b.depth) return(+1);
-			if(a.depth>b.depth) return(-1);
-			return(0);
+			if (a.depth < b.depth) return (+1);
+			if (a.depth > b.depth) return (-1);
+			return (0);
 		}
-		btAlignedObjectArray<Node>		m_nodes;
-		btVector3						m_axis;
+		btAlignedObjectArray<Node> m_nodes;
+		btVector3 m_axis;
 	};
-	static btScalar			RandUnit()
+	static btScalar RandUnit()
 	{
-		return(rand()/(btScalar)RAND_MAX);
+		return (rand() / (btScalar)RAND_MAX);
 	}
-	static btVector3		RandVector3()
+	static btVector3 RandVector3()
 	{
-		return(btVector3(RandUnit(),RandUnit(),RandUnit()));
+		return (btVector3(RandUnit(), RandUnit(), RandUnit()));
 	}
-	static btVector3		RandVector3(btScalar cs)
+	static btVector3 RandVector3(btScalar cs)
 	{
-		return(RandVector3()*cs-btVector3(cs,cs,cs)/2);
+		return (RandVector3() * cs - btVector3(cs, cs, cs) / 2);
 	}
-	static btDbvtVolume	RandVolume(btScalar cs,btScalar eb,btScalar es)
+	static btDbvtVolume RandVolume(btScalar cs, btScalar eb, btScalar es)
 	{
-		return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es));
+		return (btDbvtVolume::FromCE(RandVector3(cs), btVector3(eb, eb, eb) + RandVector3() * es));
 	}
-	static btTransform		RandTransform(btScalar cs)
+	static btTransform RandTransform(btScalar cs)
 	{
-		btTransform	t;
+		btTransform t;
 		t.setOrigin(RandVector3(cs));
-		t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized());
-		return(t);
+		t.setRotation(btQuaternion(RandUnit() * SIMD_PI * 2, RandUnit() * SIMD_PI * 2, RandUnit() * SIMD_PI * 2).normalized());
+		return (t);
 	}
-	static void				RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt)
+	static void RandTree(btScalar cs, btScalar eb, btScalar es, int leaves, btDbvt& dbvt)
 	{
 		dbvt.clear();
-		for(int i=0;i<leaves;++i)
+		for (int i = 0; i < leaves; ++i)
 		{
-			dbvt.insert(RandVolume(cs,eb,es),0);
+			dbvt.insert(RandVolume(cs, eb, es), 0);
 		}
 	}
 };
 
-void			btDbvt::benchmark()
+void btDbvt::benchmark()
 {
-	static const btScalar	cfgVolumeCenterScale		=	100;
-	static const btScalar	cfgVolumeExentsBase			=	1;
-	static const btScalar	cfgVolumeExentsScale		=	4;
-	static const int		cfgLeaves					=	8192;
-	static const bool		cfgEnable					=	true;
+	static const btScalar cfgVolumeCenterScale = 100;
+	static const btScalar cfgVolumeExentsBase = 1;
+	static const btScalar cfgVolumeExentsScale = 4;
+	static const int cfgLeaves = 8192;
+	static const bool cfgEnable = true;
 
 	//[1] btDbvtVolume intersections
-	bool					cfgBenchmark1_Enable		=	cfgEnable;
-	static const int		cfgBenchmark1_Iterations	=	8;
-	static const int		cfgBenchmark1_Reference		=	3499;
+	bool cfgBenchmark1_Enable = cfgEnable;
+	static const int cfgBenchmark1_Iterations = 8;
+	static const int cfgBenchmark1_Reference = 3499;
 	//[2] btDbvtVolume merges
-	bool					cfgBenchmark2_Enable		=	cfgEnable;
-	static const int		cfgBenchmark2_Iterations	=	4;
-	static const int		cfgBenchmark2_Reference		=	1945;
+	bool cfgBenchmark2_Enable = cfgEnable;
+	static const int cfgBenchmark2_Iterations = 4;
+	static const int cfgBenchmark2_Reference = 1945;
 	//[3] btDbvt::collideTT
-	bool					cfgBenchmark3_Enable		=	cfgEnable;
-	static const int		cfgBenchmark3_Iterations	=	512;
-	static const int		cfgBenchmark3_Reference		=	5485;
+	bool cfgBenchmark3_Enable = cfgEnable;
+	static const int cfgBenchmark3_Iterations = 512;
+	static const int cfgBenchmark3_Reference = 5485;
 	//[4] btDbvt::collideTT self
-	bool					cfgBenchmark4_Enable		=	cfgEnable;
-	static const int		cfgBenchmark4_Iterations	=	512;
-	static const int		cfgBenchmark4_Reference		=	2814;
+	bool cfgBenchmark4_Enable = cfgEnable;
+	static const int cfgBenchmark4_Iterations = 512;
+	static const int cfgBenchmark4_Reference = 2814;
 	//[5] btDbvt::collideTT xform
-	bool					cfgBenchmark5_Enable		=	cfgEnable;
-	static const int		cfgBenchmark5_Iterations	=	512;
-	static const btScalar	cfgBenchmark5_OffsetScale	=	2;
-	static const int		cfgBenchmark5_Reference		=	7379;
+	bool cfgBenchmark5_Enable = cfgEnable;
+	static const int cfgBenchmark5_Iterations = 512;
+	static const btScalar cfgBenchmark5_OffsetScale = 2;
+	static const int cfgBenchmark5_Reference = 7379;
 	//[6] btDbvt::collideTT xform,self
-	bool					cfgBenchmark6_Enable		=	cfgEnable;
-	static const int		cfgBenchmark6_Iterations	=	512;
-	static const btScalar	cfgBenchmark6_OffsetScale	=	2;
-	static const int		cfgBenchmark6_Reference		=	7270;
+	bool cfgBenchmark6_Enable = cfgEnable;
+	static const int cfgBenchmark6_Iterations = 512;
+	static const btScalar cfgBenchmark6_OffsetScale = 2;
+	static const int cfgBenchmark6_Reference = 7270;
 	//[7] btDbvt::rayTest
-	bool					cfgBenchmark7_Enable		=	cfgEnable;
-	static const int		cfgBenchmark7_Passes		=	32;
-	static const int		cfgBenchmark7_Iterations	=	65536;
-	static const int		cfgBenchmark7_Reference		=	6307;
+	bool cfgBenchmark7_Enable = cfgEnable;
+	static const int cfgBenchmark7_Passes = 32;
+	static const int cfgBenchmark7_Iterations = 65536;
+	static const int cfgBenchmark7_Reference = 6307;
 	//[8] insert/remove
-	bool					cfgBenchmark8_Enable		=	cfgEnable;
-	static const int		cfgBenchmark8_Passes		=	32;
-	static const int		cfgBenchmark8_Iterations	=	65536;
-	static const int		cfgBenchmark8_Reference		=	2105;
+	bool cfgBenchmark8_Enable = cfgEnable;
+	static const int cfgBenchmark8_Passes = 32;
+	static const int cfgBenchmark8_Iterations = 65536;
+	static const int cfgBenchmark8_Reference = 2105;
 	//[9] updates (teleport)
-	bool					cfgBenchmark9_Enable		=	cfgEnable;
-	static const int		cfgBenchmark9_Passes		=	32;
-	static const int		cfgBenchmark9_Iterations	=	65536;
-	static const int		cfgBenchmark9_Reference		=	1879;
+	bool cfgBenchmark9_Enable = cfgEnable;
+	static const int cfgBenchmark9_Passes = 32;
+	static const int cfgBenchmark9_Iterations = 65536;
+	static const int cfgBenchmark9_Reference = 1879;
 	//[10] updates (jitter)
-	bool					cfgBenchmark10_Enable		=	cfgEnable;
-	static const btScalar	cfgBenchmark10_Scale		=	cfgVolumeCenterScale/10000;
-	static const int		cfgBenchmark10_Passes		=	32;
-	static const int		cfgBenchmark10_Iterations	=	65536;
-	static const int		cfgBenchmark10_Reference	=	1244;
+	bool cfgBenchmark10_Enable = cfgEnable;
+	static const btScalar cfgBenchmark10_Scale = cfgVolumeCenterScale / 10000;
+	static const int cfgBenchmark10_Passes = 32;
+	static const int cfgBenchmark10_Iterations = 65536;
+	static const int cfgBenchmark10_Reference = 1244;
 	//[11] optimize (incremental)
-	bool					cfgBenchmark11_Enable		=	cfgEnable;
-	static const int		cfgBenchmark11_Passes		=	64;
-	static const int		cfgBenchmark11_Iterations	=	65536;
-	static const int		cfgBenchmark11_Reference	=	2510;
+	bool cfgBenchmark11_Enable = cfgEnable;
+	static const int cfgBenchmark11_Passes = 64;
+	static const int cfgBenchmark11_Iterations = 65536;
+	static const int cfgBenchmark11_Reference = 2510;
 	//[12] btDbvtVolume notequal
-	bool					cfgBenchmark12_Enable		=	cfgEnable;
-	static const int		cfgBenchmark12_Iterations	=	32;
-	static const int		cfgBenchmark12_Reference	=	3677;
+	bool cfgBenchmark12_Enable = cfgEnable;
+	static const int cfgBenchmark12_Iterations = 32;
+	static const int cfgBenchmark12_Reference = 3677;
 	//[13] culling(OCL+fullsort)
-	bool					cfgBenchmark13_Enable		=	cfgEnable;
-	static const int		cfgBenchmark13_Iterations	=	1024;
-	static const int		cfgBenchmark13_Reference	=	2231;
+	bool cfgBenchmark13_Enable = cfgEnable;
+	static const int cfgBenchmark13_Iterations = 1024;
+	static const int cfgBenchmark13_Reference = 2231;
 	//[14] culling(OCL+qsort)
-	bool					cfgBenchmark14_Enable		=	cfgEnable;
-	static const int		cfgBenchmark14_Iterations	=	8192;
-	static const int		cfgBenchmark14_Reference	=	3500;
+	bool cfgBenchmark14_Enable = cfgEnable;
+	static const int cfgBenchmark14_Iterations = 8192;
+	static const int cfgBenchmark14_Reference = 3500;
 	//[15] culling(KDOP+qsort)
-	bool					cfgBenchmark15_Enable		=	cfgEnable;
-	static const int		cfgBenchmark15_Iterations	=	8192;
-	static const int		cfgBenchmark15_Reference	=	1151;
+	bool cfgBenchmark15_Enable = cfgEnable;
+	static const int cfgBenchmark15_Iterations = 8192;
+	static const int cfgBenchmark15_Reference = 1151;
 	//[16] insert/remove batch
-	bool					cfgBenchmark16_Enable		=	cfgEnable;
-	static const int		cfgBenchmark16_BatchCount	=	256;
-	static const int		cfgBenchmark16_Passes		=	16384;
-	static const int		cfgBenchmark16_Reference	=	5138;
+	bool cfgBenchmark16_Enable = cfgEnable;
+	static const int cfgBenchmark16_BatchCount = 256;
+	static const int cfgBenchmark16_Passes = 16384;
+	static const int cfgBenchmark16_Reference = 5138;
 	//[17] select
-	bool					cfgBenchmark17_Enable		=	cfgEnable;
-	static const int		cfgBenchmark17_Iterations	=	4;
-	static const int		cfgBenchmark17_Reference	=	3390;
+	bool cfgBenchmark17_Enable = cfgEnable;
+	static const int cfgBenchmark17_Iterations = 4;
+	static const int cfgBenchmark17_Reference = 3390;
 
-	btClock					wallclock;
+	btClock wallclock;
 	printf("Benchmarking dbvt...\r\n");
-	printf("\tWorld scale: %f\r\n",cfgVolumeCenterScale);
-	printf("\tExtents base: %f\r\n",cfgVolumeExentsBase);
-	printf("\tExtents range: %f\r\n",cfgVolumeExentsScale);
-	printf("\tLeaves: %u\r\n",cfgLeaves);
-	printf("\tsizeof(btDbvtVolume): %u bytes\r\n",sizeof(btDbvtVolume));
-	printf("\tsizeof(btDbvtNode):   %u bytes\r\n",sizeof(btDbvtNode));
-	if(cfgBenchmark1_Enable)
-	{// Benchmark 1	
+	printf("\tWorld scale: %f\r\n", cfgVolumeCenterScale);
+	printf("\tExtents base: %f\r\n", cfgVolumeExentsBase);
+	printf("\tExtents range: %f\r\n", cfgVolumeExentsScale);
+	printf("\tLeaves: %u\r\n", cfgLeaves);
+	printf("\tsizeof(btDbvtVolume): %u bytes\r\n", sizeof(btDbvtVolume));
+	printf("\tsizeof(btDbvtNode):   %u bytes\r\n", sizeof(btDbvtNode));
+	if (cfgBenchmark1_Enable)
+	{  // Benchmark 1
 		srand(380843);
-		btAlignedObjectArray<btDbvtVolume>	volumes;
-		btAlignedObjectArray<bool>			results;
+		btAlignedObjectArray<btDbvtVolume> volumes;
+		btAlignedObjectArray<bool> results;
 		volumes.resize(cfgLeaves);
 		results.resize(cfgLeaves);
-		for(int i=0;i<cfgLeaves;++i)
+		for (int i = 0; i < cfgLeaves; ++i)
 		{
-			volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+			volumes[i] = btDbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
 		}
 		printf("[1] btDbvtVolume intersections: ");
 		wallclock.reset();
-		for(int i=0;i<cfgBenchmark1_Iterations;++i)
+		for (int i = 0; i < cfgBenchmark1_Iterations; ++i)
 		{
-			for(int j=0;j<cfgLeaves;++j)
+			for (int j = 0; j < cfgLeaves; ++j)
 			{
-				for(int k=0;k<cfgLeaves;++k)
+				for (int k = 0; k < cfgLeaves; ++k)
 				{
-					results[k]=Intersect(volumes[j],volumes[k]);
+					results[k] = Intersect(volumes[j], volumes[k]);
 				}
 			}
 		}
-		const int time=(int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark1_Reference)*100/time);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark1_Reference) * 100 / time);
 	}
-	if(cfgBenchmark2_Enable)
-	{// Benchmark 2	
+	if (cfgBenchmark2_Enable)
+	{  // Benchmark 2
 		srand(380843);
-		btAlignedObjectArray<btDbvtVolume>	volumes;
-		btAlignedObjectArray<btDbvtVolume>	results;
+		btAlignedObjectArray<btDbvtVolume> volumes;
+		btAlignedObjectArray<btDbvtVolume> results;
 		volumes.resize(cfgLeaves);
 		results.resize(cfgLeaves);
-		for(int i=0;i<cfgLeaves;++i)
+		for (int i = 0; i < cfgLeaves; ++i)
 		{
-			volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+			volumes[i] = btDbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
 		}
 		printf("[2] btDbvtVolume merges: ");
 		wallclock.reset();
-		for(int i=0;i<cfgBenchmark2_Iterations;++i)
+		for (int i = 0; i < cfgBenchmark2_Iterations; ++i)
 		{
-			for(int j=0;j<cfgLeaves;++j)
+			for (int j = 0; j < cfgLeaves; ++j)
 			{
-				for(int k=0;k<cfgLeaves;++k)
+				for (int k = 0; k < cfgLeaves; ++k)
 				{
-					Merge(volumes[j],volumes[k],results[k]);
+					Merge(volumes[j], volumes[k], results[k]);
 				}
 			}
 		}
-		const int time=(int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark2_Reference)*100/time);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark2_Reference) * 100 / time);
 	}
-	if(cfgBenchmark3_Enable)
-	{// Benchmark 3	
+	if (cfgBenchmark3_Enable)
+	{  // Benchmark 3
 		srand(380843);
-		btDbvt						dbvt[2];
-		btDbvtBenchmark::NilPolicy	policy;
-		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
-		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
+		btDbvt dbvt[2];
+		btDbvtBenchmark::NilPolicy policy;
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[0]);
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[1]);
 		dbvt[0].optimizeTopDown();
 		dbvt[1].optimizeTopDown();
 		printf("[3] btDbvt::collideTT: ");
 		wallclock.reset();
-		for(int i=0;i<cfgBenchmark3_Iterations;++i)
+		for (int i = 0; i < cfgBenchmark3_Iterations; ++i)
 		{
-			btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,policy);
+			btDbvt::collideTT(dbvt[0].m_root, dbvt[1].m_root, policy);
 		}
-		const int time=(int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark3_Reference)*100/time);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark3_Reference) * 100 / time);
 	}
-	if(cfgBenchmark4_Enable)
-	{// Benchmark 4
+	if (cfgBenchmark4_Enable)
+	{  // Benchmark 4
 		srand(380843);
-		btDbvt						dbvt;
-		btDbvtBenchmark::NilPolicy	policy;
-		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		btDbvt dbvt;
+		btDbvtBenchmark::NilPolicy policy;
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
 		dbvt.optimizeTopDown();
 		printf("[4] btDbvt::collideTT self: ");
 		wallclock.reset();
-		for(int i=0;i<cfgBenchmark4_Iterations;++i)
+		for (int i = 0; i < cfgBenchmark4_Iterations; ++i)
 		{
-			btDbvt::collideTT(dbvt.m_root,dbvt.m_root,policy);
+			btDbvt::collideTT(dbvt.m_root, dbvt.m_root, policy);
 		}
-		const int time=(int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark4_Reference)*100/time);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark4_Reference) * 100 / time);
 	}
-	if(cfgBenchmark5_Enable)
-	{// Benchmark 5	
+	if (cfgBenchmark5_Enable)
+	{  // Benchmark 5
 		srand(380843);
-		btDbvt								dbvt[2];
-		btAlignedObjectArray<btTransform>	transforms;
-		btDbvtBenchmark::NilPolicy			policy;
+		btDbvt dbvt[2];
+		btAlignedObjectArray<btTransform> transforms;
+		btDbvtBenchmark::NilPolicy policy;
 		transforms.resize(cfgBenchmark5_Iterations);
-		for(int i=0;i<transforms.size();++i)
+		for (int i = 0; i < transforms.size(); ++i)
 		{
-			transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale);
+			transforms[i] = btDbvtBenchmark::RandTransform(cfgVolumeCenterScale * cfgBenchmark5_OffsetScale);
 		}
-		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
-		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[0]);
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[1]);
 		dbvt[0].optimizeTopDown();
 		dbvt[1].optimizeTopDown();
 		printf("[5] btDbvt::collideTT xform: ");
 		wallclock.reset();
-		for(int i=0;i<cfgBenchmark5_Iterations;++i)
+		for (int i = 0; i < cfgBenchmark5_Iterations; ++i)
 		{
-			btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,transforms[i],policy);
+			btDbvt::collideTT(dbvt[0].m_root, dbvt[1].m_root, transforms[i], policy);
 		}
-		const int time=(int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark5_Reference)*100/time);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark5_Reference) * 100 / time);
 	}
-	if(cfgBenchmark6_Enable)
-	{// Benchmark 6	
+	if (cfgBenchmark6_Enable)
+	{  // Benchmark 6
 		srand(380843);
-		btDbvt								dbvt;
-		btAlignedObjectArray<btTransform>	transforms;
-		btDbvtBenchmark::NilPolicy			policy;
+		btDbvt dbvt;
+		btAlignedObjectArray<btTransform> transforms;
+		btDbvtBenchmark::NilPolicy policy;
 		transforms.resize(cfgBenchmark6_Iterations);
-		for(int i=0;i<transforms.size();++i)
+		for (int i = 0; i < transforms.size(); ++i)
 		{
-			transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale);
+			transforms[i] = btDbvtBenchmark::RandTransform(cfgVolumeCenterScale * cfgBenchmark6_OffsetScale);
 		}
-		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
 		dbvt.optimizeTopDown();
 		printf("[6] btDbvt::collideTT xform,self: ");
 		wallclock.reset();
-		for(int i=0;i<cfgBenchmark6_Iterations;++i)
+		for (int i = 0; i < cfgBenchmark6_Iterations; ++i)
 		{
-			btDbvt::collideTT(dbvt.m_root,dbvt.m_root,transforms[i],policy);		
+			btDbvt::collideTT(dbvt.m_root, dbvt.m_root, transforms[i], policy);
 		}
-		const int time=(int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark6_Reference)*100/time);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark6_Reference) * 100 / time);
 	}
-	if(cfgBenchmark7_Enable)
-	{// Benchmark 7	
+	if (cfgBenchmark7_Enable)
+	{  // Benchmark 7
 		srand(380843);
-		btDbvt								dbvt;
-		btAlignedObjectArray<btVector3>		rayorg;
-		btAlignedObjectArray<btVector3>		raydir;
-		btDbvtBenchmark::NilPolicy			policy;
+		btDbvt dbvt;
+		btAlignedObjectArray<btVector3> rayorg;
+		btAlignedObjectArray<btVector3> raydir;
+		btDbvtBenchmark::NilPolicy policy;
 		rayorg.resize(cfgBenchmark7_Iterations);
 		raydir.resize(cfgBenchmark7_Iterations);
-		for(int i=0;i<rayorg.size();++i)
+		for (int i = 0; i < rayorg.size(); ++i)
 		{
-			rayorg[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
-			raydir[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
+			rayorg[i] = btDbvtBenchmark::RandVector3(cfgVolumeCenterScale * 2);
+			raydir[i] = btDbvtBenchmark::RandVector3(cfgVolumeCenterScale * 2);
 		}
-		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
 		dbvt.optimizeTopDown();
 		printf("[7] btDbvt::rayTest: ");
 		wallclock.reset();
-		for(int i=0;i<cfgBenchmark7_Passes;++i)
+		for (int i = 0; i < cfgBenchmark7_Passes; ++i)
 		{
-			for(int j=0;j<cfgBenchmark7_Iterations;++j)
+			for (int j = 0; j < cfgBenchmark7_Iterations; ++j)
 			{
-				btDbvt::rayTest(dbvt.m_root,rayorg[j],rayorg[j]+raydir[j],policy);
+				btDbvt::rayTest(dbvt.m_root, rayorg[j], rayorg[j] + raydir[j], policy);
 			}
 		}
-		const int	time=(int)wallclock.getTimeMilliseconds();
-		unsigned	rays=cfgBenchmark7_Passes*cfgBenchmark7_Iterations;
-		printf("%u ms (%i%%),(%u r/s)\r\n",time,(time-cfgBenchmark7_Reference)*100/time,(rays*1000)/time);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		unsigned rays = cfgBenchmark7_Passes * cfgBenchmark7_Iterations;
+		printf("%u ms (%i%%),(%u r/s)\r\n", time, (time - cfgBenchmark7_Reference) * 100 / time, (rays * 1000) / time);
 	}
-	if(cfgBenchmark8_Enable)
-	{// Benchmark 8	
+	if (cfgBenchmark8_Enable)
+	{  // Benchmark 8
 		srand(380843);
-		btDbvt								dbvt;
-		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		btDbvt dbvt;
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
 		dbvt.optimizeTopDown();
 		printf("[8] insert/remove: ");
 		wallclock.reset();
-		for(int i=0;i<cfgBenchmark8_Passes;++i)
+		for (int i = 0; i < cfgBenchmark8_Passes; ++i)
 		{
-			for(int j=0;j<cfgBenchmark8_Iterations;++j)
+			for (int j = 0; j < cfgBenchmark8_Iterations; ++j)
 			{
-				dbvt.remove(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
+				dbvt.remove(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale), 0));
 			}
 		}
-		const int	time=(int)wallclock.getTimeMilliseconds();
-		const int	ir=cfgBenchmark8_Passes*cfgBenchmark8_Iterations;
-		printf("%u ms (%i%%),(%u ir/s)\r\n",time,(time-cfgBenchmark8_Reference)*100/time,ir*1000/time);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		const int ir = cfgBenchmark8_Passes * cfgBenchmark8_Iterations;
+		printf("%u ms (%i%%),(%u ir/s)\r\n", time, (time - cfgBenchmark8_Reference) * 100 / time, ir * 1000 / time);
 	}
-	if(cfgBenchmark9_Enable)
-	{// Benchmark 9	
+	if (cfgBenchmark9_Enable)
+	{  // Benchmark 9
 		srand(380843);
-		btDbvt										dbvt;
-		btAlignedObjectArray<const btDbvtNode*>	leaves;
-		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		btDbvt dbvt;
+		btAlignedObjectArray<const btDbvtNode*> leaves;
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
 		dbvt.optimizeTopDown();
-		dbvt.extractLeaves(dbvt.m_root,leaves);
+		dbvt.extractLeaves(dbvt.m_root, leaves);
 		printf("[9] updates (teleport): ");
 		wallclock.reset();
-		for(int i=0;i<cfgBenchmark9_Passes;++i)
+		for (int i = 0; i < cfgBenchmark9_Passes; ++i)
 		{
-			for(int j=0;j<cfgBenchmark9_Iterations;++j)
+			for (int j = 0; j < cfgBenchmark9_Iterations; ++j)
 			{
-				dbvt.update(const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]),
-					btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale));
+				dbvt.update(const_cast<btDbvtNode*>(leaves[rand() % cfgLeaves]),
+							btDbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale));
 			}
 		}
-		const int	time=(int)wallclock.getTimeMilliseconds();
-		const int	up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations;
-		printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		const int up = cfgBenchmark9_Passes * cfgBenchmark9_Iterations;
+		printf("%u ms (%i%%),(%u u/s)\r\n", time, (time - cfgBenchmark9_Reference) * 100 / time, up * 1000 / time);
 	}
-	if(cfgBenchmark10_Enable)
-	{// Benchmark 10	
+	if (cfgBenchmark10_Enable)
+	{  // Benchmark 10
 		srand(380843);
-		btDbvt										dbvt;
-		btAlignedObjectArray<const btDbvtNode*>	leaves;
-		btAlignedObjectArray<btVector3>				vectors;
+		btDbvt dbvt;
+		btAlignedObjectArray<const btDbvtNode*> leaves;
+		btAlignedObjectArray<btVector3> vectors;
 		vectors.resize(cfgBenchmark10_Iterations);
-		for(int i=0;i<vectors.size();++i)
+		for (int i = 0; i < vectors.size(); ++i)
 		{
-			vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1))*cfgBenchmark10_Scale;
+			vectors[i] = (btDbvtBenchmark::RandVector3() * 2 - btVector3(1, 1, 1)) * cfgBenchmark10_Scale;
 		}
-		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
 		dbvt.optimizeTopDown();
-		dbvt.extractLeaves(dbvt.m_root,leaves);
+		dbvt.extractLeaves(dbvt.m_root, leaves);
 		printf("[10] updates (jitter): ");
 		wallclock.reset();
 
-		for(int i=0;i<cfgBenchmark10_Passes;++i)
+		for (int i = 0; i < cfgBenchmark10_Passes; ++i)
 		{
-			for(int j=0;j<cfgBenchmark10_Iterations;++j)
-			{			
-				const btVector3&	d=vectors[j];
-				btDbvtNode*		l=const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]);
-				btDbvtVolume		v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d);
-				dbvt.update(l,v);
+			for (int j = 0; j < cfgBenchmark10_Iterations; ++j)
+			{
+				const btVector3& d = vectors[j];
+				btDbvtNode* l = const_cast<btDbvtNode*>(leaves[rand() % cfgLeaves]);
+				btDbvtVolume v = btDbvtVolume::FromMM(l->volume.Mins() + d, l->volume.Maxs() + d);
+				dbvt.update(l, v);
 			}
 		}
-		const int	time=(int)wallclock.getTimeMilliseconds();
-		const int	up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations;
-		printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		const int up = cfgBenchmark10_Passes * cfgBenchmark10_Iterations;
+		printf("%u ms (%i%%),(%u u/s)\r\n", time, (time - cfgBenchmark10_Reference) * 100 / time, up * 1000 / time);
 	}
-	if(cfgBenchmark11_Enable)
-	{// Benchmark 11	
+	if (cfgBenchmark11_Enable)
+	{  // Benchmark 11
 		srand(380843);
-		btDbvt										dbvt;
-		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		btDbvt dbvt;
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
 		dbvt.optimizeTopDown();
 		printf("[11] optimize (incremental): ");
-		wallclock.reset();	
-		for(int i=0;i<cfgBenchmark11_Passes;++i)
+		wallclock.reset();
+		for (int i = 0; i < cfgBenchmark11_Passes; ++i)
 		{
 			dbvt.optimizeIncremental(cfgBenchmark11_Iterations);
 		}
-		const int	time=(int)wallclock.getTimeMilliseconds();
-		const int	op=cfgBenchmark11_Passes*cfgBenchmark11_Iterations;
-		printf("%u ms (%i%%),(%u o/s)\r\n",time,(time-cfgBenchmark11_Reference)*100/time,op/time*1000);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		const int op = cfgBenchmark11_Passes * cfgBenchmark11_Iterations;
+		printf("%u ms (%i%%),(%u o/s)\r\n", time, (time - cfgBenchmark11_Reference) * 100 / time, op / time * 1000);
 	}
-	if(cfgBenchmark12_Enable)
-	{// Benchmark 12	
+	if (cfgBenchmark12_Enable)
+	{  // Benchmark 12
 		srand(380843);
-		btAlignedObjectArray<btDbvtVolume>	volumes;
-		btAlignedObjectArray<bool>				results;
+		btAlignedObjectArray<btDbvtVolume> volumes;
+		btAlignedObjectArray<bool> results;
 		volumes.resize(cfgLeaves);
 		results.resize(cfgLeaves);
-		for(int i=0;i<cfgLeaves;++i)
+		for (int i = 0; i < cfgLeaves; ++i)
 		{
-			volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+			volumes[i] = btDbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
 		}
 		printf("[12] btDbvtVolume notequal: ");
 		wallclock.reset();
-		for(int i=0;i<cfgBenchmark12_Iterations;++i)
+		for (int i = 0; i < cfgBenchmark12_Iterations; ++i)
 		{
-			for(int j=0;j<cfgLeaves;++j)
+			for (int j = 0; j < cfgLeaves; ++j)
 			{
-				for(int k=0;k<cfgLeaves;++k)
+				for (int k = 0; k < cfgLeaves; ++k)
 				{
-					results[k]=NotEqual(volumes[j],volumes[k]);
+					results[k] = NotEqual(volumes[j], volumes[k]);
 				}
 			}
 		}
-		const int time=(int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark12_Reference)*100/time);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark12_Reference) * 100 / time);
 	}
-	if(cfgBenchmark13_Enable)
-	{// Benchmark 13	
+	if (cfgBenchmark13_Enable)
+	{  // Benchmark 13
 		srand(380843);
-		btDbvt								dbvt;
-		btAlignedObjectArray<btVector3>		vectors;
-		btDbvtBenchmark::NilPolicy			policy;
+		btDbvt dbvt;
+		btAlignedObjectArray<btVector3> vectors;
+		btDbvtBenchmark::NilPolicy policy;
 		vectors.resize(cfgBenchmark13_Iterations);
-		for(int i=0;i<vectors.size();++i)
+		for (int i = 0; i < vectors.size(); ++i)
 		{
-			vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
+			vectors[i] = (btDbvtBenchmark::RandVector3() * 2 - btVector3(1, 1, 1)).normalized();
 		}
-		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
 		dbvt.optimizeTopDown();
 		printf("[13] culling(OCL+fullsort): ");
-		wallclock.reset();	
-		for(int i=0;i<cfgBenchmark13_Iterations;++i)
+		wallclock.reset();
+		for (int i = 0; i < cfgBenchmark13_Iterations; ++i)
 		{
-			static const btScalar	offset=0;
-			policy.m_depth=-SIMD_INFINITY;
-			dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy);
+			static const btScalar offset = 0;
+			policy.m_depth = -SIMD_INFINITY;
+			dbvt.collideOCL(dbvt.m_root, &vectors[i], &offset, vectors[i], 1, policy);
 		}
-		const int	time=(int)wallclock.getTimeMilliseconds();
-		const int	t=cfgBenchmark13_Iterations;
-		printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark13_Reference)*100/time,(t*1000)/time);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		const int t = cfgBenchmark13_Iterations;
+		printf("%u ms (%i%%),(%u t/s)\r\n", time, (time - cfgBenchmark13_Reference) * 100 / time, (t * 1000) / time);
 	}
-	if(cfgBenchmark14_Enable)
-	{// Benchmark 14	
+	if (cfgBenchmark14_Enable)
+	{  // Benchmark 14
 		srand(380843);
-		btDbvt								dbvt;
-		btAlignedObjectArray<btVector3>		vectors;
-		btDbvtBenchmark::P14				policy;
+		btDbvt dbvt;
+		btAlignedObjectArray<btVector3> vectors;
+		btDbvtBenchmark::P14 policy;
 		vectors.resize(cfgBenchmark14_Iterations);
-		for(int i=0;i<vectors.size();++i)
+		for (int i = 0; i < vectors.size(); ++i)
 		{
-			vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
+			vectors[i] = (btDbvtBenchmark::RandVector3() * 2 - btVector3(1, 1, 1)).normalized();
 		}
-		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
 		dbvt.optimizeTopDown();
 		policy.m_nodes.reserve(cfgLeaves);
 		printf("[14] culling(OCL+qsort): ");
-		wallclock.reset();	
-		for(int i=0;i<cfgBenchmark14_Iterations;++i)
+		wallclock.reset();
+		for (int i = 0; i < cfgBenchmark14_Iterations; ++i)
 		{
-			static const btScalar	offset=0;
+			static const btScalar offset = 0;
 			policy.m_nodes.resize(0);
-			dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy,false);
+			dbvt.collideOCL(dbvt.m_root, &vectors[i], &offset, vectors[i], 1, policy, false);
 			policy.m_nodes.quickSort(btDbvtBenchmark::P14::sortfnc);
 		}
-		const int	time=(int)wallclock.getTimeMilliseconds();
-		const int	t=cfgBenchmark14_Iterations;
-		printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark14_Reference)*100/time,(t*1000)/time);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		const int t = cfgBenchmark14_Iterations;
+		printf("%u ms (%i%%),(%u t/s)\r\n", time, (time - cfgBenchmark14_Reference) * 100 / time, (t * 1000) / time);
 	}
-	if(cfgBenchmark15_Enable)
-	{// Benchmark 15	
+	if (cfgBenchmark15_Enable)
+	{  // Benchmark 15
 		srand(380843);
-		btDbvt								dbvt;
-		btAlignedObjectArray<btVector3>		vectors;
-		btDbvtBenchmark::P15				policy;
+		btDbvt dbvt;
+		btAlignedObjectArray<btVector3> vectors;
+		btDbvtBenchmark::P15 policy;
 		vectors.resize(cfgBenchmark15_Iterations);
-		for(int i=0;i<vectors.size();++i)
+		for (int i = 0; i < vectors.size(); ++i)
 		{
-			vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
+			vectors[i] = (btDbvtBenchmark::RandVector3() * 2 - btVector3(1, 1, 1)).normalized();
 		}
-		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
 		dbvt.optimizeTopDown();
 		policy.m_nodes.reserve(cfgLeaves);
 		printf("[15] culling(KDOP+qsort): ");
-		wallclock.reset();	
-		for(int i=0;i<cfgBenchmark15_Iterations;++i)
+		wallclock.reset();
+		for (int i = 0; i < cfgBenchmark15_Iterations; ++i)
 		{
-			static const btScalar	offset=0;
+			static const btScalar offset = 0;
 			policy.m_nodes.resize(0);
-			policy.m_axis=vectors[i];
-			dbvt.collideKDOP(dbvt.m_root,&vectors[i],&offset,1,policy);
+			policy.m_axis = vectors[i];
+			dbvt.collideKDOP(dbvt.m_root, &vectors[i], &offset, 1, policy);
 			policy.m_nodes.quickSort(btDbvtBenchmark::P15::sortfnc);
 		}
-		const int	time=(int)wallclock.getTimeMilliseconds();
-		const int	t=cfgBenchmark15_Iterations;
-		printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark15_Reference)*100/time,(t*1000)/time);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		const int t = cfgBenchmark15_Iterations;
+		printf("%u ms (%i%%),(%u t/s)\r\n", time, (time - cfgBenchmark15_Reference) * 100 / time, (t * 1000) / time);
 	}
-	if(cfgBenchmark16_Enable)
-	{// Benchmark 16	
+	if (cfgBenchmark16_Enable)
+	{  // Benchmark 16
 		srand(380843);
-		btDbvt								dbvt;
-		btAlignedObjectArray<btDbvtNode*>	batch;
-		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		btDbvt dbvt;
+		btAlignedObjectArray<btDbvtNode*> batch;
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
 		dbvt.optimizeTopDown();
 		batch.reserve(cfgBenchmark16_BatchCount);
-		printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount);
+		printf("[16] insert/remove batch(%u): ", cfgBenchmark16_BatchCount);
 		wallclock.reset();
-		for(int i=0;i<cfgBenchmark16_Passes;++i)
+		for (int i = 0; i < cfgBenchmark16_Passes; ++i)
 		{
-			for(int j=0;j<cfgBenchmark16_BatchCount;++j)
+			for (int j = 0; j < cfgBenchmark16_BatchCount; ++j)
 			{
-				batch.push_back(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
+				batch.push_back(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale), 0));
 			}
-			for(int j=0;j<cfgBenchmark16_BatchCount;++j)
+			for (int j = 0; j < cfgBenchmark16_BatchCount; ++j)
 			{
 				dbvt.remove(batch[j]);
 			}
 			batch.resize(0);
 		}
-		const int	time=(int)wallclock.getTimeMilliseconds();
-		const int	ir=cfgBenchmark16_Passes*cfgBenchmark16_BatchCount;
-		printf("%u ms (%i%%),(%u bir/s)\r\n",time,(time-cfgBenchmark16_Reference)*100/time,int(ir*1000.0/time));
+		const int time = (int)wallclock.getTimeMilliseconds();
+		const int ir = cfgBenchmark16_Passes * cfgBenchmark16_BatchCount;
+		printf("%u ms (%i%%),(%u bir/s)\r\n", time, (time - cfgBenchmark16_Reference) * 100 / time, int(ir * 1000.0 / time));
 	}
-	if(cfgBenchmark17_Enable)
-	{// Benchmark 17
+	if (cfgBenchmark17_Enable)
+	{  // Benchmark 17
 		srand(380843);
-		btAlignedObjectArray<btDbvtVolume>	volumes;
-		btAlignedObjectArray<int>			results;
-		btAlignedObjectArray<int>			indices;
+		btAlignedObjectArray<btDbvtVolume> volumes;
+		btAlignedObjectArray<int> results;
+		btAlignedObjectArray<int> indices;
 		volumes.resize(cfgLeaves);
 		results.resize(cfgLeaves);
 		indices.resize(cfgLeaves);
-		for(int i=0;i<cfgLeaves;++i)
+		for (int i = 0; i < cfgLeaves; ++i)
 		{
-			indices[i]=i;
-			volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+			indices[i] = i;
+			volumes[i] = btDbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
 		}
-		for(int i=0;i<cfgLeaves;++i)
+		for (int i = 0; i < cfgLeaves; ++i)
 		{
-			btSwap(indices[i],indices[rand()%cfgLeaves]);
+			btSwap(indices[i], indices[rand() % cfgLeaves]);
 		}
 		printf("[17] btDbvtVolume select: ");
 		wallclock.reset();
-		for(int i=0;i<cfgBenchmark17_Iterations;++i)
+		for (int i = 0; i < cfgBenchmark17_Iterations; ++i)
 		{
-			for(int j=0;j<cfgLeaves;++j)
+			for (int j = 0; j < cfgLeaves; ++j)
 			{
-				for(int k=0;k<cfgLeaves;++k)
+				for (int k = 0; k < cfgLeaves; ++k)
 				{
-					const int idx=indices[k];
-					results[idx]=Select(volumes[idx],volumes[j],volumes[k]);
+					const int idx = indices[k];
+					results[idx] = Select(volumes[idx], volumes[j], volumes[k]);
 				}
 			}
 		}
-		const int time=(int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark17_Reference)*100/time);
+		const int time = (int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark17_Reference) * 100 / time);
 	}
 	printf("\r\n\r\n");
 }
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h
index db4e482f292..55daa7fb578 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h
@@ -21,55 +21,53 @@ subject to the following restrictions:
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btAabbUtil2.h"
-
 //
 // Compile time configuration
 //
 
-
 // Implementation profiles
-#define DBVT_IMPL_GENERIC		0	// Generic implementation	
-#define DBVT_IMPL_SSE			1	// SSE
+#define DBVT_IMPL_GENERIC 0  // Generic implementation
+#define DBVT_IMPL_SSE 1      // SSE
 
 // Template implementation of ICollide
 #ifdef _WIN32
-#if (defined (_MSC_VER) && _MSC_VER >= 1400)
-#define	DBVT_USE_TEMPLATE		1
+#if (defined(_MSC_VER) && _MSC_VER >= 1400)
+#define DBVT_USE_TEMPLATE 1
 #else
-#define	DBVT_USE_TEMPLATE		0
+#define DBVT_USE_TEMPLATE 0
 #endif
 #else
-#define	DBVT_USE_TEMPLATE		0
+#define DBVT_USE_TEMPLATE 0
 #endif
 
 // Use only intrinsics instead of inline asm
-#define DBVT_USE_INTRINSIC_SSE	1
+#define DBVT_USE_INTRINSIC_SSE 1
 
 // Using memmov for collideOCL
-#define DBVT_USE_MEMMOVE		1
+#define DBVT_USE_MEMMOVE 1
 
 // Enable benchmarking code
-#define	DBVT_ENABLE_BENCHMARK	0
+#define DBVT_ENABLE_BENCHMARK 0
 
 // Inlining
-#define DBVT_INLINE				SIMD_FORCE_INLINE
+#define DBVT_INLINE SIMD_FORCE_INLINE
 
 // Specific methods implementation
 
 //SSE gives errors on a MSVC 7.1
-#if defined (BT_USE_SSE) //&& defined (_WIN32)
-#define DBVT_SELECT_IMPL		DBVT_IMPL_SSE
-#define DBVT_MERGE_IMPL			DBVT_IMPL_SSE
-#define DBVT_INT0_IMPL			DBVT_IMPL_SSE
+#if defined(BT_USE_SSE)  //&& defined (_WIN32)
+#define DBVT_SELECT_IMPL DBVT_IMPL_SSE
+#define DBVT_MERGE_IMPL DBVT_IMPL_SSE
+#define DBVT_INT0_IMPL DBVT_IMPL_SSE
 #else
-#define DBVT_SELECT_IMPL		DBVT_IMPL_GENERIC
-#define DBVT_MERGE_IMPL			DBVT_IMPL_GENERIC
-#define DBVT_INT0_IMPL			DBVT_IMPL_GENERIC
+#define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC
+#define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC
+#define DBVT_INT0_IMPL DBVT_IMPL_GENERIC
 #endif
 
-#if	(DBVT_SELECT_IMPL==DBVT_IMPL_SSE)||	\
-	(DBVT_MERGE_IMPL==DBVT_IMPL_SSE)||	\
-	(DBVT_INT0_IMPL==DBVT_IMPL_SSE)
+#if (DBVT_SELECT_IMPL == DBVT_IMPL_SSE) || \
+	(DBVT_MERGE_IMPL == DBVT_IMPL_SSE) ||  \
+	(DBVT_INT0_IMPL == DBVT_IMPL_SSE)
 #include <emmintrin.h>
 #endif
 
@@ -78,21 +76,24 @@ subject to the following restrictions:
 //
 
 #if DBVT_USE_TEMPLATE
-#define	DBVT_VIRTUAL
+#define DBVT_VIRTUAL
 #define DBVT_VIRTUAL_DTOR(a)
-#define DBVT_PREFIX					template <typename T>
-#define DBVT_IPOLICY				T& policy
-#define DBVT_CHECKTYPE				static const ICollide&	typechecker=*(T*)1;(void)typechecker;
+#define DBVT_PREFIX template <typename T>
+#define DBVT_IPOLICY T& policy
+#define DBVT_CHECKTYPE                           \
+	static const ICollide& typechecker = *(T*)1; \
+	(void)typechecker;
 #else
-#define	DBVT_VIRTUAL_DTOR(a)		virtual ~a() {}
-#define DBVT_VIRTUAL				virtual
+#define DBVT_VIRTUAL_DTOR(a) \
+	virtual ~a() {}
+#define DBVT_VIRTUAL virtual
 #define DBVT_PREFIX
-#define DBVT_IPOLICY				ICollide& policy
+#define DBVT_IPOLICY ICollide& policy
 #define DBVT_CHECKTYPE
 #endif
 
 #if DBVT_USE_MEMMOVE
-#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__)
+#if !defined(__CELLOS_LV2__) && !defined(__MWERKS__)
 #include <memory.h>
 #endif
 #include <string.h>
@@ -126,187 +127,235 @@ subject to the following restrictions:
 // Defaults volumes
 //
 
-/* btDbvtAabbMm			*/ 
-struct	btDbvtAabbMm
+/* btDbvtAabbMm			*/
+struct btDbvtAabbMm
 {
-	DBVT_INLINE btVector3			Center() const	{ return((mi+mx)/2); }
-	DBVT_INLINE btVector3			Lengths() const	{ return(mx-mi); }
-	DBVT_INLINE btVector3			Extents() const	{ return((mx-mi)/2); }
-	DBVT_INLINE const btVector3&	Mins() const	{ return(mi); }
-	DBVT_INLINE const btVector3&	Maxs() const	{ return(mx); }
-	static inline btDbvtAabbMm		FromCE(const btVector3& c,const btVector3& e);
-	static inline btDbvtAabbMm		FromCR(const btVector3& c,btScalar r);
-	static inline btDbvtAabbMm		FromMM(const btVector3& mi,const btVector3& mx);
-	static inline btDbvtAabbMm		FromPoints(const btVector3* pts,int n);
-	static inline btDbvtAabbMm		FromPoints(const btVector3** ppts,int n);
-	DBVT_INLINE void				Expand(const btVector3& e);
-	DBVT_INLINE void				SignedExpand(const btVector3& e);
-	DBVT_INLINE bool				Contain(const btDbvtAabbMm& a) const;
-	DBVT_INLINE int					Classify(const btVector3& n,btScalar o,int s) const;
-	DBVT_INLINE btScalar			ProjectMinimum(const btVector3& v,unsigned signs) const;
-	DBVT_INLINE friend bool			Intersect(	const btDbvtAabbMm& a,
-		const btDbvtAabbMm& b);
-	
-	DBVT_INLINE friend bool			Intersect(	const btDbvtAabbMm& a,
-		const btVector3& b);
-
-	DBVT_INLINE friend btScalar		Proximity(	const btDbvtAabbMm& a,
-		const btDbvtAabbMm& b);
-	DBVT_INLINE friend int			Select(		const btDbvtAabbMm& o,
-		const btDbvtAabbMm& a,
-		const btDbvtAabbMm& b);
-	DBVT_INLINE friend void			Merge(		const btDbvtAabbMm& a,
-		const btDbvtAabbMm& b,
-		btDbvtAabbMm& r);
-	DBVT_INLINE friend bool			NotEqual(	const btDbvtAabbMm& a,
-		const btDbvtAabbMm& b);
-    
-    DBVT_INLINE btVector3&	tMins()	{ return(mi); }
-	DBVT_INLINE btVector3&	tMaxs()	{ return(mx); }
-    
+    DBVT_INLINE btDbvtAabbMm(){}
+	DBVT_INLINE btVector3 Center() const { return ((mi + mx) / 2); }
+	DBVT_INLINE btVector3 Lengths() const { return (mx - mi); }
+	DBVT_INLINE btVector3 Extents() const { return ((mx - mi) / 2); }
+	DBVT_INLINE const btVector3& Mins() const { return (mi); }
+	DBVT_INLINE const btVector3& Maxs() const { return (mx); }
+	static inline btDbvtAabbMm FromCE(const btVector3& c, const btVector3& e);
+	static inline btDbvtAabbMm FromCR(const btVector3& c, btScalar r);
+	static inline btDbvtAabbMm FromMM(const btVector3& mi, const btVector3& mx);
+	static inline btDbvtAabbMm FromPoints(const btVector3* pts, int n);
+	static inline btDbvtAabbMm FromPoints(const btVector3** ppts, int n);
+	DBVT_INLINE void Expand(const btVector3& e);
+	DBVT_INLINE void SignedExpand(const btVector3& e);
+	DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const;
+	DBVT_INLINE int Classify(const btVector3& n, btScalar o, int s) const;
+	DBVT_INLINE btScalar ProjectMinimum(const btVector3& v, unsigned signs) const;
+	DBVT_INLINE friend bool Intersect(const btDbvtAabbMm& a,
+									  const btDbvtAabbMm& b);
+
+	DBVT_INLINE friend bool Intersect(const btDbvtAabbMm& a,
+									  const btVector3& b);
+
+	DBVT_INLINE friend btScalar Proximity(const btDbvtAabbMm& a,
+										  const btDbvtAabbMm& b);
+	DBVT_INLINE friend int Select(const btDbvtAabbMm& o,
+								  const btDbvtAabbMm& a,
+								  const btDbvtAabbMm& b);
+	DBVT_INLINE friend void Merge(const btDbvtAabbMm& a,
+								  const btDbvtAabbMm& b,
+								  btDbvtAabbMm& r);
+	DBVT_INLINE friend bool NotEqual(const btDbvtAabbMm& a,
+									 const btDbvtAabbMm& b);
+
+	DBVT_INLINE btVector3& tMins() { return (mi); }
+	DBVT_INLINE btVector3& tMaxs() { return (mx); }
+
 private:
-	DBVT_INLINE void				AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
+	DBVT_INLINE void AddSpan(const btVector3& d, btScalar& smi, btScalar& smx) const;
+
 private:
-	btVector3	mi,mx;
+	btVector3 mi, mx;
 };
 
-// Types	
-typedef	btDbvtAabbMm	btDbvtVolume;
+// Types
+typedef btDbvtAabbMm btDbvtVolume;
 
-/* btDbvtNode				*/ 
-struct	btDbvtNode
+/* btDbvtNode				*/
+struct btDbvtNode
 {
-	btDbvtVolume	volume;
-	btDbvtNode*		parent;
-	DBVT_INLINE bool	isleaf() const		{ return(childs[1]==0); }
-	DBVT_INLINE bool	isinternal() const	{ return(!isleaf()); }
-	union
-	{
-		btDbvtNode*	childs[2];
-		void*	data;
-		int		dataAsInt;
+	btDbvtVolume volume;
+	btDbvtNode* parent;
+	DBVT_INLINE bool isleaf() const { return (childs[1] == 0); }
+	DBVT_INLINE bool isinternal() const { return (!isleaf()); }
+	union {
+		btDbvtNode* childs[2];
+		void* data;
+		int dataAsInt;
 	};
 };
 
+/* btDbv(normal)tNode                */
+struct btDbvntNode
+{
+    btDbvtVolume volume;
+    btVector3 normal;
+    btScalar angle;
+    DBVT_INLINE bool isleaf() const { return (childs[1] == 0); }
+    DBVT_INLINE bool isinternal() const { return (!isleaf()); }
+    btDbvntNode* childs[2];
+    void* data;
+
+    btDbvntNode(const btDbvtNode* n)
+    : volume(n->volume)
+    , normal(0,0,0)
+    , angle(0)
+    , data(n->data)
+    {
+        childs[0] = 0;
+        childs[1] = 0;
+    }
+    
+    ~btDbvntNode()
+    {
+        if (childs[0])
+            delete childs[0];
+        if (childs[1])
+            delete childs[1];
+    }
+};
+
+typedef btAlignedObjectArray<const btDbvtNode*> btNodeStack;
+
 ///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
 ///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes.
 ///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
-struct	btDbvt
+struct btDbvt
 {
-	/* Stack element	*/ 
-	struct	sStkNN
+	/* Stack element	*/
+	struct sStkNN
 	{
-		const btDbvtNode*	a;
-		const btDbvtNode*	b;
+		const btDbvtNode* a;
+		const btDbvtNode* b;
 		sStkNN() {}
-		sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
+		sStkNN(const btDbvtNode* na, const btDbvtNode* nb) : a(na), b(nb) {}
 	};
-	struct	sStkNP
+	struct sStkNP
 	{
-		const btDbvtNode*	node;
-		int			mask;
-		sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
+		const btDbvtNode* node;
+		int mask;
+		sStkNP(const btDbvtNode* n, unsigned m) : node(n), mask(m) {}
 	};
-	struct	sStkNPS
+	struct sStkNPS
 	{
-		const btDbvtNode*	node;
-		int			mask;
-		btScalar	value;
+		const btDbvtNode* node;
+		int mask;
+		btScalar value;
 		sStkNPS() {}
-		sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
+		sStkNPS(const btDbvtNode* n, unsigned m, btScalar v) : node(n), mask(m), value(v) {}
 	};
-	struct	sStkCLN
+	struct sStkCLN
 	{
-		const btDbvtNode*	node;
-		btDbvtNode*		parent;
-		sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
+		const btDbvtNode* node;
+		btDbvtNode* parent;
+		sStkCLN(const btDbvtNode* n, btDbvtNode* p) : node(n), parent(p) {}
 	};
+    
+    struct sStknNN
+    {
+        const btDbvntNode* a;
+        const btDbvntNode* b;
+        sStknNN() {}
+        sStknNN(const btDbvntNode* na, const btDbvntNode* nb) : a(na), b(nb) {}
+    };
 	// Policies/Interfaces
 
-	/* ICollide	*/ 
-	struct	ICollide
-	{		
+	/* ICollide	*/
+	struct ICollide
+	{
 		DBVT_VIRTUAL_DTOR(ICollide)
-			DBVT_VIRTUAL void	Process(const btDbvtNode*,const btDbvtNode*)		{}
-		DBVT_VIRTUAL void	Process(const btDbvtNode*)					{}
-		DBVT_VIRTUAL void	Process(const btDbvtNode* n,btScalar)			{ Process(n); }
-		DBVT_VIRTUAL bool	Descent(const btDbvtNode*)					{ return(true); }
-		DBVT_VIRTUAL bool	AllLeaves(const btDbvtNode*)					{ return(true); }
+		DBVT_VIRTUAL void Process(const btDbvtNode*, const btDbvtNode*) {}
+		DBVT_VIRTUAL void Process(const btDbvtNode*) {}
+		DBVT_VIRTUAL void Process(const btDbvtNode* n, btScalar) { Process(n); }
+        DBVT_VIRTUAL void Process(const btDbvntNode*, const btDbvntNode*) {}
+		DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return (true); }
+		DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return (true); }
 	};
-	/* IWriter	*/ 
-	struct	IWriter
+	/* IWriter	*/
+	struct IWriter
 	{
 		virtual ~IWriter() {}
-		virtual void		Prepare(const btDbvtNode* root,int numnodes)=0;
-		virtual void		WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0;
-		virtual void		WriteLeaf(const btDbvtNode*,int index,int parent)=0;
+		virtual void Prepare(const btDbvtNode* root, int numnodes) = 0;
+		virtual void WriteNode(const btDbvtNode*, int index, int parent, int child0, int child1) = 0;
+		virtual void WriteLeaf(const btDbvtNode*, int index, int parent) = 0;
 	};
-	/* IClone	*/ 
-	struct	IClone
+	/* IClone	*/
+	struct IClone
 	{
-		virtual ~IClone()	{}
-		virtual void		CloneLeaf(btDbvtNode*) {}
+		virtual ~IClone() {}
+		virtual void CloneLeaf(btDbvtNode*) {}
 	};
 
 	// Constants
-	enum	{
-		SIMPLE_STACKSIZE	=	64,
-		DOUBLE_STACKSIZE	=	SIMPLE_STACKSIZE*2
+	enum
+	{
+		SIMPLE_STACKSIZE = 64,
+		DOUBLE_STACKSIZE = SIMPLE_STACKSIZE * 2
 	};
 
 	// Fields
-	btDbvtNode*		m_root;
-	btDbvtNode*		m_free;
-	int				m_lkhd;
-	int				m_leaves;
-	unsigned		m_opath;
-
-	
-	btAlignedObjectArray<sStkNN>	m_stkStack;
-	mutable btAlignedObjectArray<const btDbvtNode*>	m_rayTestStack;
+	btDbvtNode* m_root;
+	btDbvtNode* m_free;
+	int m_lkhd;
+	int m_leaves;
+	unsigned m_opath;
 
+	btAlignedObjectArray<sStkNN> m_stkStack;
 
 	// Methods
 	btDbvt();
 	~btDbvt();
-	void			clear();
-	bool			empty() const { return(0==m_root); }
-	void			optimizeBottomUp();
-	void			optimizeTopDown(int bu_treshold=128);
-	void			optimizeIncremental(int passes);
-	btDbvtNode*		insert(const btDbvtVolume& box,void* data);
-	void			update(btDbvtNode* leaf,int lookahead=-1);
-	void			update(btDbvtNode* leaf,btDbvtVolume& volume);
-	bool			update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin);
-	bool			update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity);
-	bool			update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin);	
-	void			remove(btDbvtNode* leaf);
-	void			write(IWriter* iwriter) const;
-	void			clone(btDbvt& dest,IClone* iclone=0) const;
-	static int		maxdepth(const btDbvtNode* node);
-	static int		countLeaves(const btDbvtNode* node);
-	static void		extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves);
+	void clear();
+	bool empty() const { return (0 == m_root); }
+	void optimizeBottomUp();
+	void optimizeTopDown(int bu_treshold = 128);
+	void optimizeIncremental(int passes);
+	btDbvtNode* insert(const btDbvtVolume& box, void* data);
+	void update(btDbvtNode* leaf, int lookahead = -1);
+	void update(btDbvtNode* leaf, btDbvtVolume& volume);
+	bool update(btDbvtNode* leaf, btDbvtVolume& volume, const btVector3& velocity, btScalar margin);
+	bool update(btDbvtNode* leaf, btDbvtVolume& volume, const btVector3& velocity);
+	bool update(btDbvtNode* leaf, btDbvtVolume& volume, btScalar margin);
+	void remove(btDbvtNode* leaf);
+	void write(IWriter* iwriter) const;
+	void clone(btDbvt& dest, IClone* iclone = 0) const;
+	static int maxdepth(const btDbvtNode* node);
+	static int countLeaves(const btDbvtNode* node);
+	static void extractLeaves(const btDbvtNode* node, btAlignedObjectArray<const btDbvtNode*>& leaves);
 #if DBVT_ENABLE_BENCHMARK
-	static void		benchmark();
+	static void benchmark();
 #else
-	static void		benchmark(){}
+	static void benchmark()
+	{
+	}
 #endif
 	// DBVT_IPOLICY must support ICollide policy/interface
 	DBVT_PREFIX
-		static void		enumNodes(	const btDbvtNode* root,
-		DBVT_IPOLICY);
+	static void enumNodes(const btDbvtNode* root,
+						  DBVT_IPOLICY);
 	DBVT_PREFIX
-		static void		enumLeaves(	const btDbvtNode* root,
-		DBVT_IPOLICY);
+	static void enumLeaves(const btDbvtNode* root,
+						   DBVT_IPOLICY);
 	DBVT_PREFIX
-		void		collideTT(	const btDbvtNode* root0,
-		const btDbvtNode* root1,
-		DBVT_IPOLICY);
+	void collideTT(const btDbvtNode* root0,
+				   const btDbvtNode* root1,
+				   DBVT_IPOLICY);
+    DBVT_PREFIX
+    void selfCollideT(const btDbvntNode* root,
+                   DBVT_IPOLICY);
+    DBVT_PREFIX
+    void selfCollideTT(const btDbvtNode* root,
+                      DBVT_IPOLICY);
 
 	DBVT_PREFIX
-		void		collideTTpersistentStack(	const btDbvtNode* root0,
-		  const btDbvtNode* root1,
-		  DBVT_IPOLICY);
+	void collideTTpersistentStack(const btDbvtNode* root0,
+								  const btDbvtNode* root1,
+								  DBVT_IPOLICY);
 #if 0
 	DBVT_PREFIX
 		void		collideTT(	const btDbvtNode* root0,
@@ -322,71 +371,89 @@ struct	btDbvt
 #endif
 
 	DBVT_PREFIX
-		void		collideTV(	const btDbvtNode* root,
-		const btDbvtVolume& volume,
-		DBVT_IPOLICY) const;
+	void collideTV(const btDbvtNode* root,
+				   const btDbvtVolume& volume,
+				   DBVT_IPOLICY) const;
+
+	DBVT_PREFIX
+	void collideTVNoStackAlloc(const btDbvtNode* root,
+							   const btDbvtVolume& volume,
+							   btNodeStack& stack,
+							   DBVT_IPOLICY) const;
+
 	///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc)
 	///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time
 	DBVT_PREFIX
-		static void		rayTest(	const btDbvtNode* root,
-		const btVector3& rayFrom,
-		const btVector3& rayTo,
-		DBVT_IPOLICY);
+	static void rayTest(const btDbvtNode* root,
+						const btVector3& rayFrom,
+						const btVector3& rayTo,
+						DBVT_IPOLICY);
 	///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections
 	///rayTestInternal is used by btDbvtBroadphase to accelerate world ray casts
 	DBVT_PREFIX
-		void		rayTestInternal(	const btDbvtNode* root,
-								const btVector3& rayFrom,
-								const btVector3& rayTo,
-								const btVector3& rayDirectionInverse,
-								unsigned int signs[3],
-								btScalar lambda_max,
-								const btVector3& aabbMin,
-								const btVector3& aabbMax,
-								DBVT_IPOLICY) const;
+	void rayTestInternal(const btDbvtNode* root,
+						 const btVector3& rayFrom,
+						 const btVector3& rayTo,
+						 const btVector3& rayDirectionInverse,
+						 unsigned int signs[3],
+						 btScalar lambda_max,
+						 const btVector3& aabbMin,
+						 const btVector3& aabbMax,
+						 btAlignedObjectArray<const btDbvtNode*>& stack,
+						 DBVT_IPOLICY) const;
 
 	DBVT_PREFIX
-		static void		collideKDOP(const btDbvtNode* root,
-		const btVector3* normals,
-		const btScalar* offsets,
-		int count,
-		DBVT_IPOLICY);
+	static void collideKDOP(const btDbvtNode* root,
+							const btVector3* normals,
+							const btScalar* offsets,
+							int count,
+							DBVT_IPOLICY);
 	DBVT_PREFIX
-		static void		collideOCL(	const btDbvtNode* root,
-		const btVector3* normals,
-		const btScalar* offsets,
-		const btVector3& sortaxis,
-		int count,								
-		DBVT_IPOLICY,
-		bool fullsort=true);
+	static void collideOCL(const btDbvtNode* root,
+						   const btVector3* normals,
+						   const btScalar* offsets,
+						   const btVector3& sortaxis,
+						   int count,
+						   DBVT_IPOLICY,
+						   bool fullsort = true);
 	DBVT_PREFIX
-		static void		collideTU(	const btDbvtNode* root,
-		DBVT_IPOLICY);
-	// Helpers	
-	static DBVT_INLINE int	nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h)
+	static void collideTU(const btDbvtNode* root,
+						  DBVT_IPOLICY);
+	// Helpers
+	static DBVT_INLINE int nearest(const int* i, const btDbvt::sStkNPS* a, btScalar v, int l, int h)
 	{
-		int	m=0;
-		while(l<h)
+		int m = 0;
+		while (l < h)
 		{
-			m=(l+h)>>1;
-			if(a[i[m]].value>=v) l=m+1; else h=m;
+			m = (l + h) >> 1;
+			if (a[i[m]].value >= v)
+				l = m + 1;
+			else
+				h = m;
 		}
-		return(h);
+		return (h);
 	}
-	static DBVT_INLINE int	allocate(	btAlignedObjectArray<int>& ifree,
-		btAlignedObjectArray<sStkNPS>& stock,
-		const sStkNPS& value)
+	static DBVT_INLINE int allocate(btAlignedObjectArray<int>& ifree,
+									btAlignedObjectArray<sStkNPS>& stock,
+									const sStkNPS& value)
 	{
-		int	i;
-		if(ifree.size()>0)
-		{ i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
+		int i;
+		if (ifree.size() > 0)
+		{
+			i = ifree[ifree.size() - 1];
+			ifree.pop_back();
+			stock[i] = value;
+		}
 		else
-		{ i=stock.size();stock.push_back(value); }
-		return(i); 
+		{
+			i = stock.size();
+			stock.push_back(value);
+		}
+		return (i);
 	}
 	//
 private:
-	btDbvt(const btDbvt&)	{}	
+	btDbvt(const btDbvt&) {}
 };
 
 //
@@ -394,227 +461,252 @@ private:
 //
 
 //
-inline btDbvtAabbMm			btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
+inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c, const btVector3& e)
 {
 	btDbvtAabbMm box;
-	box.mi=c-e;box.mx=c+e;
-	return(box);
+	box.mi = c - e;
+	box.mx = c + e;
+	return (box);
 }
 
 //
-inline btDbvtAabbMm			btDbvtAabbMm::FromCR(const btVector3& c,btScalar r)
+inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c, btScalar r)
 {
-	return(FromCE(c,btVector3(r,r,r)));
+	return (FromCE(c, btVector3(r, r, r)));
 }
 
 //
-inline btDbvtAabbMm			btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
+inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi, const btVector3& mx)
 {
 	btDbvtAabbMm box;
-	box.mi=mi;box.mx=mx;
-	return(box);
+	box.mi = mi;
+	box.mx = mx;
+	return (box);
 }
 
 //
-inline btDbvtAabbMm			btDbvtAabbMm::FromPoints(const btVector3* pts,int n)
+inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts, int n)
 {
 	btDbvtAabbMm box;
-	box.mi=box.mx=pts[0];
-	for(int i=1;i<n;++i)
+	box.mi = box.mx = pts[0];
+	for (int i = 1; i < n; ++i)
 	{
 		box.mi.setMin(pts[i]);
 		box.mx.setMax(pts[i]);
 	}
-	return(box);
+	return (box);
 }
 
 //
-inline btDbvtAabbMm			btDbvtAabbMm::FromPoints(const btVector3** ppts,int n)
+inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts, int n)
 {
 	btDbvtAabbMm box;
-	box.mi=box.mx=*ppts[0];
-	for(int i=1;i<n;++i)
+	box.mi = box.mx = *ppts[0];
+	for (int i = 1; i < n; ++i)
 	{
 		box.mi.setMin(*ppts[i]);
 		box.mx.setMax(*ppts[i]);
 	}
-	return(box);
+	return (box);
 }
 
 //
-DBVT_INLINE void		btDbvtAabbMm::Expand(const btVector3& e)
+DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e)
 {
-	mi-=e;mx+=e;
+	mi -= e;
+	mx += e;
 }
 
 //
-DBVT_INLINE void		btDbvtAabbMm::SignedExpand(const btVector3& e)
+DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3& e)
 {
-	if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]);
-	if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]);
-	if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]);
+	if (e.x() > 0)
+		mx.setX(mx.x() + e[0]);
+	else
+		mi.setX(mi.x() + e[0]);
+	if (e.y() > 0)
+		mx.setY(mx.y() + e[1]);
+	else
+		mi.setY(mi.y() + e[1]);
+	if (e.z() > 0)
+		mx.setZ(mx.z() + e[2]);
+	else
+		mi.setZ(mi.z() + e[2]);
 }
 
 //
-DBVT_INLINE bool		btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
+DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
 {
-	return(	(mi.x()<=a.mi.x())&&
-		(mi.y()<=a.mi.y())&&
-		(mi.z()<=a.mi.z())&&
-		(mx.x()>=a.mx.x())&&
-		(mx.y()>=a.mx.y())&&
-		(mx.z()>=a.mx.z()));
+	return ((mi.x() <= a.mi.x()) &&
+			(mi.y() <= a.mi.y()) &&
+			(mi.z() <= a.mi.z()) &&
+			(mx.x() >= a.mx.x()) &&
+			(mx.y() >= a.mx.y()) &&
+			(mx.z() >= a.mx.z()));
 }
 
 //
-DBVT_INLINE int		btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const
+DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n, btScalar o, int s) const
 {
-	btVector3			pi,px;
-	switch(s)
+	btVector3 pi, px;
+	switch (s)
 	{
-	case	(0+0+0):	px=btVector3(mi.x(),mi.y(),mi.z());
-		pi=btVector3(mx.x(),mx.y(),mx.z());break;
-	case	(1+0+0):	px=btVector3(mx.x(),mi.y(),mi.z());
-		pi=btVector3(mi.x(),mx.y(),mx.z());break;
-	case	(0+2+0):	px=btVector3(mi.x(),mx.y(),mi.z());
-		pi=btVector3(mx.x(),mi.y(),mx.z());break;
-	case	(1+2+0):	px=btVector3(mx.x(),mx.y(),mi.z());
-		pi=btVector3(mi.x(),mi.y(),mx.z());break;
-	case	(0+0+4):	px=btVector3(mi.x(),mi.y(),mx.z());
-		pi=btVector3(mx.x(),mx.y(),mi.z());break;
-	case	(1+0+4):	px=btVector3(mx.x(),mi.y(),mx.z());
-		pi=btVector3(mi.x(),mx.y(),mi.z());break;
-	case	(0+2+4):	px=btVector3(mi.x(),mx.y(),mx.z());
-		pi=btVector3(mx.x(),mi.y(),mi.z());break;
-	case	(1+2+4):	px=btVector3(mx.x(),mx.y(),mx.z());
-		pi=btVector3(mi.x(),mi.y(),mi.z());break;
+		case (0 + 0 + 0):
+			px = btVector3(mi.x(), mi.y(), mi.z());
+			pi = btVector3(mx.x(), mx.y(), mx.z());
+			break;
+		case (1 + 0 + 0):
+			px = btVector3(mx.x(), mi.y(), mi.z());
+			pi = btVector3(mi.x(), mx.y(), mx.z());
+			break;
+		case (0 + 2 + 0):
+			px = btVector3(mi.x(), mx.y(), mi.z());
+			pi = btVector3(mx.x(), mi.y(), mx.z());
+			break;
+		case (1 + 2 + 0):
+			px = btVector3(mx.x(), mx.y(), mi.z());
+			pi = btVector3(mi.x(), mi.y(), mx.z());
+			break;
+		case (0 + 0 + 4):
+			px = btVector3(mi.x(), mi.y(), mx.z());
+			pi = btVector3(mx.x(), mx.y(), mi.z());
+			break;
+		case (1 + 0 + 4):
+			px = btVector3(mx.x(), mi.y(), mx.z());
+			pi = btVector3(mi.x(), mx.y(), mi.z());
+			break;
+		case (0 + 2 + 4):
+			px = btVector3(mi.x(), mx.y(), mx.z());
+			pi = btVector3(mx.x(), mi.y(), mi.z());
+			break;
+		case (1 + 2 + 4):
+			px = btVector3(mx.x(), mx.y(), mx.z());
+			pi = btVector3(mi.x(), mi.y(), mi.z());
+			break;
 	}
-	if((btDot(n,px)+o)<0)		return(-1);
-	if((btDot(n,pi)+o)>=0)	return(+1);
-	return(0);
+	if ((btDot(n, px) + o) < 0) return (-1);
+	if ((btDot(n, pi) + o) >= 0) return (+1);
+	return (0);
 }
 
 //
-DBVT_INLINE btScalar	btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const
+DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v, unsigned signs) const
 {
-	const btVector3*	b[]={&mx,&mi};
-	const btVector3		p(	b[(signs>>0)&1]->x(),
-		b[(signs>>1)&1]->y(),
-		b[(signs>>2)&1]->z());
-	return(btDot(p,v));
+	const btVector3* b[] = {&mx, &mi};
+	const btVector3 p(b[(signs >> 0) & 1]->x(),
+					  b[(signs >> 1) & 1]->y(),
+					  b[(signs >> 2) & 1]->z());
+	return (btDot(p, v));
 }
 
 //
-DBVT_INLINE void		btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const
+DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d, btScalar& smi, btScalar& smx) const
 {
-	for(int i=0;i<3;++i)
+	for (int i = 0; i < 3; ++i)
 	{
-		if(d[i]<0)
-		{ smi+=mx[i]*d[i];smx+=mi[i]*d[i]; }
+		if (d[i] < 0)
+		{
+			smi += mx[i] * d[i];
+			smx += mi[i] * d[i];
+		}
 		else
-		{ smi+=mi[i]*d[i];smx+=mx[i]*d[i]; }
+		{
+			smi += mi[i] * d[i];
+			smx += mx[i] * d[i];
+		}
 	}
 }
 
 //
-DBVT_INLINE bool		Intersect(	const btDbvtAabbMm& a,
-								  const btDbvtAabbMm& b)
+DBVT_INLINE bool Intersect(const btDbvtAabbMm& a,
+						   const btDbvtAabbMm& b)
 {
-#if	DBVT_INT0_IMPL == DBVT_IMPL_SSE
-	const __m128	rt(_mm_or_ps(	_mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),
-		_mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi))));
-#if defined (_WIN32)
-	const __int32*	pu((const __int32*)&rt);
+#if DBVT_INT0_IMPL == DBVT_IMPL_SSE
+	const __m128 rt(_mm_or_ps(_mm_cmplt_ps(_mm_load_ps(b.mx), _mm_load_ps(a.mi)),
+							  _mm_cmplt_ps(_mm_load_ps(a.mx), _mm_load_ps(b.mi))));
+#if defined(_WIN32)
+	const __int32* pu((const __int32*)&rt);
 #else
-    const int*	pu((const int*)&rt);
+	const int* pu((const int*)&rt);
 #endif
-	return((pu[0]|pu[1]|pu[2])==0);
+	return ((pu[0] | pu[1] | pu[2]) == 0);
 #else
-	return(	(a.mi.x()<=b.mx.x())&&
-		(a.mx.x()>=b.mi.x())&&
-		(a.mi.y()<=b.mx.y())&&
-		(a.mx.y()>=b.mi.y())&&
-		(a.mi.z()<=b.mx.z())&&		
-		(a.mx.z()>=b.mi.z()));
+	return ((a.mi.x() <= b.mx.x()) &&
+			(a.mx.x() >= b.mi.x()) &&
+			(a.mi.y() <= b.mx.y()) &&
+			(a.mx.y() >= b.mi.y()) &&
+			(a.mi.z() <= b.mx.z()) &&
+			(a.mx.z() >= b.mi.z()));
 #endif
 }
 
-
-
 //
-DBVT_INLINE bool		Intersect(	const btDbvtAabbMm& a,
-								  const btVector3& b)
+DBVT_INLINE bool Intersect(const btDbvtAabbMm& a,
+						   const btVector3& b)
 {
-	return(	(b.x()>=a.mi.x())&&
-		(b.y()>=a.mi.y())&&
-		(b.z()>=a.mi.z())&&
-		(b.x()<=a.mx.x())&&
-		(b.y()<=a.mx.y())&&
-		(b.z()<=a.mx.z()));
+	return ((b.x() >= a.mi.x()) &&
+			(b.y() >= a.mi.y()) &&
+			(b.z() >= a.mi.z()) &&
+			(b.x() <= a.mx.x()) &&
+			(b.y() <= a.mx.y()) &&
+			(b.z() <= a.mx.z()));
 }
 
-
-
-
-
 //////////////////////////////////////
 
-
 //
-DBVT_INLINE btScalar	Proximity(	const btDbvtAabbMm& a,
-								  const btDbvtAabbMm& b)
+DBVT_INLINE btScalar Proximity(const btDbvtAabbMm& a,
+							   const btDbvtAabbMm& b)
 {
-	const btVector3	d=(a.mi+a.mx)-(b.mi+b.mx);
-	return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
+	const btVector3 d = (a.mi + a.mx) - (b.mi + b.mx);
+	return (btFabs(d.x()) + btFabs(d.y()) + btFabs(d.z()));
 }
 
-
-
 //
-DBVT_INLINE int			Select(	const btDbvtAabbMm& o,
-							   const btDbvtAabbMm& a,
-							   const btDbvtAabbMm& b)
+DBVT_INLINE int Select(const btDbvtAabbMm& o,
+					   const btDbvtAabbMm& a,
+					   const btDbvtAabbMm& b)
 {
-#if	DBVT_SELECT_IMPL == DBVT_IMPL_SSE
-    
-#if defined (_WIN32)
-	static ATTRIBUTE_ALIGNED16(const unsigned __int32)	mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
+#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE
+
+#if defined(_WIN32)
+	static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff};
 #else
-    static ATTRIBUTE_ALIGNED16(const unsigned int)	mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x00000000 /*0x7fffffff*/};
+	static ATTRIBUTE_ALIGNED16(const unsigned int) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x00000000 /*0x7fffffff*/};
 #endif
 	///@todo: the intrinsic version is 11% slower
 #if DBVT_USE_INTRINSIC_SSE
 
-	union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory
+	union btSSEUnion  ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory
 	{
-	   __m128		ssereg;
-	   float		floats[4];
-	   int			ints[4];
+		__m128 ssereg;
+		float floats[4];
+		int ints[4];
 	};
 
-	__m128	omi(_mm_load_ps(o.mi));
-	omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
-	__m128	ami(_mm_load_ps(a.mi));
-	ami=_mm_add_ps(ami,_mm_load_ps(a.mx));
-	ami=_mm_sub_ps(ami,omi);
-	ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask));
-	__m128	bmi(_mm_load_ps(b.mi));
-	bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx));
-	bmi=_mm_sub_ps(bmi,omi);
-	bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask));
-	__m128	t0(_mm_movehl_ps(ami,ami));
-	ami=_mm_add_ps(ami,t0);
-	ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
-	__m128 t1(_mm_movehl_ps(bmi,bmi));
-	bmi=_mm_add_ps(bmi,t1);
-	bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
-	
+	__m128 omi(_mm_load_ps(o.mi));
+	omi = _mm_add_ps(omi, _mm_load_ps(o.mx));
+	__m128 ami(_mm_load_ps(a.mi));
+	ami = _mm_add_ps(ami, _mm_load_ps(a.mx));
+	ami = _mm_sub_ps(ami, omi);
+	ami = _mm_and_ps(ami, _mm_load_ps((const float*)mask));
+	__m128 bmi(_mm_load_ps(b.mi));
+	bmi = _mm_add_ps(bmi, _mm_load_ps(b.mx));
+	bmi = _mm_sub_ps(bmi, omi);
+	bmi = _mm_and_ps(bmi, _mm_load_ps((const float*)mask));
+	__m128 t0(_mm_movehl_ps(ami, ami));
+	ami = _mm_add_ps(ami, t0);
+	ami = _mm_add_ss(ami, _mm_shuffle_ps(ami, ami, 1));
+	__m128 t1(_mm_movehl_ps(bmi, bmi));
+	bmi = _mm_add_ps(bmi, t1);
+	bmi = _mm_add_ss(bmi, _mm_shuffle_ps(bmi, bmi, 1));
+
 	btSSEUnion tmp;
-	tmp.ssereg = _mm_cmple_ss(bmi,ami);
-	return tmp.ints[0]&1;
+	tmp.ssereg = _mm_cmple_ss(bmi, ami);
+	return tmp.ints[0] & 1;
 
 #else
-	ATTRIBUTE_ALIGNED16(__int32	r[1]);
+	ATTRIBUTE_ALIGNED16(__int32 r[1]);
 	__asm
 	{
 		mov		eax,o
@@ -642,46 +734,52 @@ DBVT_INLINE int			Select(	const btDbvtAabbMm& o,
 			cmpless	xmm2,xmm1
 			movss	r,xmm2
 	}
-	return(r[0]&1);
+	return (r[0] & 1);
 #endif
 #else
-	return(Proximity(o,a)<Proximity(o,b)?0:1);
+	return (Proximity(o, a) < Proximity(o, b) ? 0 : 1);
 #endif
 }
 
 //
-DBVT_INLINE void		Merge(	const btDbvtAabbMm& a,
-							  const btDbvtAabbMm& b,
-							  btDbvtAabbMm& r)
+DBVT_INLINE void Merge(const btDbvtAabbMm& a,
+					   const btDbvtAabbMm& b,
+					   btDbvtAabbMm& r)
 {
-#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
-	__m128	ami(_mm_load_ps(a.mi));
-	__m128	amx(_mm_load_ps(a.mx));
-	__m128	bmi(_mm_load_ps(b.mi));
-	__m128	bmx(_mm_load_ps(b.mx));
-	ami=_mm_min_ps(ami,bmi);
-	amx=_mm_max_ps(amx,bmx);
-	_mm_store_ps(r.mi,ami);
-	_mm_store_ps(r.mx,amx);
+#if DBVT_MERGE_IMPL == DBVT_IMPL_SSE
+	__m128 ami(_mm_load_ps(a.mi));
+	__m128 amx(_mm_load_ps(a.mx));
+	__m128 bmi(_mm_load_ps(b.mi));
+	__m128 bmx(_mm_load_ps(b.mx));
+	ami = _mm_min_ps(ami, bmi);
+	amx = _mm_max_ps(amx, bmx);
+	_mm_store_ps(r.mi, ami);
+	_mm_store_ps(r.mx, amx);
 #else
-	for(int i=0;i<3;++i)
+	for (int i = 0; i < 3; ++i)
 	{
-		if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
-		if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
+		if (a.mi[i] < b.mi[i])
+			r.mi[i] = a.mi[i];
+		else
+			r.mi[i] = b.mi[i];
+		if (a.mx[i] > b.mx[i])
+			r.mx[i] = a.mx[i];
+		else
+			r.mx[i] = b.mx[i];
 	}
 #endif
 }
 
 //
-DBVT_INLINE bool		NotEqual(	const btDbvtAabbMm& a,
-								 const btDbvtAabbMm& b)
+DBVT_INLINE bool NotEqual(const btDbvtAabbMm& a,
+						  const btDbvtAabbMm& b)
 {
-	return(	(a.mi.x()!=b.mi.x())||
-		(a.mi.y()!=b.mi.y())||
-		(a.mi.z()!=b.mi.z())||
-		(a.mx.x()!=b.mx.x())||
-		(a.mx.y()!=b.mx.y())||
-		(a.mx.z()!=b.mx.z()));
+	return ((a.mi.x() != b.mi.x()) ||
+			(a.mi.y() != b.mi.y()) ||
+			(a.mi.z() != b.mi.z()) ||
+			(a.mx.x() != b.mx.x()) ||
+			(a.mx.y() != b.mx.y()) ||
+			(a.mx.z() != b.mx.z()));
 }
 
 //
@@ -690,162 +788,291 @@ DBVT_INLINE bool		NotEqual(	const btDbvtAabbMm& a,
 
 //
 DBVT_PREFIX
-inline void		btDbvt::enumNodes(	const btDbvtNode* root,
-								  DBVT_IPOLICY)
+inline void btDbvt::enumNodes(const btDbvtNode* root,
+							  DBVT_IPOLICY)
 {
 	DBVT_CHECKTYPE
-		policy.Process(root);
-	if(root->isinternal())
+	policy.Process(root);
+	if (root->isinternal())
 	{
-		enumNodes(root->childs[0],policy);
-		enumNodes(root->childs[1],policy);
+		enumNodes(root->childs[0], policy);
+		enumNodes(root->childs[1], policy);
 	}
 }
 
 //
 DBVT_PREFIX
-inline void		btDbvt::enumLeaves(	const btDbvtNode* root,
-								   DBVT_IPOLICY)
+inline void btDbvt::enumLeaves(const btDbvtNode* root,
+							   DBVT_IPOLICY)
 {
 	DBVT_CHECKTYPE
-		if(root->isinternal())
-		{
-			enumLeaves(root->childs[0],policy);
-			enumLeaves(root->childs[1],policy);
-		}
-		else
-		{
-			policy.Process(root);
-		}
+	if (root->isinternal())
+	{
+		enumLeaves(root->childs[0], policy);
+		enumLeaves(root->childs[1], policy);
+	}
+	else
+	{
+		policy.Process(root);
+	}
 }
 
 //
 DBVT_PREFIX
-inline void		btDbvt::collideTT(	const btDbvtNode* root0,
-								  const btDbvtNode* root1,
-								  DBVT_IPOLICY)
+inline void btDbvt::collideTT(const btDbvtNode* root0,
+							  const btDbvtNode* root1,
+							  DBVT_IPOLICY)
 {
 	DBVT_CHECKTYPE
-		if(root0&&root1)
+	if (root0 && root1)
+	{
+		int depth = 1;
+		int treshold = DOUBLE_STACKSIZE - 4;
+		btAlignedObjectArray<sStkNN> stkStack;
+		stkStack.resize(DOUBLE_STACKSIZE);
+		stkStack[0] = sStkNN(root0, root1);
+		do
 		{
-			int								depth=1;
-			int								treshold=DOUBLE_STACKSIZE-4;
-			btAlignedObjectArray<sStkNN>	stkStack;
-			stkStack.resize(DOUBLE_STACKSIZE);
-			stkStack[0]=sStkNN(root0,root1);
-			do	{		
-				sStkNN	p=stkStack[--depth];
-				if(depth>treshold)
+			sStkNN p = stkStack[--depth];
+			if (depth > treshold)
+			{
+				stkStack.resize(stkStack.size() * 2);
+				treshold = stkStack.size() - 4;
+			}
+			if (p.a == p.b)
+			{
+				if (p.a->isinternal())
 				{
-					stkStack.resize(stkStack.size()*2);
-					treshold=stkStack.size()-4;
+					stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]);
+					stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]);
+					stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[1]);
 				}
-				if(p.a==p.b)
+			}
+			else if (Intersect(p.a->volume, p.b->volume))
+			{
+				if (p.a->isinternal())
 				{
-					if(p.a->isinternal())
+					if (p.b->isinternal())
+					{
+						stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]);
+						stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]);
+						stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]);
+						stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]);
+					}
+					else
 					{
-						stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
-						stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
-						stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
+						stkStack[depth++] = sStkNN(p.a->childs[0], p.b);
+						stkStack[depth++] = sStkNN(p.a->childs[1], p.b);
 					}
 				}
-				else if(Intersect(p.a->volume,p.b->volume))
+				else
 				{
-					if(p.a->isinternal())
+					if (p.b->isinternal())
 					{
-						if(p.b->isinternal())
-						{
-							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
-							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
-							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
-							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
-						}
-						else
-						{
-							stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
-							stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
-						}
+						stkStack[depth++] = sStkNN(p.a, p.b->childs[0]);
+						stkStack[depth++] = sStkNN(p.a, p.b->childs[1]);
 					}
 					else
 					{
-						if(p.b->isinternal())
-						{
-							stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
-							stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
-						}
-						else
-						{
-							policy.Process(p.a,p.b);
-						}
+						policy.Process(p.a, p.b);
 					}
 				}
-			} while(depth);
-		}
+			}
+		} while (depth);
+	}
+}
+
+//
+DBVT_PREFIX
+inline void btDbvt::selfCollideT(const btDbvntNode* root,
+                              DBVT_IPOLICY)
+{
+    DBVT_CHECKTYPE
+    if (root)
+    {
+        int depth = 1;
+        int treshold = DOUBLE_STACKSIZE - 4;
+        btAlignedObjectArray<sStknNN> stkStack;
+        stkStack.resize(DOUBLE_STACKSIZE);
+        stkStack[0] = sStknNN(root, root);
+        do
+        {
+            sStknNN p = stkStack[--depth];
+            if (depth > treshold)
+            {
+                stkStack.resize(stkStack.size() * 2);
+                treshold = stkStack.size() - 4;
+            }
+            if (p.a == p.b)
+            {
+                if (p.a->isinternal() && p.a->angle > SIMD_PI)
+                {
+                    stkStack[depth++] = sStknNN(p.a->childs[0], p.a->childs[0]);
+                    stkStack[depth++] = sStknNN(p.a->childs[1], p.a->childs[1]);
+                    stkStack[depth++] = sStknNN(p.a->childs[0], p.a->childs[1]);
+                }
+            }
+            else if (Intersect(p.a->volume, p.b->volume))
+            {
+                if (p.a->isinternal())
+                {
+                    if (p.b->isinternal())
+                    {
+                        stkStack[depth++] = sStknNN(p.a->childs[0], p.b->childs[0]);
+                        stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[0]);
+                        stkStack[depth++] = sStknNN(p.a->childs[0], p.b->childs[1]);
+                        stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[1]);
+                    }
+                    else
+                    {
+                        stkStack[depth++] = sStknNN(p.a->childs[0], p.b);
+                        stkStack[depth++] = sStknNN(p.a->childs[1], p.b);
+                    }
+                }
+                else
+                {
+                    if (p.b->isinternal())
+                    {
+                        stkStack[depth++] = sStknNN(p.a, p.b->childs[0]);
+                        stkStack[depth++] = sStknNN(p.a, p.b->childs[1]);
+                    }
+                    else
+                    {
+                        policy.Process(p.a, p.b);
+                    }
+                }
+            }
+        } while (depth);
+    }
 }
 
+//
+DBVT_PREFIX
+inline void btDbvt::selfCollideTT(const btDbvtNode* root,
+                                 DBVT_IPOLICY)
+{
+    DBVT_CHECKTYPE
+    if (root)
+    {
+        int depth = 1;
+        int treshold = DOUBLE_STACKSIZE - 4;
+        btAlignedObjectArray<sStkNN> stkStack;
+        stkStack.resize(DOUBLE_STACKSIZE);
+        stkStack[0] = sStkNN(root, root);
+        do
+        {
+            sStkNN p = stkStack[--depth];
+            if (depth > treshold)
+            {
+                stkStack.resize(stkStack.size() * 2);
+                treshold = stkStack.size() - 4;
+            }
+            if (p.a == p.b)
+            {
+                if (p.a->isinternal())
+                {
+                    stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]);
+                    stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]);
+                    stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[1]);
+                }
+            }
+            else if (Intersect(p.a->volume, p.b->volume))
+            {
+                if (p.a->isinternal())
+                {
+                    if (p.b->isinternal())
+                    {
+                        stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]);
+                        stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]);
+                        stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]);
+                        stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]);
+                    }
+                    else
+                    {
+                        stkStack[depth++] = sStkNN(p.a->childs[0], p.b);
+                        stkStack[depth++] = sStkNN(p.a->childs[1], p.b);
+                    }
+                }
+                else
+                {
+                    if (p.b->isinternal())
+                    {
+                        stkStack[depth++] = sStkNN(p.a, p.b->childs[0]);
+                        stkStack[depth++] = sStkNN(p.a, p.b->childs[1]);
+                    }
+                    else
+                    {
+                        policy.Process(p.a, p.b);
+                    }
+                }
+            }
+        } while (depth);
+    }
+}
 
 
 DBVT_PREFIX
-inline void		btDbvt::collideTTpersistentStack(	const btDbvtNode* root0,
-								  const btDbvtNode* root1,
-								  DBVT_IPOLICY)
+inline void btDbvt::collideTTpersistentStack(const btDbvtNode* root0,
+											 const btDbvtNode* root1,
+											 DBVT_IPOLICY)
 {
 	DBVT_CHECKTYPE
-		if(root0&&root1)
+	if (root0 && root1)
+	{
+		int depth = 1;
+		int treshold = DOUBLE_STACKSIZE - 4;
+
+		m_stkStack.resize(DOUBLE_STACKSIZE);
+		m_stkStack[0] = sStkNN(root0, root1);
+		do
 		{
-			int								depth=1;
-			int								treshold=DOUBLE_STACKSIZE-4;
-			
-			m_stkStack.resize(DOUBLE_STACKSIZE);
-			m_stkStack[0]=sStkNN(root0,root1);
-			do	{		
-				sStkNN	p=m_stkStack[--depth];
-				if(depth>treshold)
+			sStkNN p = m_stkStack[--depth];
+			if (depth > treshold)
+			{
+				m_stkStack.resize(m_stkStack.size() * 2);
+				treshold = m_stkStack.size() - 4;
+			}
+			if (p.a == p.b)
+			{
+				if (p.a->isinternal())
 				{
-					m_stkStack.resize(m_stkStack.size()*2);
-					treshold=m_stkStack.size()-4;
+					m_stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]);
+					m_stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]);
+					m_stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[1]);
 				}
-				if(p.a==p.b)
+			}
+			else if (Intersect(p.a->volume, p.b->volume))
+			{
+				if (p.a->isinternal())
 				{
-					if(p.a->isinternal())
+					if (p.b->isinternal())
 					{
-						m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
-						m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
-						m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
+						m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]);
+						m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]);
+						m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]);
+						m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]);
+					}
+					else
+					{
+						m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b);
+						m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b);
 					}
 				}
-				else if(Intersect(p.a->volume,p.b->volume))
+				else
 				{
-					if(p.a->isinternal())
+					if (p.b->isinternal())
 					{
-						if(p.b->isinternal())
-						{
-							m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
-							m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
-							m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
-							m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
-						}
-						else
-						{
-							m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
-							m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
-						}
+						m_stkStack[depth++] = sStkNN(p.a, p.b->childs[0]);
+						m_stkStack[depth++] = sStkNN(p.a, p.b->childs[1]);
 					}
 					else
 					{
-						if(p.b->isinternal())
-						{
-							m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
-							m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
-						}
-						else
-						{
-							policy.Process(p.a,p.b);
-						}
+						policy.Process(p.a, p.b);
 					}
 				}
-			} while(depth);
-		}
+			}
+		} while (depth);
+	}
 }
 
 #if 0
@@ -915,353 +1142,419 @@ inline void		btDbvt::collideTT(	const btDbvtNode* root0,
 	const btTransform	xform=xform0.inverse()*xform1;
 	collideTT(root0,root1,xform,policy);
 }
-#endif 
+#endif
+
+DBVT_PREFIX
+inline void btDbvt::collideTV(const btDbvtNode* root,
+							  const btDbvtVolume& vol,
+							  DBVT_IPOLICY) const
+{
+	DBVT_CHECKTYPE
+	if (root)
+	{
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)
+		volume(vol);
+		btAlignedObjectArray<const btDbvtNode*> stack;
+		stack.resize(0);
+#ifndef BT_DISABLE_STACK_TEMP_MEMORY
+		char tempmemory[SIMPLE_STACKSIZE * sizeof(const btDbvtNode*)];
+		stack.initializeFromBuffer(tempmemory, 0, SIMPLE_STACKSIZE);
+#else
+		stack.reserve(SIMPLE_STACKSIZE);
+#endif  //BT_DISABLE_STACK_TEMP_MEMORY
+
+		stack.push_back(root);
+		do
+		{
+			const btDbvtNode* n = stack[stack.size() - 1];
+			stack.pop_back();
+			if (Intersect(n->volume, volume))
+			{
+				if (n->isinternal())
+				{
+					stack.push_back(n->childs[0]);
+					stack.push_back(n->childs[1]);
+				}
+				else
+				{
+					policy.Process(n);
+				}
+			}
+		} while (stack.size() > 0);
+	}
+}
 
 //
 DBVT_PREFIX
-inline void		btDbvt::collideTV(	const btDbvtNode* root,
-								  const btDbvtVolume& vol,
-								  DBVT_IPOLICY) const
+inline void btDbvt::collideTVNoStackAlloc(const btDbvtNode* root,
+										  const btDbvtVolume& vol,
+										  btNodeStack& stack,
+										  DBVT_IPOLICY) const
 {
 	DBVT_CHECKTYPE
-		if(root)
+	if (root)
+	{
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)
+		volume(vol);
+		stack.resize(0);
+		stack.reserve(SIMPLE_STACKSIZE);
+		stack.push_back(root);
+		do
 		{
-			ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume(vol);
-			btAlignedObjectArray<const btDbvtNode*>	stack;
-			stack.resize(0);
-			stack.reserve(SIMPLE_STACKSIZE);
-			stack.push_back(root);
-			do	{
-				const btDbvtNode*	n=stack[stack.size()-1];
-				stack.pop_back();
-				if(Intersect(n->volume,volume))
+			const btDbvtNode* n = stack[stack.size() - 1];
+			stack.pop_back();
+			if (Intersect(n->volume, volume))
+			{
+				if (n->isinternal())
 				{
-					if(n->isinternal())
-					{
-						stack.push_back(n->childs[0]);
-						stack.push_back(n->childs[1]);
-					}
-					else
-					{
-						policy.Process(n);
-					}
+					stack.push_back(n->childs[0]);
+					stack.push_back(n->childs[1]);
 				}
-			} while(stack.size()>0);
-		}
+				else
+				{
+					policy.Process(n);
+				}
+			}
+		} while (stack.size() > 0);
+	}
 }
 
 DBVT_PREFIX
-inline void		btDbvt::rayTestInternal(	const btDbvtNode* root,
-								const btVector3& rayFrom,
-								const btVector3& rayTo,
-								const btVector3& rayDirectionInverse,
-								unsigned int signs[3],
-								btScalar lambda_max,
-								const btVector3& aabbMin,
-								const btVector3& aabbMax,
-								DBVT_IPOLICY) const
+inline void btDbvt::rayTestInternal(const btDbvtNode* root,
+									const btVector3& rayFrom,
+									const btVector3& rayTo,
+									const btVector3& rayDirectionInverse,
+									unsigned int signs[3],
+									btScalar lambda_max,
+									const btVector3& aabbMin,
+									const btVector3& aabbMax,
+									btAlignedObjectArray<const btDbvtNode*>& stack,
+									DBVT_IPOLICY) const
 {
-        (void) rayTo;
+	(void)rayTo;
 	DBVT_CHECKTYPE
-	if(root)
+	if (root)
 	{
 		btVector3 resultNormal;
 
-		int								depth=1;
-		int								treshold=DOUBLE_STACKSIZE-2;
-		btAlignedObjectArray<const btDbvtNode*>&	stack = m_rayTestStack;
+		int depth = 1;
+		int treshold = DOUBLE_STACKSIZE - 2;
 		stack.resize(DOUBLE_STACKSIZE);
-		stack[0]=root;
+		stack[0] = root;
 		btVector3 bounds[2];
-		do	
+		do
 		{
-			const btDbvtNode*	node=stack[--depth];
-			bounds[0] = node->volume.Mins()-aabbMax;
-			bounds[1] = node->volume.Maxs()-aabbMin;
-			btScalar tmin=1.f,lambda_min=0.f;
-			unsigned int result1=false;
-			result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
-			if(result1)
+			const btDbvtNode* node = stack[--depth];
+			bounds[0] = node->volume.Mins() - aabbMax;
+			bounds[1] = node->volume.Maxs() - aabbMin;
+			btScalar tmin = 1.f, lambda_min = 0.f;
+			unsigned int result1 = false;
+			result1 = btRayAabb2(rayFrom, rayDirectionInverse, signs, bounds, tmin, lambda_min, lambda_max);
+			if (result1)
 			{
-				if(node->isinternal())
+				if (node->isinternal())
 				{
-					if(depth>treshold)
+					if (depth > treshold)
 					{
-						stack.resize(stack.size()*2);
-						treshold=stack.size()-2;
+						stack.resize(stack.size() * 2);
+						treshold = stack.size() - 2;
 					}
-					stack[depth++]=node->childs[0];
-					stack[depth++]=node->childs[1];
+					stack[depth++] = node->childs[0];
+					stack[depth++] = node->childs[1];
 				}
 				else
 				{
 					policy.Process(node);
 				}
 			}
-		} while(depth);
+		} while (depth);
 	}
 }
 
 //
 DBVT_PREFIX
-inline void		btDbvt::rayTest(	const btDbvtNode* root,
-								const btVector3& rayFrom,
-								const btVector3& rayTo,
-								DBVT_IPOLICY)
+inline void btDbvt::rayTest(const btDbvtNode* root,
+							const btVector3& rayFrom,
+							const btVector3& rayTo,
+							DBVT_IPOLICY)
 {
 	DBVT_CHECKTYPE
-		if(root)
-		{
-			btVector3 rayDir = (rayTo-rayFrom);
-			rayDir.normalize ();
+	if (root)
+	{
+		btVector3 rayDir = (rayTo - rayFrom);
+		rayDir.normalize();
 
-			///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
-			btVector3 rayDirectionInverse;
-			rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
-			rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
-			rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
-			unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
+		///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
+		btVector3 rayDirectionInverse;
+		rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
+		rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
+		rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
+		unsigned int signs[3] = {rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
 
-			btScalar lambda_max = rayDir.dot(rayTo-rayFrom);
+		btScalar lambda_max = rayDir.dot(rayTo - rayFrom);
 
-			btVector3 resultNormal;
+		btVector3 resultNormal;
 
-			btAlignedObjectArray<const btDbvtNode*>	stack;
+		btAlignedObjectArray<const btDbvtNode*> stack;
 
-			int								depth=1;
-			int								treshold=DOUBLE_STACKSIZE-2;
+		int depth = 1;
+		int treshold = DOUBLE_STACKSIZE - 2;
 
-			stack.resize(DOUBLE_STACKSIZE);
-			stack[0]=root;
-			btVector3 bounds[2];
-			do	{
-				const btDbvtNode*	node=stack[--depth];
+		char tempmemory[DOUBLE_STACKSIZE * sizeof(const btDbvtNode*)];
+#ifndef BT_DISABLE_STACK_TEMP_MEMORY
+		stack.initializeFromBuffer(tempmemory, DOUBLE_STACKSIZE, DOUBLE_STACKSIZE);
+#else   //BT_DISABLE_STACK_TEMP_MEMORY
+		stack.resize(DOUBLE_STACKSIZE);
+#endif  //BT_DISABLE_STACK_TEMP_MEMORY
+		stack[0] = root;
+		btVector3 bounds[2];
+		do
+		{
+			const btDbvtNode* node = stack[--depth];
 
-				bounds[0] = node->volume.Mins();
-				bounds[1] = node->volume.Maxs();
-				
-				btScalar tmin=1.f,lambda_min=0.f;
-				unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
+			bounds[0] = node->volume.Mins();
+			bounds[1] = node->volume.Maxs();
+
+			btScalar tmin = 1.f, lambda_min = 0.f;
+			unsigned int result1 = btRayAabb2(rayFrom, rayDirectionInverse, signs, bounds, tmin, lambda_min, lambda_max);
 
 #ifdef COMPARE_BTRAY_AABB2
-				btScalar param=1.f;
-				bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
-				btAssert(result1 == result2);
-#endif //TEST_BTRAY_AABB2
+			btScalar param = 1.f;
+			bool result2 = btRayAabb(rayFrom, rayTo, node->volume.Mins(), node->volume.Maxs(), param, resultNormal);
+			btAssert(result1 == result2);
+#endif  //TEST_BTRAY_AABB2
 
-				if(result1)
+			if (result1)
+			{
+				if (node->isinternal())
 				{
-					if(node->isinternal())
-					{
-						if(depth>treshold)
-						{
-							stack.resize(stack.size()*2);
-							treshold=stack.size()-2;
-						}
-						stack[depth++]=node->childs[0];
-						stack[depth++]=node->childs[1];
-					}
-					else
+					if (depth > treshold)
 					{
-						policy.Process(node);
+						stack.resize(stack.size() * 2);
+						treshold = stack.size() - 2;
 					}
+					stack[depth++] = node->childs[0];
+					stack[depth++] = node->childs[1];
 				}
-			} while(depth);
-
-		}
+				else
+				{
+					policy.Process(node);
+				}
+			}
+		} while (depth);
+	}
 }
 
 //
 DBVT_PREFIX
-inline void		btDbvt::collideKDOP(const btDbvtNode* root,
-									const btVector3* normals,
-									const btScalar* offsets,
-									int count,
-									DBVT_IPOLICY)
+inline void btDbvt::collideKDOP(const btDbvtNode* root,
+								const btVector3* normals,
+								const btScalar* offsets,
+								int count,
+								DBVT_IPOLICY)
 {
 	DBVT_CHECKTYPE
-		if(root)
+	if (root)
+	{
+		const int inside = (1 << count) - 1;
+		btAlignedObjectArray<sStkNP> stack;
+		int signs[sizeof(unsigned) * 8];
+		btAssert(count < int(sizeof(signs) / sizeof(signs[0])));
+		for (int i = 0; i < count; ++i)
+		{
+			signs[i] = ((normals[i].x() >= 0) ? 1 : 0) +
+					   ((normals[i].y() >= 0) ? 2 : 0) +
+					   ((normals[i].z() >= 0) ? 4 : 0);
+		}
+		stack.reserve(SIMPLE_STACKSIZE);
+		stack.push_back(sStkNP(root, 0));
+		do
 		{
-			const int						inside=(1<<count)-1;
-			btAlignedObjectArray<sStkNP>	stack;
-			int								signs[sizeof(unsigned)*8];
-			btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
-			for(int i=0;i<count;++i)
+			sStkNP se = stack[stack.size() - 1];
+			bool out = false;
+			stack.pop_back();
+			for (int i = 0, j = 1; (!out) && (i < count); ++i, j <<= 1)
 			{
-				signs[i]=	((normals[i].x()>=0)?1:0)+
-					((normals[i].y()>=0)?2:0)+
-					((normals[i].z()>=0)?4:0);
-			}
-			stack.reserve(SIMPLE_STACKSIZE);
-			stack.push_back(sStkNP(root,0));
-			do	{
-				sStkNP	se=stack[stack.size()-1];
-				bool	out=false;
-				stack.pop_back();
-				for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
+				if (0 == (se.mask & j))
 				{
-					if(0==(se.mask&j))
+					const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]);
+					switch (side)
 					{
-						const int	side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
-						switch(side)
-						{
-						case	-1:	out=true;break;
-						case	+1:	se.mask|=j;break;
-						}
+						case -1:
+							out = true;
+							break;
+						case +1:
+							se.mask |= j;
+							break;
 					}
 				}
-				if(!out)
+			}
+			if (!out)
+			{
+				if ((se.mask != inside) && (se.node->isinternal()))
 				{
-					if((se.mask!=inside)&&(se.node->isinternal()))
-					{
-						stack.push_back(sStkNP(se.node->childs[0],se.mask));
-						stack.push_back(sStkNP(se.node->childs[1],se.mask));
-					}
-					else
-					{
-						if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
-					}
+					stack.push_back(sStkNP(se.node->childs[0], se.mask));
+					stack.push_back(sStkNP(se.node->childs[1], se.mask));
 				}
-			} while(stack.size());
-		}
+				else
+				{
+					if (policy.AllLeaves(se.node)) enumLeaves(se.node, policy);
+				}
+			}
+		} while (stack.size());
+	}
 }
 
 //
 DBVT_PREFIX
-inline void		btDbvt::collideOCL(	const btDbvtNode* root,
-								   const btVector3* normals,
-								   const btScalar* offsets,
-								   const btVector3& sortaxis,
-								   int count,
-								   DBVT_IPOLICY,
-								   bool fsort)
+inline void btDbvt::collideOCL(const btDbvtNode* root,
+							   const btVector3* normals,
+							   const btScalar* offsets,
+							   const btVector3& sortaxis,
+							   int count,
+							   DBVT_IPOLICY,
+							   bool fsort)
 {
 	DBVT_CHECKTYPE
-		if(root)
+	if (root)
+	{
+		const unsigned srtsgns = (sortaxis[0] >= 0 ? 1 : 0) +
+								 (sortaxis[1] >= 0 ? 2 : 0) +
+								 (sortaxis[2] >= 0 ? 4 : 0);
+		const int inside = (1 << count) - 1;
+		btAlignedObjectArray<sStkNPS> stock;
+		btAlignedObjectArray<int> ifree;
+		btAlignedObjectArray<int> stack;
+		int signs[sizeof(unsigned) * 8];
+		btAssert(count < int(sizeof(signs) / sizeof(signs[0])));
+		for (int i = 0; i < count; ++i)
+		{
+			signs[i] = ((normals[i].x() >= 0) ? 1 : 0) +
+					   ((normals[i].y() >= 0) ? 2 : 0) +
+					   ((normals[i].z() >= 0) ? 4 : 0);
+		}
+		stock.reserve(SIMPLE_STACKSIZE);
+		stack.reserve(SIMPLE_STACKSIZE);
+		ifree.reserve(SIMPLE_STACKSIZE);
+		stack.push_back(allocate(ifree, stock, sStkNPS(root, 0, root->volume.ProjectMinimum(sortaxis, srtsgns))));
+		do
 		{
-			const unsigned					srtsgns=(sortaxis[0]>=0?1:0)+
-				(sortaxis[1]>=0?2:0)+
-				(sortaxis[2]>=0?4:0);
-			const int						inside=(1<<count)-1;
-			btAlignedObjectArray<sStkNPS>	stock;
-			btAlignedObjectArray<int>		ifree;
-			btAlignedObjectArray<int>		stack;
-			int								signs[sizeof(unsigned)*8];
-			btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
-			for(int i=0;i<count;++i)
+			const int id = stack[stack.size() - 1];
+			sStkNPS se = stock[id];
+			stack.pop_back();
+			ifree.push_back(id);
+			if (se.mask != inside)
 			{
-				signs[i]=	((normals[i].x()>=0)?1:0)+
-					((normals[i].y()>=0)?2:0)+
-					((normals[i].z()>=0)?4:0);
-			}
-			stock.reserve(SIMPLE_STACKSIZE);
-			stack.reserve(SIMPLE_STACKSIZE);
-			ifree.reserve(SIMPLE_STACKSIZE);
-			stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
-			do	{
-				const int	id=stack[stack.size()-1];
-				sStkNPS		se=stock[id];
-				stack.pop_back();ifree.push_back(id);
-				if(se.mask!=inside)
+				bool out = false;
+				for (int i = 0, j = 1; (!out) && (i < count); ++i, j <<= 1)
 				{
-					bool	out=false;
-					for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
+					if (0 == (se.mask & j))
 					{
-						if(0==(se.mask&j))
+						const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]);
+						switch (side)
 						{
-							const int	side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
-							switch(side)
-							{
-							case	-1:	out=true;break;
-							case	+1:	se.mask|=j;break;
-							}
+							case -1:
+								out = true;
+								break;
+							case +1:
+								se.mask |= j;
+								break;
 						}
 					}
-					if(out) continue;
 				}
-				if(policy.Descent(se.node))
+				if (out) continue;
+			}
+			if (policy.Descent(se.node))
+			{
+				if (se.node->isinternal())
 				{
-					if(se.node->isinternal())
+					const btDbvtNode* pns[] = {se.node->childs[0], se.node->childs[1]};
+					sStkNPS nes[] = {sStkNPS(pns[0], se.mask, pns[0]->volume.ProjectMinimum(sortaxis, srtsgns)),
+									 sStkNPS(pns[1], se.mask, pns[1]->volume.ProjectMinimum(sortaxis, srtsgns))};
+					const int q = nes[0].value < nes[1].value ? 1 : 0;
+					int j = stack.size();
+					if (fsort && (j > 0))
 					{
-						const btDbvtNode* pns[]={	se.node->childs[0],se.node->childs[1]};
-						sStkNPS		nes[]={	sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
-							sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
-						const int	q=nes[0].value<nes[1].value?1:0;				
-						int			j=stack.size();
-						if(fsort&&(j>0))
-						{
-							/* Insert 0	*/ 
-							j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
-							stack.push_back(0);
-							
-							//void * memmove ( void * destination, const void * source, size_t num );
-							
+						/* Insert 0	*/
+						j = nearest(&stack[0], &stock[0], nes[q].value, 0, stack.size());
+						stack.push_back(0);
+
+						//void * memmove ( void * destination, const void * source, size_t num );
+
 #if DBVT_USE_MEMMOVE
-                     {
-                     int num_items_to_move = stack.size()-1-j;
-                     if(num_items_to_move > 0)
-                        memmove(&stack[j+1],&stack[j],sizeof(int)*num_items_to_move);
-                     }
+						{
+							int num_items_to_move = stack.size() - 1 - j;
+							if (num_items_to_move > 0)
+								memmove(&stack[j + 1], &stack[j], sizeof(int) * num_items_to_move);
+						}
 #else
-                     for(int k=stack.size()-1;k>j;--k) {
-								stack[k]=stack[k-1];
-                     }
+						for (int k = stack.size() - 1; k > j; --k)
+						{
+							stack[k] = stack[k - 1];
+						}
 #endif
-							stack[j]=allocate(ifree,stock,nes[q]);
-							/* Insert 1	*/ 
-							j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
-							stack.push_back(0);
+						stack[j] = allocate(ifree, stock, nes[q]);
+						/* Insert 1	*/
+						j = nearest(&stack[0], &stock[0], nes[1 - q].value, j, stack.size());
+						stack.push_back(0);
 #if DBVT_USE_MEMMOVE
-                     {
-                     int num_items_to_move = stack.size()-1-j;
-                     if(num_items_to_move > 0)
-                        memmove(&stack[j+1],&stack[j],sizeof(int)*num_items_to_move);
-                     }
-#else
-                     for(int k=stack.size()-1;k>j;--k) {
-                        stack[k]=stack[k-1];
-                     }
-#endif
-							stack[j]=allocate(ifree,stock,nes[1-q]);
+						{
+							int num_items_to_move = stack.size() - 1 - j;
+							if (num_items_to_move > 0)
+								memmove(&stack[j + 1], &stack[j], sizeof(int) * num_items_to_move);
 						}
-						else
+#else
+						for (int k = stack.size() - 1; k > j; --k)
 						{
-							stack.push_back(allocate(ifree,stock,nes[q]));
-							stack.push_back(allocate(ifree,stock,nes[1-q]));
+							stack[k] = stack[k - 1];
 						}
+#endif
+						stack[j] = allocate(ifree, stock, nes[1 - q]);
 					}
 					else
 					{
-						policy.Process(se.node,se.value);
+						stack.push_back(allocate(ifree, stock, nes[q]));
+						stack.push_back(allocate(ifree, stock, nes[1 - q]));
 					}
 				}
-			} while(stack.size());
-		}
+				else
+				{
+					policy.Process(se.node, se.value);
+				}
+			}
+		} while (stack.size());
+	}
 }
 
 //
 DBVT_PREFIX
-inline void		btDbvt::collideTU(	const btDbvtNode* root,
-								  DBVT_IPOLICY)
+inline void btDbvt::collideTU(const btDbvtNode* root,
+							  DBVT_IPOLICY)
 {
 	DBVT_CHECKTYPE
-		if(root)
+	if (root)
+	{
+		btAlignedObjectArray<const btDbvtNode*> stack;
+		stack.reserve(SIMPLE_STACKSIZE);
+		stack.push_back(root);
+		do
 		{
-			btAlignedObjectArray<const btDbvtNode*>	stack;
-			stack.reserve(SIMPLE_STACKSIZE);
-			stack.push_back(root);
-			do	{
-				const btDbvtNode*	n=stack[stack.size()-1];
-				stack.pop_back();
-				if(policy.Descent(n))
+			const btDbvtNode* n = stack[stack.size() - 1];
+			stack.pop_back();
+			if (policy.Descent(n))
+			{
+				if (n->isinternal())
 				{
-					if(n->isinternal())
-					{ stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
-					else
-					{ policy.Process(n); }
+					stack.push_back(n->childs[0]);
+					stack.push_back(n->childs[1]);
 				}
-			} while(stack.size()>0);
-		}
+				else
+				{
+					policy.Process(n);
+				}
+			}
+		} while (stack.size() > 0);
+	}
 }
 
 //
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
index 75cfac64368..7b39dbdc0fe 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
@@ -16,33 +16,33 @@ subject to the following restrictions:
 ///btDbvtBroadphase implementation by Nathanael Presson
 
 #include "btDbvtBroadphase.h"
-
+#include "LinearMath/btThreads.h"
+btScalar gDbvtMargin = btScalar(0.05);
 //
 // Profiling
 //
 
-#if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK
+#if DBVT_BP_PROFILE || DBVT_BP_ENABLE_BENCHMARK
 #include <stdio.h>
 #endif
 
 #if DBVT_BP_PROFILE
-struct	ProfileScope
+struct ProfileScope
 {
-	__forceinline ProfileScope(btClock& clock,unsigned long& value) :
-	m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
+	__forceinline ProfileScope(btClock& clock, unsigned long& value) : m_clock(&clock), m_value(&value), m_base(clock.getTimeMicroseconds())
 	{
 	}
 	__forceinline ~ProfileScope()
 	{
-		(*m_value)+=m_clock->getTimeMicroseconds()-m_base;
+		(*m_value) += m_clock->getTimeMicroseconds() - m_base;
 	}
-	btClock*		m_clock;
-	unsigned long*	m_value;
-	unsigned long	m_base;
+	btClock* m_clock;
+	unsigned long* m_value;
+	unsigned long m_base;
 };
-#define	SPC(_value_)	ProfileScope	spc_scope(m_clock,_value_)
+#define SPC(_value_) ProfileScope spc_scope(m_clock, _value_)
 #else
-#define	SPC(_value_)
+#define SPC(_value_)
 #endif
 
 //
@@ -51,66 +51,75 @@ struct	ProfileScope
 
 //
 template <typename T>
-static inline void	listappend(T* item,T*& list)
+static inline void listappend(T* item, T*& list)
 {
-	item->links[0]=0;
-	item->links[1]=list;
-	if(list) list->links[0]=item;
-	list=item;
+	item->links[0] = 0;
+	item->links[1] = list;
+	if (list) list->links[0] = item;
+	list = item;
 }
 
 //
 template <typename T>
-static inline void	listremove(T* item,T*& list)
+static inline void listremove(T* item, T*& list)
 {
-	if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
-	if(item->links[1]) item->links[1]->links[0]=item->links[0];
+	if (item->links[0])
+		item->links[0]->links[1] = item->links[1];
+	else
+		list = item->links[1];
+	if (item->links[1]) item->links[1]->links[0] = item->links[0];
 }
 
 //
 template <typename T>
-static inline int	listcount(T* root)
+static inline int listcount(T* root)
 {
-	int	n=0;
-	while(root) { ++n;root=root->links[1]; }
-	return(n);
+	int n = 0;
+	while (root)
+	{
+		++n;
+		root = root->links[1];
+	}
+	return (n);
 }
 
 //
 template <typename T>
-static inline void	clear(T& value)
+static inline void clear(T& value)
 {
-	static const struct ZeroDummy : T {} zerodummy;
-	value=zerodummy;
+	static const struct ZeroDummy : T
+	{
+	} zerodummy;
+	value = zerodummy;
 }
 
 //
 // Colliders
 //
 
-/* Tree collider	*/ 
-struct	btDbvtTreeCollider : btDbvt::ICollide
+/* Tree collider	*/
+struct btDbvtTreeCollider : btDbvt::ICollide
 {
-	btDbvtBroadphase*	pbp;
-	btDbvtProxy*		proxy;
+	btDbvtBroadphase* pbp;
+	btDbvtProxy* proxy;
 	btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
-	void	Process(const btDbvtNode* na,const btDbvtNode* nb)
+	void Process(const btDbvtNode* na, const btDbvtNode* nb)
 	{
-		if(na!=nb)
+		if (na != nb)
 		{
-			btDbvtProxy*	pa=(btDbvtProxy*)na->data;
-			btDbvtProxy*	pb=(btDbvtProxy*)nb->data;
+			btDbvtProxy* pa = (btDbvtProxy*)na->data;
+			btDbvtProxy* pb = (btDbvtProxy*)nb->data;
 #if DBVT_BP_SORTPAIRS
-			if(pa->m_uniqueId>pb->m_uniqueId) 
-				btSwap(pa,pb);
+			if (pa->m_uniqueId > pb->m_uniqueId)
+				btSwap(pa, pb);
 #endif
-			pbp->m_paircache->addOverlappingPair(pa,pb);
+			pbp->m_paircache->addOverlappingPair(pa, pb);
 			++pbp->m_newpairs;
 		}
 	}
-	void	Process(const btDbvtNode* n)
+	void Process(const btDbvtNode* n)
 	{
-		Process(n,proxy->leaf);
+		Process(n, proxy->leaf);
 	}
 };
 
@@ -121,27 +130,32 @@ struct	btDbvtTreeCollider : btDbvt::ICollide
 //
 btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
 {
-	m_deferedcollide	=	false;
-	m_needcleanup		=	true;
-	m_releasepaircache	=	(paircache!=0)?false:true;
-	m_prediction		=	0;
-	m_stageCurrent		=	0;
-	m_fixedleft			=	0;
-	m_fupdates			=	1;
-	m_dupdates			=	0;
-	m_cupdates			=	10;
-	m_newpairs			=	1;
-	m_updates_call		=	0;
-	m_updates_done		=	0;
-	m_updates_ratio		=	0;
-	m_paircache			=	paircache? paircache	: new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
-	m_gid				=	0;
-	m_pid				=	0;
-	m_cid				=	0;
-	for(int i=0;i<=STAGECOUNT;++i)
+	m_deferedcollide = false;
+	m_needcleanup = true;
+	m_releasepaircache = (paircache != 0) ? false : true;
+	m_prediction = 0;
+	m_stageCurrent = 0;
+	m_fixedleft = 0;
+	m_fupdates = 1;
+	m_dupdates = 0;
+	m_cupdates = 10;
+	m_newpairs = 1;
+	m_updates_call = 0;
+	m_updates_done = 0;
+	m_updates_ratio = 0;
+	m_paircache = paircache ? paircache : new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache), 16)) btHashedOverlappingPairCache();
+	m_gid = 0;
+	m_pid = 0;
+	m_cid = 0;
+	for (int i = 0; i <= STAGECOUNT; ++i)
 	{
-		m_stageRoots[i]=0;
+		m_stageRoots[i] = 0;
 	}
+#if BT_THREADSAFE
+	m_rayTestStacks.resize(BT_MAX_THREAD_COUNT);
+#else
+	m_rayTestStacks.resize(1);
+#endif
 #if DBVT_BP_PROFILE
 	clear(m_profiling);
 #endif
@@ -150,7 +164,7 @@ btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
 //
 btDbvtBroadphase::~btDbvtBroadphase()
 {
-	if(m_releasepaircache) 
+	if (m_releasepaircache)
 	{
 		m_paircache->~btOverlappingPairCache();
 		btAlignedFree(m_paircache);
@@ -158,287 +172,294 @@ btDbvtBroadphase::~btDbvtBroadphase()
 }
 
 //
-btBroadphaseProxy*				btDbvtBroadphase::createProxy(	const btVector3& aabbMin,
-															  const btVector3& aabbMax,
-															  int /*shapeType*/,
-															  void* userPtr,
-															  short int collisionFilterGroup,
-															  short int collisionFilterMask,
-															  btDispatcher* /*dispatcher*/,
-															  void* /*multiSapProxy*/)
+btBroadphaseProxy* btDbvtBroadphase::createProxy(const btVector3& aabbMin,
+												 const btVector3& aabbMax,
+												 int /*shapeType*/,
+												 void* userPtr,
+												 int collisionFilterGroup,
+												 int collisionFilterMask,
+												 btDispatcher* /*dispatcher*/)
 {
-	btDbvtProxy*		proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(	aabbMin,aabbMax,userPtr,
-		collisionFilterGroup,
-		collisionFilterMask);
+	btDbvtProxy* proxy = new (btAlignedAlloc(sizeof(btDbvtProxy), 16)) btDbvtProxy(aabbMin, aabbMax, userPtr,
+																				   collisionFilterGroup,
+																				   collisionFilterMask);
 
-	btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax);
+	btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin, aabbMax);
 
 	//bproxy->aabb			=	btDbvtVolume::FromMM(aabbMin,aabbMax);
-	proxy->stage		=	m_stageCurrent;
-	proxy->m_uniqueId	=	++m_gid;
-	proxy->leaf			=	m_sets[0].insert(aabb,proxy);
-	listappend(proxy,m_stageRoots[m_stageCurrent]);
-	if(!m_deferedcollide)
+	proxy->stage = m_stageCurrent;
+	proxy->m_uniqueId = ++m_gid;
+	proxy->leaf = m_sets[0].insert(aabb, proxy);
+	listappend(proxy, m_stageRoots[m_stageCurrent]);
+	if (!m_deferedcollide)
 	{
-		btDbvtTreeCollider	collider(this);
-		collider.proxy=proxy;
-		m_sets[0].collideTV(m_sets[0].m_root,aabb,collider);
-		m_sets[1].collideTV(m_sets[1].m_root,aabb,collider);
+		btDbvtTreeCollider collider(this);
+		collider.proxy = proxy;
+		m_sets[0].collideTV(m_sets[0].m_root, aabb, collider);
+		m_sets[1].collideTV(m_sets[1].m_root, aabb, collider);
 	}
-	return(proxy);
+	return (proxy);
 }
 
 //
-void							btDbvtBroadphase::destroyProxy(	btBroadphaseProxy* absproxy,
-															   btDispatcher* dispatcher)
+void btDbvtBroadphase::destroyProxy(btBroadphaseProxy* absproxy,
+									btDispatcher* dispatcher)
 {
-	btDbvtProxy*	proxy=(btDbvtProxy*)absproxy;
-	if(proxy->stage==STAGECOUNT)
+	btDbvtProxy* proxy = (btDbvtProxy*)absproxy;
+	if (proxy->stage == STAGECOUNT)
 		m_sets[1].remove(proxy->leaf);
 	else
 		m_sets[0].remove(proxy->leaf);
-	listremove(proxy,m_stageRoots[proxy->stage]);
-	m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
+	listremove(proxy, m_stageRoots[proxy->stage]);
+	m_paircache->removeOverlappingPairsContainingProxy(proxy, dispatcher);
 	btAlignedFree(proxy);
-	m_needcleanup=true;
+	m_needcleanup = true;
 }
 
-void	btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const
+void btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy, btVector3& aabbMin, btVector3& aabbMax) const
 {
-	btDbvtProxy*						proxy=(btDbvtProxy*)absproxy;
+	btDbvtProxy* proxy = (btDbvtProxy*)absproxy;
 	aabbMin = proxy->m_aabbMin;
 	aabbMax = proxy->m_aabbMax;
 }
 
-struct	BroadphaseRayTester : btDbvt::ICollide
+struct BroadphaseRayTester : btDbvt::ICollide
 {
 	btBroadphaseRayCallback& m_rayCallback;
 	BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
-		:m_rayCallback(orgCallback)
+		: m_rayCallback(orgCallback)
 	{
 	}
-	void					Process(const btDbvtNode* leaf)
+	void Process(const btDbvtNode* leaf)
 	{
-		btDbvtProxy*	proxy=(btDbvtProxy*)leaf->data;
+		btDbvtProxy* proxy = (btDbvtProxy*)leaf->data;
 		m_rayCallback.process(proxy);
 	}
-};	
+};
 
-void	btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
+void btDbvtBroadphase::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin, const btVector3& aabbMax)
 {
 	BroadphaseRayTester callback(rayCallback);
+	btAlignedObjectArray<const btDbvtNode*>* stack = &m_rayTestStacks[0];
+#if BT_THREADSAFE
+	// for this function to be threadsafe, each thread must have a separate copy
+	// of this stack.  This could be thread-local static to avoid dynamic allocations,
+	// instead of just a local.
+	int threadIndex = btGetCurrentThreadIndex();
+	btAlignedObjectArray<const btDbvtNode*> localStack;
+	//todo(erwincoumans, "why do we get tsan issue here?")
+	if (0)//threadIndex < m_rayTestStacks.size())
+	//if (threadIndex < m_rayTestStacks.size())
+	{
+		// use per-thread preallocated stack if possible to avoid dynamic allocations
+		stack = &m_rayTestStacks[threadIndex];
+	}
+	else
+	{
+		stack = &localStack;
+	}
+#endif
 
-	m_sets[0].rayTestInternal(	m_sets[0].m_root,
-		rayFrom,
-		rayTo,
-		rayCallback.m_rayDirectionInverse,
-		rayCallback.m_signs,
-		rayCallback.m_lambda_max,
-		aabbMin,
-		aabbMax,
-		callback);
-
-	m_sets[1].rayTestInternal(	m_sets[1].m_root,
-		rayFrom,
-		rayTo,
-		rayCallback.m_rayDirectionInverse,
-		rayCallback.m_signs,
-		rayCallback.m_lambda_max,
-		aabbMin,
-		aabbMax,
-		callback);
-
+	m_sets[0].rayTestInternal(m_sets[0].m_root,
+							  rayFrom,
+							  rayTo,
+							  rayCallback.m_rayDirectionInverse,
+							  rayCallback.m_signs,
+							  rayCallback.m_lambda_max,
+							  aabbMin,
+							  aabbMax,
+							  *stack,
+							  callback);
+
+	m_sets[1].rayTestInternal(m_sets[1].m_root,
+							  rayFrom,
+							  rayTo,
+							  rayCallback.m_rayDirectionInverse,
+							  rayCallback.m_signs,
+							  rayCallback.m_lambda_max,
+							  aabbMin,
+							  aabbMax,
+							  *stack,
+							  callback);
 }
 
-
-struct	BroadphaseAabbTester : btDbvt::ICollide
+struct BroadphaseAabbTester : btDbvt::ICollide
 {
 	btBroadphaseAabbCallback& m_aabbCallback;
 	BroadphaseAabbTester(btBroadphaseAabbCallback& orgCallback)
-		:m_aabbCallback(orgCallback)
+		: m_aabbCallback(orgCallback)
 	{
 	}
-	void					Process(const btDbvtNode* leaf)
+	void Process(const btDbvtNode* leaf)
 	{
-		btDbvtProxy*	proxy=(btDbvtProxy*)leaf->data;
+		btDbvtProxy* proxy = (btDbvtProxy*)leaf->data;
 		m_aabbCallback.process(proxy);
 	}
-};	
+};
 
-void	btDbvtBroadphase::aabbTest(const btVector3& aabbMin,const btVector3& aabbMax,btBroadphaseAabbCallback& aabbCallback)
+void btDbvtBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& aabbCallback)
 {
 	BroadphaseAabbTester callback(aabbCallback);
 
-	const ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds=btDbvtVolume::FromMM(aabbMin,aabbMax);
-		//process all children, that overlap with  the given AABB bounds
-	m_sets[0].collideTV(m_sets[0].m_root,bounds,callback);
-	m_sets[1].collideTV(m_sets[1].m_root,bounds,callback);
-
+	const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(aabbMin, aabbMax);
+	//process all children, that overlap with  the given AABB bounds
+	m_sets[0].collideTV(m_sets[0].m_root, bounds, callback);
+	m_sets[1].collideTV(m_sets[1].m_root, bounds, callback);
 }
 
-
-
 //
-void							btDbvtBroadphase::setAabb(		btBroadphaseProxy* absproxy,
-														  const btVector3& aabbMin,
-														  const btVector3& aabbMax,
-														  btDispatcher* /*dispatcher*/)
+void btDbvtBroadphase::setAabb(btBroadphaseProxy* absproxy,
+							   const btVector3& aabbMin,
+							   const btVector3& aabbMax,
+							   btDispatcher* /*dispatcher*/)
 {
-	btDbvtProxy*						proxy=(btDbvtProxy*)absproxy;
-	ATTRIBUTE_ALIGNED16(btDbvtVolume)	aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
+	btDbvtProxy* proxy = (btDbvtProxy*)absproxy;
+	ATTRIBUTE_ALIGNED16(btDbvtVolume)
+	aabb = btDbvtVolume::FromMM(aabbMin, aabbMax);
 #if DBVT_BP_PREVENTFALSEUPDATE
-	if(NotEqual(aabb,proxy->leaf->volume))
+	if (NotEqual(aabb, proxy->leaf->volume))
 #endif
 	{
-		bool	docollide=false;
-		if(proxy->stage==STAGECOUNT)
-		{/* fixed -> dynamic set	*/ 
+		bool docollide = false;
+		if (proxy->stage == STAGECOUNT)
+		{ /* fixed -> dynamic set	*/
 			m_sets[1].remove(proxy->leaf);
-			proxy->leaf=m_sets[0].insert(aabb,proxy);
-			docollide=true;
+			proxy->leaf = m_sets[0].insert(aabb, proxy);
+			docollide = true;
 		}
 		else
-		{/* dynamic set				*/ 
+		{ /* dynamic set				*/
 			++m_updates_call;
-			if(Intersect(proxy->leaf->volume,aabb))
-			{/* Moving				*/ 
-
-				const btVector3	delta=aabbMin-proxy->m_aabbMin;
-				btVector3		velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction);
-				if(delta[0]<0) velocity[0]=-velocity[0];
-				if(delta[1]<0) velocity[1]=-velocity[1];
-				if(delta[2]<0) velocity[2]=-velocity[2];
-				if	(
-#ifdef DBVT_BP_MARGIN				
-					m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
-#else
-					m_sets[0].update(proxy->leaf,aabb,velocity)
-#endif
-					)
+			if (Intersect(proxy->leaf->volume, aabb))
+			{ /* Moving				*/
+
+				const btVector3 delta = aabbMin - proxy->m_aabbMin;
+				btVector3 velocity(((proxy->m_aabbMax - proxy->m_aabbMin) / 2) * m_prediction);
+				if (delta[0] < 0) velocity[0] = -velocity[0];
+				if (delta[1] < 0) velocity[1] = -velocity[1];
+				if (delta[2] < 0) velocity[2] = -velocity[2];
+				if (
+					m_sets[0].update(proxy->leaf, aabb, velocity, gDbvtMargin)
+
+				)
 				{
 					++m_updates_done;
-					docollide=true;
+					docollide = true;
 				}
 			}
 			else
-			{/* Teleporting			*/ 
-				m_sets[0].update(proxy->leaf,aabb);
+			{ /* Teleporting			*/
+				m_sets[0].update(proxy->leaf, aabb);
 				++m_updates_done;
-				docollide=true;
-			}	
+				docollide = true;
+			}
 		}
-		listremove(proxy,m_stageRoots[proxy->stage]);
+		listremove(proxy, m_stageRoots[proxy->stage]);
 		proxy->m_aabbMin = aabbMin;
 		proxy->m_aabbMax = aabbMax;
-		proxy->stage	=	m_stageCurrent;
-		listappend(proxy,m_stageRoots[m_stageCurrent]);
-		if(docollide)
+		proxy->stage = m_stageCurrent;
+		listappend(proxy, m_stageRoots[m_stageCurrent]);
+		if (docollide)
 		{
-			m_needcleanup=true;
-			if(!m_deferedcollide)
+			m_needcleanup = true;
+			if (!m_deferedcollide)
 			{
-				btDbvtTreeCollider	collider(this);
-				m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
-				m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider);
+				btDbvtTreeCollider collider(this);
+				m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider);
+				m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider);
 			}
-		}	
+		}
 	}
 }
 
-
 //
-void							btDbvtBroadphase::setAabbForceUpdate(		btBroadphaseProxy* absproxy,
-														  const btVector3& aabbMin,
-														  const btVector3& aabbMax,
-														  btDispatcher* /*dispatcher*/)
+void btDbvtBroadphase::setAabbForceUpdate(btBroadphaseProxy* absproxy,
+										  const btVector3& aabbMin,
+										  const btVector3& aabbMax,
+										  btDispatcher* /*dispatcher*/)
 {
-	btDbvtProxy*						proxy=(btDbvtProxy*)absproxy;
-	ATTRIBUTE_ALIGNED16(btDbvtVolume)	aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
-	bool	docollide=false;
-	if(proxy->stage==STAGECOUNT)
-	{/* fixed -> dynamic set	*/ 
+	btDbvtProxy* proxy = (btDbvtProxy*)absproxy;
+	ATTRIBUTE_ALIGNED16(btDbvtVolume)
+	aabb = btDbvtVolume::FromMM(aabbMin, aabbMax);
+	bool docollide = false;
+	if (proxy->stage == STAGECOUNT)
+	{ /* fixed -> dynamic set	*/
 		m_sets[1].remove(proxy->leaf);
-		proxy->leaf=m_sets[0].insert(aabb,proxy);
-		docollide=true;
+		proxy->leaf = m_sets[0].insert(aabb, proxy);
+		docollide = true;
 	}
 	else
-	{/* dynamic set				*/ 
+	{ /* dynamic set				*/
 		++m_updates_call;
-		/* Teleporting			*/ 
-		m_sets[0].update(proxy->leaf,aabb);
+		/* Teleporting			*/
+		m_sets[0].update(proxy->leaf, aabb);
 		++m_updates_done;
-		docollide=true;
+		docollide = true;
 	}
-	listremove(proxy,m_stageRoots[proxy->stage]);
+	listremove(proxy, m_stageRoots[proxy->stage]);
 	proxy->m_aabbMin = aabbMin;
 	proxy->m_aabbMax = aabbMax;
-	proxy->stage	=	m_stageCurrent;
-	listappend(proxy,m_stageRoots[m_stageCurrent]);
-	if(docollide)
+	proxy->stage = m_stageCurrent;
+	listappend(proxy, m_stageRoots[m_stageCurrent]);
+	if (docollide)
 	{
-		m_needcleanup=true;
-		if(!m_deferedcollide)
+		m_needcleanup = true;
+		if (!m_deferedcollide)
 		{
-			btDbvtTreeCollider	collider(this);
-			m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
-			m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider);
+			btDbvtTreeCollider collider(this);
+			m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider);
+			m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider);
 		}
-	}	
+	}
 }
 
 //
-void							btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
+void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 {
 	collide(dispatcher);
 #if DBVT_BP_PROFILE
-	if(0==(m_pid%DBVT_BP_PROFILING_RATE))
-	{	
-		printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
-		unsigned int	total=m_profiling.m_total;
-		if(total<=0) total=1;
-		printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
-		printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
-		printf("cleanup:   %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
-		printf("total:     %uus\r\n",total/DBVT_BP_PROFILING_RATE);
-		const unsigned long	sum=m_profiling.m_ddcollide+
-			m_profiling.m_fdcollide+
-			m_profiling.m_cleanup;
-		printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
-		printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE));
+	if (0 == (m_pid % DBVT_BP_PROFILING_RATE))
+	{
+		printf("fixed(%u) dynamics(%u) pairs(%u)\r\n", m_sets[1].m_leaves, m_sets[0].m_leaves, m_paircache->getNumOverlappingPairs());
+		unsigned int total = m_profiling.m_total;
+		if (total <= 0) total = 1;
+		printf("ddcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_ddcollide * 100) / total, m_profiling.m_ddcollide / DBVT_BP_PROFILING_RATE);
+		printf("fdcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_fdcollide * 100) / total, m_profiling.m_fdcollide / DBVT_BP_PROFILING_RATE);
+		printf("cleanup:   %u%% (%uus)\r\n", (50 + m_profiling.m_cleanup * 100) / total, m_profiling.m_cleanup / DBVT_BP_PROFILING_RATE);
+		printf("total:     %uus\r\n", total / DBVT_BP_PROFILING_RATE);
+		const unsigned long sum = m_profiling.m_ddcollide +
+								  m_profiling.m_fdcollide +
+								  m_profiling.m_cleanup;
+		printf("leaked: %u%% (%uus)\r\n", 100 - ((50 + sum * 100) / total), (total - sum) / DBVT_BP_PROFILING_RATE);
+		printf("job counts: %u%%\r\n", (m_profiling.m_jobcount * 100) / ((m_sets[0].m_leaves + m_sets[1].m_leaves) * DBVT_BP_PROFILING_RATE));
 		clear(m_profiling);
 		m_clock.reset();
 	}
 #endif
 
 	performDeferredRemoval(dispatcher);
-
 }
 
 void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher)
 {
-
 	if (m_paircache->hasDeferredRemoval())
 	{
-
-		btBroadphasePairArray&	overlappingPairArray = m_paircache->getOverlappingPairArray();
+		btBroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray();
 
 		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
 		overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
 
 		int invalidPair = 0;
 
-		
 		int i;
 
 		btBroadphasePair previousPair;
 		previousPair.m_pProxy0 = 0;
 		previousPair.m_pProxy1 = 0;
 		previousPair.m_algorithm = 0;
-		
-		
-		for (i=0;i<overlappingPairArray.size();i++)
+
+		for (i = 0; i < overlappingPairArray.size(); i++)
 		{
-		
 			btBroadphasePair& pair = overlappingPairArray[i];
 
 			bool isDuplicate = (pair == previousPair);
@@ -450,34 +471,35 @@ void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher)
 			if (!isDuplicate)
 			{
 				//important to perform AABB check that is consistent with the broadphase
-				btDbvtProxy*		pa=(btDbvtProxy*)pair.m_pProxy0;
-				btDbvtProxy*		pb=(btDbvtProxy*)pair.m_pProxy1;
-				bool hasOverlap = Intersect(pa->leaf->volume,pb->leaf->volume);
+				btDbvtProxy* pa = (btDbvtProxy*)pair.m_pProxy0;
+				btDbvtProxy* pb = (btDbvtProxy*)pair.m_pProxy1;
+				bool hasOverlap = Intersect(pa->leaf->volume, pb->leaf->volume);
 
 				if (hasOverlap)
 				{
 					needsRemoval = false;
-				} else
+				}
+				else
 				{
 					needsRemoval = true;
 				}
-			} else
+			}
+			else
 			{
 				//remove duplicate
 				needsRemoval = true;
 				//should have no algorithm
 				btAssert(!pair.m_algorithm);
 			}
-			
+
 			if (needsRemoval)
 			{
-				m_paircache->cleanOverlappingPair(pair,dispatcher);
+				m_paircache->cleanOverlappingPair(pair, dispatcher);
 
 				pair.m_pProxy0 = 0;
 				pair.m_pProxy1 = 0;
 				invalidPair++;
-			} 
-			
+			}
 		}
 
 		//perform a sort, to sort 'invalid' pairs to the end
@@ -487,7 +509,7 @@ void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher)
 }
 
 //
-void							btDbvtBroadphase::collide(btDispatcher* dispatcher)
+void btDbvtBroadphase::collide(btDispatcher* dispatcher)
 {
 	/*printf("---------------------------------------------------------\n");
 	printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
@@ -504,293 +526,303 @@ void							btDbvtBroadphase::collide(btDispatcher* dispatcher)
 	}
 */
 
-
-
 	SPC(m_profiling.m_total);
-	/* optimize				*/ 
-	m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
-	if(m_fixedleft)
+	/* optimize				*/
+	m_sets[0].optimizeIncremental(1 + (m_sets[0].m_leaves * m_dupdates) / 100);
+	if (m_fixedleft)
 	{
-		const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
-		m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
-		m_fixedleft=btMax<int>(0,m_fixedleft-count);
+		const int count = 1 + (m_sets[1].m_leaves * m_fupdates) / 100;
+		m_sets[1].optimizeIncremental(1 + (m_sets[1].m_leaves * m_fupdates) / 100);
+		m_fixedleft = btMax<int>(0, m_fixedleft - count);
 	}
-	/* dynamic -> fixed set	*/ 
-	m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
-	btDbvtProxy*	current=m_stageRoots[m_stageCurrent];
-	if(current)
+	/* dynamic -> fixed set	*/
+	m_stageCurrent = (m_stageCurrent + 1) % STAGECOUNT;
+	btDbvtProxy* current = m_stageRoots[m_stageCurrent];
+	if (current)
 	{
-		btDbvtTreeCollider	collider(this);
-		do	{
-			btDbvtProxy*	next=current->links[1];
-			listremove(current,m_stageRoots[current->stage]);
-			listappend(current,m_stageRoots[STAGECOUNT]);
 #if DBVT_BP_ACCURATESLEEPING
-			m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher);
-			collider.proxy=current;
-			btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider);
-			btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider);
+		btDbvtTreeCollider collider(this);
+#endif
+		do
+		{
+			btDbvtProxy* next = current->links[1];
+			listremove(current, m_stageRoots[current->stage]);
+			listappend(current, m_stageRoots[STAGECOUNT]);
+#if DBVT_BP_ACCURATESLEEPING
+			m_paircache->removeOverlappingPairsContainingProxy(current, dispatcher);
+			collider.proxy = current;
+			btDbvt::collideTV(m_sets[0].m_root, current->aabb, collider);
+			btDbvt::collideTV(m_sets[1].m_root, current->aabb, collider);
 #endif
 			m_sets[0].remove(current->leaf);
-			ATTRIBUTE_ALIGNED16(btDbvtVolume)	curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax);
-			current->leaf	=	m_sets[1].insert(curAabb,current);
-			current->stage	=	STAGECOUNT;	
-			current			=	next;
-		} while(current);
-		m_fixedleft=m_sets[1].m_leaves;
-		m_needcleanup=true;
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)
+			curAabb = btDbvtVolume::FromMM(current->m_aabbMin, current->m_aabbMax);
+			current->leaf = m_sets[1].insert(curAabb, current);
+			current->stage = STAGECOUNT;
+			current = next;
+		} while (current);
+		m_fixedleft = m_sets[1].m_leaves;
+		m_needcleanup = true;
 	}
-	/* collide dynamics		*/ 
+	/* collide dynamics		*/
 	{
-		btDbvtTreeCollider	collider(this);
-		if(m_deferedcollide)
+		btDbvtTreeCollider collider(this);
+		if (m_deferedcollide)
 		{
 			SPC(m_profiling.m_fdcollide);
-			m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider);
+			m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[1].m_root, collider);
 		}
-		if(m_deferedcollide)
+		if (m_deferedcollide)
 		{
 			SPC(m_profiling.m_ddcollide);
-			m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider);
+			m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[0].m_root, collider);
 		}
 	}
-	/* clean up				*/ 
-	if(m_needcleanup)
+	/* clean up				*/
+	if (m_needcleanup)
 	{
 		SPC(m_profiling.m_cleanup);
-		btBroadphasePairArray&	pairs=m_paircache->getOverlappingPairArray();
-		if(pairs.size()>0)
+		btBroadphasePairArray& pairs = m_paircache->getOverlappingPairArray();
+		if (pairs.size() > 0)
 		{
-
-			int			ni=btMin(pairs.size(),btMax<int>(m_newpairs,(pairs.size()*m_cupdates)/100));
-			for(int i=0;i<ni;++i)
+			int ni = btMin(pairs.size(), btMax<int>(m_newpairs, (pairs.size() * m_cupdates) / 100));
+			for (int i = 0; i < ni; ++i)
 			{
-				btBroadphasePair&	p=pairs[(m_cid+i)%pairs.size()];
-				btDbvtProxy*		pa=(btDbvtProxy*)p.m_pProxy0;
-				btDbvtProxy*		pb=(btDbvtProxy*)p.m_pProxy1;
-				if(!Intersect(pa->leaf->volume,pb->leaf->volume))
+				btBroadphasePair& p = pairs[(m_cid + i) % pairs.size()];
+				btDbvtProxy* pa = (btDbvtProxy*)p.m_pProxy0;
+				btDbvtProxy* pb = (btDbvtProxy*)p.m_pProxy1;
+				if (!Intersect(pa->leaf->volume, pb->leaf->volume))
 				{
 #if DBVT_BP_SORTPAIRS
-					if(pa->m_uniqueId>pb->m_uniqueId) 
-						btSwap(pa,pb);
+					if (pa->m_uniqueId > pb->m_uniqueId)
+						btSwap(pa, pb);
 #endif
-					m_paircache->removeOverlappingPair(pa,pb,dispatcher);
-					--ni;--i;
+					m_paircache->removeOverlappingPair(pa, pb, dispatcher);
+					--ni;
+					--i;
 				}
 			}
-			if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
+			if (pairs.size() > 0)
+				m_cid = (m_cid + ni) % pairs.size();
+			else
+				m_cid = 0;
 		}
 	}
 	++m_pid;
-	m_newpairs=1;
-	m_needcleanup=false;
-	if(m_updates_call>0)
-	{ m_updates_ratio=m_updates_done/(btScalar)m_updates_call; }
+	m_newpairs = 1;
+	m_needcleanup = false;
+	if (m_updates_call > 0)
+	{
+		m_updates_ratio = m_updates_done / (btScalar)m_updates_call;
+	}
 	else
-	{ m_updates_ratio=0; }
-	m_updates_done/=2;
-	m_updates_call/=2;
+	{
+		m_updates_ratio = 0;
+	}
+	m_updates_done /= 2;
+	m_updates_call /= 2;
 }
 
 //
-void							btDbvtBroadphase::optimize()
+void btDbvtBroadphase::optimize()
 {
 	m_sets[0].optimizeTopDown();
 	m_sets[1].optimizeTopDown();
 }
 
 //
-btOverlappingPairCache*			btDbvtBroadphase::getOverlappingPairCache()
+btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache()
 {
-	return(m_paircache);
+	return (m_paircache);
 }
 
 //
-const btOverlappingPairCache*	btDbvtBroadphase::getOverlappingPairCache() const
+const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const
 {
-	return(m_paircache);
+	return (m_paircache);
 }
 
 //
-void							btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
+void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const
 {
+	ATTRIBUTE_ALIGNED16(btDbvtVolume)
+	bounds;
 
-	ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds;
-
-	if(!m_sets[0].empty())
-		if(!m_sets[1].empty())	Merge(	m_sets[0].m_root->volume,
-			m_sets[1].m_root->volume,bounds);
+	if (!m_sets[0].empty())
+		if (!m_sets[1].empty())
+			Merge(m_sets[0].m_root->volume,
+				  m_sets[1].m_root->volume, bounds);
 		else
-			bounds=m_sets[0].m_root->volume;
-	else if(!m_sets[1].empty())	bounds=m_sets[1].m_root->volume;
+			bounds = m_sets[0].m_root->volume;
+	else if (!m_sets[1].empty())
+		bounds = m_sets[1].m_root->volume;
 	else
-		bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0);
-	aabbMin=bounds.Mins();
-	aabbMax=bounds.Maxs();
+		bounds = btDbvtVolume::FromCR(btVector3(0, 0, 0), 0);
+	aabbMin = bounds.Mins();
+	aabbMax = bounds.Maxs();
 }
 
 void btDbvtBroadphase::resetPool(btDispatcher* dispatcher)
 {
-	
 	int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
 	if (!totalObjects)
 	{
 		//reset internal dynamic tree data structures
 		m_sets[0].clear();
 		m_sets[1].clear();
-		
-		m_deferedcollide	=	false;
-		m_needcleanup		=	true;
-		m_stageCurrent		=	0;
-		m_fixedleft			=	0;
-		m_fupdates			=	1;
-		m_dupdates			=	0;
-		m_cupdates			=	10;
-		m_newpairs			=	1;
-		m_updates_call		=	0;
-		m_updates_done		=	0;
-		m_updates_ratio		=	0;
-		
-		m_gid				=	0;
-		m_pid				=	0;
-		m_cid				=	0;
-		for(int i=0;i<=STAGECOUNT;++i)
+
+		m_deferedcollide = false;
+		m_needcleanup = true;
+		m_stageCurrent = 0;
+		m_fixedleft = 0;
+		m_fupdates = 1;
+		m_dupdates = 0;
+		m_cupdates = 10;
+		m_newpairs = 1;
+		m_updates_call = 0;
+		m_updates_done = 0;
+		m_updates_ratio = 0;
+
+		m_gid = 0;
+		m_pid = 0;
+		m_cid = 0;
+		for (int i = 0; i <= STAGECOUNT; ++i)
 		{
-			m_stageRoots[i]=0;
+			m_stageRoots[i] = 0;
 		}
 	}
 }
 
 //
-void							btDbvtBroadphase::printStats()
-{}
+void btDbvtBroadphase::printStats()
+{
+}
 
 //
 #if DBVT_BP_ENABLE_BENCHMARK
 
-struct	btBroadphaseBenchmark
+struct btBroadphaseBenchmark
 {
-	struct	Experiment
+	struct Experiment
 	{
-		const char*			name;
-		int					object_count;
-		int					update_count;
-		int					spawn_count;
-		int					iterations;
-		btScalar			speed;
-		btScalar			amplitude;
+		const char* name;
+		int object_count;
+		int update_count;
+		int spawn_count;
+		int iterations;
+		btScalar speed;
+		btScalar amplitude;
 	};
-	struct	Object
+	struct Object
 	{
-		btVector3			center;
-		btVector3			extents;
-		btBroadphaseProxy*	proxy;
-		btScalar			time;
-		void				update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi)
+		btVector3 center;
+		btVector3 extents;
+		btBroadphaseProxy* proxy;
+		btScalar time;
+		void update(btScalar speed, btScalar amplitude, btBroadphaseInterface* pbi)
 		{
-			time		+=	speed;
-			center[0]	=	btCos(time*(btScalar)2.17)*amplitude+
-				btSin(time)*amplitude/2;
-			center[1]	=	btCos(time*(btScalar)1.38)*amplitude+
-				btSin(time)*amplitude;
-			center[2]	=	btSin(time*(btScalar)0.777)*amplitude;
-			pbi->setAabb(proxy,center-extents,center+extents,0);
+			time += speed;
+			center[0] = btCos(time * (btScalar)2.17) * amplitude +
+						btSin(time) * amplitude / 2;
+			center[1] = btCos(time * (btScalar)1.38) * amplitude +
+						btSin(time) * amplitude;
+			center[2] = btSin(time * (btScalar)0.777) * amplitude;
+			pbi->setAabb(proxy, center - extents, center + extents, 0);
 		}
 	};
-	static int		UnsignedRand(int range=RAND_MAX-1)	{ return(rand()%(range+1)); }
-	static btScalar	UnitRand()							{ return(UnsignedRand(16384)/(btScalar)16384); }
-	static void		OutputTime(const char* name,btClock& c,unsigned count=0)
+	static int UnsignedRand(int range = RAND_MAX - 1) { return (rand() % (range + 1)); }
+	static btScalar UnitRand() { return (UnsignedRand(16384) / (btScalar)16384); }
+	static void OutputTime(const char* name, btClock& c, unsigned count = 0)
 	{
-		const unsigned long	us=c.getTimeMicroseconds();
-		const unsigned long	ms=(us+500)/1000;
-		const btScalar		sec=us/(btScalar)(1000*1000);
-		if(count>0)
-			printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec);
+		const unsigned long us = c.getTimeMicroseconds();
+		const unsigned long ms = (us + 500) / 1000;
+		const btScalar sec = us / (btScalar)(1000 * 1000);
+		if (count > 0)
+			printf("%s : %u us (%u ms), %.2f/s\r\n", name, us, ms, count / sec);
 		else
-			printf("%s : %u us (%u ms)\r\n",name,us,ms);
+			printf("%s : %u us (%u ms)\r\n", name, us, ms);
 	}
 };
 
-void							btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
+void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
 {
-	static const btBroadphaseBenchmark::Experiment		experiments[]=
-	{
-		{"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100},
-		/*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
+	static const btBroadphaseBenchmark::Experiment experiments[] =
+		{
+			{"1024o.10%", 1024, 10, 0, 8192, (btScalar)0.005, (btScalar)100},
+			/*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
 		{"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/
-	};
-	static const int										nexperiments=sizeof(experiments)/sizeof(experiments[0]);
-	btAlignedObjectArray<btBroadphaseBenchmark::Object*>	objects;
-	btClock													wallclock;
-	/* Begin			*/ 
-	for(int iexp=0;iexp<nexperiments;++iexp)
+		};
+	static const int nexperiments = sizeof(experiments) / sizeof(experiments[0]);
+	btAlignedObjectArray<btBroadphaseBenchmark::Object*> objects;
+	btClock wallclock;
+	/* Begin			*/
+	for (int iexp = 0; iexp < nexperiments; ++iexp)
 	{
-		const btBroadphaseBenchmark::Experiment&	experiment=experiments[iexp];
-		const int									object_count=experiment.object_count;
-		const int									update_count=(object_count*experiment.update_count)/100;
-		const int									spawn_count=(object_count*experiment.spawn_count)/100;
-		const btScalar								speed=experiment.speed;	
-		const btScalar								amplitude=experiment.amplitude;
-		printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
-		printf("\tObjects: %u\r\n",object_count);
-		printf("\tUpdate: %u\r\n",update_count);
-		printf("\tSpawn: %u\r\n",spawn_count);
-		printf("\tSpeed: %f\r\n",speed);
-		printf("\tAmplitude: %f\r\n",amplitude);
+		const btBroadphaseBenchmark::Experiment& experiment = experiments[iexp];
+		const int object_count = experiment.object_count;
+		const int update_count = (object_count * experiment.update_count) / 100;
+		const int spawn_count = (object_count * experiment.spawn_count) / 100;
+		const btScalar speed = experiment.speed;
+		const btScalar amplitude = experiment.amplitude;
+		printf("Experiment #%u '%s':\r\n", iexp, experiment.name);
+		printf("\tObjects: %u\r\n", object_count);
+		printf("\tUpdate: %u\r\n", update_count);
+		printf("\tSpawn: %u\r\n", spawn_count);
+		printf("\tSpeed: %f\r\n", speed);
+		printf("\tAmplitude: %f\r\n", amplitude);
 		srand(180673);
-		/* Create objects	*/ 
+		/* Create objects	*/
 		wallclock.reset();
 		objects.reserve(object_count);
-		for(int i=0;i<object_count;++i)
+		for (int i = 0; i < object_count; ++i)
 		{
-			btBroadphaseBenchmark::Object*	po=new btBroadphaseBenchmark::Object();
-			po->center[0]=btBroadphaseBenchmark::UnitRand()*50;
-			po->center[1]=btBroadphaseBenchmark::UnitRand()*50;
-			po->center[2]=btBroadphaseBenchmark::UnitRand()*50;
-			po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2;
-			po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2;
-			po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2;
-			po->time=btBroadphaseBenchmark::UnitRand()*2000;
-			po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
+			btBroadphaseBenchmark::Object* po = new btBroadphaseBenchmark::Object();
+			po->center[0] = btBroadphaseBenchmark::UnitRand() * 50;
+			po->center[1] = btBroadphaseBenchmark::UnitRand() * 50;
+			po->center[2] = btBroadphaseBenchmark::UnitRand() * 50;
+			po->extents[0] = btBroadphaseBenchmark::UnitRand() * 2 + 2;
+			po->extents[1] = btBroadphaseBenchmark::UnitRand() * 2 + 2;
+			po->extents[2] = btBroadphaseBenchmark::UnitRand() * 2 + 2;
+			po->time = btBroadphaseBenchmark::UnitRand() * 2000;
+			po->proxy = pbi->createProxy(po->center - po->extents, po->center + po->extents, 0, po, 1, 1, 0, 0);
 			objects.push_back(po);
 		}
-		btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
-		/* First update		*/ 
+		btBroadphaseBenchmark::OutputTime("\tInitialization", wallclock);
+		/* First update		*/
 		wallclock.reset();
-		for(int i=0;i<objects.size();++i)
+		for (int i = 0; i < objects.size(); ++i)
 		{
-			objects[i]->update(speed,amplitude,pbi);
+			objects[i]->update(speed, amplitude, pbi);
 		}
-		btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
-		/* Updates			*/ 
+		btBroadphaseBenchmark::OutputTime("\tFirst update", wallclock);
+		/* Updates			*/
 		wallclock.reset();
-		for(int i=0;i<experiment.iterations;++i)
+		for (int i = 0; i < experiment.iterations; ++i)
 		{
-			for(int j=0;j<update_count;++j)
-			{				
-				objects[j]->update(speed,amplitude,pbi);
+			for (int j = 0; j < update_count; ++j)
+			{
+				objects[j]->update(speed, amplitude, pbi);
 			}
 			pbi->calculateOverlappingPairs(0);
 		}
-		btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
-		/* Clean up			*/ 
+		btBroadphaseBenchmark::OutputTime("\tUpdate", wallclock, experiment.iterations);
+		/* Clean up			*/
 		wallclock.reset();
-		for(int i=0;i<objects.size();++i)
+		for (int i = 0; i < objects.size(); ++i)
 		{
-			pbi->destroyProxy(objects[i]->proxy,0);
+			pbi->destroyProxy(objects[i]->proxy, 0);
 			delete objects[i];
 		}
 		objects.resize(0);
-		btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
+		btBroadphaseBenchmark::OutputTime("\tRelease", wallclock);
 	}
-
 }
 #else
-void							btDbvtBroadphase::benchmark(btBroadphaseInterface*)
-{}
+void btDbvtBroadphase::benchmark(btBroadphaseInterface*)
+{
+}
 #endif
 
 #if DBVT_BP_PROFILE
-#undef	SPC
+#undef SPC
 #endif
-
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
index 18b64ad0e57..a71feef53bb 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
@@ -24,15 +24,16 @@ subject to the following restrictions:
 // Compile time config
 //
 
-#define	DBVT_BP_PROFILE					0
+#define DBVT_BP_PROFILE 0
 //#define DBVT_BP_SORTPAIRS				1
-#define DBVT_BP_PREVENTFALSEUPDATE		0
-#define DBVT_BP_ACCURATESLEEPING		0
-#define DBVT_BP_ENABLE_BENCHMARK		0
-#define DBVT_BP_MARGIN					(btScalar)0.05
+#define DBVT_BP_PREVENTFALSEUPDATE 0
+#define DBVT_BP_ACCURATESLEEPING 0
+#define DBVT_BP_ENABLE_BENCHMARK 0
+//#define DBVT_BP_MARGIN					(btScalar)0.05
+extern btScalar gDbvtMargin;
 
 #if DBVT_BP_PROFILE
-#define	DBVT_BP_PROFILING_RATE	256
+#define DBVT_BP_PROFILING_RATE 256
 #include "LinearMath/btQuickprof.h"
 #endif
 
@@ -41,89 +42,90 @@ subject to the following restrictions:
 //
 struct btDbvtProxy : btBroadphaseProxy
 {
-	/* Fields		*/ 
+	/* Fields		*/
 	//btDbvtAabbMm	aabb;
-	btDbvtNode*		leaf;
-	btDbvtProxy*	links[2];
-	int				stage;
-	/* ctor			*/ 
-	btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
-	btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
+	btDbvtNode* leaf;
+	btDbvtProxy* links[2];
+	int stage;
+	/* ctor			*/
+	btDbvtProxy(const btVector3& aabbMin, const btVector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask) : btBroadphaseProxy(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask)
 	{
-		links[0]=links[1]=0;
+		links[0] = links[1] = 0;
 	}
 };
 
-typedef btAlignedObjectArray<btDbvtProxy*>	btDbvtProxyArray;
+typedef btAlignedObjectArray<btDbvtProxy*> btDbvtProxyArray;
 
 ///The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see btDbvt).
 ///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other.
 ///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3.
-struct	btDbvtBroadphase : btBroadphaseInterface
+struct btDbvtBroadphase : btBroadphaseInterface
 {
-	/* Config		*/ 
-	enum	{
-		DYNAMIC_SET			=	0,	/* Dynamic set index	*/ 
-		FIXED_SET			=	1,	/* Fixed set index		*/ 
-		STAGECOUNT			=	2	/* Number of stages		*/ 
+	/* Config		*/
+	enum
+	{
+		DYNAMIC_SET = 0, /* Dynamic set index	*/
+		FIXED_SET = 1,   /* Fixed set index		*/
+		STAGECOUNT = 2   /* Number of stages		*/
 	};
-	/* Fields		*/ 
-	btDbvt					m_sets[2];					// Dbvt sets
-	btDbvtProxy*			m_stageRoots[STAGECOUNT+1];	// Stages list
-	btOverlappingPairCache*	m_paircache;				// Pair cache
-	btScalar				m_prediction;				// Velocity prediction
-	int						m_stageCurrent;				// Current stage
-	int						m_fupdates;					// % of fixed updates per frame
-	int						m_dupdates;					// % of dynamic updates per frame
-	int						m_cupdates;					// % of cleanup updates per frame
-	int						m_newpairs;					// Number of pairs created
-	int						m_fixedleft;				// Fixed optimization left
-	unsigned				m_updates_call;				// Number of updates call
-	unsigned				m_updates_done;				// Number of updates done
-	btScalar				m_updates_ratio;			// m_updates_done/m_updates_call
-	int						m_pid;						// Parse id
-	int						m_cid;						// Cleanup index
-	int						m_gid;						// Gen id
-	bool					m_releasepaircache;			// Release pair cache on delete
-	bool					m_deferedcollide;			// Defere dynamic/static collision to collide call
-	bool					m_needcleanup;				// Need to run cleanup?
+	/* Fields		*/
+	btDbvt m_sets[2];                           // Dbvt sets
+	btDbvtProxy* m_stageRoots[STAGECOUNT + 1];  // Stages list
+	btOverlappingPairCache* m_paircache;        // Pair cache
+	btScalar m_prediction;                      // Velocity prediction
+	int m_stageCurrent;                         // Current stage
+	int m_fupdates;                             // % of fixed updates per frame
+	int m_dupdates;                             // % of dynamic updates per frame
+	int m_cupdates;                             // % of cleanup updates per frame
+	int m_newpairs;                             // Number of pairs created
+	int m_fixedleft;                            // Fixed optimization left
+	unsigned m_updates_call;                    // Number of updates call
+	unsigned m_updates_done;                    // Number of updates done
+	btScalar m_updates_ratio;                   // m_updates_done/m_updates_call
+	int m_pid;                                  // Parse id
+	int m_cid;                                  // Cleanup index
+	int m_gid;                                  // Gen id
+	bool m_releasepaircache;                    // Release pair cache on delete
+	bool m_deferedcollide;                      // Defere dynamic/static collision to collide call
+	bool m_needcleanup;                         // Need to run cleanup?
+	btAlignedObjectArray<btAlignedObjectArray<const btDbvtNode*> > m_rayTestStacks;
 #if DBVT_BP_PROFILE
-	btClock					m_clock;
-	struct	{
-		unsigned long		m_total;
-		unsigned long		m_ddcollide;
-		unsigned long		m_fdcollide;
-		unsigned long		m_cleanup;
-		unsigned long		m_jobcount;
-	}				m_profiling;
+	btClock m_clock;
+	struct
+	{
+		unsigned long m_total;
+		unsigned long m_ddcollide;
+		unsigned long m_fdcollide;
+		unsigned long m_cleanup;
+		unsigned long m_jobcount;
+	} m_profiling;
 #endif
-	/* Methods		*/ 
-	btDbvtBroadphase(btOverlappingPairCache* paircache=0);
+	/* Methods		*/
+	btDbvtBroadphase(btOverlappingPairCache* paircache = 0);
 	~btDbvtBroadphase();
-	void							collide(btDispatcher* dispatcher);
-	void							optimize();
-	
-	/* btBroadphaseInterface Implementation	*/
-	btBroadphaseProxy*				createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
-	virtual void					destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
-	virtual void					setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
-	virtual void					rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0));
-	virtual void					aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
-
-	virtual void					getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
-	virtual	void					calculateOverlappingPairs(btDispatcher* dispatcher);
-	virtual	btOverlappingPairCache*	getOverlappingPairCache();
-	virtual	const btOverlappingPairCache*	getOverlappingPairCache() const;
-	virtual	void					getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
-	virtual	void					printStats();
+	void collide(btDispatcher* dispatcher);
+	void optimize();
 
+	/* btBroadphaseInterface Implementation	*/
+	btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher);
+	virtual void destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher);
+	virtual void setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher);
+	virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0));
+	virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
+
+	virtual void getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const;
+	virtual void calculateOverlappingPairs(btDispatcher* dispatcher);
+	virtual btOverlappingPairCache* getOverlappingPairCache();
+	virtual const btOverlappingPairCache* getOverlappingPairCache() const;
+	virtual void getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const;
+	virtual void printStats();
 
 	///reset broadphase internal structures, to ensure determinism/reproducability
 	virtual void resetPool(btDispatcher* dispatcher);
 
-	void	performDeferredRemoval(btDispatcher* dispatcher);
-	
-	void	setVelocityPrediction(btScalar prediction)
+	void performDeferredRemoval(btDispatcher* dispatcher);
+
+	void setVelocityPrediction(btScalar prediction)
 	{
 		m_prediction = prediction;
 	}
@@ -132,15 +134,13 @@ struct	btDbvtBroadphase : btBroadphaseInterface
 		return m_prediction;
 	}
 
-	///this setAabbForceUpdate is similar to setAabb but always forces the aabb update. 
+	///this setAabbForceUpdate is similar to setAabb but always forces the aabb update.
 	///it is not part of the btBroadphaseInterface but specific to btDbvtBroadphase.
 	///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see
 	///http://code.google.com/p/bullet/issues/detail?id=223
-	void							setAabbForceUpdate(		btBroadphaseProxy* absproxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* /*dispatcher*/);
-
-	static void						benchmark(btBroadphaseInterface*);
-
+	void setAabbForceUpdate(btBroadphaseProxy* absproxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* /*dispatcher*/);
 
+	static void benchmark(btBroadphaseInterface*);
 };
 
 #endif
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp
index 20768225b3a..d76d408aa65 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp
@@ -17,6 +17,4 @@ subject to the following restrictions:
 
 btDispatcher::~btDispatcher()
 {
-
 }
-
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h
index 89c307d14ca..b09b7d4d422 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h
@@ -20,7 +20,7 @@ subject to the following restrictions:
 class btCollisionAlgorithm;
 struct btBroadphaseProxy;
 class btRigidBody;
-class	btCollisionObject;
+class btCollisionObject;
 class btOverlappingPairCache;
 struct btCollisionObjectWrapper;
 
@@ -35,73 +35,76 @@ struct btDispatcherInfo
 		DISPATCH_CONTINUOUS
 	};
 	btDispatcherInfo()
-		:m_timeStep(btScalar(0.)),
-		m_stepCount(0),
-		m_dispatchFunc(DISPATCH_DISCRETE),
-		m_timeOfImpact(btScalar(1.)),
-		m_useContinuous(true),
-		m_debugDraw(0),
-		m_enableSatConvex(false),
-		m_enableSPU(true),
-		m_useEpa(true),
-		m_allowedCcdPenetration(btScalar(0.04)),
-		m_useConvexConservativeDistanceUtil(false),
-		m_convexConservativeDistanceThreshold(0.0f)
+		: m_timeStep(btScalar(0.)),
+		  m_stepCount(0),
+		  m_dispatchFunc(DISPATCH_DISCRETE),
+		  m_timeOfImpact(btScalar(1.)),
+		  m_useContinuous(true),
+		  m_debugDraw(0),
+		  m_enableSatConvex(false),
+		  m_enableSPU(true),
+		  m_useEpa(true),
+		  m_allowedCcdPenetration(btScalar(0.04)),
+		  m_useConvexConservativeDistanceUtil(false),
+		  m_convexConservativeDistanceThreshold(0.0f),
+		  m_deterministicOverlappingPairs(false)
 	{
-
 	}
-	btScalar	m_timeStep;
-	int			m_stepCount;
-	int			m_dispatchFunc;
-	mutable btScalar	m_timeOfImpact;
-	bool		m_useContinuous;
-	class btIDebugDraw*	m_debugDraw;
-	bool		m_enableSatConvex;
-	bool		m_enableSPU;
-	bool		m_useEpa;
-	btScalar	m_allowedCcdPenetration;
-	bool		m_useConvexConservativeDistanceUtil;
-	btScalar	m_convexConservativeDistanceThreshold;
+	btScalar m_timeStep;
+	int m_stepCount;
+	int m_dispatchFunc;
+	mutable btScalar m_timeOfImpact;
+	bool m_useContinuous;
+	class btIDebugDraw* m_debugDraw;
+	bool m_enableSatConvex;
+	bool m_enableSPU;
+	bool m_useEpa;
+	btScalar m_allowedCcdPenetration;
+	bool m_useConvexConservativeDistanceUtil;
+	btScalar m_convexConservativeDistanceThreshold;
+	bool m_deterministicOverlappingPairs;
+};
+
+enum ebtDispatcherQueryType
+{
+	BT_CONTACT_POINT_ALGORITHMS = 1,
+	BT_CLOSEST_POINT_ALGORITHMS = 2
 };
 
 ///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs.
 ///For example for pairwise collision detection, calculating contact points stored in btPersistentManifold or user callbacks (game logic).
 class btDispatcher
 {
-
-
 public:
-	virtual ~btDispatcher() ;
+	virtual ~btDispatcher();
 
-	virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold=0) = 0;
+	virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType) = 0;
 
-	virtual btPersistentManifold*	getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1)=0;
+	virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0, const btCollisionObject* b1) = 0;
 
-	virtual void releaseManifold(btPersistentManifold* manifold)=0;
+	virtual void releaseManifold(btPersistentManifold* manifold) = 0;
 
-	virtual void clearManifold(btPersistentManifold* manifold)=0;
+	virtual void clearManifold(btPersistentManifold* manifold) = 0;
 
-	virtual bool	needsCollision(const btCollisionObject* body0,const btCollisionObject* body1) = 0;
+	virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1) = 0;
 
-	virtual bool	needsResponse(const btCollisionObject* body0,const btCollisionObject* body1)=0;
+	virtual bool needsResponse(const btCollisionObject* body0, const btCollisionObject* body1) = 0;
 
-	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher)  =0;
+	virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher) = 0;
 
 	virtual int getNumManifolds() const = 0;
 
 	virtual btPersistentManifold* getManifoldByIndexInternal(int index) = 0;
 
-	virtual	btPersistentManifold**	getInternalManifoldPointer() = 0;
+	virtual btPersistentManifold** getInternalManifoldPointer() = 0;
 
-	virtual	btPoolAllocator*	getInternalManifoldPool() = 0;
+	virtual btPoolAllocator* getInternalManifoldPool() = 0;
 
-	virtual	const btPoolAllocator*	getInternalManifoldPool() const = 0;
+	virtual const btPoolAllocator* getInternalManifoldPool() const = 0;
 
-	virtual	void* allocateCollisionAlgorithm(int size)  = 0;
-
-	virtual	void freeCollisionAlgorithm(void* ptr) = 0;
+	virtual void* allocateCollisionAlgorithm(int size) = 0;
 
+	virtual void freeCollisionAlgorithm(void* ptr) = 0;
 };
 
-
-#endif //BT_DISPATCHER_H
+#endif  //BT_DISPATCHER_H
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
deleted file mode 100644
index 81369fe9b50..00000000000
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
+++ /dev/null
@@ -1,489 +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.
-*/
-
-#include "btMultiSapBroadphase.h"
-
-#include "btSimpleBroadphase.h"
-#include "LinearMath/btAabbUtil2.h"
-#include "btQuantizedBvh.h"
-
-///	btSapBroadphaseArray	m_sapBroadphases;
-
-///	btOverlappingPairCache*	m_overlappingPairs;
-extern int gOverlappingPairs;
-
-/*
-class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache
-{
-public:
-
-	virtual btBroadphasePair*	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
-	{
-		return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy);
-	}
-};
-
-*/
-
-btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache)
-:m_overlappingPairs(pairCache),
-m_optimizedAabbTree(0),
-m_ownsPairCache(false),
-m_invalidPair(0)
-{
-	if (!m_overlappingPairs)
-	{
-		m_ownsPairCache = true;
-		void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16);
-		m_overlappingPairs = new (mem)btSortedOverlappingPairCache();
-	}
-
-	struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback
-	{
-		virtual ~btMultiSapOverlapFilterCallback()
-		{}
-		// return true when pairs need collision
-		virtual bool	needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const
-		{
-			btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy;
-			btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy;
-			
-			bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0;
-			collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask);
-	
-			return collides;
-		}
-	};
-
-	void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16);
-	m_filterCallback = new (mem)btMultiSapOverlapFilterCallback();
-
-	m_overlappingPairs->setOverlapFilterCallback(m_filterCallback);
-//	mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16);
-//	m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs);
-}
-
-btMultiSapBroadphase::~btMultiSapBroadphase()
-{
-	if (m_ownsPairCache)
-	{
-		m_overlappingPairs->~btOverlappingPairCache();
-		btAlignedFree(m_overlappingPairs);
-	}
-}
-
-
-void	btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax)
-{
-	m_optimizedAabbTree = new btQuantizedBvh();
-	m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax);
-	QuantizedNodeArray&	nodes = m_optimizedAabbTree->getLeafNodeArray();
-	for (int i=0;i<m_sapBroadphases.size();i++)
-	{
-		btQuantizedBvhNode node;
-		btVector3 aabbMin,aabbMax;
-		m_sapBroadphases[i]->getBroadphaseAabb(aabbMin,aabbMax);
-		m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
-		m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
-		int partId = 0;
-		node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i;
-		nodes.push_back(node);
-	}
-	m_optimizedAabbTree->buildInternal();
-}
-
-btBroadphaseProxy*	btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* /*ignoreMe*/)
-{
-	//void* ignoreMe -> we could think of recursive multi-sap, if someone is interested
-
-	void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16);
-	btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin,  aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask);
-	m_multiSapProxies.push_back(proxy);
-
-	///this should deal with inserting/removal into child broadphases
-	setAabb(proxy,aabbMin,aabbMax,dispatcher);
-	return proxy;
-}
-
-void	btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/)
-{
-	///not yet
-	btAssert(0);
-
-}
-
-
-void	btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*	childBroadphase)
-{
-	void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16);
-	btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy;
-	bridgeProxyRef->m_childProxy = childProxy;
-	bridgeProxyRef->m_childBroadphase = childBroadphase;
-	parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef);
-}
-
-
-bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax);
-bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax)
-{
-return
-amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() &&
-amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() &&
-amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ();
-}
-
-
-
-
-
-
-void	btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
-{
-	btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
-	aabbMin = multiProxy->m_aabbMin;
-	aabbMax = multiProxy->m_aabbMax;
-}
-
-void	btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
-{
-	for (int i=0;i<m_multiSapProxies.size();i++)
-	{
-		rayCallback.process(m_multiSapProxies[i]);
-	}
-}
-
-
-//#include <stdio.h>
-
-void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
-{
-	btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
-	multiProxy->m_aabbMin = aabbMin;
-	multiProxy->m_aabbMax = aabbMax;
-	
-	
-//	bool fullyContained = false;
-//	bool alreadyInSimple = false;
-	
-
-
-	
-	struct MyNodeOverlapCallback : public btNodeOverlapCallback
-	{
-		btMultiSapBroadphase*	m_multiSap;
-		btMultiSapProxy*		m_multiProxy;
-		btDispatcher*			m_dispatcher;
-
-		MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher)
-			:m_multiSap(multiSap),
-			m_multiProxy(multiProxy),
-			m_dispatcher(dispatcher)
-		{
-
-		}
-
-		virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex)
-		{
-			btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex];
-
-			int containingBroadphaseIndex = -1;
-			//already found?
-			for (int i=0;i<m_multiProxy->m_bridgeProxies.size();i++)
-			{
-
-				if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
-				{
-					containingBroadphaseIndex = i;
-					break;
-				}
-			}
-			if (containingBroadphaseIndex<0)
-			{
-				//add it
-				btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy);
-				m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase);
-
-			}
-		}
-	};
-
-	MyNodeOverlapCallback	myNodeCallback(this,multiProxy,dispatcher);
-
-
-
-	
-	if (m_optimizedAabbTree)
-		m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
-
-	int i;
-
-	for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
-	{
-		btVector3 worldAabbMin,worldAabbMax;
-		multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
-		bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
-		if (!overlapsBroadphase)
-		{
-			//remove it now
-			btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i];
-
-			btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
-			bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
-			
-			multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1);
-			multiProxy->m_bridgeProxies.pop_back();
-
-		}
-	}
-
-
-	/*
-
-	if (1)
-	{
-
-		//find broadphase that contain this multiProxy
-		int numChildBroadphases = getBroadphaseArray().size();
-		for (int i=0;i<numChildBroadphases;i++)
-		{
-			btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
-			btVector3 worldAabbMin,worldAabbMax;
-			childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
-			bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
-			
-		//	fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
-			int containingBroadphaseIndex = -1;
-			
-			//if already contains this
-			
-			for (int i=0;i<multiProxy->m_bridgeProxies.size();i++)
-			{
-				if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
-				{
-					containingBroadphaseIndex = i;
-				}
-				alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase);
-			}
-
-			if (overlapsBroadphase)
-			{
-				if (containingBroadphaseIndex<0)
-				{
-					btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
-					childProxy->m_multiSapParentProxy = multiProxy;
-					addToChildBroadphase(multiProxy,childProxy,childBroadphase);
-				}
-			} else
-			{
-				if (containingBroadphaseIndex>=0)
-				{
-					//remove
-					btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex];
-
-					btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
-					bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
-					
-					multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1);
-					multiProxy->m_bridgeProxies.pop_back();
-				}
-			}
-		}
-
-
-		///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force)
-		///hopefully we don't end up with many entries here (can assert/provide feedback on stats)
-		if (0)//!multiProxy->m_bridgeProxies.size())
-		{
-			///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
-			///this is needed to be able to calculate the aabb overlap
-			btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
-			childProxy->m_multiSapParentProxy = multiProxy;
-			addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
-		}
-	}
-
-	if (!multiProxy->m_bridgeProxies.size())
-	{
-		///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
-		///this is needed to be able to calculate the aabb overlap
-		btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
-		childProxy->m_multiSapParentProxy = multiProxy;
-		addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
-	}
-*/
-
-
-	//update
-	for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
-	{
-		btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i];
-		bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher);
-	}
-
-}
-bool stopUpdating=false;
-
-
-
-class btMultiSapBroadphasePairSortPredicate
-{
-	public:
-
-		bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 ) const
-		{
-				btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0;
-				btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0;
-				btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0;
-				btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0;
-
-				 return aProxy0 > bProxy0 || 
-					(aProxy0 == bProxy0 && aProxy1 > bProxy1) ||
-					(aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm); 
-		}
-};
-
-
-        ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
-void    btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
-{
-
-//	m_simpleBroadphase->calculateOverlappingPairs(dispatcher);
-
-	if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval())
-	{
-	
-		btBroadphasePairArray&	overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray();
-
-	//	quicksort(overlappingPairArray,0,overlappingPairArray.size());
-
-		overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
-
-		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
-	//	overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
-
-		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
-		m_invalidPair = 0;
-
-		
-		int i;
-
-		btBroadphasePair previousPair;
-		previousPair.m_pProxy0 = 0;
-		previousPair.m_pProxy1 = 0;
-		previousPair.m_algorithm = 0;
-		
-		
-		for (i=0;i<overlappingPairArray.size();i++)
-		{
-		
-			btBroadphasePair& pair = overlappingPairArray[i];
-
-			btMultiSapProxy* aProxy0 = pair.m_pProxy0 ? (btMultiSapProxy*)pair.m_pProxy0->m_multiSapParentProxy : 0;
-			btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0;
-			btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0;
-			btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0;
-
-			bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1);
-			
-			previousPair = pair;
-
-			bool needsRemoval = false;
-
-			if (!isDuplicate)
-			{
-				bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
-
-				if (hasOverlap)
-				{
-					needsRemoval = false;//callback->processOverlap(pair);
-				} else
-				{
-					needsRemoval = true;
-				}
-			} else
-			{
-				//remove duplicate
-				needsRemoval = true;
-				//should have no algorithm
-				btAssert(!pair.m_algorithm);
-			}
-			
-			if (needsRemoval)
-			{
-				getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher);
-
-		//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
-		//		m_overlappingPairArray.pop_back();
-				pair.m_pProxy0 = 0;
-				pair.m_pProxy1 = 0;
-				m_invalidPair++;
-				gOverlappingPairs--;
-			} 
-			
-		}
-
-	///if you don't like to skip the invalid pairs in the array, execute following code:
-	#define CLEAN_INVALID_PAIRS 1
-	#ifdef CLEAN_INVALID_PAIRS
-
-		//perform a sort, to sort 'invalid' pairs to the end
-		//overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
-		overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
-
-		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
-		m_invalidPair = 0;
-	#endif//CLEAN_INVALID_PAIRS
-		
-		//printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
-	}
-
-
-}
-
-
-bool	btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1)
-{
-	btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy;
-		btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy;
-
-		return	TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax,
-			multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax);
-		
-}
-
-
-void	btMultiSapBroadphase::printStats()
-{
-/*	printf("---------------------------------\n");
-	
-		printf("btMultiSapBroadphase.h\n");
-		printf("numHandles = %d\n",m_multiSapProxies.size());
-			//find broadphase that contain this multiProxy
-		int numChildBroadphases = getBroadphaseArray().size();
-		for (int i=0;i<numChildBroadphases;i++)
-		{
-
-			btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
-			childBroadphase->printStats();
-
-		}
-		*/
-
-}
-
-void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher)
-{
-	// not yet
-}
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
deleted file mode 100644
index 7bcfe6b132a..00000000000
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
+++ /dev/null
@@ -1,151 +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.
-*/
-#ifndef BT_MULTI_SAP_BROADPHASE
-#define BT_MULTI_SAP_BROADPHASE
-
-#include "btBroadphaseInterface.h"
-#include "LinearMath/btAlignedObjectArray.h"
-#include "btOverlappingPairCache.h"
-
-
-class btBroadphaseInterface;
-class btSimpleBroadphase;
-
-
-typedef btAlignedObjectArray<btBroadphaseInterface*> btSapBroadphaseArray;
-
-///The btMultiSapBroadphase is a research project, not recommended to use in production. Use btAxisSweep3 or btDbvtBroadphase instead.
-///The btMultiSapBroadphase is a broadphase that contains multiple SAP broadphases.
-///The user can add SAP broadphases that cover the world. A btBroadphaseProxy can be in multiple child broadphases at the same time.
-///A btQuantizedBvh acceleration structures finds overlapping SAPs for each btBroadphaseProxy.
-///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328
-///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329
-class btMultiSapBroadphase :public btBroadphaseInterface
-{
-	btSapBroadphaseArray	m_sapBroadphases;
-	
-	btSimpleBroadphase*		m_simpleBroadphase;
-
-	btOverlappingPairCache*	m_overlappingPairs;
-
-	class btQuantizedBvh*			m_optimizedAabbTree;
-
-
-	bool					m_ownsPairCache;
-	
-	btOverlapFilterCallback*	m_filterCallback;
-
-	int			m_invalidPair;
-
-	struct	btBridgeProxy
-	{
-		btBroadphaseProxy*		m_childProxy;
-		btBroadphaseInterface*	m_childBroadphase;
-	};
-
-
-public:
-
-	struct	btMultiSapProxy	: public btBroadphaseProxy
-	{
-
-		///array with all the entries that this proxy belongs to
-		btAlignedObjectArray<btBridgeProxy*> m_bridgeProxies;
-		btVector3	m_aabbMin;
-		btVector3	m_aabbMax;
-
-		int	m_shapeType;
-
-/*		void*	m_userPtr;
-		short int	m_collisionFilterGroup;
-		short int	m_collisionFilterMask;
-*/
-		btMultiSapProxy(const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask)
-			:btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask),
-			m_aabbMin(aabbMin),
-			m_aabbMax(aabbMax),
-			m_shapeType(shapeType)
-		{
-			m_multiSapParentProxy =this;
-		}
-
-		
-	};
-
-protected:
-
-
-	btAlignedObjectArray<btMultiSapProxy*> m_multiSapProxies;
-
-public:
-
-	btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0);
-
-
-	btSapBroadphaseArray&	getBroadphaseArray()
-	{
-		return m_sapBroadphases;
-	}
-
-	const btSapBroadphaseArray&	getBroadphaseArray() const
-	{
-		return m_sapBroadphases;
-	}
-
-	virtual ~btMultiSapBroadphase();
-
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy);
-	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
-	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
-	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
-
-	virtual void	rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0));
-
-	void	addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*	childBroadphase);
-
-	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
-	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
-
-	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
-
-	virtual	btOverlappingPairCache*	getOverlappingPairCache()
-	{
-		return m_overlappingPairs;
-	}
-	virtual	const btOverlappingPairCache*	getOverlappingPairCache() const
-	{
-		return m_overlappingPairs;
-	}
-
-	///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
-	///will add some transform later
-	virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
-	{
-		aabbMin.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
-		aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
-	}
-
-	void	buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax);
-
-	virtual void	printStats();
-
-	void quicksort (btBroadphasePairArray& a, int lo, int hi);
-
-	///reset broadphase internal structures, to ensure determinism/reproducability
-	virtual void resetPool(btDispatcher* dispatcher);
-
-};
-
-#endif //BT_MULTI_SAP_BROADPHASE
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
index ad69fcbd712..8ce1087c9f1 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
@@ -13,8 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #include "btOverlappingPairCache.h"
 
 #include "btDispatcher.h"
@@ -23,127 +21,95 @@ subject to the following restrictions:
 
 #include <stdio.h>
 
-int	gOverlappingPairs = 0;
-
-int gRemovePairs =0;
-int gAddedPairs =0;
-int gFindPairs =0;
-
-
-
-
-btHashedOverlappingPairCache::btHashedOverlappingPairCache():
-	m_overlapFilterCallback(0),
-	m_ghostPairCallback(0)
+btHashedOverlappingPairCache::btHashedOverlappingPairCache() : m_overlapFilterCallback(0),
+															   m_ghostPairCallback(0)
 {
-	int initialAllocatedSize= 2;
+	int initialAllocatedSize = 2;
 	m_overlappingPairArray.reserve(initialAllocatedSize);
 	growTables();
 }
 
-
-
-
 btHashedOverlappingPairCache::~btHashedOverlappingPairCache()
 {
 }
 
-
-
-void	btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
+void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher)
 {
 	if (pair.m_algorithm && dispatcher)
 	{
 		{
 			pair.m_algorithm->~btCollisionAlgorithm();
 			dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
-			pair.m_algorithm=0;
+			pair.m_algorithm = 0;
 		}
 	}
 }
 
-
-
-
-void	btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
 {
-
-	class	CleanPairCallback : public btOverlapCallback
+	class CleanPairCallback : public btOverlapCallback
 	{
 		btBroadphaseProxy* m_cleanProxy;
-		btOverlappingPairCache*	m_pairCache;
+		btOverlappingPairCache* m_pairCache;
 		btDispatcher* m_dispatcher;
 
 	public:
-		CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
-			:m_cleanProxy(cleanProxy),
-			m_pairCache(pairCache),
-			m_dispatcher(dispatcher)
+		CleanPairCallback(btBroadphaseProxy* cleanProxy, btOverlappingPairCache* pairCache, btDispatcher* dispatcher)
+			: m_cleanProxy(cleanProxy),
+			  m_pairCache(pairCache),
+			  m_dispatcher(dispatcher)
 		{
 		}
-		virtual	bool	processOverlap(btBroadphasePair& pair)
+		virtual bool processOverlap(btBroadphasePair& pair)
 		{
 			if ((pair.m_pProxy0 == m_cleanProxy) ||
 				(pair.m_pProxy1 == m_cleanProxy))
 			{
-				m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
+				m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
 			}
 			return false;
 		}
-		
 	};
 
-	CleanPairCallback cleanPairs(proxy,this,dispatcher);
-
-	processAllOverlappingPairs(&cleanPairs,dispatcher);
+	CleanPairCallback cleanPairs(proxy, this, dispatcher);
 
+	processAllOverlappingPairs(&cleanPairs, dispatcher);
 }
 
-
-
-
-void	btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
 {
-
-	class	RemovePairCallback : public btOverlapCallback
+	class RemovePairCallback : public btOverlapCallback
 	{
 		btBroadphaseProxy* m_obsoleteProxy;
 
 	public:
 		RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
-			:m_obsoleteProxy(obsoleteProxy)
+			: m_obsoleteProxy(obsoleteProxy)
 		{
 		}
-		virtual	bool	processOverlap(btBroadphasePair& pair)
+		virtual bool processOverlap(btBroadphasePair& pair)
 		{
 			return ((pair.m_pProxy0 == m_obsoleteProxy) ||
-				(pair.m_pProxy1 == m_obsoleteProxy));
+					(pair.m_pProxy1 == m_obsoleteProxy));
 		}
-		
 	};
 
-
 	RemovePairCallback removeCallback(proxy);
 
-	processAllOverlappingPairs(&removeCallback,dispatcher);
+	processAllOverlappingPairs(&removeCallback, dispatcher);
 }
 
-
-
-
-
 btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
 {
-	gFindPairs++;
-	if(proxy0->m_uniqueId>proxy1->m_uniqueId) 
-		btSwap(proxy0,proxy1);
+	if (proxy0->m_uniqueId > proxy1->m_uniqueId)
+		btSwap(proxy0, proxy1);
 	int proxyId1 = proxy0->getUid();
 	int proxyId2 = proxy1->getUid();
 
-	/*if (proxyId1 > proxyId2) 
+	/*if (proxyId1 > proxyId2)
 		btSwap(proxyId1, proxyId2);*/
 
-	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
 
 	if (hash >= m_hashTable.size())
 	{
@@ -168,9 +134,8 @@ btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* prox
 
 //#include <stdio.h>
 
-void	btHashedOverlappingPairCache::growTables()
+void btHashedOverlappingPairCache::growTables()
 {
-
 	int newCapacity = m_overlappingPairArray.capacity();
 
 	if (m_hashTable.size() < newCapacity)
@@ -181,10 +146,9 @@ void	btHashedOverlappingPairCache::growTables()
 		m_hashTable.resize(newCapacity);
 		m_next.resize(newCapacity);
 
-
 		int i;
 
-		for (i= 0; i < newCapacity; ++i)
+		for (i = 0; i < newCapacity; ++i)
 		{
 			m_hashTable[i] = BT_NULL_PAIR;
 		}
@@ -193,35 +157,31 @@ void	btHashedOverlappingPairCache::growTables()
 			m_next[i] = BT_NULL_PAIR;
 		}
 
-		for(i=0;i<curHashtableSize;i++)
+		for (i = 0; i < curHashtableSize; i++)
 		{
-	
 			const btBroadphasePair& pair = m_overlappingPairArray[i];
 			int proxyId1 = pair.m_pProxy0->getUid();
 			int proxyId2 = pair.m_pProxy1->getUid();
 			/*if (proxyId1 > proxyId2) 
 				btSwap(proxyId1, proxyId2);*/
-			int	hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));	// New hash value with new mask
+			int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));  // New hash value with new mask
 			m_next[i] = m_hashTable[hashValue];
 			m_hashTable[hashValue] = i;
 		}
-
-
 	}
 }
 
 btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
 {
-	if(proxy0->m_uniqueId>proxy1->m_uniqueId) 
-		btSwap(proxy0,proxy1);
+	if (proxy0->m_uniqueId > proxy1->m_uniqueId)
+		btSwap(proxy0, proxy1);
 	int proxyId1 = proxy0->getUid();
 	int proxyId2 = proxy1->getUid();
 
 	/*if (proxyId1 > proxyId2) 
 		btSwap(proxyId1, proxyId2);*/
 
-	int	hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));	// New hash value with new mask
-
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));  // New hash value with new mask
 
 	btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
 	if (pair != NULL)
@@ -243,7 +203,7 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx
 
 	//this is where we add an actual pair, so also call the 'ghost'
 	if (m_ghostPairCallback)
-		m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
+		m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
 
 	int newCapacity = m_overlappingPairArray.capacity();
 
@@ -251,15 +211,14 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx
 	{
 		growTables();
 		//hash with new capacity
-		hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+		hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
 	}
-	
-	pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
-//	pair->m_pProxy0 = proxy0;
-//	pair->m_pProxy1 = proxy1;
+
+	pair = new (mem) btBroadphasePair(*proxy0, *proxy1);
+	//	pair->m_pProxy0 = proxy0;
+	//	pair->m_pProxy1 = proxy1;
 	pair->m_algorithm = 0;
 	pair->m_internalTmpValue = 0;
-	
 
 	m_next[count] = m_hashTable[hash];
 	m_hashTable[hash] = count;
@@ -267,20 +226,17 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx
 	return pair;
 }
 
-
-
-void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
+void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher)
 {
-	gRemovePairs++;
-	if(proxy0->m_uniqueId>proxy1->m_uniqueId) 
-		btSwap(proxy0,proxy1);
+	if (proxy0->m_uniqueId > proxy1->m_uniqueId)
+		btSwap(proxy0, proxy1);
 	int proxyId1 = proxy0->getUid();
 	int proxyId2 = proxy1->getUid();
 
-	/*if (proxyId1 > proxyId2) 
+	/*if (proxyId1 > proxyId2)
 		btSwap(proxyId1, proxyId2);*/
 
-	int	hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
 
 	btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
 	if (pair == NULL)
@@ -288,7 +244,7 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
 		return 0;
 	}
 
-	cleanOverlappingPair(*pair,dispatcher);
+	cleanOverlappingPair(*pair, dispatcher);
 
 	void* userData = pair->m_internalInfo1;
 
@@ -326,7 +282,7 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
 	int lastPairIndex = m_overlappingPairArray.size() - 1;
 
 	if (m_ghostPairCallback)
-		m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
+		m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1, dispatcher);
 
 	// If the removed pair is the last pair, we are done.
 	if (lastPairIndex == pairIndex)
@@ -337,8 +293,8 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
 
 	// Remove the last pair from the hash table.
 	const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
-		/* missing swap here too, Nat. */ 
-	int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_pProxy0->getUid()), static_cast<unsigned int>(last->m_pProxy1->getUid())) & (m_overlappingPairArray.capacity()-1));
+	/* missing swap here too, Nat. */
+	int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_pProxy0->getUid()), static_cast<unsigned int>(last->m_pProxy1->getUid())) & (m_overlappingPairArray.capacity() - 1));
 
 	index = m_hashTable[lastHash];
 	btAssert(index != BT_NULL_PAIR);
@@ -372,44 +328,106 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
 	return userData;
 }
 //#include <stdio.h>
-
-void	btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
+#include "LinearMath/btQuickprof.h"
+void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher)
 {
-
+	BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
 	int i;
 
-//	printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
-	for (i=0;i<m_overlappingPairArray.size();)
+	//	printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
+	for (i = 0; i < m_overlappingPairArray.size();)
 	{
-	
 		btBroadphasePair* pair = &m_overlappingPairArray[i];
 		if (callback->processOverlap(*pair))
 		{
-			removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
-
-			gOverlappingPairs--;
-		} else
+			removeOverlappingPair(pair->m_pProxy0, pair->m_pProxy1, dispatcher);
+		}
+		else
 		{
 			i++;
 		}
 	}
 }
 
-void	btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
+struct MyPairIndex
+{
+	int m_orgIndex;
+	int m_uidA0;
+	int m_uidA1;
+};
+
+class MyPairIndeSortPredicate
+{
+public:
+	bool operator()(const MyPairIndex& a, const MyPairIndex& b) const
+	{
+		const int uidA0 = a.m_uidA0;
+		const int uidB0 = b.m_uidA0;
+		const int uidA1 = a.m_uidA1;
+		const int uidB1 = b.m_uidA1;
+		return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
+	}
+};
+
+void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo)
+{
+	if (dispatchInfo.m_deterministicOverlappingPairs)
+	{
+		btBroadphasePairArray& pa = getOverlappingPairArray();
+		btAlignedObjectArray<MyPairIndex> indices;
+		{
+			BT_PROFILE("sortOverlappingPairs");
+			indices.resize(pa.size());
+			for (int i = 0; i < indices.size(); i++)
+			{
+				const btBroadphasePair& p = pa[i];
+				const int uidA0 = p.m_pProxy0 ? p.m_pProxy0->m_uniqueId : -1;
+				const int uidA1 = p.m_pProxy1 ? p.m_pProxy1->m_uniqueId : -1;
+
+				indices[i].m_uidA0 = uidA0;
+				indices[i].m_uidA1 = uidA1;
+				indices[i].m_orgIndex = i;
+			}
+			indices.quickSort(MyPairIndeSortPredicate());
+		}
+		{
+			BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
+			int i;
+			for (i = 0; i < indices.size();)
+			{
+				btBroadphasePair* pair = &pa[indices[i].m_orgIndex];
+				if (callback->processOverlap(*pair))
+				{
+					removeOverlappingPair(pair->m_pProxy0, pair->m_pProxy1, dispatcher);
+				}
+				else
+				{
+					i++;
+				}
+			}
+		}
+	}
+	else
+	{
+		processAllOverlappingPairs(callback, dispatcher);
+	}
+}
+
+void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
 {
 	///need to keep hashmap in sync with pair address, so rebuild all
 	btBroadphasePairArray tmpPairs;
 	int i;
-	for (i=0;i<m_overlappingPairArray.size();i++)
+	for (i = 0; i < m_overlappingPairArray.size(); i++)
 	{
 		tmpPairs.push_back(m_overlappingPairArray[i]);
 	}
 
-	for (i=0;i<tmpPairs.size();i++)
+	for (i = 0; i < tmpPairs.size(); i++)
 	{
-		removeOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1,dispatcher);
+		removeOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1, dispatcher);
 	}
-	
+
 	for (i = 0; i < m_next.size(); i++)
 	{
 		m_next[i] = BT_NULL_PAIR;
@@ -417,32 +435,28 @@ void	btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher
 
 	tmpPairs.quickSort(btBroadphasePairSortPredicate());
 
-	for (i=0;i<tmpPairs.size();i++)
+	for (i = 0; i < tmpPairs.size(); i++)
 	{
-		addOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1);
+		addOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1);
 	}
-
-	
 }
 
-
-void*	btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher )
+void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher)
 {
 	if (!hasDeferredRemoval())
 	{
-		btBroadphasePair findPair(*proxy0,*proxy1);
+		btBroadphasePair findPair(*proxy0, *proxy1);
 
 		int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
 		if (findIndex < m_overlappingPairArray.size())
 		{
-			gOverlappingPairs--;
 			btBroadphasePair& pair = m_overlappingPairArray[findIndex];
 			void* userData = pair.m_internalInfo1;
-			cleanOverlappingPair(pair,dispatcher);
+			cleanOverlappingPair(pair, dispatcher);
 			if (m_ghostPairCallback)
-				m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
-			
-			m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1);
+				m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1, dispatcher);
+
+			m_overlappingPairArray.swap(findIndex, m_overlappingPairArray.capacity() - 1);
 			m_overlappingPairArray.pop_back();
 			return userData;
 		}
@@ -451,99 +465,73 @@ void*	btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
 	return 0;
 }
 
-
-
-
-
-
-
-
-btBroadphasePair*	btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
 {
 	//don't add overlap with own
 	btAssert(proxy0 != proxy1);
 
-	if (!needsBroadphaseCollision(proxy0,proxy1))
+	if (!needsBroadphaseCollision(proxy0, proxy1))
 		return 0;
-	
+
 	void* mem = &m_overlappingPairArray.expandNonInitializing();
-	btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
-	
-	gOverlappingPairs++;
-	gAddedPairs++;
-	
+	btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0, *proxy1);
+
 	if (m_ghostPairCallback)
 		m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
 	return pair;
-	
 }
 
 ///this findPair becomes really slow. Either sort the list to speedup the query, or
 ///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
 ///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
 ///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
- btBroadphasePair*	btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+btBroadphasePair* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
 {
-	if (!needsBroadphaseCollision(proxy0,proxy1))
+	if (!needsBroadphaseCollision(proxy0, proxy1))
 		return 0;
 
-	btBroadphasePair tmpPair(*proxy0,*proxy1);
+	btBroadphasePair tmpPair(*proxy0, *proxy1);
 	int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
 
 	if (findIndex < m_overlappingPairArray.size())
 	{
 		//btAssert(it != m_overlappingPairSet.end());
-		 btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
+		btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
 		return pair;
 	}
 	return 0;
 }
 
-
-
-
-
-
-
-
-
-
 //#include <stdio.h>
 
-void	btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
+void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher)
 {
-
 	int i;
 
-	for (i=0;i<m_overlappingPairArray.size();)
+	for (i = 0; i < m_overlappingPairArray.size();)
 	{
-	
 		btBroadphasePair* pair = &m_overlappingPairArray[i];
 		if (callback->processOverlap(*pair))
 		{
-			cleanOverlappingPair(*pair,dispatcher);
+			cleanOverlappingPair(*pair, dispatcher);
 			pair->m_pProxy0 = 0;
 			pair->m_pProxy1 = 0;
-			m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+			m_overlappingPairArray.swap(i, m_overlappingPairArray.size() - 1);
 			m_overlappingPairArray.pop_back();
-			gOverlappingPairs--;
-		} else
+		}
+		else
 		{
 			i++;
 		}
 	}
 }
 
-
-
-
-btSortedOverlappingPairCache::btSortedOverlappingPairCache():
-	m_blockedForChanges(false),
-	m_hasDeferredRemoval(true),
-	m_overlapFilterCallback(0),
-	m_ghostPairCallback(0)
+btSortedOverlappingPairCache::btSortedOverlappingPairCache() : m_blockedForChanges(false),
+															   m_hasDeferredRemoval(true),
+															   m_overlapFilterCallback(0),
+															   m_ghostPairCallback(0)
 {
-	int initialAllocatedSize= 2;
+	int initialAllocatedSize = 2;
 	m_overlappingPairArray.reserve(initialAllocatedSize);
 }
 
@@ -551,82 +539,73 @@ btSortedOverlappingPairCache::~btSortedOverlappingPairCache()
 {
 }
 
-void	btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
+void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher)
 {
 	if (pair.m_algorithm)
 	{
 		{
 			pair.m_algorithm->~btCollisionAlgorithm();
 			dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
-			pair.m_algorithm=0;
-			gRemovePairs--;
+			pair.m_algorithm = 0;
 		}
 	}
 }
 
-
-void	btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
 {
-
-	class	CleanPairCallback : public btOverlapCallback
+	class CleanPairCallback : public btOverlapCallback
 	{
 		btBroadphaseProxy* m_cleanProxy;
-		btOverlappingPairCache*	m_pairCache;
+		btOverlappingPairCache* m_pairCache;
 		btDispatcher* m_dispatcher;
 
 	public:
-		CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
-			:m_cleanProxy(cleanProxy),
-			m_pairCache(pairCache),
-			m_dispatcher(dispatcher)
+		CleanPairCallback(btBroadphaseProxy* cleanProxy, btOverlappingPairCache* pairCache, btDispatcher* dispatcher)
+			: m_cleanProxy(cleanProxy),
+			  m_pairCache(pairCache),
+			  m_dispatcher(dispatcher)
 		{
 		}
-		virtual	bool	processOverlap(btBroadphasePair& pair)
+		virtual bool processOverlap(btBroadphasePair& pair)
 		{
 			if ((pair.m_pProxy0 == m_cleanProxy) ||
 				(pair.m_pProxy1 == m_cleanProxy))
 			{
-				m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
+				m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
 			}
 			return false;
 		}
-		
 	};
 
-	CleanPairCallback cleanPairs(proxy,this,dispatcher);
-
-	processAllOverlappingPairs(&cleanPairs,dispatcher);
+	CleanPairCallback cleanPairs(proxy, this, dispatcher);
 
+	processAllOverlappingPairs(&cleanPairs, dispatcher);
 }
 
-
-void	btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
 {
-
-	class	RemovePairCallback : public btOverlapCallback
+	class RemovePairCallback : public btOverlapCallback
 	{
 		btBroadphaseProxy* m_obsoleteProxy;
 
 	public:
 		RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
-			:m_obsoleteProxy(obsoleteProxy)
+			: m_obsoleteProxy(obsoleteProxy)
 		{
 		}
-		virtual	bool	processOverlap(btBroadphasePair& pair)
+		virtual bool processOverlap(btBroadphasePair& pair)
 		{
 			return ((pair.m_pProxy0 == m_obsoleteProxy) ||
-				(pair.m_pProxy1 == m_obsoleteProxy));
+					(pair.m_pProxy1 == m_obsoleteProxy));
 		}
-		
 	};
 
 	RemovePairCallback removeCallback(proxy);
 
-	processAllOverlappingPairs(&removeCallback,dispatcher);
+	processAllOverlappingPairs(&removeCallback, dispatcher);
 }
 
-void	btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
+void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
 {
 	//should already be sorted
 }
-
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
index 14614270476..56011899cb5 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
@@ -16,7 +16,6 @@ subject to the following restrictions:
 #ifndef BT_OVERLAPPING_PAIR_CACHE_H
 #define BT_OVERLAPPING_PAIR_CACHE_H
 
-
 #include "btBroadphaseInterface.h"
 #include "btBroadphaseProxy.h"
 #include "btOverlappingPairCallback.h"
@@ -24,174 +23,164 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 class btDispatcher;
 
-typedef btAlignedObjectArray<btBroadphasePair>	btBroadphasePairArray;
+typedef btAlignedObjectArray<btBroadphasePair> btBroadphasePairArray;
 
-struct	btOverlapCallback
+struct btOverlapCallback
 {
 	virtual ~btOverlapCallback()
-	{}
+	{
+	}
 	//return true for deletion of the pair
-	virtual bool	processOverlap(btBroadphasePair& pair) = 0;
-
+	virtual bool processOverlap(btBroadphasePair& pair) = 0;
 };
 
 struct btOverlapFilterCallback
 {
 	virtual ~btOverlapFilterCallback()
-	{}
+	{
+	}
 	// return true when pairs need collision
-	virtual bool	needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const = 0;
+	virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const = 0;
 };
 
-
-
-
-
-
-
-extern int gRemovePairs;
-extern int gAddedPairs;
-extern int gFindPairs;
-
-const int BT_NULL_PAIR=0xffffffff;
+const int BT_NULL_PAIR = 0xffffffff;
 
 ///The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases.
 ///The btHashedOverlappingPairCache and btSortedOverlappingPairCache classes are two implementations.
 class btOverlappingPairCache : public btOverlappingPairCallback
 {
 public:
-	virtual ~btOverlappingPairCache() {} // this is needed so we can get to the derived class destructor
+	virtual ~btOverlappingPairCache() {}  // this is needed so we can get to the derived class destructor
 
-	virtual btBroadphasePair*	getOverlappingPairArrayPtr() = 0;
-	
-	virtual const btBroadphasePair*	getOverlappingPairArrayPtr() const = 0;
+	virtual btBroadphasePair* getOverlappingPairArrayPtr() = 0;
 
-	virtual btBroadphasePairArray&	getOverlappingPairArray() = 0;
+	virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 0;
 
-	virtual	void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) = 0;
+	virtual btBroadphasePairArray& getOverlappingPairArray() = 0;
 
-	virtual int getNumOverlappingPairs() const = 0;
+	virtual void cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher) = 0;
 
-	virtual void	cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) = 0;
+	virtual int getNumOverlappingPairs() const = 0;
+	virtual bool needsBroadphaseCollision(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1) const = 0;
+	virtual btOverlapFilterCallback* getOverlapFilterCallback() = 0;
+	virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher) = 0;
 
-	virtual	void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0;
+	virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0;
 
-	virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0;
+	virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher* dispatcher) = 0;
 
+	virtual void processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher, const struct btDispatcherInfo& /*dispatchInfo*/)
+	{
+		processAllOverlappingPairs(callback, dispatcher);
+	}
 	virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0;
 
-	virtual bool	hasDeferredRemoval() = 0;
-
-	virtual	void	setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0;
-
-	virtual void	sortOverlappingPairs(btDispatcher* dispatcher) = 0;
+	virtual bool hasDeferredRemoval() = 0;
 
+	virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) = 0;
 
+	virtual void sortOverlappingPairs(btDispatcher* dispatcher) = 0;
 };
 
 /// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
-class btHashedOverlappingPairCache : public btOverlappingPairCache
+
+ATTRIBUTE_ALIGNED16(class)
+btHashedOverlappingPairCache : public btOverlappingPairCache
 {
-	btBroadphasePairArray	m_overlappingPairArray;
+	btBroadphasePairArray m_overlappingPairArray;
 	btOverlapFilterCallback* m_overlapFilterCallback;
 
 protected:
-	
-	btAlignedObjectArray<int>	m_hashTable;
-	btAlignedObjectArray<int>	m_next;
-	btOverlappingPairCallback*	m_ghostPairCallback;
-
+	btAlignedObjectArray<int> m_hashTable;
+	btAlignedObjectArray<int> m_next;
+	btOverlappingPairCallback* m_ghostPairCallback;
 
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	btHashedOverlappingPairCache();
 	virtual ~btHashedOverlappingPairCache();
 
-	
-	void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+	void removeOverlappingPairsContainingProxy(btBroadphaseProxy * proxy, btDispatcher * dispatcher);
+
+	virtual void* removeOverlappingPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1, btDispatcher * dispatcher);
 
-	virtual void*	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
-	
-	SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
+	SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1) const
 	{
 		if (m_overlapFilterCallback)
-			return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
+			return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
 
 		bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
 		collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
-		
+
 		return collides;
 	}
 
 	// Add a pair and return the new pair. If the pair already exists,
 	// no new pair is created and the old one is returned.
-	virtual btBroadphasePair* 	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+	virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1)
 	{
-		gAddedPairs++;
-
-		if (!needsBroadphaseCollision(proxy0,proxy1))
+		if (!needsBroadphaseCollision(proxy0, proxy1))
 			return 0;
 
-		return internalAddPair(proxy0,proxy1);
+		return internalAddPair(proxy0, proxy1);
 	}
 
-	
+	void cleanProxyFromPairs(btBroadphaseProxy * proxy, btDispatcher * dispatcher);
 
-	void	cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+	virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher * dispatcher);
 
-	
-	virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
+	virtual void processAllOverlappingPairs(btOverlapCallback * callback, btDispatcher * dispatcher, const struct btDispatcherInfo& dispatchInfo);
 
-	virtual btBroadphasePair*	getOverlappingPairArrayPtr()
+	virtual btBroadphasePair* getOverlappingPairArrayPtr()
 	{
 		return &m_overlappingPairArray[0];
 	}
 
-	const btBroadphasePair*	getOverlappingPairArrayPtr() const
+	const btBroadphasePair* getOverlappingPairArrayPtr() const
 	{
 		return &m_overlappingPairArray[0];
 	}
 
-	btBroadphasePairArray&	getOverlappingPairArray()
+	btBroadphasePairArray& getOverlappingPairArray()
 	{
 		return m_overlappingPairArray;
 	}
 
-	const btBroadphasePairArray&	getOverlappingPairArray() const
+	const btBroadphasePairArray& getOverlappingPairArray() const
 	{
 		return m_overlappingPairArray;
 	}
 
-	void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
+	void cleanOverlappingPair(btBroadphasePair & pair, btDispatcher * dispatcher);
 
-
-
-	btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
+	btBroadphasePair* findPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1);
 
 	int GetCount() const { return m_overlappingPairArray.size(); }
-//	btBroadphasePair* GetPairs() { return m_pairs; }
+	//	btBroadphasePair* GetPairs() { return m_pairs; }
 
 	btOverlapFilterCallback* getOverlapFilterCallback()
 	{
 		return m_overlapFilterCallback;
 	}
 
-	void setOverlapFilterCallback(btOverlapFilterCallback* callback)
+	void setOverlapFilterCallback(btOverlapFilterCallback * callback)
 	{
 		m_overlapFilterCallback = callback;
 	}
 
-	int	getNumOverlappingPairs() const
+	int getNumOverlappingPairs() const
 	{
 		return m_overlappingPairArray.size();
 	}
+
 private:
-	
-	btBroadphasePair* 	internalAddPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+	btBroadphasePair* internalAddPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1);
 
-	void	growTables();
+	void growTables();
 
 	SIMD_FORCE_INLINE bool equalsPair(const btBroadphasePair& pair, int proxyId1, int proxyId2)
-	{	
+	{
 		return pair.m_pProxy0->getUid() == proxyId1 && pair.m_pProxy1->getUid() == proxyId2;
 	}
 
@@ -211,43 +200,37 @@ private:
 	}
 	*/
 
-
-	
-	SIMD_FORCE_INLINE	unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
+	SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
 	{
-		int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16));
+		unsigned int key = proxyId1 | (proxyId2 << 16);
 		// Thomas Wang's hash
 
 		key += ~(key << 15);
-		key ^=  (key >> 10);
-		key +=  (key << 3);
-		key ^=  (key >> 6);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
 		key += ~(key << 11);
-		key ^=  (key >> 16);
-		return static_cast<unsigned int>(key);
+		key ^= (key >> 16);
+		return key;
 	}
-	
-
 
-
-
-	SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash)
+	SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1, int hash)
 	{
 		int proxyId1 = proxy0->getUid();
 		int proxyId2 = proxy1->getUid();
-		#if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
+#if 0  // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
 		if (proxyId1 > proxyId2) 
 			btSwap(proxyId1, proxyId2);
-		#endif
+#endif
 
 		int index = m_hashTable[hash];
-		
-		while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+
+		while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
 		{
 			index = m_next[index];
 		}
 
-		if ( index == BT_NULL_PAIR )
+		if (index == BT_NULL_PAIR)
 		{
 			return NULL;
 		}
@@ -257,155 +240,136 @@ private:
 		return &m_overlappingPairArray[index];
 	}
 
-	virtual bool	hasDeferredRemoval()
+	virtual bool hasDeferredRemoval()
 	{
 		return false;
 	}
 
-	virtual	void	setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
+	virtual void setInternalGhostPairCallback(btOverlappingPairCallback * ghostPairCallback)
 	{
 		m_ghostPairCallback = ghostPairCallback;
 	}
 
-	virtual void	sortOverlappingPairs(btDispatcher* dispatcher);
-	
-
-	
+	virtual void sortOverlappingPairs(btDispatcher * dispatcher);
 };
 
-
-
-
 ///btSortedOverlappingPairCache maintains the objects with overlapping AABB
 ///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase
-class	btSortedOverlappingPairCache : public btOverlappingPairCache
+class btSortedOverlappingPairCache : public btOverlappingPairCache
 {
-	protected:
-		//avoid brute-force finding all the time
-		btBroadphasePairArray	m_overlappingPairArray;
+protected:
+	//avoid brute-force finding all the time
+	btBroadphasePairArray m_overlappingPairArray;
 
-		//during the dispatch, check that user doesn't destroy/create proxy
-		bool		m_blockedForChanges;
+	//during the dispatch, check that user doesn't destroy/create proxy
+	bool m_blockedForChanges;
 
-		///by default, do the removal during the pair traversal
-		bool		m_hasDeferredRemoval;
-		
-		//if set, use the callback instead of the built in filter in needBroadphaseCollision
-		btOverlapFilterCallback* m_overlapFilterCallback;
+	///by default, do the removal during the pair traversal
+	bool m_hasDeferredRemoval;
 
-		btOverlappingPairCallback*	m_ghostPairCallback;
+	//if set, use the callback instead of the built in filter in needBroadphaseCollision
+	btOverlapFilterCallback* m_overlapFilterCallback;
 
-	public:
-			
-		btSortedOverlappingPairCache();	
-		virtual ~btSortedOverlappingPairCache();
+	btOverlappingPairCallback* m_ghostPairCallback;
 
-		virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
+public:
+	btSortedOverlappingPairCache();
+	virtual ~btSortedOverlappingPairCache();
 
-		void*	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
+	virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher* dispatcher);
 
-		void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
-		
-		btBroadphasePair*	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+	void* removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher);
 
-		btBroadphasePair*	findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
-			
-		
-		void	cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+	void cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher);
 
-		void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+	btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
 
+	btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
 
-		inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
-		{
-			if (m_overlapFilterCallback)
-				return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
+	void cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher);
 
-			bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
-			collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
-			
-			return collides;
-		}
-		
-		btBroadphasePairArray&	getOverlappingPairArray()
-		{
-			return m_overlappingPairArray;
-		}
+	void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher);
 
-		const btBroadphasePairArray&	getOverlappingPairArray() const
-		{
-			return m_overlappingPairArray;
-		}
+	inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const
+	{
+		if (m_overlapFilterCallback)
+			return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
 
-		
+		bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+		collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
 
+		return collides;
+	}
 
-		btBroadphasePair*	getOverlappingPairArrayPtr()
-		{
-			return &m_overlappingPairArray[0];
-		}
+	btBroadphasePairArray& getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
 
-		const btBroadphasePair*	getOverlappingPairArrayPtr() const
-		{
-			return &m_overlappingPairArray[0];
-		}
+	const btBroadphasePairArray& getOverlappingPairArray() const
+	{
+		return m_overlappingPairArray;
+	}
 
-		int	getNumOverlappingPairs() const
-		{
-			return m_overlappingPairArray.size();
-		}
-		
-		btOverlapFilterCallback* getOverlapFilterCallback()
-		{
-			return m_overlapFilterCallback;
-		}
+	btBroadphasePair* getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
 
-		void setOverlapFilterCallback(btOverlapFilterCallback* callback)
-		{
-			m_overlapFilterCallback = callback;
-		}
+	const btBroadphasePair* getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
 
-		virtual bool	hasDeferredRemoval()
-		{
-			return m_hasDeferredRemoval;
-		}
+	int getNumOverlappingPairs() const
+	{
+		return m_overlappingPairArray.size();
+	}
 
-		virtual	void	setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
-		{
-			m_ghostPairCallback = ghostPairCallback;
-		}
+	btOverlapFilterCallback* getOverlapFilterCallback()
+	{
+		return m_overlapFilterCallback;
+	}
 
-		virtual void	sortOverlappingPairs(btDispatcher* dispatcher);
-		
+	void setOverlapFilterCallback(btOverlapFilterCallback* callback)
+	{
+		m_overlapFilterCallback = callback;
+	}
 
-};
+	virtual bool hasDeferredRemoval()
+	{
+		return m_hasDeferredRemoval;
+	}
 
+	virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
+	{
+		m_ghostPairCallback = ghostPairCallback;
+	}
 
+	virtual void sortOverlappingPairs(btDispatcher* dispatcher);
+};
 
 ///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
 class btNullPairCache : public btOverlappingPairCache
 {
-
-	btBroadphasePairArray	m_overlappingPairArray;
+	btBroadphasePairArray m_overlappingPairArray;
 
 public:
-
-	virtual btBroadphasePair*	getOverlappingPairArrayPtr()
+	virtual btBroadphasePair* getOverlappingPairArrayPtr()
 	{
 		return &m_overlappingPairArray[0];
 	}
-	const btBroadphasePair*	getOverlappingPairArrayPtr() const
+	const btBroadphasePair* getOverlappingPairArrayPtr() const
 	{
 		return &m_overlappingPairArray[0];
 	}
-	btBroadphasePairArray&	getOverlappingPairArray()
+	btBroadphasePairArray& getOverlappingPairArray()
 	{
 		return m_overlappingPairArray;
 	}
-	
-	virtual	void	cleanOverlappingPair(btBroadphasePair& /*pair*/,btDispatcher* /*dispatcher*/)
-	{
 
+	virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/, btDispatcher* /*dispatcher*/)
+	{
 	}
 
 	virtual int getNumOverlappingPairs() const
@@ -413,16 +377,23 @@ public:
 		return 0;
 	}
 
-	virtual void	cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/)
+	virtual void cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/, btDispatcher* /*dispatcher*/)
 	{
-
 	}
 
-	virtual	void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/)
+	bool needsBroadphaseCollision(btBroadphaseProxy*, btBroadphaseProxy*) const
+	{
+		return true;
+	}
+	btOverlapFilterCallback* getOverlapFilterCallback()
+	{
+		return 0;
+	}
+	virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/)
 	{
 	}
 
-	virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* /*dispatcher*/)
+	virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher* /*dispatcher*/)
 	{
 	}
 
@@ -431,39 +402,33 @@ public:
 		return 0;
 	}
 
-	virtual bool	hasDeferredRemoval()
+	virtual bool hasDeferredRemoval()
 	{
 		return true;
 	}
 
-	virtual	void	setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */)
+	virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */)
 	{
-
 	}
 
-	virtual btBroadphasePair*	addOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/)
+	virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/)
 	{
 		return 0;
 	}
 
-	virtual void*	removeOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/,btDispatcher* /*dispatcher*/)
+	virtual void* removeOverlappingPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/, btDispatcher* /*dispatcher*/)
 	{
 		return 0;
 	}
 
-	virtual void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/)
+	virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/, btDispatcher* /*dispatcher*/)
 	{
 	}
-	
-	virtual void	sortOverlappingPairs(btDispatcher* dispatcher)
+
+	virtual void sortOverlappingPairs(btDispatcher* dispatcher)
 	{
-        (void) dispatcher;
+		(void)dispatcher;
 	}
-
-
 };
 
-
-#endif //BT_OVERLAPPING_PAIR_CACHE_H
-
-
+#endif  //BT_OVERLAPPING_PAIR_CACHE_H
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h
index 9c7b6f81367..d16c72542f1 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h
@@ -18,23 +18,24 @@ subject to the following restrictions:
 #define OVERLAPPING_PAIR_CALLBACK_H
 
 class btDispatcher;
-struct  btBroadphasePair;
+struct btBroadphasePair;
 
 ///The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache.
 class btOverlappingPairCallback
 {
+protected:
+	btOverlappingPairCallback() {}
+
 public:
 	virtual ~btOverlappingPairCallback()
 	{
-
 	}
-	
-	virtual btBroadphasePair*	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) = 0;
 
-	virtual void*	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) = 0;
+	virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0;
 
-	virtual void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher) = 0;
+	virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher) = 0;
 
+	virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0, btDispatcher* dispatcher) = 0;
 };
 
-#endif //OVERLAPPING_PAIR_CALLBACK_H
+#endif  //OVERLAPPING_PAIR_CALLBACK_H
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
index 889216df509..19f1737b73c 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
@@ -21,43 +21,38 @@ subject to the following restrictions:
 
 #define RAYAABB2
 
-btQuantizedBvh::btQuantizedBvh() : 
-					m_bulletVersion(BT_BULLET_VERSION),
-					m_useQuantization(false), 
-					//m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY)
-					m_traversalMode(TRAVERSAL_STACKLESS)
-					//m_traversalMode(TRAVERSAL_RECURSIVE)
-					,m_subtreeHeaderCount(0) //PCK: add this line
+btQuantizedBvh::btQuantizedBvh() : m_bulletVersion(BT_BULLET_VERSION),
+								   m_useQuantization(false),
+								   //m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY)
+								   m_traversalMode(TRAVERSAL_STACKLESS)
+								   //m_traversalMode(TRAVERSAL_RECURSIVE)
+								   ,
+								   m_subtreeHeaderCount(0)  //PCK: add this line
 {
-	m_bvhAabbMin.setValue(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY);
-	m_bvhAabbMax.setValue(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
+	m_bvhAabbMin.setValue(-SIMD_INFINITY, -SIMD_INFINITY, -SIMD_INFINITY);
+	m_bvhAabbMax.setValue(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
 }
 
-
-
-
-
 void btQuantizedBvh::buildInternal()
 {
 	///assumes that caller filled in the m_quantizedLeafNodes
 	m_useQuantization = true;
 	int numLeafNodes = 0;
-	
+
 	if (m_useQuantization)
 	{
 		//now we have an array of leafnodes in m_leafNodes
 		numLeafNodes = m_quantizedLeafNodes.size();
 
-		m_quantizedContiguousNodes.resize(2*numLeafNodes);
-
+		m_quantizedContiguousNodes.resize(2 * numLeafNodes);
 	}
 
 	m_curNodeIndex = 0;
 
-	buildTree(0,numLeafNodes);
+	buildTree(0, numLeafNodes);
 
 	///if the entire tree is small then subtree size, we need to create a header info for the tree
-	if(m_useQuantization && !m_SubtreeHeaders.size())
+	if (m_useQuantization && !m_SubtreeHeaders.size())
 	{
 		btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
 		subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]);
@@ -73,29 +68,24 @@ void btQuantizedBvh::buildInternal()
 	m_leafNodes.clear();
 }
 
-
-
 ///just for debugging, to visualize the individual patches/subtrees
 #ifdef DEBUG_PATCH_COLORS
-btVector3 color[4]=
-{
-	btVector3(1,0,0),
-	btVector3(0,1,0),
-	btVector3(0,0,1),
-	btVector3(0,1,1)
-};
-#endif //DEBUG_PATCH_COLORS
-
-
+btVector3 color[4] =
+	{
+		btVector3(1, 0, 0),
+		btVector3(0, 1, 0),
+		btVector3(0, 0, 1),
+		btVector3(0, 1, 1)};
+#endif  //DEBUG_PATCH_COLORS
 
-void	btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin)
+void btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, btScalar quantizationMargin)
 {
 	//enlarge the AABB to avoid division by zero when initializing the quantization values
-	btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin);
+	btVector3 clampValue(quantizationMargin, quantizationMargin, quantizationMargin);
 	m_bvhAabbMin = bvhAabbMin - clampValue;
 	m_bvhAabbMax = bvhAabbMax + clampValue;
 	btVector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin;
-	m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize;
+	m_bvhQuantization = btVector3(btScalar(65533.0), btScalar(65533.0), btScalar(65533.0)) / aabbSize;
 
 	m_useQuantization = true;
 
@@ -103,23 +93,22 @@ void	btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btV
 		unsigned short vecIn[3];
 		btVector3 v;
 		{
-			quantize(vecIn,m_bvhAabbMin,false);
+			quantize(vecIn, m_bvhAabbMin, false);
 			v = unQuantize(vecIn);
-			m_bvhAabbMin.setMin(v-clampValue);
+			m_bvhAabbMin.setMin(v - clampValue);
 		}
+		aabbSize = m_bvhAabbMax - m_bvhAabbMin;
+		m_bvhQuantization = btVector3(btScalar(65533.0), btScalar(65533.0), btScalar(65533.0)) / aabbSize;
 		{
-			quantize(vecIn,m_bvhAabbMax,true);
+			quantize(vecIn, m_bvhAabbMax, true);
 			v = unQuantize(vecIn);
-			m_bvhAabbMax.setMax(v+clampValue);
+			m_bvhAabbMax.setMax(v + clampValue);
 		}
 		aabbSize = m_bvhAabbMax - m_bvhAabbMin;
-		m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize;
+		m_bvhQuantization = btVector3(btScalar(65533.0), btScalar(65533.0), btScalar(65533.0)) / aabbSize;
 	}
 }
 
-
-
-
 btQuantizedBvh::~btQuantizedBvh()
 {
 }
@@ -127,104 +116,100 @@ btQuantizedBvh::~btQuantizedBvh()
 #ifdef DEBUG_TREE_BUILDING
 int gStackDepth = 0;
 int gMaxStackDepth = 0;
-#endif //DEBUG_TREE_BUILDING
+#endif  //DEBUG_TREE_BUILDING
 
-void	btQuantizedBvh::buildTree	(int startIndex,int endIndex)
+void btQuantizedBvh::buildTree(int startIndex, int endIndex)
 {
 #ifdef DEBUG_TREE_BUILDING
 	gStackDepth++;
 	if (gStackDepth > gMaxStackDepth)
 		gMaxStackDepth = gStackDepth;
-#endif //DEBUG_TREE_BUILDING
-
+#endif  //DEBUG_TREE_BUILDING
 
 	int splitAxis, splitIndex, i;
-	int numIndices =endIndex-startIndex;
+	int numIndices = endIndex - startIndex;
 	int curIndex = m_curNodeIndex;
 
-	btAssert(numIndices>0);
+	btAssert(numIndices > 0);
 
-	if (numIndices==1)
+	if (numIndices == 1)
 	{
 #ifdef DEBUG_TREE_BUILDING
 		gStackDepth--;
-#endif //DEBUG_TREE_BUILDING
-		
-		assignInternalNodeFromLeafNode(m_curNodeIndex,startIndex);
+#endif  //DEBUG_TREE_BUILDING
+
+		assignInternalNodeFromLeafNode(m_curNodeIndex, startIndex);
 
 		m_curNodeIndex++;
-		return;	
+		return;
 	}
 	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
-	
-	splitAxis = calcSplittingAxis(startIndex,endIndex);
 
-	splitIndex = sortAndCalcSplittingIndex(startIndex,endIndex,splitAxis);
+	splitAxis = calcSplittingAxis(startIndex, endIndex);
+
+	splitIndex = sortAndCalcSplittingIndex(startIndex, endIndex, splitAxis);
 
 	int internalNodeIndex = m_curNodeIndex;
-	
+
 	//set the min aabb to 'inf' or a max value, and set the max aabb to a -inf/minimum value.
 	//the aabb will be expanded during buildTree/mergeInternalNodeAabb with actual node values
-	setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);//can't use btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY)) because of quantization
-	setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);//can't use btVector3(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY)) because of quantization
-	
-	
-	for (i=startIndex;i<endIndex;i++)
+	setInternalNodeAabbMin(m_curNodeIndex, m_bvhAabbMax);  //can't use btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY)) because of quantization
+	setInternalNodeAabbMax(m_curNodeIndex, m_bvhAabbMin);  //can't use btVector3(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY)) because of quantization
+
+	for (i = startIndex; i < endIndex; i++)
 	{
-		mergeInternalNodeAabb(m_curNodeIndex,getAabbMin(i),getAabbMax(i));
+		mergeInternalNodeAabb(m_curNodeIndex, getAabbMin(i), getAabbMax(i));
 	}
 
 	m_curNodeIndex++;
-	
 
 	//internalNode->m_escapeIndex;
-	
+
 	int leftChildNodexIndex = m_curNodeIndex;
 
 	//build left child tree
-	buildTree(startIndex,splitIndex);
+	buildTree(startIndex, splitIndex);
 
 	int rightChildNodexIndex = m_curNodeIndex;
 	//build right child tree
-	buildTree(splitIndex,endIndex);
+	buildTree(splitIndex, endIndex);
 
 #ifdef DEBUG_TREE_BUILDING
 	gStackDepth--;
-#endif //DEBUG_TREE_BUILDING
+#endif  //DEBUG_TREE_BUILDING
 
 	int escapeIndex = m_curNodeIndex - curIndex;
 
 	if (m_useQuantization)
 	{
 		//escapeIndex is the number of nodes of this subtree
-		const int sizeQuantizedNode =sizeof(btQuantizedBvhNode);
+		const int sizeQuantizedNode = sizeof(btQuantizedBvhNode);
 		const int treeSizeInBytes = escapeIndex * sizeQuantizedNode;
 		if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES)
 		{
-			updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex);
+			updateSubtreeHeaders(leftChildNodexIndex, rightChildNodexIndex);
 		}
-	} else
+	}
+	else
 	{
-
 	}
 
-	setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex);
-
+	setInternalNodeEscapeIndex(internalNodeIndex, escapeIndex);
 }
 
-void	btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex)
+void btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex, int rightChildNodexIndex)
 {
 	btAssert(m_useQuantization);
 
 	btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex];
 	int leftSubTreeSize = leftChildNode.isLeafNode() ? 1 : leftChildNode.getEscapeIndex();
-	int leftSubTreeSizeInBytes =  leftSubTreeSize * static_cast<int>(sizeof(btQuantizedBvhNode));
-	
+	int leftSubTreeSizeInBytes = leftSubTreeSize * static_cast<int>(sizeof(btQuantizedBvhNode));
+
 	btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex];
 	int rightSubTreeSize = rightChildNode.isLeafNode() ? 1 : rightChildNode.getEscapeIndex();
-	int rightSubTreeSizeInBytes =  rightSubTreeSize *  static_cast<int>(sizeof(btQuantizedBvhNode));
+	int rightSubTreeSizeInBytes = rightSubTreeSize * static_cast<int>(sizeof(btQuantizedBvhNode));
 
-	if(leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES)
+	if (leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES)
 	{
 		btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
 		subtree.setAabbFromQuantizeNode(leftChildNode);
@@ -232,7 +217,7 @@ void	btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChild
 		subtree.m_subtreeSize = leftSubTreeSize;
 	}
 
-	if(rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES)
+	if (rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES)
 	{
 		btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
 		subtree.setAabbFromQuantizeNode(rightChildNode);
@@ -244,32 +229,31 @@ void	btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChild
 	m_subtreeHeaderCount = m_SubtreeHeaders.size();
 }
 
-
-int	btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis)
+int btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex, int endIndex, int splitAxis)
 {
 	int i;
-	int splitIndex =startIndex;
+	int splitIndex = startIndex;
 	int numIndices = endIndex - startIndex;
 	btScalar splitValue;
 
-	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
-	for (i=startIndex;i<endIndex;i++)
+	btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
-		means+=center;
+		btVector3 center = btScalar(0.5) * (getAabbMax(i) + getAabbMin(i));
+		means += center;
 	}
-	means *= (btScalar(1.)/(btScalar)numIndices);
-	
+	means *= (btScalar(1.) / (btScalar)numIndices);
+
 	splitValue = means[splitAxis];
-	
+
 	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
-	for (i=startIndex;i<endIndex;i++)
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
+		btVector3 center = btScalar(0.5) * (getAabbMax(i) + getAabbMin(i));
 		if (center[splitAxis] > splitValue)
 		{
 			//swap
-			swapLeafNodes(i,splitIndex);
+			swapLeafNodes(i, splitIndex);
 			splitIndex++;
 		}
 	}
@@ -279,56 +263,53 @@ int	btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int sp
 	//unbalanced1 is unsafe: it can cause stack overflows
 	//bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
 
-	//unbalanced2 should work too: always use center (perfect balanced trees)	
+	//unbalanced2 should work too: always use center (perfect balanced trees)
 	//bool unbalanced2 = true;
 
 	//this should be safe too:
-	int rangeBalancedIndices = numIndices/3;
-	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
-	
+	int rangeBalancedIndices = numIndices / 3;
+	bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices)));
+
 	if (unbalanced)
 	{
-		splitIndex = startIndex+ (numIndices>>1);
+		splitIndex = startIndex + (numIndices >> 1);
 	}
 
-	bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
+	bool unbal = (splitIndex == startIndex) || (splitIndex == (endIndex));
 	(void)unbal;
 	btAssert(!unbal);
 
 	return splitIndex;
 }
 
-
-int	btQuantizedBvh::calcSplittingAxis(int startIndex,int endIndex)
+int btQuantizedBvh::calcSplittingAxis(int startIndex, int endIndex)
 {
 	int i;
 
-	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
-	btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
-	int numIndices = endIndex-startIndex;
+	btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
+	btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.));
+	int numIndices = endIndex - startIndex;
 
-	for (i=startIndex;i<endIndex;i++)
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
-		means+=center;
+		btVector3 center = btScalar(0.5) * (getAabbMax(i) + getAabbMin(i));
+		means += center;
 	}
-	means *= (btScalar(1.)/(btScalar)numIndices);
-		
-	for (i=startIndex;i<endIndex;i++)
+	means *= (btScalar(1.) / (btScalar)numIndices);
+
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
-		btVector3 diff2 = center-means;
+		btVector3 center = btScalar(0.5) * (getAabbMax(i) + getAabbMin(i));
+		btVector3 diff2 = center - means;
 		diff2 = diff2 * diff2;
 		variance += diff2;
 	}
-	variance *= (btScalar(1.)/	((btScalar)numIndices-1)	);
-	
+	variance *= (btScalar(1.) / ((btScalar)numIndices - 1));
+
 	return variance.maxAxis();
 }
 
-
-
-void	btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
+void btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const
 {
 	//either choose recursive traversal (walkTree) or stackless (walkStacklessTree)
 
@@ -337,38 +318,35 @@ void	btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallb
 		///quantize query AABB
 		unsigned short int quantizedQueryAabbMin[3];
 		unsigned short int quantizedQueryAabbMax[3];
-		quantizeWithClamp(quantizedQueryAabbMin,aabbMin,0);
-		quantizeWithClamp(quantizedQueryAabbMax,aabbMax,1);
+		quantizeWithClamp(quantizedQueryAabbMin, aabbMin, 0);
+		quantizeWithClamp(quantizedQueryAabbMax, aabbMax, 1);
 
 		switch (m_traversalMode)
 		{
-		case TRAVERSAL_STACKLESS:
-				walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,0,m_curNodeIndex);
-			break;
-		case TRAVERSAL_STACKLESS_CACHE_FRIENDLY:
-				walkStacklessQuantizedTreeCacheFriendly(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
-			break;
-		case TRAVERSAL_RECURSIVE:
+			case TRAVERSAL_STACKLESS:
+				walkStacklessQuantizedTree(nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax, 0, m_curNodeIndex);
+				break;
+			case TRAVERSAL_STACKLESS_CACHE_FRIENDLY:
+				walkStacklessQuantizedTreeCacheFriendly(nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax);
+				break;
+			case TRAVERSAL_RECURSIVE:
 			{
 				const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[0];
-				walkRecursiveQuantizedTreeAgainstQueryAabb(rootNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
+				walkRecursiveQuantizedTreeAgainstQueryAabb(rootNode, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax);
 			}
 			break;
-		default:
-			//unsupported
-			btAssert(0);
+			default:
+				//unsupported
+				btAssert(0);
 		}
-	} else
+	}
+	else
 	{
-		walkStacklessTree(nodeCallback,aabbMin,aabbMax);
+		walkStacklessTree(nodeCallback, aabbMin, aabbMax);
 	}
 }
 
-
-int maxIterations = 0;
-
-
-void	btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
+void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const
 {
 	btAssert(!m_useQuantization);
 
@@ -382,33 +360,31 @@ void	btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const
 	while (curIndex < m_curNodeIndex)
 	{
 		//catch bugs in tree data
-		btAssert (walkIterations < m_curNodeIndex);
+		btAssert(walkIterations < m_curNodeIndex);
 
 		walkIterations++;
-		aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg);
+		aabbOverlap = TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode->m_aabbMinOrg, rootNode->m_aabbMaxOrg);
 		isLeafNode = rootNode->m_escapeIndex == -1;
-		
+
 		//PCK: unsigned instead of bool
 		if (isLeafNode && (aabbOverlap != 0))
 		{
-			nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex);
-		} 
-		
+			nodeCallback->processNode(rootNode->m_subPart, rootNode->m_triangleIndex);
+		}
+
 		//PCK: unsigned instead of bool
 		if ((aabbOverlap != 0) || isLeafNode)
 		{
 			rootNode++;
 			curIndex++;
-		} else
+		}
+		else
 		{
 			escapeIndex = rootNode->m_escapeIndex;
 			rootNode += escapeIndex;
 			curIndex += escapeIndex;
 		}
 	}
-	if (maxIterations < walkIterations)
-		maxIterations = walkIterations;
-
 }
 
 /*
@@ -432,39 +408,38 @@ void	btQuantizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback
 }
 */
 
-void btQuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const
+void btQuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode, btNodeOverlapCallback* nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax) const
 {
 	btAssert(m_useQuantization);
-	
+
 	bool isLeafNode;
 	//PCK: unsigned instead of bool
 	unsigned aabbOverlap;
 
 	//PCK: unsigned instead of bool
-	aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,currentNode->m_quantizedAabbMin,currentNode->m_quantizedAabbMax);
+	aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, currentNode->m_quantizedAabbMin, currentNode->m_quantizedAabbMax);
 	isLeafNode = currentNode->isLeafNode();
-		
+
 	//PCK: unsigned instead of bool
 	if (aabbOverlap != 0)
 	{
 		if (isLeafNode)
 		{
-			nodeCallback->processNode(currentNode->getPartId(),currentNode->getTriangleIndex());
-		} else
+			nodeCallback->processNode(currentNode->getPartId(), currentNode->getTriangleIndex());
+		}
+		else
 		{
 			//process left and right children
-			const btQuantizedBvhNode* leftChildNode = currentNode+1;
-			walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
+			const btQuantizedBvhNode* leftChildNode = currentNode + 1;
+			walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax);
 
-			const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode+1:leftChildNode+leftChildNode->getEscapeIndex();
-			walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
+			const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode + 1 : leftChildNode + leftChildNode->getEscapeIndex();
+			walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax);
 		}
-	}		
+	}
 }
 
-
-
-void	btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const
+void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const
 {
 	btAssert(!m_useQuantization);
 
@@ -473,11 +448,11 @@ void	btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall
 	int walkIterations = 0;
 	bool isLeafNode;
 	//PCK: unsigned instead of bool
-	unsigned aabbOverlap=0;
-	unsigned rayBoxOverlap=0;
+	unsigned aabbOverlap = 0;
+	unsigned rayBoxOverlap = 0;
 	btScalar lambda_max = 1.0;
-	
-		/* Quick pruning by quantized box */
+
+	/* Quick pruning by quantized box */
 	btVector3 rayAabbMin = raySource;
 	btVector3 rayAabbMax = raySource;
 	rayAabbMin.setMin(rayTarget);
@@ -488,15 +463,15 @@ void	btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall
 	rayAabbMax += aabbMax;
 
 #ifdef RAYAABB2
-	btVector3 rayDir = (rayTarget-raySource);
-	rayDir.normalize ();
-	lambda_max = rayDir.dot(rayTarget-raySource);
+	btVector3 rayDir = (rayTarget - raySource);
+	rayDir.safeNormalize();// stephengold changed normalize to safeNormalize 2020-02-17
+	lambda_max = rayDir.dot(rayTarget - raySource);
 	///what about division by zero? --> just set rayDirection[i] to 1.0
 	btVector3 rayDirectionInverse;
 	rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
 	rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
 	rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
-	unsigned int sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
+	unsigned int sign[3] = {rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
 #endif
 
 	btVector3 bounds[2];
@@ -505,7 +480,7 @@ void	btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall
 	{
 		btScalar param = 1.0;
 		//catch bugs in tree data
-		btAssert (walkIterations < m_curNodeIndex);
+		btAssert(walkIterations < m_curNodeIndex);
 
 		walkIterations++;
 
@@ -515,51 +490,47 @@ void	btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall
 		bounds[0] -= aabbMax;
 		bounds[1] -= aabbMin;
 
-		aabbOverlap = TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg);
+		aabbOverlap = TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, rootNode->m_aabbMinOrg, rootNode->m_aabbMaxOrg);
 		//perhaps profile if it is worth doing the aabbOverlap test first
 
 #ifdef RAYAABB2
-			///careful with this check: need to check division by zero (above) and fix the unQuantize method
-			///thanks Joerg/hiker for the reproduction case!
-			///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858
-		rayBoxOverlap = aabbOverlap ? btRayAabb2 (raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false;
+		///careful with this check: need to check division by zero (above) and fix the unQuantize method
+		///thanks Joerg/hiker for the reproduction case!
+		///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858
+		rayBoxOverlap = aabbOverlap ? btRayAabb2(raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false;
 
 #else
 		btVector3 normal;
-		rayBoxOverlap = btRayAabb(raySource, rayTarget,bounds[0],bounds[1],param, normal);
+		rayBoxOverlap = btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal);
 #endif
 
 		isLeafNode = rootNode->m_escapeIndex == -1;
-		
+
 		//PCK: unsigned instead of bool
 		if (isLeafNode && (rayBoxOverlap != 0))
 		{
-			nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex);
-		} 
-		
+			nodeCallback->processNode(rootNode->m_subPart, rootNode->m_triangleIndex);
+		}
+
 		//PCK: unsigned instead of bool
 		if ((rayBoxOverlap != 0) || isLeafNode)
 		{
 			rootNode++;
 			curIndex++;
-		} else
+		}
+		else
 		{
 			escapeIndex = rootNode->m_escapeIndex;
 			rootNode += escapeIndex;
 			curIndex += escapeIndex;
 		}
 	}
-	if (maxIterations < walkIterations)
-		maxIterations = walkIterations;
-
 }
 
-
-
-void	btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const
+void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const
 {
 	btAssert(m_useQuantization);
-	
+
 	int curIndex = startNodeIndex;
 	int walkIterations = 0;
 	int subTreeSize = endNodeIndex - startNodeIndex;
@@ -567,7 +538,7 @@ void	btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback*
 
 	const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex];
 	int escapeIndex;
-	
+
 	bool isLeafNode;
 	//PCK: unsigned instead of bool
 	unsigned boxBoxOverlap = 0;
@@ -576,14 +547,14 @@ void	btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback*
 	btScalar lambda_max = 1.0;
 
 #ifdef RAYAABB2
-	btVector3 rayDirection = (rayTarget-raySource);
-	rayDirection.normalize ();
-	lambda_max = rayDirection.dot(rayTarget-raySource);
+	btVector3 rayDirection = (rayTarget - raySource);
+	rayDirection.safeNormalize();// stephengold changed normalize to safeNormalize 2020-02-17
+	lambda_max = rayDirection.dot(rayTarget - raySource);
 	///what about division by zero? --> just set rayDirection[i] to 1.0
 	rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[0];
 	rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[1];
 	rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[2];
-	unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0};
+	unsigned int sign[3] = {rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0};
 #endif
 
 	/* Quick pruning by quantized box */
@@ -598,37 +569,36 @@ void	btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback*
 
 	unsigned short int quantizedQueryAabbMin[3];
 	unsigned short int quantizedQueryAabbMax[3];
-	quantizeWithClamp(quantizedQueryAabbMin,rayAabbMin,0);
-	quantizeWithClamp(quantizedQueryAabbMax,rayAabbMax,1);
+	quantizeWithClamp(quantizedQueryAabbMin, rayAabbMin, 0);
+	quantizeWithClamp(quantizedQueryAabbMax, rayAabbMax, 1);
 
 	while (curIndex < endNodeIndex)
 	{
-
 //#define VISUALLY_ANALYZE_BVH 1
 #ifdef VISUALLY_ANALYZE_BVH
 		//some code snippet to debugDraw aabb, to visually analyze bvh structure
 		static int drawPatch = 0;
 		//need some global access to a debugDrawer
 		extern btIDebugDraw* debugDrawerPtr;
-		if (curIndex==drawPatch)
+		if (curIndex == drawPatch)
 		{
-			btVector3 aabbMin,aabbMax;
+			btVector3 aabbMin, aabbMax;
 			aabbMin = unQuantize(rootNode->m_quantizedAabbMin);
 			aabbMax = unQuantize(rootNode->m_quantizedAabbMax);
-			btVector3	color(1,0,0);
-			debugDrawerPtr->drawAabb(aabbMin,aabbMax,color);
+			btVector3 color(1, 0, 0);
+			debugDrawerPtr->drawAabb(aabbMin, aabbMax, color);
 		}
-#endif//VISUALLY_ANALYZE_BVH
+#endif  //VISUALLY_ANALYZE_BVH
 
 		//catch bugs in tree data
-		btAssert (walkIterations < subTreeSize);
+		btAssert(walkIterations < subTreeSize);
 
 		walkIterations++;
 		//PCK: unsigned instead of bool
 		// only interested if this is closer than any previous hit
 		btScalar param = 1.0;
 		rayBoxOverlap = 0;
-		boxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
+		boxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode->m_quantizedAabbMin, rootNode->m_quantizedAabbMax);
 		isLeafNode = rootNode->isLeafNode();
 		if (boxBoxOverlap)
 		{
@@ -653,39 +623,37 @@ void	btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback*
 			///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858
 
 			//BT_PROFILE("btRayAabb2");
-			rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max);
-			
+			rayBoxOverlap = btRayAabb2(raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max);
+
 #else
-			rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal);
+			rayBoxOverlap = true;  //btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal);
 #endif
 		}
-		
+
 		if (isLeafNode && rayBoxOverlap)
 		{
-			nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex());
+			nodeCallback->processNode(rootNode->getPartId(), rootNode->getTriangleIndex());
 		}
-		
+
 		//PCK: unsigned instead of bool
 		if ((rayBoxOverlap != 0) || isLeafNode)
 		{
 			rootNode++;
 			curIndex++;
-		} else
+		}
+		else
 		{
 			escapeIndex = rootNode->getEscapeIndex();
 			rootNode += escapeIndex;
 			curIndex += escapeIndex;
 		}
 	}
-	if (maxIterations < walkIterations)
-		maxIterations = walkIterations;
-
 }
 
-void	btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const
+void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax, int startNodeIndex, int endNodeIndex) const
 {
 	btAssert(m_useQuantization);
-	
+
 	int curIndex = startNodeIndex;
 	int walkIterations = 0;
 	int subTreeSize = endNodeIndex - startNodeIndex;
@@ -693,91 +661,85 @@ void	btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb
 
 	const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex];
 	int escapeIndex;
-	
+
 	bool isLeafNode;
 	//PCK: unsigned instead of bool
 	unsigned aabbOverlap;
 
 	while (curIndex < endNodeIndex)
 	{
-
 //#define VISUALLY_ANALYZE_BVH 1
 #ifdef VISUALLY_ANALYZE_BVH
 		//some code snippet to debugDraw aabb, to visually analyze bvh structure
 		static int drawPatch = 0;
 		//need some global access to a debugDrawer
 		extern btIDebugDraw* debugDrawerPtr;
-		if (curIndex==drawPatch)
+		if (curIndex == drawPatch)
 		{
-			btVector3 aabbMin,aabbMax;
+			btVector3 aabbMin, aabbMax;
 			aabbMin = unQuantize(rootNode->m_quantizedAabbMin);
 			aabbMax = unQuantize(rootNode->m_quantizedAabbMax);
-			btVector3	color(1,0,0);
-			debugDrawerPtr->drawAabb(aabbMin,aabbMax,color);
+			btVector3 color(1, 0, 0);
+			debugDrawerPtr->drawAabb(aabbMin, aabbMax, color);
 		}
-#endif//VISUALLY_ANALYZE_BVH
+#endif  //VISUALLY_ANALYZE_BVH
 
 		//catch bugs in tree data
-		btAssert (walkIterations < subTreeSize);
+		btAssert(walkIterations < subTreeSize);
 
 		walkIterations++;
 		//PCK: unsigned instead of bool
-		aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
+		aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode->m_quantizedAabbMin, rootNode->m_quantizedAabbMax);
 		isLeafNode = rootNode->isLeafNode();
-		
+
 		if (isLeafNode && aabbOverlap)
 		{
-			nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex());
-		} 
-		
+			nodeCallback->processNode(rootNode->getPartId(), rootNode->getTriangleIndex());
+		}
+
 		//PCK: unsigned instead of bool
 		if ((aabbOverlap != 0) || isLeafNode)
 		{
 			rootNode++;
 			curIndex++;
-		} else
+		}
+		else
 		{
 			escapeIndex = rootNode->getEscapeIndex();
 			rootNode += escapeIndex;
 			curIndex += escapeIndex;
 		}
 	}
-	if (maxIterations < walkIterations)
-		maxIterations = walkIterations;
-
 }
 
 //This traversal can be called from Playstation 3 SPU
-void	btQuantizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const
+void btQuantizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax) const
 {
 	btAssert(m_useQuantization);
 
 	int i;
 
-
-	for (i=0;i<this->m_SubtreeHeaders.size();i++)
+	for (i = 0; i < this->m_SubtreeHeaders.size(); i++)
 	{
 		const btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
 
 		//PCK: unsigned instead of bool
-		unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+		unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, subtree.m_quantizedAabbMin, subtree.m_quantizedAabbMax);
 		if (overlap != 0)
 		{
-			walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,
-				subtree.m_rootNodeIndex,
-				subtree.m_rootNodeIndex+subtree.m_subtreeSize);
+			walkStacklessQuantizedTree(nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax,
+									   subtree.m_rootNodeIndex,
+									   subtree.m_rootNodeIndex + subtree.m_subtreeSize);
 		}
 	}
 }
 
-
-void	btQuantizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const
+void btQuantizedBvh::reportRayOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const
 {
-	reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,btVector3(0,0,0),btVector3(0,0,0));
+	reportBoxCastOverlappingNodex(nodeCallback, raySource, rayTarget, btVector3(0, 0, 0), btVector3(0, 0, 0));
 }
 
-
-void	btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const
+void btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) const
 {
 	//always use stackless
 
@@ -801,31 +763,31 @@ void	btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCa
 		reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax);
 	}
 	*/
-
 }
 
-
-void	btQuantizedBvh::swapLeafNodes(int i,int splitIndex)
+void btQuantizedBvh::swapLeafNodes(int i, int splitIndex)
 {
 	if (m_useQuantization)
 	{
-			btQuantizedBvhNode tmp = m_quantizedLeafNodes[i];
-			m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex];
-			m_quantizedLeafNodes[splitIndex] = tmp;
-	} else
+		btQuantizedBvhNode tmp = m_quantizedLeafNodes[i];
+		m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex];
+		m_quantizedLeafNodes[splitIndex] = tmp;
+	}
+	else
 	{
-			btOptimizedBvhNode tmp = m_leafNodes[i];
-			m_leafNodes[i] = m_leafNodes[splitIndex];
-			m_leafNodes[splitIndex] = tmp;
+		btOptimizedBvhNode tmp = m_leafNodes[i];
+		m_leafNodes[i] = m_leafNodes[splitIndex];
+		m_leafNodes[splitIndex] = tmp;
 	}
 }
 
-void	btQuantizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex)
+void btQuantizedBvh::assignInternalNodeFromLeafNode(int internalNode, int leafNodeIndex)
 {
 	if (m_useQuantization)
 	{
 		m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex];
-	} else
+	}
+	else
 	{
 		m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex];
 	}
@@ -842,11 +804,10 @@ static const unsigned BVH_ALIGNMENT_MASK = BVH_ALIGNMENT-1;
 static const unsigned BVH_ALIGNMENT_BLOCKS = 2;
 #endif
 
-
 unsigned int btQuantizedBvh::getAlignmentSerializationPadding()
 {
 	// I changed this to 0 since the extra padding is not needed or used.
-	return 0;//BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT;
+	return 0;  //BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT;
 }
 
 unsigned btQuantizedBvh::calculateSerializeBufferSize() const
@@ -860,12 +821,12 @@ unsigned btQuantizedBvh::calculateSerializeBufferSize() const
 	return baseSize + m_curNodeIndex * sizeof(btOptimizedBvhNode);
 }
 
-bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) const
+bool btQuantizedBvh::serialize(void* o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) const
 {
 	btAssert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
 	m_subtreeHeaderCount = m_SubtreeHeaders.size();
 
-/*	if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
+	/*	if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
 	{
 		///check alignedment for buffer?
 		btAssert(0);
@@ -873,7 +834,7 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe
 	}
 */
 
-	btQuantizedBvh *targetBvh = (btQuantizedBvh *)o_alignedDataBuffer;
+	btQuantizedBvh* targetBvh = (btQuantizedBvh*)o_alignedDataBuffer;
 
 	// construct the class so the virtual function table, etc will be set up
 	// Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor
@@ -883,10 +844,9 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe
 	{
 		targetBvh->m_curNodeIndex = static_cast<int>(btSwapEndian(m_curNodeIndex));
 
-
-		btSwapVector3Endian(m_bvhAabbMin,targetBvh->m_bvhAabbMin);
-		btSwapVector3Endian(m_bvhAabbMax,targetBvh->m_bvhAabbMax);
-		btSwapVector3Endian(m_bvhQuantization,targetBvh->m_bvhQuantization);
+		btSwapVector3Endian(m_bvhAabbMin, targetBvh->m_bvhAabbMin);
+		btSwapVector3Endian(m_bvhAabbMax, targetBvh->m_bvhAabbMax);
+		btSwapVector3Endian(m_bvhQuantization, targetBvh->m_bvhQuantization);
 
 		targetBvh->m_traversalMode = (btTraversalMode)btSwapEndian(m_traversalMode);
 		targetBvh->m_subtreeHeaderCount = static_cast<int>(btSwapEndian(m_subtreeHeaderCount));
@@ -903,12 +863,12 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe
 
 	targetBvh->m_useQuantization = m_useQuantization;
 
-	unsigned char *nodeData = (unsigned char *)targetBvh;
+	unsigned char* nodeData = (unsigned char*)targetBvh;
 	nodeData += sizeof(btQuantizedBvh);
-	
-	unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+
+	unsigned sizeToAdd = 0;  //(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
 	nodeData += sizeToAdd;
-	
+
 	int nodeCount = m_curNodeIndex;
 
 	if (m_useQuantization)
@@ -934,7 +894,6 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe
 		{
 			for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
 			{
-	
 				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0];
 				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1];
 				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2];
@@ -944,8 +903,6 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe
 				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2];
 
 				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex;
-
-
 			}
 		}
 		nodeData += sizeof(btQuantizedBvhNode) * nodeCount;
@@ -991,7 +948,7 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe
 		targetBvh->m_contiguousNodes.initializeFromBuffer(NULL, 0, 0);
 	}
 
-	sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+	sizeToAdd = 0;  //(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
 	nodeData += sizeToAdd;
 
 	// Now serialize the subtree headers
@@ -1046,14 +1003,13 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe
 	return true;
 }
 
-btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian)
+btQuantizedBvh* btQuantizedBvh::deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian)
 {
-
-	if (i_alignedDataBuffer == NULL)// || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
+	if (i_alignedDataBuffer == NULL)  // || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
 	{
 		return NULL;
 	}
-	btQuantizedBvh *bvh = (btQuantizedBvh *)i_alignedDataBuffer;
+	btQuantizedBvh* bvh = (btQuantizedBvh*)i_alignedDataBuffer;
 
 	if (i_swapEndian)
 	{
@@ -1075,12 +1031,12 @@ btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un
 		return NULL;
 	}
 
-	unsigned char *nodeData = (unsigned char *)bvh;
+	unsigned char* nodeData = (unsigned char*)bvh;
 	nodeData += sizeof(btQuantizedBvh);
-	
-	unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+
+	unsigned sizeToAdd = 0;  //(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
 	nodeData += sizeToAdd;
-	
+
 	int nodeCount = bvh->m_curNodeIndex;
 
 	// Must call placement new to fill in virtual function table, etc, but we don't want to overwrite most data, so call a special version of the constructor
@@ -1118,7 +1074,7 @@ btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un
 			{
 				btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg);
 				btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg);
-				
+
 				bvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast<int>(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_escapeIndex));
 				bvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast<int>(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_subPart));
 				bvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast<int>(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_triangleIndex));
@@ -1127,7 +1083,7 @@ btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un
 		nodeData += sizeof(btOptimizedBvhNode) * nodeCount;
 	}
 
-	sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+	sizeToAdd = 0;  //(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
 	nodeData += sizeToAdd;
 
 	// Now serialize the subtree headers
@@ -1153,13 +1109,11 @@ btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un
 }
 
 // Constructor that prevents btVector3's default constructor from being called
-btQuantizedBvh::btQuantizedBvh(btQuantizedBvh &self, bool /* ownsMemory */) :
-m_bvhAabbMin(self.m_bvhAabbMin),
-m_bvhAabbMax(self.m_bvhAabbMax),
-m_bvhQuantization(self.m_bvhQuantization),
-m_bulletVersion(BT_BULLET_VERSION)
+btQuantizedBvh::btQuantizedBvh(btQuantizedBvh& self, bool /* ownsMemory */) : m_bvhAabbMin(self.m_bvhAabbMin),
+																			  m_bvhAabbMax(self.m_bvhAabbMax),
+																			  m_bvhQuantization(self.m_bvhQuantization),
+																			  m_bulletVersion(BT_BULLET_VERSION)
 {
-
 }
 
 void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData)
@@ -1169,8 +1123,8 @@ void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedB
 	m_bvhQuantization.deSerializeFloat(quantizedBvhFloatData.m_bvhQuantization);
 
 	m_curNodeIndex = quantizedBvhFloatData.m_curNodeIndex;
-	m_useQuantization = quantizedBvhFloatData.m_useQuantization!=0;
-	
+	m_useQuantization = quantizedBvhFloatData.m_useQuantization != 0;
+
 	{
 		int numElem = quantizedBvhFloatData.m_numContiguousLeafNodes;
 		m_contiguousNodes.resize(numElem);
@@ -1179,7 +1133,7 @@ void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedB
 		{
 			btOptimizedBvhNodeFloatData* memPtr = quantizedBvhFloatData.m_contiguousNodesPtr;
 
-			for (int i=0;i<numElem;i++,memPtr++)
+			for (int i = 0; i < numElem; i++, memPtr++)
 			{
 				m_contiguousNodes[i].m_aabbMaxOrg.deSerializeFloat(memPtr->m_aabbMaxOrg);
 				m_contiguousNodes[i].m_aabbMinOrg.deSerializeFloat(memPtr->m_aabbMinOrg);
@@ -1193,11 +1147,11 @@ void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedB
 	{
 		int numElem = quantizedBvhFloatData.m_numQuantizedContiguousNodes;
 		m_quantizedContiguousNodes.resize(numElem);
-		
+
 		if (numElem)
 		{
 			btQuantizedBvhNodeData* memPtr = quantizedBvhFloatData.m_quantizedContiguousNodesPtr;
-			for (int i=0;i<numElem;i++,memPtr++)
+			for (int i = 0; i < numElem; i++, memPtr++)
 			{
 				m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex = memPtr->m_escapeIndexOrTriangleIndex;
 				m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0];
@@ -1211,16 +1165,16 @@ void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedB
 	}
 
 	m_traversalMode = btTraversalMode(quantizedBvhFloatData.m_traversalMode);
-	
+
 	{
 		int numElem = quantizedBvhFloatData.m_numSubtreeHeaders;
 		m_SubtreeHeaders.resize(numElem);
 		if (numElem)
 		{
 			btBvhSubtreeInfoData* memPtr = quantizedBvhFloatData.m_subTreeInfoPtr;
-			for (int i=0;i<numElem;i++,memPtr++)
+			for (int i = 0; i < numElem; i++, memPtr++)
 			{
-				m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0] ;
+				m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0];
 				m_SubtreeHeaders[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1];
 				m_SubtreeHeaders[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2];
 				m_SubtreeHeaders[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0];
@@ -1240,8 +1194,8 @@ void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantize
 	m_bvhQuantization.deSerializeDouble(quantizedBvhDoubleData.m_bvhQuantization);
 
 	m_curNodeIndex = quantizedBvhDoubleData.m_curNodeIndex;
-	m_useQuantization = quantizedBvhDoubleData.m_useQuantization!=0;
-	
+	m_useQuantization = quantizedBvhDoubleData.m_useQuantization != 0;
+
 	{
 		int numElem = quantizedBvhDoubleData.m_numContiguousLeafNodes;
 		m_contiguousNodes.resize(numElem);
@@ -1250,7 +1204,7 @@ void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantize
 		{
 			btOptimizedBvhNodeDoubleData* memPtr = quantizedBvhDoubleData.m_contiguousNodesPtr;
 
-			for (int i=0;i<numElem;i++,memPtr++)
+			for (int i = 0; i < numElem; i++, memPtr++)
 			{
 				m_contiguousNodes[i].m_aabbMaxOrg.deSerializeDouble(memPtr->m_aabbMaxOrg);
 				m_contiguousNodes[i].m_aabbMinOrg.deSerializeDouble(memPtr->m_aabbMinOrg);
@@ -1264,11 +1218,11 @@ void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantize
 	{
 		int numElem = quantizedBvhDoubleData.m_numQuantizedContiguousNodes;
 		m_quantizedContiguousNodes.resize(numElem);
-		
+
 		if (numElem)
 		{
 			btQuantizedBvhNodeData* memPtr = quantizedBvhDoubleData.m_quantizedContiguousNodesPtr;
-			for (int i=0;i<numElem;i++,memPtr++)
+			for (int i = 0; i < numElem; i++, memPtr++)
 			{
 				m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex = memPtr->m_escapeIndexOrTriangleIndex;
 				m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0];
@@ -1282,16 +1236,16 @@ void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantize
 	}
 
 	m_traversalMode = btTraversalMode(quantizedBvhDoubleData.m_traversalMode);
-	
+
 	{
 		int numElem = quantizedBvhDoubleData.m_numSubtreeHeaders;
 		m_SubtreeHeaders.resize(numElem);
 		if (numElem)
 		{
 			btBvhSubtreeInfoData* memPtr = quantizedBvhDoubleData.m_subTreeInfoPtr;
-			for (int i=0;i<numElem;i++,memPtr++)
+			for (int i = 0; i < numElem; i++, memPtr++)
 			{
-				m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0] ;
+				m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0];
 				m_SubtreeHeaders[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1];
 				m_SubtreeHeaders[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2];
 				m_SubtreeHeaders[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0];
@@ -1302,52 +1256,51 @@ void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantize
 			}
 		}
 	}
-
 }
 
-
-
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer) const
+const char* btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btQuantizedBvhData* quantizedData = (btQuantizedBvhData*)dataBuffer;
-	
+
 	m_bvhAabbMax.serialize(quantizedData->m_bvhAabbMax);
 	m_bvhAabbMin.serialize(quantizedData->m_bvhAabbMin);
 	m_bvhQuantization.serialize(quantizedData->m_bvhQuantization);
 
 	quantizedData->m_curNodeIndex = m_curNodeIndex;
 	quantizedData->m_useQuantization = m_useQuantization;
-	
+
 	quantizedData->m_numContiguousLeafNodes = m_contiguousNodes.size();
-	quantizedData->m_contiguousNodesPtr = (btOptimizedBvhNodeData*) (m_contiguousNodes.size() ? serializer->getUniquePointer((void*)&m_contiguousNodes[0]) : 0);
+	quantizedData->m_contiguousNodesPtr = (btOptimizedBvhNodeData*)(m_contiguousNodes.size() ? serializer->getUniquePointer((void*)&m_contiguousNodes[0]) : 0);
 	if (quantizedData->m_contiguousNodesPtr)
 	{
 		int sz = sizeof(btOptimizedBvhNodeData);
 		int numElem = m_contiguousNodes.size();
-		btChunk* chunk = serializer->allocate(sz,numElem);
+		btChunk* chunk = serializer->allocate(sz, numElem);
 		btOptimizedBvhNodeData* memPtr = (btOptimizedBvhNodeData*)chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
 			m_contiguousNodes[i].m_aabbMaxOrg.serialize(memPtr->m_aabbMaxOrg);
 			m_contiguousNodes[i].m_aabbMinOrg.serialize(memPtr->m_aabbMinOrg);
 			memPtr->m_escapeIndex = m_contiguousNodes[i].m_escapeIndex;
 			memPtr->m_subPart = m_contiguousNodes[i].m_subPart;
 			memPtr->m_triangleIndex = m_contiguousNodes[i].m_triangleIndex;
+			// Fill padding with zeros to appease msan.
+			memset(memPtr->m_pad, 0, sizeof(memPtr->m_pad));
 		}
-		serializer->finalizeChunk(chunk,"btOptimizedBvhNodeData",BT_ARRAY_CODE,(void*)&m_contiguousNodes[0]);
+		serializer->finalizeChunk(chunk, "btOptimizedBvhNodeData", BT_ARRAY_CODE, (void*)&m_contiguousNodes[0]);
 	}
 
 	quantizedData->m_numQuantizedContiguousNodes = m_quantizedContiguousNodes.size();
-//	printf("quantizedData->m_numQuantizedContiguousNodes=%d\n",quantizedData->m_numQuantizedContiguousNodes);
-	quantizedData->m_quantizedContiguousNodesPtr =(btQuantizedBvhNodeData*) (m_quantizedContiguousNodes.size() ? serializer->getUniquePointer((void*)&m_quantizedContiguousNodes[0]) : 0);
+	//	printf("quantizedData->m_numQuantizedContiguousNodes=%d\n",quantizedData->m_numQuantizedContiguousNodes);
+	quantizedData->m_quantizedContiguousNodesPtr = (btQuantizedBvhNodeData*)(m_quantizedContiguousNodes.size() ? serializer->getUniquePointer((void*)&m_quantizedContiguousNodes[0]) : 0);
 	if (quantizedData->m_quantizedContiguousNodesPtr)
 	{
 		int sz = sizeof(btQuantizedBvhNodeData);
 		int numElem = m_quantizedContiguousNodes.size();
-		btChunk* chunk = serializer->allocate(sz,numElem);
+		btChunk* chunk = serializer->allocate(sz, numElem);
 		btQuantizedBvhNodeData* memPtr = (btQuantizedBvhNodeData*)chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
 			memPtr->m_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex;
 			memPtr->m_quantizedAabbMax[0] = m_quantizedContiguousNodes[i].m_quantizedAabbMax[0];
@@ -1357,20 +1310,20 @@ const char*	btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer
 			memPtr->m_quantizedAabbMin[1] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[1];
 			memPtr->m_quantizedAabbMin[2] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[2];
 		}
-		serializer->finalizeChunk(chunk,"btQuantizedBvhNodeData",BT_ARRAY_CODE,(void*)&m_quantizedContiguousNodes[0]);
+		serializer->finalizeChunk(chunk, "btQuantizedBvhNodeData", BT_ARRAY_CODE, (void*)&m_quantizedContiguousNodes[0]);
 	}
 
 	quantizedData->m_traversalMode = int(m_traversalMode);
 	quantizedData->m_numSubtreeHeaders = m_SubtreeHeaders.size();
 
-	quantizedData->m_subTreeInfoPtr = (btBvhSubtreeInfoData*) (m_SubtreeHeaders.size() ? serializer->getUniquePointer((void*)&m_SubtreeHeaders[0]) : 0);
+	quantizedData->m_subTreeInfoPtr = (btBvhSubtreeInfoData*)(m_SubtreeHeaders.size() ? serializer->getUniquePointer((void*)&m_SubtreeHeaders[0]) : 0);
 	if (quantizedData->m_subTreeInfoPtr)
 	{
 		int sz = sizeof(btBvhSubtreeInfoData);
 		int numElem = m_SubtreeHeaders.size();
-		btChunk* chunk = serializer->allocate(sz,numElem);
+		btChunk* chunk = serializer->allocate(sz, numElem);
 		btBvhSubtreeInfoData* memPtr = (btBvhSubtreeInfoData*)chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
 			memPtr->m_quantizedAabbMax[0] = m_SubtreeHeaders[i].m_quantizedAabbMax[0];
 			memPtr->m_quantizedAabbMax[1] = m_SubtreeHeaders[i].m_quantizedAabbMax[1];
@@ -1382,12 +1335,7 @@ const char*	btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer
 			memPtr->m_rootNodeIndex = m_SubtreeHeaders[i].m_rootNodeIndex;
 			memPtr->m_subtreeSize = m_SubtreeHeaders[i].m_subtreeSize;
 		}
-		serializer->finalizeChunk(chunk,"btBvhSubtreeInfoData",BT_ARRAY_CODE,(void*)&m_SubtreeHeaders[0]);
+		serializer->finalizeChunk(chunk, "btBvhSubtreeInfoData", BT_ARRAY_CODE, (void*)&m_SubtreeHeaders[0]);
 	}
 	return btQuantizedBvhDataName;
 }
-
-
-
-
-
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h
index 78382da79f0..1c47b9ccf29 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h
@@ -22,11 +22,11 @@ class btSerializer;
 #ifdef DEBUG_CHECK_DEQUANTIZATION
 #ifdef __SPU__
 #define printf spu_printf
-#endif //__SPU__
+#endif  //__SPU__
 
 #include <stdio.h>
 #include <stdlib.h>
-#endif //DEBUG_CHECK_DEQUANTIZATION
+#endif  //DEBUG_CHECK_DEQUANTIZATION
 
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btAlignedAllocator.h"
@@ -41,13 +41,10 @@ class btSerializer;
 #define btQuantizedBvhDataName "btQuantizedBvhFloatData"
 #endif
 
-
-
 //http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp
 
-
 //Note: currently we have 16 bytes per quantized node
-#define MAX_SUBTREE_SIZE_IN_BYTES  2048
+#define MAX_SUBTREE_SIZE_IN_BYTES 2048
 
 // 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one
 // actually) triangles each (since the sign bit is reserved
@@ -55,15 +52,16 @@ class btSerializer;
 
 ///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
 ///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
-ATTRIBUTE_ALIGNED16	(struct) btQuantizedBvhNode
+ATTRIBUTE_ALIGNED16(struct)
+btQuantizedBvhNode
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	//12 bytes
-	unsigned short int	m_quantizedAabbMin[3];
-	unsigned short int	m_quantizedAabbMax[3];
+	unsigned short int m_quantizedAabbMin[3];
+	unsigned short int m_quantizedAabbMax[3];
 	//4 bytes
-	int	m_escapeIndexOrTriangleIndex;
+	int m_escapeIndexOrTriangleIndex;
 
 	bool isLeafNode() const
 	{
@@ -75,68 +73,67 @@ ATTRIBUTE_ALIGNED16	(struct) btQuantizedBvhNode
 		btAssert(!isLeafNode());
 		return -m_escapeIndexOrTriangleIndex;
 	}
-	int	getTriangleIndex() const
+	int getTriangleIndex() const
 	{
 		btAssert(isLeafNode());
-		unsigned int x=0;
-		unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);
+		unsigned int x = 0;
+		unsigned int y = (~(x & 0)) << (31 - MAX_NUM_PARTS_IN_BITS);
 		// Get only the lower bits where the triangle index is stored
-		return (m_escapeIndexOrTriangleIndex&~(y));
+		return (m_escapeIndexOrTriangleIndex & ~(y));
 	}
-	int	getPartId() const
+	int getPartId() const
 	{
 		btAssert(isLeafNode());
 		// Get only the highest bits where the part index is stored
-		return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));
+		return (m_escapeIndexOrTriangleIndex >> (31 - MAX_NUM_PARTS_IN_BITS));
 	}
-}
-;
+};
 
 /// btOptimizedBvhNode contains both internal and leaf node information.
 /// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes.
-ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode
+ATTRIBUTE_ALIGNED16(struct)
+btOptimizedBvhNode
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	//32 bytes
-	btVector3	m_aabbMinOrg;
-	btVector3	m_aabbMaxOrg;
+	btVector3 m_aabbMinOrg;
+	btVector3 m_aabbMaxOrg;
 
 	//4
-	int	m_escapeIndex;
+	int m_escapeIndex;
 
 	//8
 	//for child nodes
-	int	m_subPart;
-	int	m_triangleIndex;
+	int m_subPart;
+	int m_triangleIndex;
 
-//pad the size to 64 bytes
-	char	m_padding[20];
+	//pad the size to 64 bytes
+	char m_padding[20];
 };
 
-
 ///btBvhSubtreeInfo provides info to gather a subtree of limited size
-ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo
+ATTRIBUTE_ALIGNED16(class)
+btBvhSubtreeInfo
 {
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	//12 bytes
-	unsigned short int	m_quantizedAabbMin[3];
-	unsigned short int	m_quantizedAabbMax[3];
+	unsigned short int m_quantizedAabbMin[3];
+	unsigned short int m_quantizedAabbMax[3];
 	//4 bytes, points to the root of the subtree
-	int			m_rootNodeIndex;
+	int m_rootNodeIndex;
 	//4 bytes
-	int			m_subtreeSize;
-	int			m_padding[3];
+	int m_subtreeSize;
+	int m_padding[3];
 
 	btBvhSubtreeInfo()
 	{
 		//memset(&m_padding[0], 0, sizeof(m_padding));
 	}
 
-
-	void	setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode)
+	void setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode)
 	{
 		m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0];
 		m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1];
@@ -145,14 +142,12 @@ public:
 		m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1];
 		m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2];
 	}
-}
-;
-
+};
 
 class btNodeOverlapCallback
 {
 public:
-	virtual ~btNodeOverlapCallback() {};
+	virtual ~btNodeOverlapCallback(){};
 
 	virtual void processNode(int subPart, int triangleIndex) = 0;
 };
@@ -160,18 +155,16 @@ public:
 #include "LinearMath/btAlignedAllocator.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
-
-
 ///for code readability:
-typedef btAlignedObjectArray<btOptimizedBvhNode>	NodeArray;
-typedef btAlignedObjectArray<btQuantizedBvhNode>	QuantizedNodeArray;
-typedef btAlignedObjectArray<btBvhSubtreeInfo>		BvhSubtreeInfoArray;
-
+typedef btAlignedObjectArray<btOptimizedBvhNode> NodeArray;
+typedef btAlignedObjectArray<btQuantizedBvhNode> QuantizedNodeArray;
+typedef btAlignedObjectArray<btBvhSubtreeInfo> BvhSubtreeInfoArray;
 
 ///The btQuantizedBvh class stores an AABB tree that can be quickly traversed on CPU and Cell SPU.
-///It is used by the btBvhTriangleMeshShape as midphase, and by the btMultiSapBroadphase.
+///It is used by the btBvhTriangleMeshShape as midphase.
 ///It is recommended to use quantization for better performance and lower memory requirements.
-ATTRIBUTE_ALIGNED16(class) btQuantizedBvh
+ATTRIBUTE_ALIGNED16(class)
+btQuantizedBvh
 {
 public:
 	enum btTraversalMode
@@ -182,54 +175,47 @@ public:
 	};
 
 protected:
+	btVector3 m_bvhAabbMin;
+	btVector3 m_bvhAabbMax;
+	btVector3 m_bvhQuantization;
 
+	int m_bulletVersion;  //for serialization versioning. It could also be used to detect endianess.
 
-	btVector3			m_bvhAabbMin;
-	btVector3			m_bvhAabbMax;
-	btVector3			m_bvhQuantization;
-
-	int					m_bulletVersion;	//for serialization versioning. It could also be used to detect endianess.
-
-	int					m_curNodeIndex;
+	int m_curNodeIndex;
 	//quantization data
-	bool				m_useQuantization;
+	bool m_useQuantization;
 
+	NodeArray m_leafNodes;
+	NodeArray m_contiguousNodes;
+	QuantizedNodeArray m_quantizedLeafNodes;
+	QuantizedNodeArray m_quantizedContiguousNodes;
 
-
-	NodeArray			m_leafNodes;
-	NodeArray			m_contiguousNodes;
-	QuantizedNodeArray	m_quantizedLeafNodes;
-	QuantizedNodeArray	m_quantizedContiguousNodes;
-	
-	btTraversalMode	m_traversalMode;
-	BvhSubtreeInfoArray		m_SubtreeHeaders;
+	btTraversalMode m_traversalMode;
+	BvhSubtreeInfoArray m_SubtreeHeaders;
 
 	//This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray
 	mutable int m_subtreeHeaderCount;
 
-	
-
-
-
 	///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!)
 	///this might be refactored into a virtual, it is usually not calculated at run-time
-	void	setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin)
+	void setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin)
 	{
 		if (m_useQuantization)
 		{
-			quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0);
-		} else
+			quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0], aabbMin, 0);
+		}
+		else
 		{
 			m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin;
-
 		}
 	}
-	void	setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax)
+	void setInternalNodeAabbMax(int nodeIndex, const btVector3& aabbMax)
 	{
 		if (m_useQuantization)
 		{
-			quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1);
-		} else
+			quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0], aabbMax, 1);
+		}
+		else
 		{
 			m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax;
 		}
@@ -243,115 +229,102 @@ protected:
 		}
 		//non-quantized
 		return m_leafNodes[nodeIndex].m_aabbMinOrg;
-
 	}
 	btVector3 getAabbMax(int nodeIndex) const
 	{
 		if (m_useQuantization)
 		{
 			return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]);
-		} 
+		}
 		//non-quantized
 		return m_leafNodes[nodeIndex].m_aabbMaxOrg;
-		
 	}
 
-	
-	void	setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex)
+	void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex)
 	{
 		if (m_useQuantization)
 		{
 			m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex;
-		} 
+		}
 		else
 		{
 			m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex;
 		}
-
 	}
 
-	void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax) 
+	void mergeInternalNodeAabb(int nodeIndex, const btVector3& newAabbMin, const btVector3& newAabbMax)
 	{
 		if (m_useQuantization)
 		{
 			unsigned short int quantizedAabbMin[3];
 			unsigned short int quantizedAabbMax[3];
-			quantize(quantizedAabbMin,newAabbMin,0);
-			quantize(quantizedAabbMax,newAabbMax,1);
-			for (int i=0;i<3;i++)
+			quantize(quantizedAabbMin, newAabbMin, 0);
+			quantize(quantizedAabbMax, newAabbMax, 1);
+			for (int i = 0; i < 3; i++)
 			{
 				if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i])
 					m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i];
 
 				if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i])
 					m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i];
-
 			}
-		} else
+		}
+		else
 		{
 			//non-quantized
 			m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin);
-			m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax);		
+			m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax);
 		}
 	}
 
-	void	swapLeafNodes(int firstIndex,int secondIndex);
+	void swapLeafNodes(int firstIndex, int secondIndex);
 
-	void	assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex);
+	void assignInternalNodeFromLeafNode(int internalNode, int leafNodeIndex);
 
 protected:
+	void buildTree(int startIndex, int endIndex);
 
-	
+	int calcSplittingAxis(int startIndex, int endIndex);
 
-	void	buildTree	(int startIndex,int endIndex);
+	int sortAndCalcSplittingIndex(int startIndex, int endIndex, int splitAxis);
 
-	int	calcSplittingAxis(int startIndex,int endIndex);
+	void walkStacklessTree(btNodeOverlapCallback * nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const;
 
-	int	sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis);
-	
-	void	walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
-
-	void	walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
-	void	walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const;
-	void	walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
+	void walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback * nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const;
+	void walkStacklessQuantizedTree(btNodeOverlapCallback * nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax, int startNodeIndex, int endNodeIndex) const;
+	void walkStacklessTreeAgainstRay(btNodeOverlapCallback * nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const;
 
 	///tree traversal designed for small-memory processors like PS3 SPU
-	void	walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
+	void walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback * nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax) const;
 
 	///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
-	void	walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
+	void walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode, btNodeOverlapCallback* nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax) const;
 
 	///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
-	void	walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const;
-	
+	void walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA, const btQuantizedBvhNode* treeNodeB, btNodeOverlapCallback* nodeCallback) const;
 
-
-
-	void	updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex);
+	void updateSubtreeHeaders(int leftChildNodexIndex, int rightChildNodexIndex);
 
 public:
-	
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	btQuantizedBvh();
 
 	virtual ~btQuantizedBvh();
 
-	
 	///***************************************** expert/internal use only *************************
-	void	setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0));
-	QuantizedNodeArray&	getLeafNodeArray() {			return	m_quantizedLeafNodes;	}
+	void setQuantizationValues(const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, btScalar quantizationMargin = btScalar(1.0));
+	QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; }
 	///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized
-	void	buildInternal();
+	void buildInternal();
 	///***************************************** expert/internal use only *************************
 
-	void	reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
-	void	reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const;
-	void	reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const;
+	void reportAabbOverlappingNodex(btNodeOverlapCallback * nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const;
+	void reportRayOverlappingNodex(btNodeOverlapCallback * nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const;
+	void reportBoxCastOverlappingNodex(btNodeOverlapCallback * nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) const;
 
-		SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point,int isMax) const
+	SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point, int isMax) const
 	{
-
 		btAssert(m_useQuantization);
 
 		btAssert(point.getX() <= m_bvhAabbMax.getX());
@@ -368,122 +341,114 @@ public:
 		///@todo: double-check this
 		if (isMax)
 		{
-			out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1));
-			out[1] = (unsigned short) (((unsigned short)(v.getY()+btScalar(1.)) | 1));
-			out[2] = (unsigned short) (((unsigned short)(v.getZ()+btScalar(1.)) | 1));
-		} else
+			out[0] = (unsigned short)(((unsigned short)(v.getX() + btScalar(1.)) | 1));
+			out[1] = (unsigned short)(((unsigned short)(v.getY() + btScalar(1.)) | 1));
+			out[2] = (unsigned short)(((unsigned short)(v.getZ() + btScalar(1.)) | 1));
+		}
+		else
 		{
-			out[0] = (unsigned short) (((unsigned short)(v.getX()) & 0xfffe));
-			out[1] = (unsigned short) (((unsigned short)(v.getY()) & 0xfffe));
-			out[2] = (unsigned short) (((unsigned short)(v.getZ()) & 0xfffe));
+			out[0] = (unsigned short)(((unsigned short)(v.getX()) & 0xfffe));
+			out[1] = (unsigned short)(((unsigned short)(v.getY()) & 0xfffe));
+			out[2] = (unsigned short)(((unsigned short)(v.getZ()) & 0xfffe));
 		}
 
-
 #ifdef DEBUG_CHECK_DEQUANTIZATION
 		btVector3 newPoint = unQuantize(out);
 		if (isMax)
 		{
 			if (newPoint.getX() < point.getX())
 			{
-				printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
+				printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n", newPoint.getX() - point.getX(), newPoint.getX(), point.getX());
 			}
 			if (newPoint.getY() < point.getY())
 			{
-				printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
+				printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n", newPoint.getY() - point.getY(), newPoint.getY(), point.getY());
 			}
 			if (newPoint.getZ() < point.getZ())
 			{
-
-				printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
+				printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n", newPoint.getZ() - point.getZ(), newPoint.getZ(), point.getZ());
 			}
-		} else
+		}
+		else
 		{
 			if (newPoint.getX() > point.getX())
 			{
-				printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
+				printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n", newPoint.getX() - point.getX(), newPoint.getX(), point.getX());
 			}
 			if (newPoint.getY() > point.getY())
 			{
-				printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
+				printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n", newPoint.getY() - point.getY(), newPoint.getY(), point.getY());
 			}
 			if (newPoint.getZ() > point.getZ())
 			{
-				printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
+				printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n", newPoint.getZ() - point.getZ(), newPoint.getZ(), point.getZ());
 			}
 		}
-#endif //DEBUG_CHECK_DEQUANTIZATION
-
+#endif  //DEBUG_CHECK_DEQUANTIZATION
 	}
 
-
-	SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const
+	SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2, int isMax) const
 	{
-
 		btAssert(m_useQuantization);
 
 		btVector3 clampedPoint(point2);
 		clampedPoint.setMax(m_bvhAabbMin);
 		clampedPoint.setMin(m_bvhAabbMax);
 
-		quantize(out,clampedPoint,isMax);
-
+		quantize(out, clampedPoint, isMax);
 	}
-	
-	SIMD_FORCE_INLINE btVector3	unQuantize(const unsigned short* vecIn) const
+
+	SIMD_FORCE_INLINE btVector3 unQuantize(const unsigned short* vecIn) const
 	{
-			btVector3	vecOut;
-			vecOut.setValue(
+		btVector3 vecOut;
+		vecOut.setValue(
 			(btScalar)(vecIn[0]) / (m_bvhQuantization.getX()),
 			(btScalar)(vecIn[1]) / (m_bvhQuantization.getY()),
 			(btScalar)(vecIn[2]) / (m_bvhQuantization.getZ()));
-			vecOut += m_bvhAabbMin;
-			return vecOut;
+		vecOut += m_bvhAabbMin;
+		return vecOut;
 	}
 
 	///setTraversalMode let's you choose between stackless, recursive or stackless cache friendly tree traversal. Note this is only implemented for quantized trees.
-	void	setTraversalMode(btTraversalMode	traversalMode)
+	void setTraversalMode(btTraversalMode traversalMode)
 	{
 		m_traversalMode = traversalMode;
 	}
 
-
-	SIMD_FORCE_INLINE QuantizedNodeArray&	getQuantizedNodeArray()
-	{	
-		return	m_quantizedContiguousNodes;
+	SIMD_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray()
+	{
+		return m_quantizedContiguousNodes;
 	}
 
-
-	SIMD_FORCE_INLINE BvhSubtreeInfoArray&	getSubtreeInfoArray()
+	SIMD_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray()
 	{
 		return m_SubtreeHeaders;
 	}
 
-////////////////////////////////////////////////////////////////////
+	////////////////////////////////////////////////////////////////////
 
 	/////Calculate space needed to store BVH for serialization
 	unsigned calculateSerializeBufferSize() const;
 
 	/// Data buffer MUST be 16 byte aligned
-	virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const;
+	virtual bool serialize(void* o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const;
 
 	///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
-	static btQuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian);
+	static btQuantizedBvh* deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian);
 
 	static unsigned int getAlignmentSerializationPadding();
-//////////////////////////////////////////////////////////////////////
+	//////////////////////////////////////////////////////////////////////
 
-	
-	virtual	int	calculateSerializeBufferSizeNew() const;
+	virtual int calculateSerializeBufferSizeNew() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
-
-	virtual	void deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData);
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 
-	virtual	void deSerializeDouble(struct btQuantizedBvhDoubleData& quantizedBvhDoubleData);
+	virtual void deSerializeFloat(struct btQuantizedBvhFloatData & quantizedBvhFloatData);
 
+	virtual void deSerializeDouble(struct btQuantizedBvhDoubleData & quantizedBvhDoubleData);
 
-////////////////////////////////////////////////////////////////////
+	////////////////////////////////////////////////////////////////////
 
 	SIMD_FORCE_INLINE bool isQuantized()
 	{
@@ -494,38 +459,37 @@ private:
 	// Special "copy" constructor that allows for in-place deserialization
 	// Prevents btVector3's default constructor from being called, but doesn't inialize much else
 	// ownsMemory should most likely be false if deserializing, and if you are not, don't call this (it also changes the function signature, which we need)
-	btQuantizedBvh(btQuantizedBvh &other, bool ownsMemory);
-
-}
-;
-
+	btQuantizedBvh(btQuantizedBvh & other, bool ownsMemory);
+};
 
-struct	btBvhSubtreeInfoData
+// clang-format off
+// parser needs * with the name
+struct btBvhSubtreeInfoData
 {
-	int			m_rootNodeIndex;
-	int			m_subtreeSize;
+	int m_rootNodeIndex;
+	int m_subtreeSize;
 	unsigned short m_quantizedAabbMin[3];
 	unsigned short m_quantizedAabbMax[3];
 };
 
 struct btOptimizedBvhNodeFloatData
 {
-	btVector3FloatData	m_aabbMinOrg;
-	btVector3FloatData	m_aabbMaxOrg;
-	int	m_escapeIndex;
-	int	m_subPart;
-	int	m_triangleIndex;
+	btVector3FloatData m_aabbMinOrg;
+	btVector3FloatData m_aabbMaxOrg;
+	int m_escapeIndex;
+	int m_subPart;
+	int m_triangleIndex;
 	char m_pad[4];
 };
 
 struct btOptimizedBvhNodeDoubleData
 {
-	btVector3DoubleData	m_aabbMinOrg;
-	btVector3DoubleData	m_aabbMaxOrg;
-	int	m_escapeIndex;
-	int	m_subPart;
-	int	m_triangleIndex;
-	char	m_pad[4];
+	btVector3DoubleData m_aabbMinOrg;
+	btVector3DoubleData m_aabbMaxOrg;
+	int m_escapeIndex;
+	int m_subPart;
+	int m_triangleIndex;
+	char m_pad[4];
 };
 
 
@@ -569,13 +533,11 @@ struct	btQuantizedBvhDoubleData
 	int							m_numSubtreeHeaders;
 	btBvhSubtreeInfoData		*m_subTreeInfoPtr;
 };
+// clang-format on
 
-
-SIMD_FORCE_INLINE	int	btQuantizedBvh::calculateSerializeBufferSizeNew() const
+SIMD_FORCE_INLINE int btQuantizedBvh::calculateSerializeBufferSizeNew() const
 {
 	return sizeof(btQuantizedBvhData);
 }
 
-
-
-#endif //BT_QUANTIZED_BVH_H
+#endif  //BT_QUANTIZED_BVH_H
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
index 752fcd0fef2..b7fe0a1f341 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
@@ -24,52 +24,45 @@ subject to the following restrictions:
 
 #include <new>
 
-extern int gOverlappingPairs;
-
-void	btSimpleBroadphase::validate()
+void btSimpleBroadphase::validate()
 {
-	for (int i=0;i<m_numHandles;i++)
+	for (int i = 0; i < m_numHandles; i++)
 	{
-		for (int j=i+1;j<m_numHandles;j++)
+		for (int j = i + 1; j < m_numHandles; j++)
 		{
 			btAssert(&m_pHandles[i] != &m_pHandles[j]);
 		}
 	}
-	
 }
 
 btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache)
-	:m_pairCache(overlappingPairCache),
-	m_ownsPairCache(false),
-	m_invalidPair(0)
+	: m_pairCache(overlappingPairCache),
+	  m_ownsPairCache(false),
+	  m_invalidPair(0)
 {
-
 	if (!overlappingPairCache)
 	{
-		void* mem = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16);
-		m_pairCache = new (mem)btHashedOverlappingPairCache();
+		void* mem = btAlignedAlloc(sizeof(btHashedOverlappingPairCache), 16);
+		m_pairCache = new (mem) btHashedOverlappingPairCache();
 		m_ownsPairCache = true;
 	}
 
 	// allocate handles buffer and put all handles on free list
-	m_pHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy)*maxProxies,16);
-	m_pHandles = new(m_pHandlesRawPtr) btSimpleBroadphaseProxy[maxProxies];
+	m_pHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy) * maxProxies, 16);
+	m_pHandles = new (m_pHandlesRawPtr) btSimpleBroadphaseProxy[maxProxies];
 	m_maxHandles = maxProxies;
 	m_numHandles = 0;
 	m_firstFreeHandle = 0;
 	m_LastHandleIndex = -1;
-	
 
 	{
 		for (int i = m_firstFreeHandle; i < maxProxies; i++)
 		{
 			m_pHandles[i].SetNextFree(i + 1);
-			m_pHandles[i].m_uniqueId = i+2;//any UID will do, we just avoid too trivial values (0,1) for debugging purposes
+			m_pHandles[i].m_uniqueId = i + 2;  //any UID will do, we just avoid too trivial values (0,1) for debugging purposes
 		}
 		m_pHandles[maxProxies - 1].SetNextFree(0);
-	
 	}
-
 }
 
 btSimpleBroadphase::~btSimpleBroadphase()
@@ -83,26 +76,25 @@ btSimpleBroadphase::~btSimpleBroadphase()
 	}
 }
 
-
-btBroadphaseProxy*	btSimpleBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* /*dispatcher*/,void* multiSapProxy)
+btBroadphaseProxy* btSimpleBroadphase::createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* /*dispatcher*/)
 {
 	if (m_numHandles >= m_maxHandles)
 	{
 		btAssert(0);
-		return 0; //should never happen, but don't let the game crash ;-)
+		return 0;  //should never happen, but don't let the game crash ;-)
 	}
-	btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
+	btAssert(aabbMin[0] <= aabbMax[0] && aabbMin[1] <= aabbMax[1] && aabbMin[2] <= aabbMax[2]);
 
 	int newHandleIndex = allocHandle();
-	btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy);
+	btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex]) btSimpleBroadphaseProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask);
 
 	return proxy;
 }
 
-class	RemovingOverlapCallback : public btOverlapCallback
+class RemovingOverlapCallback : public btOverlapCallback
 {
 protected:
-	virtual bool	processOverlap(btBroadphasePair& pair)
+	virtual bool processOverlap(btBroadphasePair& pair)
 	{
 		(void)pair;
 		btAssert(0);
@@ -112,12 +104,13 @@ protected:
 
 class RemovePairContainingProxy
 {
+	btBroadphaseProxy* m_targetProxy;
 
-	btBroadphaseProxy*	m_targetProxy;
-	public:
+public:
 	virtual ~RemovePairContainingProxy()
 	{
 	}
+
 protected:
 	virtual bool processOverlap(btBroadphasePair& pair)
 	{
@@ -128,38 +121,36 @@ protected:
 	};
 };
 
-void	btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher)
+void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg, btDispatcher* dispatcher)
 {
-		
-		btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
-		freeHandle(proxy0);
+	m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg, dispatcher);
 
-		m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher);
+	btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
+	freeHandle(proxy0);
 
-		//validate();
-		
+	//validate();
 }
 
-void	btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+void btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const
 {
 	const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
 	aabbMin = sbp->m_aabbMin;
 	aabbMax = sbp->m_aabbMax;
 }
 
-void	btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
+void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
 {
 	btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
 	sbp->m_aabbMin = aabbMin;
 	sbp->m_aabbMax = aabbMax;
 }
 
-void	btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
+void btSimpleBroadphase::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin, const btVector3& aabbMax)
 {
-	for (int i=0; i <= m_LastHandleIndex; i++)
+	for (int i = 0; i <= m_LastHandleIndex; i++)
 	{
 		btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
-		if(!proxy->m_clientObject)
+		if (!proxy->m_clientObject)
 		{
 			continue;
 		}
@@ -167,69 +158,59 @@ void	btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo
 	}
 }
 
-
-void	btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
+void btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
 {
-	for (int i=0; i <= m_LastHandleIndex; i++)
+	for (int i = 0; i <= m_LastHandleIndex; i++)
 	{
 		btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
-		if(!proxy->m_clientObject)
+		if (!proxy->m_clientObject)
 		{
 			continue;
 		}
-		if (TestAabbAgainstAabb2(aabbMin,aabbMax,proxy->m_aabbMin,proxy->m_aabbMax))
+		if (TestAabbAgainstAabb2(aabbMin, aabbMax, proxy->m_aabbMin, proxy->m_aabbMax))
 		{
 			callback.process(proxy);
 		}
 	}
 }
 
-
-
-	
-
-
-
-bool	btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1)
+bool btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0, btSimpleBroadphaseProxy* proxy1)
 {
-	return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] && 
+	return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] &&
 		   proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
 		   proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
-
 }
 
-
-
 //then remove non-overlapping ones
 class CheckOverlapCallback : public btOverlapCallback
 {
 public:
 	virtual bool processOverlap(btBroadphasePair& pair)
 	{
-		return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0),static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1)));
+		return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0), static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1)));
 	}
 };
 
-void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
+void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 {
 	//first check for new overlapping pairs
-	int i,j;
+	int i, j;
 	if (m_numHandles >= 0)
 	{
 		int new_largest_index = -1;
-		for (i=0; i <= m_LastHandleIndex; i++)
+		for (i = 0; i <= m_LastHandleIndex; i++)
 		{
 			btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
-			if(!proxy0->m_clientObject)
+			if (!proxy0->m_clientObject)
 			{
 				continue;
 			}
 			new_largest_index = i;
-			for (j=i+1; j <= m_LastHandleIndex; j++)
+			for (j = i + 1; j <= m_LastHandleIndex; j++)
 			{
 				btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j];
 				btAssert(proxy0 != proxy1);
-				if(!proxy1->m_clientObject)
+				if (!proxy1->m_clientObject)
 				{
 					continue;
 				}
@@ -237,19 +218,20 @@ void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 				btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
 				btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
 
-				if (aabbOverlap(p0,p1))
+				if (aabbOverlap(p0, p1))
 				{
-					if ( !m_pairCache->findPair(proxy0,proxy1))
+					if (!m_pairCache->findPair(proxy0, proxy1))
 					{
-						m_pairCache->addOverlappingPair(proxy0,proxy1);
+						m_pairCache->addOverlappingPair(proxy0, proxy1);
 					}
-				} else
+				}
+				else
 				{
 					if (!m_pairCache->hasDeferredRemoval())
 					{
-						if ( m_pairCache->findPair(proxy0,proxy1))
+						if (m_pairCache->findPair(proxy0, proxy1))
 						{
-							m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
+							m_pairCache->removeOverlappingPair(proxy0, proxy1, dispatcher);
 						}
 					}
 				}
@@ -260,8 +242,7 @@ void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 
 		if (m_ownsPairCache && m_pairCache->hasDeferredRemoval())
 		{
-
-			btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();
+			btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray();
 
 			//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
 			overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
@@ -269,16 +250,13 @@ void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 			overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 			m_invalidPair = 0;
 
-
 			btBroadphasePair previousPair;
 			previousPair.m_pProxy0 = 0;
 			previousPair.m_pProxy1 = 0;
 			previousPair.m_algorithm = 0;
 
-
-			for (i=0;i<overlappingPairArray.size();i++)
+			for (i = 0; i < overlappingPairArray.size(); i++)
 			{
-
 				btBroadphasePair& pair = overlappingPairArray[i];
 
 				bool isDuplicate = (pair == previousPair);
@@ -289,16 +267,18 @@ void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 
 				if (!isDuplicate)
 				{
-					bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+					bool hasOverlap = testAabbOverlap(pair.m_pProxy0, pair.m_pProxy1);
 
 					if (hasOverlap)
 					{
-						needsRemoval = false;//callback->processOverlap(pair);
-					} else
+						needsRemoval = false;  //callback->processOverlap(pair);
+					}
+					else
 					{
 						needsRemoval = true;
 					}
-				} else
+				}
+				else
 				{
 					//remove duplicate
 					needsRemoval = true;
@@ -308,16 +288,14 @@ void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 
 				if (needsRemoval)
 				{
-					m_pairCache->cleanOverlappingPair(pair,dispatcher);
+					m_pairCache->cleanOverlappingPair(pair, dispatcher);
 
 					//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
 					//		m_overlappingPairArray.pop_back();
 					pair.m_pProxy0 = 0;
 					pair.m_pProxy1 = 0;
 					m_invalidPair++;
-					gOverlappingPairs--;
-				} 
-
+				}
 			}
 
 			///if you don't like to skip the invalid pairs in the array, execute following code:
@@ -329,21 +307,19 @@ void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 
 			overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 			m_invalidPair = 0;
-#endif//CLEAN_INVALID_PAIRS
-
+#endif  //CLEAN_INVALID_PAIRS
 		}
 	}
 }
 
-
-bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
 {
 	btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
 	btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
-	return aabbOverlap(p0,p1);
+	return aabbOverlap(p0, p1);
 }
 
-void	btSimpleBroadphase::resetPool(btDispatcher* dispatcher)
+void btSimpleBroadphase::resetPool(btDispatcher* dispatcher)
 {
 	//not yet
 }
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
index 7cb3c40a043..3e02fdc0036 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
@@ -16,57 +16,47 @@ subject to the following restrictions:
 #ifndef BT_SIMPLE_BROADPHASE_H
 #define BT_SIMPLE_BROADPHASE_H
 
-
 #include "btOverlappingPairCache.h"
 
-
 struct btSimpleBroadphaseProxy : public btBroadphaseProxy
 {
-	int			m_nextFree;
-	
-//	int			m_handleId;
+	int m_nextFree;
 
-	
-	btSimpleBroadphaseProxy() {};
+	//	int			m_handleId;
 
-	btSimpleBroadphaseProxy(const btVector3& minpt,const btVector3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy)
-	:btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy)
+	btSimpleBroadphaseProxy(){};
+
+	btSimpleBroadphaseProxy(const btVector3& minpt, const btVector3& maxpt, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask)
+		: btBroadphaseProxy(minpt, maxpt, userPtr, collisionFilterGroup, collisionFilterMask)
 	{
 		(void)shapeType;
 	}
-	
-	
-	SIMD_FORCE_INLINE void SetNextFree(int next) {m_nextFree = next;}
-	SIMD_FORCE_INLINE int GetNextFree() const {return m_nextFree;}
-
-	
-
 
+	SIMD_FORCE_INLINE void SetNextFree(int next) { m_nextFree = next; }
+	SIMD_FORCE_INLINE int GetNextFree() const { return m_nextFree; }
 };
 
 ///The SimpleBroadphase is just a unit-test for btAxisSweep3, bt32BitAxisSweep3, or btDbvtBroadphase, so use those classes instead.
 ///It is a brute force aabb culling broadphase based on O(n^2) aabb checks
 class btSimpleBroadphase : public btBroadphaseInterface
 {
-
 protected:
+	int m_numHandles;  // number of active handles
+	int m_maxHandles;  // max number of handles
+	int m_LastHandleIndex;
 
-	int		m_numHandles;						// number of active handles
-	int		m_maxHandles;						// max number of handles
-	int		m_LastHandleIndex;							
-	
-	btSimpleBroadphaseProxy* m_pHandles;						// handles pool
+	btSimpleBroadphaseProxy* m_pHandles;  // handles pool
 
 	void* m_pHandlesRawPtr;
-	int		m_firstFreeHandle;		// free handles list
-	
+	int m_firstFreeHandle;  // free handles list
+
 	int allocHandle()
 	{
 		btAssert(m_numHandles < m_maxHandles);
 		int freeHandle = m_firstFreeHandle;
 		m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree();
 		m_numHandles++;
-		if(freeHandle > m_LastHandleIndex)
+		if (freeHandle > m_LastHandleIndex)
 		{
 			m_LastHandleIndex = freeHandle;
 		}
@@ -75,9 +65,9 @@ protected:
 
 	void freeHandle(btSimpleBroadphaseProxy* proxy)
 	{
-		int handle = int(proxy-m_pHandles);
+		int handle = int(proxy - m_pHandles);
 		btAssert(handle >= 0 && handle < m_maxHandles);
-		if(handle == m_LastHandleIndex)
+		if (handle == m_LastHandleIndex)
 		{
 			m_LastHandleIndex--;
 		}
@@ -89,20 +79,18 @@ protected:
 		m_numHandles--;
 	}
 
-	btOverlappingPairCache*	m_pairCache;
-	bool	m_ownsPairCache;
+	btOverlappingPairCache* m_pairCache;
+	bool m_ownsPairCache;
 
-	int	m_invalidPair;
+	int m_invalidPair;
 
-	
-	
-	inline btSimpleBroadphaseProxy*	getSimpleProxyFromProxy(btBroadphaseProxy* proxy)
+	inline btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy)
 	{
 		btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxy);
 		return proxy0;
 	}
 
-	inline const btSimpleBroadphaseProxy*	getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const
+	inline const btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const
 	{
 		const btSimpleBroadphaseProxy* proxy0 = static_cast<const btSimpleBroadphaseProxy*>(proxy);
 		return proxy0;
@@ -111,61 +99,50 @@ protected:
 	///reset broadphase internal structures, to ensure determinism/reproducability
 	virtual void resetPool(btDispatcher* dispatcher);
 
-
-	void	validate();
+	void validate();
 
 protected:
-
-
-	
-
 public:
-	btSimpleBroadphase(int maxProxies=16384,btOverlappingPairCache* overlappingPairCache=0);
+	btSimpleBroadphase(int maxProxies = 16384, btOverlappingPairCache* overlappingPairCache = 0);
 	virtual ~btSimpleBroadphase();
 
+	static bool aabbOverlap(btSimpleBroadphaseProxy* proxy0, btSimpleBroadphaseProxy* proxy1);
 
-		static bool	aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1);
-
+	virtual btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher);
 
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy);
+	virtual void calculateOverlappingPairs(btDispatcher* dispatcher);
 
-	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
+	virtual void destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher);
+	virtual void setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher);
+	virtual void getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const;
 
-	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
-	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
-	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+	virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0));
+	virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
 
-	virtual void	rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0));
-	virtual void	aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
-		
-	btOverlappingPairCache*	getOverlappingPairCache()
+	btOverlappingPairCache* getOverlappingPairCache()
 	{
 		return m_pairCache;
 	}
-	const btOverlappingPairCache*	getOverlappingPairCache() const
+	const btOverlappingPairCache* getOverlappingPairCache() const
 	{
 		return m_pairCache;
 	}
 
-	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
-
+	bool testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
 
 	///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
 	///will add some transform later
-	virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
+	virtual void getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const
 	{
-		aabbMin.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
-		aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
+		aabbMin.setValue(-BT_LARGE_FLOAT, -BT_LARGE_FLOAT, -BT_LARGE_FLOAT);
+		aabbMax.setValue(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
 	}
 
-	virtual void	printStats()
+	virtual void printStats()
 	{
-//		printf("btSimpleBroadphase.h\n");
-//		printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
+		//		printf("btSimpleBroadphase.h\n");
+		//		printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
 	}
 };
 
-
-
-#endif //BT_SIMPLE_BROADPHASE_H
-
+#endif  //BT_SIMPLE_BROADPHASE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
index 63401780970..7647f673600 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
@@ -18,145 +18,162 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
 
-
-SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold)
-:m_sphere(sphere),
-m_triangle(triangle),
-m_contactBreakingThreshold(contactBreakingThreshold)
+SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere, btTriangleShape* triangle, btScalar contactBreakingThreshold)
+	: m_sphere(sphere),
+	  m_triangle(triangle),
+	  m_contactBreakingThreshold(contactBreakingThreshold)
 {
-
 }
 
-void	SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
+void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults)
 {
-
 	(void)debugDraw;
 	const btTransform& transformA = input.m_transformA;
 	const btTransform& transformB = input.m_transformB;
 
-	btVector3 point,normal;
+	btVector3 point, normal;
 	btScalar timeOfImpact = btScalar(1.);
 	btScalar depth = btScalar(0.);
-//	output.m_distance = btScalar(BT_LARGE_FLOAT);
+	//	output.m_distance = btScalar(BT_LARGE_FLOAT);
 	//move sphere into triangle space
-	btTransform	sphereInTr = transformB.inverseTimes(transformA);
+	btTransform sphereInTr = transformB.inverseTimes(transformA);
 
-	if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold))
+	if (collide(sphereInTr.getOrigin(), point, normal, depth, timeOfImpact, m_contactBreakingThreshold))
 	{
 		if (swapResults)
 		{
-			btVector3 normalOnB = transformB.getBasis()*normal;
+			btVector3 normalOnB = transformB.getBasis() * normal;
 			btVector3 normalOnA = -normalOnB;
-			btVector3 pointOnA = transformB*point+normalOnB*depth;
-			output.addContactPoint(normalOnA,pointOnA,depth);
-		} else
+			btVector3 pointOnA = transformB * point + normalOnB * depth;
+			output.addContactPoint(normalOnA, pointOnA, depth);
+		}
+		else
 		{
-			output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
+			output.addContactPoint(transformB.getBasis() * normal, transformB * point, depth);
 		}
 	}
-
 }
 
-
-
 // See also geometrictools.com
 // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
-btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest);
+btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to, const btVector3& p, btVector3& nearest);
 
-btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
+btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to, const btVector3& p, btVector3& nearest)
+{
 	btVector3 diff = p - from;
 	btVector3 v = to - from;
 	btScalar t = v.dot(diff);
-	
-	if (t > 0) {
+
+	if (t > 0)
+	{
 		btScalar dotVV = v.dot(v);
-		if (t < dotVV) {
+		if (t < dotVV)
+		{
 			t /= dotVV;
-			diff -= t*v;
-		} else {
+			diff -= t * v;
+		}
+		else
+		{
 			t = 1;
 			diff -= v;
 		}
-	} else
+	}
+	else
 		t = 0;
 
-	nearest = from + t*v;
-	return diff.dot(diff);	
+	nearest = from + t * v;
+	return diff.dot(diff);
 }
 
-bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal)  {
+bool SphereTriangleDetector::facecontains(const btVector3& p, const btVector3* vertices, btVector3& normal)
+{
 	btVector3 lp(p);
 	btVector3 lnormal(normal);
-	
+
 	return pointInTriangle(vertices, lnormal, &lp);
 }
 
-bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
+bool SphereTriangleDetector::collide(const btVector3& sphereCenter, btVector3& point, btVector3& resultNormal, btScalar& depth, btScalar& timeOfImpact, btScalar contactBreakingThreshold)
 {
-
 	const btVector3* vertices = &m_triangle->getVertexPtr(0);
-	
+
 	btScalar radius = m_sphere->getRadius();
 	btScalar radiusWithThreshold = radius + contactBreakingThreshold;
 
-	btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
-	normal.normalize();
-	btVector3 p1ToCentre = sphereCenter - vertices[0];
-	btScalar distanceFromPlane = p1ToCentre.dot(normal);
-
-	if (distanceFromPlane < btScalar(0.))
-	{
-		//triangle facing the other way
-		distanceFromPlane *= btScalar(-1.);
-		normal *= btScalar(-1.);
-	}
+	btVector3 normal = (vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]);
 
-	bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
-	
-	// Check for contact / intersection
+	btScalar l2 = normal.length2();
 	bool hasContact = false;
 	btVector3 contactPoint;
-	if (isInsideContactPlane) {
-		if (facecontains(sphereCenter,vertices,normal)) {
-			// Inside the contact wedge - touches a point on the shell plane
-			hasContact = true;
-			contactPoint = sphereCenter - normal*distanceFromPlane;
-		} else {
-			// Could be inside one of the contact capsules
-			btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
-			btVector3 nearestOnEdge;
-			for (int i = 0; i < m_triangle->getNumEdges(); i++) {
-				
-				btVector3 pa;
-				btVector3 pb;
-				
-				m_triangle->getEdge(i,pa,pb);
-
-				btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge);
-				if (distanceSqr < contactCapsuleRadiusSqr) {
-					// Yep, we're inside a capsule
-					hasContact = true;
-					contactPoint = nearestOnEdge;
+
+	if (l2 >= SIMD_EPSILON * SIMD_EPSILON)
+	{
+		normal /= btSqrt(l2);
+
+		btVector3 p1ToCentre = sphereCenter - vertices[0];
+		btScalar distanceFromPlane = p1ToCentre.dot(normal);
+
+		if (distanceFromPlane < btScalar(0.))
+		{
+			//triangle facing the other way
+			distanceFromPlane *= btScalar(-1.);
+			normal *= btScalar(-1.);
+		}
+
+		bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
+
+		// Check for contact / intersection
+
+		if (isInsideContactPlane)
+		{
+			if (facecontains(sphereCenter, vertices, normal))
+			{
+				// Inside the contact wedge - touches a point on the shell plane
+				hasContact = true;
+				contactPoint = sphereCenter - normal * distanceFromPlane;
+			}
+			else
+			{
+				// Could be inside one of the contact capsules
+				btScalar contactCapsuleRadiusSqr = radiusWithThreshold * radiusWithThreshold;
+				btScalar minDistSqr = contactCapsuleRadiusSqr;
+				btVector3 nearestOnEdge;
+				for (int i = 0; i < m_triangle->getNumEdges(); i++)
+				{
+					btVector3 pa;
+					btVector3 pb;
+
+					m_triangle->getEdge(i, pa, pb);
+
+					btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge);
+					if (distanceSqr < minDistSqr)
+					{
+						// Yep, we're inside a capsule, and record the capsule with smallest distance
+						minDistSqr = distanceSqr;
+						hasContact = true;
+						contactPoint = nearestOnEdge;
+					}
 				}
-				
 			}
 		}
 	}
 
-	if (hasContact) {
+	if (hasContact)
+	{
 		btVector3 contactToCentre = sphereCenter - contactPoint;
 		btScalar distanceSqr = contactToCentre.length2();
 
-		if (distanceSqr < radiusWithThreshold*radiusWithThreshold)
+		if (distanceSqr < radiusWithThreshold * radiusWithThreshold)
 		{
-			if (distanceSqr>SIMD_EPSILON)
+			if (distanceSqr > SIMD_EPSILON)
 			{
 				btScalar distance = btSqrt(distanceSqr);
 				resultNormal = contactToCentre;
 				resultNormal.normalize();
 				point = contactPoint;
-				depth = -(radius-distance);
-			} else
+				depth = -(radius - distance);
+			}
+			else
 			{
 				resultNormal = normal;
 				point = contactPoint;
@@ -165,36 +182,34 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
 			return true;
 		}
 	}
-	
+
 	return false;
 }
 
-
-bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
+bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3& normal, btVector3* p)
 {
 	const btVector3* p1 = &vertices[0];
 	const btVector3* p2 = &vertices[1];
 	const btVector3* p3 = &vertices[2];
 
-	btVector3 edge1( *p2 - *p1 );
-	btVector3 edge2( *p3 - *p2 );
-	btVector3 edge3( *p1 - *p3 );
+	btVector3 edge1(*p2 - *p1);
+	btVector3 edge2(*p3 - *p2);
+	btVector3 edge3(*p1 - *p3);
 
-	btVector3 p1_to_p( *p - *p1 );
-	btVector3 p2_to_p( *p - *p2 );
-	btVector3 p3_to_p( *p - *p3 );
+	btVector3 p1_to_p(*p - *p1);
+	btVector3 p2_to_p(*p - *p2);
+	btVector3 p3_to_p(*p - *p3);
+
+	btVector3 edge1_normal(edge1.cross(normal));
+	btVector3 edge2_normal(edge2.cross(normal));
+	btVector3 edge3_normal(edge3.cross(normal));
 
-	btVector3 edge1_normal( edge1.cross(normal));
-	btVector3 edge2_normal( edge2.cross(normal));
-	btVector3 edge3_normal( edge3.cross(normal));
-	
 	btScalar r1, r2, r3;
-	r1 = edge1_normal.dot( p1_to_p );
-	r2 = edge2_normal.dot( p2_to_p );
-	r3 = edge3_normal.dot( p3_to_p );
-	if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
-	     ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
+	r1 = edge1_normal.dot(p1_to_p);
+	r2 = edge2_normal.dot(p2_to_p);
+	r3 = edge3_normal.dot(p3_to_p);
+	if ((r1 > 0 && r2 > 0 && r3 > 0) ||
+		(r1 <= 0 && r2 <= 0 && r3 <= 0))
 		return true;
 	return false;
-
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
index 22953af43fd..d47e47530d4 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
@@ -18,34 +18,26 @@ subject to the following restrictions:
 
 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
 
-
-
 class btSphereShape;
 class btTriangleShape;
 
-
-
 /// sphere-triangle to match the btDiscreteCollisionDetectorInterface
 struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface
 {
-	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
+	virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false);
 
-	SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle, btScalar contactBreakingThreshold);
+	SphereTriangleDetector(btSphereShape* sphere, btTriangleShape* triangle, btScalar contactBreakingThreshold);
 
-	virtual ~SphereTriangleDetector() {};
+	virtual ~SphereTriangleDetector(){};
 
-	bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar	contactBreakingThreshold);
+	bool collide(const btVector3& sphereCenter, btVector3& point, btVector3& resultNormal, btScalar& depth, btScalar& timeOfImpact, btScalar contactBreakingThreshold);
 
 private:
-
-	
-	bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p );
-	bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal);
+	bool pointInTriangle(const btVector3 vertices[], const btVector3& normal, btVector3* p);
+	bool facecontains(const btVector3& p, const btVector3* vertices, btVector3& normal);
 
 	btSphereShape* m_sphere;
 	btTriangleShape* m_triangle;
-	btScalar	m_contactBreakingThreshold;
-	
+	btScalar m_contactBreakingThreshold;
 };
-#endif //BT_SPHERE_TRIANGLE_DETECTOR_H
-
+#endif  //BT_SPHERE_TRIANGLE_DETECTOR_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp
index 57f14649353..ac5de45d271 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp
@@ -17,31 +17,31 @@ subject to the following restrictions:
 #include "btCollisionDispatcher.h"
 #include "btCollisionObject.h"
 
-btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci)
-:btCollisionAlgorithm(ci)
+btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+	: btCollisionAlgorithm(ci)
 //,
 //m_colObj0(0),
 //m_colObj1(0)
 {
 }
-btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* )
-:btCollisionAlgorithm(ci)
+btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper*, const btCollisionObjectWrapper*)
+	: btCollisionAlgorithm(ci)
 //,
 //m_colObj0(0),
 //m_colObj1(0)
 {
-//	if (ci.m_dispatcher1->needsCollision(colObj0,colObj1))
-//	{
-//		m_colObj0 = colObj0;
-//		m_colObj1 = colObj1;
-//		
-//		m_colObj0->activate();
-//		m_colObj1->activate();
-//	}
+	//	if (ci.m_dispatcher1->needsCollision(colObj0,colObj1))
+	//	{
+	//		m_colObj0 = colObj0;
+	//		m_colObj1 = colObj1;
+	//
+	//		m_colObj0->activate();
+	//		m_colObj1->activate();
+	//	}
 }
 
 btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm()
 {
-//		m_colObj0->activate();
-//		m_colObj1->activate();
+	//		m_colObj0->activate();
+	//		m_colObj1->activate();
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h
index 489812b9663..862060571b5 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h
@@ -21,16 +21,15 @@ subject to the following restrictions:
 ///This class is not enabled yet (work-in-progress) to more aggressively activate objects.
 class btActivatingCollisionAlgorithm : public btCollisionAlgorithm
 {
-//	btCollisionObject* m_colObj0;
-//	btCollisionObject* m_colObj1;
+	//	btCollisionObject* m_colObj0;
+	//	btCollisionObject* m_colObj1;
 
-public:
-
-	btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci);
+protected:
+	btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci);
 
-	btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+	btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap);
 
+public:
 	virtual ~btActivatingCollisionAlgorithm();
-
 };
-#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H
+#endif  //__BT_ACTIVATING_COLLISION_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp
index c1da9f36cfb..6873a95d90a 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp
@@ -26,61 +26,55 @@ subject to the following restrictions:
 
 #define USE_PERSISTENT_CONTACTS 1
 
-btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap)
-: btActivatingCollisionAlgorithm(ci,obj0Wrap,obj1Wrap),
-m_ownManifold(false),
-m_manifoldPtr(mf)
+btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* obj0Wrap, const btCollisionObjectWrapper* obj1Wrap)
+	: btActivatingCollisionAlgorithm(ci, obj0Wrap, obj1Wrap),
+	  m_ownManifold(false),
+	  m_manifoldPtr(mf)
 {
-	if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject()))
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(), obj1Wrap->getCollisionObject()))
 	{
-		m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject());
+		m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(), obj1Wrap->getCollisionObject());
 		m_ownManifold = true;
 	}
 }
 
 btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm()
 {
-	
 	if (m_ownManifold)
 	{
 		if (m_manifoldPtr)
 			m_dispatcher->releaseManifold(m_manifoldPtr);
 	}
-	
 }
 
-
-void b2CollidePolygons(btManifoldResult* manifold,  const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB);
+void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB);
 
 //#include <stdio.h>
-void btBox2dBox2dCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btBox2dBox2dCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	if (!m_manifoldPtr)
 		return;
 
-	
 	const btBox2dShape* box0 = (const btBox2dShape*)body0Wrap->getCollisionShape();
 	const btBox2dShape* box1 = (const btBox2dShape*)body1Wrap->getCollisionShape();
 
 	resultOut->setPersistentManifold(m_manifoldPtr);
 
-	b2CollidePolygons(resultOut,box0,body0Wrap->getWorldTransform(),box1,body1Wrap->getWorldTransform());
+	b2CollidePolygons(resultOut, box0, body0Wrap->getWorldTransform(), box1, body1Wrap->getWorldTransform());
 
 	//  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
 	if (m_ownManifold)
 	{
 		resultOut->refreshContactPoints();
 	}
-
 }
 
-btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
+btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/, btCollisionObject* /*body1*/, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/)
 {
 	//not yet
 	return 1.f;
 }
 
-
 struct ClipVertex
 {
 	btVector3 v;
@@ -89,16 +83,16 @@ struct ClipVertex
 	//b2ContactID id;
 };
 
-#define b2Dot(a,b) (a).dot(b)
-#define b2Mul(a,b) (a)*(b)
-#define b2MulT(a,b) (a).transpose()*(b)
-#define b2Cross(a,b) (a).cross(b)
-#define btCrossS(a,s) btVector3(s * a.getY(), -s * a.getX(),0.f)
+#define b2Dot(a, b) (a).dot(b)
+#define b2Mul(a, b) (a) * (b)
+#define b2MulT(a, b) (a).transpose() * (b)
+#define b2Cross(a, b) (a).cross(b)
+#define btCrossS(a, s) btVector3(s* a.getY(), -s* a.getX(), 0.f)
 
-int b2_maxManifoldPoints =2;
+int b2_maxManifoldPoints = 2;
 
 static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
-					  const btVector3& normal, btScalar offset)
+							 const btVector3& normal, btScalar offset)
 {
 	// Start with no output points
 	int numOut = 0;
@@ -133,7 +127,7 @@ static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
 
 // Find the separation between poly1 and poly2 for a give edge normal on poly1.
 static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1,
-							  const btBox2dShape* poly2, const btTransform& xf2)
+							   const btBox2dShape* poly2, const btTransform& xf2)
 {
 	const btVector3* vertices1 = poly1->getVertices();
 	const btVector3* normals1 = poly1->getNormals();
@@ -151,8 +145,8 @@ static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1
 	int index = 0;
 	btScalar minDot = BT_LARGE_FLOAT;
 
-	if( count2 > 0 )
-		index = (int) normal1.minDot( vertices2, count2, minDot);
+	if (count2 > 0)
+		index = (int)normal1.minDot(vertices2, count2, minDot);
 
 	btVector3 v1 = b2Mul(xf1, vertices1[edge1]);
 	btVector3 v2 = b2Mul(xf2, vertices2[index]);
@@ -162,8 +156,8 @@ static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1
 
 // Find the max separation between poly1 and poly2 using edge normals from poly1.
 static btScalar FindMaxSeparation(int* edgeIndex,
-								 const btBox2dShape* poly1, const btTransform& xf1,
-								 const btBox2dShape* poly2, const btTransform& xf2)
+								  const btBox2dShape* poly1, const btTransform& xf1,
+								  const btBox2dShape* poly2, const btTransform& xf2)
 {
 	int count1 = poly1->getVertexCount();
 	const btVector3* normals1 = poly1->getNormals();
@@ -175,8 +169,8 @@ static btScalar FindMaxSeparation(int* edgeIndex,
 	// Find edge normal on poly1 that has the largest projection onto d.
 	int edge = 0;
 	btScalar maxDot;
-	if( count1 > 0 )
-		edge = (int) dLocal1.maxDot( normals1, count1, maxDot);
+	if (count1 > 0)
+		edge = (int)dLocal1.maxDot(normals1, count1, maxDot);
 
 	// Get the separation for the edge normal.
 	btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
@@ -224,7 +218,7 @@ static btScalar FindMaxSeparation(int* edgeIndex,
 	}
 
 	// Perform a local search for the best edge normal.
-	for ( ; ; )
+	for (;;)
 	{
 		if (increment == -1)
 			edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
@@ -285,14 +279,14 @@ static void FindIncidentEdge(ClipVertex c[2],
 	int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
 
 	c[0].v = b2Mul(xf2, vertices2[i1]);
-//	c[0].id.features.referenceEdge = (unsigned char)edge1;
-//	c[0].id.features.incidentEdge = (unsigned char)i1;
-//	c[0].id.features.incidentVertex = 0;
+	//	c[0].id.features.referenceEdge = (unsigned char)edge1;
+	//	c[0].id.features.incidentEdge = (unsigned char)i1;
+	//	c[0].id.features.incidentVertex = 0;
 
 	c[1].v = b2Mul(xf2, vertices2[i2]);
-//	c[1].id.features.referenceEdge = (unsigned char)edge1;
-//	c[1].id.features.incidentEdge = (unsigned char)i2;
-//	c[1].id.features.incidentVertex = 1;
+	//	c[1].id.features.referenceEdge = (unsigned char)edge1;
+	//	c[1].id.features.incidentEdge = (unsigned char)i2;
+	//	c[1].id.features.incidentVertex = 1;
 }
 
 // Find edge normal of max separation on A - return if separating axis is found
@@ -303,10 +297,9 @@ static void FindIncidentEdge(ClipVertex c[2],
 
 // The normal points from 1 to 2
 void b2CollidePolygons(btManifoldResult* manifold,
-					  const btBox2dShape* polyA, const btTransform& xfA,
-					  const btBox2dShape* polyB, const btTransform& xfB)
+					   const btBox2dShape* polyA, const btTransform& xfA,
+					   const btBox2dShape* polyB, const btTransform& xfB)
 {
-
 	int edgeA = 0;
 	btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
 	if (separationA > 0.0f)
@@ -317,10 +310,10 @@ void b2CollidePolygons(btManifoldResult* manifold,
 	if (separationB > 0.0f)
 		return;
 
-	const btBox2dShape* poly1;	// reference poly
-	const btBox2dShape* poly2;	// incident poly
+	const btBox2dShape* poly1;  // reference poly
+	const btBox2dShape* poly2;  // incident poly
 	btTransform xf1, xf2;
-	int edge1;		// reference edge
+	int edge1;  // reference edge
 	unsigned char flip;
 	const btScalar k_relativeTol = 0.98f;
 	const btScalar k_absoluteTol = 0.001f;
@@ -352,14 +345,13 @@ void b2CollidePolygons(btManifoldResult* manifold,
 	const btVector3* vertices1 = poly1->getVertices();
 
 	btVector3 v11 = vertices1[edge1];
-	btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
+	btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
 
 	//btVector3 dv = v12 - v11;
 	btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11);
 	sideNormal.normalize();
 	btVector3 frontNormal = btCrossS(sideNormal, 1.0f);
-	
-	
+
 	v11 = b2Mul(xf1, v11);
 	v12 = b2Mul(xf1, v12);
 
@@ -369,13 +361,12 @@ void b2CollidePolygons(btManifoldResult* manifold,
 
 	// Clip incident edge against extruded edge1 side edges.
 	ClipVertex clipPoints1[2];
-	clipPoints1[0].v.setValue(0,0,0);
-	clipPoints1[1].v.setValue(0,0,0);
+	clipPoints1[0].v.setValue(0, 0, 0);
+	clipPoints1[1].v.setValue(0, 0, 0);
 
 	ClipVertex clipPoints2[2];
-	clipPoints2[0].v.setValue(0,0,0);
-	clipPoints2[1].v.setValue(0,0,0);
-
+	clipPoints2[0].v.setValue(0, 0, 0);
+	clipPoints2[1].v.setValue(0, 0, 0);
 
 	int np;
 
@@ -386,7 +377,7 @@ void b2CollidePolygons(btManifoldResult* manifold,
 		return;
 
 	// Clip to negative box side 1
-	np = ClipSegmentToLine(clipPoints2, clipPoints1,  sideNormal, sideOffset2);
+	np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
 
 	if (np < 2)
 	{
@@ -403,19 +394,18 @@ void b2CollidePolygons(btManifoldResult* manifold,
 
 		if (separation <= 0.0f)
 		{
-			
 			//b2ManifoldPoint* cp = manifold->points + pointCount;
 			//btScalar separation = separation;
 			//cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
 			//cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
 
-			manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation);
+			manifold->addContactPoint(-manifoldNormal, clipPoints2[i].v, separation);
 
-//			cp->id = clipPoints2[i].id;
-//			cp->id.features.flip = flip;
+			//			cp->id = clipPoints2[i].id;
+			//			cp->id.features.flip = flip;
 			++pointCount;
 		}
 	}
 
-//	manifold->pointCount = pointCount;}
+	//	manifold->pointCount = pointCount;}
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h
index 6ea6e89bda6..3b66d1fd0b9 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h
@@ -26,22 +26,22 @@ class btPersistentManifold;
 ///box-box collision detection
 class btBox2dBox2dCollisionAlgorithm : public btActivatingCollisionAlgorithm
 {
-	bool	m_ownManifold;
-	btPersistentManifold*	m_manifoldPtr;
-	
+	bool m_ownManifold;
+	btPersistentManifold* m_manifoldPtr;
+
 public:
 	btBox2dBox2dCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
 		: btActivatingCollisionAlgorithm(ci) {}
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+	btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap);
 
 	virtual ~btBox2dBox2dCollisionAlgorithm();
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
 	{
 		if (m_manifoldPtr && m_ownManifold)
 		{
@@ -49,18 +49,15 @@ public:
 		}
 	}
 
-
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			int bbsize = sizeof(btBox2dBox2dCollisionAlgorithm);
 			void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
-			return new(ptr) btBox2dBox2dCollisionAlgorithm(0,ci,body0Wrap,body1Wrap);
+			return new (ptr) btBox2dBox2dCollisionAlgorithm(0, ci, body0Wrap, body1Wrap);
 		}
 	};
-
 };
 
-#endif //BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H
-
+#endif  //BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
index ac68968f590..7a391e059a7 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
@@ -21,14 +21,14 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 #define USE_PERSISTENT_CONTACTS 1
 
-btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
-: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
-m_ownManifold(false),
-m_manifoldPtr(mf)
+btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
+	: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
+	  m_ownManifold(false),
+	  m_manifoldPtr(mf)
 {
-	if (!m_manifoldPtr && m_dispatcher->needsCollision(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()))
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()))
 	{
-		m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
 		m_ownManifold = true;
 	}
 }
@@ -42,30 +42,27 @@ btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm()
 	}
 }
 
-void btBoxBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btBoxBoxCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	if (!m_manifoldPtr)
 		return;
 
-	
 	const btBoxShape* box0 = (btBoxShape*)body0Wrap->getCollisionShape();
 	const btBoxShape* box1 = (btBoxShape*)body1Wrap->getCollisionShape();
 
-
-
 	/// report a contact. internally this will be kept persistent, and contact reduction is done
 	resultOut->setPersistentManifold(m_manifoldPtr);
-#ifndef USE_PERSISTENT_CONTACTS	
+#ifndef USE_PERSISTENT_CONTACTS
 	m_manifoldPtr->clearManifold();
-#endif //USE_PERSISTENT_CONTACTS
+#endif  //USE_PERSISTENT_CONTACTS
 
 	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
 	input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
 	input.m_transformA = body0Wrap->getWorldTransform();
 	input.m_transformB = body1Wrap->getWorldTransform();
 
-	btBoxBoxDetector detector(box0,box1);
-	detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+	btBoxBoxDetector detector(box0, box1);
+	detector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw);
 
 #ifdef USE_PERSISTENT_CONTACTS
 	//  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
@@ -73,11 +70,10 @@ void btBoxBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrappe
 	{
 		resultOut->refreshContactPoints();
 	}
-#endif //USE_PERSISTENT_CONTACTS
-
+#endif  //USE_PERSISTENT_CONTACTS
 }
 
-btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
+btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/, btCollisionObject* /*body1*/, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/)
 {
 	//not yet
 	return 1.f;
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h
index 59808df5a9d..eb210657659 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h
@@ -26,22 +26,22 @@ class btPersistentManifold;
 ///box-box collision detection
 class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
 {
-	bool	m_ownManifold;
-	btPersistentManifold*	m_manifoldPtr;
-	
+	bool m_ownManifold;
+	btPersistentManifold* m_manifoldPtr;
+
 public:
 	btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
 		: btActivatingCollisionAlgorithm(ci) {}
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+	btBoxBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap);
 
 	virtual ~btBoxBoxCollisionAlgorithm();
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
 	{
 		if (m_manifoldPtr && m_ownManifold)
 		{
@@ -49,18 +49,15 @@ public:
 		}
 	}
 
-
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			int bbsize = sizeof(btBoxBoxCollisionAlgorithm);
 			void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
-			return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap);
+			return new (ptr) btBoxBoxCollisionAlgorithm(0, ci, body0Wrap, body1Wrap);
 		}
 	};
-
 };
 
-#endif //BT_BOX_BOX__COLLISION_ALGORITHM_H
-
+#endif  //BT_BOX_BOX__COLLISION_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
index 7043bde34f5..202039956e7 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
@@ -24,14 +24,12 @@ subject to the following restrictions:
 #include <float.h>
 #include <string.h>
 
-btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2)
-: m_box1(box1),
-m_box2(box2)
+btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1, const btBoxShape* box2)
+	: m_box1(box1),
+	  m_box2(box2)
 {
-
 }
 
-
 // given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
 // generate contact points. this returns 0 if there is no contact otherwise
 // it returns the number of contacts generated.
@@ -48,67 +46,66 @@ m_box2(box2)
 // collision functions. this function only fills in the position and depth
 // fields.
 struct dContactGeom;
-#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
+#define dDOTpq(a, b, p, q) ((a)[0] * (b)[0] + (a)[p] * (b)[q] + (a)[2 * (p)] * (b)[2 * (q)])
 #define dInfinity FLT_MAX
 
-
 /*PURE_INLINE btScalar dDOT   (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
 PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); }
 PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); }
 PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); }
 */
-static btScalar dDOT   (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
-static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); }
-static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); }
-static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); }
-#define dMULTIPLYOP1_331(A,op,B,C) \
-{\
-  (A)[0] op dDOT41((B),(C)); \
-  (A)[1] op dDOT41((B+1),(C)); \
-  (A)[2] op dDOT41((B+2),(C)); \
-}
+static btScalar dDOT(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 1, 1); }
+static btScalar dDOT44(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 4, 4); }
+static btScalar dDOT41(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 4, 1); }
+static btScalar dDOT14(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 1, 4); }
+#define dMULTIPLYOP1_331(A, op, B, C)   \
+	{                                   \
+		(A)[0] op dDOT41((B), (C));     \
+		(A)[1] op dDOT41((B + 1), (C)); \
+		(A)[2] op dDOT41((B + 2), (C)); \
+	}
 
-#define dMULTIPLYOP0_331(A,op,B,C) \
-{ \
-  (A)[0] op dDOT((B),(C)); \
-  (A)[1] op dDOT((B+4),(C)); \
-  (A)[2] op dDOT((B+8),(C)); \
-} 
+#define dMULTIPLYOP0_331(A, op, B, C) \
+	{                                 \
+		(A)[0] op dDOT((B), (C));     \
+		(A)[1] op dDOT((B + 4), (C)); \
+		(A)[2] op dDOT((B + 8), (C)); \
+	}
 
-#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C)
-#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)
+#define dMULTIPLY1_331(A, B, C) dMULTIPLYOP1_331(A, =, B, C)
+#define dMULTIPLY0_331(A, B, C) dMULTIPLYOP0_331(A, =, B, C)
 
-typedef btScalar dMatrix3[4*3];
+typedef btScalar dMatrix3[4 * 3];
 
-void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
-			   const btVector3& pb, const btVector3& ub,
-			   btScalar *alpha, btScalar *beta);
-void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
-			   const btVector3& pb, const btVector3& ub,
-			   btScalar *alpha, btScalar *beta)
+void dLineClosestApproach(const btVector3& pa, const btVector3& ua,
+						  const btVector3& pb, const btVector3& ub,
+						  btScalar* alpha, btScalar* beta);
+void dLineClosestApproach(const btVector3& pa, const btVector3& ua,
+						  const btVector3& pb, const btVector3& ub,
+						  btScalar* alpha, btScalar* beta)
 {
-  btVector3 p;
-  p[0] = pb[0] - pa[0];
-  p[1] = pb[1] - pa[1];
-  p[2] = pb[2] - pa[2];
-  btScalar uaub = dDOT(ua,ub);
-  btScalar q1 =  dDOT(ua,p);
-  btScalar q2 = -dDOT(ub,p);
-  btScalar d = 1-uaub*uaub;
-  if (d <= btScalar(0.0001f)) {
-    // @@@ this needs to be made more robust
-    *alpha = 0;
-    *beta  = 0;
-  }
-  else {
-    d = 1.f/d;
-    *alpha = (q1 + uaub*q2)*d;
-    *beta  = (uaub*q1 + q2)*d;
-  }
+	btVector3 p;
+	p[0] = pb[0] - pa[0];
+	p[1] = pb[1] - pa[1];
+	p[2] = pb[2] - pa[2];
+	btScalar uaub = dDOT(ua, ub);
+	btScalar q1 = dDOT(ua, p);
+	btScalar q2 = -dDOT(ub, p);
+	btScalar d = 1 - uaub * uaub;
+	if (d <= btScalar(0.0001f))
+	{
+		// @@@ this needs to be made more robust
+		*alpha = 0;
+		*beta = 0;
+	}
+	else
+	{
+		d = 1.f / d;
+		*alpha = (q1 + uaub * q2) * d;
+		*beta = (uaub * q1 + q2) * d;
+	}
 }
 
-
-
 // find all the intersection points between the 2D rectangle with vertices
 // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
 // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
@@ -117,60 +114,66 @@ void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
 // the number of intersection points is returned by the function (this will
 // be in the range 0 to 8).
 
-static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16])
+static int intersectRectQuad2(btScalar h[2], btScalar p[8], btScalar ret[16])
 {
-  // q (and r) contain nq (and nr) coordinate points for the current (and
-  // chopped) polygons
-  int nq=4,nr=0;
-  btScalar buffer[16];
-  btScalar *q = p;
-  btScalar *r = ret;
-  for (int dir=0; dir <= 1; dir++) {
-    // direction notation: xy[0] = x axis, xy[1] = y axis
-    for (int sign=-1; sign <= 1; sign += 2) {
-      // chop q along the line xy[dir] = sign*h[dir]
-      btScalar *pq = q;
-      btScalar *pr = r;
-      nr = 0;
-      for (int i=nq; i > 0; i--) {
-	// go through all points in q and all lines between adjacent points
-	if (sign*pq[dir] < h[dir]) {
-	  // this point is inside the chopping line
-	  pr[0] = pq[0];
-	  pr[1] = pq[1];
-	  pr += 2;
-	  nr++;
-	  if (nr & 8) {
-	    q = r;
-	    goto done;
-	  }
-	}
-	btScalar *nextq = (i > 1) ? pq+2 : q;
-	if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) {
-	  // this line crosses the chopping line
-	  pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
-	    (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
-	  pr[dir] = sign*h[dir];
-	  pr += 2;
-	  nr++;
-	  if (nr & 8) {
-	    q = r;
-	    goto done;
-	  }
+	// q (and r) contain nq (and nr) coordinate points for the current (and
+	// chopped) polygons
+	int nq = 4, nr = 0;
+	btScalar buffer[16];
+	btScalar* q = p;
+	btScalar* r = ret;
+	for (int dir = 0; dir <= 1; dir++)
+	{
+		// direction notation: xy[0] = x axis, xy[1] = y axis
+		for (int sign = -1; sign <= 1; sign += 2)
+		{
+			// chop q along the line xy[dir] = sign*h[dir]
+			btScalar* pq = q;
+			btScalar* pr = r;
+			nr = 0;
+			for (int i = nq; i > 0; i--)
+			{
+				// go through all points in q and all lines between adjacent points
+				if (sign * pq[dir] < h[dir])
+				{
+					// this point is inside the chopping line
+					pr[0] = pq[0];
+					pr[1] = pq[1];
+					pr += 2;
+					nr++;
+					if (nr & 8)
+					{
+						q = r;
+						goto done;
+					}
+				}
+				btScalar* nextq = (i > 1) ? pq + 2 : q;
+				if ((sign * pq[dir] < h[dir]) ^ (sign * nextq[dir] < h[dir]))
+				{
+					// this line crosses the chopping line
+					pr[1 - dir] = pq[1 - dir] + (nextq[1 - dir] - pq[1 - dir]) /
+													(nextq[dir] - pq[dir]) * (sign * h[dir] - pq[dir]);
+					pr[dir] = sign * h[dir];
+					pr += 2;
+					nr++;
+					if (nr & 8)
+					{
+						q = r;
+						goto done;
+					}
+				}
+				pq += 2;
+			}
+			q = r;
+			r = (q == ret) ? buffer : ret;
+			nq = nr;
+		}
 	}
-	pq += 2;
-      }
-      q = r;
-      r = (q==ret) ? buffer : ret;
-      nq = nr;
-    }
-  }
- done:
-  if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar));
-  return nr;
+done:
+	if (q != ret) memcpy(ret, q, nr * 2 * sizeof(btScalar));
+	return nr;
 }
 
-
 #define M__PI 3.14159265f
 
 // given n points in the plane (array p, of size 2*n), generate m points that
@@ -181,538 +184,584 @@ static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16])
 // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
 // in the range [0..n-1].
 
-void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]);
-void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[])
+void cullPoints2(int n, btScalar p[], int m, int i0, int iret[]);
+void cullPoints2(int n, btScalar p[], int m, int i0, int iret[])
 {
-  // compute the centroid of the polygon in cx,cy
-  int i,j;
-  btScalar a,cx,cy,q;
-  if (n==1) {
-    cx = p[0];
-    cy = p[1];
-  }
-  else if (n==2) {
-    cx = btScalar(0.5)*(p[0] + p[2]);
-    cy = btScalar(0.5)*(p[1] + p[3]);
-  }
-  else {
-    a = 0;
-    cx = 0;
-    cy = 0;
-    for (i=0; i<(n-1); i++) {
-      q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
-      a += q;
-      cx += q*(p[i*2]+p[i*2+2]);
-      cy += q*(p[i*2+1]+p[i*2+3]);
-    }
-    q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
-	if (btFabs(a+q) > SIMD_EPSILON)
+	// compute the centroid of the polygon in cx,cy
+	int i, j;
+	btScalar a, cx, cy, q;
+	if (n == 1)
 	{
-		a = 1.f/(btScalar(3.0)*(a+q));
-	} else
+		cx = p[0];
+		cy = p[1];
+	}
+	else if (n == 2)
 	{
-		a=BT_LARGE_FLOAT;
+		cx = btScalar(0.5) * (p[0] + p[2]);
+		cy = btScalar(0.5) * (p[1] + p[3]);
 	}
-    cx = a*(cx + q*(p[n*2-2]+p[0]));
-    cy = a*(cy + q*(p[n*2-1]+p[1]));
-  }
-
-  // compute the angle of each point w.r.t. the centroid
-  btScalar A[8];
-  for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx);
-
-  // search for points that have angles closest to A[i0] + i*(2*pi/m).
-  int avail[8];
-  for (i=0; i<n; i++) avail[i] = 1;
-  avail[i0] = 0;
-  iret[0] = i0;
-  iret++;
-  for (j=1; j<m; j++) {
-    a = btScalar(j)*(2*M__PI/m) + A[i0];
-    if (a > M__PI) a -= 2*M__PI;
-    btScalar maxdiff=1e9,diff;
-
-    *iret = i0;			// iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
-
-    for (i=0; i<n; i++) {
-      if (avail[i]) {
-	diff = btFabs (A[i]-a);
-	if (diff > M__PI) diff = 2*M__PI - diff;
-	if (diff < maxdiff) {
-	  maxdiff = diff;
-	  *iret = i;
+	else
+	{
+		a = 0;
+		cx = 0;
+		cy = 0;
+		for (i = 0; i < (n - 1); i++)
+		{
+			q = p[i * 2] * p[i * 2 + 3] - p[i * 2 + 2] * p[i * 2 + 1];
+			a += q;
+			cx += q * (p[i * 2] + p[i * 2 + 2]);
+			cy += q * (p[i * 2 + 1] + p[i * 2 + 3]);
+		}
+		q = p[n * 2 - 2] * p[1] - p[0] * p[n * 2 - 1];
+		if (btFabs(a + q) > SIMD_EPSILON)
+		{
+			a = 1.f / (btScalar(3.0) * (a + q));
+		}
+		else
+		{
+			a = BT_LARGE_FLOAT;
+		}
+		cx = a * (cx + q * (p[n * 2 - 2] + p[0]));
+		cy = a * (cy + q * (p[n * 2 - 1] + p[1]));
 	}
-      }
-    }
-#if defined(DEBUG) || defined (_DEBUG)
-    btAssert (*iret != i0);	// ensure iret got set
+
+	// compute the angle of each point w.r.t. the centroid
+	btScalar A[8];
+	for (i = 0; i < n; i++) A[i] = btAtan2(p[i * 2 + 1] - cy, p[i * 2] - cx);
+
+	// search for points that have angles closest to A[i0] + i*(2*pi/m).
+	int avail[8];
+	for (i = 0; i < n; i++) avail[i] = 1;
+	avail[i0] = 0;
+	iret[0] = i0;
+	iret++;
+	for (j = 1; j < m; j++)
+	{
+		a = btScalar(j) * (2 * M__PI / m) + A[i0];
+		if (a > M__PI) a -= 2 * M__PI;
+		btScalar maxdiff = 1e9, diff;
+
+		*iret = i0;  // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
+
+		for (i = 0; i < n; i++)
+		{
+			if (avail[i])
+			{
+				diff = btFabs(A[i] - a);
+				if (diff > M__PI) diff = 2 * M__PI - diff;
+				if (diff < maxdiff)
+				{
+					maxdiff = diff;
+					*iret = i;
+				}
+			}
+		}
+#if defined(DEBUG) || defined(_DEBUG)
+		btAssert(*iret != i0);  // ensure iret got set
 #endif
-    avail[*iret] = 0;
-    iret++;
-  }
+		avail[*iret] = 0;
+		iret++;
+	}
 }
 
+int dBoxBox2(const btVector3& p1, const dMatrix3 R1,
+			 const btVector3& side1, const btVector3& p2,
+			 const dMatrix3 R2, const btVector3& side2,
+			 btVector3& normal, btScalar* depth, int* return_code,
+			 int maxc, dContactGeom* /*contact*/, int /*skip*/, btDiscreteCollisionDetectorInterface::Result& output);
+int dBoxBox2(const btVector3& p1, const dMatrix3 R1,
+			 const btVector3& side1, const btVector3& p2,
+			 const dMatrix3 R2, const btVector3& side2,
+			 btVector3& normal, btScalar* depth, int* return_code,
+			 int maxc, dContactGeom* /*contact*/, int /*skip*/, btDiscreteCollisionDetectorInterface::Result& output)
+{
+	const btScalar fudge_factor = btScalar(1.05);
+	btVector3 p, pp, normalC(0.f, 0.f, 0.f);
+	const btScalar* normalR = 0;
+	btScalar A[3], B[3], R11, R12, R13, R21, R22, R23, R31, R32, R33,
+		Q11, Q12, Q13, Q21, Q22, Q23, Q31, Q32, Q33, s, s2, l;
+	int i, j, invert_normal, code;
+
+	// get vector from centers of box 1 to box 2, relative to box 1
+	p = p2 - p1;
+	dMULTIPLY1_331(pp, R1, p);  // get pp = p relative to body 1
+
+	// get side lengths / 2
+	A[0] = side1[0] * btScalar(0.5);
+	A[1] = side1[1] * btScalar(0.5);
+	A[2] = side1[2] * btScalar(0.5);
+	B[0] = side2[0] * btScalar(0.5);
+	B[1] = side2[1] * btScalar(0.5);
+	B[2] = side2[2] * btScalar(0.5);
+
+	// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
+	R11 = dDOT44(R1 + 0, R2 + 0);
+	R12 = dDOT44(R1 + 0, R2 + 1);
+	R13 = dDOT44(R1 + 0, R2 + 2);
+	R21 = dDOT44(R1 + 1, R2 + 0);
+	R22 = dDOT44(R1 + 1, R2 + 1);
+	R23 = dDOT44(R1 + 1, R2 + 2);
+	R31 = dDOT44(R1 + 2, R2 + 0);
+	R32 = dDOT44(R1 + 2, R2 + 1);
+	R33 = dDOT44(R1 + 2, R2 + 2);
+
+	Q11 = btFabs(R11);
+	Q12 = btFabs(R12);
+	Q13 = btFabs(R13);
+	Q21 = btFabs(R21);
+	Q22 = btFabs(R22);
+	Q23 = btFabs(R23);
+	Q31 = btFabs(R31);
+	Q32 = btFabs(R32);
+	Q33 = btFabs(R33);
+
+	// for all 15 possible separating axes:
+	//   * see if the axis separates the boxes. if so, return 0.
+	//   * find the depth of the penetration along the separating axis (s2)
+	//   * if this is the largest depth so far, record it.
+	// the normal vector will be set to the separating axis with the smallest
+	// depth. note: normalR is set to point to a column of R1 or R2 if that is
+	// the smallest depth normal so far. otherwise normalR is 0 and normalC is
+	// set to a vector relative to body 1. invert_normal is 1 if the sign of
+	// the normal should be flipped.
+
+#define TST(expr1, expr2, norm, cc)    \
+	s2 = btFabs(expr1) - (expr2);      \
+	if (s2 > 0) return 0;              \
+	if (s2 > s)                        \
+	{                                  \
+		s = s2;                        \
+		normalR = norm;                \
+		invert_normal = ((expr1) < 0); \
+		code = (cc);                   \
+	}
 
+	s = -dInfinity;
+	invert_normal = 0;
+	code = 0;
 
-int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
-	     const btVector3& side1, const btVector3& p2,
-	     const dMatrix3 R2, const btVector3& side2,
-	     btVector3& normal, btScalar *depth, int *return_code,
-		 int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output);
-int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
-	     const btVector3& side1, const btVector3& p2,
-	     const dMatrix3 R2, const btVector3& side2,
-	     btVector3& normal, btScalar *depth, int *return_code,
-		 int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output)
-{
-  const btScalar fudge_factor = btScalar(1.05);
-  btVector3 p,pp,normalC(0.f,0.f,0.f);
-  const btScalar *normalR = 0;
-  btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
-    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
-  int i,j,invert_normal,code;
-
-  // get vector from centers of box 1 to box 2, relative to box 1
-  p = p2 - p1;
-  dMULTIPLY1_331 (pp,R1,p);		// get pp = p relative to body 1
-
-  // get side lengths / 2
-  A[0] = side1[0]*btScalar(0.5);
-  A[1] = side1[1]*btScalar(0.5);
-  A[2] = side1[2]*btScalar(0.5);
-  B[0] = side2[0]*btScalar(0.5);
-  B[1] = side2[1]*btScalar(0.5);
-  B[2] = side2[2]*btScalar(0.5);
-
-  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
-  R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
-  R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
-  R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
-
-  Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13);
-  Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23);
-  Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33);
-
-  // for all 15 possible separating axes:
-  //   * see if the axis separates the boxes. if so, return 0.
-  //   * find the depth of the penetration along the separating axis (s2)
-  //   * if this is the largest depth so far, record it.
-  // the normal vector will be set to the separating axis with the smallest
-  // depth. note: normalR is set to point to a column of R1 or R2 if that is
-  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
-  // set to a vector relative to body 1. invert_normal is 1 if the sign of
-  // the normal should be flipped.
-
-#define TST(expr1,expr2,norm,cc) \
-  s2 = btFabs(expr1) - (expr2); \
-  if (s2 > 0) return 0; \
-  if (s2 > s) { \
-    s = s2; \
-    normalR = norm; \
-    invert_normal = ((expr1) < 0); \
-    code = (cc); \
-  }
-
-  s = -dInfinity;
-  invert_normal = 0;
-  code = 0;
-
-  // separating axis = u1,u2,u3
-  TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
-  TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
-  TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
-
-  // separating axis = v1,v2,v3
-  TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
-  TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
-  TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
-
-  // note: cross product axes need to be scaled when s is computed.
-  // normal (n1,n2,n3) is relative to box 1.
+	// separating axis = u1,u2,u3
+	TST(pp[0], (A[0] + B[0] * Q11 + B[1] * Q12 + B[2] * Q13), R1 + 0, 1);
+	TST(pp[1], (A[1] + B[0] * Q21 + B[1] * Q22 + B[2] * Q23), R1 + 1, 2);
+	TST(pp[2], (A[2] + B[0] * Q31 + B[1] * Q32 + B[2] * Q33), R1 + 2, 3);
+
+	// separating axis = v1,v2,v3
+	TST(dDOT41(R2 + 0, p), (A[0] * Q11 + A[1] * Q21 + A[2] * Q31 + B[0]), R2 + 0, 4);
+	TST(dDOT41(R2 + 1, p), (A[0] * Q12 + A[1] * Q22 + A[2] * Q32 + B[1]), R2 + 1, 5);
+	TST(dDOT41(R2 + 2, p), (A[0] * Q13 + A[1] * Q23 + A[2] * Q33 + B[2]), R2 + 2, 6);
+
+	// note: cross product axes need to be scaled when s is computed.
+	// normal (n1,n2,n3) is relative to box 1.
 #undef TST
-#define TST(expr1,expr2,n1,n2,n3,cc) \
-  s2 = btFabs(expr1) - (expr2); \
-  if (s2 > SIMD_EPSILON) return 0; \
-  l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
-  if (l > SIMD_EPSILON) { \
-    s2 /= l; \
-    if (s2*fudge_factor > s) { \
-      s = s2; \
-      normalR = 0; \
-      normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
-      invert_normal = ((expr1) < 0); \
-      code = (cc); \
-    } \
-  }
-
-  btScalar fudge2 (1.0e-5f);
-
-  Q11 += fudge2;
-  Q12 += fudge2;
-  Q13 += fudge2;
-
-  Q21 += fudge2;
-  Q22 += fudge2;
-  Q23 += fudge2;
-
-  Q31 += fudge2;
-  Q32 += fudge2;
-  Q33 += fudge2;
-
-  // separating axis = u1 x (v1,v2,v3)
-  TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
-  TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
-  TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
-
-  // separating axis = u2 x (v1,v2,v3)
-  TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
-  TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
-  TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
-
-  // separating axis = u3 x (v1,v2,v3)
-  TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
-  TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
-  TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
+#define TST(expr1, expr2, n1, n2, n3, cc)                \
+	s2 = btFabs(expr1) - (expr2);                        \
+	if (s2 > SIMD_EPSILON) return 0;                     \
+	l = btSqrt((n1) * (n1) + (n2) * (n2) + (n3) * (n3)); \
+	if (l > SIMD_EPSILON)                                \
+	{                                                    \
+		s2 /= l;                                         \
+		if (s2 * fudge_factor > s)                       \
+		{                                                \
+			s = s2;                                      \
+			normalR = 0;                                 \
+			normalC[0] = (n1) / l;                       \
+			normalC[1] = (n2) / l;                       \
+			normalC[2] = (n3) / l;                       \
+			invert_normal = ((expr1) < 0);               \
+			code = (cc);                                 \
+		}                                                \
+	}
+
+	btScalar fudge2(1.0e-5f);
+
+	Q11 += fudge2;
+	Q12 += fudge2;
+	Q13 += fudge2;
+
+	Q21 += fudge2;
+	Q22 += fudge2;
+	Q23 += fudge2;
+
+	Q31 += fudge2;
+	Q32 += fudge2;
+	Q33 += fudge2;
+
+	// separating axis = u1 x (v1,v2,v3)
+	TST(pp[2] * R21 - pp[1] * R31, (A[1] * Q31 + A[2] * Q21 + B[1] * Q13 + B[2] * Q12), 0, -R31, R21, 7);
+	TST(pp[2] * R22 - pp[1] * R32, (A[1] * Q32 + A[2] * Q22 + B[0] * Q13 + B[2] * Q11), 0, -R32, R22, 8);
+	TST(pp[2] * R23 - pp[1] * R33, (A[1] * Q33 + A[2] * Q23 + B[0] * Q12 + B[1] * Q11), 0, -R33, R23, 9);
+
+	// separating axis = u2 x (v1,v2,v3)
+	TST(pp[0] * R31 - pp[2] * R11, (A[0] * Q31 + A[2] * Q11 + B[1] * Q23 + B[2] * Q22), R31, 0, -R11, 10);
+	TST(pp[0] * R32 - pp[2] * R12, (A[0] * Q32 + A[2] * Q12 + B[0] * Q23 + B[2] * Q21), R32, 0, -R12, 11);
+	TST(pp[0] * R33 - pp[2] * R13, (A[0] * Q33 + A[2] * Q13 + B[0] * Q22 + B[1] * Q21), R33, 0, -R13, 12);
+
+	// separating axis = u3 x (v1,v2,v3)
+	TST(pp[1] * R11 - pp[0] * R21, (A[0] * Q21 + A[1] * Q11 + B[1] * Q33 + B[2] * Q32), -R21, R11, 0, 13);
+	TST(pp[1] * R12 - pp[0] * R22, (A[0] * Q22 + A[1] * Q12 + B[0] * Q33 + B[2] * Q31), -R22, R12, 0, 14);
+	TST(pp[1] * R13 - pp[0] * R23, (A[0] * Q23 + A[1] * Q13 + B[0] * Q32 + B[1] * Q31), -R23, R13, 0, 15);
 
 #undef TST
 
-  if (!code) return 0;
-
-  // if we get to this point, the boxes interpenetrate. compute the normal
-  // in global coordinates.
-  if (normalR) {
-    normal[0] = normalR[0];
-    normal[1] = normalR[4];
-    normal[2] = normalR[8];
-  }
-  else {
-    dMULTIPLY0_331 (normal,R1,normalC);
-  }
-  if (invert_normal) {
-    normal[0] = -normal[0];
-    normal[1] = -normal[1];
-    normal[2] = -normal[2];
-  }
-  *depth = -s;
-
-  // compute contact point(s)
-
-  if (code > 6) {
-    // an edge from box 1 touches an edge from box 2.
-    // find a point pa on the intersecting edge of box 1
-    btVector3 pa;
-    btScalar sign;
-    for (i=0; i<3; i++) pa[i] = p1[i];
-    for (j=0; j<3; j++) {
-      sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0);
-      for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
-    }
-
-    // find a point pb on the intersecting edge of box 2
-    btVector3 pb;
-    for (i=0; i<3; i++) pb[i] = p2[i];
-    for (j=0; j<3; j++) {
-      sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0);
-      for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
-    }
-
-    btScalar alpha,beta;
-    btVector3 ua,ub;
-    for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
-    for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
-
-    dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
-    for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
-    for (i=0; i<3; i++) pb[i] += ub[i]*beta;
+	if (!code) return 0;
 
+	// if we get to this point, the boxes interpenetrate. compute the normal
+	// in global coordinates.
+	if (normalR)
+	{
+		normal[0] = normalR[0];
+		normal[1] = normalR[4];
+		normal[2] = normalR[8];
+	}
+	else
 	{
-		
-		//contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
-		//contact[0].depth = *depth;
-		btVector3 pointInWorld;
+		dMULTIPLY0_331(normal, R1, normalC);
+	}
+	if (invert_normal)
+	{
+		normal[0] = -normal[0];
+		normal[1] = -normal[1];
+		normal[2] = -normal[2];
+	}
+	*depth = -s;
+
+	// compute contact point(s)
+
+	if (code > 6)
+	{
+		// an edge from box 1 touches an edge from box 2.
+		// find a point pa on the intersecting edge of box 1
+		btVector3 pa;
+		btScalar sign;
+		for (i = 0; i < 3; i++) pa[i] = p1[i];
+		for (j = 0; j < 3; j++)
+		{
+			sign = (dDOT14(normal, R1 + j) > 0) ? btScalar(1.0) : btScalar(-1.0);
+			for (i = 0; i < 3; i++) pa[i] += sign * A[j] * R1[i * 4 + j];
+		}
+
+		// find a point pb on the intersecting edge of box 2
+		btVector3 pb;
+		for (i = 0; i < 3; i++) pb[i] = p2[i];
+		for (j = 0; j < 3; j++)
+		{
+			sign = (dDOT14(normal, R2 + j) > 0) ? btScalar(-1.0) : btScalar(1.0);
+			for (i = 0; i < 3; i++) pb[i] += sign * B[j] * R2[i * 4 + j];
+		}
+
+		btScalar alpha, beta;
+		btVector3 ua, ub;
+		for (i = 0; i < 3; i++) ua[i] = R1[((code)-7) / 3 + i * 4];
+		for (i = 0; i < 3; i++) ub[i] = R2[((code)-7) % 3 + i * 4];
+
+		dLineClosestApproach(pa, ua, pb, ub, &alpha, &beta);
+		for (i = 0; i < 3; i++) pa[i] += ua[i] * alpha;
+		for (i = 0; i < 3; i++) pb[i] += ub[i] * beta;
+
+		{
+			//contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
+			//contact[0].depth = *depth;
+			btVector3 pointInWorld;
 
 #ifdef USE_CENTER_POINT
-	    for (i=0; i<3; i++) 
-			pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5);
-		output.addContactPoint(-normal,pointInWorld,-*depth);
+			for (i = 0; i < 3; i++)
+				pointInWorld[i] = (pa[i] + pb[i]) * btScalar(0.5);
+			output.addContactPoint(-normal, pointInWorld, -*depth);
 #else
-		output.addContactPoint(-normal,pb,-*depth);
+			output.addContactPoint(-normal, pb, -*depth);
 
-#endif //
-		*return_code = code;
+#endif  //
+			*return_code = code;
+		}
+		return 1;
+	}
+
+	// okay, we have a face-something intersection (because the separating
+	// axis is perpendicular to a face). define face 'a' to be the reference
+	// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
+	// the incident face (the closest face of the other box).
+
+	const btScalar *Ra, *Rb, *pa, *pb, *Sa, *Sb;
+	if (code <= 3)
+	{
+		Ra = R1;
+		Rb = R2;
+		pa = p1;
+		pb = p2;
+		Sa = A;
+		Sb = B;
+	}
+	else
+	{
+		Ra = R2;
+		Rb = R1;
+		pa = p2;
+		pb = p1;
+		Sa = B;
+		Sb = A;
+	}
+
+	// nr = normal vector of reference face dotted with axes of incident box.
+	// anr = absolute values of nr.
+	btVector3 normal2, nr, anr;
+	if (code <= 3)
+	{
+		normal2[0] = normal[0];
+		normal2[1] = normal[1];
+		normal2[2] = normal[2];
+	}
+	else
+	{
+		normal2[0] = -normal[0];
+		normal2[1] = -normal[1];
+		normal2[2] = -normal[2];
 	}
-    return 1;
-  }
-
-  // okay, we have a face-something intersection (because the separating
-  // axis is perpendicular to a face). define face 'a' to be the reference
-  // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
-  // the incident face (the closest face of the other box).
-
-  const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb;
-  if (code <= 3) {
-    Ra = R1;
-    Rb = R2;
-    pa = p1;
-    pb = p2;
-    Sa = A;
-    Sb = B;
-  }
-  else {
-    Ra = R2;
-    Rb = R1;
-    pa = p2;
-    pb = p1;
-    Sa = B;
-    Sb = A;
-  }
-
-  // nr = normal vector of reference face dotted with axes of incident box.
-  // anr = absolute values of nr.
-  btVector3 normal2,nr,anr;
-  if (code <= 3) {
-    normal2[0] = normal[0];
-    normal2[1] = normal[1];
-    normal2[2] = normal[2];
-  }
-  else {
-    normal2[0] = -normal[0];
-    normal2[1] = -normal[1];
-    normal2[2] = -normal[2];
-  }
-  dMULTIPLY1_331 (nr,Rb,normal2);
-  anr[0] = btFabs (nr[0]);
-  anr[1] = btFabs (nr[1]);
-  anr[2] = btFabs (nr[2]);
-
-  // find the largest compontent of anr: this corresponds to the normal
-  // for the indident face. the other axis numbers of the indicent face
-  // are stored in a1,a2.
-  int lanr,a1,a2;
-  if (anr[1] > anr[0]) {
-    if (anr[1] > anr[2]) {
-      a1 = 0;
-      lanr = 1;
-      a2 = 2;
-    }
-    else {
-      a1 = 0;
-      a2 = 1;
-      lanr = 2;
-    }
-  }
-  else {
-    if (anr[0] > anr[2]) {
-      lanr = 0;
-      a1 = 1;
-      a2 = 2;
-    }
-    else {
-      a1 = 0;
-      a2 = 1;
-      lanr = 2;
-    }
-  }
-
-  // compute center point of incident face, in reference-face coordinates
-  btVector3 center;
-  if (nr[lanr] < 0) {
-    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
-  }
-  else {
-    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
-  }
-
-  // find the normal and non-normal axis numbers of the reference box
-  int codeN,code1,code2;
-  if (code <= 3) codeN = code-1; else codeN = code-4;
-  if (codeN==0) {
-    code1 = 1;
-    code2 = 2;
-  }
-  else if (codeN==1) {
-    code1 = 0;
-    code2 = 2;
-  }
-  else {
-    code1 = 0;
-    code2 = 1;
-  }
-
-  // find the four corners of the incident face, in reference-face coordinates
-  btScalar quad[8];	// 2D coordinate of incident face (x,y pairs)
-  btScalar c1,c2,m11,m12,m21,m22;
-  c1 = dDOT14 (center,Ra+code1);
-  c2 = dDOT14 (center,Ra+code2);
-  // optimize this? - we have already computed this data above, but it is not
-  // stored in an easy-to-index format. for now it's quicker just to recompute
-  // the four dot products.
-  m11 = dDOT44 (Ra+code1,Rb+a1);
-  m12 = dDOT44 (Ra+code1,Rb+a2);
-  m21 = dDOT44 (Ra+code2,Rb+a1);
-  m22 = dDOT44 (Ra+code2,Rb+a2);
-  {
-    btScalar k1 = m11*Sb[a1];
-    btScalar k2 = m21*Sb[a1];
-    btScalar k3 = m12*Sb[a2];
-    btScalar k4 = m22*Sb[a2];
-    quad[0] = c1 - k1 - k3;
-    quad[1] = c2 - k2 - k4;
-    quad[2] = c1 - k1 + k3;
-    quad[3] = c2 - k2 + k4;
-    quad[4] = c1 + k1 + k3;
-    quad[5] = c2 + k2 + k4;
-    quad[6] = c1 + k1 - k3;
-    quad[7] = c2 + k2 - k4;
-  }
-
-  // find the size of the reference face
-  btScalar rect[2];
-  rect[0] = Sa[code1];
-  rect[1] = Sa[code2];
-
-  // intersect the incident and reference faces
-  btScalar ret[16];
-  int n = intersectRectQuad2 (rect,quad,ret);
-  if (n < 1) return 0;		// this should never happen
-
-  // convert the intersection points into reference-face coordinates,
-  // and compute the contact position and depth for each point. only keep
-  // those points that have a positive (penetrating) depth. delete points in
-  // the 'ret' array as necessary so that 'point' and 'ret' correspond.
-  btScalar point[3*8];		// penetrating contact points
-  btScalar dep[8];			// depths for those points
-  btScalar det1 = 1.f/(m11*m22 - m12*m21);
-  m11 *= det1;
-  m12 *= det1;
-  m21 *= det1;
-  m22 *= det1;
-  int cnum = 0;			// number of penetrating contact points found
-  for (j=0; j < n; j++) {
-    btScalar k1 =  m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
-    btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
-    for (i=0; i<3; i++) point[cnum*3+i] =
-			  center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
-    dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
-    if (dep[cnum] >= 0) {
-      ret[cnum*2] = ret[j*2];
-      ret[cnum*2+1] = ret[j*2+1];
-      cnum++;
-    }
-  }
-  if (cnum < 1) return 0;	// this should never happen
-
-  // we can't generate more contacts than we actually have
-  if (maxc > cnum) maxc = cnum;
-  if (maxc < 1) maxc = 1;
-
-  if (cnum <= maxc) {
-
-	  if (code<4) 
-	  {
-    // we have less contacts than we need, so we use them all
-    for (j=0; j < cnum; j++) 
+	dMULTIPLY1_331(nr, Rb, normal2);
+	anr[0] = btFabs(nr[0]);
+	anr[1] = btFabs(nr[1]);
+	anr[2] = btFabs(nr[2]);
+
+	// find the largest compontent of anr: this corresponds to the normal
+	// for the indident face. the other axis numbers of the indicent face
+	// are stored in a1,a2.
+	int lanr, a1, a2;
+	if (anr[1] > anr[0])
 	{
-		btVector3 pointInWorld;
-		for (i=0; i<3; i++) 
-			pointInWorld[i] = point[j*3+i] + pa[i];
-		output.addContactPoint(-normal,pointInWorld,-dep[j]);
-
-    }
-	  } else
-	  {
-		  // we have less contacts than we need, so we use them all
-		for (j=0; j < cnum; j++) 
+		if (anr[1] > anr[2])
 		{
-			btVector3 pointInWorld;
-			for (i=0; i<3; i++) 
-				pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j];
+			a1 = 0;
+			lanr = 1;
+			a2 = 2;
+		}
+		else
+		{
+			a1 = 0;
+			a2 = 1;
+			lanr = 2;
+		}
+	}
+	else
+	{
+		if (anr[0] > anr[2])
+		{
+			lanr = 0;
+			a1 = 1;
+			a2 = 2;
+		}
+		else
+		{
+			a1 = 0;
+			a2 = 1;
+			lanr = 2;
+		}
+	}
+
+	// compute center point of incident face, in reference-face coordinates
+	btVector3 center;
+	if (nr[lanr] < 0)
+	{
+		for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i * 4 + lanr];
+	}
+	else
+	{
+		for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i * 4 + lanr];
+	}
+
+	// find the normal and non-normal axis numbers of the reference box
+	int codeN, code1, code2;
+	if (code <= 3)
+		codeN = code - 1;
+	else
+		codeN = code - 4;
+	if (codeN == 0)
+	{
+		code1 = 1;
+		code2 = 2;
+	}
+	else if (codeN == 1)
+	{
+		code1 = 0;
+		code2 = 2;
+	}
+	else
+	{
+		code1 = 0;
+		code2 = 1;
+	}
+
+	// find the four corners of the incident face, in reference-face coordinates
+	btScalar quad[8];  // 2D coordinate of incident face (x,y pairs)
+	btScalar c1, c2, m11, m12, m21, m22;
+	c1 = dDOT14(center, Ra + code1);
+	c2 = dDOT14(center, Ra + code2);
+	// optimize this? - we have already computed this data above, but it is not
+	// stored in an easy-to-index format. for now it's quicker just to recompute
+	// the four dot products.
+	m11 = dDOT44(Ra + code1, Rb + a1);
+	m12 = dDOT44(Ra + code1, Rb + a2);
+	m21 = dDOT44(Ra + code2, Rb + a1);
+	m22 = dDOT44(Ra + code2, Rb + a2);
+	{
+		btScalar k1 = m11 * Sb[a1];
+		btScalar k2 = m21 * Sb[a1];
+		btScalar k3 = m12 * Sb[a2];
+		btScalar k4 = m22 * Sb[a2];
+		quad[0] = c1 - k1 - k3;
+		quad[1] = c2 - k2 - k4;
+		quad[2] = c1 - k1 + k3;
+		quad[3] = c2 - k2 + k4;
+		quad[4] = c1 + k1 + k3;
+		quad[5] = c2 + k2 + k4;
+		quad[6] = c1 + k1 - k3;
+		quad[7] = c2 + k2 - k4;
+	}
+
+	// find the size of the reference face
+	btScalar rect[2];
+	rect[0] = Sa[code1];
+	rect[1] = Sa[code2];
+
+	// intersect the incident and reference faces
+	btScalar ret[16];
+	int n = intersectRectQuad2(rect, quad, ret);
+	if (n < 1) return 0;  // this should never happen
+
+	// convert the intersection points into reference-face coordinates,
+	// and compute the contact position and depth for each point. only keep
+	// those points that have a positive (penetrating) depth. delete points in
+	// the 'ret' array as necessary so that 'point' and 'ret' correspond.
+	btScalar point[3 * 8];  // penetrating contact points
+	btScalar dep[8];        // depths for those points
+	btScalar det1 = 1.f / (m11 * m22 - m12 * m21);
+	m11 *= det1;
+	m12 *= det1;
+	m21 *= det1;
+	m22 *= det1;
+	int cnum = 0;  // number of penetrating contact points found
+	for (j = 0; j < n; j++)
+	{
+		btScalar k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
+		btScalar k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
+		for (i = 0; i < 3; i++) point[cnum * 3 + i] =
+									center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
+		dep[cnum] = Sa[codeN] - dDOT(normal2, point + cnum * 3);
+		if (dep[cnum] >= 0)
+		{
+			ret[cnum * 2] = ret[j * 2];
+			ret[cnum * 2 + 1] = ret[j * 2 + 1];
+			cnum++;
+		}
+	}
+	if (cnum < 1) return 0;  // this should never happen
+
+	// we can't generate more contacts than we actually have
+	if (maxc > cnum) maxc = cnum;
+	if (maxc < 1) maxc = 1;
+
+	if (cnum <= maxc)
+	{
+		if (code < 4)
+		{
+			// we have less contacts than we need, so we use them all
+			for (j = 0; j < cnum; j++)
+			{
+				btVector3 pointInWorld;
+				for (i = 0; i < 3; i++)
+					pointInWorld[i] = point[j * 3 + i] + pa[i];
+				output.addContactPoint(-normal, pointInWorld, -dep[j]);
+			}
+		}
+		else
+		{
+			// we have less contacts than we need, so we use them all
+			for (j = 0; j < cnum; j++)
+			{
+				btVector3 pointInWorld;
+				for (i = 0; i < 3; i++)
+					pointInWorld[i] = point[j * 3 + i] + pa[i] - normal[i] * dep[j];
 				//pointInWorld[i] = point[j*3+i] + pa[i];
-			output.addContactPoint(-normal,pointInWorld,-dep[j]);
+				output.addContactPoint(-normal, pointInWorld, -dep[j]);
+			}
 		}
-	  }
-  }
-  else {
-    // we have more contacts than are wanted, some of them must be culled.
-    // find the deepest point, it is always the first contact.
-    int i1 = 0;
-    btScalar maxdepth = dep[0];
-    for (i=1; i<cnum; i++) {
-      if (dep[i] > maxdepth) {
-	maxdepth = dep[i];
-	i1 = i;
-      }
-    }
-
-    int iret[8];
-    cullPoints2 (cnum,ret,maxc,i1,iret);
-
-    for (j=0; j < maxc; j++) {
-//      dContactGeom *con = CONTACT(contact,skip*j);
-  //    for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
-    //  con->depth = dep[iret[j]];
-
-		btVector3 posInWorld;
-		for (i=0; i<3; i++) 
-			posInWorld[i] = point[iret[j]*3+i] + pa[i];
-		if (code<4) 
-	   {
-			output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
-		} else
+	}
+	else
+	{
+		// we have more contacts than are wanted, some of them must be culled.
+		// find the deepest point, it is always the first contact.
+		int i1 = 0;
+		btScalar maxdepth = dep[0];
+		for (i = 1; i < cnum; i++)
 		{
-			output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]);
+			if (dep[i] > maxdepth)
+			{
+				maxdepth = dep[i];
+				i1 = i;
+			}
 		}
-    }
-    cnum = maxc;
-  }
 
-  *return_code = code;
-  return cnum;
+		int iret[8];
+		cullPoints2(cnum, ret, maxc, i1, iret);
+
+		for (j = 0; j < maxc; j++)
+		{
+			//      dContactGeom *con = CONTACT(contact,skip*j);
+			//    for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
+			//  con->depth = dep[iret[j]];
+
+			btVector3 posInWorld;
+			for (i = 0; i < 3; i++)
+				posInWorld[i] = point[iret[j] * 3 + i] + pa[i];
+			if (code < 4)
+			{
+				output.addContactPoint(-normal, posInWorld, -dep[iret[j]]);
+			}
+			else
+			{
+				output.addContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]);
+			}
+		}
+		cnum = maxc;
+	}
+
+	*return_code = code;
+	return cnum;
 }
 
-void	btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/)
+void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* /*debugDraw*/, bool /*swapResults*/)
 {
-	
 	const btTransform& transformA = input.m_transformA;
 	const btTransform& transformB = input.m_transformB;
-	
+
 	int skip = 0;
-	dContactGeom *contact = 0;
+	dContactGeom* contact = 0;
 
 	dMatrix3 R1;
 	dMatrix3 R2;
 
-	for (int j=0;j<3;j++)
+	for (int j = 0; j < 3; j++)
 	{
-		R1[0+4*j] = transformA.getBasis()[j].x();
-		R2[0+4*j] = transformB.getBasis()[j].x();
-
-		R1[1+4*j] = transformA.getBasis()[j].y();
-		R2[1+4*j] = transformB.getBasis()[j].y();
-
+		R1[0 + 4 * j] = transformA.getBasis()[j].x();
+		R2[0 + 4 * j] = transformB.getBasis()[j].x();
 
-		R1[2+4*j] = transformA.getBasis()[j].z();
-		R2[2+4*j] = transformB.getBasis()[j].z();
+		R1[1 + 4 * j] = transformA.getBasis()[j].y();
+		R2[1 + 4 * j] = transformB.getBasis()[j].y();
 
+		R1[2 + 4 * j] = transformA.getBasis()[j].z();
+		R2[2 + 4 * j] = transformB.getBasis()[j].z();
 	}
 
-	
-
 	btVector3 normal;
 	btScalar depth;
 	int return_code;
 	int maxc = 4;
 
-
-	dBoxBox2 (transformA.getOrigin(), 
-	R1,
-	2.f*m_box1->getHalfExtentsWithMargin(),
-	transformB.getOrigin(),
-	R2, 
-	2.f*m_box2->getHalfExtentsWithMargin(),
-	normal, &depth, &return_code,
-	maxc, contact, skip,
-	output
-	);
-
+	dBoxBox2(transformA.getOrigin(),
+			 R1,
+			 2.f * m_box1->getHalfExtentsWithMargin(),
+			 transformB.getOrigin(),
+			 R2,
+			 2.f * m_box2->getHalfExtentsWithMargin(),
+			 normal, &depth, &return_code,
+			 maxc, contact, skip,
+			 output);
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h
index 39243777051..9f7d988fc11 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h
@@ -19,11 +19,9 @@ subject to the following restrictions:
 #ifndef BT_BOX_BOX_DETECTOR_H
 #define BT_BOX_BOX_DETECTOR_H
 
-
 class btBoxShape;
 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
 
-
 /// btBoxBoxDetector wraps the ODE box-box collision detector
 /// re-distributed under the Zlib license with permission from Russell L. Smith
 struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface
@@ -32,13 +30,11 @@ struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface
 	const btBoxShape* m_box2;
 
 public:
+	btBoxBoxDetector(const btBoxShape* box1, const btBoxShape* box2);
 
-	btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2);
-
-	virtual ~btBoxBoxDetector() {};
-
-	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
+	virtual ~btBoxBoxDetector(){};
 
+	virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false);
 };
 
-#endif //BT_BOX_BOX_DETECTOR_H
+#endif  //BT_BOX_BOX_DETECTOR_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h
index 66949849448..d6e15f555bf 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h
@@ -23,11 +23,9 @@ class btPoolAllocator;
 ///btCollisionConfiguration allows to configure Bullet collision detection
 ///stack allocator size, default collision algorithms and persistent manifold pool size
 ///@todo: describe the meaning
-class	btCollisionConfiguration
+class btCollisionConfiguration
 {
-
 public:
-
 	virtual ~btCollisionConfiguration()
 	{
 	}
@@ -37,10 +35,9 @@ public:
 
 	virtual btPoolAllocator* getCollisionAlgorithmPool() = 0;
 
+	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0;
 
-	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0;
-
+	virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0;
 };
 
-#endif //BT_COLLISION_CONFIGURATION
-
+#endif  //BT_COLLISION_CONFIGURATION
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h
index 62ee66c4e9a..bd812849393 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h
@@ -26,20 +26,18 @@ struct btCollisionAlgorithmConstructionInfo;
 struct btCollisionAlgorithmCreateFunc
 {
 	bool m_swapped;
-	
+
 	btCollisionAlgorithmCreateFunc()
-		:m_swapped(false)
+		: m_swapped(false)
 	{
 	}
 	virtual ~btCollisionAlgorithmCreateFunc(){};
 
-	virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& , const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+	virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo&, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 	{
-		
 		(void)body0Wrap;
 		(void)body1Wrap;
 		return 0;
 	}
 };
-#endif //BT_COLLISION_CREATE_FUNC
-
+#endif  //BT_COLLISION_CREATE_FUNC
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
index 3b6913c0e1e..25b2b1ea464 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
@@ -13,10 +13,8 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #include "btCollisionDispatcher.h"
-
+#include "LinearMath/btQuickprof.h"
 
 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 
@@ -27,82 +25,73 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 
-int gNumManifold = 0;
-
 #ifdef BT_DEBUG
 #include <stdio.h>
 #endif
 
-
-btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): 
-m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD),
-	m_collisionConfiguration(collisionConfiguration)
+btCollisionDispatcher::btCollisionDispatcher(btCollisionConfiguration* collisionConfiguration) : m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD),
+																								 m_collisionConfiguration(collisionConfiguration)
 {
 	int i;
 
 	setNearCallback(defaultNearCallback);
-	
+
 	m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool();
 
 	m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool();
 
-	for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
+	for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++)
 	{
-		for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
+		for (int j = 0; j < MAX_BROADPHASE_COLLISION_TYPES; j++)
 		{
-			m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
-			btAssert(m_doubleDispatch[i][j]);
+			m_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i, j);
+			btAssert(m_doubleDispatchContactPoints[i][j]);
+			m_doubleDispatchClosestPoints[i][j] = m_collisionConfiguration->getClosestPointsAlgorithmCreateFunc(i, j);
 		}
 	}
-	
-	
 }
 
+void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc)
+{
+	m_doubleDispatchContactPoints[proxyType0][proxyType1] = createFunc;
+}
 
-void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
+void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc)
 {
-	m_doubleDispatch[proxyType0][proxyType1] = createFunc;
+	m_doubleDispatchClosestPoints[proxyType0][proxyType1] = createFunc;
 }
 
 btCollisionDispatcher::~btCollisionDispatcher()
 {
 }
 
-btPersistentManifold*	btCollisionDispatcher::getNewManifold(const btCollisionObject* body0,const btCollisionObject* body1) 
-{ 
-	gNumManifold++;
-	
+btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1)
+{
 	//btAssert(gNumManifold < 65535);
-	
-
 
 	//optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
-	
-	btScalar contactBreakingThreshold =  (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? 
-		btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold) , body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold))
-		: gContactBreakingThreshold ;
-
-	btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold());
-		
- 	void* mem = 0;
-	
-	if (m_persistentManifoldPoolAllocator->getFreeCount())
-	{
-		mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
-	} else
+
+	btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold), body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold))
+																																: gContactBreakingThreshold;
+
+	btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold());
+
+	void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
+	if (NULL == mem)
 	{
 		//we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert.
-		if ((m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION)==0)
+		if ((m_dispatcherFlags & CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION) == 0)
 		{
-			mem = btAlignedAlloc(sizeof(btPersistentManifold),16);
-		} else
+			mem = btAlignedAlloc(sizeof(btPersistentManifold), 16);
+		}
+		else
 		{
 			btAssert(0);
 			//make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration
 			return 0;
 		}
 	}
-	btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold);
+	btPersistentManifold* manifold = new (mem) btPersistentManifold(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold);
 	manifold->m_index1a = m_manifoldsPtr.size();
 	m_manifoldsPtr.push_back(manifold);
 
@@ -114,18 +103,14 @@ void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold)
 	manifold->clearManifold();
 }
 
-	
 void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
 {
-	
-	gNumManifold--;
-
 	//printf("releaseManifold: gNumManifold %d\n",gNumManifold);
 	clearManifold(manifold);
 
 	int findIndex = manifold->m_index1a;
 	btAssert(findIndex < m_manifoldsPtr.size());
-	m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1);
+	m_manifoldsPtr.swap(findIndex, m_manifoldsPtr.size() - 1);
 	m_manifoldsPtr[findIndex]->m_index1a = findIndex;
 	m_manifoldsPtr.pop_back();
 
@@ -133,42 +118,44 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
 	if (m_persistentManifoldPoolAllocator->validPtr(manifold))
 	{
 		m_persistentManifoldPoolAllocator->freeMemory(manifold);
-	} else
+	}
+	else
 	{
 		btAlignedFree(manifold);
 	}
-	
 }
 
-	
-
-btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold)
+btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType)
 {
-	
 	btCollisionAlgorithmConstructionInfo ci;
 
 	ci.m_dispatcher1 = this;
 	ci.m_manifold = sharedManifold;
-	btCollisionAlgorithm* algo = m_doubleDispatch[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0Wrap,body1Wrap);
+	btCollisionAlgorithm* algo = 0;
+	if (algoType == BT_CONTACT_POINT_ALGORITHMS)
+	{
+		algo = m_doubleDispatchContactPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap);
+	}
+	else
+	{
+		algo = m_doubleDispatchClosestPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap);
+	}
 
 	return algo;
 }
 
-
-
-
-bool	btCollisionDispatcher::needsResponse(const btCollisionObject* body0,const btCollisionObject* body1)
+bool btCollisionDispatcher::needsResponse(const btCollisionObject* body0, const btCollisionObject* body1)
 {
 	//here you can do filtering
-	bool hasResponse = 
+	bool hasResponse =
 		(body0->hasContactResponse() && body1->hasContactResponse());
 	//no response between two static/kinematic bodies:
 	hasResponse = hasResponse &&
-		((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject()));
+				  ((!body0->isStaticOrKinematicObject()) || (!body1->isStaticOrKinematicObject()));
 	return hasResponse;
 }
 
-bool	btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const btCollisionObject* body1)
+bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1)
 {
 	btAssert(body0);
 	btAssert(body1);
@@ -185,31 +172,27 @@ bool	btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const
 			printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
 		}
 	}
-#endif //BT_DEBUG
+#endif  //BT_DEBUG
 
 	if ((!body0->isActive()) && (!body1->isActive()))
 		needsCollision = false;
 	else if ((!body0->checkCollideWith(body1)) || (!body1->checkCollideWith(body0)))
 		needsCollision = false;
-	
-	return needsCollision ;
 
+	return needsCollision;
 }
 
-
-
 ///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc)
 ///this is useful for the collision dispatcher.
 class btCollisionPairCallback : public btOverlapCallback
 {
 	const btDispatcherInfo& m_dispatchInfo;
-	btCollisionDispatcher*	m_dispatcher;
+	btCollisionDispatcher* m_dispatcher;
 
 public:
-
-	btCollisionPairCallback(const btDispatcherInfo& dispatchInfo,btCollisionDispatcher*	dispatcher)
-	:m_dispatchInfo(dispatchInfo),
-	m_dispatcher(dispatcher)
+	btCollisionPairCallback(const btDispatcherInfo& dispatchInfo, btCollisionDispatcher* dispatcher)
+		: m_dispatchInfo(dispatchInfo),
+		  m_dispatcher(dispatcher)
 	{
 	}
 
@@ -221,85 +204,76 @@ public:
 	}
 	*/
 
-
 	virtual ~btCollisionPairCallback() {}
 
-
-	virtual bool	processOverlap(btBroadphasePair& pair)
+	virtual bool processOverlap(btBroadphasePair& pair)
 	{
-		(*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
-
+		(*m_dispatcher->getNearCallback())(pair, *m_dispatcher, m_dispatchInfo);
 		return false;
 	}
 };
 
-
-
-void	btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) 
+void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher)
 {
 	//m_blockedForChanges = true;
 
-	btCollisionPairCallback	collisionCallback(dispatchInfo,this);
+	btCollisionPairCallback collisionCallback(dispatchInfo, this);
 
-	pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher);
+	{
+		BT_PROFILE("processAllOverlappingPairs");
+		pairCache->processAllOverlappingPairs(&collisionCallback, dispatcher, dispatchInfo);
+	}
 
 	//m_blockedForChanges = false;
-
 }
 
-
-
-
 //by default, Bullet will use this near callback
 void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo)
 {
-		btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
-		btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+	btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
+	btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+
+	if (dispatcher.needsCollision(colObj0, colObj1))
+	{
+		btCollisionObjectWrapper obj0Wrap(0, colObj0->getCollisionShape(), colObj0, colObj0->getWorldTransform(), -1, -1);
+		btCollisionObjectWrapper obj1Wrap(0, colObj1->getCollisionShape(), colObj1, colObj1->getWorldTransform(), -1, -1);
 
-		if (dispatcher.needsCollision(colObj0,colObj1))
+		//dispatcher will keep algorithms persistent in the collision pair
+		if (!collisionPair.m_algorithm)
 		{
-			btCollisionObjectWrapper obj0Wrap(0,colObj0->getCollisionShape(),colObj0,colObj0->getWorldTransform(),-1,-1);
-			btCollisionObjectWrapper obj1Wrap(0,colObj1->getCollisionShape(),colObj1,colObj1->getWorldTransform(),-1,-1);
+			collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap, &obj1Wrap, 0, BT_CONTACT_POINT_ALGORITHMS);
+		}
 
+		if (collisionPair.m_algorithm)
+		{
+			btManifoldResult contactPointResult(&obj0Wrap, &obj1Wrap);
 
-			//dispatcher will keep algorithms persistent in the collision pair
-			if (!collisionPair.m_algorithm)
+			if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE)
 			{
-				collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap);
-			}
+				//discrete collision detection query
 
-			if (collisionPair.m_algorithm)
+				collisionPair.m_algorithm->processCollision(&obj0Wrap, &obj1Wrap, dispatchInfo, &contactPointResult);
+			}
+			else
 			{
-				btManifoldResult contactPointResult(&obj0Wrap,&obj1Wrap);
-				
-				if (dispatchInfo.m_dispatchFunc == 		btDispatcherInfo::DISPATCH_DISCRETE)
-				{
-					//discrete collision detection query
-					
-					collisionPair.m_algorithm->processCollision(&obj0Wrap,&obj1Wrap,dispatchInfo,&contactPointResult);
-				} else
-				{
-					//continuous collision detection query, time of impact (toi)
-					btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult);
-					if (dispatchInfo.m_timeOfImpact > toi)
-						dispatchInfo.m_timeOfImpact = toi;
-
-				}
+				//continuous collision detection query, time of impact (toi)
+				btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0, colObj1, dispatchInfo, &contactPointResult);
+				if (dispatchInfo.m_timeOfImpact > toi)
+					dispatchInfo.m_timeOfImpact = toi;
 			}
 		}
-
+	}
 }
 
-
 void* btCollisionDispatcher::allocateCollisionAlgorithm(int size)
 {
-	if (m_collisionAlgorithmPoolAllocator->getFreeCount())
+	void* mem = m_collisionAlgorithmPoolAllocator->allocate(size);
+	if (NULL == mem)
 	{
-		return m_collisionAlgorithmPoolAllocator->allocate(size);
+		//warn user for overflow?
+		return btAlignedAlloc(static_cast<size_t>(size), 16);
 	}
-	
-	//warn user for overflow?
-	return	btAlignedAlloc(static_cast<size_t>(size), 16);
+	return mem;
 }
 
 void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr)
@@ -307,7 +281,8 @@ void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr)
 	if (m_collisionAlgorithmPoolAllocator->validPtr(ptr))
 	{
 		m_collisionAlgorithmPoolAllocator->freeMemory(ptr);
-	} else
+	}
+	else
 	{
 		btAlignedFree(ptr);
 	}
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
index 92696ee5429..04309670cfc 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
@@ -37,33 +37,28 @@ class btCollisionDispatcher;
 ///user can override this nearcallback for collision filtering and more finegrained control over collision detection
 typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo);
 
-
 ///btCollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs.
 ///Time of Impact, Closest Points and Penetration Depth.
 class btCollisionDispatcher : public btDispatcher
 {
-
 protected:
+	int m_dispatcherFlags;
 
-	int		m_dispatcherFlags;
+	btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr;
 
-	btAlignedObjectArray<btPersistentManifold*>	m_manifoldsPtr;
+	btNearCallback m_nearCallback;
 
-	btManifoldResult	m_defaultManifoldResult;
+	btPoolAllocator* m_collisionAlgorithmPoolAllocator;
 
-	btNearCallback		m_nearCallback;
-	
-	btPoolAllocator*	m_collisionAlgorithmPoolAllocator;
+	btPoolAllocator* m_persistentManifoldPoolAllocator;
 
-	btPoolAllocator*	m_persistentManifoldPoolAllocator;
+	btCollisionAlgorithmCreateFunc* m_doubleDispatchContactPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
 
-	btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
-
-	btCollisionConfiguration*	m_collisionConfiguration;
+	btCollisionAlgorithmCreateFunc* m_doubleDispatchClosestPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
 
+	btCollisionConfiguration* m_collisionConfiguration;
 
 public:
-
 	enum DispatcherFlags
 	{
 		CD_STATIC_STATIC_REPORTED = 1,
@@ -71,101 +66,104 @@ public:
 		CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION = 4
 	};
 
-	int	getDispatcherFlags() const
+	int getDispatcherFlags() const
 	{
 		return m_dispatcherFlags;
 	}
 
-	void	setDispatcherFlags(int flags)
+	void setDispatcherFlags(int flags)
 	{
 		m_dispatcherFlags = flags;
 	}
 
 	///registerCollisionCreateFunc allows registration of custom/alternative collision create functions
-	void	registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc);
+	void registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc);
 
-	int	getNumManifolds() const
-	{ 
-		return int( m_manifoldsPtr.size());
+	void registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc);
+
+	int getNumManifolds() const
+	{
+		return int(m_manifoldsPtr.size());
 	}
 
-	btPersistentManifold**	getInternalManifoldPointer()
+	btPersistentManifold** getInternalManifoldPointer()
 	{
-		return m_manifoldsPtr.size()? &m_manifoldsPtr[0] : 0;
+		return m_manifoldsPtr.size() ? &m_manifoldsPtr[0] : 0;
 	}
 
-	 btPersistentManifold* getManifoldByIndexInternal(int index)
+	btPersistentManifold* getManifoldByIndexInternal(int index)
 	{
+		btAssert(index>=0);
+		btAssert(index<m_manifoldsPtr.size());
 		return m_manifoldsPtr[index];
 	}
 
-	 const btPersistentManifold* getManifoldByIndexInternal(int index) const
+	const btPersistentManifold* getManifoldByIndexInternal(int index) const
 	{
+		btAssert(index>=0);
+		btAssert(index<m_manifoldsPtr.size());
 		return m_manifoldsPtr[index];
 	}
 
-	btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration);
+	btCollisionDispatcher(btCollisionConfiguration* collisionConfiguration);
 
 	virtual ~btCollisionDispatcher();
 
-	virtual btPersistentManifold*	getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1);
-	
-	virtual void releaseManifold(btPersistentManifold* manifold);
+	virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0, const btCollisionObject* b1);
 
+	virtual void releaseManifold(btPersistentManifold* manifold);
 
 	virtual void clearManifold(btPersistentManifold* manifold);
 
-	btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold = 0);
-		
-	virtual bool	needsCollision(const btCollisionObject* body0,const btCollisionObject* body1);
-	
-	virtual bool	needsResponse(const btCollisionObject* body0,const btCollisionObject* body1);
-	
-	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ;
+	btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType);
+
+	virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1);
+
+	virtual bool needsResponse(const btCollisionObject* body0, const btCollisionObject* body1);
 
-	void	setNearCallback(btNearCallback	nearCallback)
+	virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher);
+
+	void setNearCallback(btNearCallback nearCallback)
 	{
-		m_nearCallback = nearCallback; 
+		m_nearCallback = nearCallback;
 	}
 
-	btNearCallback	getNearCallback() const
+	btNearCallback getNearCallback() const
 	{
 		return m_nearCallback;
 	}
 
 	//by default, Bullet will use this near callback
-	static void  defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo);
+	static void defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo);
 
-	virtual	void* allocateCollisionAlgorithm(int size);
+	virtual void* allocateCollisionAlgorithm(int size);
 
-	virtual	void freeCollisionAlgorithm(void* ptr);
+	virtual void freeCollisionAlgorithm(void* ptr);
 
-	btCollisionConfiguration*	getCollisionConfiguration()
+	btCollisionConfiguration* getCollisionConfiguration()
 	{
 		return m_collisionConfiguration;
 	}
 
-	const btCollisionConfiguration*	getCollisionConfiguration() const
+	const btCollisionConfiguration* getCollisionConfiguration() const
 	{
 		return m_collisionConfiguration;
 	}
 
-	void	setCollisionConfiguration(btCollisionConfiguration* config)
+	void setCollisionConfiguration(btCollisionConfiguration* config)
 	{
 		m_collisionConfiguration = config;
 	}
 
-	virtual	btPoolAllocator*	getInternalManifoldPool()
+	virtual btPoolAllocator* getInternalManifoldPool()
 	{
 		return m_persistentManifoldPoolAllocator;
 	}
 
-	virtual	const btPoolAllocator*	getInternalManifoldPool() const
+	virtual const btPoolAllocator* getInternalManifoldPool() const
 	{
 		return m_persistentManifoldPoolAllocator;
 	}
-
 };
 
-#endif //BT_COLLISION__DISPATCHER_H
-
+#endif  //BT_COLLISION__DISPATCHER_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp
new file mode 100644
index 00000000000..89bc8d920ef
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp
@@ -0,0 +1,162 @@
+/*
+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 "btCollisionDispatcherMt.h"
+#include "LinearMath/btQuickprof.h"
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "LinearMath/btPoolAllocator.h"
+#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+btCollisionDispatcherMt::btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize)
+	: btCollisionDispatcher(config)
+{
+	m_batchManifoldsPtr.resize(btGetTaskScheduler()->getNumThreads());
+	m_batchUpdating = false;
+	m_grainSize = grainSize;  // iterations per task
+}
+
+btPersistentManifold* btCollisionDispatcherMt::getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1)
+{
+	//optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
+
+	btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold), body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold))
+																																: gContactBreakingThreshold;
+
+	btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold());
+
+	void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
+	if (NULL == mem)
+	{
+		//we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert.
+		if ((m_dispatcherFlags & CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION) == 0)
+		{
+			mem = btAlignedAlloc(sizeof(btPersistentManifold), 16);
+		}
+		else
+		{
+			btAssert(0);
+			//make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration
+			return 0;
+		}
+	}
+	btPersistentManifold* manifold = new (mem) btPersistentManifold(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold);
+	if (!m_batchUpdating)
+	{
+		// batch updater will update manifold pointers array after finishing, so
+		// only need to update array when not batch-updating
+		//btAssert( !btThreadsAreRunning() );
+		manifold->m_index1a = m_manifoldsPtr.size();
+		m_manifoldsPtr.push_back(manifold);
+	}
+	else
+	{
+		m_batchManifoldsPtr[btGetCurrentThreadIndex()].push_back(manifold);
+	}
+
+	return manifold;
+}
+
+void btCollisionDispatcherMt::releaseManifold(btPersistentManifold* manifold)
+{
+	clearManifold(manifold);
+	//btAssert( !btThreadsAreRunning() );
+	if (!m_batchUpdating)
+	{
+		// batch updater will update manifold pointers array after finishing, so
+		// only need to update array when not batch-updating
+		int findIndex = manifold->m_index1a;
+		btAssert(findIndex < m_manifoldsPtr.size());
+		m_manifoldsPtr.swap(findIndex, m_manifoldsPtr.size() - 1);
+		m_manifoldsPtr[findIndex]->m_index1a = findIndex;
+		m_manifoldsPtr.pop_back();
+	}
+
+	manifold->~btPersistentManifold();
+	if (m_persistentManifoldPoolAllocator->validPtr(manifold))
+	{
+		m_persistentManifoldPoolAllocator->freeMemory(manifold);
+	}
+	else
+	{
+		btAlignedFree(manifold);
+	}
+}
+
+struct CollisionDispatcherUpdater : public btIParallelForBody
+{
+	btBroadphasePair* mPairArray;
+	btNearCallback mCallback;
+	btCollisionDispatcher* mDispatcher;
+	const btDispatcherInfo* mInfo;
+
+	CollisionDispatcherUpdater()
+	{
+		mPairArray = NULL;
+		mCallback = NULL;
+		mDispatcher = NULL;
+		mInfo = NULL;
+	}
+	void forLoop(int iBegin, int iEnd) const
+	{
+		for (int i = iBegin; i < iEnd; ++i)
+		{
+			btBroadphasePair* pair = &mPairArray[i];
+			mCallback(*pair, *mDispatcher, *mInfo);
+		}
+	}
+};
+
+void btCollisionDispatcherMt::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher)
+{
+	const int pairCount = pairCache->getNumOverlappingPairs();
+	if (pairCount == 0)
+	{
+		return;
+	}
+	CollisionDispatcherUpdater updater;
+	updater.mCallback = getNearCallback();
+	updater.mPairArray = pairCache->getOverlappingPairArrayPtr();
+	updater.mDispatcher = this;
+	updater.mInfo = &info;
+
+	m_batchUpdating = true;
+	btParallelFor(0, pairCount, m_grainSize, updater);
+	m_batchUpdating = false;
+
+	// merge new manifolds, if any
+	for (int i = 0; i < m_batchManifoldsPtr.size(); ++i)
+	{
+		btAlignedObjectArray<btPersistentManifold*>& batchManifoldsPtr = m_batchManifoldsPtr[i];
+
+		for (int j = 0; j < batchManifoldsPtr.size(); ++j)
+		{
+			m_manifoldsPtr.push_back(batchManifoldsPtr[j]);
+		}
+
+		batchManifoldsPtr.resizeNoInitialize(0);
+	}
+
+	// update the indices (used when releasing manifolds)
+	for (int i = 0; i < m_manifoldsPtr.size(); ++i)
+	{
+		m_manifoldsPtr[i]->m_index1a = i;
+	}
+}
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h
new file mode 100644
index 00000000000..1155de2cfe6
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h
@@ -0,0 +1,38 @@
+/*
+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_COLLISION_DISPATCHER_MT_H
+#define BT_COLLISION_DISPATCHER_MT_H
+
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "LinearMath/btThreads.h"
+
+class btCollisionDispatcherMt : public btCollisionDispatcher
+{
+public:
+	btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize = 40);
+
+	virtual btPersistentManifold* getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1) BT_OVERRIDE;
+	virtual void releaseManifold(btPersistentManifold* manifold) BT_OVERRIDE;
+
+	virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) BT_OVERRIDE;
+
+protected:
+	btAlignedObjectArray<btAlignedObjectArray<btPersistentManifold*> > m_batchManifoldsPtr;
+	bool m_batchUpdating;
+	int m_grainSize;
+};
+
+#endif  //BT_COLLISION_DISPATCHER_MT_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
index 395df3a550f..b48d9301d74 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
@@ -13,36 +13,45 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btCollisionObject.h"
 #include "LinearMath/btSerializer.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 
 btCollisionObject::btCollisionObject()
-	:	m_anisotropicFriction(1.f,1.f,1.f),
-	m_hasAnisotropicFriction(false),
-	m_contactProcessingThreshold(BT_LARGE_FLOAT),
-		m_broadphaseHandle(0),
-		m_collisionShape(0),
-		m_extensionPointer(0),
-		m_rootCollisionShape(0),
-		m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT),
-		m_islandTag1(-1),
-		m_companionId(-1),
-		m_activationState1(1),
-		m_deactivationTime(btScalar(0.)),
-		m_friction(btScalar(0.5)),
-		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.)),
-		m_checkCollideWith(false),
-		m_updateRevision(0)
+	: m_interpolationLinearVelocity(0.f, 0.f, 0.f),
+	  m_interpolationAngularVelocity(0.f, 0.f, 0.f),
+	  m_anisotropicFriction(1.f, 1.f, 1.f),
+	  m_hasAnisotropicFriction(false),
+	  m_contactProcessingThreshold(BT_LARGE_FLOAT),
+	  m_broadphaseHandle(0),
+	  m_collisionShape(0),
+	  m_extensionPointer(0),
+	  m_rootCollisionShape(0),
+	  m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT),
+	  m_islandTag1(-1),
+	  m_companionId(-1),
+	  m_worldArrayIndex(-1),
+	  m_activationState1(1),
+	  m_deactivationTime(btScalar(0.)),
+	  m_friction(btScalar(0.5)),
+	  m_restitution(btScalar(0.)),
+	  m_rollingFriction(0.0f),
+	  m_spinningFriction(0.f),
+	  m_contactDamping(.1),
+	  m_contactStiffness(BT_LARGE_FLOAT),
+	  m_internalType(CO_COLLISION_OBJECT),
+	  m_userObjectPointer(0),
+	  m_userIndex2(-1),
+	  m_userIndex(-1),
+	  m_userIndex3(-1),
+	  m_hitFraction(btScalar(1.)),
+	  m_ccdSweptSphereRadius(btScalar(0.)),
+	  m_ccdMotionThreshold(btScalar(0.)),
+	  m_checkCollideWith(false),
+	  m_updateRevision(0)
 {
 	m_worldTransform.setIdentity();
+	m_interpolationWorldTransform.setIdentity();
 }
 
 btCollisionObject::~btCollisionObject()
@@ -50,8 +59,8 @@ btCollisionObject::~btCollisionObject()
 }
 
 void btCollisionObject::setActivationState(int newState) const
-{ 
-	if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION))
+{
+	if ((m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION))
 		m_activationState1 = newState;
 }
 
@@ -62,7 +71,7 @@ void btCollisionObject::forceActivationState(int newState) const
 
 void btCollisionObject::activate(bool forceActivation) const
 {
-	if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT)))
+	if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT)))
 	{
 		setActivationState(ACTIVE_TAG);
 		m_deactivationTime = btScalar(0.);
@@ -71,7 +80,6 @@ void btCollisionObject::activate(bool forceActivation) const
 
 const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-
 	btCollisionObjectData* dataOut = (btCollisionObjectData*)dataBuffer;
 
 	m_worldTransform.serialize(dataOut->m_worldTransform);
@@ -83,7 +91,7 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
 	dataOut->m_contactProcessingThreshold = m_contactProcessingThreshold;
 	dataOut->m_broadphaseHandle = 0;
 	dataOut->m_collisionShape = serializer->getUniquePointer(m_collisionShape);
-	dataOut->m_rootCollisionShape = 0;//@todo
+	dataOut->m_rootCollisionShape = 0;  //@todo
 	dataOut->m_collisionFlags = m_collisionFlags;
 	dataOut->m_islandTag1 = m_islandTag1;
 	dataOut->m_companionId = m_companionId;
@@ -91,10 +99,12 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
 	dataOut->m_deactivationTime = m_deactivationTime;
 	dataOut->m_friction = m_friction;
 	dataOut->m_rollingFriction = m_rollingFriction;
+	dataOut->m_contactDamping = m_contactDamping;
+	dataOut->m_contactStiffness = m_contactStiffness;
 	dataOut->m_restitution = m_restitution;
 	dataOut->m_internalType = m_internalType;
-	
-	char* name = (char*) serializer->findNameForPointer(this);
+
+	char* name = (char*)serializer->findNameForPointer(this);
 	dataOut->m_name = (char*)serializer->getUniquePointer(name);
 	if (dataOut->m_name)
 	{
@@ -104,15 +114,25 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
 	dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius;
 	dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold;
 	dataOut->m_checkCollideWith = m_checkCollideWith;
-
+	if (m_broadphaseHandle)
+	{
+		dataOut->m_collisionFilterGroup = m_broadphaseHandle->m_collisionFilterGroup;
+		dataOut->m_collisionFilterMask = m_broadphaseHandle->m_collisionFilterMask;
+		dataOut->m_uniqueId = m_broadphaseHandle->m_uniqueId;
+	}
+	else
+	{
+		dataOut->m_collisionFilterGroup = 0;
+		dataOut->m_collisionFilterMask = 0;
+		dataOut->m_uniqueId = -1;
+	}
 	return btCollisionObjectDataName;
 }
 
-
 void btCollisionObject::serializeSingleObject(class btSerializer* serializer) const
 {
 	int len = calculateSerializeBufferSize();
-	btChunk* chunk = serializer->allocate(len,1);
+	btChunk* chunk = serializer->allocate(len, 1);
 	const char* structType = serialize(chunk->m_oldPtr, serializer);
-	serializer->finalizeChunk(chunk,structType,BT_COLLISIONOBJECT_CODE,(void*)this);
+	serializer->finalizeChunk(chunk, structType, BT_COLLISIONOBJECT_CODE, (void*)this);
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h
index c68402418f7..3a1c271af5b 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h
@@ -25,8 +25,8 @@ subject to the following restrictions:
 #define DISABLE_DEACTIVATION 4
 #define DISABLE_SIMULATION 5
 
-struct	btBroadphaseProxy;
-class	btCollisionShape;
+struct btBroadphaseProxy;
+class btCollisionShape;
 struct btCollisionShapeData;
 #include "LinearMath/btMotionState.h"
 #include "LinearMath/btAlignedAllocator.h"
@@ -42,110 +42,120 @@ typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;
 #define btCollisionObjectDataName "btCollisionObjectFloatData"
 #endif
 
-
-/// btCollisionObject can be used to manage collision detection objects. 
+/// btCollisionObject can be used to manage collision detection objects.
 /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
 /// They can be added to the btCollisionWorld.
-ATTRIBUTE_ALIGNED16(class)	btCollisionObject
+ATTRIBUTE_ALIGNED16(class)
+btCollisionObject
 {
-
 protected:
-
-	btTransform	m_worldTransform;
+	btTransform m_worldTransform;
 
 	///m_interpolationWorldTransform is used for CCD and interpolation
 	///it can be either previous or future (predicted) transform
-	btTransform	m_interpolationWorldTransform;
-	//those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) 
+	btTransform m_interpolationWorldTransform;
+	//those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities)
 	//without destroying the continuous interpolated motion (which uses this interpolation velocities)
-	btVector3	m_interpolationLinearVelocity;
-	btVector3	m_interpolationAngularVelocity;
-	
-	btVector3	m_anisotropicFriction;
-	int			m_hasAnisotropicFriction;
-	btScalar	m_contactProcessingThreshold;	
-
-	btBroadphaseProxy*		m_broadphaseHandle;
-	btCollisionShape*		m_collisionShape;
+	btVector3 m_interpolationLinearVelocity;
+	btVector3 m_interpolationAngularVelocity;
+
+	btVector3 m_anisotropicFriction;
+	int m_hasAnisotropicFriction;
+	btScalar m_contactProcessingThreshold;
+
+	btBroadphaseProxy* m_broadphaseHandle;
+	btCollisionShape* m_collisionShape;
 	///m_extensionPointer is used by some internal low-level Bullet extensions.
-	void*					m_extensionPointer;
-	
+	void* m_extensionPointer;
+
 	///m_rootCollisionShape is temporarily used to store the original collision shape
 	///The m_collisionShape might be temporarily replaced by a child collision shape during collision detection purposes
 	///If it is NULL, the m_collisionShape is not temporarily replaced.
-	btCollisionShape*		m_rootCollisionShape;
+	btCollisionShape* m_rootCollisionShape;
 
-	int				m_collisionFlags;
+	int m_collisionFlags;
 
-	int				m_islandTag1;
-	int				m_companionId;
+	int m_islandTag1;
+	int m_companionId;
+	int m_worldArrayIndex;  // index of object in world's collisionObjects array
 
-	mutable int				m_activationState1;
-	mutable btScalar			m_deactivationTime;
+	mutable int m_activationState1;
+	mutable btScalar m_deactivationTime;
 
-	btScalar		m_friction;
-	btScalar		m_restitution;
-	btScalar		m_rollingFriction;
+	btScalar m_friction;
+	btScalar m_restitution;
+	btScalar m_rollingFriction;   //torsional friction orthogonal to contact normal (useful to stop spheres rolling forever)
+	btScalar m_spinningFriction;  // torsional friction around the contact normal (useful for grasping)
+	btScalar m_contactDamping;
+	btScalar m_contactStiffness;
 
 	///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc.
 	///do not assign your own m_internalType unless you write a new dynamics object class.
-	int				m_internalType;
+	int m_internalType;
 
 	///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
 
-    void*			m_userObjectPointer;
-    
-    int	m_userIndex;
+	void* m_userObjectPointer;
+
+	int m_userIndex2;
+
+	int m_userIndex;
+
+	int m_userIndex3;
 
 	///time of impact calculation
-	btScalar		m_hitFraction; 
-	
+	btScalar m_hitFraction;
+
 	///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
-	btScalar		m_ccdSweptSphereRadius;
+	btScalar m_ccdSweptSphereRadius;
 
 	/// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
-	btScalar		m_ccdMotionThreshold;
-	
+	btScalar m_ccdMotionThreshold;
+
 	/// If some object should have elaborate collision filtering by sub-classes
-	int			m_checkCollideWith;
+	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;
+	int m_updateRevision;
 
+	btVector3 m_customDebugColorRGB;
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	enum CollisionFlags
 	{
-		CF_STATIC_OBJECT= 1,
-		CF_KINEMATIC_OBJECT= 2,
+		CF_STATIC_OBJECT = 1,
+		CF_KINEMATIC_OBJECT = 2,
 		CF_NO_CONTACT_RESPONSE = 4,
-		CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
+		CF_CUSTOM_MATERIAL_CALLBACK = 8,  //this allows per-triangle material (friction/restitution)
 		CF_CHARACTER_OBJECT = 16,
-		CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
-		CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing
+		CF_DISABLE_VISUALIZE_OBJECT = 32,          //disable debug drawing
+		CF_DISABLE_SPU_COLLISION_PROCESSING = 64,  //disable parallel/SPU processing
+		CF_HAS_CONTACT_STIFFNESS_DAMPING = 128,
+		CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256,
+		CF_HAS_FRICTION_ANCHOR = 512,
+		CF_HAS_COLLISION_SOUND_TRIGGER = 1024
 	};
 
-	enum	CollisionObjectTypes
+	enum CollisionObjectTypes
 	{
-		CO_COLLISION_OBJECT =1,
-		CO_RIGID_BODY=2,
+		CO_COLLISION_OBJECT = 1,
+		CO_RIGID_BODY = 2,
 		///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter
 		///It is useful for collision sensors, explosion objects, character controller etc.
-		CO_GHOST_OBJECT=4,
-		CO_SOFT_BODY=8,
-		CO_HF_FLUID=16,
-		CO_USER_TYPE=32,
-		CO_FEATHERSTONE_LINK=64
+		CO_GHOST_OBJECT = 4,
+		CO_SOFT_BODY = 8,
+		CO_HF_FLUID = 16,
+		CO_USER_TYPE = 32,
+		CO_FEATHERSTONE_LINK = 64
 	};
 
 	enum AnisotropicFrictionFlags
 	{
-		CF_ANISOTROPIC_FRICTION_DISABLED=0,
+		CF_ANISOTROPIC_FRICTION_DISABLED = 0,
 		CF_ANISOTROPIC_FRICTION = 1,
 		CF_ANISOTROPIC_ROLLING_FRICTION = 2
 	};
@@ -153,76 +163,77 @@ public:
 	SIMD_FORCE_INLINE bool mergesSimulationIslands() const
 	{
 		///static objects, kinematic and object without contact response don't merge islands
-		return  ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0);
+		return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE)) == 0);
 	}
 
 	const btVector3& getAnisotropicFriction() const
 	{
 		return m_anisotropicFriction;
 	}
-	void	setAnisotropicFriction(const btVector3& anisotropicFriction, int frictionMode = CF_ANISOTROPIC_FRICTION)
+	void setAnisotropicFriction(const btVector3& anisotropicFriction, int frictionMode = CF_ANISOTROPIC_FRICTION)
 	{
 		m_anisotropicFriction = anisotropicFriction;
-		bool isUnity = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f);
-		m_hasAnisotropicFriction = isUnity?frictionMode : 0;
+		bool isUnity = (anisotropicFriction[0] != 1.f) || (anisotropicFriction[1] != 1.f) || (anisotropicFriction[2] != 1.f);
+		m_hasAnisotropicFriction = isUnity ? frictionMode : 0;
 	}
-	bool	hasAnisotropicFriction(int frictionMode = CF_ANISOTROPIC_FRICTION) const
+	bool hasAnisotropicFriction(int frictionMode = CF_ANISOTROPIC_FRICTION) const
 	{
-		return (m_hasAnisotropicFriction&frictionMode)!=0;
+		return (m_hasAnisotropicFriction & frictionMode) != 0;
 	}
 
 	///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default.
 	///Note that using contacts with positive distance can improve stability. It increases, however, the chance of colliding with degerate contacts, such as 'interior' triangle edges
-	void	setContactProcessingThreshold( btScalar contactProcessingThreshold)
+	void setContactProcessingThreshold(btScalar contactProcessingThreshold)
 	{
 		m_contactProcessingThreshold = contactProcessingThreshold;
 	}
-	btScalar	getContactProcessingThreshold() const
+	btScalar getContactProcessingThreshold() const
 	{
 		return m_contactProcessingThreshold;
 	}
 
-	SIMD_FORCE_INLINE bool		isStaticObject() const {
+	SIMD_FORCE_INLINE bool isStaticObject() const
+	{
 		return (m_collisionFlags & CF_STATIC_OBJECT) != 0;
 	}
 
-	SIMD_FORCE_INLINE bool		isKinematicObject() const
+	SIMD_FORCE_INLINE bool isKinematicObject() const
 	{
 		return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0;
 	}
 
-	SIMD_FORCE_INLINE bool		isStaticOrKinematicObject() const
+	SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const
 	{
-		return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ;
+		return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0;
 	}
 
-	SIMD_FORCE_INLINE bool		hasContactResponse() const {
-		return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0;
+	SIMD_FORCE_INLINE bool hasContactResponse() const
+	{
+		return (m_collisionFlags & CF_NO_CONTACT_RESPONSE) == 0;
 	}
 
-	
 	btCollisionObject();
 
 	virtual ~btCollisionObject();
 
-	virtual void	setCollisionShape(btCollisionShape* collisionShape)
+	virtual void setCollisionShape(btCollisionShape * collisionShape)
 	{
 		m_updateRevision++;
 		m_collisionShape = collisionShape;
 		m_rootCollisionShape = collisionShape;
 	}
 
-	SIMD_FORCE_INLINE const btCollisionShape*	getCollisionShape() const
+	SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const
 	{
 		return m_collisionShape;
 	}
 
-	SIMD_FORCE_INLINE btCollisionShape*	getCollisionShape()
+	SIMD_FORCE_INLINE btCollisionShape* getCollisionShape()
 	{
 		return m_collisionShape;
 	}
 
-	void	setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck)
+	void setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck)
 	{
 		if (ignoreCollisionCheck)
 		{
@@ -240,7 +251,17 @@ public:
 		m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0;
 	}
 
-	virtual bool	checkCollideWithOverride(const btCollisionObject*  co) const
+        int getNumObjectsWithoutCollision() const
+	{
+		return m_objectsWithoutCollisionCheck.size();
+	}
+
+	const btCollisionObject* getObjectWithoutCollision(int index)
+	{
+		return m_objectsWithoutCollisionCheck[index];
+	}
+
+	virtual bool checkCollideWithOverride(const btCollisionObject* co) const
 	{
 		int index = m_objectsWithoutCollisionCheck.findLinearSearch(co);
 		if (index < m_objectsWithoutCollisionCheck.size())
@@ -250,249 +271,325 @@ public:
 		return true;
 	}
 
-
-	
-
-	///Avoid using this internal API call, the extension pointer is used by some Bullet extensions. 
+	///Avoid using this internal API call, the extension pointer is used by some Bullet extensions.
 	///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead.
-	void*		internalGetExtensionPointer() const
+	void* internalGetExtensionPointer() const
 	{
 		return m_extensionPointer;
 	}
 	///Avoid using this internal API call, the extension pointer is used by some Bullet extensions
 	///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead.
-	void	internalSetExtensionPointer(void* pointer)
+	void internalSetExtensionPointer(void* pointer)
 	{
 		m_extensionPointer = pointer;
 	}
 
-	SIMD_FORCE_INLINE	int	getActivationState() const { return m_activationState1;}
-	
+	SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1; }
+
 	void setActivationState(int newState) const;
 
-	void	setDeactivationTime(btScalar time)
+	void setDeactivationTime(btScalar time)
 	{
 		m_deactivationTime = time;
 	}
-	btScalar	getDeactivationTime() const
+	btScalar getDeactivationTime() const
 	{
 		return m_deactivationTime;
 	}
 
 	void forceActivationState(int newState) const;
 
-	void	activate(bool forceActivation = false) const;
+	void activate(bool forceActivation = false) const;
 
 	SIMD_FORCE_INLINE bool isActive() const
 	{
 		return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION));
 	}
 
-	void	setRestitution(btScalar rest)
+	void setRestitution(btScalar rest)
 	{
 		m_updateRevision++;
 		m_restitution = rest;
 	}
-	btScalar	getRestitution() const
+	btScalar getRestitution() const
 	{
 		return m_restitution;
 	}
-	void	setFriction(btScalar frict)
+	void setFriction(btScalar frict)
 	{
 		m_updateRevision++;
 		m_friction = frict;
 	}
-	btScalar	getFriction() const
+	btScalar getFriction() const
 	{
 		return m_friction;
 	}
 
-	void	setRollingFriction(btScalar frict)
+	void setRollingFriction(btScalar frict)
 	{
 		m_updateRevision++;
 		m_rollingFriction = frict;
 	}
-	btScalar	getRollingFriction() const
+	btScalar getRollingFriction() const
 	{
 		return m_rollingFriction;
 	}
+	void setSpinningFriction(btScalar frict)
+	{
+		m_updateRevision++;
+		m_spinningFriction = frict;
+	}
+	btScalar getSpinningFriction() const
+	{
+		return m_spinningFriction;
+	}
+	void setContactStiffnessAndDamping(btScalar stiffness, btScalar damping)
+	{
+		m_updateRevision++;
+		m_contactStiffness = stiffness;
+		m_contactDamping = damping;
 
+		m_collisionFlags |= CF_HAS_CONTACT_STIFFNESS_DAMPING;
+
+		//avoid divisions by zero...
+		if (m_contactStiffness < SIMD_EPSILON)
+		{
+			m_contactStiffness = SIMD_EPSILON;
+		}
+	}
+
+	btScalar getContactStiffness() const
+	{
+		return m_contactStiffness;
+	}
+
+	btScalar getContactDamping() const
+	{
+		return m_contactDamping;
+	}
 
 	///reserved for Bullet internal usage
-	int	getInternalType() const
+	int getInternalType() const
 	{
 		return m_internalType;
 	}
 
-	btTransform&	getWorldTransform()
+	btTransform& getWorldTransform()
 	{
 		return m_worldTransform;
 	}
 
-	const btTransform&	getWorldTransform() const
+	const btTransform& getWorldTransform() const
 	{
 		return m_worldTransform;
 	}
 
-	void	setWorldTransform(const btTransform& worldTrans)
+	void setWorldTransform(const btTransform& worldTrans)
 	{
 		m_updateRevision++;
 		m_worldTransform = worldTrans;
 	}
 
-
-	SIMD_FORCE_INLINE btBroadphaseProxy*	getBroadphaseHandle()
+	SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle()
 	{
 		return m_broadphaseHandle;
 	}
 
-	SIMD_FORCE_INLINE const btBroadphaseProxy*	getBroadphaseHandle() const
+	SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const
 	{
 		return m_broadphaseHandle;
 	}
 
-	void	setBroadphaseHandle(btBroadphaseProxy* handle)
+	void setBroadphaseHandle(btBroadphaseProxy * handle)
 	{
 		m_broadphaseHandle = handle;
 	}
 
-
-	const btTransform&	getInterpolationWorldTransform() const
+	const btTransform& getInterpolationWorldTransform() const
 	{
 		return m_interpolationWorldTransform;
 	}
 
-	btTransform&	getInterpolationWorldTransform()
+	btTransform& getInterpolationWorldTransform()
 	{
 		return m_interpolationWorldTransform;
 	}
 
-	void	setInterpolationWorldTransform(const btTransform&	trans)
+	void setInterpolationWorldTransform(const btTransform& trans)
 	{
 		m_updateRevision++;
 		m_interpolationWorldTransform = trans;
 	}
 
-	void	setInterpolationLinearVelocity(const btVector3& linvel)
+	void setInterpolationLinearVelocity(const btVector3& linvel)
 	{
 		m_updateRevision++;
 		m_interpolationLinearVelocity = linvel;
 	}
 
-	void	setInterpolationAngularVelocity(const btVector3& angvel)
+	void setInterpolationAngularVelocity(const btVector3& angvel)
 	{
 		m_updateRevision++;
 		m_interpolationAngularVelocity = angvel;
 	}
 
-	const btVector3&	getInterpolationLinearVelocity() const
+	const btVector3& getInterpolationLinearVelocity() const
 	{
 		return m_interpolationLinearVelocity;
 	}
 
-	const btVector3&	getInterpolationAngularVelocity() const
+	const btVector3& getInterpolationAngularVelocity() const
 	{
 		return m_interpolationAngularVelocity;
 	}
 
 	SIMD_FORCE_INLINE int getIslandTag() const
 	{
-		return	m_islandTag1;
+		return m_islandTag1;
 	}
 
-	void	setIslandTag(int tag)
+	void setIslandTag(int tag)
 	{
 		m_islandTag1 = tag;
 	}
 
 	SIMD_FORCE_INLINE int getCompanionId() const
 	{
-		return	m_companionId;
+		return m_companionId;
 	}
 
-	void	setCompanionId(int id)
+	void setCompanionId(int id)
 	{
 		m_companionId = id;
 	}
 
-	SIMD_FORCE_INLINE btScalar			getHitFraction() const
+	SIMD_FORCE_INLINE int getWorldArrayIndex() const
 	{
-		return m_hitFraction; 
+		return m_worldArrayIndex;
 	}
 
-	void	setHitFraction(btScalar hitFraction)
+	// only should be called by CollisionWorld
+	void setWorldArrayIndex(int ix)
+	{
+		m_worldArrayIndex = ix;
+	}
+
+	SIMD_FORCE_INLINE btScalar getHitFraction() const
+	{
+		return m_hitFraction;
+	}
+
+	void setHitFraction(btScalar hitFraction)
 	{
 		m_hitFraction = hitFraction;
 	}
 
-	
-	SIMD_FORCE_INLINE int	getCollisionFlags() const
+	SIMD_FORCE_INLINE int getCollisionFlags() const
 	{
 		return m_collisionFlags;
 	}
 
-	void	setCollisionFlags(int flags)
+	void setCollisionFlags(int flags)
 	{
 		m_collisionFlags = flags;
 	}
-	
+
 	///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
-	btScalar			getCcdSweptSphereRadius() const
+	btScalar getCcdSweptSphereRadius() const
 	{
 		return m_ccdSweptSphereRadius;
 	}
 
 	///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
-	void	setCcdSweptSphereRadius(btScalar radius)
+	void setCcdSweptSphereRadius(btScalar radius)
 	{
 		m_ccdSweptSphereRadius = radius;
 	}
 
-	btScalar 	getCcdMotionThreshold() const
+	btScalar getCcdMotionThreshold() const
 	{
 		return m_ccdMotionThreshold;
 	}
 
-	btScalar 	getCcdSquareMotionThreshold() const
+	btScalar getCcdSquareMotionThreshold() const
 	{
-		return m_ccdMotionThreshold*m_ccdMotionThreshold;
+		return m_ccdMotionThreshold * m_ccdMotionThreshold;
 	}
 
-
-
 	/// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
-	void	setCcdMotionThreshold(btScalar ccdMotionThreshold)
+	void setCcdMotionThreshold(btScalar ccdMotionThreshold)
 	{
 		m_ccdMotionThreshold = ccdMotionThreshold;
 	}
 
 	///users can point to their objects, userPointer is not used by Bullet
-	void*	getUserPointer() const
+	void* getUserPointer() const
 	{
 		return m_userObjectPointer;
 	}
 
-	int	getUserIndex() const
+	int getUserIndex() const
 	{
 		return m_userIndex;
 	}
+
+	int getUserIndex2() const
+	{
+		return m_userIndex2;
+	}
+
+	int getUserIndex3() const
+	{
+		return m_userIndex3;
+	}
+
 	///users can point to their objects, userPointer is not used by Bullet
-	void	setUserPointer(void* userPointer)
+	void setUserPointer(void* userPointer)
 	{
 		m_userObjectPointer = userPointer;
 	}
 
 	///users can point to their objects, userPointer is not used by Bullet
-	void	setUserIndex(int index)
+	void setUserIndex(int index)
 	{
 		m_userIndex = index;
 	}
 
-	int	getUpdateRevisionInternal() const
+	void setUserIndex2(int index)
+	{
+		m_userIndex2 = index;
+	}
+
+	void setUserIndex3(int index)
+	{
+		m_userIndex3 = index;
+	}
+
+	int getUpdateRevisionInternal() const
 	{
 		return m_updateRevision;
 	}
 
+	void setCustomDebugColor(const btVector3& colorRGB)
+	{
+		m_customDebugColorRGB = colorRGB;
+		m_collisionFlags |= CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR;
+	}
+
+	void removeCustomDebugColor()
+	{
+		m_collisionFlags &= ~CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR;
+	}
+
+	bool getCustomDebugColor(btVector3 & colorRGB) const
+	{
+		bool hasCustomColor = (0 != (m_collisionFlags & CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR));
+		if (hasCustomColor)
+		{
+			colorRGB = m_customDebugColorRGB;
+		}
+		return hasCustomColor;
+	}
 
 	inline bool checkCollideWith(const btCollisionObject* co) const
 	{
@@ -502,15 +599,16 @@ public:
 		return true;
 	}
 
-	virtual	int	calculateSerializeBufferSize()	const;
+	virtual int calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer, class btSerializer* serializer) const;
-
-	virtual void serializeSingleObject(class btSerializer* serializer) const;
+	virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
 
+	virtual void serializeSingleObject(class btSerializer * serializer) const;
 };
 
+// clang-format off
+
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btCollisionObjectDoubleData
 {
@@ -528,11 +626,12 @@ struct	btCollisionObjectDoubleData
 	double					m_deactivationTime;
 	double					m_friction;
 	double					m_rollingFriction;
+	double                  m_contactDamping;
+	double                  m_contactStiffness;
 	double					m_restitution;
 	double					m_hitFraction; 
 	double					m_ccdSweptSphereRadius;
 	double					m_ccdMotionThreshold;
-
 	int						m_hasAnisotropicFriction;
 	int						m_collisionFlags;
 	int						m_islandTag1;
@@ -540,8 +639,9 @@ struct	btCollisionObjectDoubleData
 	int						m_activationState1;
 	int						m_internalType;
 	int						m_checkCollideWith;
-
-	char	m_padding[4];
+	int						m_collisionFilterGroup;
+	int						m_collisionFilterMask;
+	int						m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
@@ -561,12 +661,12 @@ struct	btCollisionObjectFloatData
 	float					m_deactivationTime;
 	float					m_friction;
 	float					m_rollingFriction;
-
+	float                   m_contactDamping;
+    float                   m_contactStiffness;
 	float					m_restitution;
 	float					m_hitFraction; 
 	float					m_ccdSweptSphereRadius;
 	float					m_ccdMotionThreshold;
-
 	int						m_hasAnisotropicFriction;
 	int						m_collisionFlags;
 	int						m_islandTag1;
@@ -574,16 +674,15 @@ struct	btCollisionObjectFloatData
 	int						m_activationState1;
 	int						m_internalType;
 	int						m_checkCollideWith;
-	char					m_padding[4];
+	int						m_collisionFilterGroup;
+	int						m_collisionFilterMask;
+	int						m_uniqueId;
 };
+// clang-format on
 
-
-
-SIMD_FORCE_INLINE	int	btCollisionObject::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const
 {
 	return sizeof(btCollisionObjectData);
 }
 
-
-
-#endif //BT_COLLISION_OBJECT_H
+#endif  //BT_COLLISION_OBJECT_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h
index 952440b7dee..56341b7d292 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h
@@ -1,25 +1,25 @@
 #ifndef BT_COLLISION_OBJECT_WRAPPER_H
 #define BT_COLLISION_OBJECT_WRAPPER_H
 
-///btCollisionObjectWrapperis an internal data structure. 
+///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
+#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*);
+private:                             \
+	void* operator new(size_t size); \
+	void operator delete(void*);
 
 struct btCollisionObjectWrapper;
 struct btCollisionObjectWrapper
 {
-BT_DECLARE_STACK_ONLY_OBJECT
+	BT_DECLARE_STACK_ONLY_OBJECT
 
 private:
-	btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed.
+	btCollisionObjectWrapper(const btCollisionObjectWrapper&);  // not implemented. Not allowed.
 	btCollisionObjectWrapper* operator=(const btCollisionObjectWrapper&);
 
 public:
@@ -27,17 +27,23 @@ public:
 	const btCollisionShape* m_shape;
 	const btCollisionObject* m_collisionObject;
 	const btTransform& m_worldTransform;
-	int		m_partId;
-	int		m_index;
+    const btTransform* m_preTransform;
+	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)
-	{}
+		: m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_preTransform(NULL), m_partId(partId), m_index(index)
+	{
+	}
+    
+    btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, const btTransform& preTransform, int partId, int index)
+    : m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_preTransform(&preTransform), 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
+#endif  //BT_COLLISION_OBJECT_WRAPPER_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index c505ed5d508..71184f36acd 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -19,8 +19,10 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
-#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting
-#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting
+#include "BulletCollision/CollisionShapes/btSphereShape.h"                 //for raycasting
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"        //for raycasting
+#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"  //for raycasting
+#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"     //for raycasting
 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
@@ -37,7 +39,6 @@ subject to the following restrictions:
 
 //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
 
-
 //#define USE_BRUTEFORCE_RAYBROADPHASE 1
 //RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation'  or 'updateAabbs' before using a rayTest
 //#define RECALCULATE_AABB_RAYCAST 1
@@ -47,7 +48,6 @@ subject to the following restrictions:
 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
 #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
 
-
 ///for debug drawing
 
 //for debug rendering
@@ -64,25 +64,21 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
 
-
-
-btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
-:m_dispatcher1(dispatcher),
-m_broadphasePairCache(pairCache),
-m_debugDrawer(0),
-m_forceUpdateAllAabbs(true)
+btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
+	: m_dispatcher1(dispatcher),
+	  m_broadphasePairCache(pairCache),
+	  m_debugDrawer(0),
+	  m_forceUpdateAllAabbs(true)
 {
 }
 
-
 btCollisionWorld::~btCollisionWorld()
 {
-
 	//clean up remaining objects
 	int i;
-	for (i=0;i<m_collisionObjects.size();i++)
+	for (i = 0; i < m_collisionObjects.size(); i++)
 	{
-		btCollisionObject* collisionObject= m_collisionObjects[i];
+		btCollisionObject* collisionObject = m_collisionObjects[i];
 
 		btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
 		if (bp)
@@ -90,73 +86,83 @@ btCollisionWorld::~btCollisionWorld()
 			//
 			// only clear the cached algorithms
 			//
-			getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
-			getBroadphase()->destroyProxy(bp,m_dispatcher1);
+			getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp, m_dispatcher1);
+			getBroadphase()->destroyProxy(bp, m_dispatcher1);
 			collisionObject->setBroadphaseHandle(0);
 		}
 	}
-
-
 }
 
-
-
-
-
-
-
-
-
-
-void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
+void btCollisionWorld::refreshBroadphaseProxy(btCollisionObject* collisionObject)
 {
+	if (collisionObject->getBroadphaseHandle())
+	{
+		int collisionFilterGroup = collisionObject->getBroadphaseHandle()->m_collisionFilterGroup;
+		int collisionFilterMask = collisionObject->getBroadphaseHandle()->m_collisionFilterMask;
+
+		getBroadphase()->destroyProxy(collisionObject->getBroadphaseHandle(), getDispatcher());
+
+		//calculate new AABB
+		btTransform trans = collisionObject->getWorldTransform();
+
+		btVector3 minAabb;
+		btVector3 maxAabb;
+		collisionObject->getCollisionShape()->getAabb(trans, minAabb, maxAabb);
+
+		int type = collisionObject->getCollisionShape()->getShapeType();
+		collisionObject->setBroadphaseHandle(getBroadphase()->createProxy(
+			minAabb,
+			maxAabb,
+			type,
+			collisionObject,
+			collisionFilterGroup,
+			collisionFilterMask,
+			m_dispatcher1));
+	}
+}
 
+void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask)
+{
 	btAssert(collisionObject);
 
 	//check that the object isn't already added
-	btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
+	btAssert(m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size());
+	btAssert(collisionObject->getWorldArrayIndex() == -1);  // do not add the same object to more than one collision world
 
+	collisionObject->setWorldArrayIndex(m_collisionObjects.size());
 	m_collisionObjects.push_back(collisionObject);
 
 	//calculate new AABB
 	btTransform trans = collisionObject->getWorldTransform();
 
-	btVector3	minAabb;
-	btVector3	maxAabb;
-	collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
+	btVector3 minAabb;
+	btVector3 maxAabb;
+	collisionObject->getCollisionShape()->getAabb(trans, minAabb, maxAabb);
 
 	int type = collisionObject->getCollisionShape()->getShapeType();
-	collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
+	collisionObject->setBroadphaseHandle(getBroadphase()->createProxy(
 		minAabb,
 		maxAabb,
 		type,
 		collisionObject,
 		collisionFilterGroup,
 		collisionFilterMask,
-		m_dispatcher1,0
-		))	;
-
-
-
-
-
+		m_dispatcher1));
 }
 
-
-
-void	btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
+void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
 {
-	btVector3 minAabb,maxAabb;
-	colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+	btVector3 minAabb, maxAabb;
+	colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb);
 	//need to increase the aabb for contact thresholds
-	btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
+	btVector3 contactThreshold(gContactBreakingThreshold, gContactBreakingThreshold, gContactBreakingThreshold);
 	minAabb -= contactThreshold;
 	maxAabb += contactThreshold;
 
-	if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
+	if (getDispatchInfo().m_useContinuous && colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
 	{
-		btVector3 minAabb2,maxAabb2;
-		colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
+		btVector3 minAabb2, maxAabb2;
+		colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(), minAabb2, maxAabb2);
 		minAabb2 -= contactThreshold;
 		maxAabb2 += contactThreshold;
 		minAabb.setMin(minAabb2);
@@ -166,10 +172,11 @@ void	btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
 	btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
 
 	//moving objects should be moderately sized, probably something wrong if not
-	if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
+	if (colObj->isStaticObject() || ((maxAabb - minAabb).length2() < btScalar(1e12)))
 	{
-		bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
-	} else
+		bp->setAabb(colObj->getBroadphaseHandle(), minAabb, maxAabb, m_dispatcher1);
+	}
+	else
 	{
 		//something went wrong, investigate
 		//this assert is unwanted in 3D modelers (danger of loosing work)
@@ -187,14 +194,15 @@ void	btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
 	}
 }
 
-void	btCollisionWorld::updateAabbs()
+void btCollisionWorld::updateAabbs()
 {
 	BT_PROFILE("updateAabbs");
 
 	btTransform predictedTrans;
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
+		btAssert(colObj->getWorldArrayIndex() == i);
 
 		//only update aabb of active objects
 		if (m_forceUpdateAllAabbs || colObj->isActive())
@@ -204,14 +212,13 @@ void	btCollisionWorld::updateAabbs()
 	}
 }
 
-
-void	btCollisionWorld::computeOverlappingPairs()
+void btCollisionWorld::computeOverlappingPairs()
 {
 	BT_PROFILE("calculateOverlappingPairs");
 	m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
 }
 
-void	btCollisionWorld::performDiscreteCollisionDetection()
+void btCollisionWorld::performDiscreteCollisionDetection()
 {
 	BT_PROFILE("performDiscreteCollisionDetection");
 
@@ -225,53 +232,61 @@ void	btCollisionWorld::performDiscreteCollisionDetection()
 	{
 		BT_PROFILE("dispatchAllCollisionPairs");
 		if (dispatcher)
-			dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
+			dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(), dispatchInfo, m_dispatcher1);
 	}
-
 }
 
-
-
-void	btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
+void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
 {
-
-
 	//bool removeFromBroadphase = false;
 
 	{
-
 		btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
 		if (bp)
 		{
 			//
 			// only clear the cached algorithms
 			//
-			getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
-			getBroadphase()->destroyProxy(bp,m_dispatcher1);
+			getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp, m_dispatcher1);
+			getBroadphase()->destroyProxy(bp, m_dispatcher1);
 			collisionObject->setBroadphaseHandle(0);
 		}
 	}
 
-
-	//swapremove
-	m_collisionObjects.remove(collisionObject);
-
+	int iObj = collisionObject->getWorldArrayIndex();
+	//    btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously?
+	if (iObj >= 0 && iObj < m_collisionObjects.size())
+	{
+		btAssert(collisionObject == m_collisionObjects[iObj]);
+		m_collisionObjects.swap(iObj, m_collisionObjects.size() - 1);
+		m_collisionObjects.pop_back();
+		if (iObj < m_collisionObjects.size())
+		{
+			m_collisionObjects[iObj]->setWorldArrayIndex(iObj);
+		}
+	}
+	else
+	{
+		// slow linear search
+		//swapremove
+		m_collisionObjects.remove(collisionObject);
+	}
+	collisionObject->setWorldArrayIndex(-1);
 }
 
-
-void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
-										btCollisionObject* collisionObject,
-										const btCollisionShape* collisionShape,
-										const btTransform& colObjWorldTransform,
-										RayResultCallback& resultCallback)
+void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
+									 btCollisionObject* collisionObject,
+									 const btCollisionShape* collisionShape,
+									 const btTransform& colObjWorldTransform,
+									 RayResultCallback& resultCallback)
 {
-	btCollisionObjectWrapper colObWrap(0,collisionShape,collisionObject,colObjWorldTransform,-1,-1);
-	btCollisionWorld::rayTestSingleInternal(rayFromTrans,rayToTrans,&colObWrap,resultCallback);
+	btCollisionObjectWrapper colObWrap(0, collisionShape, collisionObject, colObjWorldTransform, -1, -1);
+	btCollisionWorld::rayTestSingleInternal(rayFromTrans, rayToTrans, &colObWrap, resultCallback);
 }
 
-void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,const btTransform& rayToTrans,
-										const btCollisionObjectWrapper* collisionObjectWrap,
-										RayResultCallback& resultCallback)
+void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans, const btTransform& rayToTrans,
+											 const btCollisionObjectWrapper* collisionObjectWrap,
+											 RayResultCallback& resultCallback)
 {
 	btSphereShape pointShape(btScalar(0.0));
 	pointShape.setMargin(0.f);
@@ -285,12 +300,12 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 		btConvexCast::CastResult castResult;
 		castResult.m_fraction = resultCallback.m_closestHitFraction;
 
-		btConvexShape* convexShape = (btConvexShape*) collisionShape;
-		btVoronoiSimplexSolver	simplexSolver;
-		btSubsimplexConvexCast subSimplexConvexCaster(castShape,convexShape,&simplexSolver);
-		
-		btGjkConvexCast	gjkConvexCaster(castShape,convexShape,&simplexSolver);
-		
+		btConvexShape* convexShape = (btConvexShape*)collisionShape;
+		btVoronoiSimplexSolver simplexSolver;
+		btSubsimplexConvexCast subSimplexConvexCaster(castShape, convexShape, &simplexSolver);
+
+		btGjkConvexCast gjkConvexCaster(castShape, convexShape, &simplexSolver);
+
 		//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
 
 		btConvexCast* convexCasterPtr = 0;
@@ -299,10 +314,10 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 			convexCasterPtr = &gjkConvexCaster;
 		else
 			convexCasterPtr = &subSimplexConvexCaster;
-		
+
 		btConvexCast& convexCaster = *convexCasterPtr;
 
-		if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
+		if (convexCaster.calcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
 		{
 			//add hit
 			if (castResult.m_normal.length2() > btScalar(0.0001))
@@ -313,81 +328,105 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
 					//rotate normal into worldspace
 					castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
-#endif //USE_SUBSIMPLEX_CONVEX_CAST
+#endif  //USE_SUBSIMPLEX_CONVEX_CAST
 
 					castResult.m_normal.normalize();
-					btCollisionWorld::LocalRayResult localRayResult
-						(
+					btCollisionWorld::LocalRayResult localRayResult(
 						collisionObjectWrap->getCollisionObject(),
 						0,
 						castResult.m_normal,
-						castResult.m_fraction
-						);
+						castResult.m_fraction);
 
 					bool normalInWorldSpace = true;
 					resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
-
 				}
 			}
 		}
-	} else {
+	}
+	else
+	{
 		if (collisionShape->isConcave())
 		{
-
 			//ConvexCast::CastResult
-				struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
+			struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
+			{
+				btCollisionWorld::RayResultCallback* m_resultCallback;
+				const btCollisionObject* m_collisionObject;
+				const btConcaveShape* m_triangleMesh;
+
+				btTransform m_colObjWorldTransform;
+
+				BridgeTriangleRaycastCallback(const btVector3& from, const btVector3& to,
+											  btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject, const btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform) :  //@BP Mod
+																																																							btTriangleRaycastCallback(from, to, resultCallback->m_flags),
+																																																							m_resultCallback(resultCallback),
+																																																							m_collisionObject(collisionObject),
+																																																							m_triangleMesh(triangleMesh),
+																																																							m_colObjWorldTransform(colObjWorldTransform)
 				{
-					btCollisionWorld::RayResultCallback* m_resultCallback;
-					const btCollisionObject*	m_collisionObject;
-					const btConcaveShape*	m_triangleMesh;
-
-					btTransform m_colObjWorldTransform;
-
-					BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
-					btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,const btConcaveShape*	triangleMesh,const btTransform& colObjWorldTransform):
-						//@BP Mod
-						btTriangleRaycastCallback(from,to, resultCallback->m_flags),
-							m_resultCallback(resultCallback),
-							m_collisionObject(collisionObject),
-							m_triangleMesh(triangleMesh),
-							m_colObjWorldTransform(colObjWorldTransform)
-						{
-						}
-
-
-					virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
-					{
-						btCollisionWorld::LocalShapeInfo	shapeInfo;
-						shapeInfo.m_shapePart = partId;
-						shapeInfo.m_triangleIndex = triangleIndex;
+				}
 
-						btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
+				virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex)
+				{
+					btCollisionWorld::LocalShapeInfo shapeInfo;
+					shapeInfo.m_shapePart = partId;
+					shapeInfo.m_triangleIndex = triangleIndex;
 
-						btCollisionWorld::LocalRayResult rayResult
-							(m_collisionObject,
-							&shapeInfo,
-							hitNormalWorld,
-							hitFraction);
+					btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
 
-						bool	normalInWorldSpace = true;
-						return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
-					}
+					btCollisionWorld::LocalRayResult rayResult(m_collisionObject,
+															   &shapeInfo,
+															   hitNormalWorld,
+															   hitFraction);
 
-				};
+					bool normalInWorldSpace = true;
+					return m_resultCallback->addSingleResult(rayResult, normalInWorldSpace);
+				}
+			};
 
 			btTransform worldTocollisionObject = colObjWorldTransform.inverse();
 			btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
 			btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
 
 			//			BT_PROFILE("rayTestConcave");
-			if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
+			if (collisionShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
 			{
 				///optimized version for btBvhTriangleMeshShape
 				btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
-				
-				BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),triangleMesh,colObjWorldTransform);
+
+				BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), triangleMesh, colObjWorldTransform);
 				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
-				triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
+				triangleMesh->performRaycast(&rcb, rayFromLocal, rayToLocal);
+			}
+			else if (collisionShape->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
+			{
+				///optimized version for btScaledBvhTriangleMeshShape
+				btScaledBvhTriangleMeshShape* scaledTriangleMesh = (btScaledBvhTriangleMeshShape*)collisionShape;
+				btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)scaledTriangleMesh->getChildShape();
+
+				//scale the ray positions
+				btVector3 scale = scaledTriangleMesh->getLocalScaling();
+				btVector3 rayFromLocalScaled = rayFromLocal / scale;
+				btVector3 rayToLocalScaled = rayToLocal / scale;
+
+				//perform raycast in the underlying btBvhTriangleMeshShape
+				BridgeTriangleRaycastCallback rcb(rayFromLocalScaled, rayToLocalScaled, &resultCallback, collisionObjectWrap->getCollisionObject(), triangleMesh, colObjWorldTransform);
+				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+				triangleMesh->performRaycast(&rcb, rayFromLocalScaled, rayToLocalScaled);
+			}
+			else if (((resultCallback.m_flags&btTriangleRaycastCallback::kF_DisableHeightfieldAccelerator)==0) 
+				&& collisionShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE 
+				)
+			{
+				///optimized version for btHeightfieldTerrainShape
+				btHeightfieldTerrainShape* heightField = (btHeightfieldTerrainShape*)collisionShape;
+				btTransform worldTocollisionObject = colObjWorldTransform.inverse();
+				btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
+				btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
+
+				BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), heightField, colObjWorldTransform);
+				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+				heightField->performRaycast(&rcb, rayFromLocal, rayToLocal);
 			}
 			else
 			{
@@ -404,45 +443,40 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 				struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
 				{
 					btCollisionWorld::RayResultCallback* m_resultCallback;
-					const btCollisionObject*	m_collisionObject;
-					btConcaveShape*	m_triangleMesh;
+					const btCollisionObject* m_collisionObject;
+					btConcaveShape* m_triangleMesh;
 
 					btTransform m_colObjWorldTransform;
 
-					BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
-						btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape*	triangleMesh, const btTransform& colObjWorldTransform):
-					//@BP Mod
-					btTriangleRaycastCallback(from,to, resultCallback->m_flags),
-						m_resultCallback(resultCallback),
-						m_collisionObject(collisionObject),
-						m_triangleMesh(triangleMesh),
-						m_colObjWorldTransform(colObjWorldTransform)
+					BridgeTriangleRaycastCallback(const btVector3& from, const btVector3& to,
+												  btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject, btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform) :  //@BP Mod
+																																																						  btTriangleRaycastCallback(from, to, resultCallback->m_flags),
+																																																						  m_resultCallback(resultCallback),
+																																																						  m_collisionObject(collisionObject),
+																																																						  m_triangleMesh(triangleMesh),
+																																																						  m_colObjWorldTransform(colObjWorldTransform)
 					{
 					}
 
-
-					virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
+					virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex)
 					{
-						btCollisionWorld::LocalShapeInfo	shapeInfo;
+						btCollisionWorld::LocalShapeInfo shapeInfo;
 						shapeInfo.m_shapePart = partId;
 						shapeInfo.m_triangleIndex = triangleIndex;
 
 						btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
 
-						btCollisionWorld::LocalRayResult rayResult
-							(m_collisionObject,
-							&shapeInfo,
-							hitNormalWorld,
-							hitFraction);
+						btCollisionWorld::LocalRayResult rayResult(m_collisionObject,
+																   &shapeInfo,
+																   hitNormalWorld,
+																   hitFraction);
 
-						bool	normalInWorldSpace = true;
-						return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
+						bool normalInWorldSpace = true;
+						return m_resultCallback->addSingleResult(rayResult, normalInWorldSpace);
 					}
-
 				};
 
-
-				BridgeTriangleRaycastCallback	rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform);
+				BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), concaveShape, colObjWorldTransform);
 				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
 
 				btVector3 rayAabbMinLocal = rayFromLocal;
@@ -450,9 +484,11 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 				btVector3 rayAabbMaxLocal = rayFromLocal;
 				rayAabbMaxLocal.setMax(rayToLocal);
 
-				concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
+				concaveShape->processAllTriangles(&rcb, rayAabbMinLocal, rayAabbMaxLocal);
 			}
-		} else {
+		}
+		else
+		{
 			//			BT_PROFILE("rayTestCompound");
 			if (collisionShape->isCompound())
 			{
@@ -460,10 +496,10 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 				{
 					RayResultCallback* m_userCallback;
 					int m_i;
-					
-					LocalInfoAdder2 (int i, RayResultCallback *user)
+
+					LocalInfoAdder2(int i, RayResultCallback* user)
 						: m_userCallback(user), m_i(i)
-					{ 
+					{
 						m_closestHitFraction = m_userCallback->m_closestHitFraction;
 						m_flags = m_userCallback->m_flags;
 					}
@@ -472,7 +508,7 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 						return m_userCallback->needsCollision(p);
 					}
 
-					virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
+					virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& r, bool b)
 					{
 						btCollisionWorld::LocalShapeInfo shapeInfo;
 						shapeInfo.m_shapePart = -1;
@@ -485,7 +521,7 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 						return result;
 					}
 				};
-				
+
 				struct RayTester : btDbvt::ICollide
 				{
 					const btCollisionObject* m_collisionObject;
@@ -494,33 +530,29 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 					const btTransform& m_rayFromTrans;
 					const btTransform& m_rayToTrans;
 					RayResultCallback& m_resultCallback;
-					
+
 					RayTester(const btCollisionObject* collisionObject,
-							const btCompoundShape* compoundShape,
-							const btTransform& colObjWorldTransform,
-							const btTransform& rayFromTrans,
-							const btTransform& rayToTrans,
-							RayResultCallback& resultCallback):
-						m_collisionObject(collisionObject),
-						m_compoundShape(compoundShape),
-						m_colObjWorldTransform(colObjWorldTransform),
-						m_rayFromTrans(rayFromTrans),
-						m_rayToTrans(rayToTrans),
-						m_resultCallback(resultCallback)
+							  const btCompoundShape* compoundShape,
+							  const btTransform& colObjWorldTransform,
+							  const btTransform& rayFromTrans,
+							  const btTransform& rayToTrans,
+							  RayResultCallback& resultCallback) : m_collisionObject(collisionObject),
+																   m_compoundShape(compoundShape),
+																   m_colObjWorldTransform(colObjWorldTransform),
+																   m_rayFromTrans(rayFromTrans),
+																   m_rayToTrans(rayToTrans),
+																   m_resultCallback(resultCallback)
 					{
-						
 					}
-					
+
 					void ProcessLeaf(int i)
 					{
 						const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i);
 						const btTransform& childTrans = m_compoundShape->getChildTransform(i);
 						btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
-						
-						btCollisionObjectWrapper tmpOb(0,childCollisionShape,m_collisionObject,childWorldTrans,-1,i);
-						// replace collision shape so that callback can determine the triangle
 
-						
+						btCollisionObjectWrapper tmpOb(0, childCollisionShape, m_collisionObject, childWorldTrans, -1, i);
+						// replace collision shape so that callback can determine the triangle
 
 						LocalInfoAdder2 my_cb(i, &m_resultCallback);
 
@@ -529,19 +561,17 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 							m_rayToTrans,
 							&tmpOb,
 							my_cb);
-						
 					}
-				
+
 					void Process(const btDbvtNode* leaf)
 					{
 						ProcessLeaf(leaf->dataAsInt);
 					}
 				};
-				
+
 				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
 				const btDbvt* dbvt = compoundShape->getDynamicAabbTree();
 
-
 				RayTester rayCB(
 					collisionObjectWrap->getCollisionObject(),
 					compoundShape,
@@ -549,39 +579,39 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 					rayFromTrans,
 					rayToTrans,
 					resultCallback);
-#ifndef	DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
+#ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
 				if (dbvt)
 				{
 					btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin();
 					btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin();
-					btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB);
+					btDbvt::rayTest(dbvt->m_root, localRayFrom, localRayTo, rayCB);
 				}
 				else
-#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
+#endif  //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
 				{
 					for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i)
 					{
 						rayCB.ProcessLeaf(i);
-					}	
+					}
 				}
 			}
 		}
 	}
 }
 
-void	btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
-											btCollisionObject* collisionObject,
-											const btCollisionShape* collisionShape,
-											const btTransform& colObjWorldTransform,
-											ConvexResultCallback& resultCallback, btScalar allowedPenetration)
+void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans,
+										 btCollisionObject* collisionObject,
+										 const btCollisionShape* collisionShape,
+										 const btTransform& colObjWorldTransform,
+										 ConvexResultCallback& resultCallback, btScalar allowedPenetration)
 {
-	btCollisionObjectWrapper tmpOb(0,collisionShape,collisionObject,colObjWorldTransform,-1,-1);
-	btCollisionWorld::objectQuerySingleInternal(castShape,convexFromTrans,convexToTrans,&tmpOb,resultCallback,allowedPenetration);
+	btCollisionObjectWrapper tmpOb(0, collisionShape, collisionObject, colObjWorldTransform, -1, -1);
+	btCollisionWorld::objectQuerySingleInternal(castShape, convexFromTrans, convexToTrans, &tmpOb, resultCallback, allowedPenetration);
 }
 
-void	btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
-											const btCollisionObjectWrapper* colObjWrap,
-											ConvexResultCallback& resultCallback, btScalar allowedPenetration)
+void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans,
+												 const btCollisionObjectWrapper* colObjWrap,
+												 ConvexResultCallback& resultCallback, btScalar allowedPenetration)
 {
 	const btCollisionShape* collisionShape = colObjWrap->getCollisionShape();
 	const btTransform& colObjWorldTransform = colObjWrap->getWorldTransform();
@@ -591,21 +621,19 @@ void	btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,
 		//BT_PROFILE("convexSweepConvex");
 		btConvexCast::CastResult castResult;
 		castResult.m_allowedPenetration = allowedPenetration;
-		castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
+		castResult.m_fraction = resultCallback.m_closestHitFraction;  //btScalar(1.);//??
 
-		btConvexShape* convexShape = (btConvexShape*) collisionShape;
-		btVoronoiSimplexSolver	simplexSolver;
-		btGjkEpaPenetrationDepthSolver	gjkEpaPenetrationSolver;
+		btConvexShape* convexShape = (btConvexShape*)collisionShape;
+		btVoronoiSimplexSolver simplexSolver;
+		btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
 
-		btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
+		btContinuousConvexCollision convexCaster1(castShape, convexShape, &simplexSolver, &gjkEpaPenetrationSolver);
 		//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
 		//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
 
 		btConvexCast* castPtr = &convexCaster1;
 
-
-
-		if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
+		if (castPtr->calcTimeOfImpact(convexFromTrans, convexToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
 		{
 			//add hit
 			if (castResult.m_normal.length2() > btScalar(0.0001))
@@ -613,25 +641,24 @@ void	btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,
 				if (castResult.m_fraction < resultCallback.m_closestHitFraction)
 				{
 					castResult.m_normal.normalize();
-					btCollisionWorld::LocalConvexResult localConvexResult
-						(
+					btCollisionWorld::LocalConvexResult localConvexResult(
 						colObjWrap->getCollisionObject(),
 						0,
 						castResult.m_normal,
 						castResult.m_hitPoint,
-						castResult.m_fraction
-						);
+						castResult.m_fraction);
 
 					bool normalInWorldSpace = true;
 					resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
-
 				}
 			}
 		}
-	} else {
+	}
+	else
+	{
 		if (collisionShape->isConcave())
 		{
-			if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
+			if (collisionShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
 			{
 				//BT_PROFILE("convexSweepbtBvhTriangleMesh");
 				btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
@@ -645,62 +672,57 @@ void	btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,
 				struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
 				{
 					btCollisionWorld::ConvexResultCallback* m_resultCallback;
-					const btCollisionObject*	m_collisionObject;
-					btTriangleMeshShape*	m_triangleMesh;
-
-					BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
-						btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btTriangleMeshShape*	triangleMesh, const btTransform& triangleToWorld):
-					btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
-						m_resultCallback(resultCallback),
-						m_collisionObject(collisionObject),
-						m_triangleMesh(triangleMesh)
+					const btCollisionObject* m_collisionObject;
+					btTriangleMeshShape* m_triangleMesh;
+
+					BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from, const btTransform& to,
+													 btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject, btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld) : btTriangleConvexcastCallback(castShape, from, to, triangleToWorld, triangleMesh->getMargin()),
+																																																								m_resultCallback(resultCallback),
+																																																								m_collisionObject(collisionObject),
+																																																								m_triangleMesh(triangleMesh)
 					{
 					}
 
-
-					virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
+					virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex)
 					{
-						btCollisionWorld::LocalShapeInfo	shapeInfo;
+						btCollisionWorld::LocalShapeInfo shapeInfo;
 						shapeInfo.m_shapePart = partId;
 						shapeInfo.m_triangleIndex = triangleIndex;
 						if (hitFraction <= m_resultCallback->m_closestHitFraction)
 						{
+							btCollisionWorld::LocalConvexResult convexResult(m_collisionObject,
+																			 &shapeInfo,
+																			 hitNormalLocal,
+																			 hitPointLocal,
+																			 hitFraction);
 
-							btCollisionWorld::LocalConvexResult convexResult
-								(m_collisionObject,
-								&shapeInfo,
-								hitNormalLocal,
-								hitPointLocal,
-								hitFraction);
-
-							bool	normalInWorldSpace = true;
+							bool normalInWorldSpace = true;
 
-
-							return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
+							return m_resultCallback->addSingleResult(convexResult, normalInWorldSpace);
 						}
 						return hitFraction;
 					}
-
 				};
 
-				BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),triangleMesh, colObjWorldTransform);
+				BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans, convexToTrans, &resultCallback, colObjWrap->getCollisionObject(), triangleMesh, colObjWorldTransform);
 				tccb.m_hitFraction = resultCallback.m_closestHitFraction;
 				tccb.m_allowedPenetration = allowedPenetration;
 				btVector3 boxMinLocal, boxMaxLocal;
 				castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
-				triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
-			} else
+				triangleMesh->performConvexcast(&tccb, convexFromLocal, convexToLocal, boxMinLocal, boxMaxLocal);
+			}
+			else
 			{
-				if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE)
+				if (collisionShape->getShapeType() == STATIC_PLANE_PROXYTYPE)
 				{
 					btConvexCast::CastResult castResult;
 					castResult.m_allowedPenetration = allowedPenetration;
 					castResult.m_fraction = resultCallback.m_closestHitFraction;
-					btStaticPlaneShape* planeShape = (btStaticPlaneShape*) collisionShape;
-					btContinuousConvexCollision convexCaster1(castShape,planeShape);
+					btStaticPlaneShape* planeShape = (btStaticPlaneShape*)collisionShape;
+					btContinuousConvexCollision convexCaster1(castShape, planeShape);
 					btConvexCast* castPtr = &convexCaster1;
 
-					if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
+					if (castPtr->calcTimeOfImpact(convexFromTrans, convexToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
 					{
 						//add hit
 						if (castResult.m_normal.length2() > btScalar(0.0001))
@@ -708,22 +730,20 @@ void	btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,
 							if (castResult.m_fraction < resultCallback.m_closestHitFraction)
 							{
 								castResult.m_normal.normalize();
-								btCollisionWorld::LocalConvexResult localConvexResult
-									(
+								btCollisionWorld::LocalConvexResult localConvexResult(
 									colObjWrap->getCollisionObject(),
 									0,
 									castResult.m_normal,
 									castResult.m_hitPoint,
-									castResult.m_fraction
-									);
+									castResult.m_fraction);
 
 								bool normalInWorldSpace = true;
 								resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
 							}
 						}
 					}
-
-				} else
+				}
+				else
 				{
 					//BT_PROFILE("convexSweepConcave");
 					btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
@@ -737,44 +757,39 @@ void	btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,
 					struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
 					{
 						btCollisionWorld::ConvexResultCallback* m_resultCallback;
-						const btCollisionObject*	m_collisionObject;
-						btConcaveShape*	m_triangleMesh;
-
-						BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
-							btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape*	triangleMesh, const btTransform& triangleToWorld):
-						btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
-							m_resultCallback(resultCallback),
-							m_collisionObject(collisionObject),
-							m_triangleMesh(triangleMesh)
+						const btCollisionObject* m_collisionObject;
+						btConcaveShape* m_triangleMesh;
+
+						BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from, const btTransform& to,
+														 btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject, btConcaveShape* triangleMesh, const btTransform& triangleToWorld) : btTriangleConvexcastCallback(castShape, from, to, triangleToWorld, triangleMesh->getMargin()),
+																																																							   m_resultCallback(resultCallback),
+																																																							   m_collisionObject(collisionObject),
+																																																							   m_triangleMesh(triangleMesh)
 						{
 						}
 
-
-						virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
+						virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex)
 						{
-							btCollisionWorld::LocalShapeInfo	shapeInfo;
+							btCollisionWorld::LocalShapeInfo shapeInfo;
 							shapeInfo.m_shapePart = partId;
 							shapeInfo.m_triangleIndex = triangleIndex;
 							if (hitFraction <= m_resultCallback->m_closestHitFraction)
 							{
+								btCollisionWorld::LocalConvexResult convexResult(m_collisionObject,
+																				 &shapeInfo,
+																				 hitNormalLocal,
+																				 hitPointLocal,
+																				 hitFraction);
 
-								btCollisionWorld::LocalConvexResult convexResult
-									(m_collisionObject,
-									&shapeInfo,
-									hitNormalLocal,
-									hitPointLocal,
-									hitFraction);
-
-								bool	normalInWorldSpace = true;
+								bool normalInWorldSpace = true;
 
-								return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
+								return m_resultCallback->addSingleResult(convexResult, normalInWorldSpace);
 							}
 							return hitFraction;
 						}
-
 					};
 
-					BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),concaveShape, colObjWorldTransform);
+					BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans, convexToTrans, &resultCallback, colObjWrap->getCollisionObject(), concaveShape, colObjWorldTransform);
 					tccb.m_hitFraction = resultCallback.m_closestHitFraction;
 					tccb.m_allowedPenetration = allowedPenetration;
 					btVector3 boxMinLocal, boxMaxLocal;
@@ -786,27 +801,56 @@ void	btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,
 					rayAabbMaxLocal.setMax(convexToLocal);
 					rayAabbMinLocal += boxMinLocal;
 					rayAabbMaxLocal += boxMaxLocal;
-					concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
+					concaveShape->processAllTriangles(&tccb, rayAabbMinLocal, rayAabbMaxLocal);
 				}
 			}
-		} else {
-			///@todo : use AABB tree or other BVH acceleration structure!
+		}
+		else
+		{
 			if (collisionShape->isCompound())
 			{
-				BT_PROFILE("convexSweepCompound");
-				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
-				int i=0;
-				for (i=0;i<compoundShape->getNumChildShapes();i++)
+				struct btCompoundLeafCallback : btDbvt::ICollide
 				{
-					btTransform childTrans = compoundShape->getChildTransform(i);
-					const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
-					btTransform childWorldTrans = colObjWorldTransform * childTrans;
-					
-                    struct	LocalInfoAdder : public ConvexResultCallback {
-                            ConvexResultCallback* m_userCallback;
+					btCompoundLeafCallback(
+						const btCollisionObjectWrapper* colObjWrap,
+						const btConvexShape* castShape,
+						const btTransform& convexFromTrans,
+						const btTransform& convexToTrans,
+						btScalar allowedPenetration,
+						const btCompoundShape* compoundShape,
+						const btTransform& colObjWorldTransform,
+						ConvexResultCallback& resultCallback)
+						: m_colObjWrap(colObjWrap),
+						  m_castShape(castShape),
+						  m_convexFromTrans(convexFromTrans),
+						  m_convexToTrans(convexToTrans),
+						  m_allowedPenetration(allowedPenetration),
+						  m_compoundShape(compoundShape),
+						  m_colObjWorldTransform(colObjWorldTransform),
+						  m_resultCallback(resultCallback)
+					{
+					}
+
+					const btCollisionObjectWrapper* m_colObjWrap;
+					const btConvexShape* m_castShape;
+					const btTransform& m_convexFromTrans;
+					const btTransform& m_convexToTrans;
+					btScalar m_allowedPenetration;
+					const btCompoundShape* m_compoundShape;
+					const btTransform& m_colObjWorldTransform;
+					ConvexResultCallback& m_resultCallback;
+
+				public:
+					void ProcessChild(int index, const btTransform& childTrans, const btCollisionShape* childCollisionShape)
+					{
+						btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
+
+						struct LocalInfoAdder : public ConvexResultCallback
+						{
+							ConvexResultCallback* m_userCallback;
 							int m_i;
 
-                            LocalInfoAdder (int i, ConvexResultCallback *user)
+							LocalInfoAdder(int i, ConvexResultCallback* user)
 								: m_userCallback(user), m_i(i)
 							{
 								m_closestHitFraction = m_userCallback->m_closestHitFraction;
@@ -815,60 +859,99 @@ void	btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,
 							{
 								return m_userCallback->needsCollision(p);
 							}
-                            virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult&	r,	bool b)
-                            {
-                                    btCollisionWorld::LocalShapeInfo	shapeInfo;
-                                    shapeInfo.m_shapePart = -1;
-                                    shapeInfo.m_triangleIndex = m_i;
-                                    if (r.m_localShapeInfo == NULL)
-                                        r.m_localShapeInfo = &shapeInfo;
-									const btScalar result = m_userCallback->addSingleResult(r, b);
-									m_closestHitFraction = m_userCallback->m_closestHitFraction;
-									return result;
-                                    
-                            }
-                    };
-
-                    LocalInfoAdder my_cb(i, &resultCallback);
-					
-					btCollisionObjectWrapper tmpObj(colObjWrap,childCollisionShape,colObjWrap->getCollisionObject(),childWorldTrans,-1,i);
-
-					objectQuerySingleInternal(castShape, convexFromTrans,convexToTrans,
-						&tmpObj,my_cb, allowedPenetration);
-					
+							virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& r, bool b)
+							{
+								btCollisionWorld::LocalShapeInfo shapeInfo;
+								shapeInfo.m_shapePart = -1;
+								shapeInfo.m_triangleIndex = m_i;
+								if (r.m_localShapeInfo == NULL)
+									r.m_localShapeInfo = &shapeInfo;
+								const btScalar result = m_userCallback->addSingleResult(r, b);
+								m_closestHitFraction = m_userCallback->m_closestHitFraction;
+								return result;
+							}
+						};
+
+						LocalInfoAdder my_cb(index, &m_resultCallback);
+
+						btCollisionObjectWrapper tmpObj(m_colObjWrap, childCollisionShape, m_colObjWrap->getCollisionObject(), childWorldTrans, -1, index);
+
+						objectQuerySingleInternal(m_castShape, m_convexFromTrans, m_convexToTrans, &tmpObj, my_cb, m_allowedPenetration);
+					}
+
+					void Process(const btDbvtNode* leaf)
+					{
+						// Processing leaf node
+						int index = leaf->dataAsInt;
+
+						btTransform childTrans = m_compoundShape->getChildTransform(index);
+						const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(index);
+
+						ProcessChild(index, childTrans, childCollisionShape);
+					}
+				};
+
+				BT_PROFILE("convexSweepCompound");
+				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
+
+				btVector3 fromLocalAabbMin, fromLocalAabbMax;
+				btVector3 toLocalAabbMin, toLocalAabbMax;
+
+				castShape->getAabb(colObjWorldTransform.inverse() * convexFromTrans, fromLocalAabbMin, fromLocalAabbMax);
+				castShape->getAabb(colObjWorldTransform.inverse() * convexToTrans, toLocalAabbMin, toLocalAabbMax);
+
+				fromLocalAabbMin.setMin(toLocalAabbMin);
+				fromLocalAabbMax.setMax(toLocalAabbMax);
+
+				btCompoundLeafCallback callback(colObjWrap, castShape, convexFromTrans, convexToTrans,
+												allowedPenetration, compoundShape, colObjWorldTransform, resultCallback);
+
+				const btDbvt* tree = compoundShape->getDynamicAabbTree();
+				if (tree)
+				{
+					const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(fromLocalAabbMin, fromLocalAabbMax);
+					tree->collideTV(tree->m_root, bounds, callback);
+				}
+				else
+				{
+					int i;
+					for (i = 0; i < compoundShape->getNumChildShapes(); i++)
+					{
+						const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
+						btTransform childTrans = compoundShape->getChildTransform(i);
+						callback.ProcessChild(i, childTrans, childCollisionShape);
+					}
 				}
 			}
 		}
 	}
 }
 
-
 struct btSingleRayCallback : public btBroadphaseRayCallback
 {
-
-	btVector3	m_rayFromWorld;
-	btVector3	m_rayToWorld;
-	btTransform	m_rayFromTrans;
-	btTransform	m_rayToTrans;
-	btVector3	m_hitNormal;
-
-	const btCollisionWorld*	m_world;
-	btCollisionWorld::RayResultCallback&	m_resultCallback;
-
-	btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
-		:m_rayFromWorld(rayFromWorld),
-		m_rayToWorld(rayToWorld),
-		m_world(world),
-		m_resultCallback(resultCallback)
+	btVector3 m_rayFromWorld;
+	btVector3 m_rayToWorld;
+	btTransform m_rayFromTrans;
+	btTransform m_rayToTrans;
+	btVector3 m_hitNormal;
+
+	const btCollisionWorld* m_world;
+	btCollisionWorld::RayResultCallback& m_resultCallback;
+
+	btSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btCollisionWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
+		: m_rayFromWorld(rayFromWorld),
+		  m_rayToWorld(rayToWorld),
+		  m_world(world),
+		  m_resultCallback(resultCallback)
 	{
 		m_rayFromTrans.setIdentity();
 		m_rayFromTrans.setOrigin(m_rayFromWorld);
 		m_rayToTrans.setIdentity();
 		m_rayToTrans.setOrigin(m_rayToWorld);
 
-		btVector3 rayDir = (rayToWorld-rayFromWorld);
+		btVector3 rayDir = (rayToWorld - rayFromWorld);
 
-		rayDir.normalize ();
+		rayDir.normalize();
 		///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
 		m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
 		m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
@@ -877,22 +960,19 @@ struct btSingleRayCallback : public btBroadphaseRayCallback
 		m_signs[1] = m_rayDirectionInverse[1] < 0.0;
 		m_signs[2] = m_rayDirectionInverse[2] < 0.0;
 
-		m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
-
+		m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
 	}
 
-
-
-	virtual bool	process(const btBroadphaseProxy* proxy)
+	virtual bool process(const btBroadphaseProxy* proxy)
 	{
 		///terminate further ray tests, once the closestHitFraction reached zero
 		if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
 			return false;
 
-		btCollisionObject*	collisionObject = (btCollisionObject*)proxy->m_clientObject;
+		btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
 
 		//only perform raycast if filterMask matches
-		if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
+		if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
 		{
 			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
 			//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
@@ -910,57 +990,53 @@ struct btSingleRayCallback : public btBroadphaseRayCallback
 			//culling already done by broadphase
 			//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
 			{
-				m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
-					collisionObject,
-					collisionObject->getCollisionShape(),
-					collisionObject->getWorldTransform(),
-					m_resultCallback);
+				m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
+									   collisionObject,
+									   collisionObject->getCollisionShape(),
+									   collisionObject->getWorldTransform(),
+									   m_resultCallback);
 			}
 		}
 		return true;
 	}
 };
 
-void	btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
+void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
 {
 	//BT_PROFILE("rayTest");
 	/// use the broadphase to accelerate the search for objects, based on their aabb
 	/// and for each object with ray-aabb overlap, perform an exact ray test
-	btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
+	btSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback);
 
 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
-	m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
+	m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
 #else
-	for (int i=0;i<this->getNumCollisionObjects();i++)
+	for (int i = 0; i < this->getNumCollisionObjects(); i++)
 	{
 		rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
-	}	
-#endif //USE_BRUTEFORCE_RAYBROADPHASE
-
+	}
+#endif  //USE_BRUTEFORCE_RAYBROADPHASE
 }
 
-
 struct btSingleSweepCallback : public btBroadphaseRayCallback
 {
-
-	btTransform	m_convexFromTrans;
-	btTransform	m_convexToTrans;
-	btVector3	m_hitNormal;
-	const btCollisionWorld*	m_world;
-	btCollisionWorld::ConvexResultCallback&	m_resultCallback;
-	btScalar	m_allowedCcdPenetration;
+	btTransform m_convexFromTrans;
+	btTransform m_convexToTrans;
+	btVector3 m_hitNormal;
+	const btCollisionWorld* m_world;
+	btCollisionWorld::ConvexResultCallback& m_resultCallback;
+	btScalar m_allowedCcdPenetration;
 	const btConvexShape* m_castShape;
 
-
-	btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration)
-		:m_convexFromTrans(convexFromTrans),
-		m_convexToTrans(convexToTrans),
-		m_world(world),
-		m_resultCallback(resultCallback),
-		m_allowedCcdPenetration(allowedPenetration),
-		m_castShape(castShape)
+	btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans, const btCollisionWorld* world, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedPenetration)
+		: m_convexFromTrans(convexFromTrans),
+		  m_convexToTrans(convexToTrans),
+		  m_world(world),
+		  m_resultCallback(resultCallback),
+		  m_allowedCcdPenetration(allowedPenetration),
+		  m_castShape(castShape)
 	{
-		btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
+		btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin() - m_convexFromTrans.getOrigin());
 		btVector3 rayDir = unnormalizedRayDir.normalized();
 		///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
 		m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
@@ -971,109 +1047,102 @@ struct btSingleSweepCallback : public btBroadphaseRayCallback
 		m_signs[2] = m_rayDirectionInverse[2] < 0.0;
 
 		m_lambda_max = rayDir.dot(unnormalizedRayDir);
-
 	}
 
-	virtual bool	process(const btBroadphaseProxy* proxy)
+	virtual bool process(const btBroadphaseProxy* proxy)
 	{
 		///terminate further convex sweep tests, once the closestHitFraction reached zero
 		if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
 			return false;
 
-		btCollisionObject*	collisionObject = (btCollisionObject*)proxy->m_clientObject;
+		btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
 
 		//only perform raycast if filterMask matches
-		if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
+		if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
+		{
 			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
-			m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
-				collisionObject,
-				collisionObject->getCollisionShape(),
-				collisionObject->getWorldTransform(),
-				m_resultCallback,
-				m_allowedCcdPenetration);
+			m_world->objectQuerySingle(m_castShape, m_convexFromTrans, m_convexToTrans,
+									   collisionObject,
+									   collisionObject->getCollisionShape(),
+									   collisionObject->getWorldTransform(),
+									   m_resultCallback,
+									   m_allowedCcdPenetration);
 		}
 
 		return true;
 	}
 };
 
-
-
-void	btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
+void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
 {
-
 	BT_PROFILE("convexSweepTest");
 	/// use the broadphase to accelerate the search for objects, based on their aabb
 	/// and for each object with ray-aabb overlap, perform an exact ray test
 	/// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
 
-
-
-	btTransform	convexFromTrans,convexToTrans;
+	btTransform convexFromTrans, convexToTrans;
 	convexFromTrans = convexFromWorld;
 	convexToTrans = convexToWorld;
 	btVector3 castShapeAabbMin, castShapeAabbMax;
 	/* Compute AABB that encompasses angular movement */
 	{
 		btVector3 linVel, angVel;
-		btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0f, linVel, angVel);
+		btTransformUtil::calculateVelocity(convexFromTrans, convexToTrans, 1.0f, linVel, angVel);
 		btVector3 zeroLinVel;
-		zeroLinVel.setValue(0,0,0);
+		zeroLinVel.setValue(0, 0, 0);
 		btTransform R;
-		R.setIdentity ();
-		R.setRotation (convexFromTrans.getRotation());
-		castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0f, castShapeAabbMin, castShapeAabbMax);
+		R.setIdentity();
+		R.setRotation(convexFromTrans.getRotation());
+		castShape->calculateTemporalAabb(R, zeroLinVel, angVel, 1.0f, castShapeAabbMin, castShapeAabbMax);
 	}
 
 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
 
-	btSingleSweepCallback	convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration);
+	btSingleSweepCallback convexCB(castShape, convexFromWorld, convexToWorld, this, resultCallback, allowedCcdPenetration);
 
-	m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax);
+	m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(), convexToTrans.getOrigin(), convexCB, castShapeAabbMin, castShapeAabbMax);
 
 #else
 	/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
 	// do a ray-shape query using convexCaster (CCD)
 	int i;
-	for (i=0;i<m_collisionObjects.size();i++)
+	for (i = 0; i < m_collisionObjects.size(); i++)
 	{
-		btCollisionObject*	collisionObject= m_collisionObjects[i];
+		btCollisionObject* collisionObject = m_collisionObjects[i];
 		//only perform raycast if filterMask matches
-		if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
+		if (resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
+		{
 			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
-			btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
-			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
-			AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
-			btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
+			btVector3 collisionObjectAabbMin, collisionObjectAabbMax;
+			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(), collisionObjectAabbMin, collisionObjectAabbMax);
+			AabbExpand(collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
+			btScalar hitLambda = btScalar(1.);  //could use resultCallback.m_closestHitFraction, but needs testing
 			btVector3 hitNormal;
-			if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
+			if (btRayAabb(convexFromWorld.getOrigin(), convexToWorld.getOrigin(), collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal))
 			{
-				objectQuerySingle(castShape, convexFromTrans,convexToTrans,
-					collisionObject,
-					collisionObject->getCollisionShape(),
-					collisionObject->getWorldTransform(),
-					resultCallback,
-					allowedCcdPenetration);
+				objectQuerySingle(castShape, convexFromTrans, convexToTrans,
+								  collisionObject,
+								  collisionObject->getCollisionShape(),
+								  collisionObject->getWorldTransform(),
+								  resultCallback,
+								  allowedCcdPenetration);
 			}
 		}
 	}
-#endif //USE_BRUTEFORCE_RAYBROADPHASE
+#endif  //USE_BRUTEFORCE_RAYBROADPHASE
 }
 
-
-
 struct btBridgedManifoldResult : public btManifoldResult
 {
+	btCollisionWorld::ContactResultCallback& m_resultCallback;
 
-	btCollisionWorld::ContactResultCallback&	m_resultCallback;
-
-	btBridgedManifoldResult( const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap,btCollisionWorld::ContactResultCallback& resultCallback )
-		:btManifoldResult(obj0Wrap,obj1Wrap),
-		m_resultCallback(resultCallback)
+	btBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap, const btCollisionObjectWrapper* obj1Wrap, btCollisionWorld::ContactResultCallback& resultCallback)
+		: btManifoldResult(obj0Wrap, obj1Wrap),
+		  m_resultCallback(resultCallback)
 	{
 	}
 
-	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+	virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
 	{
 		bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
 		btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
@@ -1081,78 +1150,74 @@ struct btBridgedManifoldResult : public btManifoldResult
 		btVector3 localB;
 		if (isSwapped)
 		{
-			localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
+			localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA);
 			localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
-		} else
+		}
+		else
 		{
-			localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
+			localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA);
 			localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
 		}
-		
-		btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
+
+		btManifoldPoint newPt(localA, localB, normalOnBInWorld, depth);
 		newPt.m_positionWorldOnA = pointA;
 		newPt.m_positionWorldOnB = pointInWorld;
-		
-	   //BP mod, store contact triangles.
+
+		//BP mod, store contact triangles.
 		if (isSwapped)
 		{
 			newPt.m_partId0 = m_partId1;
 			newPt.m_partId1 = m_partId0;
-			newPt.m_index0  = m_index1;
-			newPt.m_index1  = m_index0;
-		} else
+			newPt.m_index0 = m_index1;
+			newPt.m_index1 = m_index0;
+		}
+		else
 		{
 			newPt.m_partId0 = m_partId0;
 			newPt.m_partId1 = m_partId1;
-			newPt.m_index0  = m_index0;
-			newPt.m_index1  = m_index1;
+			newPt.m_index0 = m_index0;
+			newPt.m_index1 = m_index1;
 		}
 
 		//experimental feature info, for per-triangle material etc.
-		const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap;
-		const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap;
-		m_resultCallback.addSingleResult(newPt,obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1);
-
+		const btCollisionObjectWrapper* obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap;
+		const btCollisionObjectWrapper* obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap;
+		m_resultCallback.addSingleResult(newPt, obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1);
 	}
-	
 };
 
-
-
 struct btSingleContactCallback : public btBroadphaseAabbCallback
 {
-
 	btCollisionObject* m_collisionObject;
-	btCollisionWorld*	m_world;
-	btCollisionWorld::ContactResultCallback&	m_resultCallback;
-	
-	
-	btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback)
-		:m_collisionObject(collisionObject),
-		m_world(world),
-		m_resultCallback(resultCallback)
+	btCollisionWorld* m_world;
+	btCollisionWorld::ContactResultCallback& m_resultCallback;
+
+	btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world, btCollisionWorld::ContactResultCallback& resultCallback)
+		: m_collisionObject(collisionObject),
+		  m_world(world),
+		  m_resultCallback(resultCallback)
 	{
 	}
 
-	virtual bool	process(const btBroadphaseProxy* proxy)
+	virtual bool process(const btBroadphaseProxy* proxy)
 	{
-		btCollisionObject*	collisionObject = (btCollisionObject*)proxy->m_clientObject;
+		btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
 		if (collisionObject == m_collisionObject)
 			return true;
 
 		//only perform raycast if filterMask matches
-		if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
+		if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
 		{
-			btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform(),-1,-1);
-			btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform(),-1,-1);
+			btCollisionObjectWrapper ob0(0, m_collisionObject->getCollisionShape(), m_collisionObject, m_collisionObject->getWorldTransform(), -1, -1);
+			btCollisionObjectWrapper ob1(0, collisionObject->getCollisionShape(), collisionObject, collisionObject->getWorldTransform(), -1, -1);
 
-			btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1);
+			btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0, &ob1, 0, BT_CLOSEST_POINT_ALGORITHMS);
 			if (algorithm)
 			{
-				btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback);
+				btBridgedManifoldResult contactPointResult(&ob0, &ob1, m_resultCallback);
 				//discrete collision detection query
-				
-				algorithm->processCollision(&ob0,&ob1, m_world->getDispatchInfo(),&contactPointResult);
+
+				algorithm->processCollision(&ob0, &ob1, m_world->getDispatchInfo(), &contactPointResult);
 
 				algorithm->~btCollisionAlgorithm();
 				m_world->getDispatcher()->freeCollisionAlgorithm(algorithm);
@@ -1162,294 +1227,271 @@ struct btSingleContactCallback : public btBroadphaseAabbCallback
 	}
 };
 
-
 ///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback.
 ///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
-void	btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback)
+void btCollisionWorld::contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback)
 {
-	btVector3 aabbMin,aabbMax;
-	colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax);
-	btSingleContactCallback	contactCB(colObj,this,resultCallback);
-	
-	m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB);
-}
+	btVector3 aabbMin, aabbMax;
+	colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), aabbMin, aabbMax);
+	btSingleContactCallback contactCB(colObj, this, resultCallback);
 
+	m_broadphasePairCache->aabbTest(aabbMin, aabbMax, contactCB);
+}
 
 ///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
 ///it reports one or more contact points (including the one with deepest penetration)
-void	btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
+void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
 {
-	btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform(),-1,-1);
-	btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform(),-1,-1);
+	btCollisionObjectWrapper obA(0, colObjA->getCollisionShape(), colObjA, colObjA->getWorldTransform(), -1, -1);
+	btCollisionObjectWrapper obB(0, colObjB->getCollisionShape(), colObjB, colObjB->getWorldTransform(), -1, -1);
 
-	btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB);
+	btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA, &obB, 0, BT_CLOSEST_POINT_ALGORITHMS);
 	if (algorithm)
 	{
-		btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback);
+		btBridgedManifoldResult contactPointResult(&obA, &obB, resultCallback);
+		contactPointResult.m_closestPointDistanceThreshold = resultCallback.m_closestDistanceThreshold;
 		//discrete collision detection query
-		algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult);
+		algorithm->processCollision(&obA, &obB, getDispatchInfo(), &contactPointResult);
 
 		algorithm->~btCollisionAlgorithm();
 		getDispatcher()->freeCollisionAlgorithm(algorithm);
 	}
-
 }
 
-
-
-
 class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
 {
-	btIDebugDraw*	m_debugDrawer;
-	btVector3	m_color;
-	btTransform	m_worldTrans;
+	btIDebugDraw* m_debugDrawer;
+	btVector3 m_color;
+	btTransform m_worldTrans;
 
 public:
+	DebugDrawcallback(btIDebugDraw* debugDrawer, const btTransform& worldTrans, const btVector3& color) : m_debugDrawer(debugDrawer),
+																										  m_color(color),
+																										  m_worldTrans(worldTrans)
+	{
+	}
 
-	DebugDrawcallback(btIDebugDraw*	debugDrawer,const btTransform& worldTrans,const btVector3& color) :
-	  m_debugDrawer(debugDrawer),
-		  m_color(color),
-		  m_worldTrans(worldTrans)
-	  {
-	  }
-
-	  virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
-	  {
-		  processTriangle(triangle,partId,triangleIndex);
-	  }
-
-	  virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
-	  {
-		  (void)partId;
-		  (void)triangleIndex;
-
-		  btVector3 wv0,wv1,wv2;
-		  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 )
-          {
-		    btVector3 normal = (wv1-wv0).cross(wv2-wv0);
-		    normal.normalize();
-		    btVector3 normalColor(1,1,0);
-		    m_debugDrawer->drawLine(center,center+normal,normalColor);
-          }
-		  m_debugDrawer->drawLine(wv0,wv1,m_color);
-		  m_debugDrawer->drawLine(wv1,wv2,m_color);
-		  m_debugDrawer->drawLine(wv2,wv0,m_color);
-	  }
-};
+	virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
+	{
+		processTriangle(triangle, partId, triangleIndex);
+	}
+
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+	{
+		(void)partId;
+		(void)triangleIndex;
 
+		btVector3 wv0, wv1, wv2;
+		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)
+		{
+			btVector3 normal = (wv1 - wv0).cross(wv2 - wv0);
+			normal.normalize();
+			btVector3 normalColor(1, 1, 0);
+			m_debugDrawer->drawLine(center, center + normal, normalColor);
+		}
+		m_debugDrawer->drawLine(wv0, wv1, m_color);
+		m_debugDrawer->drawLine(wv1, wv2, m_color);
+		m_debugDrawer->drawLine(wv2, wv0, m_color);
+	}
+};
 
 void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
 {
 	// Draw a small simplex at the center of the object
 	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames)
 	{
-		getDebugDrawer()->drawTransform(worldTransform,1);
+		getDebugDrawer()->drawTransform(worldTransform, .1);
 	}
 
 	if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
 	{
 		const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
-		for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
+		for (int i = compoundShape->getNumChildShapes() - 1; i >= 0; i--)
 		{
 			btTransform childTrans = compoundShape->getChildTransform(i);
 			const btCollisionShape* colShape = compoundShape->getChildShape(i);
-			debugDrawObject(worldTransform*childTrans,colShape,color);
+			debugDrawObject(worldTransform * childTrans, colShape, color);
 		}
-
-	} else
+	}
+	else
 	{
+		switch (shape->getShapeType())
+		{
+			case BOX_SHAPE_PROXYTYPE:
+			{
+				const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
+				btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
+				getDebugDrawer()->drawBox(-halfExtents, halfExtents, worldTransform, color);
+				break;
+			}
 
-        switch (shape->getShapeType())
-        {
-
-        case BOX_SHAPE_PROXYTYPE:
-            {
-                const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
-                btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
-                getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
-                break;
-            }
-
-        case SPHERE_SHAPE_PROXYTYPE:
-            {
-                const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
-                btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
-
-                getDebugDrawer()->drawSphere(radius, worldTransform, color);
-                break;
-            }
-        case MULTI_SPHERE_SHAPE_PROXYTYPE:
-            {
-                const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
-
-                btTransform childTransform;
-                childTransform.setIdentity();
-
-                for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
-                {
-                    childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
-                    getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
-                }
-
-                break;
-            }
-        case CAPSULE_SHAPE_PROXYTYPE:
-            {
-                const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
-
-                btScalar radius = capsuleShape->getRadius();
-                btScalar halfHeight = capsuleShape->getHalfHeight();
-
-                int upAxis = capsuleShape->getUpAxis();
-                getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
-                break;
-            }
-        case CONE_SHAPE_PROXYTYPE:
-            {
-                const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
-                btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
-                btScalar height = coneShape->getHeight();//+coneShape->getMargin();
-
-                int upAxis= coneShape->getConeUpIndex();
-                getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
-                break;
-
-            }
-        case CYLINDER_SHAPE_PROXYTYPE:
-            {
-                const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
-                int upAxis = cylinder->getUpAxis();
-                btScalar radius = cylinder->getRadius();
-                btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
-                getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
-                break;
-            }
-
-        case STATIC_PLANE_PROXYTYPE:
-            {
-                const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
-                btScalar planeConst = staticPlaneShape->getPlaneConstant();
-                const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
-                getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color);
-                break;
-
-            }
-        default:
-            {
-
-                /// for polyhedral shapes
-                if (shape->isPolyhedral())
-                {
-                    btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
-                    
-                    int i;
-                    if (polyshape->getConvexPolyhedron())
-                    {
-                        const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron();
-                        for (i=0;i<poly->m_faces.size();i++)
-                        {
-                            btVector3 centroid(0,0,0);
-                            int numVerts = poly->m_faces[i].m_indices.size();
-                            if (numVerts)
-                            {
-                                int lastV = poly->m_faces[i].m_indices[numVerts-1];
-                                for (int v=0;v<poly->m_faces[i].m_indices.size();v++)
-                                {
-                                    int curVert = poly->m_faces[i].m_indices[v];
-                                    centroid+=poly->m_vertices[curVert];
-                                    getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color);
-                                    lastV = curVert;
-                                }
-                            }
-                            centroid*= btScalar(1.f)/btScalar(numVerts);
-                            if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals)
-                            {
-                                btVector3 normalColor(1,1,0);
-                                btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]);
-                                getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor);
-                            }
-                            
-                        }
-                        
-                        
-                    } else
-                    {
-                        for (i=0;i<polyshape->getNumEdges();i++)
-                        {
-                            btVector3 a,b;
-                            polyshape->getEdge(i,a,b);
-                            btVector3 wa = worldTransform * a;
-                            btVector3 wb = worldTransform * b;
-                            getDebugDrawer()->drawLine(wa,wb,color);
-                        }
-                    }
-                    
-                    
-                }
-                    
-                if (shape->isConcave())
-                {
-                    btConcaveShape* concaveMesh = (btConcaveShape*) shape;
-
-                    ///@todo pass camera, for some culling? no -> we are not a graphics lib
-                    btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-                    btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
-
-                    DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
-                    concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
-
-                }
-
-                if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
-                {
-                    btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
-                    //todo: pass camera for some culling			
-                    btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-                    btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
-                    //DebugDrawcallback drawCallback;
-                    DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
-                    convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
-                }
-
-
-                
-            }
-       
+			case SPHERE_SHAPE_PROXYTYPE:
+			{
+				const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
+				btScalar radius = sphereShape->getMargin();  //radius doesn't include the margin, so draw with margin
+
+				getDebugDrawer()->drawSphere(radius, worldTransform, color);
+				break;
+			}
+			case MULTI_SPHERE_SHAPE_PROXYTYPE:
+			{
+				const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
+
+				btTransform childTransform;
+				childTransform.setIdentity();
+
+				for (int i = multiSphereShape->getSphereCount() - 1; i >= 0; i--)
+				{
+					childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
+					getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform * childTransform, color);
+				}
+
+				break;
+			}
+			case CAPSULE_SHAPE_PROXYTYPE:
+			{
+				const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
+
+				btScalar radius = capsuleShape->getRadius();
+				btScalar halfHeight = capsuleShape->getHalfHeight();
+
+				int upAxis = capsuleShape->getUpAxis();
+				getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
+				break;
+			}
+			case CONE_SHAPE_PROXYTYPE:
+			{
+				const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
+				btScalar radius = coneShape->getRadius();  //+coneShape->getMargin();
+				btScalar height = coneShape->getHeight();  //+coneShape->getMargin();
+
+				int upAxis = coneShape->getConeUpIndex();
+				getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
+				break;
+			}
+			case CYLINDER_SHAPE_PROXYTYPE:
+			{
+				const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
+				int upAxis = cylinder->getUpAxis();
+				btScalar radius = cylinder->getRadius();
+				btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
+				getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
+				break;
+			}
+
+			case STATIC_PLANE_PROXYTYPE:
+			{
+				const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
+				btScalar planeConst = staticPlaneShape->getPlaneConstant();
+				const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
+				getDebugDrawer()->drawPlane(planeNormal, planeConst, worldTransform, color);
+				break;
+			}
+			default:
+			{
+				/// for polyhedral shapes
+				if (shape->isPolyhedral())
+				{
+					btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*)shape;
+
+					int i;
+					if (polyshape->getConvexPolyhedron())
+					{
+						const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron();
+						for (i = 0; i < poly->m_faces.size(); i++)
+						{
+							btVector3 centroid(0, 0, 0);
+							int numVerts = poly->m_faces[i].m_indices.size();
+							if (numVerts)
+							{
+								int lastV = poly->m_faces[i].m_indices[numVerts - 1];
+								for (int v = 0; v < poly->m_faces[i].m_indices.size(); v++)
+								{
+									int curVert = poly->m_faces[i].m_indices[v];
+									centroid += poly->m_vertices[curVert];
+									getDebugDrawer()->drawLine(worldTransform * poly->m_vertices[lastV], worldTransform * poly->m_vertices[curVert], color);
+									lastV = curVert;
+								}
+							}
+							centroid *= btScalar(1.f) / btScalar(numVerts);
+							if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals)
+							{
+								btVector3 normalColor(1, 1, 0);
+								btVector3 faceNormal(poly->m_faces[i].m_plane[0], poly->m_faces[i].m_plane[1], poly->m_faces[i].m_plane[2]);
+								getDebugDrawer()->drawLine(worldTransform * centroid, worldTransform * (centroid + faceNormal), normalColor);
+							}
+						}
+					}
+					else
+					{
+						for (i = 0; i < polyshape->getNumEdges(); i++)
+						{
+							btVector3 a, b;
+							polyshape->getEdge(i, a, b);
+							btVector3 wa = worldTransform * a;
+							btVector3 wb = worldTransform * b;
+							getDebugDrawer()->drawLine(wa, wb, color);
+						}
+					}
+				}
+
+				if (shape->isConcave())
+				{
+					btConcaveShape* concaveMesh = (btConcaveShape*)shape;
+
+					///@todo pass camera, for some culling? no -> we are not a graphics lib
+					btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+					btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
+
+					DebugDrawcallback drawCallback(getDebugDrawer(), worldTransform, color);
+					concaveMesh->processAllTriangles(&drawCallback, aabbMin, aabbMax);
+				}
+
+				if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
+				{
+					btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*)shape;
+					//todo: pass camera for some culling
+					btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+					btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
+					//DebugDrawcallback drawCallback;
+					DebugDrawcallback drawCallback(getDebugDrawer(), worldTransform, color);
+					convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback, aabbMin, aabbMax);
+				}
+			}
 		}
 	}
 }
 
-
-void	btCollisionWorld::debugDrawWorld()
+void btCollisionWorld::debugDrawWorld()
 {
 	if (getDebugDrawer())
 	{
+		getDebugDrawer()->clearLines();
+
 		btIDebugDraw::DefaultColors defaultColors = getDebugDrawer()->getDefaultColors();
 
-		if ( getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
+		if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
 		{
-		
-
 			if (getDispatcher())
 			{
 				int numManifolds = getDispatcher()->getNumManifolds();
-			
-				for (int i=0;i<numManifolds;i++)
+
+				for (int i = 0; i < numManifolds; i++)
 				{
 					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++)
+					for (int j = 0; j < numContacts; j++)
 					{
 						btManifoldPoint& cp = contactManifold->getContactPoint(j);
-						getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),defaultColors.m_contactPoint);
+						getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB, cp.m_normalWorldOnB, cp.getDistance(), cp.getLifeTime(), defaultColors.m_contactPoint);
 					}
 				}
 			}
@@ -1459,56 +1501,63 @@ void	btCollisionWorld::debugDrawWorld()
 		{
 			int i;
 
-			for (  i=0;i<m_collisionObjects.size();i++)
+			for (i = 0; i < m_collisionObjects.size(); i++)
 			{
 				btCollisionObject* colObj = m_collisionObjects[i];
-				if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
+				if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT) == 0)
 				{
 					if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe))
 					{
-						btVector3 color(btScalar(0.4),btScalar(0.4),btScalar(0.4));
+						btVector3 color(btScalar(0.4), btScalar(0.4), btScalar(0.4));
 
-						switch(colObj->getActivationState())
+						switch (colObj->getActivationState())
 						{
-						case  ACTIVE_TAG:
-							color = defaultColors.m_activeObject; break;
-						case ISLAND_SLEEPING:
-							color =  defaultColors.m_deactivatedObject;break;
-						case WANTS_DEACTIVATION:
-							color = defaultColors.m_wantsDeactivationObject;break;
-						case DISABLE_DEACTIVATION:
-							color = defaultColors.m_disabledDeactivationObject;break;
-						case DISABLE_SIMULATION:
-							color = defaultColors.m_disabledSimulationObject;break;
-						default:
+							case ACTIVE_TAG:
+								color = defaultColors.m_activeObject;
+								break;
+							case ISLAND_SLEEPING:
+								color = defaultColors.m_deactivatedObject;
+								break;
+							case WANTS_DEACTIVATION:
+								color = defaultColors.m_wantsDeactivationObject;
+								break;
+							case DISABLE_DEACTIVATION:
+								color = defaultColors.m_disabledDeactivationObject;
+								break;
+							case DISABLE_SIMULATION:
+								color = defaultColors.m_disabledSimulationObject;
+								break;
+							default:
 							{
-								color = btVector3(btScalar(.3),btScalar(0.3),btScalar(0.3));
+								color = btVector3(btScalar(.3), btScalar(0.3), btScalar(0.3));
 							}
 						};
 
-						debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
+						colObj->getCustomDebugColor(color);
+
+						debugDrawObject(colObj->getWorldTransform(), colObj->getCollisionShape(), color);
 					}
 					if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
 					{
-						btVector3 minAabb,maxAabb;
+						btVector3 minAabb, maxAabb;
 						btVector3 colorvec = defaultColors.m_aabb;
-						colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
-						btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
+						colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb);
+						btVector3 contactThreshold(gContactBreakingThreshold, gContactBreakingThreshold, gContactBreakingThreshold);
 						minAabb -= contactThreshold;
 						maxAabb += contactThreshold;
 
-						btVector3 minAabb2,maxAabb2;
+						btVector3 minAabb2, maxAabb2;
 
-						if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
+						if (getDispatchInfo().m_useContinuous && colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
 						{
-							colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
+							colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(), minAabb2, maxAabb2);
 							minAabb2 -= contactThreshold;
 							maxAabb2 += contactThreshold;
 							minAabb.setMin(minAabb2);
 							maxAabb.setMax(maxAabb2);
 						}
 
-						m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
+						m_debugDrawer->drawAabb(minAabb, maxAabb, colorvec);
 					}
 				}
 			}
@@ -1516,45 +1565,63 @@ void	btCollisionWorld::debugDrawWorld()
 	}
 }
 
-
-void	btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
+void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
 {
 	int i;
 
 	///keep track of shapes already serialized
-	btHashMap<btHashPtr,btCollisionShape*>	serializedShapes;
+	btHashMap<btHashPtr, btCollisionShape*> serializedShapes;
 
-	for (i=0;i<m_collisionObjects.size();i++)
+	for (i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btCollisionShape* shape = colObj->getCollisionShape();
 
 		if (!serializedShapes.find(shape))
 		{
-			serializedShapes.insert(shape,shape);
+			serializedShapes.insert(shape, shape);
 			shape->serializeSingleShape(serializer);
 		}
 	}
 
 	//serialize all collision objects
-	for (i=0;i<m_collisionObjects.size();i++)
+	for (i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
-		if ((colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) || (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK))
+		if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
 		{
 			colObj->serializeSingleObject(serializer);
 		}
 	}
 }
 
-
-void	btCollisionWorld::serialize(btSerializer* serializer)
+void btCollisionWorld::serializeContactManifolds(btSerializer* serializer)
 {
+	if (serializer->getSerializationFlags() & BT_SERIALIZE_CONTACT_MANIFOLDS)
+	{
+		int numManifolds = getDispatcher()->getNumManifolds();
+		for (int i = 0; i < numManifolds; i++)
+		{
+			const btPersistentManifold* manifold = getDispatcher()->getInternalManifoldPointer()[i];
+			//don't serialize empty manifolds, they just take space
+			//(may have to do it anyway if it destroys determinism)
+			if (manifold->getNumContacts() == 0)
+				continue;
+
+			btChunk* chunk = serializer->allocate(manifold->calculateSerializeBufferSize(), 1);
+			const char* structType = manifold->serialize(manifold, chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk, structType, BT_CONTACTMANIFOLD_CODE, (void*)manifold);
+		}
+	}
+}
 
+void btCollisionWorld::serialize(btSerializer* serializer)
+{
 	serializer->startSerialization();
-	
+
 	serializeCollisionObjects(serializer);
-	
+
+	serializeContactManifolds(serializer);
+
 	serializer->finishSerialization();
 }
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
index ec40c969f51..fd0e5b9bbdd 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
@@ -13,9 +13,8 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 /**
- * @page Bullet Documentation
+ * @mainpage Bullet Documentation
  *
  * @section intro_sec Introduction
  * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ).
@@ -66,8 +65,6 @@ subject to the following restrictions:
  * For up-to-data information and copyright and contributors list check out the Bullet_User_Manual.pdf
  * 
  */
- 
- 
 
 #ifndef BT_COLLISION_WORLD_H
 #define BT_COLLISION_WORLD_H
@@ -87,144 +84,138 @@ class btSerializer;
 ///CollisionWorld is interface and container for the collision detection
 class btCollisionWorld
 {
-
-	
 protected:
+	btAlignedObjectArray<btCollisionObject*> m_collisionObjects;
 
-	btAlignedObjectArray<btCollisionObject*>	m_collisionObjects;
-	
-	btDispatcher*	m_dispatcher1;
+	btDispatcher* m_dispatcher1;
 
-	btDispatcherInfo	m_dispatchInfo;
+	btDispatcherInfo m_dispatchInfo;
 
-	btBroadphaseInterface*	m_broadphasePairCache;
+	btBroadphaseInterface* m_broadphasePairCache;
 
-	btIDebugDraw*	m_debugDrawer;
+	btIDebugDraw* m_debugDrawer;
 
 	///m_forceUpdateAllAabbs can be set to false as an optimization to only update active object AABBs
 	///it is true by default, because it is error-prone (setting the position of static objects wouldn't update their AABB)
 	bool m_forceUpdateAllAabbs;
 
-	void	serializeCollisionObjects(btSerializer* serializer);
+	void serializeCollisionObjects(btSerializer* serializer);
 
-public:
+	void serializeContactManifolds(btSerializer* serializer);
 
+public:
 	//this constructor doesn't own the dispatcher and paircache/broadphase
-	btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration);
+	btCollisionWorld(btDispatcher* dispatcher, btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration);
 
 	virtual ~btCollisionWorld();
 
-	void	setBroadphase(btBroadphaseInterface*	pairCache)
+	void setBroadphase(btBroadphaseInterface* pairCache)
 	{
 		m_broadphasePairCache = pairCache;
 	}
 
-	const btBroadphaseInterface*	getBroadphase() const
+	const btBroadphaseInterface* getBroadphase() const
 	{
 		return m_broadphasePairCache;
 	}
 
-	btBroadphaseInterface*	getBroadphase()
+	btBroadphaseInterface* getBroadphase()
 	{
 		return m_broadphasePairCache;
 	}
 
-	btOverlappingPairCache*	getPairCache()
+	btOverlappingPairCache* getPairCache()
 	{
 		return m_broadphasePairCache->getOverlappingPairCache();
 	}
 
-
-	btDispatcher*	getDispatcher()
+	btDispatcher* getDispatcher()
 	{
 		return m_dispatcher1;
 	}
 
-	const btDispatcher*	getDispatcher() const
+	const btDispatcher* getDispatcher() const
 	{
 		return m_dispatcher1;
 	}
 
-	void	updateSingleAabb(btCollisionObject* colObj);
+	void updateSingleAabb(btCollisionObject* colObj);
 
-	virtual void	updateAabbs();
+	virtual void updateAabbs();
 
 	///the computeOverlappingPairs is usually already called by performDiscreteCollisionDetection (or stepSimulation)
 	///it can be useful to use if you perform ray tests without collision detection/simulation
-	virtual void	computeOverlappingPairs();
+	virtual void computeOverlappingPairs();
 
-	
-	virtual void	setDebugDrawer(btIDebugDraw*	debugDrawer)
+	virtual void setDebugDrawer(btIDebugDraw* debugDrawer)
 	{
-			m_debugDrawer = debugDrawer;
+		m_debugDrawer = debugDrawer;
 	}
 
-	virtual btIDebugDraw*	getDebugDrawer()
+	virtual btIDebugDraw* getDebugDrawer()
 	{
 		return m_debugDrawer;
 	}
 
-	virtual void	debugDrawWorld();
+	virtual void debugDrawWorld();
 
 	virtual void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
 
-
 	///LocalShapeInfo gives extra information for complex shapes
 	///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart
-	struct	LocalShapeInfo
+	struct LocalShapeInfo
 	{
-		int	m_shapePart;
-		int	m_triangleIndex;
-		
+		int m_shapePart;
+		int m_triangleIndex;
+
 		//const btCollisionShape*	m_shapeTemp;
 		//const btTransform*	m_shapeLocalTransform;
 	};
 
-	struct	LocalRayResult
+	struct LocalRayResult
 	{
-		LocalRayResult(const btCollisionObject*	collisionObject, 
-			LocalShapeInfo*	localShapeInfo,
-			const btVector3&		hitNormalLocal,
-			btScalar hitFraction)
-		:m_collisionObject(collisionObject),
-		m_localShapeInfo(localShapeInfo),
-		m_hitNormalLocal(hitNormalLocal),
-		m_hitFraction(hitFraction)
+		LocalRayResult(const btCollisionObject* collisionObject,
+					   LocalShapeInfo* localShapeInfo,
+					   const btVector3& hitNormalLocal,
+					   btScalar hitFraction)
+			: m_collisionObject(collisionObject),
+			  m_localShapeInfo(localShapeInfo),
+			  m_hitNormalLocal(hitNormalLocal),
+			  m_hitFraction(hitFraction)
 		{
 		}
 
-		const btCollisionObject*		m_collisionObject;
-		LocalShapeInfo*			m_localShapeInfo;
-		btVector3				m_hitNormalLocal;
-		btScalar				m_hitFraction;
-
+		const btCollisionObject* m_collisionObject;
+		LocalShapeInfo* m_localShapeInfo;
+		btVector3 m_hitNormalLocal;
+		btScalar m_hitFraction;
 	};
 
 	///RayResultCallback is used to report new raycast results
-	struct	RayResultCallback
+	struct RayResultCallback
 	{
-		btScalar	m_closestHitFraction;
-		const btCollisionObject*		m_collisionObject;
-		short int	m_collisionFilterGroup;
-		short int	m_collisionFilterMask;
+		btScalar m_closestHitFraction;
+		const btCollisionObject* m_collisionObject;
+		int m_collisionFilterGroup;
+		int m_collisionFilterMask;
 		//@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback.h. Apply any of the EFlags defined there on m_flags here to invoke.
 		unsigned int m_flags;
 
 		virtual ~RayResultCallback()
 		{
 		}
-		bool	hasHit() const
+		bool hasHit() const
 		{
 			return (m_collisionObject != 0);
 		}
 
 		RayResultCallback()
-			:m_closestHitFraction(btScalar(1.)),
-			m_collisionObject(0),
-			m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
-			m_collisionFilterMask(btBroadphaseProxy::AllFilter),
-			//@BP Mod
-			m_flags(0)
+			: m_closestHitFraction(btScalar(1.)),
+			  m_collisionObject(0),
+			  m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
+			  m_collisionFilterMask(btBroadphaseProxy::AllFilter),
+			  //@BP Mod
+			  m_flags(0)
 		{
 		}
 
@@ -235,62 +226,62 @@ public:
 			return collides;
 		}
 
-
-		virtual	btScalar	addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) = 0;
+		virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace) = 0;
 	};
 
-	struct	ClosestRayResultCallback : public RayResultCallback
+	struct ClosestRayResultCallback : public RayResultCallback
 	{
-		ClosestRayResultCallback(const btVector3&	rayFromWorld,const btVector3&	rayToWorld)
-		:m_rayFromWorld(rayFromWorld),
-		m_rayToWorld(rayToWorld)
+		ClosestRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+			: m_rayFromWorld(rayFromWorld),
+			  m_rayToWorld(rayToWorld)
 		{
 		}
 
-		btVector3	m_rayFromWorld;//used to calculate hitPointWorld from hitFraction
-		btVector3	m_rayToWorld;
+		btVector3 m_rayFromWorld;  //used to calculate hitPointWorld from hitFraction
+		btVector3 m_rayToWorld;
+
+		btVector3 m_hitNormalWorld;
+		btVector3 m_hitPointWorld;
 
-		btVector3	m_hitNormalWorld;
-		btVector3	m_hitPointWorld;
-			
-		virtual	btScalar	addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace)
+		virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace)
 		{
 			//caller already does the filter on the m_closestHitFraction
 			btAssert(rayResult.m_hitFraction <= m_closestHitFraction);
-			
+
 			m_closestHitFraction = rayResult.m_hitFraction;
 			m_collisionObject = rayResult.m_collisionObject;
 			if (normalInWorldSpace)
 			{
 				m_hitNormalWorld = rayResult.m_hitNormalLocal;
-			} else
+			}
+			else
 			{
 				///need to transform normal into worldspace
-				m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal;
+				m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal;
 			}
-			m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
+			m_hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction);
 			return rayResult.m_hitFraction;
 		}
 	};
 
-	struct	AllHitsRayResultCallback : public RayResultCallback
+	struct AllHitsRayResultCallback : public RayResultCallback
 	{
-		AllHitsRayResultCallback(const btVector3&	rayFromWorld,const btVector3&	rayToWorld)
-		:m_rayFromWorld(rayFromWorld),
-		m_rayToWorld(rayToWorld)
+		AllHitsRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+			: m_rayFromWorld(rayFromWorld),
+			  m_rayToWorld(rayToWorld)
 		{
 		}
 
-		btAlignedObjectArray<const btCollisionObject*>		m_collisionObjects;
+		btAlignedObjectArray<const btCollisionObject*> m_collisionObjects;
 
-		btVector3	m_rayFromWorld;//used to calculate hitPointWorld from hitFraction
-		btVector3	m_rayToWorld;
+		btVector3 m_rayFromWorld;  //used to calculate hitPointWorld from hitFraction
+		btVector3 m_rayToWorld;
 
-		btAlignedObjectArray<btVector3>	m_hitNormalWorld;
-		btAlignedObjectArray<btVector3>	m_hitPointWorld;
+		btAlignedObjectArray<btVector3> m_hitNormalWorld;
+		btAlignedObjectArray<btVector3> m_hitPointWorld;
 		btAlignedObjectArray<btScalar> m_hitFractions;
-			
-		virtual	btScalar	addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace)
+
+		virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace)
 		{
 			m_collisionObject = rayResult.m_collisionObject;
 			m_collisionObjects.push_back(rayResult.m_collisionObject);
@@ -298,69 +289,66 @@ public:
 			if (normalInWorldSpace)
 			{
 				hitNormalWorld = rayResult.m_hitNormalLocal;
-			} else
+			}
+			else
 			{
 				///need to transform normal into worldspace
-				hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal;
+				hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal;
 			}
 			m_hitNormalWorld.push_back(hitNormalWorld);
 			btVector3 hitPointWorld;
-			hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
+			hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction);
 			m_hitPointWorld.push_back(hitPointWorld);
 			m_hitFractions.push_back(rayResult.m_hitFraction);
 			return m_closestHitFraction;
 		}
 	};
 
-
 	struct LocalConvexResult
 	{
-		LocalConvexResult(const btCollisionObject*	hitCollisionObject, 
-			LocalShapeInfo*	localShapeInfo,
-			const btVector3&		hitNormalLocal,
-			const btVector3&		hitPointLocal,
-			btScalar hitFraction
-			)
-		:m_hitCollisionObject(hitCollisionObject),
-		m_localShapeInfo(localShapeInfo),
-		m_hitNormalLocal(hitNormalLocal),
-		m_hitPointLocal(hitPointLocal),
-		m_hitFraction(hitFraction)
+		LocalConvexResult(const btCollisionObject* hitCollisionObject,
+						  LocalShapeInfo* localShapeInfo,
+						  const btVector3& hitNormalLocal,
+						  const btVector3& hitPointLocal,
+						  btScalar hitFraction)
+			: m_hitCollisionObject(hitCollisionObject),
+			  m_localShapeInfo(localShapeInfo),
+			  m_hitNormalLocal(hitNormalLocal),
+			  m_hitPointLocal(hitPointLocal),
+			  m_hitFraction(hitFraction)
 		{
 		}
 
-		const btCollisionObject*		m_hitCollisionObject;
-		LocalShapeInfo*			m_localShapeInfo;
-		btVector3				m_hitNormalLocal;
-		btVector3				m_hitPointLocal;
-		btScalar				m_hitFraction;
+		const btCollisionObject* m_hitCollisionObject;
+		LocalShapeInfo* m_localShapeInfo;
+		btVector3 m_hitNormalLocal;
+		btVector3 m_hitPointLocal;
+		btScalar m_hitFraction;
 	};
 
 	///RayResultCallback is used to report new raycast results
-	struct	ConvexResultCallback
+	struct ConvexResultCallback
 	{
-		btScalar	m_closestHitFraction;
-		short int	m_collisionFilterGroup;
-		short int	m_collisionFilterMask;
-		
+		btScalar m_closestHitFraction;
+		int m_collisionFilterGroup;
+		int m_collisionFilterMask;
+
 		ConvexResultCallback()
-			:m_closestHitFraction(btScalar(1.)),
-			m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
-			m_collisionFilterMask(btBroadphaseProxy::AllFilter)
+			: m_closestHitFraction(btScalar(1.)),
+			  m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
+			  m_collisionFilterMask(btBroadphaseProxy::AllFilter)
 		{
 		}
 
 		virtual ~ConvexResultCallback()
 		{
 		}
-		
-		bool	hasHit() const
+
+		bool hasHit() const
 		{
 			return (m_closestHitFraction < btScalar(1.));
 		}
 
-		
-
 		virtual bool needsCollision(btBroadphaseProxy* proxy0) const
 		{
 			bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
@@ -368,39 +356,40 @@ public:
 			return collides;
 		}
 
-		virtual	btScalar	addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) = 0;
+		virtual btScalar addSingleResult(LocalConvexResult& convexResult, bool normalInWorldSpace) = 0;
 	};
 
-	struct	ClosestConvexResultCallback : public ConvexResultCallback
+	struct ClosestConvexResultCallback : public ConvexResultCallback
 	{
-		ClosestConvexResultCallback(const btVector3&	convexFromWorld,const btVector3&	convexToWorld)
-		:m_convexFromWorld(convexFromWorld),
-		m_convexToWorld(convexToWorld),
-		m_hitCollisionObject(0)
+		ClosestConvexResultCallback(const btVector3& convexFromWorld, const btVector3& convexToWorld)
+			: m_convexFromWorld(convexFromWorld),
+			  m_convexToWorld(convexToWorld),
+			  m_hitCollisionObject(0)
 		{
 		}
 
-		btVector3	m_convexFromWorld;//used to calculate hitPointWorld from hitFraction
-		btVector3	m_convexToWorld;
+		btVector3 m_convexFromWorld;  //used to calculate hitPointWorld from hitFraction
+		btVector3 m_convexToWorld;
 
-		btVector3	m_hitNormalWorld;
-		btVector3	m_hitPointWorld;
-		const btCollisionObject*	m_hitCollisionObject;
-		
-		virtual	btScalar	addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace)
+		btVector3 m_hitNormalWorld;
+		btVector3 m_hitPointWorld;
+		const btCollisionObject* m_hitCollisionObject;
+
+		virtual btScalar addSingleResult(LocalConvexResult& convexResult, bool normalInWorldSpace)
 		{
-//caller already does the filter on the m_closestHitFraction
+			//caller already does the filter on the m_closestHitFraction
 			btAssert(convexResult.m_hitFraction <= m_closestHitFraction);
-						
+
 			m_closestHitFraction = convexResult.m_hitFraction;
 			m_hitCollisionObject = convexResult.m_hitCollisionObject;
 			if (normalInWorldSpace)
 			{
 				m_hitNormalWorld = convexResult.m_hitNormalLocal;
-			} else
+			}
+			else
 			{
 				///need to transform normal into worldspace
-				m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
+				m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis() * convexResult.m_hitNormalLocal;
 			}
 			m_hitPointWorld = convexResult.m_hitPointLocal;
 			return convexResult.m_hitFraction;
@@ -408,21 +397,23 @@ public:
 	};
 
 	///ContactResultCallback is used to report contact points
-	struct	ContactResultCallback
+	struct ContactResultCallback
 	{
-		short int	m_collisionFilterGroup;
-		short int	m_collisionFilterMask;
-		
+		int m_collisionFilterGroup;
+		int m_collisionFilterMask;
+		btScalar m_closestDistanceThreshold;
+
 		ContactResultCallback()
-			:m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
-			m_collisionFilterMask(btBroadphaseProxy::AllFilter)
+			: m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
+			  m_collisionFilterMask(btBroadphaseProxy::AllFilter),
+			  m_closestDistanceThreshold(0)
 		{
 		}
 
 		virtual ~ContactResultCallback()
 		{
 		}
-		
+
 		virtual bool needsCollision(btBroadphaseProxy* proxy0) const
 		{
 			bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
@@ -430,58 +421,57 @@ public:
 			return collides;
 		}
 
-		virtual	btScalar	addSingleResult(btManifoldPoint& cp,	const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) = 0;
+		virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) = 0;
 	};
 
-
-
-	int	getNumCollisionObjects() const
+	int getNumCollisionObjects() const
 	{
 		return int(m_collisionObjects.size());
 	}
 
 	/// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback
 	/// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback.
-	virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; 
+	virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const;
 
 	/// convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback
 	/// This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
-	void    convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback,  btScalar allowedCcdPenetration = btScalar(0.)) const;
+	void convexSweepTest(const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = btScalar(0.)) const;
 
 	///contactTest performs a discrete collision test between colObj against all objects in the btCollisionWorld, and calls the resultCallback.
 	///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
-	void	contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback);
+	void contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback);
 
 	///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
 	///it reports one or more contact points (including the one with deepest penetration)
-	void	contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback);
-
+	void contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback);
 
 	/// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
 	/// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape.
 	/// This allows more customization.
-	static void	rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
-					  btCollisionObject* collisionObject,
-					  const btCollisionShape* collisionShape,
-					  const btTransform& colObjWorldTransform,
-					  RayResultCallback& resultCallback);
+	static void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
+							  btCollisionObject* collisionObject,
+							  const btCollisionShape* collisionShape,
+							  const btTransform& colObjWorldTransform,
+							  RayResultCallback& resultCallback);
 
-	static void	rayTestSingleInternal(const btTransform& rayFromTrans,const btTransform& rayToTrans,
-					  const btCollisionObjectWrapper* collisionObjectWrap,
-					  RayResultCallback& resultCallback);
+	static void rayTestSingleInternal(const btTransform& rayFromTrans, const btTransform& rayToTrans,
+									  const btCollisionObjectWrapper* collisionObjectWrap,
+									  RayResultCallback& resultCallback);
 
 	/// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
-	static void	objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans,const btTransform& rayToTrans,
-					  btCollisionObject* collisionObject,
-					  const btCollisionShape* collisionShape,
-					  const btTransform& colObjWorldTransform,
-					  ConvexResultCallback& resultCallback, btScalar	allowedPenetration);
+	static void objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans, const btTransform& rayToTrans,
+								  btCollisionObject* collisionObject,
+								  const btCollisionShape* collisionShape,
+								  const btTransform& colObjWorldTransform,
+								  ConvexResultCallback& resultCallback, btScalar allowedPenetration);
+
+	static void objectQuerySingleInternal(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans,
+										  const btCollisionObjectWrapper* colObjWrap,
+										  ConvexResultCallback& resultCallback, btScalar allowedPenetration);
 
-	static void	objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
-											const btCollisionObjectWrapper* colObjWrap,
-											ConvexResultCallback& resultCallback, btScalar allowedPenetration);
+	virtual void addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
 
-	virtual void	addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter);
+	virtual void refreshBroadphaseProxy(btCollisionObject* collisionObject);
 
 	btCollisionObjectArray& getCollisionObjectArray()
 	{
@@ -493,10 +483,9 @@ public:
 		return m_collisionObjects;
 	}
 
+	virtual void removeCollisionObject(btCollisionObject* collisionObject);
 
-	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
-
-	virtual void	performDiscreteCollisionDetection();
+	virtual void performDiscreteCollisionDetection();
 
 	btDispatcherInfo& getDispatchInfo()
 	{
@@ -507,20 +496,18 @@ public:
 	{
 		return m_dispatchInfo;
 	}
-	
-	bool	getForceUpdateAllAabbs() const
+
+	bool getForceUpdateAllAabbs() const
 	{
 		return m_forceUpdateAllAabbs;
 	}
-	void setForceUpdateAllAabbs( bool forceUpdateAllAabbs)
+	void setForceUpdateAllAabbs(bool forceUpdateAllAabbs)
 	{
 		m_forceUpdateAllAabbs = forceUpdateAllAabbs;
 	}
 
 	///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (Bullet/Demos/SerializeDemo)
-	virtual	void	serialize(btSerializer* serializer);
-
+	virtual void serialize(btSerializer* serializer);
 };
 
-
-#endif //BT_COLLISION_WORLD_H
+#endif  //BT_COLLISION_WORLD_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp
index 57eb81703e7..e56e73dcf5b 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp
@@ -15,269 +15,251 @@ subject to the following restrictions:
 
 #include "btCollisionWorldImporter.h"
 #include "btBulletCollisionCommon.h"
-#include "LinearMath/btSerializer.h" //for btBulletSerializedArrays definition
+#include "LinearMath/btSerializer.h"  //for btBulletSerializedArrays definition
 
 #ifdef SUPPORT_GIMPACT_SHAPE_IMPORT
 #include "BulletCollision/Gimpact/btGImpactShape.h"
-#endif //SUPPORT_GIMPACT_SHAPE_IMPORT
+#endif  //SUPPORT_GIMPACT_SHAPE_IMPORT
 
 btCollisionWorldImporter::btCollisionWorldImporter(btCollisionWorld* world)
-:m_collisionWorld(world),
-m_verboseMode(0)
+	: m_collisionWorld(world),
+	  m_verboseMode(0)
 {
-
 }
 
 btCollisionWorldImporter::~btCollisionWorldImporter()
 {
 }
 
-
-
-
-
-bool	btCollisionWorldImporter::convertAllObjects( btBulletSerializedArrays* arrays)
+bool btCollisionWorldImporter::convertAllObjects(btBulletSerializedArrays* arrays)
 {
-
 	m_shapeMap.clear();
 	m_bodyMap.clear();
 
 	int i;
 
-	for (i=0;i<arrays->m_bvhsDouble.size();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);
+		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];
+	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);
+		m_bvhMap.insert(arrays->m_bvhsFloat[i], bvh);
 	}
 
-
-
-
-
-	for (i=0;i<arrays->m_colShapeData.size();i++)
+	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);
+			//		printf("shapeMap.insert(%x,%x)\n",shapeData,shape);
+			m_shapeMap.insert(shapeData, shape);
 		}
 
-		if (shape&& shapeData->m_name)
+		if (shape && shapeData->m_name)
 		{
 			char* newname = duplicateName(shapeData->m_name);
-			m_objectNameMap.insert(shape,newname);
-			m_nameShapeMap.insert(newname,shape);
+			m_objectNameMap.insert(shape, newname);
+			m_nameShapeMap.insert(newname, shape);
 		}
 	}
 
-
-	for (i=0;i<arrays->m_collisionObjectDataDouble.size();i++)
+	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));
+		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");
-        }
+			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++)
+	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);
+		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);
+			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");
-        }
-    }
+			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++)
+	for (i = 0; i < m_allocatedCollisionObjects.size(); i++)
 	{
-		if(m_collisionWorld)
+		if (m_collisionWorld)
 			m_collisionWorld->removeCollisionObject(m_allocatedCollisionObjects[i]);
 		delete m_allocatedCollisionObjects[i];
 	}
 
 	m_allocatedCollisionObjects.clear();
 
-
-	for (i=0;i<m_allocatedCollisionShapes.size();i++)
+	for (i = 0; i < m_allocatedCollisionShapes.size(); i++)
 	{
 		delete m_allocatedCollisionShapes[i];
 	}
 	m_allocatedCollisionShapes.clear();
 
-
-	for (i=0;i<m_allocatedBvhs.size();i++)
+	for (i = 0; i < m_allocatedBvhs.size(); i++)
 	{
 		delete m_allocatedBvhs[i];
 	}
 	m_allocatedBvhs.clear();
 
-	for (i=0;i<m_allocatedTriangleInfoMaps.size();i++)
+	for (i = 0; i < m_allocatedTriangleInfoMaps.size(); i++)
 	{
 		delete m_allocatedTriangleInfoMaps[i];
 	}
 	m_allocatedTriangleInfoMaps.clear();
-	for (i=0;i<m_allocatedTriangleIndexArrays.size();i++)
+	for (i = 0; i < m_allocatedTriangleIndexArrays.size(); i++)
 	{
 		delete m_allocatedTriangleIndexArrays[i];
 	}
 	m_allocatedTriangleIndexArrays.clear();
-	for (i=0;i<m_allocatedNames.size();i++)
+	for (i = 0; i < m_allocatedNames.size(); i++)
 	{
 		delete[] m_allocatedNames[i];
 	}
 	m_allocatedNames.clear();
 
-	for (i=0;i<m_allocatedbtStridingMeshInterfaceDatas.size();i++)
+	for (i = 0; i < m_allocatedbtStridingMeshInterfaceDatas.size(); i++)
 	{
 		btStridingMeshInterfaceData* curData = m_allocatedbtStridingMeshInterfaceDatas[i];
 
-		for(int a = 0;a < curData->m_numMeshParts;a++)
+		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_vertices3f)
+				delete[] curPart->m_vertices3f;
 
-			if(curPart->m_vertices3d)
-				delete [] curPart->m_vertices3d;
+			if (curPart->m_vertices3d)
+				delete[] curPart->m_vertices3d;
 
-			if(curPart->m_indices32)
-				delete [] curPart->m_indices32;
+			if (curPart->m_indices32)
+				delete[] curPart->m_indices32;
 
-			if(curPart->m_3indices16)
-				delete [] curPart->m_3indices16;
+			if (curPart->m_3indices16)
+				delete[] curPart->m_3indices16;
 
-			if(curPart->m_indices16)
-				delete [] curPart->m_indices16;
+			if (curPart->m_indices16)
+				delete[] curPart->m_indices16;
 
 			if (curPart->m_3indices8)
-				delete [] curPart->m_3indices8;
-
+				delete[] curPart->m_3indices8;
 		}
-		delete [] curData->m_meshPartsPtr;
+		delete[] curData->m_meshPartsPtr;
 		delete curData;
 	}
 	m_allocatedbtStridingMeshInterfaceDatas.clear();
 
-	for (i=0;i<m_indexArrays.size();i++)
+	for (i = 0; i < m_indexArrays.size(); i++)
 	{
 		btAlignedFree(m_indexArrays[i]);
 	}
-  m_indexArrays.clear();
+	m_indexArrays.clear();
 
-	for (i=0;i<m_shortIndexArrays.size();i++)
+	for (i = 0; i < m_shortIndexArrays.size(); i++)
 	{
 		btAlignedFree(m_shortIndexArrays[i]);
 	}
-  m_shortIndexArrays.clear();
+	m_shortIndexArrays.clear();
 
-	for (i=0;i<m_charIndexArrays.size();i++)
+	for (i = 0; i < m_charIndexArrays.size(); i++)
 	{
 		btAlignedFree(m_charIndexArrays[i]);
 	}
-  m_charIndexArrays.clear();
+	m_charIndexArrays.clear();
 
-	for (i=0;i<m_floatVertexArrays.size();i++)
+	for (i = 0; i < m_floatVertexArrays.size(); i++)
 	{
 		btAlignedFree(m_floatVertexArrays[i]);
 	}
-  m_floatVertexArrays.clear();
+	m_floatVertexArrays.clear();
 
-	for (i=0;i<m_doubleVertexArrays.size();i++)
+	for (i = 0; i < m_doubleVertexArrays.size(); i++)
 	{
 		btAlignedFree(m_doubleVertexArrays[i]);
 	}
-   m_doubleVertexArrays.clear();
-
-
+	m_doubleVertexArrays.clear();
 }
 
-
-
-btCollisionShape* btCollisionWorldImporter::convertCollisionShape(  btCollisionShapeData* shapeData  )
+btCollisionShape* btCollisionWorldImporter::convertCollisionShape(btCollisionShapeData* shapeData)
 {
 	btCollisionShape* shape = 0;
 
 	switch (shapeData->m_shapeType)
-		{
-	case STATIC_PLANE_PROXYTYPE:
+	{
+		case STATIC_PLANE_PROXYTYPE:
 		{
 			btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*)shapeData;
-			btVector3 planeNormal,localScaling;
+			btVector3 planeNormal, localScaling;
 			planeNormal.deSerializeFloat(planeData->m_planeNormal);
 			localScaling.deSerializeFloat(planeData->m_localScaling);
-			shape = createPlaneShape(planeNormal,planeData->m_planeConstant);
+			shape = createPlaneShape(planeNormal, planeData->m_planeConstant);
 			shape->setLocalScaling(localScaling);
 
 			break;
 		}
-	case SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE:
+		case SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE:
 		{
-			btScaledTriangleMeshShapeData* scaledMesh = (btScaledTriangleMeshShapeData*) shapeData;
-			btCollisionShapeData* colShapeData = (btCollisionShapeData*) &scaledMesh->m_trimeshShapeData;
+			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;
@@ -288,15 +270,14 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape(  btCollisionS
 			break;
 		}
 #ifdef SUPPORT_GIMPACT_SHAPE_IMPORT
-	case GIMPACT_SHAPE_PROXYTYPE:
+		case GIMPACT_SHAPE_PROXYTYPE:
 		{
-			btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*) shapeData;
+			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);
@@ -304,47 +285,45 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape(  btCollisionS
 				gimpactShape->setMargin(btScalar(gimpactData->m_collisionMargin));
 				gimpactShape->updateBound();
 				shape = gimpactShape;
-			} else
+			}
+			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
+#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:
+				case 0:
 				{
-					shape = createCapsuleShapeX(1,1);
+					shape = createCapsuleShapeX(1, 1);
 					break;
 				}
-			case 1:
+				case 1:
 				{
-					shape = createCapsuleShapeY(1,1);
+					shape = createCapsuleShapeY(1, 1);
 					break;
 				}
-			case 2:
+				case 2:
 				{
-					shape = createCapsuleShapeZ(1,1);
+					shape = createCapsuleShapeZ(1, 1);
 					break;
 				}
-			default:
+				default:
 				{
 					printf("error: wrong up axis for btCapsuleShape\n");
 				}
-
-
 			};
 			if (shape)
 			{
-				btCapsuleShape* cap = (btCapsuleShape*) shape;
+				btCapsuleShape* cap = (btCapsuleShape*)shape;
 				cap->deSerializeFloat(capData);
 			}
 			break;
@@ -355,163 +334,156 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape(  btCollisionS
 		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)
 			{
-				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:
 				{
-					case BOX_SHAPE_PROXYTYPE:
-						{
-							btBoxShape* box= (btBoxShape*)createBoxShape(implicitShapeDimensions/localScaling+margin);
-							//box->initializePolyhedralFeatures();
-							shape = box;
+					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 SPHERE_SHAPE_PROXYTYPE:
+						case 1:
 						{
-							shape = createSphereShape(implicitShapeDimensions.getX());
+							shape = createCylinderShapeY(halfExtents.getX(), halfExtents.getY());
 							break;
 						}
-
-					case CYLINDER_SHAPE_PROXYTYPE:
+						case 2:
 						{
-							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");
-								}
-
-							};
-
-
-
+							shape = createCylinderShapeZ(halfExtents.getX(), halfExtents.getZ());
 							break;
 						}
-					case CONE_SHAPE_PROXYTYPE:
+						default:
 						{
-							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");
-								}
-
-							};
-
-
+							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 MULTI_SPHERE_SHAPE_PROXYTYPE:
+						case 1:
 						{
-							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);
+							shape = createConeShapeY(halfExtents.getX(), halfExtents.getY());
 							break;
 						}
-					case CONVEX_HULL_SHAPE_PROXYTYPE:
+						case 2:
 						{
-						//	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;
+							shape = createConeShapeZ(halfExtents.getX(), halfExtents.getZ());
 							break;
 						}
-					default:
+						default:
 						{
-							printf("error: cannot create shape type (%d)\n",shapeData->m_shapeType);
+							printf("unknown Cone up axis\n");
 						}
-				}
+					};
 
-				if (shape)
+					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:
 				{
-					shape->setMargin(bsd->m_collisionMargin);
+					//	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);
+				}
+			}
 
-					btVector3 localScaling;
-					localScaling.deSerializeFloat(bsd->m_localScaling);
-					shape->setLocalScaling(localScaling);
+			if (shape)
+			{
+				shape->setMargin(bsd->m_collisionMargin);
 
-				}
-				break;
+				btVector3 localScaling;
+				localScaling.deSerializeFloat(bsd->m_localScaling);
+				shape->setLocalScaling(localScaling);
 			}
+			break;
+		}
 		case TRIANGLE_MESH_SHAPE_PROXYTYPE:
 		{
 			btTriangleMeshShapeData* trimesh = (btTriangleMeshShapeData*)shapeData;
@@ -522,10 +494,10 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape(  btCollisionS
 				return 0;
 			}
 
-			btVector3 scaling; scaling.deSerializeFloat(trimesh->m_meshInterface.m_scaling);
+			btVector3 scaling;
+			scaling.deSerializeFloat(trimesh->m_meshInterface.m_scaling);
 			meshInterface->setScaling(scaling);
 
-
 			btOptimizedBvh* bvh = 0;
 #if 1
 			if (trimesh->m_quantizedFloatBvh)
@@ -534,7 +506,8 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape(  btCollisionS
 				if (bvhPtr && *bvhPtr)
 				{
 					bvh = *bvhPtr;
-				} else
+				}
+				else
 				{
 					bvh = createOptimizedBvh();
 					bvh->deSerializeFloat(*trimesh->m_quantizedFloatBvh);
@@ -546,7 +519,8 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape(  btCollisionS
 				if (bvhPtr && *bvhPtr)
 				{
 					bvh = *bvhPtr;
-				} else
+				}
+				else
 				{
 					bvh = createOptimizedBvh();
 					bvh->deSerializeDouble(*trimesh->m_quantizedDoubleBvh);
@@ -554,8 +528,7 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape(  btCollisionS
 			}
 #endif
 
-
-			btBvhTriangleMeshShape* trimeshShape = createBvhTriangleMeshShape(meshInterface,bvh);
+			btBvhTriangleMeshShape* trimeshShape = createBvhTriangleMeshShape(meshInterface, bvh);
 			trimeshShape->setMargin(trimesh->m_collisionMargin);
 			shape = trimeshShape;
 
@@ -567,67 +540,66 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape(  btCollisionS
 
 #ifdef USE_INTERNAL_EDGE_UTILITY
 				gContactAddedCallback = btAdjustInternalEdgeContactsCallback;
-#endif //USE_INTERNAL_EDGE_UTILITY
-
+#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++)
 			{
-				btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData;
-				btCompoundShape* compoundShape = createCompoundShape();
+				//btCompoundShapeChildData* ptr = &compoundData->m_childShapePtr[i];
 
+				btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape;
 
-				btAlignedObjectArray<btCollisionShape*> childShapes;
-				for (int i=0;i<compoundData->m_numChildShapes;i++)
+				btCollisionShape* childShape = convertCollisionShape(cd);
+				if (childShape)
+				{
+					btTransform localTransform;
+					localTransform.deSerializeFloat(compoundData->m_childShapePtr[i].m_transform);
+					compoundShape->addChildShape(localTransform, childShape);
+				}
+				else
 				{
-					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");
+					printf("error: couldn't create childShape for compoundShape\n");
 #endif
-					}
-
 				}
-				shape = compoundShape;
-
-				break;
 			}
+			shape = compoundShape;
+
+			break;
+		}
 		case SOFTBODY_SHAPE_PROXYTYPE:
-			{
-				return 0;
-			}
+		{
+			return 0;
+		}
 		default:
-			{
+		{
 #ifdef _DEBUG
-				printf("unsupported shape type (%d)\n",shapeData->m_shapeType);
+			printf("unsupported shape type (%d)\n", shapeData->m_shapeType);
 #endif
-			}
 		}
+	}
 
-		return shape;
-
+	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);
+		char* newName = new char[l + 1];
+		memcpy(newName, name, l);
 		newName[l] = 0;
 		m_allocatedNames.push_back(newName);
 		return newName;
@@ -635,53 +607,43 @@ char* btCollisionWorldImporter::duplicateName(const char* name)
 	return 0;
 }
 
-
-
-
-
-
-
-
-
-
-
-btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStridingMeshInterfaceData&  meshData)
+btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStridingMeshInterfaceData& meshData)
 {
 	btTriangleIndexVertexArray* meshInterface = createTriangleMeshContainer();
 
-	for (int i=0;i<meshData.m_numMeshParts;i++)
+	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);
+			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++)
+			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
+		}
+		else
 		{
 			if (meshData.m_meshPartsPtr[i].m_3indices16)
 			{
 				meshPart.m_indexType = PHY_SHORT;
-				meshPart.m_triangleIndexStride = sizeof(short int)*3;//sizeof(btShortIntIndexTripletData);
+				meshPart.m_triangleIndexStride = sizeof(short int) * 3;  //sizeof(btShortIntIndexTripletData);
 
-				short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int)*3*meshPart.m_numTriangles,16);
+				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++)
+				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];
+					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;
@@ -689,10 +651,10 @@ btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStri
 			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);
+				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++)
+				for (int j = 0; j < 3 * meshPart.m_numTriangles; j++)
 				{
 					indexArray[j] = meshData.m_meshPartsPtr[i].m_indices16[j].m_value;
 				}
@@ -703,16 +665,16 @@ btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStri
 			if (meshData.m_meshPartsPtr[i].m_3indices8)
 			{
 				meshPart.m_indexType = PHY_UCHAR;
-				meshPart.m_triangleIndexStride = sizeof(unsigned char)*3;
+				meshPart.m_triangleIndexStride = sizeof(unsigned char) * 3;
 
-				unsigned char* indexArray = (unsigned char*)btAlignedAlloc(sizeof(unsigned char)*3*meshPart.m_numTriangles,16);
+				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++)
+				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];
+					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;
@@ -723,10 +685,10 @@ btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStri
 		{
 			meshPart.m_vertexType = PHY_FLOAT;
 			meshPart.m_vertexStride = sizeof(btVector3FloatData);
-			btVector3FloatData* vertices = (btVector3FloatData*) btAlignedAlloc(sizeof(btVector3FloatData)*meshPart.m_numVertices,16);
+			btVector3FloatData* vertices = (btVector3FloatData*)btAlignedAlloc(sizeof(btVector3FloatData) * meshPart.m_numVertices, 16);
 			m_floatVertexArrays.push_back(vertices);
 
-			for (int j=0;j<meshPart.m_numVertices;j++)
+			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];
@@ -734,16 +696,16 @@ btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStri
 				vertices[j].m_floats[3] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[3];
 			}
 			meshPart.m_vertexBase = (const unsigned char*)vertices;
-		} else
+		}
+		else
 		{
 			meshPart.m_vertexType = PHY_DOUBLE;
 			meshPart.m_vertexStride = sizeof(btVector3DoubleData);
 
-
-			btVector3DoubleData* vertices = (btVector3DoubleData*) btAlignedAlloc(sizeof(btVector3DoubleData)*meshPart.m_numVertices,16);
+			btVector3DoubleData* vertices = (btVector3DoubleData*)btAlignedAlloc(sizeof(btVector3DoubleData) * meshPart.m_numVertices, 16);
 			m_doubleVertexArrays.push_back(vertices);
 
-			for (int j=0;j<meshPart.m_numVertices;j++)
+			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];
@@ -755,14 +717,13 @@ btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStri
 
 		if (meshPart.m_triangleIndexBase && meshPart.m_vertexBase)
 		{
-			meshInterface->addIndexedMesh(meshPart,meshPart.m_indexType);
+			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
@@ -772,7 +733,7 @@ btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfa
 	newData->m_numMeshParts = interfaceData->m_numMeshParts;
 	newData->m_meshPartsPtr = new btMeshPartData[newData->m_numMeshParts];
 
-	for(int i = 0;i < newData->m_numMeshParts;i++)
+	for (int i = 0; i < newData->m_numMeshParts; i++)
 	{
 		btMeshPartData* curPart = &interfaceData->m_meshPartsPtr[i];
 		btMeshPartData* curNewPart = &newData->m_meshPartsPtr[i];
@@ -780,18 +741,18 @@ btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfa
 		curNewPart->m_numTriangles = curPart->m_numTriangles;
 		curNewPart->m_numVertices = curPart->m_numVertices;
 
-		if(curPart->m_vertices3f)
+		if (curPart->m_vertices3f)
 		{
 			curNewPart->m_vertices3f = new btVector3FloatData[curNewPart->m_numVertices];
-			memcpy(curNewPart->m_vertices3f,curPart->m_vertices3f,sizeof(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)
+		if (curPart->m_vertices3d)
 		{
 			curNewPart->m_vertices3d = new btVector3DoubleData[curNewPart->m_numVertices];
-			memcpy(curNewPart->m_vertices3d,curPart->m_vertices3d,sizeof(btVector3DoubleData) * curNewPart->m_numVertices);
+			memcpy(curNewPart->m_vertices3d, curPart->m_vertices3d, sizeof(btVector3DoubleData) * curNewPart->m_numVertices);
 		}
 		else
 			curNewPart->m_vertices3d = NULL;
@@ -799,63 +760,60 @@ btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfa
 		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;
+		bool uninitialized3indices8Workaround = false;
 
-		if(curPart->m_indices32)
+		if (curPart->m_indices32)
 		{
-			uninitialized3indices8Workaround=true;
+			uninitialized3indices8Workaround = true;
 			curNewPart->m_indices32 = new btIntIndexData[numIndices];
-			memcpy(curNewPart->m_indices32,curPart->m_indices32,sizeof(btIntIndexData) * numIndices);
+			memcpy(curNewPart->m_indices32, curPart->m_indices32, sizeof(btIntIndexData) * numIndices);
 		}
 		else
 			curNewPart->m_indices32 = NULL;
 
-		if(curPart->m_3indices16)
+		if (curPart->m_3indices16)
 		{
-			uninitialized3indices8Workaround=true;
+			uninitialized3indices8Workaround = true;
 			curNewPart->m_3indices16 = new btShortIntIndexTripletData[curNewPart->m_numTriangles];
-			memcpy(curNewPart->m_3indices16,curPart->m_3indices16,sizeof(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)
+		if (curPart->m_indices16)
 		{
-			uninitialized3indices8Workaround=true;
+			uninitialized3indices8Workaround = true;
 			curNewPart->m_indices16 = new btShortIntIndexData[numIndices];
-			memcpy(curNewPart->m_indices16,curPart->m_indices16,sizeof(btShortIntIndexData) * numIndices);
+			memcpy(curNewPart->m_indices16, curPart->m_indices16, sizeof(btShortIntIndexData) * numIndices);
 		}
 		else
 			curNewPart->m_indices16 = NULL;
 
-		if(!uninitialized3indices8Workaround && curPart->m_3indices8)
+		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);
+			memcpy(curNewPart->m_3indices8, curPart->m_3indices8, sizeof(btCharIndexTripletData) * curNewPart->m_numTriangles);
 		}
 		else
 			curNewPart->m_3indices8 = NULL;
-
 	}
 
 	m_allocatedbtStridingMeshInterfaceDatas.push_back(newData);
 
-	return(newData);
+	return (newData);
 }
 
 #ifdef USE_INTERNAL_EDGE_UTILITY
-extern ContactAddedCallback		gContactAddedCallback;
+extern ContactAddedCallback gContactAddedCallback;
 
-static bool btAdjustInternalEdgeContactsCallback(btManifoldPoint& cp,	const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1)
+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);
+	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
-
+#endif  //USE_INTERNAL_EDGE_UTILITY
 
 /*
 btRigidBody*  btWorldImporter::createRigidBody(bool isDynamic, btScalar mass, const btTransform& startTransform,btCollisionShape* shape,const char* bodyName)
@@ -894,29 +852,27 @@ btCollisionObject* btCollisionWorldImporter::getCollisionObjectByName(const char
 	return 0;
 }
 
-btCollisionObject* btCollisionWorldImporter::createCollisionObject(const btTransform& startTransform,btCollisionShape* shape, const char* bodyName)
+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
+	m_collisionWorld->addCollisionObject(colObj);  //todo: flags etc
 
 	if (bodyName)
 	{
 		char* newname = duplicateName(bodyName);
-		m_objectNameMap.insert(colObj,newname);
-		m_nameColObjMap.insert(newname,colObj);
+		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)
+btCollisionShape* btCollisionWorldImporter::createPlaneShape(const btVector3& planeNormal, btScalar planeConstant)
 {
-	btStaticPlaneShape* shape = new btStaticPlaneShape(planeNormal,planeConstant);
+	btStaticPlaneShape* shape = new btStaticPlaneShape(planeNormal, planeConstant);
 	m_allocatedCollisionShapes.push_back(shape);
 	return shape;
 }
@@ -933,85 +889,83 @@ btCollisionShape* btCollisionWorldImporter::createSphereShape(btScalar radius)
 	return shape;
 }
 
-
 btCollisionShape* btCollisionWorldImporter::createCapsuleShapeX(btScalar radius, btScalar height)
 {
-	btCapsuleShapeX* shape = new btCapsuleShapeX(radius,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);
+	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);
+	btCapsuleShapeZ* shape = new btCapsuleShapeZ(radius, height);
 	m_allocatedCollisionShapes.push_back(shape);
 	return shape;
 }
 
-btCollisionShape* btCollisionWorldImporter::createCylinderShapeX(btScalar radius,btScalar height)
+btCollisionShape* btCollisionWorldImporter::createCylinderShapeX(btScalar radius, btScalar height)
 {
-	btCylinderShapeX* shape = new btCylinderShapeX(btVector3(height,radius,radius));
+	btCylinderShapeX* shape = new btCylinderShapeX(btVector3(height, radius, radius));
 	m_allocatedCollisionShapes.push_back(shape);
 	return shape;
 }
 
-btCollisionShape* btCollisionWorldImporter::createCylinderShapeY(btScalar radius,btScalar height)
+btCollisionShape* btCollisionWorldImporter::createCylinderShapeY(btScalar radius, btScalar height)
 {
-	btCylinderShape* shape = new btCylinderShape(btVector3(radius,height,radius));
+	btCylinderShape* shape = new btCylinderShape(btVector3(radius, height, radius));
 	m_allocatedCollisionShapes.push_back(shape);
 	return shape;
 }
 
-btCollisionShape* btCollisionWorldImporter::createCylinderShapeZ(btScalar radius,btScalar height)
+btCollisionShape* btCollisionWorldImporter::createCylinderShapeZ(btScalar radius, btScalar height)
 {
-	btCylinderShapeZ* shape = new btCylinderShapeZ(btVector3(radius,radius,height));
+	btCylinderShapeZ* shape = new btCylinderShapeZ(btVector3(radius, radius, height));
 	m_allocatedCollisionShapes.push_back(shape);
 	return shape;
 }
 
-btCollisionShape* btCollisionWorldImporter::createConeShapeX(btScalar radius,btScalar height)
+btCollisionShape* btCollisionWorldImporter::createConeShapeX(btScalar radius, btScalar height)
 {
-	btConeShapeX* shape = new btConeShapeX(radius,height);
+	btConeShapeX* shape = new btConeShapeX(radius, height);
 	m_allocatedCollisionShapes.push_back(shape);
 	return shape;
 }
 
-btCollisionShape* btCollisionWorldImporter::createConeShapeY(btScalar radius,btScalar height)
+btCollisionShape* btCollisionWorldImporter::createConeShapeY(btScalar radius, btScalar height)
 {
-	btConeShape* shape = new btConeShape(radius,height);
+	btConeShape* shape = new btConeShape(radius, height);
 	m_allocatedCollisionShapes.push_back(shape);
 	return shape;
 }
 
-btCollisionShape* btCollisionWorldImporter::createConeShapeZ(btScalar radius,btScalar height)
+btCollisionShape* btCollisionWorldImporter::createConeShapeZ(btScalar radius, btScalar height)
 {
-	btConeShapeZ* shape = new btConeShapeZ(radius,height);
+	btConeShapeZ* shape = new btConeShapeZ(radius, height);
 	m_allocatedCollisionShapes.push_back(shape);
 	return shape;
 }
 
-btTriangleIndexVertexArray*	btCollisionWorldImporter::createTriangleMeshContainer()
+btTriangleIndexVertexArray* btCollisionWorldImporter::createTriangleMeshContainer()
 {
 	btTriangleIndexVertexArray* in = new btTriangleIndexVertexArray();
 	m_allocatedTriangleIndexArrays.push_back(in);
 	return in;
 }
 
-btOptimizedBvh*	btCollisionWorldImporter::createOptimizedBvh()
+btOptimizedBvh* btCollisionWorldImporter::createOptimizedBvh()
 {
 	btOptimizedBvh* bvh = new btOptimizedBvh();
 	m_allocatedBvhs.push_back(bvh);
 	return bvh;
 }
 
-
 btTriangleInfoMap* btCollisionWorldImporter::createTriangleInfoMap()
 {
 	btTriangleInfoMap* tim = new btTriangleInfoMap();
@@ -1023,16 +977,15 @@ btBvhTriangleMeshShape* btCollisionWorldImporter::createBvhTriangleMeshShape(btS
 {
 	if (bvh)
 	{
-		btBvhTriangleMeshShape* bvhTriMesh = new btBvhTriangleMeshShape(trimesh,bvh->isQuantized(), false);
+		btBvhTriangleMeshShape* bvhTriMesh = new btBvhTriangleMeshShape(trimesh, bvh->isQuantized(), false);
 		bvhTriMesh->setOptimizedBvh(bvh);
 		m_allocatedCollisionShapes.push_back(bvhTriMesh);
 		return bvhTriMesh;
 	}
 
-	btBvhTriangleMeshShape* ts = new btBvhTriangleMeshShape(trimesh,true);
+	btBvhTriangleMeshShape* ts = new btBvhTriangleMeshShape(trimesh, true);
 	m_allocatedCollisionShapes.push_back(ts);
 	return ts;
-
 }
 btCollisionShape* btCollisionWorldImporter::createConvexTriangleMeshShape(btStridingMeshInterface* trimesh)
 {
@@ -1044,9 +997,8 @@ btGImpactMeshShape* btCollisionWorldImporter::createGimpactShape(btStridingMeshI
 	btGImpactMeshShape* shape = new btGImpactMeshShape(trimesh);
 	m_allocatedCollisionShapes.push_back(shape);
 	return shape;
-
 }
-#endif //SUPPORT_GIMPACT_SHAPE_IMPORT
+#endif  //SUPPORT_GIMPACT_SHAPE_IMPORT
 
 btConvexHullShape* btCollisionWorldImporter::createConvexHullShape()
 {
@@ -1062,25 +1014,22 @@ btCompoundShape* btCollisionWorldImporter::createCompoundShape()
 	return shape;
 }
 
-
-btScaledBvhTriangleMeshShape* btCollisionWorldImporter::createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScaling)
+btScaledBvhTriangleMeshShape* btCollisionWorldImporter::createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape, const btVector3& localScaling)
 {
-	btScaledBvhTriangleMeshShape* shape = new btScaledBvhTriangleMeshShape(meshShape,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* 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
+// query for data
+int btCollisionWorldImporter::getNumCollisionShapes() const
 {
 	return m_allocatedCollisionShapes.size();
 }
@@ -1093,23 +1042,21 @@ btCollisionShape* btCollisionWorldImporter::getCollisionShapeByIndex(int index)
 btCollisionShape* btCollisionWorldImporter::getCollisionShapeByName(const char* name)
 {
 	btCollisionShape** shapePtr = m_nameShapeMap.find(name);
-	if (shapePtr&& *shapePtr)
+	if (shapePtr && *shapePtr)
 	{
 		return *shapePtr;
 	}
 	return 0;
 }
 
-
-const char*	btCollisionWorldImporter::getNameForPointer(const void* ptr) const
+const char* btCollisionWorldImporter::getNameForPointer(const void* ptr) const
 {
-	const char*const * namePtr = m_objectNameMap.find(ptr);
+	const char* const* namePtr = m_objectNameMap.find(ptr);
 	if (namePtr && *namePtr)
 		return *namePtr;
 	return 0;
 }
 
-
 int btCollisionWorldImporter::getNumRigidBodies() const
 {
 	return m_allocatedRigidBodies.size();
@@ -1120,12 +1067,11 @@ btCollisionObject* btCollisionWorldImporter::getRigidBodyByIndex(int index) cons
 	return m_allocatedRigidBodies[index];
 }
 
-
 int btCollisionWorldImporter::getNumBvhs() const
 {
 	return m_allocatedBvhs.size();
 }
- btOptimizedBvh* btCollisionWorldImporter::getBvhByIndex(int index) const
+btOptimizedBvh* btCollisionWorldImporter::getBvhByIndex(int index) const
 {
 	return m_allocatedBvhs[index];
 }
@@ -1139,5 +1085,3 @@ btTriangleInfoMap* btCollisionWorldImporter::getTriangleInfoMapByIndex(int index
 {
 	return m_allocatedTriangleInfoMaps[index];
 }
-
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h
index 9a6d16fbea7..5e8bc95341d 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h
@@ -13,7 +13,6 @@ subject to the following restrictions:
 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
 
@@ -26,7 +25,6 @@ class btCollisionShape;
 class btCollisionObject;
 struct btBulletSerializedArrays;
 
-
 struct ConstraintInput;
 class btCollisionWorld;
 struct btCollisionShapeData;
@@ -46,9 +44,6 @@ class btSliderConstraint;
 class btGearConstraint;
 struct btContactSolverInfo;
 
-
-
-
 class btCollisionWorldImporter
 {
 protected:
@@ -56,60 +51,53 @@ protected:
 
 	int m_verboseMode;
 
-	btAlignedObjectArray<btCollisionShape*>  m_allocatedCollisionShapes;
+	btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
 	btAlignedObjectArray<btCollisionObject*> m_allocatedRigidBodies;
 
-	btAlignedObjectArray<btOptimizedBvh*>	 m_allocatedBvhs;
+	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<char*>				m_allocatedNames;
+	btAlignedObjectArray<int*> m_indexArrays;
+	btAlignedObjectArray<short int*> m_shortIndexArrays;
+	btAlignedObjectArray<unsigned char*> m_charIndexArrays;
 
-	btAlignedObjectArray<int*>				m_indexArrays;
-	btAlignedObjectArray<short int*>		m_shortIndexArrays;
-	btAlignedObjectArray<unsigned char*>	m_charIndexArrays;
+	btAlignedObjectArray<btVector3FloatData*> m_floatVertexArrays;
+	btAlignedObjectArray<btVector3DoubleData*> m_doubleVertexArrays;
 
-	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,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;
+	btHashMap<btHashPtr, const char*> m_objectNameMap;
 
+	btHashMap<btHashPtr, btCollisionShape*> m_shapeMap;
+	btHashMap<btHashPtr, btCollisionObject*> m_bodyMap;
 
 	//methods
 
+	char* duplicateName(const char* name);
 
-
-	char*	duplicateName(const char* name);
-
-	btCollisionShape* convertCollisionShape(  btCollisionShapeData* shapeData  );
-
+	btCollisionShape* convertCollisionShape(btCollisionShapeData* shapeData);
 
 public:
-
 	btCollisionWorldImporter(btCollisionWorld* world);
 
 	virtual ~btCollisionWorldImporter();
 
-    bool	convertAllObjects( btBulletSerializedArrays* arrays);
+	bool convertAllObjects(btBulletSerializedArrays* arrays);
 
-		///delete all memory collision shapes, rigid bodies, constraints etc. allocated during the load.
+	///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)
+	void setVerboseMode(int verboseMode)
 	{
 		m_verboseMode = verboseMode;
 	}
@@ -119,15 +107,14 @@ public:
 		return m_verboseMode;
 	}
 
-		// query for data
-	int	getNumCollisionShapes() const;
+	// 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;
+	btOptimizedBvh* getBvhByIndex(int index) const;
 	int getNumTriangleInfoMaps() const;
 	btTriangleInfoMap* getTriangleInfoMapByIndex(int index) const;
 
@@ -135,56 +122,48 @@ public:
 	btCollisionShape* getCollisionShapeByName(const char* name);
 	btCollisionObject* getCollisionObjectByName(const char* name);
 
-
-	const char*	getNameForPointer(const void* ptr) const;
+	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);
+	virtual btCollisionObject* createCollisionObject(const btTransform& startTransform, btCollisionShape* shape, const char* bodyName);
 
 	///shapes
 
-	virtual btCollisionShape* createPlaneShape(const btVector3& planeNormal,btScalar planeConstant);
+	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* 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
+#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 btScaledBvhTriangleMeshShape* createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape, const btVector3& localScalingbtBvhTriangleMeshShape);
 
-	virtual class btMultiSphereShape* createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres);
+	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 btOptimizedBvh* createOptimizedBvh();
 	virtual btTriangleInfoMap* createTriangleInfoMap();
-
-
-
-
 };
 
-
-#endif //BT_WORLD_IMPORTER_H
+#endif  //BT_WORLD_IMPORTER_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
index 589a7f55f2b..b5f4a3c869f 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
@@ -25,56 +25,58 @@ subject to the following restrictions:
 
 btShapePairCallback gCompoundChildShapePairCallback = 0;
 
-btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
-:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
-m_isSwapped(isSwapped),
-m_sharedManifold(ci.m_manifold)
+btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
+	: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
+	  m_isSwapped(isSwapped),
+	  m_sharedManifold(ci.m_manifold)
 {
 	m_ownsManifold = false;
 
-	const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
-	btAssert (colObjWrap->getCollisionShape()->isCompound());
-	
+	const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
+	btAssert(colObjWrap->getCollisionShape()->isCompound());
+
 	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
 	m_compoundShapeRevision = compoundShape->getUpdateRevision();
-	
-	
-	preallocateChildAlgorithms(body0Wrap,body1Wrap);
+
+	preallocateChildAlgorithms(body0Wrap, body1Wrap);
 }
 
-void	btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 {
-	const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
-	const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap;
-	btAssert (colObjWrap->getCollisionShape()->isCompound());
-	
+	const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
+	const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
+	btAssert(colObjWrap->getCollisionShape()->isCompound());
+
 	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
 
 	int numChildren = compoundShape->getNumChildShapes();
 	int i;
-	
+
 	m_childCollisionAlgorithms.resize(numChildren);
-	for (i=0;i<numChildren;i++)
+	for (i = 0; i < numChildren; i++)
 	{
 		if (compoundShape->getDynamicAabbTree())
 		{
 			m_childCollisionAlgorithms[i] = 0;
-		} else
+		}
+		else
 		{
-			
 			const btCollisionShape* childShape = compoundShape->getChildShape(i);
 
-			btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully)
-			m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold);
+			btCollisionObjectWrapper childWrap(colObjWrap, childShape, colObjWrap->getCollisionObject(), colObjWrap->getWorldTransform(), -1, i);  //wrong child trans, but unused (hopefully)
+			m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap, otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
+
+			btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsContact;
+			btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsClosestPoints;
 		}
 	}
 }
 
-void	btCompoundCollisionAlgorithm::removeChildAlgorithms()
+void btCompoundCollisionAlgorithm::removeChildAlgorithms()
 {
 	int numChildren = m_childCollisionAlgorithms.size();
 	int i;
-	for (i=0;i<numChildren;i++)
+	for (i = 0; i < numChildren; i++)
 	{
 		if (m_childCollisionAlgorithms[i])
 		{
@@ -89,66 +91,79 @@ btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
 	removeChildAlgorithms();
 }
 
-
-
-
-struct	btCompoundLeafCallback : btDbvt::ICollide
+struct btCompoundLeafCallback : btDbvt::ICollide
 {
-
 public:
-
 	const btCollisionObjectWrapper* m_compoundColObjWrap;
 	const btCollisionObjectWrapper* m_otherObjWrap;
 	btDispatcher* m_dispatcher;
 	const btDispatcherInfo& m_dispatchInfo;
-	btManifoldResult*	m_resultOut;
-	btCollisionAlgorithm**	m_childCollisionAlgorithms;
-	btPersistentManifold*	m_sharedManifold;
-	
-	btCompoundLeafCallback (const btCollisionObjectWrapper* compoundObjWrap,const btCollisionObjectWrapper* otherObjWrap,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult*	resultOut,btCollisionAlgorithm**	childCollisionAlgorithms,btPersistentManifold*	sharedManifold)
-		:m_compoundColObjWrap(compoundObjWrap),m_otherObjWrap(otherObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
-		m_childCollisionAlgorithms(childCollisionAlgorithms),
-		m_sharedManifold(sharedManifold)
-	{
+	btManifoldResult* m_resultOut;
+	btCollisionAlgorithm** m_childCollisionAlgorithms;
+	btPersistentManifold* m_sharedManifold;
 
+	btCompoundLeafCallback(const btCollisionObjectWrapper* compoundObjWrap, const btCollisionObjectWrapper* otherObjWrap, btDispatcher* dispatcher, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut, btCollisionAlgorithm** childCollisionAlgorithms, btPersistentManifold* sharedManifold)
+		: m_compoundColObjWrap(compoundObjWrap), m_otherObjWrap(otherObjWrap), m_dispatcher(dispatcher), m_dispatchInfo(dispatchInfo), m_resultOut(resultOut), m_childCollisionAlgorithms(childCollisionAlgorithms), m_sharedManifold(sharedManifold)
+	{
 	}
 
-
-	void	ProcessChildShape(const btCollisionShape* childShape,int index)
+	void ProcessChildShape(const btCollisionShape* childShape, int index)
 	{
-		btAssert(index>=0);
+		btAssert(index >= 0);
 		const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape());
-		btAssert(index<compoundShape->getNumChildShapes());
+		btAssert(index < compoundShape->getNumChildShapes());
 
+		if (gCompoundChildShapePairCallback)
+		{
+			if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape))
+				return;
+		}
 
 		//backup
-		btTransform	orgTrans = m_compoundColObjWrap->getWorldTransform();
-		
+		btTransform orgTrans = m_compoundColObjWrap->getWorldTransform();
+
 		const btTransform& childTrans = compoundShape->getChildTransform(index);
-		btTransform	newChildWorldTrans = orgTrans*childTrans ;
+		btTransform newChildWorldTrans = orgTrans * childTrans;
 
 		//perform an AABB check first
-		btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
-		childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
-		m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
+		btVector3 aabbMin0, aabbMax0;
+		childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0);
 
-		if (gCompoundChildShapePairCallback)
-		{
-			if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape))
-				return;
-		}
+		btVector3 extendAabb(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
+		aabbMin0 -= extendAabb;
+		aabbMax0 += extendAabb;
 
-		if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
-		{
+		btVector3 aabbMin1, aabbMax1;
+		m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1);
 
-			btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index);
 
+		if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
+		{
+			btTransform preTransform = childTrans;
+			if (this->m_compoundColObjWrap->m_preTransform)
+			{
+				preTransform = preTransform *(*(this->m_compoundColObjWrap->m_preTransform));
+			}
+			btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, preTransform, -1, index);
+
+			btCollisionAlgorithm* algo = 0;
+			bool allocatedAlgorithm = false;
 
-			//the contactpoint is still projected back using the original inverted worldtrans
-			if (!m_childCollisionAlgorithms[index])
-				m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap,m_otherObjWrap,m_sharedManifold);
+			if (m_resultOut->m_closestPointDistanceThreshold > 0)
+			{
+				algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
+				allocatedAlgorithm = true;
+			}
+			else
+			{
+				//the contactpoint is still projected back using the original inverted worldtrans
+				if (!m_childCollisionAlgorithms[index])
+				{
+					m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
+				}
+				algo = m_childCollisionAlgorithms[index];
+			}
 
-			
 			const btCollisionObjectWrapper* tmpWrap = 0;
 
 			///detect swapping case
@@ -156,16 +171,16 @@ public:
 			{
 				tmpWrap = m_resultOut->getBody0Wrap();
 				m_resultOut->setBody0Wrap(&compoundWrap);
-				m_resultOut->setShapeIdentifiersA(-1,index);
-			} else
+				m_resultOut->setShapeIdentifiersA(-1, index);
+			}
+			else
 			{
 				tmpWrap = m_resultOut->getBody1Wrap();
 				m_resultOut->setBody1Wrap(&compoundWrap);
-				m_resultOut->setShapeIdentifiersB(-1,index);
+				m_resultOut->setShapeIdentifiersB(-1, index);
 			}
 
-
-			m_childCollisionAlgorithms[index]->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut);
+			algo->processCollision(&compoundWrap, m_otherObjWrap, m_dispatchInfo, m_resultOut);
 
 #if 0
 			if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
@@ -179,14 +194,19 @@ public:
 			if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject())
 			{
 				m_resultOut->setBody0Wrap(tmpWrap);
-			} else
+			}
+			else
 			{
 				m_resultOut->setBody1Wrap(tmpWrap);
 			}
-			
+			if (allocatedAlgorithm)
+			{
+				algo->~btCollisionAlgorithm();
+				m_dispatcher->freeCollisionAlgorithm(algo);
+			}
 		}
 	}
-	void		Process(const btDbvtNode* leaf)
+	void Process(const btDbvtNode* leaf)
 	{
 		int index = leaf->dataAsInt;
 
@@ -203,22 +223,16 @@ public:
 		}
 #endif
 
-		ProcessChildShape(childShape,index);
-
+		ProcessChildShape(childShape, index);
 	}
 };
 
-
-
-
-
-
-void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
-	const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
-	const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap;
+	const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
+	const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
 
-	btAssert (colObjWrap->getCollisionShape()->isCompound());
+	btAssert(colObjWrap->getCollisionShape()->isCompound());
 	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
 
 	///btCompoundShape might have changed:
@@ -227,36 +241,36 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 	{
 		///clear and update all
 		removeChildAlgorithms();
-		
-		preallocateChildAlgorithms(body0Wrap,body1Wrap);
+
+		preallocateChildAlgorithms(body0Wrap, body1Wrap);
 		m_compoundShapeRevision = compoundShape->getUpdateRevision();
 	}
 
-	if (m_childCollisionAlgorithms.size()==0)
+	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);
+	btCompoundLeafCallback callback(colObjWrap, otherObjWrap, m_dispatcher, dispatchInfo, resultOut, &m_childCollisionAlgorithms[0], m_sharedManifold);
 
 	///we need to refresh all contact manifolds
 	///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
 	///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
 	{
 		int i;
-		btManifoldArray manifoldArray;
-		for (i=0;i<m_childCollisionAlgorithms.size();i++)
+		manifoldArray.resize(0);
+		for (i = 0; i < m_childCollisionAlgorithms.size(); i++)
 		{
 			if (m_childCollisionAlgorithms[i])
 			{
 				m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
-				for (int m=0;m<manifoldArray.size();m++)
+				for (int m = 0; m < manifoldArray.size(); m++)
 				{
 					if (manifoldArray[m]->getNumContacts())
 					{
 						resultOut->setPersistentManifold(manifoldArray[m]);
 						resultOut->refreshContactPoints();
-						resultOut->setPersistentManifold(0);//??necessary?
+						resultOut->setPersistentManifold(0);  //??necessary?
 					}
 				}
 				manifoldArray.resize(0);
@@ -266,54 +280,56 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 
 	if (tree)
 	{
-
-		btVector3 localAabbMin,localAabbMax;
+		btVector3 localAabbMin, localAabbMax;
 		btTransform otherInCompoundSpace;
 		otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform();
-		otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
+		otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace, localAabbMin, localAabbMax);
+		btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
+		localAabbMin -= extraExtends;
+		localAabbMax += extraExtends;
 
-		const ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+		const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
 		//process all children, that overlap with  the given AABB bounds
-		tree->collideTV(tree->m_root,bounds,callback);
-
-	} else
+		tree->collideTVNoStackAlloc(tree->m_root, bounds, stack2, callback);
+	}
+	else
 	{
 		//iterate over all children, perform an AABB check inside ProcessChildShape
 		int numChildren = m_childCollisionAlgorithms.size();
 		int i;
-		for (i=0;i<numChildren;i++)
+		for (i = 0; i < numChildren; i++)
 		{
-			callback.ProcessChildShape(compoundShape->getChildShape(i),i);
+			callback.ProcessChildShape(compoundShape->getChildShape(i), i);
 		}
 	}
 
 	{
-				//iterate over all children, perform an AABB check inside ProcessChildShape
+		//iterate over all children, perform an AABB check inside ProcessChildShape
 		int numChildren = m_childCollisionAlgorithms.size();
 		int i;
-		btManifoldArray	manifoldArray;
-        const btCollisionShape* childShape = 0;
-        btTransform	orgTrans;
-        
-        btTransform	newChildWorldTrans;
-        btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;        
-        
-		for (i=0;i<numChildren;i++)
+		manifoldArray.resize(0);
+		const btCollisionShape* childShape = 0;
+		btTransform orgTrans;
+
+		btTransform newChildWorldTrans;
+		btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
+
+		for (i = 0; i < numChildren; i++)
 		{
 			if (m_childCollisionAlgorithms[i])
 			{
 				childShape = compoundShape->getChildShape(i);
-			//if not longer overlapping, remove the algorithm
+				//if not longer overlapping, remove the algorithm
 				orgTrans = colObjWrap->getWorldTransform();
-                
+
 				const btTransform& childTrans = compoundShape->getChildTransform(i);
-                newChildWorldTrans = orgTrans*childTrans ;
+				newChildWorldTrans = orgTrans * childTrans;
 
 				//perform an AABB check first
-				childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
-				otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
+				childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0);
+				otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1);
 
-				if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+				if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
 				{
 					m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
 					m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
@@ -324,15 +340,15 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 	}
 }
 
-btScalar	btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	btAssert(0);
 	//needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures
-	btCollisionObject* colObj = m_isSwapped? body1 : body0;
-	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+	btCollisionObject* colObj = m_isSwapped ? body1 : body0;
+	btCollisionObject* otherObj = m_isSwapped ? body0 : body1;
+
+	btAssert(colObj->getCollisionShape()->isCompound());
 
-	btAssert (colObj->getCollisionShape()->isCompound());
-	
 	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
 
 	//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
@@ -346,33 +362,29 @@ btScalar	btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject*
 
 	int numChildren = m_childCollisionAlgorithms.size();
 	int i;
-    btTransform	orgTrans;
-    btScalar frac;
-	for (i=0;i<numChildren;i++)
+	btTransform orgTrans;
+	btScalar frac;
+	for (i = 0; i < numChildren; i++)
 	{
 		//btCollisionShape* childShape = compoundShape->getChildShape(i);
 
 		//backup
-        orgTrans = colObj->getWorldTransform();
-	
+		orgTrans = colObj->getWorldTransform();
+
 		const btTransform& childTrans = compoundShape->getChildTransform(i);
 		//btTransform	newChildWorldTrans = orgTrans*childTrans ;
-		colObj->setWorldTransform( orgTrans*childTrans );
+		colObj->setWorldTransform(orgTrans * childTrans);
 
 		//btCollisionShape* tmpShape = colObj->getCollisionShape();
 		//colObj->internalSetTemporaryCollisionShape( childShape );
-        frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
-		if (frac<hitFraction)
+		frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj, otherObj, dispatchInfo, resultOut);
+		if (frac < hitFraction)
 		{
 			hitFraction = frac;
 		}
 		//revert back
 		//colObj->internalSetTemporaryCollisionShape( tmpShape);
-		colObj->setWorldTransform( orgTrans);
+		colObj->setWorldTransform(orgTrans);
 	}
 	return hitFraction;
-
 }
-
-
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
index 7d792c18d34..4ea5e77185b 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
@@ -26,6 +26,7 @@ class btDispatcher;
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 #include "btCollisionCreateFunc.h"
 #include "LinearMath/btAlignedObjectArray.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
 class btDispatcher;
 class btCollisionObject;
 
@@ -34,67 +35,65 @@ typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCol
 extern btShapePairCallback gCompoundChildShapePairCallback;
 
 /// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
-class btCompoundCollisionAlgorithm  : public btActivatingCollisionAlgorithm
+class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm
 {
+	btNodeStack stack2;
+	btManifoldArray manifoldArray;
+
 protected:
 	btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
 	bool m_isSwapped;
 
-	class btPersistentManifold*	m_sharedManifold;
-	bool					m_ownsManifold;
+	class btPersistentManifold* m_sharedManifold;
+	bool m_ownsManifold;
 
+	int m_compoundShapeRevision;  //to keep track of changes, so that childAlgorithm array can be updated
 
-	int	m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated
-	
-	void	removeChildAlgorithms();
-	
-	void	preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+	void removeChildAlgorithms();
 
-public:
+	void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap);
 
-	btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
+public:
+	btCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped);
 
 	virtual ~btCompoundCollisionAlgorithm();
 
-	btCollisionAlgorithm* getChildAlgorithm (int n) const
+	btCollisionAlgorithm* getChildAlgorithm(int n) const
 	{
 		return m_childCollisionAlgorithms[n];
 	}
 
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
-
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
 	{
 		int i;
-		for (i=0;i<m_childCollisionAlgorithms.size();i++)
+		for (i = 0; i < m_childCollisionAlgorithms.size(); i++)
 		{
 			if (m_childCollisionAlgorithms[i])
 				m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
 		}
 	}
 
-	
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm));
-			return new(mem) btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false);
+			return new (mem) btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, false);
 		}
 	};
 
-	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm));
-			return new(mem) btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true);
+			return new (mem) btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, true);
 		}
 	};
-
 };
 
-#endif //BT_COMPOUND_COLLISION_ALGORITHM_H
+#endif  //BT_COMPOUND_COLLISION_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
index 1d64d84b87b..044b60dbb1d 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
@@ -15,6 +15,7 @@ subject to the following restrictions:
 */
 
 #include "btCompoundCompoundCollisionAlgorithm.h"
+#include "LinearMath/btQuickprof.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
@@ -23,32 +24,30 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 
+//USE_LOCAL_STACK will avoid most (often all) dynamic memory allocations due to resizing in processCollision and MycollideTT
+#define USE_LOCAL_STACK 1
 
 btShapePairCallback gCompoundCompoundChildShapePairCallback = 0;
 
-btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
-:btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped)
+btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
+	: btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, isSwapped)
 {
-
-	void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16);
-	m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache();
+	void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache), 16);
+	m_childCollisionAlgorithmCache = new (ptr) btHashedSimplePairCache();
 
 	const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
-	btAssert (col0ObjWrap->getCollisionShape()->isCompound());
+	btAssert(col0ObjWrap->getCollisionShape()->isCompound());
 
 	const btCollisionObjectWrapper* col1ObjWrap = body1Wrap;
-	btAssert (col1ObjWrap->getCollisionShape()->isCompound());
-	
+	btAssert(col1ObjWrap->getCollisionShape()->isCompound());
+
 	const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
 	m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
 
 	const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
 	m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
-	
-	
 }
 
-
 btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm()
 {
 	removeChildAlgorithms();
@@ -56,32 +55,30 @@ btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm()
 	btAlignedFree(m_childCollisionAlgorithmCache);
 }
 
-void	btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray&	manifoldArray)
+void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
 {
 	int i;
 	btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
-	for (i=0;i<pairs.size();i++)
+	for (i = 0; i < pairs.size(); i++)
 	{
 		if (pairs[i].m_userPointer)
 		{
-			
 			((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray);
 		}
 	}
 }
 
-
-void	btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms()
+void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms()
 {
 	btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
 
 	int numChildren = pairs.size();
 	int i;
-	for (i=0;i<numChildren;i++)
+	for (i = 0; i < numChildren; i++)
 	{
 		if (pairs[i].m_userPointer)
 		{
-			btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
+			btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
 			algo->~btCollisionAlgorithm();
 			m_dispatcher->freeCollisionAlgorithm(algo);
 		}
@@ -89,106 +86,106 @@ void	btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms()
 	m_childCollisionAlgorithmCache->removeAllPairs();
 }
 
-struct	btCompoundCompoundLeafCallback : btDbvt::ICollide
+struct btCompoundCompoundLeafCallback : btDbvt::ICollide
 {
 	int m_numOverlapPairs;
 
-
 	const btCollisionObjectWrapper* m_compound0ColObjWrap;
 	const btCollisionObjectWrapper* m_compound1ColObjWrap;
 	btDispatcher* m_dispatcher;
 	const btDispatcherInfo& m_dispatchInfo;
-	btManifoldResult*	m_resultOut;
-	
-	
-	class btHashedSimplePairCache*	m_childCollisionAlgorithmCache;
-	
-	btPersistentManifold*	m_sharedManifold;
-	
-	btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap,
-									const btCollisionObjectWrapper* compound0ObjWrap,
-									btDispatcher* dispatcher,
-									const btDispatcherInfo& dispatchInfo,
-									btManifoldResult*	resultOut,
-									btHashedSimplePairCache* childAlgorithmsCache,
-									btPersistentManifold*	sharedManifold)
-		:m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
-		m_childCollisionAlgorithmCache(childAlgorithmsCache),
-		m_sharedManifold(sharedManifold)
-	{
+	btManifoldResult* m_resultOut;
 
-	}
+	class btHashedSimplePairCache* m_childCollisionAlgorithmCache;
 
+	btPersistentManifold* m_sharedManifold;
 
+	btCompoundCompoundLeafCallback(const btCollisionObjectWrapper* compound1ObjWrap,
+								   const btCollisionObjectWrapper* compound0ObjWrap,
+								   btDispatcher* dispatcher,
+								   const btDispatcherInfo& dispatchInfo,
+								   btManifoldResult* resultOut,
+								   btHashedSimplePairCache* childAlgorithmsCache,
+								   btPersistentManifold* sharedManifold)
+		: m_numOverlapPairs(0), m_compound0ColObjWrap(compound1ObjWrap), m_compound1ColObjWrap(compound0ObjWrap), m_dispatcher(dispatcher), m_dispatchInfo(dispatchInfo), m_resultOut(resultOut), m_childCollisionAlgorithmCache(childAlgorithmsCache), m_sharedManifold(sharedManifold)
+	{
+	}
 
-	
-	void		Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1)
+	void Process(const btDbvtNode* leaf0, const btDbvtNode* leaf1)
 	{
+		BT_PROFILE("btCompoundCompoundLeafCallback::Process");
 		m_numOverlapPairs++;
 
-
 		int childIndex0 = leaf0->dataAsInt;
 		int childIndex1 = leaf1->dataAsInt;
-		
-
-		btAssert(childIndex0>=0);
-		btAssert(childIndex1>=0);
 
+		btAssert(childIndex0 >= 0);
+		btAssert(childIndex1 >= 0);
 
 		const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape());
-		btAssert(childIndex0<compoundShape0->getNumChildShapes());
+		btAssert(childIndex0 < compoundShape0->getNumChildShapes());
 
 		const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape());
-		btAssert(childIndex1<compoundShape1->getNumChildShapes());
+		btAssert(childIndex1 < compoundShape1->getNumChildShapes());
 
 		const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0);
 		const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1);
 
 		//backup
-		btTransform	orgTrans0 = m_compound0ColObjWrap->getWorldTransform();
+		btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform();
 		const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0);
-		btTransform	newChildWorldTrans0 = orgTrans0*childTrans0 ;
-		
-		btTransform	orgTrans1 = m_compound1ColObjWrap->getWorldTransform();
+		btTransform newChildWorldTrans0 = orgTrans0 * childTrans0;
+
+		btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform();
 		const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1);
-		btTransform	newChildWorldTrans1 = orgTrans1*childTrans1 ;
-		
+		btTransform newChildWorldTrans1 = orgTrans1 * childTrans1;
 
 		//perform an AABB check first
-		btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
-		childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
-		childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
-		
+		btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
+		childShape0->getAabb(newChildWorldTrans0, aabbMin0, aabbMax0);
+		childShape1->getAabb(newChildWorldTrans1, aabbMin1, aabbMax1);
+
+		btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
+
+		aabbMin0 -= thresholdVec;
+		aabbMax0 += thresholdVec;
+
 		if (gCompoundCompoundChildShapePairCallback)
 		{
-			if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
+			if (!gCompoundCompoundChildShapePairCallback(childShape0, childShape1))
 				return;
 		}
 
-		if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+		if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
 		{
-			btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0);
-			btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1);
-			
-
-			btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
+			btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap, childShape0, m_compound0ColObjWrap->getCollisionObject(), newChildWorldTrans0, -1, childIndex0);
+			btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap, childShape1, m_compound1ColObjWrap->getCollisionObject(), newChildWorldTrans1, -1, childIndex1);
 
+			btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0, childIndex1);
+			bool removePair = false;
 			btCollisionAlgorithm* colAlgo = 0;
-
-			if (pair)
+			if (m_resultOut->m_closestPointDistanceThreshold > 0)
 			{
-				colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
-				
-			} else
+				colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS);
+				removePair = true;
+			}
+			else
 			{
-				colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold);
-				pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1);
-				btAssert(pair);
-				pair->m_userPointer = colAlgo;
+				if (pair)
+				{
+					colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
+				}
+				else
+				{
+					colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
+					pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0, childIndex1);
+					btAssert(pair);
+					pair->m_userPointer = colAlgo;
+				}
 			}
 
 			btAssert(colAlgo);
-						
+
 			const btCollisionObjectWrapper* tmpWrap0 = 0;
 			const btCollisionObjectWrapper* tmpWrap1 = 0;
 
@@ -198,94 +195,100 @@ struct	btCompoundCompoundLeafCallback : btDbvt::ICollide
 			m_resultOut->setBody0Wrap(&compoundWrap0);
 			m_resultOut->setBody1Wrap(&compoundWrap1);
 
-			m_resultOut->setShapeIdentifiersA(-1,childIndex0);
-			m_resultOut->setShapeIdentifiersB(-1,childIndex1);
+			m_resultOut->setShapeIdentifiersA(-1, childIndex0);
+			m_resultOut->setShapeIdentifiersB(-1, childIndex1);
 
+			colAlgo->processCollision(&compoundWrap0, &compoundWrap1, m_dispatchInfo, m_resultOut);
 
-			colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut);
-			
 			m_resultOut->setBody0Wrap(tmpWrap0);
 			m_resultOut->setBody1Wrap(tmpWrap1);
-			
-
 
+			if (removePair)
+			{
+				colAlgo->~btCollisionAlgorithm();
+				m_dispatcher->freeCollisionAlgorithm(colAlgo);
+			}
 		}
 	}
 };
 
-
-static DBVT_INLINE bool		MyIntersect(	const btDbvtAabbMm& a,
-								  const btDbvtAabbMm& b, const btTransform& xform)
+static DBVT_INLINE bool MyIntersect(const btDbvtAabbMm& a,
+									const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold)
 {
-	btVector3 newmin,newmax;
-	btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
-	btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
-	return Intersect(a,newb);
+	btVector3 newmin, newmax;
+	btTransformAabb(b.Mins(), b.Maxs(), 0.f, xform, newmin, newmax);
+	newmin -= btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
+	newmax += btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
+	btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin, newmax);
+	return Intersect(a, newb);
 }
 
-
-static inline void		MycollideTT(	const btDbvtNode* root0,
-								  const btDbvtNode* root1,
-								  const btTransform& xform,
-								  btCompoundCompoundLeafCallback* callback)
+static inline void MycollideTT(const btDbvtNode* root0,
+							   const btDbvtNode* root1,
+							   const btTransform& xform,
+							   btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold)
 {
-
-		if(root0&&root1)
+	if (root0 && root1)
+	{
+		int depth = 1;
+		int treshold = btDbvt::DOUBLE_STACKSIZE - 4;
+		btAlignedObjectArray<btDbvt::sStkNN> stkStack;
+#ifdef USE_LOCAL_STACK
+		ATTRIBUTE_ALIGNED16(btDbvt::sStkNN localStack[btDbvt::DOUBLE_STACKSIZE]);
+		stkStack.initializeFromBuffer(&localStack, btDbvt::DOUBLE_STACKSIZE, btDbvt::DOUBLE_STACKSIZE);
+#else
+		stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
+#endif
+		stkStack[0] = btDbvt::sStkNN(root0, root1);
+		do
 		{
-			int								depth=1;
-			int								treshold=btDbvt::DOUBLE_STACKSIZE-4;
-			btAlignedObjectArray<btDbvt::sStkNN>	stkStack;
-			stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
-			stkStack[0]=btDbvt::sStkNN(root0,root1);
-			do	{
-				btDbvt::sStkNN	p=stkStack[--depth];
-				if(MyIntersect(p.a->volume,p.b->volume,xform))
+			btDbvt::sStkNN p = stkStack[--depth];
+			if (MyIntersect(p.a->volume, p.b->volume, xform, distanceThreshold))
+			{
+				if (depth > treshold)
 				{
-					if(depth>treshold)
+					stkStack.resize(stkStack.size() * 2);
+					treshold = stkStack.size() - 4;
+				}
+				if (p.a->isinternal())
+				{
+					if (p.b->isinternal())
+					{
+						stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b->childs[0]);
+						stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b->childs[0]);
+						stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b->childs[1]);
+						stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b->childs[1]);
+					}
+					else
 					{
-						stkStack.resize(stkStack.size()*2);
-						treshold=stkStack.size()-4;
+						stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b);
+						stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b);
 					}
-					if(p.a->isinternal())
+				}
+				else
+				{
+					if (p.b->isinternal())
 					{
-						if(p.b->isinternal())
-						{					
-							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]);
-							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]);
-							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]);
-							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]);
-						}
-						else
-						{
-							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b);
-							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b);
-						}
+						stkStack[depth++] = btDbvt::sStkNN(p.a, p.b->childs[0]);
+						stkStack[depth++] = btDbvt::sStkNN(p.a, p.b->childs[1]);
 					}
 					else
 					{
-						if(p.b->isinternal())
-						{
-							stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]);
-							stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]);
-						}
-						else
-						{
-							callback->Process(p.a,p.b);
-						}
+						callback->Process(p.a, p.b);
 					}
 				}
-			} while(depth);
-		}
+			}
+		} while (depth);
+	}
 }
 
-void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btCompoundCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
-
 	const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
-	const btCollisionObjectWrapper* col1ObjWrap= body1Wrap;
+	const btCollisionObjectWrapper* col1ObjWrap = body1Wrap;
 
-	btAssert (col0ObjWrap->getCollisionShape()->isCompound());
-	btAssert (col1ObjWrap->getCollisionShape()->isCompound());
+	btAssert(col0ObjWrap->getCollisionShape()->isCompound());
+	btAssert(col1ObjWrap->getCollisionShape()->isCompound());
 	const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
 	const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
 
@@ -293,7 +296,7 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
 	const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
 	if (!tree0 || !tree1)
 	{
-		return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut);
+		return btCompoundCollisionAlgorithm::processCollision(body0Wrap, body1Wrap, dispatchInfo, resultOut);
 	}
 	///btCompoundShape might have changed:
 	////make sure the internal child collision algorithm caches are still valid
@@ -303,24 +306,26 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
 		removeChildAlgorithms();
 		m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
 		m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
-
 	}
 
-
 	///we need to refresh all contact manifolds
 	///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
 	///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
 	{
 		int i;
 		btManifoldArray manifoldArray;
+#ifdef USE_LOCAL_STACK
+		btPersistentManifold localManifolds[4];
+		manifoldArray.initializeFromBuffer(&localManifolds, 0, 4);
+#endif
 		btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
-		for (i=0;i<pairs.size();i++)
+		for (i = 0; i < pairs.size(); i++)
 		{
 			if (pairs[i].m_userPointer)
 			{
-				btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
+				btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
 				algo->getAllContactManifolds(manifoldArray);
-				for (int m=0;m<manifoldArray.size();m++)
+				for (int m = 0; m < manifoldArray.size(); m++)
 				{
 					if (manifoldArray[m]->getNumContacts())
 					{
@@ -334,93 +339,75 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
 		}
 	}
 
+	btCompoundCompoundLeafCallback callback(col0ObjWrap, col1ObjWrap, this->m_dispatcher, dispatchInfo, resultOut, this->m_childCollisionAlgorithmCache, m_sharedManifold);
 
-	
-
-	btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold);
-
-
-	const btTransform	xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
-	MycollideTT(tree0->m_root,tree1->m_root,xform,&callback);
+	const btTransform xform = col0ObjWrap->getWorldTransform().inverse() * col1ObjWrap->getWorldTransform();
+	MycollideTT(tree0->m_root, tree1->m_root, xform, &callback, resultOut->m_closestPointDistanceThreshold);
 
 	//printf("#compound-compound child/leaf overlap =%d                      \r",callback.m_numOverlapPairs);
 
 	//remove non-overlapping child pairs
 
 	{
-		btAssert(m_removePairs.size()==0);
+		btAssert(m_removePairs.size() == 0);
 
 		//iterate over all children, perform an AABB check inside ProcessChildShape
 		btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
-		
+
 		int i;
-		btManifoldArray	manifoldArray;
-        
-		
-
-        
-        
-        btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;        
-        
-		for (i=0;i<pairs.size();i++)
+		btManifoldArray manifoldArray;
+
+		btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
+
+		for (i = 0; i < pairs.size(); i++)
 		{
 			if (pairs[i].m_userPointer)
 			{
 				btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
 
 				{
-					btTransform	orgTrans0;
 					const btCollisionShape* childShape0 = 0;
-					
-					btTransform	newChildWorldTrans0;
-					btTransform	orgInterpolationTrans0;
+
+					btTransform newChildWorldTrans0;
 					childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA);
-					orgTrans0 = col0ObjWrap->getWorldTransform();
-					orgInterpolationTrans0 = col0ObjWrap->getWorldTransform();
 					const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA);
-					newChildWorldTrans0 = orgTrans0*childTrans0 ;
-					childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
+					newChildWorldTrans0 = col0ObjWrap->getWorldTransform() * childTrans0;
+					childShape0->getAabb(newChildWorldTrans0, aabbMin0, aabbMax0);
 				}
-
+				btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
+				aabbMin0 -= thresholdVec;
+				aabbMax0 += thresholdVec;
 				{
-					btTransform	orgInterpolationTrans1;
 					const btCollisionShape* childShape1 = 0;
-					btTransform	orgTrans1;
-					btTransform	newChildWorldTrans1;
+					btTransform newChildWorldTrans1;
 
 					childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB);
-					orgTrans1 = col1ObjWrap->getWorldTransform();
-					orgInterpolationTrans1 = col1ObjWrap->getWorldTransform();
 					const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB);
-					newChildWorldTrans1 = orgTrans1*childTrans1 ;
-					childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
+					newChildWorldTrans1 = col1ObjWrap->getWorldTransform() * childTrans1;
+					childShape1->getAabb(newChildWorldTrans1, aabbMin1, aabbMax1);
 				}
-				
-				
 
-				if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+				aabbMin1 -= thresholdVec;
+				aabbMax1 += thresholdVec;
+
+				if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
 				{
 					algo->~btCollisionAlgorithm();
 					m_dispatcher->freeCollisionAlgorithm(algo);
-					m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB));
+					m_removePairs.push_back(btSimplePair(pairs[i].m_indexA, pairs[i].m_indexB));
 				}
 			}
 		}
-		for (int i=0;i<m_removePairs.size();i++)
+		for (int i = 0; i < m_removePairs.size(); i++)
 		{
-			m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB);
+			m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA, m_removePairs[i].m_indexB);
 		}
 		m_removePairs.clear();
 	}
-
 }
 
-btScalar	btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	btAssert(0);
 	return 0.f;
-
 }
-
-
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h
index 06a762f209d..a940d840e0c 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h
@@ -33,57 +33,50 @@ class btDispatcher;
 class btCollisionObject;
 
 class btCollisionShape;
-typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1);
+
 extern btShapePairCallback gCompoundCompoundChildShapePairCallback;
 
 /// btCompoundCompoundCollisionAlgorithm  supports collision between two btCompoundCollisionShape shapes
-class btCompoundCompoundCollisionAlgorithm  : public btCompoundCollisionAlgorithm
+class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm
 {
-
-	class btHashedSimplePairCache*	m_childCollisionAlgorithmCache;
+	class btHashedSimplePairCache* m_childCollisionAlgorithmCache;
 	btSimplePairArray m_removePairs;
 
+	int m_compoundShapeRevision0;  //to keep track of changes, so that childAlgorithm array can be updated
+	int m_compoundShapeRevision1;
 
-	int	m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated
-	int	m_compoundShapeRevision1;
-	
-	void	removeChildAlgorithms();
-	
-//	void	preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+	void removeChildAlgorithms();
 
-public:
+	//	void	preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
 
-	btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
+public:
+	btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped);
 
 	virtual ~btCompoundCompoundCollisionAlgorithm();
 
-	
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray);
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray);
-	
-	
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm));
-			return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false);
+			return new (mem) btCompoundCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, false);
 		}
 	};
 
-	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm));
-			return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true);
+			return new (mem) btCompoundCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, true);
 		}
 	};
-
 };
 
-#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
+#endif  //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp
index 1cb3d2e7a14..9087f843985 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp
@@ -22,7 +22,6 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
 
-
 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
@@ -34,8 +33,6 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
 
-
-
 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
 
@@ -45,31 +42,28 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 
-btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
 {
 	m_simplexSolver = simplexSolver;
 	m_pdSolver = pdSolver;
 }
 
-btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() 
-{ 
+btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc()
+{
 }
 
-btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */)
-: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
-m_simplexSolver(simplexSolver),
-m_pdSolver(pdSolver),
-m_ownManifold (false),
-m_manifoldPtr(mf),
-m_lowLevelOfDetail(false)
+btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */)
+	: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
+	  m_simplexSolver(simplexSolver),
+	  m_pdSolver(pdSolver),
+	  m_ownManifold(false),
+	  m_manifoldPtr(mf),
+	  m_lowLevelOfDetail(false)
 {
 	(void)body0Wrap;
 	(void)body1Wrap;
 }
 
-
-
-
 btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm()
 {
 	if (m_ownManifold)
@@ -79,26 +73,22 @@ btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm()
 	}
 }
 
-void	btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
+void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
 {
 	m_lowLevelOfDetail = useLowLevel;
 }
 
-
-
 extern btScalar gContactBreakingThreshold;
 
-
 //
 // Convex-Convex collision algorithm
 //
-void btConvex2dConvex2dAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btConvex2dConvex2dAlgorithm ::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
-
 	if (!m_manifoldPtr)
 	{
 		//swapped?
-		m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
 		m_ownManifold = true;
 	}
 	resultOut->setPersistentManifold(m_manifoldPtr);
@@ -106,49 +96,41 @@ void btConvex2dConvex2dAlgorithm ::processCollision (const btCollisionObjectWrap
 	//comment-out next line to test multi-contact generation
 	//resultOut->getPersistentManifold()->clearManifold();
 
-
 	const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
 	const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());
 
-	btVector3  normalOnB;
-	btVector3  pointOnBWorld;
+	btVector3 normalOnB;
+	btVector3 pointOnBWorld;
 
 	{
-
-
 		btGjkPairDetector::ClosestPointInput input;
 
-		btGjkPairDetector	gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
+		btGjkPairDetector gjkPairDetector(min0, min1, m_simplexSolver, m_pdSolver);
 		//TODO: if (dispatchInfo.m_useContinuous)
 		gjkPairDetector.setMinkowskiA(min0);
 		gjkPairDetector.setMinkowskiB(min1);
 
 		{
 			input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
-			input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
+			input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
 		}
 
 		input.m_transformA = body0Wrap->getWorldTransform();
 		input.m_transformB = body1Wrap->getWorldTransform();
 
-		gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+		gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw);
 
-		btVector3 v0,v1;
+		btVector3 v0, v1;
 		btVector3 sepNormalWorldSpace;
-
 	}
 
 	if (m_ownManifold)
 	{
 		resultOut->refreshContactPoints();
 	}
-
 }
 
-
-
-
-btScalar	btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	(void)resultOut;
 	(void)dispatchInfo;
@@ -158,7 +140,6 @@ btScalar	btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* c
 	///col0->m_worldTransform,
 	btScalar resultFraction = btScalar(1.);
 
-
 	btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
 	btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
 
@@ -166,77 +147,65 @@ btScalar	btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* c
 		squareMot1 < col1->getCcdSquareMotionThreshold())
 		return resultFraction;
 
-
 	//An adhoc way of testing the Continuous Collision Detection algorithms
 	//One object is approximated as a sphere, to simplify things
 	//Starting in penetration should report no time of impact
 	//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
 	//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
 
-
 	/// Convex0 against sphere for Convex1
 	{
 		btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
 
-		btSphereShape	sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+		btSphereShape sphere1(col1->getCcdSweptSphereRadius());  //todo: allow non-zero sphere sizes, for better approximation
 		btConvexCast::CastResult result;
 		btVoronoiSimplexSolver voronoiSimplex;
 		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
 		///Simplification, one object is simplified as a sphere
-		btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
+		btGjkConvexCast ccd1(convex0, &sphere1, &voronoiSimplex);
 		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
-		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
-			col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(),
+								  col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result))
 		{
-
 			//store result.m_fraction in both bodies
 
-			if (col0->getHitFraction()> result.m_fraction)
-				col0->setHitFraction( result.m_fraction );
+			if (col0->getHitFraction() > result.m_fraction)
+				col0->setHitFraction(result.m_fraction);
 
 			if (col1->getHitFraction() > result.m_fraction)
-				col1->setHitFraction( result.m_fraction);
+				col1->setHitFraction(result.m_fraction);
 
 			if (resultFraction > result.m_fraction)
 				resultFraction = result.m_fraction;
-
 		}
-
-
-
-
 	}
 
 	/// Sphere (for convex0) against Convex1
 	{
 		btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
 
-		btSphereShape	sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+		btSphereShape sphere0(col0->getCcdSweptSphereRadius());  //todo: allow non-zero sphere sizes, for better approximation
 		btConvexCast::CastResult result;
 		btVoronoiSimplexSolver voronoiSimplex;
 		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
 		///Simplification, one object is simplified as a sphere
-		btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
+		btGjkConvexCast ccd1(&sphere0, convex1, &voronoiSimplex);
 		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
-		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
-			col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(),
+								  col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result))
 		{
-
 			//store result.m_fraction in both bodies
 
-			if (col0->getHitFraction()	> result.m_fraction)
-				col0->setHitFraction( result.m_fraction);
+			if (col0->getHitFraction() > result.m_fraction)
+				col0->setHitFraction(result.m_fraction);
 
 			if (col1->getHitFraction() > result.m_fraction)
-				col1->setHitFraction( result.m_fraction);
+				col1->setHitFraction(result.m_fraction);
 
 			if (resultFraction > result.m_fraction)
 				resultFraction = result.m_fraction;
-
 		}
 	}
 
 	return resultFraction;
-
 }
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h
index 24d13367786..9fca463fbe4 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h
@@ -23,70 +23,61 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
 #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
-#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
+#include "LinearMath/btTransformUtil.h"  //for btConvexSeparatingDistanceUtil
 
 class btConvexPenetrationDepthSolver;
 
-
 ///The convex2dConvex2dAlgorithm collision algorithm support 2d collision detection for btConvex2dShape
 ///Currently it requires the btMinkowskiPenetrationDepthSolver, it has support for 2d penetration depth computation
 class btConvex2dConvex2dAlgorithm : public btActivatingCollisionAlgorithm
 {
-	btSimplexSolverInterface*		m_simplexSolver;
+	btSimplexSolverInterface* m_simplexSolver;
 	btConvexPenetrationDepthSolver* m_pdSolver;
 
-	
-	bool	m_ownManifold;
-	btPersistentManifold*	m_manifoldPtr;
-	bool			m_lowLevelOfDetail;
-	
-public:
-
-	btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
+	bool m_ownManifold;
+	btPersistentManifold* m_manifoldPtr;
+	bool m_lowLevelOfDetail;
 
+public:
+	btConvex2dConvex2dAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
 
 	virtual ~btConvex2dConvex2dAlgorithm();
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
 	{
 		///should we use m_ownManifold to avoid adding duplicates?
 		if (m_manifoldPtr && m_ownManifold)
 			manifoldArray.push_back(m_manifoldPtr);
 	}
 
+	void setLowLevelOfDetail(bool useLowLevel);
 
-	void	setLowLevelOfDetail(bool useLowLevel);
-
-
-	const btPersistentManifold*	getManifold()
+	const btPersistentManifold* getManifold()
 	{
 		return m_manifoldPtr;
 	}
 
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-
-		btConvexPenetrationDepthSolver*		m_pdSolver;
-		btSimplexSolverInterface*			m_simplexSolver;
+		btConvexPenetrationDepthSolver* m_pdSolver;
+		btSimplexSolverInterface* m_simplexSolver;
 		int m_numPerturbationIterations;
 		int m_minimumPointsPerturbationThreshold;
 
-		CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
-		
+		CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+
 		virtual ~CreateFunc();
 
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvex2dConvex2dAlgorithm));
-			return new(mem) btConvex2dConvex2dAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+			return new (mem) btConvex2dConvex2dAlgorithm(ci.m_manifold, ci, body0Wrap, body1Wrap, m_simplexSolver, m_pdSolver, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold);
 		}
 	};
-
-
 };
 
-#endif //BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H
+#endif  //BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
index 912a5285568..e50f85e2bbe 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
@@ -13,8 +13,8 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btConvexConcaveCollisionAlgorithm.h"
+#include "LinearMath/btQuickprof.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
@@ -26,11 +26,12 @@ subject to the following restrictions:
 #include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h"
 
-btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
-: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
-m_isSwapped(isSwapped),
-m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped)
+btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
+	: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
+	  m_btConvexTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped),
+	  m_isSwapped(isSwapped)
 {
 }
 
@@ -38,7 +39,7 @@ btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
 {
 }
 
-void	btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray&	manifoldArray)
+void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
 {
 	if (m_btConvexTriangleCallback.m_manifoldPtr)
 	{
@@ -46,55 +47,46 @@ void	btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray&
 	}
 }
 
-
-btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher*  dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped):
-	  m_dispatcher(dispatcher),
-	m_dispatchInfoPtr(0)
+btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher),
+																																													 m_dispatchInfoPtr(0)
 {
-	m_convexBodyWrap = isSwapped? body1Wrap:body0Wrap;
-	m_triBodyWrap = isSwapped? body0Wrap:body1Wrap;
-	
-	  //
-	  // create the manifold from the dispatcher 'manifold pool'
-	  //
-	  m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(),m_triBodyWrap->getCollisionObject());
+	m_convexBodyWrap = isSwapped ? body1Wrap : body0Wrap;
+	m_triBodyWrap = isSwapped ? body0Wrap : body1Wrap;
+
+	//
+	// create the manifold from the dispatcher 'manifold pool'
+	//
+	m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(), m_triBodyWrap->getCollisionObject());
 
-  	  clearCache();
+	clearCache();
 }
 
 btConvexTriangleCallback::~btConvexTriangleCallback()
 {
 	clearCache();
-	m_dispatcher->releaseManifold( m_manifoldPtr );
-  
+	m_dispatcher->releaseManifold(m_manifoldPtr);
 }
-  
 
-void	btConvexTriangleCallback::clearCache()
+void btConvexTriangleCallback::clearCache()
 {
 	m_dispatcher->clearManifold(m_manifoldPtr);
 }
 
-
-void btConvexTriangleCallback::processTriangle(btVector3* triangle,int
-partId, int triangleIndex)
+void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex)
 {
+	BT_PROFILE("btConvexTriangleCallback::processTriangle");
 
 	if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax))
 	{
 		return;
 	}
 
-        //just for debugging purposes
-        //printf("triangle %d",m_triangleCount++);
-
-
+	//just for debugging purposes
+	//printf("triangle %d",m_triangleCount++);
 
 	btCollisionAlgorithmConstructionInfo ci;
 	ci.m_dispatcher1 = m_dispatcher;
 
-
-
 #if 0	
 	
 	///debug drawing of the overlapping triangles
@@ -108,52 +100,55 @@ partId, int triangleIndex)
 		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
 	}
 #endif
-	
+
 	if (m_convexBodyWrap->getCollisionShape()->isConvex())
 	{
-		btTriangleShape tm(triangle[0],triangle[1],triangle[2]);	
+		btTriangleShape tm(triangle[0], triangle[1], triangle[2]);
 		tm.setMargin(m_collisionMarginTriangle);
-		
-		
-		btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform?
-		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr);
 
+		btCollisionObjectWrapper triObWrap(m_triBodyWrap, &tm, m_triBodyWrap->getCollisionObject(), m_triBodyWrap->getWorldTransform(), partId, triangleIndex);  //correct transform?
+		btCollisionAlgorithm* colAlgo = 0;
+
+		if (m_resultOut->m_closestPointDistanceThreshold > 0)
+		{
+			colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
+		}
+		else
+		{
+			colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, m_manifoldPtr, BT_CONTACT_POINT_ALGORITHMS);
+		}
 		const btCollisionObjectWrapper* tmpWrap = 0;
 
 		if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
 		{
 			tmpWrap = m_resultOut->getBody0Wrap();
 			m_resultOut->setBody0Wrap(&triObWrap);
-			m_resultOut->setShapeIdentifiersA(partId,triangleIndex);
+			m_resultOut->setShapeIdentifiersA(partId, triangleIndex);
 		}
 		else
 		{
 			tmpWrap = m_resultOut->getBody1Wrap();
 			m_resultOut->setBody1Wrap(&triObWrap);
-			m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
+			m_resultOut->setShapeIdentifiersB(partId, triangleIndex);
 		}
-	
-		colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut);
+
+		colAlgo->processCollision(m_convexBodyWrap, &triObWrap, *m_dispatchInfoPtr, m_resultOut);
 
 		if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
 		{
 			m_resultOut->setBody0Wrap(tmpWrap);
-		} else
+		}
+		else
 		{
 			m_resultOut->setBody1Wrap(tmpWrap);
 		}
-		
-
 
 		colAlgo->~btCollisionAlgorithm();
 		ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
 	}
-
 }
 
-
-
-void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
+void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
 {
 	m_convexBodyWrap = convexBodyWrap;
 	m_triBodyWrap = triBodyWrap;
@@ -167,65 +162,120 @@ void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr
 	convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform();
 	const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
 	//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
-	convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
-	btScalar extraMargin = collisionMarginTriangle;
-	btVector3 extra(extraMargin,extraMargin,extraMargin);
+	convexShape->getAabb(convexInTriangleSpace, m_aabbMin, m_aabbMax);
+	btScalar extraMargin = collisionMarginTriangle + resultOut->m_closestPointDistanceThreshold;
+
+	btVector3 extra(extraMargin, extraMargin, extraMargin);
 
 	m_aabbMax += extra;
 	m_aabbMin -= extra;
-	
 }
 
 void btConvexConcaveCollisionAlgorithm::clearCache()
 {
 	m_btConvexTriangleCallback.clearCache();
-
 }
 
-void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btConvexConcaveCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
-	
-	
+	BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
+
 	const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
 	const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
 
 	if (triBodyWrap->getCollisionShape()->isConcave())
 	{
+		if (triBodyWrap->getCollisionShape()->getShapeType() == SDF_SHAPE_PROXYTYPE)
+		{
+			btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape();
+			if (convexBodyWrap->getCollisionShape()->isConvex())
+			{
+				btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape();
+				btAlignedObjectArray<btVector3> queryVertices;
+
+				if (convex->isPolyhedral())
+				{
+					btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex;
+					for (int v = 0; v < poly->getNumVertices(); v++)
+					{
+						btVector3 vtx;
+						poly->getVertex(v, vtx);
+						queryVertices.push_back(vtx);
+					}
+				}
+				btScalar maxDist = SIMD_EPSILON;
+
+				if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
+				{
+					queryVertices.push_back(btVector3(0, 0, 0));
+					btSphereShape* sphere = (btSphereShape*)convex;
+					maxDist = sphere->getRadius() + SIMD_EPSILON;
+				}
+				if (queryVertices.size())
+				{
+					resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+					//m_btConvexTriangleCallback.m_manifoldPtr->clearManifold();
+
+					btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex;
+					for (int v = 0; v < queryVertices.size(); v++)
+					{
+						const btVector3& vtx = queryVertices[v];
+						btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform() * vtx;
+						btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace);
+
+						btVector3 normalLocal;
+						btScalar dist;
+						if (sdfShape->queryPoint(vtxInSdf, dist, normalLocal))
+						{
+							if (dist <= maxDist)
+							{
+								normalLocal.safeNormalize();
+								btVector3 normal = triBodyWrap->getWorldTransform().getBasis() * normalLocal;
+
+								if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
+								{
+									btSphereShape* sphere = (btSphereShape*)convex;
+									dist -= sphere->getRadius();
+									vtxWorldSpace -= sphere->getRadius() * normal;
+								}
+								resultOut->addContactPoint(normal, vtxWorldSpace - normal * dist, dist);
+							}
+						}
+					}
+					resultOut->refreshContactPoints();
+				}
+			}
+		}
+		else
+		{
+			const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>(triBodyWrap->getCollisionShape());
 
+			if (convexBodyWrap->getCollisionShape()->isConvex())
+			{
+				btScalar collisionMarginTriangle = concaveShape->getMargin();
 
-		
-		const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
-		
-		if (convexBodyWrap->getCollisionShape()->isConvex())
-		{
-			btScalar collisionMarginTriangle = concaveShape->getMargin();
-					
-			resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
-			m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
+				resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+				m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, convexBodyWrap, triBodyWrap, resultOut);
 
-			m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
+				m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(), triBodyWrap->getCollisionObject());
 
-			concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
-			
-			resultOut->refreshContactPoints();
+				concaveShape->processAllTriangles(&m_btConvexTriangleCallback, m_btConvexTriangleCallback.getAabbMin(), m_btConvexTriangleCallback.getAabbMax());
 
-			m_btConvexTriangleCallback.clearWrapperData();
-	
+				resultOut->refreshContactPoints();
+
+				m_btConvexTriangleCallback.clearWrapperData();
+			}
 		}
-	
 	}
-
 }
 
-
-btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	(void)resultOut;
 	(void)dispatchInfo;
 	btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
 	btCollisionObject* triBody = m_isSwapped ? body0 : body1;
 
-
 	//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
 
 	//only perform CCD above a certain threshold, this prevents blocking on the long run
@@ -244,27 +294,26 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
 	btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
 	btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
 
-	struct LocalTriangleSphereCastCallback	: public btTriangleCallback
+	struct LocalTriangleSphereCastCallback : public btTriangleCallback
 	{
 		btTransform m_ccdSphereFromTrans;
 		btTransform m_ccdSphereToTrans;
-		btTransform	m_meshTransform;
+		btTransform m_meshTransform;
 
-		btScalar	m_ccdSphereRadius;
-		btScalar	m_hitFraction;
-	
+		btScalar m_ccdSphereRadius;
+		btScalar m_hitFraction;
 
-		LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
-			:m_ccdSphereFromTrans(from),
-			m_ccdSphereToTrans(to),
-			m_ccdSphereRadius(ccdSphereRadius),
-			m_hitFraction(hitFraction)
-		{			
+		LocalTriangleSphereCastCallback(const btTransform& from, const btTransform& to, btScalar ccdSphereRadius, btScalar hitFraction)
+			: m_ccdSphereFromTrans(from),
+			  m_ccdSphereToTrans(to),
+			  m_ccdSphereRadius(ccdSphereRadius),
+			  m_hitFraction(hitFraction)
+		{
 		}
-		
-		
+
 		virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 		{
+			BT_PROFILE("processTriangle");
 			(void)partId;
 			(void)triangleIndex;
 			//do a swept sphere for now
@@ -272,29 +321,23 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
 			ident.setIdentity();
 			btConvexCast::CastResult castResult;
 			castResult.m_fraction = m_hitFraction;
-			btSphereShape	pointShape(m_ccdSphereRadius);
-			btTriangleShape	triShape(triangle[0],triangle[1],triangle[2]);
-			btVoronoiSimplexSolver	simplexSolver;
-			btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
+			btSphereShape pointShape(m_ccdSphereRadius);
+			btTriangleShape triShape(triangle[0], triangle[1], triangle[2]);
+			btVoronoiSimplexSolver simplexSolver;
+			btSubsimplexConvexCast convexCaster(&pointShape, &triShape, &simplexSolver);
 			//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
 			//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
 			//local space?
 
-			if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
-				ident,ident,castResult))
+			if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans,
+											  ident, ident, castResult))
 			{
 				if (m_hitFraction > castResult.m_fraction)
 					m_hitFraction = castResult.m_fraction;
 			}
-
 		}
-
 	};
 
-
-	
-
-	
 	if (triBody->getCollisionShape()->isConcave())
 	{
 		btVector3 rayAabbMin = convexFromLocal.getOrigin();
@@ -302,33 +345,30 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
 		btVector3 rayAabbMax = convexFromLocal.getOrigin();
 		rayAabbMax.setMax(convexToLocal.getOrigin());
 		btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
-		rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
-		rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+		rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
+		rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
 
-		btScalar curHitFraction = btScalar(1.); //is this available?
-		LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
-			convexbody->getCcdSweptSphereRadius(),curHitFraction);
+		btScalar curHitFraction = btScalar(1.);  //is this available?
+		LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal,
+														convexbody->getCcdSweptSphereRadius(), curHitFraction);
 
 		raycastCallback.m_hitFraction = convexbody->getHitFraction();
 
 		btCollisionObject* concavebody = triBody;
 
-		btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
-		
+		btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape();
+
 		if (triangleMesh)
 		{
-			triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
+			triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax);
 		}
-	
-
 
 		if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
 		{
-			convexbody->setHitFraction( raycastCallback.m_hitFraction);
+			convexbody->setHitFraction(raycastCallback.m_hitFraction);
 			return raycastCallback.m_hitFraction;
 		}
 	}
 
 	return btScalar(1.);
-
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
index e90d06eb191..b72e4029811 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
@@ -26,38 +26,40 @@ class btDispatcher;
 #include "btCollisionCreateFunc.h"
 
 ///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called.
-class btConvexTriangleCallback : public btTriangleCallback
+ATTRIBUTE_ALIGNED16(class)
+btConvexTriangleCallback : public btTriangleCallback
 {
+	btVector3 m_aabbMin;
+	btVector3 m_aabbMax;
+
 	const btCollisionObjectWrapper* m_convexBodyWrap;
 	const btCollisionObjectWrapper* m_triBodyWrap;
 
-	btVector3	m_aabbMin;
-	btVector3	m_aabbMax ;
-
-
 	btManifoldResult* m_resultOut;
-	btDispatcher*	m_dispatcher;
+	btDispatcher* m_dispatcher;
 	const btDispatcherInfo* m_dispatchInfoPtr;
 	btScalar m_collisionMarginTriangle;
-	
+
 public:
-int	m_triangleCount;
-	
-	btPersistentManifold*	m_manifoldPtr;
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	int m_triangleCount;
 
-	btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
+	btPersistentManifold* m_manifoldPtr;
 
-	void	setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut);
+	btConvexTriangleCallback(btDispatcher * dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped);
 
-	void	clearWrapperData()
+	void setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut);
+
+	void clearWrapperData()
 	{
 		m_convexBodyWrap = 0;
 		m_triBodyWrap = 0;
 	}
 	virtual ~btConvexTriangleCallback();
 
-	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
-	
+	virtual void processTriangle(btVector3 * triangle, int partId, int triangleIndex);
+
 	void clearCache();
 
 	SIMD_FORCE_INLINE const btVector3& getAabbMin() const
@@ -68,54 +70,48 @@ int	m_triangleCount;
 	{
 		return m_aabbMax;
 	}
-
 };
 
-
-
-
 /// btConvexConcaveCollisionAlgorithm  supports collision between convex shapes and (concave) trianges meshes.
-class btConvexConcaveCollisionAlgorithm  : public btActivatingCollisionAlgorithm
+ATTRIBUTE_ALIGNED16(class)
+btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm
 {
-
-	bool	m_isSwapped;
-
 	btConvexTriangleCallback m_btConvexTriangleCallback;
 
-
+	bool m_isSwapped;
 
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
+	btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped);
 
 	virtual ~btConvexConcaveCollisionAlgorithm();
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
+
+	btScalar calculateTimeOfImpact(btCollisionObject * body0, btCollisionObject * body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void getAllContactManifolds(btManifoldArray & manifoldArray);
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray);
-	
-	void	clearCache();
+	void clearCache();
 
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm));
-			return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,false);
+			return new (mem) btConvexConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, false);
 		}
 	};
 
-	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm));
-			return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,true);
+			return new (mem) btConvexConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, true);
 		}
 	};
-
 };
 
-#endif //BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H
+#endif  //BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
index 7f2722aa463..b48d97f2b28 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
@@ -16,7 +16,7 @@ subject to the following restrictions:
 ///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance
 ///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums
 ///with reproduction case
-//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
+//#define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
 //#define ZERO_MARGIN
 
 #include "btConvexConvexAlgorithm.h"
@@ -28,8 +28,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
-
-
+#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
 
 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
@@ -42,8 +41,6 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
 
-
-
 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
 
@@ -56,8 +53,6 @@ subject to the following restrictions:
 
 ///////////
 
-
-
 static SIMD_FORCE_INLINE void segmentsClosestPoints(
 	btVector3& ptsVector,
 	btVector3& offsetA,
@@ -65,43 +60,49 @@ static SIMD_FORCE_INLINE void segmentsClosestPoints(
 	btScalar& tA, btScalar& tB,
 	const btVector3& translation,
 	const btVector3& dirA, btScalar hlenA,
-	const btVector3& dirB, btScalar hlenB )
+	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 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 ) {
+	if (denom == 0.0f)
+	{
 		tA = 0.0f;
-	} else {
-		tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
-		if ( tA < -hlenA )
+	}
+	else
+	{
+		tA = (dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB) / denom;
+		if (tA < -hlenA)
 			tA = -hlenA;
-		else if ( tA > hlenA )
+		else if (tA > hlenA)
 			tA = hlenA;
 	}
 
 	tB = tA * dirA_dot_dirB - dirB_dot_trans;
 
-	if ( tB < -hlenB ) {
+	if (tB < -hlenB)
+	{
 		tB = -hlenB;
 		tA = tB * dirA_dot_dirB + dirA_dot_trans;
 
-		if ( tA < -hlenA )
+		if (tA < -hlenA)
 			tA = -hlenA;
-		else if ( tA > hlenA )
+		else if (tA > hlenA)
 			tA = hlenA;
-	} else if ( tB > hlenB ) {
+	}
+	else if (tB > hlenB)
+	{
 		tB = hlenB;
 		tA = tB * dirA_dot_dirB + dirA_dot_trans;
 
-		if ( tA < -hlenA )
+		if (tA < -hlenA)
 			tA = -hlenA;
-		else if ( tA > hlenA )
+		else if (tA > hlenA)
 			tA = hlenA;
 	}
 
@@ -113,19 +114,18 @@ static SIMD_FORCE_INLINE void segmentsClosestPoints(
 	ptsVector = translation - offsetA + offsetB;
 }
 
-
 static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
 	btVector3& normalOnB,
 	btVector3& pointOnB,
 	btScalar capsuleLengthA,
-	btScalar	capsuleRadiusA,
+	btScalar capsuleRadiusA,
 	btScalar capsuleLengthB,
-	btScalar	capsuleRadiusB,
+	btScalar capsuleRadiusB,
 	int capsuleAxisA,
 	int capsuleAxisB,
 	const btTransform& transformA,
 	const btTransform& transformB,
-	btScalar distanceThreshold )
+	btScalar distanceThreshold)
 {
 	btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA);
 	btVector3 translationA = transformA.getOrigin();
@@ -138,80 +138,66 @@ static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
 
 	// compute the closest points of the capsule line segments
 
-	btVector3 ptsVector;           // the vector between the closest points
-	
-	btVector3 offsetA, offsetB;    // offsets from segment centers to their closest points
-	btScalar tA, tB;              // parameters on line segment
+	btVector3 ptsVector;  // the vector between the closest points
+
+	btVector3 offsetA, offsetB;  // offsets from segment centers to their closest points
+	btScalar tA, tB;             // parameters on line segment
 
-	segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
-						   directionA, capsuleLengthA, directionB, capsuleLengthB );
+	segmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB, translation,
+						  directionA, capsuleLengthA, directionB, capsuleLengthB);
 
 	btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
 
-	if ( distance > distanceThreshold )
+	if (distance > distanceThreshold)
 		return distance;
 
 	btScalar lenSqr = ptsVector.length2();
-	if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON))
+	if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON))
 	{
 		//degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
 		btVector3 q;
-		btPlaneSpace1(directionA,normalOnB,q);
-	} else
+		btPlaneSpace1(directionA, normalOnB, q);
+	}
+	else
 	{
 		// compute the contact normal
-		normalOnB = ptsVector*-btRecipSqrt(lenSqr);
+		normalOnB = ptsVector * -btRecipSqrt(lenSqr);
 	}
-	pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB;
+	pointOnB = transformB.getOrigin() + offsetB + normalOnB * capsuleRadiusB;
 
 	return distance;
 }
 
-
-
-
-
-
-
 //////////
 
-
-
-
-
-btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+btConvexConvexAlgorithm::CreateFunc::CreateFunc(btConvexPenetrationDepthSolver* pdSolver)
 {
 	m_numPerturbationIterations = 0;
 	m_minimumPointsPerturbationThreshold = 3;
-	m_simplexSolver = simplexSolver;
 	m_pdSolver = pdSolver;
 }
 
-btConvexConvexAlgorithm::CreateFunc::~CreateFunc() 
-{ 
+btConvexConvexAlgorithm::CreateFunc::~CreateFunc()
+{
 }
 
-btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
-: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
-m_simplexSolver(simplexSolver),
-m_pdSolver(pdSolver),
-m_ownManifold (false),
-m_manifoldPtr(mf),
-m_lowLevelOfDetail(false),
+btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+	: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
+	  m_pdSolver(pdSolver),
+	  m_ownManifold(false),
+	  m_manifoldPtr(mf),
+	  m_lowLevelOfDetail(false),
 #ifdef USE_SEPDISTANCE_UTIL2
-m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
-			  (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
+	  m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
+					(static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
 #endif
-m_numPerturbationIterations(numPerturbationIterations),
-m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
+	  m_numPerturbationIterations(numPerturbationIterations),
+	  m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
 {
 	(void)body0Wrap;
 	(void)body1Wrap;
 }
 
-
-
-
 btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
 {
 	if (m_ownManifold)
@@ -221,339 +207,466 @@ btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
 	}
 }
 
-void	btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
+void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
 {
 	m_lowLevelOfDetail = useLowLevel;
 }
 
-
 struct btPerturbedContactResult : public btManifoldResult
 {
 	btManifoldResult* m_originalManifoldResult;
 	btTransform m_transformA;
 	btTransform m_transformB;
-	btTransform	m_unPerturbedTransform;
-	bool	m_perturbA;
-	btIDebugDraw*	m_debugDrawer;
-
-
-	btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer)
-		:m_originalManifoldResult(originalResult),
-		m_transformA(transformA),
-		m_transformB(transformB),
-		m_unPerturbedTransform(unPerturbedTransform),
-		m_perturbA(perturbA),
-		m_debugDrawer(debugDrawer)
+	btTransform m_unPerturbedTransform;
+	bool m_perturbA;
+	btIDebugDraw* m_debugDrawer;
+
+	btPerturbedContactResult(btManifoldResult* originalResult, const btTransform& transformA, const btTransform& transformB, const btTransform& unPerturbedTransform, bool perturbA, btIDebugDraw* debugDrawer)
+		: m_originalManifoldResult(originalResult),
+		  m_transformA(transformA),
+		  m_transformB(transformB),
+		  m_unPerturbedTransform(unPerturbedTransform),
+		  m_perturbA(perturbA),
+		  m_debugDrawer(debugDrawer)
 	{
 	}
-	virtual ~ btPerturbedContactResult()
+	virtual ~btPerturbedContactResult()
 	{
 	}
 
-	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
+	virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar orgDepth)
 	{
-		btVector3 endPt,startPt;
+		btVector3 endPt, startPt;
 		btScalar newDepth;
 		btVector3 newNormal;
 
 		if (m_perturbA)
 		{
-			btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
-			endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
-			newDepth = (endPt -  pointInWorld).dot(normalOnBInWorld);
-			startPt = endPt+normalOnBInWorld*newDepth;
-		} else
+			btVector3 endPtOrg = pointInWorld + normalOnBInWorld * orgDepth;
+			endPt = (m_unPerturbedTransform * m_transformA.inverse())(endPtOrg);
+			newDepth = (endPt - pointInWorld).dot(normalOnBInWorld);
+			startPt = endPt - normalOnBInWorld * newDepth;
+		}
+		else
 		{
-			endPt = pointInWorld + normalOnBInWorld*orgDepth;
-			startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
-			newDepth = (endPt -  startPt).dot(normalOnBInWorld);
-			
+			endPt = pointInWorld + normalOnBInWorld * orgDepth;
+			startPt = (m_unPerturbedTransform * m_transformB.inverse())(pointInWorld);
+			newDepth = (endPt - startPt).dot(normalOnBInWorld);
 		}
 
 //#define DEBUG_CONTACTS 1
 #ifdef DEBUG_CONTACTS
-		m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0));
-		m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0));
-		m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1));
-#endif //DEBUG_CONTACTS
+		m_debugDrawer->drawLine(startPt, endPt, btVector3(1, 0, 0));
+		m_debugDrawer->drawSphere(startPt, 0.05, btVector3(0, 1, 0));
+		m_debugDrawer->drawSphere(endPt, 0.05, btVector3(0, 0, 1));
+#endif  //DEBUG_CONTACTS
 
-		
-		m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
+		m_originalManifoldResult->addContactPoint(normalOnBInWorld, startPt, newDepth);
 	}
-
 };
 
 extern btScalar gContactBreakingThreshold;
 
-
 //
 // Convex-Convex collision algorithm
 //
-void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
-
 	if (!m_manifoldPtr)
 	{
 		//swapped?
-		m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
 		m_ownManifold = true;
 	}
 	resultOut->setPersistentManifold(m_manifoldPtr);
 
 	//comment-out next line to test multi-contact generation
 	//resultOut->getPersistentManifold()->clearManifold();
-	
 
 	const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
 	const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());
 
-	btVector3  normalOnB;
-		btVector3  pointOnBWorld;
+	btVector3 normalOnB;
+	btVector3 pointOnBWorld;
 #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
 	if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
 	{
-		btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
-		btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
-	//	btVector3 localScalingA = capsuleA->getLocalScaling();
-	//	btVector3 localScalingB = capsuleB->getLocalScaling();
-		
-		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
-
-		btScalar dist = capsuleCapsuleDistance(normalOnB,	pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
-			capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
-			body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold);
-
-		if (dist<threshold)
+		//m_manifoldPtr->clearManifold();
+
+		btCapsuleShape* capsuleA = (btCapsuleShape*)min0;
+		btCapsuleShape* capsuleB = (btCapsuleShape*)min1;
+
+		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
+
+		btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(),
+											   capsuleB->getHalfHeight(), capsuleB->getRadius(), capsuleA->getUpAxis(), capsuleB->getUpAxis(),
+											   body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold);
+
+		if (dist < threshold)
 		{
-			btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
-			resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);	
+			btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON));
+			resultOut->addContactPoint(normalOnB, pointOnBWorld, dist);
 		}
 		resultOut->refreshContactPoints();
 		return;
 	}
-#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
 
+	if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == SPHERE_SHAPE_PROXYTYPE))
+	{
+		//m_manifoldPtr->clearManifold();
 
+		btCapsuleShape* capsuleA = (btCapsuleShape*)min0;
+		btSphereShape* capsuleB = (btSphereShape*)min1;
 
+		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 
-#ifdef USE_SEPDISTANCE_UTIL2
-	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
-	{
-		m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
-	}
+		btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(),
+											   0., capsuleB->getRadius(), capsuleA->getUpAxis(), 1,
+											   body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold);
 
-	if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
-#endif //USE_SEPDISTANCE_UTIL2
+		if (dist < threshold)
+		{
+			btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON));
+			resultOut->addContactPoint(normalOnB, pointOnBWorld, dist);
+		}
+		resultOut->refreshContactPoints();
+		return;
+	}
 
+	if ((min0->getShapeType() == SPHERE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
 	{
+		//m_manifoldPtr->clearManifold();
+
+		btSphereShape* capsuleA = (btSphereShape*)min0;
+		btCapsuleShape* capsuleB = (btCapsuleShape*)min1;
 
-	
-	btGjkPairDetector::ClosestPointInput input;
+		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 
-	btGjkPairDetector	gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
-	//TODO: if (dispatchInfo.m_useContinuous)
-	gjkPairDetector.setMinkowskiA(min0);
-	gjkPairDetector.setMinkowskiB(min1);
+		btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, 0., capsuleA->getRadius(),
+											   capsuleB->getHalfHeight(), capsuleB->getRadius(), 1, capsuleB->getUpAxis(),
+											   body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold);
+
+		if (dist < threshold)
+		{
+			btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON));
+			resultOut->addContactPoint(normalOnB, pointOnBWorld, dist);
+		}
+		resultOut->refreshContactPoints();
+		return;
+	}
+#endif  //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
 
 #ifdef USE_SEPDISTANCE_UTIL2
 	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
 	{
-		input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
-	} else
-#endif //USE_SEPDISTANCE_UTIL2
-	{
-		//if (dispatchInfo.m_convexMaxDistanceUseCPT)
-		//{
-		//	input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
-		//} else
-		//{
-		input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
-//		}
-
-		input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
+		m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(), body1->getWorldTransform());
 	}
 
-	input.m_transformA = body0Wrap->getWorldTransform();
-	input.m_transformB = body1Wrap->getWorldTransform();
-
-
+	if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance() <= 0.f)
+#endif  //USE_SEPDISTANCE_UTIL2
 
-	
+	{
+		btGjkPairDetector::ClosestPointInput input;
+		btVoronoiSimplexSolver simplexSolver;
+		btGjkPairDetector gjkPairDetector(min0, min1, &simplexSolver, m_pdSolver);
+		//TODO: if (dispatchInfo.m_useContinuous)
+		gjkPairDetector.setMinkowskiA(min0);
+		gjkPairDetector.setMinkowskiB(min1);
 
 #ifdef USE_SEPDISTANCE_UTIL2
-	btScalar sepDist = 0.f;
-	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
-	{
-		sepDist = gjkPairDetector.getCachedSeparatingDistance();
-		if (sepDist>SIMD_EPSILON)
+		if (dispatchInfo.m_useConvexConservativeDistanceUtil)
 		{
-			sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
-			//now perturbe directions to get multiple contact points
-			
+			input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
+		}
+		else
+#endif  //USE_SEPDISTANCE_UTIL2
+		{
+			//if (dispatchInfo.m_convexMaxDistanceUseCPT)
+			//{
+			//	input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
+			//} else
+			//{
+			input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold() + resultOut->m_closestPointDistanceThreshold;
+			//		}
+
+			input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
 		}
-	}
-#endif //USE_SEPDISTANCE_UTIL2
-
-	if (min0->isPolyhedral() && min1->isPolyhedral())
-	{
 
+		input.m_transformA = body0Wrap->getWorldTransform();
+		input.m_transformB = body1Wrap->getWorldTransform();
 
-		struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
+#ifdef USE_SEPDISTANCE_UTIL2
+		btScalar sepDist = 0.f;
+		if (dispatchInfo.m_useConvexConservativeDistanceUtil)
 		{
-			virtual void setShapeIdentifiersA(int partId0,int index0){}
-			virtual void setShapeIdentifiersB(int partId1,int index1){}
-			virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) 
+			sepDist = gjkPairDetector.getCachedSeparatingDistance();
+			if (sepDist > SIMD_EPSILON)
 			{
+				sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
+				//now perturbe directions to get multiple contact points
 			}
-		};
+		}
+#endif  //USE_SEPDISTANCE_UTIL2
 
-		
-		struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result
+		if (min0->isPolyhedral() && min1->isPolyhedral())
 		{
-			btDiscreteCollisionDetectorInterface::Result* m_originalResult;
-			btVector3	m_reportedNormalOnWorld;
-			btScalar m_marginOnA;
-			btScalar m_marginOnB;
-			btScalar	m_reportedDistance;
-			
-			bool		m_foundResult;
-			btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB)
-			:m_originalResult(result),
-			m_marginOnA(marginOnA),
-			m_marginOnB(marginOnB),
-			m_foundResult(false)
-			{
-			}
-			
-			virtual void setShapeIdentifiersA(int partId0,int index0){}
-			virtual void setShapeIdentifiersB(int partId1,int index1){}
-			virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorldOrg,btScalar depthOrg) 
+			struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
 			{
-				m_reportedDistance = depthOrg;
-				m_reportedNormalOnWorld = normalOnBInWorld;
-				
-				btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB;
-				m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB);
-				if (m_reportedDistance<0.f)
+				btVector3 m_normalOnBInWorld;
+				btVector3 m_pointInWorld;
+				btScalar m_depth;
+				bool m_hasContact;
+
+				btDummyResult()
+					: m_hasContact(false)
 				{
-					m_foundResult = true;					
 				}
-				m_originalResult->addContactPoint(normalOnBInWorld,adjustedPointB,m_reportedDistance);
-			}
-		};
 
-		
-		btDummyResult dummy;
-
-///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it
+				virtual void setShapeIdentifiersA(int partId0, int index0) {}
+				virtual void setShapeIdentifiersB(int partId1, int index1) {}
+				virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
+				{
+					m_hasContact = true;
+					m_normalOnBInWorld = normalOnBInWorld;
+					m_pointInWorld = pointInWorld;
+					m_depth = depth;
+				}
+			};
 
-		btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin();
-		btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin();
+			struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result
+			{
+				btDiscreteCollisionDetectorInterface::Result* m_originalResult;
+				btVector3 m_reportedNormalOnWorld;
+				btScalar m_marginOnA;
+				btScalar m_marginOnB;
+				btScalar m_reportedDistance;
+
+				bool m_foundResult;
+				btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB)
+					: m_originalResult(result),
+					  m_marginOnA(marginOnA),
+					  m_marginOnB(marginOnB),
+					  m_foundResult(false)
+				{
+				}
 
-		btWithoutMarginResult	withoutMargin(resultOut, min0Margin,min1Margin);
+				virtual void setShapeIdentifiersA(int partId0, int index0) {}
+				virtual void setShapeIdentifiersB(int partId1, int index1) {}
+				virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorldOrg, btScalar depthOrg)
+				{
+					m_reportedDistance = depthOrg;
+					m_reportedNormalOnWorld = normalOnBInWorld;
 
-		btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
-		btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
-		if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
-		{
+					btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld * m_marginOnB;
+					m_reportedDistance = depthOrg + (m_marginOnA + m_marginOnB);
+					if (m_reportedDistance < 0.f)
+					{
+						m_foundResult = true;
+					}
+					m_originalResult->addContactPoint(normalOnBInWorld, adjustedPointB, m_reportedDistance);
+				}
+			};
 
+			btDummyResult dummy;
 
-			
+			///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it
 
-			btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+			btScalar min0Margin = min0->getShapeType() == BOX_SHAPE_PROXYTYPE ? 0.f : min0->getMargin();
+			btScalar min1Margin = min1->getShapeType() == BOX_SHAPE_PROXYTYPE ? 0.f : min1->getMargin();
 
-			btScalar minDist = -1e30f;
-			btVector3 sepNormalWorldSpace;
-			bool foundSepAxis  = true;
+			btWithoutMarginResult withoutMargin(resultOut, min0Margin, min1Margin);
 
-			if (dispatchInfo.m_enableSatConvex)
-			{
-				foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
-					*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
-					body0Wrap->getWorldTransform(), 
-					body1Wrap->getWorldTransform(),
-					sepNormalWorldSpace,*resultOut);
-			} else
+			btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*)min0;
+			btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*)min1;
+			if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
 			{
+				btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
+
+				btScalar minDist = -1e30f;
+				btVector3 sepNormalWorldSpace;
+				bool foundSepAxis = true;
+
+				if (dispatchInfo.m_enableSatConvex)
+				{
+					foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
+						*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
+						body0Wrap->getWorldTransform(),
+						body1Wrap->getWorldTransform(),
+						sepNormalWorldSpace, *resultOut);
+				}
+				else
+				{
 #ifdef ZERO_MARGIN
-				gjkPairDetector.setIgnoreMargin(true);
-				gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+					gjkPairDetector.setIgnoreMargin(true);
+					gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw);
 #else
 
+					gjkPairDetector.getClosestPoints(input, withoutMargin, dispatchInfo.m_debugDraw);
+					//gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
+#endif  //ZERO_MARGIN
+					//btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
+					//if (l2>SIMD_EPSILON)
+					{
+						sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;  //gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
+						//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
+						minDist = withoutMargin.m_reportedDistance;  //gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin();
 
-				gjkPairDetector.getClosestPoints(input,withoutMargin,dispatchInfo.m_debugDraw);
-				//gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
-#endif //ZERO_MARGIN
-				//btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
-				//if (l2>SIMD_EPSILON)
-				{
-					sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
-					//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
-					minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin();
-	
 #ifdef ZERO_MARGIN
-					foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
+						foundSepAxis = true;  //gjkPairDetector.getCachedSeparatingDistance()<0.f;
 #else
-					foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin());
+						foundSepAxis = withoutMargin.m_foundResult && minDist < 0;  //-(min0->getMargin()+min1->getMargin());
 #endif
+					}
 				}
-			}
-			if (foundSepAxis)
-			{
-				
-//				printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
+				if (foundSepAxis)
+				{
+					//				printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
 
-				btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
-					body0Wrap->getWorldTransform(), 
-					body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut);
- 				
+					worldVertsB1.resize(0);
+					btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
+																	 body0Wrap->getWorldTransform(),
+																	 body1Wrap->getWorldTransform(), minDist - threshold, threshold, worldVertsB1, worldVertsB2,
+																	 *resultOut);
+				}
+				if (m_ownManifold)
+				{
+					resultOut->refreshContactPoints();
+				}
+				return;
 			}
-			if (m_ownManifold)
+			else
 			{
-				resultOut->refreshContactPoints();
-			}
-			return;
+				//we can also deal with convex versus triangle (without connectivity data)
+				if (dispatchInfo.m_enableSatConvex && polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE)
+				{
+					btVertexArray worldSpaceVertices;
+					btTriangleShape* tri = (btTriangleShape*)polyhedronB;
+					worldSpaceVertices.push_back(body1Wrap->getWorldTransform() * tri->m_vertices1[0]);
+					worldSpaceVertices.push_back(body1Wrap->getWorldTransform() * tri->m_vertices1[1]);
+					worldSpaceVertices.push_back(body1Wrap->getWorldTransform() * tri->m_vertices1[2]);
 
-		} else
-		{
-			//we can also deal with convex versus triangle (without connectivity data)
-			if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
-			{
+					//tri->initializePolyhedralFeatures();
 
-				btVertexArray vertices;
-				btTriangleShape* tri = (btTriangleShape*)polyhedronB;
-				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
-				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
-				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
-				
-				//tri->initializePolyhedralFeatures();
+					btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 
-				btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+					btVector3 sepNormalWorldSpace;
+					btScalar minDist = -1e30f;
+					btScalar maxDist = threshold;
 
-				btVector3 sepNormalWorldSpace;
-				btScalar minDist =-1e30f;
-				btScalar maxDist = threshold;
-				
-				bool foundSepAxis = false;
-				if (0)
-				{
-					polyhedronB->initializePolyhedralFeatures();
-					 foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
-					*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
-					body0Wrap->getWorldTransform(), 
-					body1Wrap->getWorldTransform(),
-					sepNormalWorldSpace,*resultOut);
-				//	 printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
-
-				} else
-				{
+					bool foundSepAxis = false;
+					bool useSatSepNormal = true;
+
+					if (useSatSepNormal)
+					{
+#if 0
+					if (0)
+					{
+						//initializePolyhedralFeatures performs a convex hull computation, not needed for a single triangle
+						polyhedronB->initializePolyhedralFeatures();
+					} else
+#endif
+						{
+							btVector3 uniqueEdges[3] = {tri->m_vertices1[1] - tri->m_vertices1[0],
+														tri->m_vertices1[2] - tri->m_vertices1[1],
+														tri->m_vertices1[0] - tri->m_vertices1[2]};
+
+							uniqueEdges[0].normalize();
+							uniqueEdges[1].normalize();
+							uniqueEdges[2].normalize();
+
+							btConvexPolyhedron polyhedron;
+							polyhedron.m_vertices.push_back(tri->m_vertices1[2]);
+							polyhedron.m_vertices.push_back(tri->m_vertices1[0]);
+							polyhedron.m_vertices.push_back(tri->m_vertices1[1]);
+
+							{
+								btFace combinedFaceA;
+								combinedFaceA.m_indices.push_back(0);
+								combinedFaceA.m_indices.push_back(1);
+								combinedFaceA.m_indices.push_back(2);
+								btVector3 faceNormal = uniqueEdges[0].cross(uniqueEdges[1]);
+								faceNormal.normalize();
+								btScalar planeEq = 1e30f;
+								for (int v = 0; v < combinedFaceA.m_indices.size(); v++)
+								{
+									btScalar eq = tri->m_vertices1[combinedFaceA.m_indices[v]].dot(faceNormal);
+									if (planeEq > eq)
+									{
+										planeEq = eq;
+									}
+								}
+								combinedFaceA.m_plane[0] = faceNormal[0];
+								combinedFaceA.m_plane[1] = faceNormal[1];
+								combinedFaceA.m_plane[2] = faceNormal[2];
+								combinedFaceA.m_plane[3] = -planeEq;
+								polyhedron.m_faces.push_back(combinedFaceA);
+							}
+							{
+								btFace combinedFaceB;
+								combinedFaceB.m_indices.push_back(0);
+								combinedFaceB.m_indices.push_back(2);
+								combinedFaceB.m_indices.push_back(1);
+								btVector3 faceNormal = -uniqueEdges[0].cross(uniqueEdges[1]);
+								faceNormal.normalize();
+								btScalar planeEq = 1e30f;
+								for (int v = 0; v < combinedFaceB.m_indices.size(); v++)
+								{
+									btScalar eq = tri->m_vertices1[combinedFaceB.m_indices[v]].dot(faceNormal);
+									if (planeEq > eq)
+									{
+										planeEq = eq;
+									}
+								}
+
+								combinedFaceB.m_plane[0] = faceNormal[0];
+								combinedFaceB.m_plane[1] = faceNormal[1];
+								combinedFaceB.m_plane[2] = faceNormal[2];
+								combinedFaceB.m_plane[3] = -planeEq;
+								polyhedron.m_faces.push_back(combinedFaceB);
+							}
+
+							polyhedron.m_uniqueEdges.push_back(uniqueEdges[0]);
+							polyhedron.m_uniqueEdges.push_back(uniqueEdges[1]);
+							polyhedron.m_uniqueEdges.push_back(uniqueEdges[2]);
+							polyhedron.initialize2();
+
+							polyhedronB->setPolyhedralFeatures(polyhedron);
+						}
+
+						foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
+							*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
+							body0Wrap->getWorldTransform(),
+							body1Wrap->getWorldTransform(),
+							sepNormalWorldSpace, *resultOut);
+						//	 printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
+					}
+					else
+					{
 #ifdef ZERO_MARGIN
-					gjkPairDetector.setIgnoreMargin(true);
-					gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+						gjkPairDetector.setIgnoreMargin(true);
+						gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw);
 #else
-					gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
-#endif//ZERO_MARGIN
-					
+						gjkPairDetector.getClosestPoints(input, dummy, dispatchInfo.m_debugDraw);
+#endif  //ZERO_MARGIN
+
+						if (dummy.m_hasContact && dummy.m_depth < 0)
+						{
+							if (foundSepAxis)
+							{
+								if (dummy.m_normalOnBInWorld.dot(sepNormalWorldSpace) < 0.99)
+								{
+									printf("?\n");
+								}
+							}
+							else
+							{
+								printf("!\n");
+							}
+							sepNormalWorldSpace.setValue(0, 0, 1);  // = dummy.m_normalOnBInWorld;
+							//minDist = dummy.m_depth;
+							foundSepAxis = true;
+						}
+#if 0
 					btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
 					if (l2>SIMD_EPSILON)
 					{
@@ -563,143 +676,132 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 						minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
 						foundSepAxis = true;
 					}
-				}
+#endif
+					}
 
-				
-			if (foundSepAxis)
-			{
-				btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), 
-					body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut);
-			}
-				
-				
-				if (m_ownManifold)
-				{
-					resultOut->refreshContactPoints();
+					if (foundSepAxis)
+					{
+						worldVertsB2.resize(0);
+						btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(),
+																		 body0Wrap->getWorldTransform(), worldSpaceVertices, worldVertsB2, minDist - threshold, maxDist, *resultOut);
+					}
+
+					if (m_ownManifold)
+					{
+						resultOut->refreshContactPoints();
+					}
+
+					return;
 				}
-				
-				return;
 			}
-			
 		}
 
+		gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw);
 
-	}
-	
-	gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+		//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
 
-	//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
-	
-	//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
-	if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
-	{
-		
-		int i;
-		btVector3 v0,v1;
-		btVector3 sepNormalWorldSpace;
-		btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
-	
-		if (l2>SIMD_EPSILON)
+		//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
+		if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
 		{
-			sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
-			
-			btPlaneSpace1(sepNormalWorldSpace,v0,v1);
-
+			int i;
+			btVector3 v0, v1;
+			btVector3 sepNormalWorldSpace;
+			btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
 
-			bool perturbeA = true;
-			const btScalar angleLimit = 0.125f * SIMD_PI;
-			btScalar perturbeAngle;
-			btScalar radiusA = min0->getAngularMotionDisc();
-			btScalar radiusB = min1->getAngularMotionDisc();
-			if (radiusA < radiusB)
-			{
-				perturbeAngle = gContactBreakingThreshold /radiusA;
-				perturbeA = true;
-			} else
+			if (l2 > SIMD_EPSILON)
 			{
-				perturbeAngle = gContactBreakingThreshold / radiusB;
-				perturbeA = false;
-			}
-			if ( perturbeAngle > angleLimit ) 
-					perturbeAngle = angleLimit;
+				sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis() * (1.f / l2);
 
-			btTransform unPerturbedTransform;
-			if (perturbeA)
-			{
-				unPerturbedTransform = input.m_transformA;
-			} else
-			{
-				unPerturbedTransform = input.m_transformB;
-			}
-			
-			for ( i=0;i<m_numPerturbationIterations;i++)
-			{
-				if (v0.length2()>SIMD_EPSILON)
+				btPlaneSpace1(sepNormalWorldSpace, v0, v1);
+
+				bool perturbeA = true;
+				const btScalar angleLimit = 0.125f * SIMD_PI;
+				btScalar perturbeAngle;
+				btScalar radiusA = min0->getAngularMotionDisc();
+				btScalar radiusB = min1->getAngularMotionDisc();
+				if (radiusA < radiusB)
+				{
+					perturbeAngle = gContactBreakingThreshold / radiusA;
+					perturbeA = true;
+				}
+				else
 				{
-				btQuaternion perturbeRot(v0,perturbeAngle);
-				btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
-				btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
-				
-				
+					perturbeAngle = gContactBreakingThreshold / radiusB;
+					perturbeA = false;
+				}
+				if (perturbeAngle > angleLimit)
+					perturbeAngle = angleLimit;
+
+				btTransform unPerturbedTransform;
 				if (perturbeA)
 				{
-					input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis());
-					input.m_transformB = body1Wrap->getWorldTransform();
-	#ifdef DEBUG_CONTACTS
-					dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
-	#endif //DEBUG_CONTACTS
-				} else
+					unPerturbedTransform = input.m_transformA;
+				}
+				else
 				{
-					input.m_transformA = body0Wrap->getWorldTransform();
-					input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis());
-	#ifdef DEBUG_CONTACTS
-					dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
-	#endif
+					unPerturbedTransform = input.m_transformB;
 				}
-				
-				btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
-				gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
+
+				for (i = 0; i < m_numPerturbationIterations; i++)
+				{
+					if (v0.length2() > SIMD_EPSILON)
+					{
+						btQuaternion perturbeRot(v0, perturbeAngle);
+						btScalar iterationAngle = i * (SIMD_2_PI / btScalar(m_numPerturbationIterations));
+						btQuaternion rotq(sepNormalWorldSpace, iterationAngle);
+
+						if (perturbeA)
+						{
+							input.m_transformA.setBasis(btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body0Wrap->getWorldTransform().getBasis());
+							input.m_transformB = body1Wrap->getWorldTransform();
+#ifdef DEBUG_CONTACTS
+							dispatchInfo.m_debugDraw->drawTransform(input.m_transformA, 10.0);
+#endif  //DEBUG_CONTACTS
+						}
+						else
+						{
+							input.m_transformA = body0Wrap->getWorldTransform();
+							input.m_transformB.setBasis(btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body1Wrap->getWorldTransform().getBasis());
+#ifdef DEBUG_CONTACTS
+							dispatchInfo.m_debugDraw->drawTransform(input.m_transformB, 10.0);
+#endif
+						}
+
+						btPerturbedContactResult perturbedResultOut(resultOut, input.m_transformA, input.m_transformB, unPerturbedTransform, perturbeA, dispatchInfo.m_debugDraw);
+						gjkPairDetector.getClosestPoints(input, perturbedResultOut, dispatchInfo.m_debugDraw);
+					}
 				}
 			}
 		}
-	}
-
-	
 
 #ifdef USE_SEPDISTANCE_UTIL2
-	if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
-	{
-		m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
-	}
-#endif //USE_SEPDISTANCE_UTIL2
-
-
+		if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > SIMD_EPSILON))
+		{
+			m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0->getWorldTransform(), body1->getWorldTransform());
+		}
+#endif  //USE_SEPDISTANCE_UTIL2
 	}
 
 	if (m_ownManifold)
 	{
 		resultOut->refreshContactPoints();
 	}
-
 }
 
-
-
 bool disableCcd = false;
-btScalar	btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	(void)resultOut;
 	(void)dispatchInfo;
 	///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
-    
+
 	///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
 	///col0->m_worldTransform,
 	btScalar resultFraction = btScalar(1.);
 
-
 	btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
 	btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
-    
+
 	if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
 		squareMot1 < col1->getCcdSquareMotionThreshold())
 		return resultFraction;
@@ -707,77 +809,65 @@ btScalar	btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,
 	if (disableCcd)
 		return btScalar(1.);
 
-
 	//An adhoc way of testing the Continuous Collision Detection algorithms
 	//One object is approximated as a sphere, to simplify things
 	//Starting in penetration should report no time of impact
 	//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
 	//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
 
-		
 	/// Convex0 against sphere for Convex1
 	{
 		btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
 
-		btSphereShape	sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+		btSphereShape sphere1(col1->getCcdSweptSphereRadius());  //todo: allow non-zero sphere sizes, for better approximation
 		btConvexCast::CastResult result;
 		btVoronoiSimplexSolver voronoiSimplex;
 		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
 		///Simplification, one object is simplified as a sphere
-		btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
+		btGjkConvexCast ccd1(convex0, &sphere1, &voronoiSimplex);
 		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
-		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
-			col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(),
+								  col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result))
 		{
-		
 			//store result.m_fraction in both bodies
-		
-			if (col0->getHitFraction()> result.m_fraction)
-				col0->setHitFraction( result.m_fraction );
+
+			if (col0->getHitFraction() > result.m_fraction)
+				col0->setHitFraction(result.m_fraction);
 
 			if (col1->getHitFraction() > result.m_fraction)
-				col1->setHitFraction( result.m_fraction);
+				col1->setHitFraction(result.m_fraction);
 
 			if (resultFraction > result.m_fraction)
 				resultFraction = result.m_fraction;
-
 		}
-		
-		
-
-
 	}
 
 	/// Sphere (for convex0) against Convex1
 	{
 		btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
 
-		btSphereShape	sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+		btSphereShape sphere0(col0->getCcdSweptSphereRadius());  //todo: allow non-zero sphere sizes, for better approximation
 		btConvexCast::CastResult result;
 		btVoronoiSimplexSolver voronoiSimplex;
 		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
 		///Simplification, one object is simplified as a sphere
-		btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
+		btGjkConvexCast ccd1(&sphere0, convex1, &voronoiSimplex);
 		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
-		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
-			col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(),
+								  col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result))
 		{
-		
 			//store result.m_fraction in both bodies
-		
-			if (col0->getHitFraction()	> result.m_fraction)
-				col0->setHitFraction( result.m_fraction);
+
+			if (col0->getHitFraction() > result.m_fraction)
+				col0->setHitFraction(result.m_fraction);
 
 			if (col1->getHitFraction() > result.m_fraction)
-				col1->setHitFraction( result.m_fraction);
+				col1->setHitFraction(result.m_fraction);
 
 			if (resultFraction > result.m_fraction)
 				resultFraction = result.m_fraction;
-
 		}
 	}
-	
-	return resultFraction;
 
+	return resultFraction;
 }
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
index 51db0c6548d..eac5b4d8248 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
@@ -23,7 +23,8 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
 #include "btCollisionCreateFunc.h"
 #include "btCollisionDispatcher.h"
-#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
+#include "LinearMath/btTransformUtil.h"  //for btConvexSeparatingDistanceUtil
+#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
 
 class btConvexPenetrationDepthSolver;
 
@@ -40,69 +41,61 @@ class btConvexPenetrationDepthSolver;
 class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
 {
 #ifdef USE_SEPDISTANCE_UTIL2
-	btConvexSeparatingDistanceUtil	m_sepDistance;
+	btConvexSeparatingDistanceUtil m_sepDistance;
 #endif
-	btSimplexSolverInterface*		m_simplexSolver;
 	btConvexPenetrationDepthSolver* m_pdSolver;
 
-	
-	bool	m_ownManifold;
-	btPersistentManifold*	m_manifoldPtr;
-	bool			m_lowLevelOfDetail;
-	
+	btVertexArray worldVertsB1;
+	btVertexArray worldVertsB2;
+
+	bool m_ownManifold;
+	btPersistentManifold* m_manifoldPtr;
+	bool m_lowLevelOfDetail;
+
 	int m_numPerturbationIterations;
 	int m_minimumPointsPerturbationThreshold;
 
-
 	///cache separating vector to speedup collision detection
-	
 
 public:
-
-	btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
+	btConvexConvexAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
 
 	virtual ~btConvexConvexAlgorithm();
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
 	{
 		///should we use m_ownManifold to avoid adding duplicates?
 		if (m_manifoldPtr && m_ownManifold)
 			manifoldArray.push_back(m_manifoldPtr);
 	}
 
+	void setLowLevelOfDetail(bool useLowLevel);
 
-	void	setLowLevelOfDetail(bool useLowLevel);
-
-
-	const btPersistentManifold*	getManifold()
+	const btPersistentManifold* getManifold()
 	{
 		return m_manifoldPtr;
 	}
 
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-
-		btConvexPenetrationDepthSolver*		m_pdSolver;
-		btSimplexSolverInterface*			m_simplexSolver;
+		btConvexPenetrationDepthSolver* m_pdSolver;
 		int m_numPerturbationIterations;
 		int m_minimumPointsPerturbationThreshold;
 
-		CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
-		
+		CreateFunc(btConvexPenetrationDepthSolver* pdSolver);
+
 		virtual ~CreateFunc();
 
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm));
-			return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+			return new (mem) btConvexConvexAlgorithm(ci.m_manifold, ci, body0Wrap, body1Wrap, m_pdSolver, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold);
 		}
 	};
-
-
 };
 
-#endif //BT_CONVEX_CONVEX_ALGORITHM_H
+#endif  //BT_CONVEX_CONVEX_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
index cce2d95bcf9..ba1bc06b69a 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
@@ -23,25 +23,24 @@ subject to the following restrictions:
 
 //#include <stdio.h>
 
-btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold)
-: btCollisionAlgorithm(ci),
-m_ownManifold(false),
-m_manifoldPtr(mf),
-m_isSwapped(isSwapped),
-m_numPerturbationIterations(numPerturbationIterations),
-m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
+btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+	: btCollisionAlgorithm(ci),
+	  m_ownManifold(false),
+	  m_manifoldPtr(mf),
+	  m_isSwapped(isSwapped),
+	  m_numPerturbationIterations(numPerturbationIterations),
+	  m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
 {
-	const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? col1Wrap : col0Wrap;
-	const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? col0Wrap : col1Wrap;
+	const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? col1Wrap : col0Wrap;
+	const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? col0Wrap : col1Wrap;
 
-	if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject()))
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(), planeObjWrap->getCollisionObject()))
 	{
-		m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject());
+		m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(), planeObjWrap->getCollisionObject());
 		m_ownManifold = true;
 	}
 }
 
-
 btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm()
 {
 	if (m_ownManifold)
@@ -51,32 +50,32 @@ btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm()
 	}
 }
 
-void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btConvexPlaneCollisionAlgorithm::collideSingleContact(const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
-    const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap;
-	const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap;
+	const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
+	const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
 
-	btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape();
-	btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape();
+	btConvexShape* convexShape = (btConvexShape*)convexObjWrap->getCollisionShape();
+	btStaticPlaneShape* planeShape = (btStaticPlaneShape*)planeObjWrap->getCollisionShape();
 
-    bool hasCollision = false;
+	bool hasCollision = false;
 	const btVector3& planeNormal = planeShape->getPlaneNormal();
 	const btScalar& planeConstant = planeShape->getPlaneConstant();
-	
+
 	btTransform convexWorldTransform = convexObjWrap->getWorldTransform();
 	btTransform convexInPlaneTrans;
-	convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexWorldTransform;
+	convexInPlaneTrans = planeObjWrap->getWorldTransform().inverse() * convexWorldTransform;
 	//now perturbe the convex-world transform
-	convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot);
+	convexWorldTransform.getBasis() *= btMatrix3x3(perturbeRot);
 	btTransform planeInConvex;
-	planeInConvex= convexWorldTransform.inverse() * planeObjWrap->getWorldTransform();
-	
-	btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
+	planeInConvex = convexWorldTransform.inverse() * planeObjWrap->getWorldTransform();
+
+	btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal);
 
 	btVector3 vtxInPlane = convexInPlaneTrans(vtx);
 	btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
 
-	btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
+	btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
 	btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected;
 
 	hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
@@ -86,70 +85,69 @@ void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion&
 		/// report a contact. internally this will be kept persistent, and contact reduction is done
 		btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal;
 		btVector3 pOnB = vtxInPlaneWorld;
-		resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
+		resultOut->addContactPoint(normalOnSurfaceB, pOnB, distance);
 	}
 }
 
-
-void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btConvexPlaneCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	(void)dispatchInfo;
 	if (!m_manifoldPtr)
 		return;
 
-	const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap;
-	const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap;
+	const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
+	const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
 
-	btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape();
-	btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape();
+	btConvexShape* convexShape = (btConvexShape*)convexObjWrap->getCollisionShape();
+	btStaticPlaneShape* planeShape = (btStaticPlaneShape*)planeObjWrap->getCollisionShape();
 
 	bool hasCollision = false;
 	const btVector3& planeNormal = planeShape->getPlaneNormal();
 	const btScalar& planeConstant = planeShape->getPlaneConstant();
 	btTransform planeInConvex;
-	planeInConvex= convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform();
+	planeInConvex = convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform();
 	btTransform convexInPlaneTrans;
-	convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform();
+	convexInPlaneTrans = planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform();
 
-	btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
+	btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal);
 	btVector3 vtxInPlane = convexInPlaneTrans(vtx);
 	btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
 
-	btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
+	btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
 	btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected;
 
-	hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
+	hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 	resultOut->setPersistentManifold(m_manifoldPtr);
 	if (hasCollision)
 	{
 		/// report a contact. internally this will be kept persistent, and contact reduction is done
 		btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal;
 		btVector3 pOnB = vtxInPlaneWorld;
-		resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
+		resultOut->addContactPoint(normalOnSurfaceB, pOnB, distance);
 	}
 
 	//the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones:
 	//they keep on rolling forever because of the additional off-center contact points
 	//so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc)
-	if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold)
+	if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
 	{
-		btVector3 v0,v1;
-		btPlaneSpace1(planeNormal,v0,v1);
+		btVector3 v0, v1;
+		btPlaneSpace1(planeNormal, v0, v1);
 		//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
 
 		const btScalar angleLimit = 0.125f * SIMD_PI;
 		btScalar perturbeAngle;
 		btScalar radius = convexShape->getAngularMotionDisc();
 		perturbeAngle = gContactBreakingThreshold / radius;
-		if ( perturbeAngle > angleLimit ) 
-				perturbeAngle = angleLimit;
+		if (perturbeAngle > angleLimit)
+			perturbeAngle = angleLimit;
 
-		btQuaternion perturbeRot(v0,perturbeAngle);
-		for (int i=0;i<m_numPerturbationIterations;i++)
+		btQuaternion perturbeRot(v0, perturbeAngle);
+		for (int i = 0; i < m_numPerturbationIterations; i++)
 		{
-			btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
-			btQuaternion rotq(planeNormal,iterationAngle);
-			collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0Wrap,body1Wrap,dispatchInfo,resultOut);
+			btScalar iterationAngle = i * (SIMD_2_PI / btScalar(m_numPerturbationIterations));
+			btQuaternion rotq(planeNormal, iterationAngle);
+			collideSingleContact(rotq.inverse() * perturbeRot * rotq, body0Wrap, body1Wrap, dispatchInfo, resultOut);
 		}
 	}
 
@@ -162,7 +160,7 @@ void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectW
 	}
 }
 
-btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	(void)resultOut;
 	(void)dispatchInfo;
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
index d28c430c4c1..b693da118fe 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
@@ -28,25 +28,24 @@ class btPersistentManifold;
 /// Other features are frame-coherency (persistent data) and collision response.
 class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm
 {
-	bool		m_ownManifold;
-	btPersistentManifold*	m_manifoldPtr;
-	bool		m_isSwapped;
-	int			m_numPerturbationIterations;
-	int			m_minimumPointsPerturbationThreshold;
+	bool m_ownManifold;
+	btPersistentManifold* m_manifoldPtr;
+	bool m_isSwapped;
+	int m_numPerturbationIterations;
+	int m_minimumPointsPerturbationThreshold;
 
 public:
-
-	btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold);
+	btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
 
 	virtual ~btConvexPlaneCollisionAlgorithm();
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	void collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	void collideSingleContact(const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
 	{
 		if (m_manifoldPtr && m_ownManifold)
 		{
@@ -54,31 +53,30 @@ public:
 		}
 	}
 
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		int	m_numPerturbationIterations;
+		int m_numPerturbationIterations;
 		int m_minimumPointsPerturbationThreshold;
-			
-		CreateFunc() 
+
+		CreateFunc()
 			: m_numPerturbationIterations(1),
-			m_minimumPointsPerturbationThreshold(0)
+			  m_minimumPointsPerturbationThreshold(0)
 		{
 		}
-		
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm));
 			if (!m_swapped)
 			{
-				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
-			} else
+				return new (mem) btConvexPlaneCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, false, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold);
+			}
+			else
 			{
-				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+				return new (mem) btConvexPlaneCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, true, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold);
 			}
 		}
 	};
-
 };
 
-#endif //BT_CONVEX_PLANE_COLLISION_ALGORITHM_H
-
+#endif  //BT_CONVEX_PLANE_COLLISION_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
index d42f00a637f..ef3ea9e394c 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
@@ -26,115 +26,108 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
 #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
 #include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h"
-#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+#endif  //USE_BUGGY_SPHERE_BOX_ALGORITHM
 #include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
 
-
-
 #include "LinearMath/btPoolAllocator.h"
 
-
-
-
-
 btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo)
 //btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc*	stackAlloc,btPoolAllocator*	persistentManifoldPool,btPoolAllocator*	collisionAlgorithmPool)
 {
-
-	void* mem = btAlignedAlloc(sizeof(btVoronoiSimplexSolver),16);
-	m_simplexSolver = new (mem)btVoronoiSimplexSolver();
-
+	void* mem = NULL;
 	if (constructionInfo.m_useEpaPenetrationAlgorithm)
 	{
-		mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16);
-		m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver;
-	}else
+		mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver), 16);
+		m_pdSolver = new (mem) btGjkEpaPenetrationDepthSolver;
+	}
+	else
 	{
-		mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16);
-		m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver;
+		mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver), 16);
+		m_pdSolver = new (mem) btMinkowskiPenetrationDepthSolver;
 	}
-	
+
 	//default CreationFunctions, filling the m_doubleDispatch table
-	mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16);
-	m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver);
-	mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
-	m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc;
-	mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
-	m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc;
-	mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16);
-	m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc;
-
-	mem = btAlignedAlloc(sizeof(btCompoundCompoundCollisionAlgorithm::CreateFunc),16);
-	m_compoundCompoundCreateFunc = new (mem)btCompoundCompoundCollisionAlgorithm::CreateFunc;
-
-	mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16);
-	m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc;
-	mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16);
-	m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc;
-	
-	mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16);
-	m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc), 16);
+	m_convexConvexCreateFunc = new (mem) btConvexConvexAlgorithm::CreateFunc(m_pdSolver);
+	mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc), 16);
+	m_convexConcaveCreateFunc = new (mem) btConvexConcaveCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc), 16);
+	m_swappedConvexConcaveCreateFunc = new (mem) btConvexConcaveCollisionAlgorithm::SwappedCreateFunc;
+	mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc), 16);
+	m_compoundCreateFunc = new (mem) btCompoundCollisionAlgorithm::CreateFunc;
+
+	mem = btAlignedAlloc(sizeof(btCompoundCompoundCollisionAlgorithm::CreateFunc), 16);
+	m_compoundCompoundCreateFunc = new (mem) btCompoundCompoundCollisionAlgorithm::CreateFunc;
+
+	mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc), 16);
+	m_swappedCompoundCreateFunc = new (mem) btCompoundCollisionAlgorithm::SwappedCreateFunc;
+	mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc), 16);
+	m_emptyCreateFunc = new (mem) btEmptyAlgorithm::CreateFunc;
+
+	mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc), 16);
+	m_sphereSphereCF = new (mem) btSphereSphereCollisionAlgorithm::CreateFunc;
 #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
-	mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16);
-	m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc;
-	mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16);
-	m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc), 16);
+	m_sphereBoxCF = new (mem) btSphereBoxCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc), 16);
+	m_boxSphereCF = new (mem) btSphereBoxCollisionAlgorithm::CreateFunc;
 	m_boxSphereCF->m_swapped = true;
-#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+#endif  //USE_BUGGY_SPHERE_BOX_ALGORITHM
 
-	mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16);
-	m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
-	mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16);
-	m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc), 16);
+	m_sphereTriangleCF = new (mem) btSphereTriangleCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc), 16);
+	m_triangleSphereCF = new (mem) btSphereTriangleCollisionAlgorithm::CreateFunc;
 	m_triangleSphereCF->m_swapped = true;
-	
-	mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc),16);
-	m_boxBoxCF = new(mem)btBoxBoxCollisionAlgorithm::CreateFunc;
+
+	mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc), 16);
+	m_boxBoxCF = new (mem) btBoxBoxCollisionAlgorithm::CreateFunc;
 
 	//convex versus plane
-	mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16);
+	mem = btAlignedAlloc(sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc), 16);
 	m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc;
-	mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16);
+	mem = btAlignedAlloc(sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc), 16);
 	m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc;
 	m_planeConvexCF->m_swapped = true;
-	
+
 	///calculate maximum element size, big enough to fit any collision algorithm in the memory pool
 	int maxSize = sizeof(btConvexConvexAlgorithm);
 	int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm);
 	int maxSize3 = sizeof(btCompoundCollisionAlgorithm);
 	int maxSize4 = sizeof(btCompoundCompoundCollisionAlgorithm);
 
-	int	collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize);
-	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
-	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);
-	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4);
-		
+	int collisionAlgorithmMaxElementSize = btMax(maxSize, constructionInfo.m_customCollisionAlgorithmMaxElementSize);
+	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize2);
+	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize3);
+	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize4);
+
 	if (constructionInfo.m_persistentManifoldPool)
 	{
 		m_ownsPersistentManifoldPool = false;
 		m_persistentManifoldPool = constructionInfo.m_persistentManifoldPool;
-	} else
+	}
+	else
 	{
 		m_ownsPersistentManifoldPool = true;
-		void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
-		m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize);
+		void* mem = btAlignedAlloc(sizeof(btPoolAllocator), 16);
+		m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold), constructionInfo.m_defaultMaxPersistentManifoldPoolSize);
 	}
-	
+
+	collisionAlgorithmMaxElementSize = (collisionAlgorithmMaxElementSize + 16) & 0xffffffffffff0;
 	if (constructionInfo.m_collisionAlgorithmPool)
 	{
 		m_ownsCollisionAlgorithmPool = false;
 		m_collisionAlgorithmPool = constructionInfo.m_collisionAlgorithmPool;
-	} else
+	}
+	else
 	{
 		m_ownsCollisionAlgorithmPool = true;
-		void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
-		m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize);
+		void* mem = btAlignedAlloc(sizeof(btPoolAllocator), 16);
+		m_collisionAlgorithmPool = new (mem) btPoolAllocator(collisionAlgorithmMaxElementSize, constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize);
 	}
-
-
 }
 
 btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
@@ -151,95 +144,159 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
 	}
 
 	m_convexConvexCreateFunc->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree(	m_convexConvexCreateFunc);
+	btAlignedFree(m_convexConvexCreateFunc);
 
 	m_convexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree( m_convexConcaveCreateFunc);
+	btAlignedFree(m_convexConcaveCreateFunc);
 	m_swappedConvexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree( m_swappedConvexConcaveCreateFunc);
+	btAlignedFree(m_swappedConvexConcaveCreateFunc);
 
 	m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree( m_compoundCreateFunc);
+	btAlignedFree(m_compoundCreateFunc);
 
 	m_compoundCompoundCreateFunc->~btCollisionAlgorithmCreateFunc();
 	btAlignedFree(m_compoundCompoundCreateFunc);
 
 	m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree( m_swappedCompoundCreateFunc);
+	btAlignedFree(m_swappedCompoundCreateFunc);
 
 	m_emptyCreateFunc->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree( m_emptyCreateFunc);
+	btAlignedFree(m_emptyCreateFunc);
 
 	m_sphereSphereCF->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree( m_sphereSphereCF);
+	btAlignedFree(m_sphereSphereCF);
 
 #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
 	m_sphereBoxCF->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree( m_sphereBoxCF);
+	btAlignedFree(m_sphereBoxCF);
 	m_boxSphereCF->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree( m_boxSphereCF);
-#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+	btAlignedFree(m_boxSphereCF);
+#endif  //USE_BUGGY_SPHERE_BOX_ALGORITHM
 
 	m_sphereTriangleCF->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree( m_sphereTriangleCF);
+	btAlignedFree(m_sphereTriangleCF);
 	m_triangleSphereCF->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree( m_triangleSphereCF);
+	btAlignedFree(m_triangleSphereCF);
 	m_boxBoxCF->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree( m_boxBoxCF);
+	btAlignedFree(m_boxBoxCF);
 
 	m_convexPlaneCF->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree( m_convexPlaneCF);
+	btAlignedFree(m_convexPlaneCF);
 	m_planeConvexCF->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree( m_planeConvexCF);
-
-	m_simplexSolver->~btVoronoiSimplexSolver();
-	btAlignedFree(m_simplexSolver);
+	btAlignedFree(m_planeConvexCF);
 
 	m_pdSolver->~btConvexPenetrationDepthSolver();
-	
+
 	btAlignedFree(m_pdSolver);
+}
 
+btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1)
+{
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
+	{
+		return m_sphereSphereCF;
+	}
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE))
+	{
+		return m_sphereBoxCF;
+	}
 
-}
+	if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
+	{
+		return m_boxSphereCF;
+	}
+#endif  //USE_BUGGY_SPHERE_BOX_ALGORITHM
 
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE))
+	{
+		return m_sphereTriangleCF;
+	}
 
-btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)
-{
+	if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
+	{
+		return m_triangleSphereCF;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE))
+	{
+		return m_convexPlaneCF;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE))
+	{
+		return m_planeConvexCF;
+	}
 
+	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
+	{
+		return m_convexConvexCreateFunc;
+	}
 
+	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1))
+	{
+		return m_convexConcaveCreateFunc;
+	}
 
-	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+	if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0))
 	{
-		return	m_sphereSphereCF;
+		return m_swappedConvexConcaveCreateFunc;
 	}
-#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
-	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE))
+
+	if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1))
 	{
-		return	m_sphereBoxCF;
+		return m_compoundCompoundCreateFunc;
 	}
 
-	if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+	if (btBroadphaseProxy::isCompound(proxyType0))
 	{
-		return	m_boxSphereCF;
+		return m_compoundCreateFunc;
+	}
+	else
+	{
+		if (btBroadphaseProxy::isCompound(proxyType1))
+		{
+			return m_swappedCompoundCreateFunc;
+		}
 	}
-#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
 
+	//failed to find an algorithm
+	return m_emptyCreateFunc;
+}
 
-	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE))
+btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1)
+{
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
 	{
-		return	m_sphereTriangleCF;
+		return m_sphereSphereCF;
+	}
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE))
+	{
+		return m_sphereBoxCF;
 	}
 
-	if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE  ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+	if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
 	{
-		return	m_triangleSphereCF;
-	} 
+		return m_boxSphereCF;
+	}
+#endif  //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE))
+	{
+		return m_sphereTriangleCF;
+	}
+
+	if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
+	{
+		return m_triangleSphereCF;
+	}
 
 	if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE))
 	{
 		return m_boxBoxCF;
 	}
-	
+
 	if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE))
 	{
 		return m_convexPlaneCF;
@@ -249,8 +306,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg
 	{
 		return m_planeConvexCF;
 	}
-	
-
 
 	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
 	{
@@ -267,7 +322,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg
 		return m_swappedConvexConcaveCreateFunc;
 	}
 
-
 	if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1))
 	{
 		return m_compoundCompoundCreateFunc;
@@ -276,7 +330,8 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg
 	if (btBroadphaseProxy::isCompound(proxyType0))
 	{
 		return m_compoundCreateFunc;
-	} else
+	}
+	else
 	{
 		if (btBroadphaseProxy::isCompound(proxyType1))
 		{
@@ -290,17 +345,17 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg
 
 void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold)
 {
-	btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*) m_convexConvexCreateFunc;
+	btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*)m_convexConvexCreateFunc;
 	convexConvex->m_numPerturbationIterations = numPerturbationIterations;
 	convexConvex->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
 }
 
-void	btDefaultCollisionConfiguration::setPlaneConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+void btDefaultCollisionConfiguration::setPlaneConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold)
 {
 	btConvexPlaneCollisionAlgorithm::CreateFunc* cpCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_convexPlaneCF;
 	cpCF->m_numPerturbationIterations = numPerturbationIterations;
 	cpCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
-	
+
 	btConvexPlaneCollisionAlgorithm::CreateFunc* pcCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_planeConvexCF;
 	pcCF->m_numPerturbationIterations = numPerturbationIterations;
 	pcCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
index 2078420e198..b39a3f41de1 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
@@ -20,77 +20,68 @@ subject to the following restrictions:
 class btVoronoiSimplexSolver;
 class btConvexPenetrationDepthSolver;
 
-struct	btDefaultCollisionConstructionInfo
+struct btDefaultCollisionConstructionInfo
 {
-	btPoolAllocator*	m_persistentManifoldPool;
-	btPoolAllocator*	m_collisionAlgorithmPool;
-	int					m_defaultMaxPersistentManifoldPoolSize;
-	int					m_defaultMaxCollisionAlgorithmPoolSize;
-	int					m_customCollisionAlgorithmMaxElementSize;
-	int					m_useEpaPenetrationAlgorithm;
+	btPoolAllocator* m_persistentManifoldPool;
+	btPoolAllocator* m_collisionAlgorithmPool;
+	int m_defaultMaxPersistentManifoldPoolSize;
+	int m_defaultMaxCollisionAlgorithmPoolSize;
+	int m_customCollisionAlgorithmMaxElementSize;
+	int m_useEpaPenetrationAlgorithm;
 
 	btDefaultCollisionConstructionInfo()
-		:m_persistentManifoldPool(0),
-		m_collisionAlgorithmPool(0),
-		m_defaultMaxPersistentManifoldPoolSize(4096),
-		m_defaultMaxCollisionAlgorithmPoolSize(4096),
-		m_customCollisionAlgorithmMaxElementSize(0),
-		m_useEpaPenetrationAlgorithm(true)
+		: m_persistentManifoldPool(0),
+		  m_collisionAlgorithmPool(0),
+		  m_defaultMaxPersistentManifoldPoolSize(4096),
+		  m_defaultMaxCollisionAlgorithmPoolSize(4096),
+		  m_customCollisionAlgorithmMaxElementSize(0),
+		  m_useEpaPenetrationAlgorithm(true)
 	{
 	}
 };
 
-
-
 ///btCollisionConfiguration allows to configure Bullet collision detection
 ///stack allocator, pool memory allocators
 ///@todo: describe the meaning
-class	btDefaultCollisionConfiguration : public btCollisionConfiguration
+class btDefaultCollisionConfiguration : public btCollisionConfiguration
 {
-
 protected:
+	int m_persistentManifoldPoolSize;
 
-	int	m_persistentManifoldPoolSize;
-	
-
-	btPoolAllocator*	m_persistentManifoldPool;
-	bool	m_ownsPersistentManifoldPool;
+	btPoolAllocator* m_persistentManifoldPool;
+	bool m_ownsPersistentManifoldPool;
 
+	btPoolAllocator* m_collisionAlgorithmPool;
+	bool m_ownsCollisionAlgorithmPool;
 
-	btPoolAllocator*	m_collisionAlgorithmPool;
-	bool	m_ownsCollisionAlgorithmPool;
+	//default penetration depth solver
+	btConvexPenetrationDepthSolver* m_pdSolver;
 
-	//default simplex/penetration depth solvers
-	btVoronoiSimplexSolver*	m_simplexSolver;
-	btConvexPenetrationDepthSolver*	m_pdSolver;
-	
 	//default CreationFunctions, filling the m_doubleDispatch table
-	btCollisionAlgorithmCreateFunc*	m_convexConvexCreateFunc;
-	btCollisionAlgorithmCreateFunc*	m_convexConcaveCreateFunc;
-	btCollisionAlgorithmCreateFunc*	m_swappedConvexConcaveCreateFunc;
-	btCollisionAlgorithmCreateFunc*	m_compoundCreateFunc;
-	btCollisionAlgorithmCreateFunc*	m_compoundCompoundCreateFunc;
-	
-	btCollisionAlgorithmCreateFunc*	m_swappedCompoundCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_compoundCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_compoundCompoundCreateFunc;
+
+	btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc;
 	btCollisionAlgorithmCreateFunc* m_emptyCreateFunc;
 	btCollisionAlgorithmCreateFunc* m_sphereSphereCF;
 	btCollisionAlgorithmCreateFunc* m_sphereBoxCF;
 	btCollisionAlgorithmCreateFunc* m_boxSphereCF;
 
 	btCollisionAlgorithmCreateFunc* m_boxBoxCF;
-	btCollisionAlgorithmCreateFunc*	m_sphereTriangleCF;
-	btCollisionAlgorithmCreateFunc*	m_triangleSphereCF;
-	btCollisionAlgorithmCreateFunc*	m_planeConvexCF;
-	btCollisionAlgorithmCreateFunc*	m_convexPlaneCF;
-	
-public:
-
+	btCollisionAlgorithmCreateFunc* m_sphereTriangleCF;
+	btCollisionAlgorithmCreateFunc* m_triangleSphereCF;
+	btCollisionAlgorithmCreateFunc* m_planeConvexCF;
+	btCollisionAlgorithmCreateFunc* m_convexPlaneCF;
 
+public:
 	btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo());
 
 	virtual ~btDefaultCollisionConfiguration();
 
-		///memory pools
+	///memory pools
 	virtual btPoolAllocator* getPersistentManifoldPool()
 	{
 		return m_persistentManifoldPool;
@@ -101,14 +92,9 @@ public:
 		return m_collisionAlgorithmPool;
 	}
 
+	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1);
 
-	virtual	btVoronoiSimplexSolver*	getSimplexSolver()
-	{
-		return m_simplexSolver;
-	}
-
-
-	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
+	virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1);
 
 	///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm.
 	///By default, this feature is disabled for best performance.
@@ -117,11 +103,9 @@ public:
 	///3 is a good value for both params, if you want to enable the feature. This is because the default contact cache contains a maximum of 4 points, and one collision query at the unperturbed orientation is performed first.
 	///See Bullet/Demos/CollisionDemo for an example how this feature gathers multiple points.
 	///@todo we could add a per-object setting of those parameters, for level-of-detail collision detection.
-	void	setConvexConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3);
-
-	void	setPlaneConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3);
+	void setConvexConvexMultipointIterations(int numPerturbationIterations = 3, int minimumPointsPerturbationThreshold = 3);
 
+	void setPlaneConvexMultipointIterations(int numPerturbationIterations = 3, int minimumPointsPerturbationThreshold = 3);
 };
 
-#endif //BT_DEFAULT_COLLISION_CONFIGURATION
-
+#endif  //BT_DEFAULT_COLLISION_CONFIGURATION
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp
index 5fa1c8be5e4..7cd41bdb33e 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp
@@ -15,20 +15,16 @@ subject to the following restrictions:
 
 #include "btEmptyCollisionAlgorithm.h"
 
-
-
 btEmptyAlgorithm::btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
 	: btCollisionAlgorithm(ci)
 {
 }
 
-void btEmptyAlgorithm::processCollision (const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* ,const btDispatcherInfo& ,btManifoldResult* )
+void btEmptyAlgorithm::processCollision(const btCollisionObjectWrapper*, const btCollisionObjectWrapper*, const btDispatcherInfo&, btManifoldResult*)
 {
 }
 
-btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* )
+btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject*, btCollisionObject*, const btDispatcherInfo&, btManifoldResult*)
 {
 	return btScalar(1.);
 }
-
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h
index cb0f152183c..65ef83e0946 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h
@@ -25,30 +25,28 @@ subject to the following restrictions:
 ///The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame.
 class btEmptyAlgorithm : public btCollisionAlgorithm
 {
-
 public:
-	
 	btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci);
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
 	{
 	}
 
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-        virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
-        {
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
+		{
 			(void)body0Wrap;
 			(void)body1Wrap;
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btEmptyAlgorithm));
-			return new(mem) btEmptyAlgorithm(ci);
+			return new (mem) btEmptyAlgorithm(ci);
 		}
 	};
 
 } ATTRIBUTE_ALIGNED(16);
 
-#endif //BT_EMPTY_ALGORITH
+#endif  //BT_EMPTY_ALGORITH
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp
index 86141fa6899..00f16fd0a8a 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp
@@ -29,60 +29,58 @@ btGhostObject::~btGhostObject()
 	btAssert(!m_overlappingObjects.size());
 }
 
-
-void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy)
+void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy)
 {
 	btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
 	btAssert(otherObject);
 	///if this linearSearch becomes too slow (too many overlapping objects) we should add a more appropriate data structure
 	int index = m_overlappingObjects.findLinearSearch(otherObject);
-	if (index==m_overlappingObjects.size())
+	if (index == m_overlappingObjects.size())
 	{
 		//not found
 		m_overlappingObjects.push_back(otherObject);
 	}
 }
 
-void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy)
+void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btDispatcher* dispatcher, btBroadphaseProxy* thisProxy)
 {
 	btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
 	btAssert(otherObject);
 	int index = m_overlappingObjects.findLinearSearch(otherObject);
-	if (index<m_overlappingObjects.size())
+	if (index < m_overlappingObjects.size())
 	{
-		m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1];
+		m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size() - 1];
 		m_overlappingObjects.pop_back();
 	}
 }
 
-
 btPairCachingGhostObject::btPairCachingGhostObject()
 {
-	m_hashPairCache = new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
+	m_hashPairCache = new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache), 16)) btHashedOverlappingPairCache();
 }
 
 btPairCachingGhostObject::~btPairCachingGhostObject()
 {
 	m_hashPairCache->~btHashedOverlappingPairCache();
-	btAlignedFree( m_hashPairCache );
+	btAlignedFree(m_hashPairCache);
 }
 
-void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy)
+void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy)
 {
-	btBroadphaseProxy*actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle();
+	btBroadphaseProxy* actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle();
 	btAssert(actualThisProxy);
 
 	btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
 	btAssert(otherObject);
 	int index = m_overlappingObjects.findLinearSearch(otherObject);
-	if (index==m_overlappingObjects.size())
+	if (index == m_overlappingObjects.size())
 	{
 		m_overlappingObjects.push_back(otherObject);
-		m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy);
+		m_hashPairCache->addOverlappingPair(actualThisProxy, otherProxy);
 	}
 }
 
-void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1)
+void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btDispatcher* dispatcher, btBroadphaseProxy* thisProxy1)
 {
 	btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
 	btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle();
@@ -90,82 +88,79 @@ void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy
 
 	btAssert(otherObject);
 	int index = m_overlappingObjects.findLinearSearch(otherObject);
-	if (index<m_overlappingObjects.size())
+	if (index < m_overlappingObjects.size())
 	{
-		m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1];
+		m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size() - 1];
 		m_overlappingObjects.pop_back();
-		m_hashPairCache->removeOverlappingPair(actualThisProxy,otherProxy,dispatcher);
+		m_hashPairCache->removeOverlappingPair(actualThisProxy, otherProxy, dispatcher);
 	}
 }
 
-
-void	btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
+void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
 {
-	btTransform	convexFromTrans,convexToTrans;
+	btTransform convexFromTrans, convexToTrans;
 	convexFromTrans = convexFromWorld;
 	convexToTrans = convexToWorld;
 	btVector3 castShapeAabbMin, castShapeAabbMax;
 	/* Compute AABB that encompasses angular movement */
 	{
 		btVector3 linVel, angVel;
-		btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
+		btTransformUtil::calculateVelocity(convexFromTrans, convexToTrans, 1.0, linVel, angVel);
 		btTransform R;
-		R.setIdentity ();
-		R.setRotation (convexFromTrans.getRotation());
-		castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
+		R.setIdentity();
+		R.setRotation(convexFromTrans.getRotation());
+		castShape->calculateTemporalAabb(R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
 	}
 
 	/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
 	// do a ray-shape query using convexCaster (CCD)
 	int i;
-	for (i=0;i<m_overlappingObjects.size();i++)
+	for (i = 0; i < m_overlappingObjects.size(); i++)
 	{
-		btCollisionObject*	collisionObject= m_overlappingObjects[i];
+		btCollisionObject* collisionObject = m_overlappingObjects[i];
 		//only perform raycast if filterMask matches
-		if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
+		if (resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
+		{
 			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
-			btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
-			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
-			AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
-			btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
+			btVector3 collisionObjectAabbMin, collisionObjectAabbMax;
+			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(), collisionObjectAabbMin, collisionObjectAabbMax);
+			AabbExpand(collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
+			btScalar hitLambda = btScalar(1.);  //could use resultCallback.m_closestHitFraction, but needs testing
 			btVector3 hitNormal;
-			if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
+			if (btRayAabb(convexFromWorld.getOrigin(), convexToWorld.getOrigin(), collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal))
 			{
-				btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans,
-					collisionObject,
-						collisionObject->getCollisionShape(),
-						collisionObject->getWorldTransform(),
-						resultCallback,
-						allowedCcdPenetration);
+				btCollisionWorld::objectQuerySingle(castShape, convexFromTrans, convexToTrans,
+													collisionObject,
+													collisionObject->getCollisionShape(),
+													collisionObject->getWorldTransform(),
+													resultCallback,
+													allowedCcdPenetration);
 			}
 		}
 	}
-
 }
 
-void	btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const
+void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const
 {
 	btTransform rayFromTrans;
 	rayFromTrans.setIdentity();
 	rayFromTrans.setOrigin(rayFromWorld);
-	btTransform  rayToTrans;
+	btTransform rayToTrans;
 	rayToTrans.setIdentity();
 	rayToTrans.setOrigin(rayToWorld);
 
-
 	int i;
-	for (i=0;i<m_overlappingObjects.size();i++)
+	for (i = 0; i < m_overlappingObjects.size(); i++)
 	{
-		btCollisionObject*	collisionObject= m_overlappingObjects[i];
+		btCollisionObject* collisionObject = m_overlappingObjects[i];
 		//only perform raycast if filterMask matches
-		if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
+		if (resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
 		{
-			btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,
-							collisionObject,
-								collisionObject->getCollisionShape(),
-								collisionObject->getWorldTransform(),
-								resultCallback);
+			btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans,
+											collisionObject,
+											collisionObject->getCollisionShape(),
+											collisionObject->getWorldTransform(),
+											resultCallback);
 		}
 	}
 }
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h
index 8ec86138575..aa7f48d5cb4 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h
@@ -16,7 +16,6 @@ subject to the following restrictions:
 #ifndef BT_GHOST_OBJECT_H
 #define BT_GHOST_OBJECT_H
 
-
 #include "btCollisionObject.h"
 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h"
 #include "LinearMath/btAlignedAllocator.h"
@@ -31,48 +30,47 @@ class btDispatcher;
 ///By default, this overlap is based on the AABB
 ///This is useful for creating a character controller, collision sensors/triggers, explosions etc.
 ///We plan on adding rayTest and other queries for the btGhostObject
-ATTRIBUTE_ALIGNED16(class) btGhostObject : public btCollisionObject
+ATTRIBUTE_ALIGNED16(class)
+btGhostObject : public btCollisionObject
 {
 protected:
-
 	btAlignedObjectArray<btCollisionObject*> m_overlappingObjects;
 
 public:
-
 	btGhostObject();
 
 	virtual ~btGhostObject();
 
-	void	convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const;
+	void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const;
 
-	void	rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const; 
+	void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const;
 
 	///this method is mainly for expert/internal use only.
-	virtual void	addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0);
+	virtual void addOverlappingObjectInternal(btBroadphaseProxy * otherProxy, btBroadphaseProxy* thisProxy = 0);
 	///this method is mainly for expert/internal use only.
-	virtual void	removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0);
+	virtual void removeOverlappingObjectInternal(btBroadphaseProxy * otherProxy, btDispatcher * dispatcher, btBroadphaseProxy* thisProxy = 0);
 
-	int	getNumOverlappingObjects() const
+	int getNumOverlappingObjects() const
 	{
 		return m_overlappingObjects.size();
 	}
 
-	btCollisionObject*	getOverlappingObject(int index)
+	btCollisionObject* getOverlappingObject(int index)
 	{
 		return m_overlappingObjects[index];
 	}
 
-	const btCollisionObject*	getOverlappingObject(int index) const
+	const btCollisionObject* getOverlappingObject(int index) const
 	{
 		return m_overlappingObjects[index];
 	}
 
-	btAlignedObjectArray<btCollisionObject*>&	getOverlappingPairs()
+	btAlignedObjectArray<btCollisionObject*>& getOverlappingPairs()
 	{
 		return m_overlappingObjects;
 	}
 
-	const btAlignedObjectArray<btCollisionObject*>	getOverlappingPairs() const
+	const btAlignedObjectArray<btCollisionObject*> getOverlappingPairs() const
 	{
 		return m_overlappingObjects;
 	}
@@ -81,49 +79,43 @@ public:
 	// internal cast
 	//
 
-	static const btGhostObject*	upcast(const btCollisionObject* colObj)
+	static const btGhostObject* upcast(const btCollisionObject* colObj)
 	{
-		if (colObj->getInternalType()==CO_GHOST_OBJECT)
+		if (colObj->getInternalType() == CO_GHOST_OBJECT)
 			return (const btGhostObject*)colObj;
 		return 0;
 	}
-	static btGhostObject*			upcast(btCollisionObject* colObj)
+	static btGhostObject* upcast(btCollisionObject * colObj)
 	{
-		if (colObj->getInternalType()==CO_GHOST_OBJECT)
+		if (colObj->getInternalType() == CO_GHOST_OBJECT)
 			return (btGhostObject*)colObj;
 		return 0;
 	}
-
 };
 
-class	btPairCachingGhostObject : public btGhostObject
+class btPairCachingGhostObject : public btGhostObject
 {
-	btHashedOverlappingPairCache*	m_hashPairCache;
+	btHashedOverlappingPairCache* m_hashPairCache;
 
 public:
-
 	btPairCachingGhostObject();
 
 	virtual ~btPairCachingGhostObject();
 
 	///this method is mainly for expert/internal use only.
-	virtual void	addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0);
+	virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy = 0);
 
-	virtual void	removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0);
+	virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btDispatcher* dispatcher, btBroadphaseProxy* thisProxy = 0);
 
-	btHashedOverlappingPairCache*	getOverlappingPairCache()
+	btHashedOverlappingPairCache* getOverlappingPairCache()
 	{
 		return m_hashPairCache;
 	}
-
 };
 
-
-
 ///The btGhostPairCallback interfaces and forwards adding and removal of overlapping pairs from the btBroadphaseInterface to btGhostObject.
 class btGhostPairCallback : public btOverlappingPairCallback
 {
-	
 public:
 	btGhostPairCallback()
 	{
@@ -131,15 +123,14 @@ public:
 
 	virtual ~btGhostPairCallback()
 	{
-		
 	}
 
-	virtual btBroadphasePair*	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+	virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
 	{
-		btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject;
-		btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject;
-		btGhostObject* ghost0 = 		btGhostObject::upcast(colObj0);
-		btGhostObject* ghost1 = 		btGhostObject::upcast(colObj1);
+		btCollisionObject* colObj0 = (btCollisionObject*)proxy0->m_clientObject;
+		btCollisionObject* colObj1 = (btCollisionObject*)proxy1->m_clientObject;
+		btGhostObject* ghost0 = btGhostObject::upcast(colObj0);
+		btGhostObject* ghost1 = btGhostObject::upcast(colObj1);
 		if (ghost0)
 			ghost0->addOverlappingObjectInternal(proxy1, proxy0);
 		if (ghost1)
@@ -147,29 +138,25 @@ public:
 		return 0;
 	}
 
-	virtual void*	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
+	virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher)
 	{
-		btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject;
-		btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject;
-		btGhostObject* ghost0 = 		btGhostObject::upcast(colObj0);
-		btGhostObject* ghost1 = 		btGhostObject::upcast(colObj1);
+		btCollisionObject* colObj0 = (btCollisionObject*)proxy0->m_clientObject;
+		btCollisionObject* colObj1 = (btCollisionObject*)proxy1->m_clientObject;
+		btGhostObject* ghost0 = btGhostObject::upcast(colObj0);
+		btGhostObject* ghost1 = btGhostObject::upcast(colObj1);
 		if (ghost0)
-			ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0);
+			ghost0->removeOverlappingObjectInternal(proxy1, dispatcher, proxy0);
 		if (ghost1)
-			ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1);
+			ghost1->removeOverlappingObjectInternal(proxy0, dispatcher, proxy1);
 		return 0;
 	}
 
-	virtual void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/)
+	virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/, btDispatcher* /*dispatcher*/)
 	{
 		btAssert(0);
 		//need to keep track of all ghost objects and call them here
 		//m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher);
 	}
-
-	
-
 };
 
 #endif
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp
index 8c8a7c3c1e3..b686d98d1e7 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp
@@ -13,61 +13,49 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #include "btHashedSimplePairCache.h"
 
-
 #include <stdio.h>
 
-int	gOverlappingSimplePairs = 0;
-int gRemoveSimplePairs =0;
-int gAddedSimplePairs =0;
-int gFindSimplePairs =0;
-
-
-
+#ifdef BT_DEBUG_COLLISION_PAIRS
+int gOverlappingSimplePairs = 0;
+int gRemoveSimplePairs = 0;
+int gAddedSimplePairs = 0;
+int gFindSimplePairs = 0;
+#endif  //BT_DEBUG_COLLISION_PAIRS
 
-btHashedSimplePairCache::btHashedSimplePairCache() {
-	int initialAllocatedSize= 2;
+btHashedSimplePairCache::btHashedSimplePairCache()
+{
+	int initialAllocatedSize = 2;
 	m_overlappingPairArray.reserve(initialAllocatedSize);
 	growTables();
 }
 
-
-
-
 btHashedSimplePairCache::~btHashedSimplePairCache()
 {
 }
 
-
-
-
-
-
 void btHashedSimplePairCache::removeAllPairs()
 {
 	m_overlappingPairArray.clear();
 	m_hashTable.clear();
 	m_next.clear();
 
-	int initialAllocatedSize= 2;
+	int initialAllocatedSize = 2;
 	m_overlappingPairArray.reserve(initialAllocatedSize);
 	growTables();
 }
 
-
-
 btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB)
 {
+#ifdef BT_DEBUG_COLLISION_PAIRS
 	gFindSimplePairs++;
-	
-	
+#endif
+
 	/*if (indexA > indexB) 
 		btSwap(indexA, indexB);*/
 
-	int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1));
 
 	if (hash >= m_hashTable.size())
 	{
@@ -92,9 +80,8 @@ btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB)
 
 //#include <stdio.h>
 
-void	btHashedSimplePairCache::growTables()
+void btHashedSimplePairCache::growTables()
 {
-
 	int newCapacity = m_overlappingPairArray.capacity();
 
 	if (m_hashTable.size() < newCapacity)
@@ -105,10 +92,9 @@ void	btHashedSimplePairCache::growTables()
 		m_hashTable.resize(newCapacity);
 		m_next.resize(newCapacity);
 
-
 		int i;
 
-		for (i= 0; i < newCapacity; ++i)
+		for (i = 0; i < newCapacity; ++i)
 		{
 			m_hashTable[i] = BT_SIMPLE_NULL_PAIR;
 		}
@@ -117,27 +103,22 @@ void	btHashedSimplePairCache::growTables()
 			m_next[i] = BT_SIMPLE_NULL_PAIR;
 		}
 
-		for(i=0;i<curHashtableSize;i++)
+		for (i = 0; i < curHashtableSize; i++)
 		{
-	
 			const btSimplePair& pair = m_overlappingPairArray[i];
 			int indexA = pair.m_indexA;
 			int indexB = pair.m_indexB;
-			
-			int	hashValue = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));	// New hash value with new mask
+
+			int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1));  // New hash value with new mask
 			m_next[i] = m_hashTable[hashValue];
 			m_hashTable[hashValue] = i;
 		}
-
-
 	}
 }
 
 btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB)
 {
-
-	int	hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));	// New hash value with new mask
-
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1));  // New hash value with new mask
 
 	btSimplePair* pair = internalFindPair(indexA, indexB, hash);
 	if (pair != NULL)
@@ -155,30 +136,29 @@ btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB)
 	{
 		growTables();
 		//hash with new capacity
-		hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));
+		hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1));
 	}
-	
-	pair = new (mem) btSimplePair(indexA,indexB);
+
+	pair = new (mem) btSimplePair(indexA, indexB);
 
 	pair->m_userPointer = 0;
-	
+
 	m_next[count] = m_hashTable[hash];
 	m_hashTable[hash] = count;
 
 	return pair;
 }
 
-
-
 void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB)
 {
+#ifdef BT_DEBUG_COLLISION_PAIRS
 	gRemoveSimplePairs++;
-	
+#endif
 
 	/*if (indexA > indexB) 
 		btSwap(indexA, indexB);*/
 
-	int	hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1));
 
 	btSimplePair* pair = internalFindPair(indexA, indexB, hash);
 	if (pair == NULL)
@@ -186,10 +166,8 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB)
 		return 0;
 	}
 
-	
 	void* userData = pair->m_userPointer;
 
-
 	int pairIndex = int(pair - &m_overlappingPairArray[0]);
 	btAssert(pairIndex < m_overlappingPairArray.size());
 
@@ -229,8 +207,8 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB)
 
 	// Remove the last pair from the hash table.
 	const btSimplePair* last = &m_overlappingPairArray[lastPairIndex];
-		/* missing swap here too, Nat. */ 
-	int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_indexA), static_cast<unsigned int>(last->m_indexB)) & (m_overlappingPairArray.capacity()-1));
+	/* missing swap here too, Nat. */
+	int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_indexA), static_cast<unsigned int>(last->m_indexB)) & (m_overlappingPairArray.capacity() - 1));
 
 	index = m_hashTable[lastHash];
 	btAssert(index != BT_SIMPLE_NULL_PAIR);
@@ -264,13 +242,3 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB)
 	return userData;
 }
 //#include <stdio.h>
-
-
-
-
-
-
-
-
-
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h
index 186964d723e..fd38a4f0e19 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h
@@ -16,142 +16,126 @@ subject to the following restrictions:
 #ifndef BT_HASHED_SIMPLE_PAIR_CACHE_H
 #define BT_HASHED_SIMPLE_PAIR_CACHE_H
 
-
-
 #include "LinearMath/btAlignedObjectArray.h"
 
-const int BT_SIMPLE_NULL_PAIR=0xffffffff;
+const int BT_SIMPLE_NULL_PAIR = 0xffffffff;
 
 struct btSimplePair
 {
-	btSimplePair(int indexA,int indexB)
-		:m_indexA(indexA),
-		m_indexB(indexB),
-		m_userPointer(0)
+	btSimplePair(int indexA, int indexB)
+		: m_indexA(indexA),
+		  m_indexB(indexB),
+		  m_userPointer(0)
 	{
 	}
 
 	int m_indexA;
 	int m_indexB;
-	union
-	{
-		void*	m_userPointer;
-		int		m_userValue;
+	union {
+		void* m_userPointer;
+		int m_userValue;
 	};
 };
 
-typedef btAlignedObjectArray<btSimplePair>	btSimplePairArray;
-
-
+typedef btAlignedObjectArray<btSimplePair> btSimplePairArray;
 
+#ifdef BT_DEBUG_COLLISION_PAIRS
 extern int gOverlappingSimplePairs;
 extern int gRemoveSimplePairs;
 extern int gAddedSimplePairs;
 extern int gFindSimplePairs;
-
-
-
+#endif  //BT_DEBUG_COLLISION_PAIRS
 
 class btHashedSimplePairCache
 {
-	btSimplePairArray	m_overlappingPairArray;
-		
+	btSimplePairArray m_overlappingPairArray;
 
 protected:
-	
-	btAlignedObjectArray<int>	m_hashTable;
-	btAlignedObjectArray<int>	m_next;
-	
+	btAlignedObjectArray<int> m_hashTable;
+	btAlignedObjectArray<int> m_next;
 
 public:
 	btHashedSimplePairCache();
 	virtual ~btHashedSimplePairCache();
-	
+
 	void removeAllPairs();
 
-	virtual void*	removeOverlappingPair(int indexA,int indexB);
-	
+	virtual void* removeOverlappingPair(int indexA, int indexB);
+
 	// Add a pair and return the new pair. If the pair already exists,
 	// no new pair is created and the old one is returned.
-	virtual btSimplePair* 	addOverlappingPair(int indexA,int indexB)
+	virtual btSimplePair* addOverlappingPair(int indexA, int indexB)
 	{
+#ifdef BT_DEBUG_COLLISION_PAIRS
 		gAddedSimplePairs++;
+#endif
 
-		return internalAddPair(indexA,indexB);
+		return internalAddPair(indexA, indexB);
 	}
 
-	
-	virtual btSimplePair*	getOverlappingPairArrayPtr()
+	virtual btSimplePair* getOverlappingPairArrayPtr()
 	{
 		return &m_overlappingPairArray[0];
 	}
 
-	const btSimplePair*	getOverlappingPairArrayPtr() const
+	const btSimplePair* getOverlappingPairArrayPtr() const
 	{
 		return &m_overlappingPairArray[0];
 	}
 
-	btSimplePairArray&	getOverlappingPairArray()
+	btSimplePairArray& getOverlappingPairArray()
 	{
 		return m_overlappingPairArray;
 	}
 
-	const btSimplePairArray&	getOverlappingPairArray() const
+	const btSimplePairArray& getOverlappingPairArray() const
 	{
 		return m_overlappingPairArray;
 	}
 
-	
-	btSimplePair* findPair(int indexA,int indexB);
+	btSimplePair* findPair(int indexA, int indexB);
 
 	int GetCount() const { return m_overlappingPairArray.size(); }
 
-	int	getNumOverlappingPairs() const
+	int getNumOverlappingPairs() const
 	{
 		return m_overlappingPairArray.size();
 	}
+
 private:
-	
-	btSimplePair* 	internalAddPair(int indexA, int indexB);
+	btSimplePair* internalAddPair(int indexA, int indexB);
 
-	void	growTables();
+	void growTables();
 
 	SIMD_FORCE_INLINE bool equalsPair(const btSimplePair& pair, int indexA, int indexB)
-	{	
+	{
 		return pair.m_indexA == indexA && pair.m_indexB == indexB;
 	}
 
-	
-	
-	SIMD_FORCE_INLINE	unsigned int getHash(unsigned int indexA, unsigned int indexB)
+	SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB)
 	{
-		int key = static_cast<int>(((unsigned int)indexA) | (((unsigned int)indexB) <<16));
+		unsigned int key = indexA | (indexB << 16);
 		// Thomas Wang's hash
 
 		key += ~(key << 15);
-		key ^=  (key >> 10);
-		key +=  (key << 3);
-		key ^=  (key >> 6);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
 		key += ~(key << 11);
-		key ^=  (key >> 16);
-		return static_cast<unsigned int>(key);
+		key ^= (key >> 16);
+		return key;
 	}
-	
-
-
 
-
-	SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA , int proxyIdB, int hash)
+	SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA, int proxyIdB, int hash)
 	{
-		
 		int index = m_hashTable[hash];
-		
-		while( index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false)
+
+		while (index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false)
 		{
 			index = m_next[index];
 		}
 
-		if ( index == BT_SIMPLE_NULL_PAIR )
+		if (index == BT_SIMPLE_NULL_PAIR)
 		{
 			return NULL;
 		}
@@ -160,13 +144,6 @@ private:
 
 		return &m_overlappingPairArray[index];
 	}
-
-	
 };
 
-
-
-
-#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H
-
-
+#endif  //BT_HASHED_SIMPLE_PAIR_CACHE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
index 6cba442ca5d..a71700f58af 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
@@ -1,6 +1,8 @@
 #include "btInternalEdgeUtility.h"
 
 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
+
 #include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
@@ -12,50 +14,44 @@
 
 #ifdef DEBUG_INTERNAL_EDGE
 #include <stdio.h>
-#endif //DEBUG_INTERNAL_EDGE
-
+#endif  //DEBUG_INTERNAL_EDGE
 
 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 static btIDebugDraw* gDebugDrawer = 0;
 
-void	btSetDebugDrawer(btIDebugDraw* debugDrawer)
+void btSetDebugDrawer(btIDebugDraw* debugDrawer)
 {
 	gDebugDrawer = debugDrawer;
 }
 
-static void    btDebugDrawLine(const btVector3& from,const btVector3& to, const btVector3& color)
+static void btDebugDrawLine(const btVector3& from, const btVector3& to, const btVector3& color)
 {
 	if (gDebugDrawer)
-		gDebugDrawer->drawLine(from,to,color);
+		gDebugDrawer->drawLine(from, to, color);
 }
-#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
-
+#endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 
-static int	btGetHash(int partId, int triangleIndex)
+static int btGetHash(int partId, int triangleIndex)
 {
-	int hash = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
+	int hash = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
 	return hash;
 }
 
-
-
-static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA,const btVector3& normalB)
+static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA, const btVector3& normalB)
 {
-	const btVector3 refAxis0  = edgeA;
-	const btVector3 refAxis1  = normalA;
+	const btVector3 refAxis0 = edgeA;
+	const btVector3 refAxis1 = normalA;
 	const btVector3 swingAxis = normalB;
 	btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
-	return  angle;
+	return angle;
 }
 
-
 struct btConnectivityProcessor : public btTriangleCallback
 {
-	int				m_partIdA;
-	int				m_triangleIndexA;
-	btVector3*		m_triangleVerticesA;
-	btTriangleInfoMap*	m_triangleInfoMap;
-
+	int m_partIdA;
+	int m_triangleIndexA;
+	btVector3* m_triangleVerticesA;
+	btTriangleInfoMap* m_triangleInfoMap;
 
 	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 	{
@@ -69,18 +65,17 @@ struct btConnectivityProcessor : public btTriangleCallback
 
 		//search for shared vertices and edges
 		int numshared = 0;
-		int sharedVertsA[3]={-1,-1,-1};
-		int sharedVertsB[3]={-1,-1,-1};
+		int sharedVertsA[3] = {-1, -1, -1};
+		int sharedVertsB[3] = {-1, -1, -1};
 
 		///skip degenerate triangles
-		btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2();
+		btScalar crossBSqr = ((triangle[1] - triangle[0]).cross(triangle[2] - triangle[0])).length2();
 		if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold)
 			return;
 
-
-		btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2();
+		btScalar crossASqr = ((m_triangleVerticesA[1] - m_triangleVerticesA[0]).cross(m_triangleVerticesA[2] - m_triangleVerticesA[0])).length2();
 		///skip degenerate triangles
-		if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold)
+		if (crossASqr < m_triangleInfoMap->m_equalVertexThreshold)
 			return;
 
 #if 0
@@ -96,36 +91,36 @@ struct btConnectivityProcessor : public btTriangleCallback
 			triangle[2].getX(),triangle[2].getY(),triangle[2].getZ());
 #endif
 
-		for (int i=0;i<3;i++)
+		for (int i = 0; i < 3; i++)
 		{
-			for (int j=0;j<3;j++)
+			for (int j = 0; j < 3; j++)
 			{
-				if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold)
+				if ((m_triangleVerticesA[i] - triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold)
 				{
 					sharedVertsA[numshared] = i;
 					sharedVertsB[numshared] = j;
 					numshared++;
 					///degenerate case
-					if(numshared >= 3)
+					if (numshared >= 3)
 						return;
 				}
 			}
 			///degenerate case
-			if(numshared >= 3)
+			if (numshared >= 3)
 				return;
 		}
 		switch (numshared)
 		{
-		case 0:
+			case 0:
 			{
 				break;
 			}
-		case 1:
+			case 1:
 			{
 				//shared vertex
 				break;
 			}
-		case 2:
+			case 2:
 			{
 				//shared edge
 				//we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
@@ -138,26 +133,25 @@ struct btConnectivityProcessor : public btTriangleCallback
 					sharedVertsB[0] = tmp;
 				}
 
-				int hash = btGetHash(m_partIdA,m_triangleIndexA);
+				int hash = btGetHash(m_partIdA, m_triangleIndexA);
 
 				btTriangleInfo* info = m_triangleInfoMap->find(hash);
 				if (!info)
 				{
 					btTriangleInfo tmp;
-					m_triangleInfoMap->insert(hash,tmp);
+					m_triangleInfoMap->insert(hash, tmp);
 					info = m_triangleInfoMap->find(hash);
 				}
 
-				int sumvertsA = sharedVertsA[0]+sharedVertsA[1];
-				int otherIndexA = 3-sumvertsA;
+				int sumvertsA = sharedVertsA[0] + sharedVertsA[1];
+				int otherIndexA = 3 - sumvertsA;
 
-				
-				btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]);
+				btVector3 edge(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]);
 
-				btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]);
-				int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]);
+				btTriangleShape tA(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]);
+				int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]);
 
-				btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]);
+				btTriangleShape tB(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]);
 				//btTriangleShape tB(triangle[0],triangle[1],triangle[2]);
 
 				btVector3 normalA;
@@ -168,26 +162,25 @@ struct btConnectivityProcessor : public btTriangleCallback
 				btVector3 edgeCrossA = edge.cross(normalA).normalize();
 
 				{
-					btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]];
+					btVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]];
 					if (edgeCrossA.dot(tmp) < 0)
 					{
-						edgeCrossA*=-1;
+						edgeCrossA *= -1;
 					}
 				}
 
 				btVector3 edgeCrossB = edge.cross(normalB).normalize();
 
 				{
-					btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]];
+					btVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]];
 					if (edgeCrossB.dot(tmp) < 0)
 					{
-						edgeCrossB*=-1;
+						edgeCrossB *= -1;
 					}
 				}
 
-				btScalar	angle2 = 0;
-				btScalar	ang4 = 0.f;
-
+				btScalar angle2 = 0;
+				btScalar ang4 = 0.f;
 
 				btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB);
 				btScalar len2 = calculatedEdge.length2();
@@ -196,52 +189,47 @@ struct btConnectivityProcessor : public btTriangleCallback
 				//btVector3 calculatedNormalB = normalA;
 				bool isConvex = false;
 
-				if (len2<m_triangleInfoMap->m_planarEpsilon)
+				if (len2 < m_triangleInfoMap->m_planarEpsilon)
 				{
 					angle2 = 0.f;
 					ang4 = 0.f;
-				} else
+				}
+				else
 				{
-
 					calculatedEdge.normalize();
 					btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA);
 					calculatedNormalA.normalize();
-					angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB);
-					ang4 = SIMD_PI-angle2;
+					angle2 = btGetAngle(calculatedNormalA, edgeCrossA, edgeCrossB);
+					ang4 = SIMD_PI - angle2;
 					btScalar dotA = normalA.dot(edgeCrossB);
 					///@todo: check if we need some epsilon, due to floating point imprecision
-					isConvex = (dotA<0.);
+					isConvex = (dotA < 0.);
 
 					correctedAngle = isConvex ? ang4 : -ang4;
 				}
 
-				
-
-				
-							
-				//alternatively use 
+				//alternatively use
 				//btVector3 calculatedNormalB2 = quatRotate(orn,normalA);
 
-
 				switch (sumvertsA)
 				{
-				case 1:
+					case 1:
 					{
-						btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1];
-						btQuaternion orn(edge,-correctedAngle);
-						btVector3 computedNormalB = quatRotate(orn,normalA);
+						btVector3 edge = m_triangleVerticesA[0] - m_triangleVerticesA[1];
+						btQuaternion orn(edge, -correctedAngle);
+						btVector3 computedNormalB = quatRotate(orn, normalA);
 						btScalar bla = computedNormalB.dot(normalB);
-						if (bla<0)
+						if (bla < 0)
 						{
-							computedNormalB*=-1;
+							computedNormalB *= -1;
 							info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB;
 						}
 #ifdef DEBUG_INTERNAL_EDGE
-						if ((computedNormalB-normalB).length()>0.0001)
+						if ((computedNormalB - normalB).length() > 0.0001)
 						{
 							printf("warning: normals not identical\n");
 						}
-#endif//DEBUG_INTERNAL_EDGE
+#endif  //DEBUG_INTERNAL_EDGE
 
 						info->m_edgeV0V1Angle = -correctedAngle;
 
@@ -249,44 +237,44 @@ struct btConnectivityProcessor : public btTriangleCallback
 							info->m_flags |= TRI_INFO_V0V1_CONVEX;
 						break;
 					}
-				case 2:
+					case 2:
 					{
-						btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0];
-						btQuaternion orn(edge,-correctedAngle);
-						btVector3 computedNormalB = quatRotate(orn,normalA);
-						if (computedNormalB.dot(normalB)<0)
+						btVector3 edge = m_triangleVerticesA[2] - m_triangleVerticesA[0];
+						btQuaternion orn(edge, -correctedAngle);
+						btVector3 computedNormalB = quatRotate(orn, normalA);
+						if (computedNormalB.dot(normalB) < 0)
 						{
-							computedNormalB*=-1;
+							computedNormalB *= -1;
 							info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB;
 						}
 
 #ifdef DEBUG_INTERNAL_EDGE
-						if ((computedNormalB-normalB).length()>0.0001)
+						if ((computedNormalB - normalB).length() > 0.0001)
 						{
 							printf("warning: normals not identical\n");
 						}
-#endif //DEBUG_INTERNAL_EDGE
+#endif  //DEBUG_INTERNAL_EDGE
 						info->m_edgeV2V0Angle = -correctedAngle;
 						if (isConvex)
 							info->m_flags |= TRI_INFO_V2V0_CONVEX;
-						break;	
+						break;
 					}
-				case 3:
+					case 3:
 					{
-						btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2];
-						btQuaternion orn(edge,-correctedAngle);
-						btVector3 computedNormalB = quatRotate(orn,normalA);
-						if (computedNormalB.dot(normalB)<0)
+						btVector3 edge = m_triangleVerticesA[1] - m_triangleVerticesA[2];
+						btQuaternion orn(edge, -correctedAngle);
+						btVector3 computedNormalB = quatRotate(orn, normalA);
+						if (computedNormalB.dot(normalB) < 0)
 						{
 							info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB;
-							computedNormalB*=-1;
+							computedNormalB *= -1;
 						}
 #ifdef DEBUG_INTERNAL_EDGE
-						if ((computedNormalB-normalB).length()>0.0001)
+						if ((computedNormalB - normalB).length() > 0.0001)
 						{
 							printf("warning: normals not identical\n");
 						}
-#endif //DEBUG_INTERNAL_EDGE
+#endif  //DEBUG_INTERNAL_EDGE
 						info->m_edgeV1V2Angle = -correctedAngle;
 
 						if (isConvex)
@@ -297,18 +285,50 @@ struct btConnectivityProcessor : public btTriangleCallback
 
 				break;
 			}
-		default:
+			default:
 			{
 				//				printf("warning: duplicate triangle\n");
 			}
-
 		}
 	}
 };
+
+
+struct b3ProcessAllTrianglesHeightfield: public btTriangleCallback
+{
+	btHeightfieldTerrainShape* m_heightfieldShape;
+	btTriangleInfoMap* m_triangleInfoMap;
+	
+
+	b3ProcessAllTrianglesHeightfield(btHeightfieldTerrainShape* heightFieldShape, btTriangleInfoMap* triangleInfoMap)
+		:m_heightfieldShape(heightFieldShape),
+		m_triangleInfoMap(triangleInfoMap)
+	{
+	}
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+	{
+		btConnectivityProcessor connectivityProcessor;
+		connectivityProcessor.m_partIdA = partId;
+		connectivityProcessor.m_triangleIndexA = triangleIndex;
+		connectivityProcessor.m_triangleVerticesA = triangle;
+		connectivityProcessor.m_triangleInfoMap = m_triangleInfoMap;
+		btVector3 aabbMin, aabbMax;
+		aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+		aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
+		aabbMin.setMin(triangle[0]);
+		aabbMax.setMax(triangle[0]);
+		aabbMin.setMin(triangle[1]);
+		aabbMax.setMax(triangle[1]);
+		aabbMin.setMin(triangle[2]);
+		aabbMax.setMax(triangle[2]);
+
+		m_heightfieldShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax);
+	}
+};
 /////////////////////////////////////////////////////////
 /////////////////////////////////////////////////////////
 
-void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap)
+void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap)
 {
 	//the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
 	if (trimeshShape->getTriangleInfoMap())
@@ -319,46 +339,51 @@ void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangle
 	btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface();
 	const btVector3& meshScaling = meshInterface->getScaling();
 
-	for (int partId = 0; partId< meshInterface->getNumSubParts();partId++)
+	for (int partId = 0; partId < meshInterface->getNumSubParts(); partId++)
 	{
-		const unsigned char *vertexbase = 0;
+		const unsigned char* vertexbase = 0;
 		int numverts = 0;
 		PHY_ScalarType type = PHY_INTEGER;
 		int stride = 0;
-		const unsigned char *indexbase = 0;
+		const unsigned char* indexbase = 0;
 		int indexstride = 0;
 		int numfaces = 0;
 		PHY_ScalarType indicestype = PHY_INTEGER;
 		//PHY_ScalarType indexType=0;
 
 		btVector3 triangleVerts[3];
-		meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,	type,stride,&indexbase,indexstride,numfaces,indicestype,partId);
-		btVector3 aabbMin,aabbMax;
+		meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, partId);
+		btVector3 aabbMin, aabbMax;
 
-		for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++)
+		for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++)
 		{
-			unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride);
+			unsigned int* gfxbase = (unsigned int*)(indexbase + triangleIndex * indexstride);
 
-			for (int j=2;j>=0;j--)
+			for (int j = 2; j >= 0; j--)
 			{
-
-				int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
+				int graphicsindex;
+                                switch (indicestype) {
+                                        case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
+                                        case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
+                                        case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
+                                        default: btAssert(0);
+                                }
 				if (type == PHY_FLOAT)
 				{
-					float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
+					float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
 					triangleVerts[j] = btVector3(
-						graphicsbase[0]*meshScaling.getX(),
-						graphicsbase[1]*meshScaling.getY(),
-						graphicsbase[2]*meshScaling.getZ());
+						graphicsbase[0] * meshScaling.getX(),
+						graphicsbase[1] * meshScaling.getY(),
+						graphicsbase[2] * meshScaling.getZ());
 				}
 				else
 				{
-					double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
-					triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
+					double* graphicsbase = (double*)(vertexbase + graphicsindex * stride);
+					triangleVerts[j] = btVector3(btScalar(graphicsbase[0] * meshScaling.getX()), btScalar(graphicsbase[1] * meshScaling.getY()), btScalar(graphicsbase[2] * meshScaling.getZ()));
 				}
 			}
-			aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-			aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); 
+			aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+			aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
 			aabbMin.setMin(triangleVerts[0]);
 			aabbMax.setMax(triangleVerts[0]);
 			aabbMin.setMin(triangleVerts[1]);
@@ -370,131 +395,177 @@ void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangle
 			connectivityProcessor.m_partIdA = partId;
 			connectivityProcessor.m_triangleIndexA = triangleIndex;
 			connectivityProcessor.m_triangleVerticesA = &triangleVerts[0];
-			connectivityProcessor.m_triangleInfoMap  = triangleInfoMap;
+			connectivityProcessor.m_triangleInfoMap = triangleInfoMap;
 
-			trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax);
+			trimeshShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax);
 		}
-
 	}
-
 }
 
 
+void btGenerateInternalEdgeInfo(btHeightfieldTerrainShape* heightfieldShape, btTriangleInfoMap* triangleInfoMap)
+{
+
+	//the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
+	if (heightfieldShape->getTriangleInfoMap())
+		return;
+
+	heightfieldShape->setTriangleInfoMap(triangleInfoMap);
+
+	//get all the triangles of the heightfield
 
+	btVector3 aabbMin, aabbMax;
+
+	aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+	aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
+
+	b3ProcessAllTrianglesHeightfield processHeightfield(heightfieldShape, triangleInfoMap);
+	heightfieldShape->processAllTriangles(&processHeightfield, aabbMin, aabbMax);
+
+}
 
 // Given a point and a line segment (defined by two points), compute the closest point
 // in the line.  Cap the point at the endpoints of the line segment.
-void btNearestPointInLineSegment(const btVector3 &point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint)
+void btNearestPointInLineSegment(const btVector3& point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint)
 {
-	btVector3 lineDelta     = line1 - line0;
+	btVector3 lineDelta = line1 - line0;
 
 	// Handle degenerate lines
-	if ( lineDelta.fuzzyZero())
+	if (lineDelta.fuzzyZero())
 	{
 		nearestPoint = line0;
 	}
 	else
 	{
-		btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta);
+		btScalar delta = (point - line0).dot(lineDelta) / (lineDelta).dot(lineDelta);
 
 		// Clamp the point to conform to the segment's endpoints
-		if ( delta < 0 )
+		if (delta < 0)
 			delta = 0;
-		else if ( delta > 1 )
+		else if (delta > 1)
 			delta = 1;
 
-		nearestPoint = line0 + lineDelta*delta;
+		nearestPoint = line0 + lineDelta * delta;
 	}
 }
 
-
-
-
-bool	btClampNormal(const btVector3& edge,const btVector3& tri_normal_org,const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 & clampedLocalNormal)
+bool btClampNormal(const btVector3& edge, const btVector3& tri_normal_org, const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3& clampedLocalNormal)
 {
 	btVector3 tri_normal = tri_normal_org;
 	//we only have a local triangle normal, not a local contact normal -> only normal in world space...
 	//either compute the current angle all in local space, or all in world space
 
 	btVector3 edgeCross = edge.cross(tri_normal).normalize();
-	btScalar curAngle = btGetAngle(edgeCross,tri_normal,localContactNormalOnB);
+	btScalar curAngle = btGetAngle(edgeCross, tri_normal, localContactNormalOnB);
 
-	if (correctedEdgeAngle<0)
+	if (correctedEdgeAngle < 0)
 	{
 		if (curAngle < correctedEdgeAngle)
 		{
-			btScalar diffAngle = correctedEdgeAngle-curAngle;
-			btQuaternion rotation(edge,diffAngle );
-			clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
+			btScalar diffAngle = correctedEdgeAngle - curAngle;
+			btQuaternion rotation(edge, diffAngle);
+			clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB;
 			return true;
 		}
 	}
 
-	if (correctedEdgeAngle>=0)
+	if (correctedEdgeAngle >= 0)
 	{
 		if (curAngle > correctedEdgeAngle)
 		{
-			btScalar diffAngle = correctedEdgeAngle-curAngle;
-			btQuaternion rotation(edge,diffAngle );
-			clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
+			btScalar diffAngle = correctedEdgeAngle - curAngle;
+			btQuaternion rotation(edge, diffAngle);
+			clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB;
 			return true;
 		}
 	}
 	return false;
 }
 
-
-
 /// Changes a btManifoldPoint collision normal to the normal from the mesh.
-void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
+void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
 {
 	//btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
 	if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
 		return;
 
+	
+	btTriangleInfoMap* triangleInfoMapPtr = 0;
+
+	if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TERRAIN_SHAPE_PROXYTYPE)
+	{
+		btHeightfieldTerrainShape* heightfield = (btHeightfieldTerrainShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
+		triangleInfoMapPtr = heightfield->getTriangleInfoMap();
+
+//#define USE_HEIGHTFIELD_TRIANGLES
+#ifdef USE_HEIGHTFIELD_TRIANGLES
+		btVector3 newNormal = btVector3(0, 0, 1);
+
+		const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
+		btVector3 tri_normal;
+		tri_shape->calcNormal(tri_normal);
+		newNormal = tri_normal;
+		//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
+		cp.m_normalWorldOnB = newNormal;
+		// Reproject collision point along normal. (what about cp.m_distance1?)
+		cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
+		cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
+		return;
+#endif
+	}
+
+
 	btBvhTriangleMeshShape* trimesh = 0;
+
+	if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
+	{
+		trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
+	}
+	else
+	{
+		if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
+		{
+			trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
+		}
+	}
+	if (trimesh)
+	{
+		triangleInfoMapPtr = (btTriangleInfoMap*)trimesh->getTriangleInfoMap();
+	}
+	
 	
-	if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
-	   trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
-   else	   
-	   trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
-	   
-   	btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
 	if (!triangleInfoMapPtr)
 		return;
 
-	int hash = btGetHash(partId0,index0);
-
+	int hash = btGetHash(partId0, index0);
 
 	btTriangleInfo* info = triangleInfoMapPtr->find(hash);
 	if (!info)
 		return;
 
-	btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f;
-	
+	btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.f : -1.f;
+
 	const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
-	btVector3 v0,v1,v2;
-	tri_shape->getVertex(0,v0);
-	tri_shape->getVertex(1,v1);
-	tri_shape->getVertex(2,v2);
+	btVector3 v0, v1, v2;
+	tri_shape->getVertex(0, v0);
+	tri_shape->getVertex(1, v1);
+	tri_shape->getVertex(2, v2);
 
 	//btVector3 center = (v0+v1+v2)*btScalar(1./3.);
 
-	btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);
+	btVector3 red(1, 0, 0), green(0, 1, 0), blue(0, 0, 1), white(1, 1, 1), black(0, 0, 0);
 	btVector3 tri_normal;
 	tri_shape->calcNormal(tri_normal);
 
 	//btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
 	btVector3 nearest;
-	btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest);
+	btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest);
 
 	btVector3 contact = cp.m_localPointB;
 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 	const btTransform& tr = colObj0->getWorldTransform();
-	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red);
-#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
-
-
+	btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, red);
+#endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 
 	bool isNearEdge = false;
 
@@ -502,334 +573,325 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWr
 	int numConvexEdgeHits = 0;
 
 	btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
-	localContactNormalOnB.normalize();//is this necessary?
-	
+	localContactNormalOnB.normalize();  //is this necessary?
+
 	// Get closest edge
-	int      bestedge=-1;
-	btScalar    disttobestedge=BT_LARGE_FLOAT;
+	int bestedge = -1;
+	btScalar disttobestedge = BT_LARGE_FLOAT;
 	//
 	// Edge 0 -> 1
-	if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
-	{	
-	   btVector3 nearest;
-	   btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest );
-	   btScalar     len=(contact-nearest).length();
-	   //
-	   if( len < disttobestedge )
-	   {
-	      bestedge=0;
-	      disttobestedge=len;
-      }	      
-   }	   
+	if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
+	{
+		btVector3 nearest;
+		btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest);
+		btScalar len = (contact - nearest).length();
+		//
+		if (len < disttobestedge)
+		{
+			bestedge = 0;
+			disttobestedge = len;
+		}
+	}
 	// Edge 1 -> 2
-	if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
-	{	
-	   btVector3 nearest;
-	   btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest );
-	   btScalar     len=(contact-nearest).length();
-	   //
-	   if( len < disttobestedge )
-	   {
-	      bestedge=1;
-	      disttobestedge=len;
-      }	      
-   }	   
+	if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
+	{
+		btVector3 nearest;
+		btNearestPointInLineSegment(cp.m_localPointB, v1, v2, nearest);
+		btScalar len = (contact - nearest).length();
+		//
+		if (len < disttobestedge)
+		{
+			bestedge = 1;
+			disttobestedge = len;
+		}
+	}
 	// Edge 2 -> 0
-	if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
-	{	
-	   btVector3 nearest;
-	   btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest );
-	   btScalar     len=(contact-nearest).length();
-	   //
-	   if( len < disttobestedge )
-	   {
-	      bestedge=2;
-	      disttobestedge=len;
-      }	      
-   }   	      	
-	
+	if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
+	{
+		btVector3 nearest;
+		btNearestPointInLineSegment(cp.m_localPointB, v2, v0, nearest);
+		btScalar len = (contact - nearest).length();
+		//
+		if (len < disttobestedge)
+		{
+			bestedge = 2;
+			disttobestedge = len;
+		}
+	}
+
 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
-   btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f);
-   btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red );
-#endif   
-	if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
+	btVector3 upfix = tri_normal * btVector3(0.1f, 0.1f, 0.1f);
+	btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
+#endif
+	if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
 	{
 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
-		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
+		btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
 #endif
-		btScalar len = (contact-nearest).length();
-		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
-		if( bestedge==0 )
-		{
-			btVector3 edge(v0-v1);
-			isNearEdge = true;
-
-			if (info->m_edgeV0V1Angle==btScalar(0))
-			{
-				numConcaveEdgeHits++;
-			} else
+		btScalar len = (contact - nearest).length();
+		if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
+			if (bestedge == 0)
 			{
+				btVector3 edge(v0 - v1);
+				isNearEdge = true;
 
-				bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);
-				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
-	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
-				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
-	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+				if (info->m_edgeV0V1Angle == btScalar(0))
+				{
+					numConcaveEdgeHits++;
+				}
+				else
+				{
+					bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);
+					btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+					btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
+#endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 
-				btVector3 nA = swapFactor * tri_normal;
+					btVector3 nA = swapFactor * tri_normal;
 
-				btQuaternion orn(edge,info->m_edgeV0V1Angle);
-				btVector3 computedNormalB = quatRotate(orn,tri_normal);
-				if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)
-					computedNormalB*=-1;
-				btVector3 nB = swapFactor*computedNormalB;
+					btQuaternion orn(edge, info->m_edgeV0V1Angle);
+					btVector3 computedNormalB = quatRotate(orn, tri_normal);
+					if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)
+						computedNormalB *= -1;
+					btVector3 nB = swapFactor * computedNormalB;
 
-				btScalar	NdotA = localContactNormalOnB.dot(nA);
-				btScalar	NdotB = localContactNormalOnB.dot(nB);
-				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
+					btScalar NdotA = localContactNormalOnB.dot(nA);
+					btScalar NdotB = localContactNormalOnB.dot(nB);
+					bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
 
 #ifdef DEBUG_INTERNAL_EDGE
-				{
-					
-					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
-				}
-#endif //DEBUG_INTERNAL_EDGE
-
+					{
+						btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red);
+					}
+#endif  //DEBUG_INTERNAL_EDGE
 
-				if (backFacingNormal)
-				{
-					numConcaveEdgeHits++;
-				}
-				else
-				{
-					numConvexEdgeHits++;
-					btVector3 clampedLocalNormal;
-					bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal);
-					if (isClamped)
+					if (backFacingNormal)
 					{
-						if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
+						numConcaveEdgeHits++;
+					}
+					else
+					{
+						numConvexEdgeHits++;
+						btVector3 clampedLocalNormal;
+						bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV0V1Angle, clampedLocalNormal);
+						if (isClamped)
 						{
-							btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
-							//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
-							cp.m_normalWorldOnB = newNormal;
-							// Reproject collision point along normal. (what about cp.m_distance1?)
-							cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
-							cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
-							
+							if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0))
+							{
+								btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
+								//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
+								cp.m_normalWorldOnB = newNormal;
+								// Reproject collision point along normal. (what about cp.m_distance1?)
+								cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
+								cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
+							}
 						}
 					}
 				}
 			}
-		}
 	}
 
-	btNearestPointInLineSegment(contact,v1,v2,nearest);
+	btNearestPointInLineSegment(contact, v1, v2, nearest);
 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
-	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green);
-#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+	btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, green);
+#endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 
 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
-   btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green );
-#endif   
+	btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green);
+#endif
 
-	if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
+	if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
 	{
 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
-		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
-#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
-
-
-
-		btScalar len = (contact-nearest).length();
-		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
-		if( bestedge==1 )
-		{
-			isNearEdge = true;
-#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
-			btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
-#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
-
-			btVector3 edge(v1-v2);
-
-			isNearEdge = true;
+		btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
+#endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 
-			if (info->m_edgeV1V2Angle == btScalar(0))
+		btScalar len = (contact - nearest).length();
+		if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
+			if (bestedge == 1)
 			{
-				numConcaveEdgeHits++;
-			} else
-			{
-				bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0;
-				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
-	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
-				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
-	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
-
-				btVector3 nA = swapFactor * tri_normal;
-				
-				btQuaternion orn(edge,info->m_edgeV1V2Angle);
-				btVector3 computedNormalB = quatRotate(orn,tri_normal);
-				if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB)
-					computedNormalB*=-1;
-				btVector3 nB = swapFactor*computedNormalB;
-
-#ifdef DEBUG_INTERNAL_EDGE
-				{
-					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
-				}
-#endif //DEBUG_INTERNAL_EDGE
+				isNearEdge = true;
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+				btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
+#endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 
+				btVector3 edge(v1 - v2);
 
-				btScalar	NdotA = localContactNormalOnB.dot(nA);
-				btScalar	NdotB = localContactNormalOnB.dot(nB);
-				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
+				isNearEdge = true;
 
-				if (backFacingNormal)
+				if (info->m_edgeV1V2Angle == btScalar(0))
 				{
 					numConcaveEdgeHits++;
 				}
 				else
 				{
-					numConvexEdgeHits++;
-					btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
-					btVector3 clampedLocalNormal;
-					bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal);
-					if (isClamped)
+					bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX) != 0;
+					btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+					btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
+#endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+					btVector3 nA = swapFactor * tri_normal;
+
+					btQuaternion orn(edge, info->m_edgeV1V2Angle);
+					btVector3 computedNormalB = quatRotate(orn, tri_normal);
+					if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB)
+						computedNormalB *= -1;
+					btVector3 nB = swapFactor * computedNormalB;
+
+#ifdef DEBUG_INTERNAL_EDGE
+					{
+						btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red);
+					}
+#endif  //DEBUG_INTERNAL_EDGE
+
+					btScalar NdotA = localContactNormalOnB.dot(nA);
+					btScalar NdotB = localContactNormalOnB.dot(nB);
+					bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
+
+					if (backFacingNormal)
+					{
+						numConcaveEdgeHits++;
+					}
+					else
 					{
-						if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
+						numConvexEdgeHits++;
+						btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
+						btVector3 clampedLocalNormal;
+						bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV1V2Angle, clampedLocalNormal);
+						if (isClamped)
 						{
-							btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
-							//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
-							cp.m_normalWorldOnB = newNormal;
-							// Reproject collision point along normal.
-							cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
-							cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
+							if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0))
+							{
+								btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
+								//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
+								cp.m_normalWorldOnB = newNormal;
+								// Reproject collision point along normal.
+								cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
+								cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
+							}
 						}
 					}
 				}
 			}
-		}
 	}
 
-	btNearestPointInLineSegment(contact,v2,v0,nearest);
+	btNearestPointInLineSegment(contact, v2, v0, nearest);
 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
-	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue);
-#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+	btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue);
+#endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
-   btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue );
-#endif   
+	btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue);
+#endif
 
-	if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
+	if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
 	{
-
-#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
-		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
-#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
-
-		btScalar len = (contact-nearest).length();
-		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
-		if( bestedge==2 )
-		{
-			isNearEdge = true;
 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
-			btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
-#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
-
-			btVector3 edge(v2-v0);
+		btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
+#endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 
-			if (info->m_edgeV2V0Angle==btScalar(0))
-			{
-				numConcaveEdgeHits++;
-			} else
+		btScalar len = (contact - nearest).length();
+		if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
+			if (bestedge == 2)
 			{
+				isNearEdge = true;
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+				btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
+#endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 
-				bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0;
-				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
-	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
-				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
-	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
-
-				btVector3 nA = swapFactor * tri_normal;
-				btQuaternion orn(edge,info->m_edgeV2V0Angle);
-				btVector3 computedNormalB = quatRotate(orn,tri_normal);
-				if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB)
-					computedNormalB*=-1;
-				btVector3 nB = swapFactor*computedNormalB;
-
-#ifdef DEBUG_INTERNAL_EDGE
-				{
-					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
-				}
-#endif //DEBUG_INTERNAL_EDGE
-
-				btScalar	NdotA = localContactNormalOnB.dot(nA);
-				btScalar	NdotB = localContactNormalOnB.dot(nB);
-				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
+				btVector3 edge(v2 - v0);
 
-				if (backFacingNormal)
+				if (info->m_edgeV2V0Angle == btScalar(0))
 				{
 					numConcaveEdgeHits++;
 				}
 				else
 				{
-					numConvexEdgeHits++;
-					//				printf("hitting convex edge\n");
+					bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX) != 0;
+					btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+					btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
+#endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+					btVector3 nA = swapFactor * tri_normal;
+					btQuaternion orn(edge, info->m_edgeV2V0Angle);
+					btVector3 computedNormalB = quatRotate(orn, tri_normal);
+					if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB)
+						computedNormalB *= -1;
+					btVector3 nB = swapFactor * computedNormalB;
+
+#ifdef DEBUG_INTERNAL_EDGE
+					{
+						btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red);
+					}
+#endif  //DEBUG_INTERNAL_EDGE
 
+					btScalar NdotA = localContactNormalOnB.dot(nA);
+					btScalar NdotB = localContactNormalOnB.dot(nB);
+					bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
 
-					btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
-					btVector3 clampedLocalNormal;
-					bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal);
-					if (isClamped)
+					if (backFacingNormal)
+					{
+						numConcaveEdgeHits++;
+					}
+					else
 					{
-						if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
+						numConvexEdgeHits++;
+						//				printf("hitting convex edge\n");
+
+						btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
+						btVector3 clampedLocalNormal;
+						bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV2V0Angle, clampedLocalNormal);
+						if (isClamped)
 						{
-							btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
-							//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
-							cp.m_normalWorldOnB = newNormal;
-							// Reproject collision point along normal.
-							cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
-							cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
+							if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0))
+							{
+								btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
+								//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
+								cp.m_normalWorldOnB = newNormal;
+								// Reproject collision point along normal.
+								cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
+								cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
+							}
 						}
 					}
-				} 
+				}
 			}
-			
-
-		}
 	}
 
 #ifdef DEBUG_INTERNAL_EDGE
 	{
-		btVector3 color(0,1,1);
-		btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color);
+		btVector3 color(0, 1, 1);
+		btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + cp.m_normalWorldOnB * 10, color);
 	}
-#endif //DEBUG_INTERNAL_EDGE
+#endif  //DEBUG_INTERNAL_EDGE
 
 	if (isNearEdge)
 	{
-
-		if (numConcaveEdgeHits>0)
+		if (numConcaveEdgeHits > 0)
 		{
-			if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0)
+			if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED) != 0)
 			{
 				//fix tri_normal so it pointing the same direction as the current local contact normal
 				if (tri_normal.dot(localContactNormalOnB) < 0)
 				{
 					tri_normal *= -1;
 				}
-				cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis()*tri_normal;
-			} else
+				cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * tri_normal;
+			}
+			else
 			{
-				btVector3 newNormal = tri_normal *frontFacing;
+				btVector3 newNormal = tri_normal * frontFacing;
 				//if the tri_normal is pointing opposite direction as the current local contact normal, skip it
-				btScalar d = newNormal.dot(localContactNormalOnB) ;
-				if (d< 0)
+				btScalar d = newNormal.dot(localContactNormalOnB);
+				if (d < 0)
 				{
 					return;
 				}
 				//modify the normal to be the triangle normal (or backfacing normal)
-				cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() *newNormal;
+				cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * newNormal;
 			}
-						
+
 			// Reproject collision point along normal.
 			cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
 			cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h
index 7d9aafeee69..cc6d11c2418 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h
@@ -15,33 +15,30 @@ class btCollisionObject;
 struct btCollisionObjectWrapper;
 class btManifoldPoint;
 class btIDebugDraw;
-
-
+class btHeightfieldTerrainShape;
 
 enum btInternalEdgeAdjustFlags
 {
 	BT_TRIANGLE_CONVEX_BACKFACE_MODE = 1,
-	BT_TRIANGLE_CONCAVE_DOUBLE_SIDED = 2, //double sided options are experimental, single sided is recommended
+	BT_TRIANGLE_CONCAVE_DOUBLE_SIDED = 2,  //double sided options are experimental, single sided is recommended
 	BT_TRIANGLE_CONVEX_DOUBLE_SIDED = 4
 };
 
-
 ///Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo'
-void	btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap);
+void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap);
 
+void btGenerateInternalEdgeInfo(btHeightfieldTerrainShape* trimeshShape, btTriangleInfoMap* triangleInfoMap);
 
 ///Call the btFixMeshNormal to adjust the collision normal, using the triangle info map (generated using btGenerateInternalEdgeInfo)
 ///If this info map is missing, or the triangle is not store in this map, nothing will be done
-void	btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* trimeshColObj0Wrap,const btCollisionObjectWrapper* otherColObj1Wrap, int partId0, int index0, int normalAdjustFlags = 0);
+void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* trimeshColObj0Wrap, const btCollisionObjectWrapper* otherColObj1Wrap, int partId0, int index0, int normalAdjustFlags = 0);
 
 ///Enable the BT_INTERNAL_EDGE_DEBUG_DRAW define and call btSetDebugDrawer, to get visual info to see if the internal edge utility works properly.
 ///If the utility doesn't work properly, you might have to adjust the threshold values in btTriangleInfoMap
 //#define BT_INTERNAL_EDGE_DEBUG_DRAW
 
 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
-void	btSetDebugDrawer(btIDebugDraw* debugDrawer);
-#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
-
-
-#endif //BT_INTERNAL_EDGE_UTILITY_H
+void btSetDebugDrawer(btIDebugDraw* debugDrawer);
+#endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 
+#endif  //BT_INTERNAL_EDGE_UTILITY_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
index 4b2986a0087..770eb243691 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
@@ -13,142 +13,191 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btManifoldResult.h"
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 
 ///This is to allow MaterialCombiner/Custom Friction/Restitution values
-ContactAddedCallback		gContactAddedCallback=0;
-
+ContactAddedCallback gContactAddedCallback = 0;
 
+CalculateCombinedCallback gCalculateCombinedRestitutionCallback = &btManifoldResult::calculateCombinedRestitution;
+CalculateCombinedCallback gCalculateCombinedFrictionCallback = &btManifoldResult::calculateCombinedFriction;
+CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback = &btManifoldResult::calculateCombinedRollingFriction;
+CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback = &btManifoldResult::calculateCombinedSpinningFriction;
+CalculateCombinedCallback gCalculateCombinedContactDampingCallback = &btManifoldResult::calculateCombinedContactDamping;
+CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback = &btManifoldResult::calculateCombinedContactStiffness;
 
-///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
-inline btScalar	calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0, const btCollisionObject* body1)
 {
-	btScalar friction = body0->getRollingFriction() * body1->getRollingFriction();
+	btScalar friction = body0->getRollingFriction() * body1->getFriction() + body1->getRollingFriction() * body0->getFriction();
 
-	const btScalar MAX_FRICTION  = btScalar(10.);
+	const btScalar MAX_FRICTION = btScalar(10.);
 	if (friction < -MAX_FRICTION)
 		friction = -MAX_FRICTION;
 	if (friction > MAX_FRICTION)
 		friction = MAX_FRICTION;
 	return friction;
-
 }
 
+btScalar btManifoldResult::calculateCombinedSpinningFriction(const btCollisionObject* body0, const btCollisionObject* body1)
+{
+	btScalar friction = body0->getSpinningFriction() * body1->getFriction() + body1->getSpinningFriction() * body0->getFriction();
+
+	const btScalar MAX_FRICTION = btScalar(10.);
+	if (friction < -MAX_FRICTION)
+		friction = -MAX_FRICTION;
+	if (friction > MAX_FRICTION)
+		friction = MAX_FRICTION;
+	return friction;
+}
 
 ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
-btScalar	btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+btScalar btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0, const btCollisionObject* body1)
 {
 	btScalar friction = body0->getFriction() * body1->getFriction();
 
-	const btScalar MAX_FRICTION  = btScalar(10.);
+	const btScalar MAX_FRICTION = btScalar(10.);
 	if (friction < -MAX_FRICTION)
 		friction = -MAX_FRICTION;
 	if (friction > MAX_FRICTION)
 		friction = MAX_FRICTION;
 	return friction;
-
 }
 
-btScalar	btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1)
+btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0, const btCollisionObject* body1)
 {
 	return body0->getRestitution() * body1->getRestitution();
 }
 
+btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0, const btCollisionObject* body1)
+{
+	return body0->getContactDamping() + body1->getContactDamping();
+}
+
+btScalar btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0, const btCollisionObject* body1)
+{
+	btScalar s0 = body0->getContactStiffness();
+	btScalar s1 = body1->getContactStiffness();
 
+	btScalar tmp0 = btScalar(1) / s0;
+	btScalar tmp1 = btScalar(1) / s1;
+	btScalar combinedStiffness = btScalar(1) / (tmp0 + tmp1);
+	return combinedStiffness;
+}
 
-btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
-		:m_manifoldPtr(0),
-		m_body0Wrap(body0Wrap),
-		m_body1Wrap(body1Wrap)
+btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
+	: m_manifoldPtr(0),
+	  m_body0Wrap(body0Wrap),
+	  m_body1Wrap(body1Wrap)
 #ifdef DEBUG_PART_INDEX
-		,m_partId0(-1),
-	m_partId1(-1),
-	m_index0(-1),
-	m_index1(-1)
-#endif //DEBUG_PART_INDEX
+	  ,
+	  m_partId0(-1),
+	  m_partId1(-1),
+	  m_index0(-1),
+	  m_index1(-1)
+#endif  //DEBUG_PART_INDEX
+	  ,
+	  m_closestPointDistanceThreshold(0)
 {
 }
 
-
-void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
 {
 	btAssert(m_manifoldPtr);
 	//order in manifold needs to match
 
 	if (depth > m_manifoldPtr->getContactBreakingThreshold())
-//	if (depth > m_manifoldPtr->getContactProcessingThreshold())
+		//	if (depth > m_manifoldPtr->getContactProcessingThreshold())
 		return;
 
 	bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
+	bool isNewCollision = m_manifoldPtr->getNumContacts() == 0;
 
 	btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
 
 	btVector3 localA;
 	btVector3 localB;
-	
+
 	if (isSwapped)
 	{
-		localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
+		localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA);
 		localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
-	} else
+	}
+	else
 	{
-		localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
+		localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA);
 		localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
 	}
 
-	btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
+	btManifoldPoint newPt(localA, localB, normalOnBInWorld, depth);
 	newPt.m_positionWorldOnA = pointA;
 	newPt.m_positionWorldOnB = pointInWorld;
-	
+
 	int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
 
-	newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
-	newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
-	newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
-	btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);
-	
+	newPt.m_combinedFriction = gCalculateCombinedFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
+	newPt.m_combinedRestitution = gCalculateCombinedRestitutionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
+	newPt.m_combinedRollingFriction = gCalculateCombinedRollingFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
+	newPt.m_combinedSpinningFriction = gCalculateCombinedSpinningFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
 
-	
-   //BP mod, store contact triangles.
+	if ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) ||
+		(m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING))
+	{
+		newPt.m_combinedContactDamping1 = gCalculateCombinedContactDampingCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
+		newPt.m_combinedContactStiffness1 = gCalculateCombinedContactStiffnessCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
+		newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING;
+	}
+
+	if ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR) ||
+		(m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR))
+	{
+		newPt.m_contactPointFlags |= BT_CONTACT_FLAG_FRICTION_ANCHOR;
+	}
+
+	btPlaneSpace1(newPt.m_normalWorldOnB, newPt.m_lateralFrictionDir1, newPt.m_lateralFrictionDir2);
+
+	//BP mod, store contact triangles.
 	if (isSwapped)
 	{
 		newPt.m_partId0 = m_partId1;
 		newPt.m_partId1 = m_partId0;
-		newPt.m_index0  = m_index1;
-		newPt.m_index1  = m_index0;
-	} else
+		newPt.m_index0 = m_index1;
+		newPt.m_index1 = m_index0;
+	}
+	else
 	{
 		newPt.m_partId0 = m_partId0;
 		newPt.m_partId1 = m_partId1;
-		newPt.m_index0  = m_index0;
-		newPt.m_index1  = m_index1;
+		newPt.m_index0 = m_index0;
+		newPt.m_index1 = m_index1;
 	}
 	//printf("depth=%f\n",depth);
 	///@todo, check this for any side effects
 	if (insertIndex >= 0)
 	{
 		//const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
-		m_manifoldPtr->replaceContactPoint(newPt,insertIndex);
-	} else
+		m_manifoldPtr->replaceContactPoint(newPt, insertIndex);
+	}
+	else
 	{
 		insertIndex = m_manifoldPtr->addManifoldPoint(newPt);
 	}
-	
+
 	//User can override friction and/or restitution
 	if (gContactAddedCallback &&
 		//and if either of the two bodies requires custom material
-		 ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
-		   (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
+		((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
+		 (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
 	{
 		//experimental feature info, for per-triangle material etc.
-		const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap;
-		const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap;
-		(*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1);
+		const btCollisionObjectWrapper* obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap;
+		const btCollisionObjectWrapper* obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap;
+		(*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex), obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1);
 	}
 
+	if (gContactStartedCallback && isNewCollision)
+	{
+		gContactStartedCallback(m_manifoldPtr);
+	}
 }
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h
index 977b9a02fc5..6c0a2d9a43f 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_MANIFOLD_RESULT_H
 #define BT_MANIFOLD_RESULT_H
 
@@ -29,74 +28,81 @@ class btManifoldPoint;
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
-typedef bool (*ContactAddedCallback)(btManifoldPoint& cp,	const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1);
-extern ContactAddedCallback		gContactAddedCallback;
+typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1);
+extern ContactAddedCallback gContactAddedCallback;
 
 //#define DEBUG_PART_INDEX 1
 
+/// These callbacks are used to customize the algorith that combine restitution, friction, damping, Stiffness
+typedef btScalar (*CalculateCombinedCallback)(const btCollisionObject* body0, const btCollisionObject* body1);
+
+extern CalculateCombinedCallback gCalculateCombinedRestitutionCallback;
+extern CalculateCombinedCallback gCalculateCombinedFrictionCallback;
+extern CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback;
+extern CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback;
+extern CalculateCombinedCallback gCalculateCombinedContactDampingCallback;
+extern CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback;
 
 ///btManifoldResult is a helper class to manage  contact results.
 class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result
 {
 protected:
-
 	btPersistentManifold* m_manifoldPtr;
 
 	const btCollisionObjectWrapper* m_body0Wrap;
 	const btCollisionObjectWrapper* m_body1Wrap;
-	int	m_partId0;
+	int m_partId0;
 	int m_partId1;
 	int m_index0;
 	int m_index1;
-	
 
 public:
-
 	btManifoldResult()
-#ifdef DEBUG_PART_INDEX
 		:
-	m_partId0(-1),
-	m_partId1(-1),
-	m_index0(-1),
-	m_index1(-1)
-#endif //DEBUG_PART_INDEX
+#ifdef DEBUG_PART_INDEX
+
+		  m_partId0(-1),
+		  m_partId1(-1),
+		  m_index0(-1),
+		  m_index1(-1)
+#endif  //DEBUG_PART_INDEX
+			  m_closestPointDistanceThreshold(0)
 	{
 	}
 
-	btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+	btManifoldResult(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap);
 
-	virtual ~btManifoldResult() {};
+	virtual ~btManifoldResult(){};
 
-	void	setPersistentManifold(btPersistentManifold* manifoldPtr)
+	void setPersistentManifold(btPersistentManifold* manifoldPtr)
 	{
 		m_manifoldPtr = manifoldPtr;
 	}
 
-	const btPersistentManifold*	getPersistentManifold() const
+	const btPersistentManifold* getPersistentManifold() const
 	{
 		return m_manifoldPtr;
 	}
-	btPersistentManifold*	getPersistentManifold()
+	btPersistentManifold* getPersistentManifold()
 	{
 		return m_manifoldPtr;
 	}
 
-	virtual void setShapeIdentifiersA(int partId0,int index0)
+	virtual void setShapeIdentifiersA(int partId0, int index0)
 	{
-		m_partId0=partId0;
-		m_index0=index0;
+		m_partId0 = partId0;
+		m_index0 = index0;
 	}
 
-	virtual void setShapeIdentifiersB(	int partId1,int index1)
+	virtual void setShapeIdentifiersB(int partId1, int index1)
 	{
-		m_partId1=partId1;
-		m_index1=index1;
+		m_partId1 = partId1;
+		m_index1 = index1;
 	}
 
+	virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth);
 
-	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth);
-
-	SIMD_FORCE_INLINE	void refreshContactPoints()
+	SIMD_FORCE_INLINE void refreshContactPoints()
 	{
 		btAssert(m_manifoldPtr);
 		if (!m_manifoldPtr->getNumContacts())
@@ -106,10 +112,11 @@ public:
 
 		if (isSwapped)
 		{
-			m_manifoldPtr->refreshContactPoints(m_body1Wrap->getCollisionObject()->getWorldTransform(),m_body0Wrap->getCollisionObject()->getWorldTransform());
-		} else
+			m_manifoldPtr->refreshContactPoints(m_body1Wrap->getCollisionObject()->getWorldTransform(), m_body0Wrap->getCollisionObject()->getWorldTransform());
+		}
+		else
 		{
-			m_manifoldPtr->refreshContactPoints(m_body0Wrap->getCollisionObject()->getWorldTransform(),m_body1Wrap->getCollisionObject()->getWorldTransform());
+			m_manifoldPtr->refreshContactPoints(m_body0Wrap->getCollisionObject()->getWorldTransform(), m_body1Wrap->getCollisionObject()->getWorldTransform());
 		}
 	}
 
@@ -142,9 +149,15 @@ public:
 		return m_body1Wrap->getCollisionObject();
 	}
 
+	btScalar m_closestPointDistanceThreshold;
+
 	/// in the future we can let the user override the methods to combine restitution and friction
-	static btScalar	calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1);
-	static btScalar	calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1);
+	static btScalar calculateCombinedRestitution(const btCollisionObject* body0, const btCollisionObject* body1);
+	static btScalar calculateCombinedFriction(const btCollisionObject* body0, const btCollisionObject* body1);
+	static btScalar calculateCombinedRollingFriction(const btCollisionObject* body0, const btCollisionObject* body1);
+	static btScalar calculateCombinedSpinningFriction(const btCollisionObject* body0, const btCollisionObject* body1);
+	static btScalar calculateCombinedContactDamping(const btCollisionObject* body0, const btCollisionObject* body1);
+	static btScalar calculateCombinedContactStiffness(const btCollisionObject* body0, const btCollisionObject* body1);
 };
 
-#endif //BT_MANIFOLD_RESULT_H
+#endif  //BT_MANIFOLD_RESULT_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
index 13447822571..327b3f076a6 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
@@ -14,7 +14,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "LinearMath/btScalar.h"
 #include "btSimulationIslandManager.h"
 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
@@ -25,8 +24,7 @@ subject to the following restrictions:
 //#include <stdio.h>
 #include "LinearMath/btQuickprof.h"
 
-btSimulationIslandManager::btSimulationIslandManager():
-m_splitIslands(true)
+btSimulationIslandManager::btSimulationIslandManager() : m_splitIslands(true)
 {
 }
 
@@ -34,53 +32,47 @@ btSimulationIslandManager::~btSimulationIslandManager()
 {
 }
 
-
 void btSimulationIslandManager::initUnionFind(int n)
 {
-		m_unionFind.reset(n);
+	m_unionFind.reset(n);
 }
-		
 
-void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld)
+void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */, btCollisionWorld* colWorld)
 {
-	
 	{
 		btOverlappingPairCache* pairCachePtr = colWorld->getPairCache();
 		const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs();
 		if (numOverlappingPairs)
 		{
-		btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
-		
-		for (int i=0;i<numOverlappingPairs;i++)
-		{
-			const btBroadphasePair& collisionPair = pairPtr[i];
-			btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
-			btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+			btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
 
-			if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
-				((colObj1) && ((colObj1)->mergesSimulationIslands())))
+			for (int i = 0; i < numOverlappingPairs; i++)
 			{
+				const btBroadphasePair& collisionPair = pairPtr[i];
+				btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
+				btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
 
-				m_unionFind.unite((colObj0)->getIslandTag(),
-					(colObj1)->getIslandTag());
+				if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
+					((colObj1) && ((colObj1)->mergesSimulationIslands())))
+				{
+					m_unionFind.unite((colObj0)->getIslandTag(),
+									  (colObj1)->getIslandTag());
+				}
 			}
 		}
-		}
 	}
 }
 
 #ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
-void   btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
+void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher)
 {
-
-	// put the index into m_controllers into m_tag   
+	// put the index into m_controllers into m_tag
 	int index = 0;
 	{
-
 		int i;
-		for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
+		for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
 		{
-			btCollisionObject*   collisionObject= colWorld->getCollisionObjectArray()[i];
+			btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
 			//Adding filtering here
 			if (!collisionObject->isStaticOrKinematicObject())
 			{
@@ -92,28 +84,29 @@ void   btSimulationIslandManager::updateActivationState(btCollisionWorld* colWor
 	}
 	// do the union find
 
-	initUnionFind( index );
+	initUnionFind(index);
 
-	findUnions(dispatcher,colWorld);
+	findUnions(dispatcher, colWorld);
 }
 
-void   btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
+void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
 {
-	// put the islandId ('find' value) into m_tag   
+	// put the islandId ('find' value) into m_tag
 	{
 		int index = 0;
 		int i;
-		for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
+		for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
 		{
-			btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+			btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
 			if (!collisionObject->isStaticOrKinematicObject())
 			{
-				collisionObject->setIslandTag( m_unionFind.find(index) );
+				collisionObject->setIslandTag(m_unionFind.find(index));
 				//Set the correct object offset in Collision Object Array
 				m_unionFind.getElement(index).m_sz = i;
 				collisionObject->setCompanionId(-1);
 				index++;
-			} else
+			}
+			else
 			{
 				collisionObject->setIslandTag(-1);
 				collisionObject->setCompanionId(-2);
@@ -122,49 +115,44 @@ void   btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* c
 	}
 }
 
-
-#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
-void	btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
+#else  //STATIC_SIMULATION_ISLAND_OPTIMIZATION
+void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher)
 {
+	initUnionFind(int(colWorld->getCollisionObjectArray().size()));
 
-	initUnionFind( int (colWorld->getCollisionObjectArray().size()));
-
-	// put the index into m_controllers into m_tag	
+	// put the index into m_controllers into m_tag
 	{
-
 		int index = 0;
 		int i;
-		for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
+		for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
 		{
-			btCollisionObject*	collisionObject= colWorld->getCollisionObjectArray()[i];
+			btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
 			collisionObject->setIslandTag(index);
 			collisionObject->setCompanionId(-1);
 			collisionObject->setHitFraction(btScalar(1.));
 			index++;
-
 		}
 	}
 	// do the union find
 
-	findUnions(dispatcher,colWorld);
+	findUnions(dispatcher, colWorld);
 }
 
-void	btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
+void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
 {
-	// put the islandId ('find' value) into m_tag	
+	// put the islandId ('find' value) into m_tag
 	{
-
-
 		int index = 0;
 		int i;
-		for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
+		for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
 		{
-			btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+			btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
 			if (!collisionObject->isStaticOrKinematicObject())
 			{
-				collisionObject->setIslandTag( m_unionFind.find(index) );
+				collisionObject->setIslandTag(m_unionFind.find(index));
 				collisionObject->setCompanionId(-1);
-			} else
+			}
+			else
 			{
 				collisionObject->setIslandTag(-1);
 				collisionObject->setCompanionId(-2);
@@ -174,56 +162,59 @@ void	btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col
 	}
 }
 
-#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
+#endif  //STATIC_SIMULATION_ISLAND_OPTIMIZATION
 
-inline	int	getIslandId(const btPersistentManifold* lhs)
+inline int getIslandId(const btPersistentManifold* lhs)
 {
 	int islandId;
 	const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
 	const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
-	islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
+	islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag();
 	return islandId;
-
 }
 
-
-
 /// function object that routes calls to operator<
 class btPersistentManifoldSortPredicate
 {
-	public:
-
-		SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) const
-		{
-			return getIslandId(lhs) < getIslandId(rhs);
-		}
+public:
+	SIMD_FORCE_INLINE bool operator()(const btPersistentManifold* lhs, const btPersistentManifold* rhs) const
+	{
+		return getIslandId(lhs) < getIslandId(rhs);
+	}
 };
 
-
-void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
+class btPersistentManifoldSortPredicateDeterministic
 {
+public:
+	SIMD_FORCE_INLINE bool operator()(const btPersistentManifold* lhs, const btPersistentManifold* rhs) const
+	{
+		return (
+			(getIslandId(lhs) < getIslandId(rhs)) || ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) || ((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) && (lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId)));
+	}
+};
 
+void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld)
+{
 	BT_PROFILE("islandUnionFindAndQuickSort");
-	
+
 	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
 
 	m_islandmanifold.resize(0);
 
 	//we are going to sort the unionfind array, and store the element id in the size
 	//afterwards, we clean unionfind, to make sure no-one uses it anymore
-	
+
 	getUnionFind().sortIslands();
 	int numElem = getUnionFind().getNumElements();
 
-	int endIslandIndex=1;
+	int endIslandIndex = 1;
 	int startIslandIndex;
 
-
 	//update the sleeping state for bodies, if all are sleeping
-	for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
+	for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
 	{
 		int islandId = getUnionFind().getElement(startIslandIndex).m_id;
-		for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
+		for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
 		{
 		}
 
@@ -232,71 +223,69 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
 		bool allSleeping = true;
 
 		int idx;
-		for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+		for (idx = startIslandIndex; idx < endIslandIndex; idx++)
 		{
 			int i = getUnionFind().getElement(idx).m_sz;
 
 			btCollisionObject* colObj0 = collisionObjects[i];
 			if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
 			{
-//				printf("error in island management\n");
+				//				printf("error in island management\n");
 			}
 
-			btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+            btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 			if (colObj0->getIslandTag() == islandId)
 			{
-				if (colObj0->getActivationState()== ACTIVE_TAG)
-				{
-					allSleeping = false;
-				}
-				if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
+				if (colObj0->getActivationState() == ACTIVE_TAG ||
+					colObj0->getActivationState() == DISABLE_DEACTIVATION)
 				{
 					allSleeping = false;
+					break;
 				}
 			}
 		}
-			
 
 		if (allSleeping)
 		{
 			int idx;
-			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+			for (idx = startIslandIndex; idx < endIslandIndex; idx++)
 			{
 				int i = getUnionFind().getElement(idx).m_sz;
 				btCollisionObject* colObj0 = collisionObjects[i];
 				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
 				{
-//					printf("error in island management\n");
+					//					printf("error in island management\n");
 				}
 
-				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+                btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 
 				if (colObj0->getIslandTag() == islandId)
 				{
-					colObj0->setActivationState( ISLAND_SLEEPING );
+					colObj0->setActivationState(ISLAND_SLEEPING);
 				}
 			}
-		} else
+		}
+		else
 		{
-
 			int idx;
-			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+			for (idx = startIslandIndex; idx < endIslandIndex; idx++)
 			{
 				int i = getUnionFind().getElement(idx).m_sz;
 
 				btCollisionObject* colObj0 = collisionObjects[i];
 				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
 				{
-//					printf("error in island management\n");
+					//					printf("error in island management\n");
 				}
 
-				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
 
 				if (colObj0->getIslandTag() == islandId)
 				{
-					if ( colObj0->getActivationState() == ISLAND_SLEEPING)
+					if (colObj0->getActivationState() == ISLAND_SLEEPING)
 					{
-						colObj0->setActivationState( WANTS_DEACTIVATION);
+						colObj0->setActivationState(WANTS_DEACTIVATION);
 						colObj0->setDeactivationTime(0.f);
 					}
 				}
@@ -304,29 +293,30 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
 		}
 	}
 
-	
 	int i;
 	int maxNumManifolds = dispatcher->getNumManifolds();
 
-//#define SPLIT_ISLANDS 1
-//#ifdef SPLIT_ISLANDS
+	//#define SPLIT_ISLANDS 1
+	//#ifdef SPLIT_ISLANDS
 
-	
-//#endif //SPLIT_ISLANDS
+	//#endif //SPLIT_ISLANDS
 
-	
-	for (i=0;i<maxNumManifolds ;i++)
+	for (i = 0; i < maxNumManifolds; i++)
 	{
-		 btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
-		 
-		 const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
-		 const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
-		
-		 ///@todo: check sleeping conditions!
-		 if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
+		btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
+		if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
+		{
+			if (manifold->getNumContacts() == 0)
+				continue;
+		}
+
+		const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
+		const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
+
+		///@todo: check sleeping conditions!
+		if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
 			((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
 		{
-		
 			//kinematic objects don't merge islands, but wake up all connected objects
 			if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
 			{
@@ -338,10 +328,10 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
 				if (colObj1->hasContactResponse())
 					colObj0->activate();
 			}
-			if(m_splitIslands)
-			{ 
+			if (m_splitIslands)
+			{
 				//filtering for response
-				if (dispatcher->needsResponse(colObj0,colObj1))
+				if (dispatcher->needsResponse(colObj0, colObj1))
 					m_islandmanifold.push_back(manifold);
 			}
 		}
@@ -349,25 +339,27 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
 }
 
 
-
 ///@todo: this is random access, it can be walked 'cache friendly'!
-void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback)
+void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback)
 {
-	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
-
-	buildIslands(dispatcher,collisionWorld);
+	buildIslands(dispatcher, collisionWorld);
+    processIslands(dispatcher, collisionWorld, callback);
+}
 
-	int endIslandIndex=1;
+void btSimulationIslandManager::processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback)
+{
+    btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+	int endIslandIndex = 1;
 	int startIslandIndex;
 	int numElem = getUnionFind().getNumElements();
 
 	BT_PROFILE("processIslands");
 
-	if(!m_splitIslands)
+	if (!m_splitIslands)
 	{
 		btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
 		int maxNumManifolds = dispatcher->getNumManifolds();
-		callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
+		callback->processIsland(&collisionObjects[0], collisionObjects.size(), manifold, maxNumManifolds, -1);
 	}
 	else
 	{
@@ -375,11 +367,21 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 		// Sort the vector using predicate and std::sort
 		//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
 
-		int numManifolds = int (m_islandmanifold.size());
+		int numManifolds = int(m_islandmanifold.size());
 
 		//tried a radix sort, but quicksort/heapsort seems still faster
 		//@todo rewrite island management
-		m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
+		//btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid,
+		//but also based on object0 unique id and object1 unique id
+		if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
+		{
+			m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic());
+		}
+		else
+		{
+			m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
+		}
+
 		//m_islandmanifold.heapSort(btPersistentManifoldSortPredicate());
 
 		//now process all active islands (sets of manifolds for now)
@@ -389,55 +391,49 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 
 		//int islandId;
 
-		
-
-	//	printf("Start Islands\n");
+		//	printf("Start Islands\n");
 
 		//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
-		for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
+		for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
 		{
 			int islandId = getUnionFind().getElement(startIslandIndex).m_id;
 
+			bool islandSleeping = true;
 
-			   bool islandSleeping = true;
-	                
-					for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
-					{
-							int i = getUnionFind().getElement(endIslandIndex).m_sz;
-							btCollisionObject* colObj0 = collisionObjects[i];
-							m_islandBodies.push_back(colObj0);
-							if (colObj0->isActive())
-									islandSleeping = false;
-					}
-	                
+			for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
+			{
+				int i = getUnionFind().getElement(endIslandIndex).m_sz;
+				btCollisionObject* colObj0 = collisionObjects[i];
+				m_islandBodies.push_back(colObj0);
+				if (colObj0->isActive())
+					islandSleeping = false;
+			}
 
 			//find the accompanying contact manifold for this islandId
 			int numIslandManifolds = 0;
 			btPersistentManifold** startManifold = 0;
 
-			if (startManifoldIndex<numManifolds)
+			if (startManifoldIndex < numManifolds)
 			{
 				int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
 				if (curIslandId == islandId)
 				{
 					startManifold = &m_islandmanifold[startManifoldIndex];
-				
-					for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
-					{
 
+					for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex])); endManifoldIndex++)
+					{
 					}
 					/// Process the actual simulation, only if not sleeping/deactivated
-					numIslandManifolds = endManifoldIndex-startManifoldIndex;
+					numIslandManifolds = endManifoldIndex - startManifoldIndex;
 				}
-
 			}
 
 			if (!islandSleeping)
 			{
-				callback->processIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
-	//			printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
+				callback->processIsland(&m_islandBodies[0], m_islandBodies.size(), startManifold, numIslandManifolds, islandId);
+				//			printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
 			}
-			
+
 			if (numIslandManifolds)
 			{
 				startManifoldIndex = endManifoldIndex;
@@ -445,6 +441,5 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 
 			m_islandBodies.resize(0);
 		}
-	} // else if(!splitIslands) 
-
+	}  // else if(!splitIslands)
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
index e24c6afeca1..197bb457cf6 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
@@ -26,46 +26,42 @@ class btCollisionWorld;
 class btDispatcher;
 class btPersistentManifold;
 
-
 ///SimulationIslandManager creates and handles simulation islands, using btUnionFind
 class btSimulationIslandManager
 {
 	btUnionFind m_unionFind;
 
-	btAlignedObjectArray<btPersistentManifold*>  m_islandmanifold;
-	btAlignedObjectArray<btCollisionObject* >  m_islandBodies;
-	
+	btAlignedObjectArray<btPersistentManifold*> m_islandmanifold;
+	btAlignedObjectArray<btCollisionObject*> m_islandBodies;
+
 	bool m_splitIslands;
-	
+
 public:
 	btSimulationIslandManager();
 	virtual ~btSimulationIslandManager();
 
+	void initUnionFind(int n);
 
-	void initUnionFind(int n);	
-	
-		
-	btUnionFind& getUnionFind() { return m_unionFind;}
-
-	virtual	void	updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher);
-	virtual	void	storeIslandActivationState(btCollisionWorld* world);
+	btUnionFind& getUnionFind() { return m_unionFind; }
 
+	virtual void updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher);
+	virtual void storeIslandActivationState(btCollisionWorld* world);
 
-	void	findUnions(btDispatcher* dispatcher,btCollisionWorld* colWorld);
+	void findUnions(btDispatcher* dispatcher, btCollisionWorld* colWorld);
 
-	
-
-	struct	IslandCallback
+	struct IslandCallback
 	{
-		virtual ~IslandCallback() {};
+		virtual ~IslandCallback(){};
 
-		virtual	void	processIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold**	manifolds,int numManifolds, int islandId) = 0;
+		virtual void processIsland(btCollisionObject** bodies, int numBodies, class btPersistentManifold** manifolds, int numManifolds, int islandId) = 0;
 	};
 
-	void	buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback);
-
-	void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
+	void buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback);
+    
+	void buildIslands(btDispatcher* dispatcher, btCollisionWorld* colWorld);
 
+    void processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback);
+    
 	bool getSplitIslands()
 	{
 		return m_splitIslands;
@@ -74,8 +70,6 @@ public:
 	{
 		m_splitIslands = doSplitIslands;
 	}
-
 };
 
-#endif //BT_SIMULATION_ISLAND_MANAGER_H
-
+#endif  //BT_SIMULATION_ISLAND_MANAGER_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
index e8b567e0efc..bc68b285b8d 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
@@ -21,23 +21,22 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 //#include <stdio.h>
 
-btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped)
-: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap),
-m_ownManifold(false),
-m_manifoldPtr(mf),
-m_isSwapped(isSwapped)
+btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, bool isSwapped)
+	: btActivatingCollisionAlgorithm(ci, col0Wrap, col1Wrap),
+	  m_ownManifold(false),
+	  m_manifoldPtr(mf),
+	  m_isSwapped(isSwapped)
 {
-	const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? col1Wrap : col0Wrap;
-	const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? col0Wrap : col1Wrap;
-	
-	if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject()))
+	const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped ? col1Wrap : col0Wrap;
+	const btCollisionObjectWrapper* boxObjWrap = m_isSwapped ? col0Wrap : col1Wrap;
+
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(), boxObjWrap->getCollisionObject()))
 	{
-		m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject());
+		m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(), boxObjWrap->getCollisionObject());
 		m_ownManifold = true;
 	}
 }
 
-
 btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
 {
 	if (m_ownManifold)
@@ -47,17 +46,15 @@ btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
 	}
 }
 
-
-
-void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
+void btSphereBoxCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	(void)dispatchInfo;
 	(void)resultOut;
 	if (!m_manifoldPtr)
 		return;
 
-	const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? body1Wrap : body0Wrap;
-	const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? body0Wrap : body1Wrap;
+	const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
+	const btCollisionObjectWrapper* boxObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
 
 	btVector3 pOnBox;
 
@@ -83,10 +80,9 @@ void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWra
 			resultOut->refreshContactPoints();
 		}
 	}
-
 }
 
-btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	(void)resultOut;
 	(void)dispatchInfo;
@@ -97,27 +93,26 @@ btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject*
 	return btScalar(1.);
 }
 
-
-bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance ) 
+bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance)
 {
-	const btBoxShape* boxShape= (const btBoxShape*)boxObjWrap->getCollisionShape();
-	btVector3 const &boxHalfExtent = boxShape->getHalfExtentsWithoutMargin();
+	const btBoxShape* boxShape = (const btBoxShape*)boxObjWrap->getCollisionShape();
+	btVector3 const& boxHalfExtent = boxShape->getHalfExtentsWithoutMargin();
 	btScalar boxMargin = boxShape->getMargin();
 	penetrationDepth = 1.0f;
 
 	// convert the sphere position to the box's local space
-	btTransform const &m44T = boxObjWrap->getWorldTransform();
+	btTransform const& m44T = boxObjWrap->getWorldTransform();
 	btVector3 sphereRelPos = m44T.invXform(sphereCenter);
 
 	// Determine the closest point to the sphere center in the box
 	btVector3 closestPoint = sphereRelPos;
-	closestPoint.setX( btMin(boxHalfExtent.getX(), closestPoint.getX()) );
-	closestPoint.setX( btMax(-boxHalfExtent.getX(), closestPoint.getX()) );
-	closestPoint.setY( btMin(boxHalfExtent.getY(), closestPoint.getY()) );
-	closestPoint.setY( btMax(-boxHalfExtent.getY(), closestPoint.getY()) );
-	closestPoint.setZ( btMin(boxHalfExtent.getZ(), closestPoint.getZ()) );
-	closestPoint.setZ( btMax(-boxHalfExtent.getZ(), closestPoint.getZ()) );
-	
+	closestPoint.setX(btMin(boxHalfExtent.getX(), closestPoint.getX()));
+	closestPoint.setX(btMax(-boxHalfExtent.getX(), closestPoint.getX()));
+	closestPoint.setY(btMin(boxHalfExtent.getY(), closestPoint.getY()));
+	closestPoint.setY(btMax(-boxHalfExtent.getY(), closestPoint.getY()));
+	closestPoint.setZ(btMin(boxHalfExtent.getZ(), closestPoint.getZ()));
+	closestPoint.setZ(btMax(-boxHalfExtent.getZ(), closestPoint.getZ()));
+
 	btScalar intersectionDist = fRadius + boxMargin;
 	btScalar contactDist = intersectionDist + maxContactDistance;
 	normal = sphereRelPos - closestPoint;
@@ -136,42 +131,42 @@ bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWra
 	{
 		distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal);
 	}
-	else //compute the penetration details
+	else  //compute the penetration details
 	{
 		distance = normal.length();
 		normal /= distance;
 	}
 
 	pointOnBox = closestPoint + normal * boxMargin;
-//	v3PointOnSphere = sphereRelPos - (normal * fRadius);	
+	//	v3PointOnSphere = sphereRelPos - (normal * fRadius);
 	penetrationDepth = distance - intersectionDist;
 
 	// transform back in world space
 	btVector3 tmp = m44T(pointOnBox);
 	pointOnBox = tmp;
-//	tmp = m44T(v3PointOnSphere);
-//	v3PointOnSphere = tmp;
+	//	tmp = m44T(v3PointOnSphere);
+	//	v3PointOnSphere = tmp;
 	tmp = m44T.getBasis() * normal;
 	normal = tmp;
 
 	return true;
 }
 
-btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal ) 
+btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration(btVector3 const& boxHalfExtent, btVector3 const& sphereRelPos, btVector3& closestPoint, btVector3& normal)
 {
 	//project the center of the sphere on the closest face of the box
 	btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX();
 	btScalar minDist = faceDist;
-	closestPoint.setX( boxHalfExtent.getX() );
-	normal.setValue(btScalar(1.0f),  btScalar(0.0f),  btScalar(0.0f));
+	closestPoint.setX(boxHalfExtent.getX());
+	normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f));
 
 	faceDist = boxHalfExtent.getX() + sphereRelPos.getX();
 	if (faceDist < minDist)
 	{
 		minDist = faceDist;
 		closestPoint = sphereRelPos;
-		closestPoint.setX( -boxHalfExtent.getX() );
-		normal.setValue(btScalar(-1.0f),  btScalar(0.0f),  btScalar(0.0f));
+		closestPoint.setX(-boxHalfExtent.getX());
+		normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f));
 	}
 
 	faceDist = boxHalfExtent.getY() - sphereRelPos.getY();
@@ -179,8 +174,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b
 	{
 		minDist = faceDist;
 		closestPoint = sphereRelPos;
-		closestPoint.setY( boxHalfExtent.getY() );
-		normal.setValue(btScalar(0.0f),  btScalar(1.0f),  btScalar(0.0f));
+		closestPoint.setY(boxHalfExtent.getY());
+		normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f));
 	}
 
 	faceDist = boxHalfExtent.getY() + sphereRelPos.getY();
@@ -188,8 +183,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b
 	{
 		minDist = faceDist;
 		closestPoint = sphereRelPos;
-		closestPoint.setY( -boxHalfExtent.getY() );
-		normal.setValue(btScalar(0.0f),  btScalar(-1.0f),  btScalar(0.0f));
+		closestPoint.setY(-boxHalfExtent.getY());
+		normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f));
 	}
 
 	faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ();
@@ -197,8 +192,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b
 	{
 		minDist = faceDist;
 		closestPoint = sphereRelPos;
-		closestPoint.setZ( boxHalfExtent.getZ() );
-		normal.setValue(btScalar(0.0f),  btScalar(0.0f),  btScalar(1.0f));
+		closestPoint.setZ(boxHalfExtent.getZ());
+		normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f));
 	}
 
 	faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ();
@@ -206,8 +201,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b
 	{
 		minDist = faceDist;
 		closestPoint = sphereRelPos;
-		closestPoint.setZ( -boxHalfExtent.getZ() );
-		normal.setValue(btScalar(0.0f),  btScalar(0.0f),  btScalar(-1.0f));
+		closestPoint.setZ(-boxHalfExtent.getZ());
+		normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f));
 	}
 
 	return minDist;
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h
index eefaedc9e7e..3348bc89af5 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h
@@ -28,21 +28,20 @@ class btPersistentManifold;
 /// Other features are frame-coherency (persistent data) and collision response.
 class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
 {
-	bool	m_ownManifold;
-	btPersistentManifold*	m_manifoldPtr;
-	bool	m_isSwapped;
-	
-public:
+	bool m_ownManifold;
+	btPersistentManifold* m_manifoldPtr;
+	bool m_isSwapped;
 
-	btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, bool isSwapped);
+public:
+	btSphereBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped);
 
 	virtual ~btSphereBoxCollisionAlgorithm();
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
 	{
 		if (m_manifoldPtr && m_ownManifold)
 		{
@@ -50,26 +49,25 @@ public:
 		}
 	}
 
-	bool getSphereDistance( const btCollisionObjectWrapper* boxObjWrap, btVector3& v3PointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& v3SphereCenter, btScalar fRadius, btScalar maxContactDistance );
+	bool getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& v3PointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& v3SphereCenter, btScalar fRadius, btScalar maxContactDistance);
+
+	btScalar getSpherePenetration(btVector3 const& boxHalfExtent, btVector3 const& sphereRelPos, btVector3& closestPoint, btVector3& normal);
 
-	btScalar getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal );
-	
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm));
 			if (!m_swapped)
 			{
-				return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false);
-			} else
+				return new (mem) btSphereBoxCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, false);
+			}
+			else
 			{
-				return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true);
+				return new (mem) btSphereBoxCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, true);
 			}
 		}
 	};
-
 };
 
-#endif //BT_SPHERE_BOX_COLLISION_ALGORITHM_H
-
+#endif  //BT_SPHERE_BOX_COLLISION_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
index 36ba21f5bb0..7fa0559f975 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
@@ -12,6 +12,7 @@ subject to the following restrictions:
 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.
 */
+#define CLEAR_MANIFOLD 1
 
 #include "btSphereSphereCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
@@ -19,14 +20,14 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 
-btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap)
-: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap),
-m_ownManifold(false),
-m_manifoldPtr(mf)
+btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap)
+	: btActivatingCollisionAlgorithm(ci, col0Wrap, col1Wrap),
+	  m_ownManifold(false),
+	  m_manifoldPtr(mf)
 {
 	if (!m_manifoldPtr)
 	{
-		m_manifoldPtr = m_dispatcher->getNewManifold(col0Wrap->getCollisionObject(),col1Wrap->getCollisionObject());
+		m_manifoldPtr = m_dispatcher->getNewManifold(col0Wrap->getCollisionObject(), col1Wrap->getCollisionObject());
 		m_ownManifold = true;
 	}
 }
@@ -40,7 +41,7 @@ btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm()
 	}
 }
 
-void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btSphereSphereCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	(void)dispatchInfo;
 
@@ -52,27 +53,27 @@ void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObject
 	btSphereShape* sphere0 = (btSphereShape*)col0Wrap->getCollisionShape();
 	btSphereShape* sphere1 = (btSphereShape*)col1Wrap->getCollisionShape();
 
-	btVector3 diff = col0Wrap->getWorldTransform().getOrigin()-  col1Wrap->getWorldTransform().getOrigin();
+	btVector3 diff = col0Wrap->getWorldTransform().getOrigin() - col1Wrap->getWorldTransform().getOrigin();
 	btScalar len = diff.length();
 	btScalar radius0 = sphere0->getRadius();
 	btScalar radius1 = sphere1->getRadius();
 
 #ifdef CLEAR_MANIFOLD
-	m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting
+	m_manifoldPtr->clearManifold();  //don't do this, it disables warmstarting
 #endif
 
 	///iff distance positive, don't generate a new contact
-	if ( len > (radius0+radius1))
+	if (len > (radius0 + radius1 + resultOut->m_closestPointDistanceThreshold))
 	{
 #ifndef CLEAR_MANIFOLD
 		resultOut->refreshContactPoints();
-#endif //CLEAR_MANIFOLD
+#endif  //CLEAR_MANIFOLD
 		return;
 	}
 	///distance (negative means penetration)
-	btScalar dist = len - (radius0+radius1);
+	btScalar dist = len - (radius0 + radius1);
 
-	btVector3 normalOnSurfaceB(1,0,0);
+	btVector3 normalOnSurfaceB(1, 0, 0);
 	if (len > SIMD_EPSILON)
 	{
 		normalOnSurfaceB = diff / len;
@@ -81,20 +82,18 @@ void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObject
 	///point on A (worldspace)
 	///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
 	///point on B (worldspace)
-	btVector3 pos1 = col1Wrap->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB;
+	btVector3 pos1 = col1Wrap->getWorldTransform().getOrigin() + radius1 * normalOnSurfaceB;
 
 	/// report a contact. internally this will be kept persistent, and contact reduction is done
-	
-	
-	resultOut->addContactPoint(normalOnSurfaceB,pos1,dist);
+
+	resultOut->addContactPoint(normalOnSurfaceB, pos1, dist);
 
 #ifndef CLEAR_MANIFOLD
 	resultOut->refreshContactPoints();
-#endif //CLEAR_MANIFOLD
-
+#endif  //CLEAR_MANIFOLD
 }
 
-btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	(void)col0;
 	(void)col1;
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
index 3517a568a99..b08d0df76d8 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
@@ -28,39 +28,37 @@ class btPersistentManifold;
 /// Also provides the most basic sample for custom/user btCollisionAlgorithm
 class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm
 {
-	bool	m_ownManifold;
-	btPersistentManifold*	m_manifoldPtr;
-	
+	bool m_ownManifold;
+	btPersistentManifold* m_manifoldPtr;
+
 public:
-	btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap);
+	btSphereSphereCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap);
 
 	btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
 		: btActivatingCollisionAlgorithm(ci) {}
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
 	{
 		if (m_manifoldPtr && m_ownManifold)
 		{
 			manifoldArray.push_back(m_manifoldPtr);
 		}
 	}
-	
+
 	virtual ~btSphereSphereCollisionAlgorithm();
 
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm));
-			return new(mem) btSphereSphereCollisionAlgorithm(0,ci,col0Wrap,col1Wrap);
+			return new (mem) btSphereSphereCollisionAlgorithm(0, ci, col0Wrap, col1Wrap);
 		}
 	};
-
 };
 
-#endif //BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H
-
+#endif  //BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
index 280a4d355fd..1bc3056c013 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btSphereTriangleCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
@@ -21,15 +20,15 @@ subject to the following restrictions:
 #include "SphereTriangleDetector.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 
-btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool swapped)
-: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
-m_ownManifold(false),
-m_manifoldPtr(mf),
-m_swapped(swapped)
+btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool swapped)
+	: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
+	  m_ownManifold(false),
+	  m_manifoldPtr(mf),
+	  m_swapped(swapped)
 {
 	if (!m_manifoldPtr)
 	{
-		m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
 		m_ownManifold = true;
 	}
 }
@@ -43,36 +42,35 @@ btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm()
 	}
 }
 
-void btSphereTriangleCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btSphereTriangleCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	if (!m_manifoldPtr)
 		return;
 
-	const btCollisionObjectWrapper* sphereObjWrap = m_swapped? col1Wrap : col0Wrap;
-	const btCollisionObjectWrapper* triObjWrap = m_swapped? col0Wrap : col1Wrap;
+	const btCollisionObjectWrapper* sphereObjWrap = m_swapped ? col1Wrap : col0Wrap;
+	const btCollisionObjectWrapper* triObjWrap = m_swapped ? col0Wrap : col1Wrap;
 
 	btSphereShape* sphere = (btSphereShape*)sphereObjWrap->getCollisionShape();
 	btTriangleShape* triangle = (btTriangleShape*)triObjWrap->getCollisionShape();
-	
+
 	/// report a contact. internally this will be kept persistent, and contact reduction is done
 	resultOut->setPersistentManifold(m_manifoldPtr);
-	SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold());
-	
+	SphereTriangleDetector detector(sphere, triangle, m_manifoldPtr->getContactBreakingThreshold() + resultOut->m_closestPointDistanceThreshold);
+
 	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
-	input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds
+	input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);  ///@todo: tighter bounds
 	input.m_transformA = sphereObjWrap->getWorldTransform();
 	input.m_transformB = triObjWrap->getWorldTransform();
 
 	bool swapResults = m_swapped;
 
-	detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults);
+	detector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw, swapResults);
 
 	if (m_ownManifold)
 		resultOut->refreshContactPoints();
-	
 }
 
-btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	(void)resultOut;
 	(void)dispatchInfo;
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
index 6b6e39a72b6..d660222f164 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
@@ -27,43 +27,39 @@ class btPersistentManifold;
 /// Also provides the most basic sample for custom/user btCollisionAlgorithm
 class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm
 {
-	bool	m_ownManifold;
-	btPersistentManifold*	m_manifoldPtr;
-	bool	m_swapped;
-	
+	bool m_ownManifold;
+	btPersistentManifold* m_manifoldPtr;
+	bool m_swapped;
+
 public:
-	btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool swapped);
+	btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool swapped);
 
 	btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
 		: btActivatingCollisionAlgorithm(ci) {}
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
 	{
 		if (m_manifoldPtr && m_ownManifold)
 		{
 			manifoldArray.push_back(m_manifoldPtr);
 		}
 	}
-	
+
 	virtual ~btSphereTriangleCollisionAlgorithm();
 
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
-			
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm));
 
-			return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_swapped);
+			return new (mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold, ci, body0Wrap, body1Wrap, m_swapped);
 		}
 	};
-
 };
 
-#endif //BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
-
+#endif  //BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp
index 5222933595d..816bf1e6adc 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp
@@ -15,68 +15,60 @@ subject to the following restrictions:
 
 #include "btUnionFind.h"
 
-
-
 btUnionFind::~btUnionFind()
 {
 	Free();
-
 }
 
 btUnionFind::btUnionFind()
-{ 
-
+{
 }
 
-void	btUnionFind::allocate(int N)
+void btUnionFind::allocate(int N)
 {
 	m_elements.resize(N);
 }
-void	btUnionFind::Free()
+void btUnionFind::Free()
 {
 	m_elements.clear();
 }
 
-
-void	btUnionFind::reset(int N)
+void btUnionFind::reset(int N)
 {
 	allocate(N);
 
-	for (int i = 0; i < N; i++) 
-	{ 
-		m_elements[i].m_id = i; m_elements[i].m_sz = 1; 
-	} 
+	for (int i = 0; i < N; i++)
+	{
+		m_elements[i].m_id = i;
+		m_elements[i].m_sz = 1;
+	}
 }
 
-
 class btUnionFindElementSortPredicate
 {
-	public:
-
-		bool operator() ( const btElement& lhs, const btElement& rhs ) const
-		{
-			return lhs.m_id < rhs.m_id;
-		}
+public:
+	bool operator()(const btElement& lhs, const btElement& rhs) const
+	{
+		return lhs.m_id < rhs.m_id;
+	}
 };
 
 ///this is a special operation, destroying the content of btUnionFind.
 ///it sorts the elements, based on island id, in order to make it easy to iterate over islands
-void	btUnionFind::sortIslands()
+void btUnionFind::sortIslands()
 {
-
 	//first store the original body index, and islandId
 	int numElements = m_elements.size();
-	
-	for (int i=0;i<numElements;i++)
+
+	for (int i = 0; i < numElements; i++)
 	{
 		m_elements[i].m_id = find(i);
 #ifndef STATIC_SIMULATION_ISLAND_OPTIMIZATION
 		m_elements[i].m_sz = i;
-#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
+#endif  //STATIC_SIMULATION_ISLAND_OPTIMIZATION
 	}
-	
-	 // Sort the vector using predicate and std::sort
-	  //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate);
-	  m_elements.quickSort(btUnionFindElementSortPredicate());
 
+	// Sort the vector using predicate and std::sort
+	//std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate);
+	m_elements.quickSort(btUnionFindElementSortPredicate());
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h
index ef2a29202f7..d422ef55eb3 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h
@@ -23,107 +23,101 @@ subject to the following restrictions:
 ///see for discussion of static island optimizations by Vroonsh here: http://code.google.com/p/bullet/issues/detail?id=406
 #define STATIC_SIMULATION_ISLAND_OPTIMIZATION 1
 
-struct	btElement
+struct btElement
 {
-	int	m_id;
-	int	m_sz;
+	int m_id;
+	int m_sz;
 };
 
 ///UnionFind calculates connected subsets
 // Implements weighted Quick Union with path compression
 // optimization: could use short ints instead of ints (halving memory, would limit the number of rigid bodies to 64k, sounds reasonable)
 class btUnionFind
-  {
-    private:
-		btAlignedObjectArray<btElement>	m_elements;
-
-    public:
-	  
-		btUnionFind();
-		~btUnionFind();
-
-	
-		//this is a special operation, destroying the content of btUnionFind.
-		//it sorts the elements, based on island id, in order to make it easy to iterate over islands
-		void	sortIslands();
-
-	  void	reset(int N);
-
-	  SIMD_FORCE_INLINE int	getNumElements() const
-	  {
-		  return int(m_elements.size());
-	  }
-	  SIMD_FORCE_INLINE bool  isRoot(int x) const
-	  {
-		  return (x == m_elements[x].m_id);
-	  }
-
-	  btElement&	getElement(int index)
-	  {
-		  return m_elements[index];
-	  }
-	  const btElement& getElement(int index) const
-	  {
-		  return m_elements[index];
-	  }
-   
-	  void	allocate(int N);
-	  void	Free();
-
-
-
-
-	  int find(int p, int q)
-		{ 
-			return (find(p) == find(q)); 
-		}
-
-		void unite(int p, int q)
-		{
-			int i = find(p), j = find(q);
-			if (i == j) 
-				return;
+{
+private:
+	btAlignedObjectArray<btElement> m_elements;
+
+public:
+	btUnionFind();
+	~btUnionFind();
+
+	//this is a special operation, destroying the content of btUnionFind.
+	//it sorts the elements, based on island id, in order to make it easy to iterate over islands
+	void sortIslands();
+
+	void reset(int N);
+
+	SIMD_FORCE_INLINE int getNumElements() const
+	{
+		return int(m_elements.size());
+	}
+	SIMD_FORCE_INLINE bool isRoot(int x) const
+	{
+		return (x == m_elements[x].m_id);
+	}
+
+	btElement& getElement(int index)
+	{
+		return m_elements[index];
+	}
+	const btElement& getElement(int index) const
+	{
+		return m_elements[index];
+	}
+
+	void allocate(int N);
+	void Free();
+
+	int find(int p, int q)
+	{
+		return (find(p) == find(q));
+	}
+
+	void unite(int p, int q)
+	{
+		int i = find(p), j = find(q);
+		if (i == j)
+			return;
 
 #ifndef USE_PATH_COMPRESSION
-			//weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) )
-			if (m_elements[i].m_sz < m_elements[j].m_sz)
-			{ 
-				m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; 
-			}
-			else 
-			{ 
-				m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz; 
-			}
-#else
-			m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; 
-#endif //USE_PATH_COMPRESSION
+		//weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) )
+		if (m_elements[i].m_sz < m_elements[j].m_sz)
+		{
+			m_elements[i].m_id = j;
+			m_elements[j].m_sz += m_elements[i].m_sz;
+		}
+		else
+		{
+			m_elements[j].m_id = i;
+			m_elements[i].m_sz += m_elements[j].m_sz;
 		}
+#else
+		m_elements[i].m_id = j;
+		m_elements[j].m_sz += m_elements[i].m_sz;
+#endif  //USE_PATH_COMPRESSION
+	}
+
+	int find(int x)
+	{
+		//btAssert(x < m_N);
+		//btAssert(x >= 0);
 
-		int find(int x)
-		{ 
+		while (x != m_elements[x].m_id)
+		{
+			//not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically
+
+#ifdef USE_PATH_COMPRESSION
+			const btElement* elementPtr = &m_elements[m_elements[x].m_id];
+			m_elements[x].m_id = elementPtr->m_id;
+			x = elementPtr->m_id;
+#else  //
+			x = m_elements[x].m_id;
+#endif
 			//btAssert(x < m_N);
 			//btAssert(x >= 0);
-
-			while (x != m_elements[x].m_id) 
-			{
-		//not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically
-	
-		#ifdef USE_PATH_COMPRESSION
-				const btElement* elementPtr = &m_elements[m_elements[x].m_id];
-				m_elements[x].m_id = elementPtr->m_id;
-				x = elementPtr->m_id;			
-		#else//
-				x = m_elements[x].m_id;
-		#endif		
-				//btAssert(x < m_N);
-				//btAssert(x >= 0);
-
-			}
-			return x; 
 		}
+		return x;
+	}
+};
 
-
-  };
-
-
-#endif //BT_UNION_FIND_H
+#endif  //BT_UNION_FIND_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp
index ecce028c2e6..a3d8075dafa 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp
@@ -15,28 +15,23 @@ subject to the following restrictions:
 
 #include "btBox2dShape.h"
 
+//{
 
-//{ 
-
-
-void btBox2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+void btBox2dShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 {
-	btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
+	btTransformAabb(getHalfExtentsWithoutMargin(), getMargin(), t, aabbMin, aabbMax);
 }
 
-
-void	btBox2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btBox2dShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
 	//btScalar margin = btScalar(0.);
 	btVector3 halfExtents = getHalfExtentsWithMargin();
 
-	btScalar lx=btScalar(2.)*(halfExtents.x());
-	btScalar ly=btScalar(2.)*(halfExtents.y());
-	btScalar lz=btScalar(2.)*(halfExtents.z());
-
-	inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
-					mass/(btScalar(12.0)) * (lx*lx + lz*lz),
-					mass/(btScalar(12.0)) * (lx*lx + ly*ly));
+	btScalar lx = btScalar(2.) * (halfExtents.x());
+	btScalar ly = btScalar(2.) * (halfExtents.y());
+	btScalar lz = btScalar(2.) * (halfExtents.z());
 
+	inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz),
+					 mass / (btScalar(12.0)) * (lx * lx + lz * lz),
+					 mass / (btScalar(12.0)) * (lx * lx + ly * ly));
 }
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h
index ce333783e44..7e085f9e2ea 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h
@@ -23,9 +23,9 @@ subject to the following restrictions:
 #include "LinearMath/btMinMax.h"
 
 ///The btBox2dShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space.
-ATTRIBUTE_ALIGNED16(class) btBox2dShape: public btPolyhedralConvexShape
+ATTRIBUTE_ALIGNED16(class)
+btBox2dShape : public btPolyhedralConvexShape
 {
-
 	//btVector3	m_boxHalfExtents1; //use m_implicitShapeDimensions instead
 
 	btVector3 m_centroid;
@@ -33,122 +33,111 @@ ATTRIBUTE_ALIGNED16(class) btBox2dShape: public btPolyhedralConvexShape
 	btVector3 m_normals[4];
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	btVector3 getHalfExtentsWithMargin() const
 	{
 		btVector3 halfExtents = getHalfExtentsWithoutMargin();
-		btVector3 margin(getMargin(),getMargin(),getMargin());
+		btVector3 margin(getMargin(), getMargin(), getMargin());
 		halfExtents += margin;
 		return halfExtents;
 	}
-	
+
 	const btVector3& getHalfExtentsWithoutMargin() const
 	{
-		return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included
+		return m_implicitShapeDimensions;  //changed in Bullet 2.63: assume the scaling and margin are included
 	}
-	
 
-	virtual btVector3	localGetSupportingVertex(const btVector3& vec) const
+	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
 	{
 		btVector3 halfExtents = getHalfExtentsWithoutMargin();
-		btVector3 margin(getMargin(),getMargin(),getMargin());
+		btVector3 margin(getMargin(), getMargin(), getMargin());
 		halfExtents += margin;
-		
+
 		return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
-			btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
-			btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
+						 btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
+						 btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
 	}
 
-	SIMD_FORCE_INLINE  btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+	SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 	{
 		const btVector3& halfExtents = getHalfExtentsWithoutMargin();
-		
+
 		return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
-			btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
-			btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
+						 btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
+						 btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
 	}
 
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 	{
 		const btVector3& halfExtents = getHalfExtentsWithoutMargin();
-	
-		for (int i=0;i<numVectors;i++)
+
+		for (int i = 0; i < numVectors; i++)
 		{
 			const btVector3& vec = vectors[i];
 			supportVerticesOut[i].setValue(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
-				btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
-				btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); 
+										   btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
+										   btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
 		}
-
 	}
 
-
 	///a btBox2dShape is a flat 2D box in the X-Y plane (Z extents are zero)
-	btBox2dShape( const btVector3& boxHalfExtents) 
+	btBox2dShape(const btVector3& boxHalfExtents)
 		: btPolyhedralConvexShape(),
-		m_centroid(0,0,0)
+		  m_centroid(0, 0, 0)
 	{
-		m_vertices[0].setValue(-boxHalfExtents.getX(),-boxHalfExtents.getY(),0);
-		m_vertices[1].setValue(boxHalfExtents.getX(),-boxHalfExtents.getY(),0);
-		m_vertices[2].setValue(boxHalfExtents.getX(),boxHalfExtents.getY(),0);
-		m_vertices[3].setValue(-boxHalfExtents.getX(),boxHalfExtents.getY(),0);
+		m_vertices[0].setValue(-boxHalfExtents.getX(), -boxHalfExtents.getY(), 0);
+		m_vertices[1].setValue(boxHalfExtents.getX(), -boxHalfExtents.getY(), 0);
+		m_vertices[2].setValue(boxHalfExtents.getX(), boxHalfExtents.getY(), 0);
+		m_vertices[3].setValue(-boxHalfExtents.getX(), boxHalfExtents.getY(), 0);
 
-		m_normals[0].setValue(0,-1,0);
-		m_normals[1].setValue(1,0,0);
-		m_normals[2].setValue(0,1,0);
-		m_normals[3].setValue(-1,0,0);
+		m_normals[0].setValue(0, -1, 0);
+		m_normals[1].setValue(1, 0, 0);
+		m_normals[2].setValue(0, 1, 0);
+		m_normals[3].setValue(-1, 0, 0);
 
 		btScalar minDimension = boxHalfExtents.getX();
-		if (minDimension>boxHalfExtents.getY())
+		if (minDimension > boxHalfExtents.getY())
 			minDimension = boxHalfExtents.getY();
-		setSafeMargin(minDimension);
 
 		m_shapeType = BOX_2D_SHAPE_PROXYTYPE;
-		btVector3 margin(getMargin(),getMargin(),getMargin());
+		btVector3 margin(getMargin(), getMargin(), getMargin());
 		m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
+
+		setSafeMargin(minDimension);
 	};
 
 	virtual void setMargin(btScalar collisionMargin)
 	{
 		//correct the m_implicitShapeDimensions for the margin
-		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
-		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
-		
+		btVector3 oldMargin(getMargin(), getMargin(), getMargin());
+		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin;
+
 		btConvexInternalShape::setMargin(collisionMargin);
-		btVector3 newMargin(getMargin(),getMargin(),getMargin());
+		btVector3 newMargin(getMargin(), getMargin(), getMargin());
 		m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
-
 	}
-	virtual void	setLocalScaling(const btVector3& scaling)
+	virtual void setLocalScaling(const btVector3& scaling)
 	{
-		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
-		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+		btVector3 oldMargin(getMargin(), getMargin(), getMargin());
+		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin;
 		btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
 
 		btConvexInternalShape::setLocalScaling(scaling);
 
 		m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
-
 	}
 
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
-
-	
-
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
 
-
-
-
-	int	getVertexCount() const
+	int getVertexCount() const
 	{
 		return 4;
 	}
 
-	virtual int getNumVertices()const
+	virtual int getNumVertices() const
 	{
 		return 4;
 	}
@@ -163,82 +152,70 @@ public:
 		return &m_normals[0];
 	}
 
-
-
-
-
-
-
-	virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const
+	virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const
 	{
 		//this plane might not be aligned...
-		btVector4 plane ;
-		getPlaneEquation(plane,i);
-		planeNormal = btVector3(plane.getX(),plane.getY(),plane.getZ());
+		btVector4 plane;
+		getPlaneEquation(plane, i);
+		planeNormal = btVector3(plane.getX(), plane.getY(), plane.getZ());
 		planeSupport = localGetSupportingVertex(-planeNormal);
 	}
 
-
 	const btVector3& getCentroid() const
 	{
 		return m_centroid;
 	}
-	
+
 	virtual int getNumPlanes() const
 	{
 		return 6;
-	}	
-	
-	
+	}
 
 	virtual int getNumEdges() const
 	{
 		return 12;
 	}
 
-
-	virtual void getVertex(int i,btVector3& vtx) const
+	virtual void getVertex(int i, btVector3& vtx) const
 	{
 		btVector3 halfExtents = getHalfExtentsWithoutMargin();
 
 		vtx = btVector3(
-				halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1),
-				halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1),
-				halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2));
+			halfExtents.x() * (1 - (i & 1)) - halfExtents.x() * (i & 1),
+			halfExtents.y() * (1 - ((i & 2) >> 1)) - halfExtents.y() * ((i & 2) >> 1),
+			halfExtents.z() * (1 - ((i & 4) >> 2)) - halfExtents.z() * ((i & 4) >> 2));
 	}
-	
 
-	virtual void	getPlaneEquation(btVector4& plane,int i) const
+	virtual void getPlaneEquation(btVector4 & plane, int i) const
 	{
 		btVector3 halfExtents = getHalfExtentsWithoutMargin();
 
 		switch (i)
 		{
-		case 0:
-			plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x());
-			break;
-		case 1:
-			plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x());
-			break;
-		case 2:
-			plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y());
-			break;
-		case 3:
-			plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y());
-			break;
-		case 4:
-			plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z());
-			break;
-		case 5:
-			plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z());
-			break;
-		default:
-			btAssert(0);
+			case 0:
+				plane.setValue(btScalar(1.), btScalar(0.), btScalar(0.), -halfExtents.x());
+				break;
+			case 1:
+				plane.setValue(btScalar(-1.), btScalar(0.), btScalar(0.), -halfExtents.x());
+				break;
+			case 2:
+				plane.setValue(btScalar(0.), btScalar(1.), btScalar(0.), -halfExtents.y());
+				break;
+			case 3:
+				plane.setValue(btScalar(0.), btScalar(-1.), btScalar(0.), -halfExtents.y());
+				break;
+			case 4:
+				plane.setValue(btScalar(0.), btScalar(0.), btScalar(1.), -halfExtents.z());
+				break;
+			case 5:
+				plane.setValue(btScalar(0.), btScalar(0.), btScalar(-1.), -halfExtents.z());
+				break;
+			default:
+				btAssert(0);
 		}
 	}
 
-	
-	virtual void getEdge(int i,btVector3& pa,btVector3& pb) const
+	virtual void getEdge(int i, btVector3& pa, btVector3& pb) const
 	//virtual void getEdge(int i,Edge& edge) const
 	{
 		int edgeVert0 = 0;
@@ -246,126 +223,117 @@ public:
 
 		switch (i)
 		{
-		case 0:
+			case 0:
 				edgeVert0 = 0;
 				edgeVert1 = 1;
-			break;
-		case 1:
+				break;
+			case 1:
 				edgeVert0 = 0;
 				edgeVert1 = 2;
-			break;
-		case 2:
-			edgeVert0 = 1;
-			edgeVert1 = 3;
-
-			break;
-		case 3:
-			edgeVert0 = 2;
-			edgeVert1 = 3;
-			break;
-		case 4:
-			edgeVert0 = 0;
-			edgeVert1 = 4;
-			break;
-		case 5:
-			edgeVert0 = 1;
-			edgeVert1 = 5;
-
-			break;
-		case 6:
-			edgeVert0 = 2;
-			edgeVert1 = 6;
-			break;
-		case 7:
-			edgeVert0 = 3;
-			edgeVert1 = 7;
-			break;
-		case 8:
-			edgeVert0 = 4;
-			edgeVert1 = 5;
-			break;
-		case 9:
-			edgeVert0 = 4;
-			edgeVert1 = 6;
-			break;
-		case 10:
-			edgeVert0 = 5;
-			edgeVert1 = 7;
-			break;
-		case 11:
-			edgeVert0 = 6;
-			edgeVert1 = 7;
-			break;
-		default:
-			btAssert(0);
-
+				break;
+			case 2:
+				edgeVert0 = 1;
+				edgeVert1 = 3;
+
+				break;
+			case 3:
+				edgeVert0 = 2;
+				edgeVert1 = 3;
+				break;
+			case 4:
+				edgeVert0 = 0;
+				edgeVert1 = 4;
+				break;
+			case 5:
+				edgeVert0 = 1;
+				edgeVert1 = 5;
+
+				break;
+			case 6:
+				edgeVert0 = 2;
+				edgeVert1 = 6;
+				break;
+			case 7:
+				edgeVert0 = 3;
+				edgeVert1 = 7;
+				break;
+			case 8:
+				edgeVert0 = 4;
+				edgeVert1 = 5;
+				break;
+			case 9:
+				edgeVert0 = 4;
+				edgeVert1 = 6;
+				break;
+			case 10:
+				edgeVert0 = 5;
+				edgeVert1 = 7;
+				break;
+			case 11:
+				edgeVert0 = 6;
+				edgeVert1 = 7;
+				break;
+			default:
+				btAssert(0);
 		}
 
-		getVertex(edgeVert0,pa );
-		getVertex(edgeVert1,pb );
+		getVertex(edgeVert0, pa);
+		getVertex(edgeVert1, pb);
 	}
 
-
-
-
-	
-	virtual	bool isInside(const btVector3& pt,btScalar tolerance) const
+	virtual bool isInside(const btVector3& pt, btScalar tolerance) const
 	{
 		btVector3 halfExtents = getHalfExtentsWithoutMargin();
 
 		//btScalar minDist = 2*tolerance;
-		
-		bool result =	(pt.x() <= (halfExtents.x()+tolerance)) &&
-						(pt.x() >= (-halfExtents.x()-tolerance)) &&
-						(pt.y() <= (halfExtents.y()+tolerance)) &&
-						(pt.y() >= (-halfExtents.y()-tolerance)) &&
-						(pt.z() <= (halfExtents.z()+tolerance)) &&
-						(pt.z() >= (-halfExtents.z()-tolerance));
-		
+
+		bool result = (pt.x() <= (halfExtents.x() + tolerance)) &&
+					  (pt.x() >= (-halfExtents.x() - tolerance)) &&
+					  (pt.y() <= (halfExtents.y() + tolerance)) &&
+					  (pt.y() >= (-halfExtents.y() - tolerance)) &&
+					  (pt.z() <= (halfExtents.z() + tolerance)) &&
+					  (pt.z() >= (-halfExtents.z() - tolerance));
+
 		return result;
 	}
 
-
 	//debugging
-	virtual const char*	getName()const
+	virtual const char* getName() const
 	{
 		return "Box2d";
 	}
 
-	virtual int		getNumPreferredPenetrationDirections() const
+	virtual int getNumPreferredPenetrationDirections() const
 	{
 		return 6;
 	}
-	
-	virtual void	getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+
+	virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
 	{
 		switch (index)
 		{
-		case 0:
-			penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.));
-			break;
-		case 1:
-			penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.));
-			break;
-		case 2:
-			penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.));
-			break;
-		case 3:
-			penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.));
-			break;
-		case 4:
-			penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.));
-			break;
-		case 5:
-			penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.));
-			break;
-		default:
-			btAssert(0);
+			case 0:
+				penetrationVector.setValue(btScalar(1.), btScalar(0.), btScalar(0.));
+				break;
+			case 1:
+				penetrationVector.setValue(btScalar(-1.), btScalar(0.), btScalar(0.));
+				break;
+			case 2:
+				penetrationVector.setValue(btScalar(0.), btScalar(1.), btScalar(0.));
+				break;
+			case 3:
+				penetrationVector.setValue(btScalar(0.), btScalar(-1.), btScalar(0.));
+				break;
+			case 4:
+				penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(1.));
+				break;
+			case 5:
+				penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(-1.));
+				break;
+			default:
+				btAssert(0);
 		}
 	}
-
 };
 
-#endif //BT_OBB_BOX_2D_SHAPE_H
-
-
+#endif  //BT_OBB_BOX_2D_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp
index 3859138f18a..cb91d023e40 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp
@@ -14,38 +14,32 @@ subject to the following restrictions:
 */
 #include "btBoxShape.h"
 
-btBoxShape::btBoxShape( const btVector3& boxHalfExtents) 
-: btPolyhedralConvexShape()
+btBoxShape::btBoxShape(const btVector3& boxHalfExtents)
+	: btPolyhedralConvexShape()
 {
 	m_shapeType = BOX_SHAPE_PROXYTYPE;
 
-	setSafeMargin(boxHalfExtents);
-
-	btVector3 margin(getMargin(),getMargin(),getMargin());
+	btVector3 margin(getMargin(), getMargin(), getMargin());
 	m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
-};
-
-
 
+	setSafeMargin(boxHalfExtents);
+};
 
-void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+void btBoxShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 {
-	btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
+	btTransformAabb(getHalfExtentsWithoutMargin(), getMargin(), t, aabbMin, aabbMax);
 }
 
-
-void	btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btBoxShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
 	//btScalar margin = btScalar(0.);
 	btVector3 halfExtents = getHalfExtentsWithMargin();
 
-	btScalar lx=btScalar(2.)*(halfExtents.x());
-	btScalar ly=btScalar(2.)*(halfExtents.y());
-	btScalar lz=btScalar(2.)*(halfExtents.z());
-
-	inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
-					mass/(btScalar(12.0)) * (lx*lx + lz*lz),
-					mass/(btScalar(12.0)) * (lx*lx + ly*ly));
+	btScalar lx = btScalar(2.) * (halfExtents.x());
+	btScalar ly = btScalar(2.) * (halfExtents.y());
+	btScalar lz = btScalar(2.) * (halfExtents.z());
 
+	inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz),
+					 mass / (btScalar(12.0)) * (lx * lx + lz * lz),
+					 mass / (btScalar(12.0)) * (lx * lx + ly * ly));
 }
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h
index 715e3f2ab98..3c65505d5bb 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h
@@ -23,112 +23,102 @@ subject to the following restrictions:
 #include "LinearMath/btMinMax.h"
 
 ///The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space.
-ATTRIBUTE_ALIGNED16(class) btBoxShape: public btPolyhedralConvexShape
+ATTRIBUTE_ALIGNED16(class)
+btBoxShape : public btPolyhedralConvexShape
 {
-
 	//btVector3	m_boxHalfExtents1; //use m_implicitShapeDimensions instead
 
-
 public:
-
-BT_DECLARE_ALIGNED_ALLOCATOR();
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	btVector3 getHalfExtentsWithMargin() const
 	{
 		btVector3 halfExtents = getHalfExtentsWithoutMargin();
-		btVector3 margin(getMargin(),getMargin(),getMargin());
+		btVector3 margin(getMargin(), getMargin(), getMargin());
 		halfExtents += margin;
 		return halfExtents;
 	}
-	
+
 	const btVector3& getHalfExtentsWithoutMargin() const
 	{
-		return m_implicitShapeDimensions;//scaling is included, margin is not
+		return m_implicitShapeDimensions;  //scaling is included, margin is not
 	}
-	
 
-	virtual btVector3	localGetSupportingVertex(const btVector3& vec) const
+	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
 	{
 		btVector3 halfExtents = getHalfExtentsWithoutMargin();
-		btVector3 margin(getMargin(),getMargin(),getMargin());
+		btVector3 margin(getMargin(), getMargin(), getMargin());
 		halfExtents += margin;
-		
+
 		return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
-			btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
-			btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
+						 btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
+						 btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
 	}
 
-	SIMD_FORCE_INLINE  btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+	SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 	{
 		const btVector3& halfExtents = getHalfExtentsWithoutMargin();
-		
+
 		return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
-			btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
-			btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
+						 btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
+						 btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
 	}
 
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 	{
 		const btVector3& halfExtents = getHalfExtentsWithoutMargin();
-	
-		for (int i=0;i<numVectors;i++)
+
+		for (int i = 0; i < numVectors; i++)
 		{
 			const btVector3& vec = vectors[i];
 			supportVerticesOut[i].setValue(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
-				btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
-				btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); 
+										   btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
+										   btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
 		}
-
 	}
 
-
-	btBoxShape( const btVector3& boxHalfExtents);
+	btBoxShape(const btVector3& boxHalfExtents);
 
 	virtual void setMargin(btScalar collisionMargin)
 	{
 		//correct the m_implicitShapeDimensions for the margin
-		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
-		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
-		
+		btVector3 oldMargin(getMargin(), getMargin(), getMargin());
+		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin;
+
 		btConvexInternalShape::setMargin(collisionMargin);
-		btVector3 newMargin(getMargin(),getMargin(),getMargin());
+		btVector3 newMargin(getMargin(), getMargin(), getMargin());
 		m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
-
 	}
-	virtual void	setLocalScaling(const btVector3& scaling)
+	virtual void setLocalScaling(const btVector3& scaling)
 	{
-		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
-		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+		btVector3 oldMargin(getMargin(), getMargin(), getMargin());
+		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin;
 		btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
 
 		btConvexInternalShape::setLocalScaling(scaling);
 
 		m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
-
 	}
 
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
-	
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
 
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
-
-	virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const
+	virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const
 	{
 		//this plane might not be aligned...
-		btVector4 plane ;
-		getPlaneEquation(plane,i);
-		planeNormal = btVector3(plane.getX(),plane.getY(),plane.getZ());
+		btVector4 plane;
+		getPlaneEquation(plane, i);
+		planeNormal = btVector3(plane.getX(), plane.getY(), plane.getZ());
 		planeSupport = localGetSupportingVertex(-planeNormal);
 	}
 
-	
 	virtual int getNumPlanes() const
 	{
 		return 6;
-	}	
-	
-	virtual int	getNumVertices() const 
+	}
+
+	virtual int getNumVertices() const
 	{
 		return 8;
 	}
@@ -138,49 +128,46 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 		return 12;
 	}
 
-
-	virtual void getVertex(int i,btVector3& vtx) const
+	virtual void getVertex(int i, btVector3& vtx) const
 	{
 		btVector3 halfExtents = getHalfExtentsWithMargin();
 
 		vtx = btVector3(
-				halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1),
-				halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1),
-				halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2));
+			halfExtents.x() * (1 - (i & 1)) - halfExtents.x() * (i & 1),
+			halfExtents.y() * (1 - ((i & 2) >> 1)) - halfExtents.y() * ((i & 2) >> 1),
+			halfExtents.z() * (1 - ((i & 4) >> 2)) - halfExtents.z() * ((i & 4) >> 2));
 	}
-	
 
-	virtual void	getPlaneEquation(btVector4& plane,int i) const
+	virtual void getPlaneEquation(btVector4 & plane, int i) const
 	{
 		btVector3 halfExtents = getHalfExtentsWithoutMargin();
 
 		switch (i)
 		{
-		case 0:
-			plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x());
-			break;
-		case 1:
-			plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x());
-			break;
-		case 2:
-			plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y());
-			break;
-		case 3:
-			plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y());
-			break;
-		case 4:
-			plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z());
-			break;
-		case 5:
-			plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z());
-			break;
-		default:
-			btAssert(0);
+			case 0:
+				plane.setValue(btScalar(1.), btScalar(0.), btScalar(0.), -halfExtents.x());
+				break;
+			case 1:
+				plane.setValue(btScalar(-1.), btScalar(0.), btScalar(0.), -halfExtents.x());
+				break;
+			case 2:
+				plane.setValue(btScalar(0.), btScalar(1.), btScalar(0.), -halfExtents.y());
+				break;
+			case 3:
+				plane.setValue(btScalar(0.), btScalar(-1.), btScalar(0.), -halfExtents.y());
+				break;
+			case 4:
+				plane.setValue(btScalar(0.), btScalar(0.), btScalar(1.), -halfExtents.z());
+				break;
+			case 5:
+				plane.setValue(btScalar(0.), btScalar(0.), btScalar(-1.), -halfExtents.z());
+				break;
+			default:
+				btAssert(0);
 		}
 	}
 
-	
-	virtual void getEdge(int i,btVector3& pa,btVector3& pb) const
+	virtual void getEdge(int i, btVector3& pa, btVector3& pb) const
 	//virtual void getEdge(int i,Edge& edge) const
 	{
 		int edgeVert0 = 0;
@@ -188,127 +175,117 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 
 		switch (i)
 		{
-		case 0:
+			case 0:
 				edgeVert0 = 0;
 				edgeVert1 = 1;
-			break;
-		case 1:
+				break;
+			case 1:
 				edgeVert0 = 0;
 				edgeVert1 = 2;
-			break;
-		case 2:
-			edgeVert0 = 1;
-			edgeVert1 = 3;
-
-			break;
-		case 3:
-			edgeVert0 = 2;
-			edgeVert1 = 3;
-			break;
-		case 4:
-			edgeVert0 = 0;
-			edgeVert1 = 4;
-			break;
-		case 5:
-			edgeVert0 = 1;
-			edgeVert1 = 5;
-
-			break;
-		case 6:
-			edgeVert0 = 2;
-			edgeVert1 = 6;
-			break;
-		case 7:
-			edgeVert0 = 3;
-			edgeVert1 = 7;
-			break;
-		case 8:
-			edgeVert0 = 4;
-			edgeVert1 = 5;
-			break;
-		case 9:
-			edgeVert0 = 4;
-			edgeVert1 = 6;
-			break;
-		case 10:
-			edgeVert0 = 5;
-			edgeVert1 = 7;
-			break;
-		case 11:
-			edgeVert0 = 6;
-			edgeVert1 = 7;
-			break;
-		default:
-			btAssert(0);
-
+				break;
+			case 2:
+				edgeVert0 = 1;
+				edgeVert1 = 3;
+
+				break;
+			case 3:
+				edgeVert0 = 2;
+				edgeVert1 = 3;
+				break;
+			case 4:
+				edgeVert0 = 0;
+				edgeVert1 = 4;
+				break;
+			case 5:
+				edgeVert0 = 1;
+				edgeVert1 = 5;
+
+				break;
+			case 6:
+				edgeVert0 = 2;
+				edgeVert1 = 6;
+				break;
+			case 7:
+				edgeVert0 = 3;
+				edgeVert1 = 7;
+				break;
+			case 8:
+				edgeVert0 = 4;
+				edgeVert1 = 5;
+				break;
+			case 9:
+				edgeVert0 = 4;
+				edgeVert1 = 6;
+				break;
+			case 10:
+				edgeVert0 = 5;
+				edgeVert1 = 7;
+				break;
+			case 11:
+				edgeVert0 = 6;
+				edgeVert1 = 7;
+				break;
+			default:
+				btAssert(0);
 		}
 
-		getVertex(edgeVert0,pa );
-		getVertex(edgeVert1,pb );
+		getVertex(edgeVert0, pa);
+		getVertex(edgeVert1, pb);
 	}
 
-
-
-
-	
-	virtual	bool isInside(const btVector3& pt,btScalar tolerance) const
+	virtual bool isInside(const btVector3& pt, btScalar tolerance) const
 	{
 		btVector3 halfExtents = getHalfExtentsWithoutMargin();
 
 		//btScalar minDist = 2*tolerance;
-		
-		bool result =	(pt.x() <= (halfExtents.x()+tolerance)) &&
-						(pt.x() >= (-halfExtents.x()-tolerance)) &&
-						(pt.y() <= (halfExtents.y()+tolerance)) &&
-						(pt.y() >= (-halfExtents.y()-tolerance)) &&
-						(pt.z() <= (halfExtents.z()+tolerance)) &&
-						(pt.z() >= (-halfExtents.z()-tolerance));
-		
+
+		bool result = (pt.x() <= (halfExtents.x() + tolerance)) &&
+					  (pt.x() >= (-halfExtents.x() - tolerance)) &&
+					  (pt.y() <= (halfExtents.y() + tolerance)) &&
+					  (pt.y() >= (-halfExtents.y() - tolerance)) &&
+					  (pt.z() <= (halfExtents.z() + tolerance)) &&
+					  (pt.z() >= (-halfExtents.z() - tolerance));
+
 		return result;
 	}
 
-
 	//debugging
-	virtual const char*	getName()const
+	virtual const char* getName() const
 	{
 		return "Box";
 	}
 
-	virtual int		getNumPreferredPenetrationDirections() const
+	virtual int getNumPreferredPenetrationDirections() const
 	{
 		return 6;
 	}
-	
-	virtual void	getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+
+	virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
 	{
 		switch (index)
 		{
-		case 0:
-			penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.));
-			break;
-		case 1:
-			penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.));
-			break;
-		case 2:
-			penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.));
-			break;
-		case 3:
-			penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.));
-			break;
-		case 4:
-			penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.));
-			break;
-		case 5:
-			penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.));
-			break;
-		default:
-			btAssert(0);
+			case 0:
+				penetrationVector.setValue(btScalar(1.), btScalar(0.), btScalar(0.));
+				break;
+			case 1:
+				penetrationVector.setValue(btScalar(-1.), btScalar(0.), btScalar(0.));
+				break;
+			case 2:
+				penetrationVector.setValue(btScalar(0.), btScalar(1.), btScalar(0.));
+				break;
+			case 3:
+				penetrationVector.setValue(btScalar(0.), btScalar(-1.), btScalar(0.));
+				break;
+			case 4:
+				penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(1.));
+				break;
+			case 5:
+				penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(-1.));
+				break;
+			default:
+				btAssert(0);
 		}
 	}
-
 };
 
-
-#endif //BT_OBB_BOX_MINKOWSKI_H
-
-
+#endif  //BT_OBB_BOX_MINKOWSKI_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp
index ace4cfa2646..c66ce58e3ef 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp
@@ -22,11 +22,11 @@ subject to the following restrictions:
 ///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization.
 ///Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
 btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh)
-:btTriangleMeshShape(meshInterface),
-m_bvh(0),
-m_triangleInfoMap(0),
-m_useQuantizedAabbCompression(useQuantizedAabbCompression),
-m_ownsBvh(false)
+	: btTriangleMeshShape(meshInterface),
+	  m_bvh(0),
+	  m_triangleInfoMap(0),
+	  m_useQuantizedAabbCompression(useQuantizedAabbCompression),
+	  m_ownsBvh(false)
 {
 	m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
 	//construct bvh from meshInterface
@@ -37,16 +37,15 @@ m_ownsBvh(false)
 		buildOptimizedBvh();
 	}
 
-#endif //DISABLE_BVH
-
+#endif  //DISABLE_BVH
 }
 
-btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,bool buildBvh)
-:btTriangleMeshShape(meshInterface),
-m_bvh(0),
-m_triangleInfoMap(0),
-m_useQuantizedAabbCompression(useQuantizedAabbCompression),
-m_ownsBvh(false)
+btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, bool buildBvh)
+	: btTriangleMeshShape(meshInterface),
+	  m_bvh(0),
+	  m_triangleInfoMap(0),
+	  m_useQuantizedAabbCompression(useQuantizedAabbCompression),
+	  m_ownsBvh(false)
 {
 	m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
 	//construct bvh from meshInterface
@@ -54,30 +53,28 @@ m_ownsBvh(false)
 
 	if (buildBvh)
 	{
-		void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16);
+		void* mem = btAlignedAlloc(sizeof(btOptimizedBvh), 16);
 		m_bvh = new (mem) btOptimizedBvh();
-		
-		m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax);
+
+		m_bvh->build(meshInterface, m_useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax);
 		m_ownsBvh = true;
 	}
 
-#endif //DISABLE_BVH
-
+#endif  //DISABLE_BVH
 }
 
-void	btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax)
+void btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin, const btVector3& aabbMax)
 {
-	m_bvh->refitPartial( m_meshInterface,aabbMin,aabbMax );
-	
+	m_bvh->refitPartial(m_meshInterface, aabbMin, aabbMax);
+
 	m_localAabbMin.setMin(aabbMin);
 	m_localAabbMax.setMax(aabbMax);
 }
 
-
-void	btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin,const btVector3& aabbMax)
+void btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin, const btVector3& aabbMax)
 {
-	m_bvh->refit( m_meshInterface, aabbMin,aabbMax );
-	
+	m_bvh->refit(m_meshInterface, aabbMin, aabbMax);
+
 	recalcLocalAabb();
 }
 
@@ -90,27 +87,27 @@ btBvhTriangleMeshShape::~btBvhTriangleMeshShape()
 	}
 }
 
-void	btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget)
+void btBvhTriangleMeshShape::performRaycast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget)
 {
-	struct	MyNodeOverlapCallback : public btNodeOverlapCallback
+	struct MyNodeOverlapCallback : public btNodeOverlapCallback
 	{
-		btStridingMeshInterface*	m_meshInterface;
+		btStridingMeshInterface* m_meshInterface;
 		btTriangleCallback* m_callback;
 
-		MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
-			:m_meshInterface(meshInterface),
-			m_callback(callback)
+		MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface)
+			: m_meshInterface(meshInterface),
+			  m_callback(callback)
 		{
 		}
-				
+
 		virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
 		{
 			btVector3 m_triangle[3];
-			const unsigned char *vertexbase;
+			const unsigned char* vertexbase;
 			int numverts;
 			PHY_ScalarType type;
 			int stride;
-			const unsigned char *indexbase;
+			const unsigned char* indexbase;
 			int indexstride;
 			int numfaces;
 			PHY_ScalarType indicestype;
@@ -126,60 +123,65 @@ void	btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const
 				indicestype,
 				nodeSubPart);
 
-			unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
-			btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
-	
+			unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride);
+
 			const btVector3& meshScaling = m_meshInterface->getScaling();
-			for (int j=2;j>=0;j--)
+			for (int j = 2; j >= 0; j--)
 			{
-				int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
-				
+				int graphicsindex;
+                                switch (indicestype) {
+                                        case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
+                                        case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
+                                        case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
+                                        default: btAssert(0);
+                                }
+
 				if (type == PHY_FLOAT)
 				{
-					float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
-					
-					m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());		
+					float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
+
+					m_triangle[j] = btVector3(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ());
 				}
 				else
 				{
-					double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
-					
-					m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ());		
+					double* graphicsbase = (double*)(vertexbase + graphicsindex * stride);
+
+					m_triangle[j] = btVector3(btScalar(graphicsbase[0]) * meshScaling.getX(), btScalar(graphicsbase[1]) * meshScaling.getY(), btScalar(graphicsbase[2]) * meshScaling.getZ());
 				}
 			}
 
 			/* Perform ray vs. triangle collision here */
-			m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex);
+			m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex);
 			m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
 		}
 	};
 
-	MyNodeOverlapCallback	myNodeCallback(callback,m_meshInterface);
+	MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface);
 
-	m_bvh->reportRayOverlappingNodex(&myNodeCallback,raySource,rayTarget);
+	m_bvh->reportRayOverlappingNodex(&myNodeCallback, raySource, rayTarget);
 }
 
-void	btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax)
+void btBvhTriangleMeshShape::performConvexcast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax)
 {
-	struct	MyNodeOverlapCallback : public btNodeOverlapCallback
+	struct MyNodeOverlapCallback : public btNodeOverlapCallback
 	{
-		btStridingMeshInterface*	m_meshInterface;
+		btStridingMeshInterface* m_meshInterface;
 		btTriangleCallback* m_callback;
 
-		MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
-			:m_meshInterface(meshInterface),
-			m_callback(callback)
+		MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface)
+			: m_meshInterface(meshInterface),
+			  m_callback(callback)
 		{
 		}
-				
+
 		virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
 		{
 			btVector3 m_triangle[3];
-			const unsigned char *vertexbase;
+			const unsigned char* vertexbase;
 			int numverts;
 			PHY_ScalarType type;
 			int stride;
-			const unsigned char *indexbase;
+			const unsigned char* indexbase;
 			int indexstride;
 			int numfaces;
 			PHY_ScalarType indicestype;
@@ -195,75 +197,79 @@ void	btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, co
 				indicestype,
 				nodeSubPart);
 
-			unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
-			btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
-	
+			unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride);
+
 			const btVector3& meshScaling = m_meshInterface->getScaling();
-			for (int j=2;j>=0;j--)
+			for (int j = 2; j >= 0; j--)
 			{
-				int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
+				int graphicsindex;
+                                switch (indicestype) {
+                                        case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
+                                        case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
+                                        case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
+                                        default: btAssert(0);
+                                }
 
 				if (type == PHY_FLOAT)
 				{
-					float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
+					float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
 
-					m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());		
+					m_triangle[j] = btVector3(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ());
 				}
 				else
 				{
-					double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
-					
-					m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ());		
+					double* graphicsbase = (double*)(vertexbase + graphicsindex * stride);
+
+					m_triangle[j] = btVector3(btScalar(graphicsbase[0]) * meshScaling.getX(), btScalar(graphicsbase[1]) * meshScaling.getY(), btScalar(graphicsbase[2]) * meshScaling.getZ());
 				}
 			}
 
 			/* Perform ray vs. triangle collision here */
-			m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex);
+			m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex);
 			m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
 		}
 	};
 
-	MyNodeOverlapCallback	myNodeCallback(callback,m_meshInterface);
+	MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface);
 
-	m_bvh->reportBoxCastOverlappingNodex (&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax);
+	m_bvh->reportBoxCastOverlappingNodex(&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax);
 }
 
 //perform bvh tree traversal and report overlapping triangles to 'callback'
-void	btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
 {
-
 #ifdef DISABLE_BVH
 	//brute force traverse all triangles
-	btTriangleMeshShape::processAllTriangles(callback,aabbMin,aabbMax);
+	btTriangleMeshShape::processAllTriangles(callback, aabbMin, aabbMax);
 #else
 
 	//first get all the nodes
 
-	
-	struct	MyNodeOverlapCallback : public btNodeOverlapCallback
+	struct MyNodeOverlapCallback : public btNodeOverlapCallback
 	{
-		btStridingMeshInterface*	m_meshInterface;
-		btTriangleCallback*		m_callback;
-		btVector3				m_triangle[3];
-
+		btStridingMeshInterface* m_meshInterface;
+		btTriangleCallback* m_callback;
+		btVector3 m_triangle[3];
+		int m_numOverlap;
 
-		MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
-			:m_meshInterface(meshInterface),
-			m_callback(callback)
+		MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface)
+			: m_meshInterface(meshInterface),
+			  m_callback(callback),
+			  m_numOverlap(0)
 		{
 		}
-				
+
 		virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
 		{
-			const unsigned char *vertexbase;
+			m_numOverlap++;
+			const unsigned char* vertexbase;
 			int numverts;
 			PHY_ScalarType type;
 			int stride;
-			const unsigned char *indexbase;
+			const unsigned char* indexbase;
 			int indexstride;
 			int numfaces;
 			PHY_ScalarType indicestype;
-			
 
 			m_meshInterface->getLockedReadOnlyVertexIndexBase(
 				&vertexbase,
@@ -276,68 +282,62 @@ void	btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
 				indicestype,
 				nodeSubPart);
 
-			unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
-			btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT||indicestype==PHY_UCHAR);
-	
+			unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride);
+			btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT || indicestype == PHY_UCHAR);
+
 			const btVector3& meshScaling = m_meshInterface->getScaling();
-			for (int j=2;j>=0;j--)
+			for (int j = 2; j >= 0; j--)
 			{
-				
-				int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:indicestype==PHY_INTEGER?gfxbase[j]:((unsigned char*)gfxbase)[j];
-
+				int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : indicestype == PHY_INTEGER ? gfxbase[j] : ((unsigned char*)gfxbase)[j];
 
 #ifdef DEBUG_TRIANGLE_MESH
-				printf("%d ,",graphicsindex);
-#endif //DEBUG_TRIANGLE_MESH
+				printf("%d ,", graphicsindex);
+#endif  //DEBUG_TRIANGLE_MESH
 				if (type == PHY_FLOAT)
 				{
-					float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
-					
+					float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
+
 					m_triangle[j] = btVector3(
-																		graphicsbase[0]*meshScaling.getX(),
-																		graphicsbase[1]*meshScaling.getY(),
-																		graphicsbase[2]*meshScaling.getZ());
+						graphicsbase[0] * meshScaling.getX(),
+						graphicsbase[1] * meshScaling.getY(),
+						graphicsbase[2] * meshScaling.getZ());
 				}
 				else
 				{
-					double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
+					double* graphicsbase = (double*)(vertexbase + graphicsindex * stride);
 
 					m_triangle[j] = btVector3(
-						btScalar(graphicsbase[0])*meshScaling.getX(),
-						btScalar(graphicsbase[1])*meshScaling.getY(),
-						btScalar(graphicsbase[2])*meshScaling.getZ());
+						btScalar(graphicsbase[0]) * meshScaling.getX(),
+						btScalar(graphicsbase[1]) * meshScaling.getY(),
+						btScalar(graphicsbase[2]) * meshScaling.getZ());
 				}
 #ifdef DEBUG_TRIANGLE_MESH
-				printf("triangle vertices:%f,%f,%f\n",triangle[j].x(),triangle[j].y(),triangle[j].z());
-#endif //DEBUG_TRIANGLE_MESH
+				printf("triangle vertices:%f,%f,%f\n", triangle[j].x(), triangle[j].y(), triangle[j].z());
+#endif  //DEBUG_TRIANGLE_MESH
 			}
 
-			m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex);
+			m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex);
 			m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
 		}
-
 	};
 
-	MyNodeOverlapCallback	myNodeCallback(callback,m_meshInterface);
-
-	m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
-
-
-#endif//DISABLE_BVH
+	MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface);
 
+	m_bvh->reportAabbOverlappingNodex(&myNodeCallback, aabbMin, aabbMax);
 
+#endif  //DISABLE_BVH
 }
 
-void   btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling)
+void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling)
 {
-   if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON)
-   {
-      btTriangleMeshShape::setLocalScaling(scaling);
-	  buildOptimizedBvh();
-   }
+	if ((getLocalScaling() - scaling).length2() > SIMD_EPSILON)
+	{
+		btTriangleMeshShape::setLocalScaling(scaling);
+		buildOptimizedBvh();
+	}
 }
 
-void   btBvhTriangleMeshShape::buildOptimizedBvh()
+void btBvhTriangleMeshShape::buildOptimizedBvh()
 {
 	if (m_ownsBvh)
 	{
@@ -345,43 +345,39 @@ void   btBvhTriangleMeshShape::buildOptimizedBvh()
 		btAlignedFree(m_bvh);
 	}
 	///m_localAabbMin/m_localAabbMax is already re-calculated in btTriangleMeshShape. We could just scale aabb, but this needs some more work
-	void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16);
-	m_bvh = new(mem) btOptimizedBvh();
+	void* mem = btAlignedAlloc(sizeof(btOptimizedBvh), 16);
+	m_bvh = new (mem) btOptimizedBvh();
 	//rebuild the bvh...
-	m_bvh->build(m_meshInterface,m_useQuantizedAabbCompression,m_localAabbMin,m_localAabbMax);
+	m_bvh->build(m_meshInterface, m_useQuantizedAabbCompression, m_localAabbMin, m_localAabbMax);
 	m_ownsBvh = true;
 }
 
-void   btBvhTriangleMeshShape::setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& scaling)
+void btBvhTriangleMeshShape::setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& scaling)
 {
-   btAssert(!m_bvh);
-   btAssert(!m_ownsBvh);
-
-   m_bvh = bvh;
-   m_ownsBvh = false;
-   // update the scaling without rebuilding the bvh
-   if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON)
-   {
-      btTriangleMeshShape::setLocalScaling(scaling);
-   }
-}
-
+	btAssert(!m_bvh);
+	btAssert(!m_ownsBvh);
 
+	m_bvh = bvh;
+	m_ownsBvh = false;
+	// update the scaling without rebuilding the bvh
+	if ((getLocalScaling() - scaling).length2() > SIMD_EPSILON)
+	{
+		btTriangleMeshShape::setLocalScaling(scaling);
+	}
+}
 
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const
+const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btTriangleMeshShapeData* trimeshData = (btTriangleMeshShapeData*) dataBuffer;
+	btTriangleMeshShapeData* trimeshData = (btTriangleMeshShapeData*)dataBuffer;
 
-	btCollisionShape::serialize(&trimeshData->m_collisionShapeData,serializer);
+	btCollisionShape::serialize(&trimeshData->m_collisionShapeData, serializer);
 
 	m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer);
 
 	trimeshData->m_collisionMargin = float(m_collisionMargin);
 
-	
-
-	if (m_bvh && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_BVH))
+	if (m_bvh && !(serializer->getSerializationFlags() & BT_SERIALIZE_NO_BVH))
 	{
 		void* chunk = serializer->findPointer(m_bvh);
 		if (chunk)
@@ -390,77 +386,77 @@ const char*	btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* se
 			trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)chunk;
 			trimeshData->m_quantizedFloatBvh = 0;
 #else
-			trimeshData->m_quantizedFloatBvh  = (btQuantizedBvhData*)chunk;
-			trimeshData->m_quantizedDoubleBvh= 0;
-#endif //BT_USE_DOUBLE_PRECISION
-		} else
+			trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)chunk;
+			trimeshData->m_quantizedDoubleBvh = 0;
+#endif  //BT_USE_DOUBLE_PRECISION
+		}
+		else
 		{
-
 #ifdef BT_USE_DOUBLE_PRECISION
 			trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh);
 			trimeshData->m_quantizedFloatBvh = 0;
 #else
-			trimeshData->m_quantizedFloatBvh  = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh);
-			trimeshData->m_quantizedDoubleBvh= 0;
-#endif //BT_USE_DOUBLE_PRECISION
-	
+			trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh);
+			trimeshData->m_quantizedDoubleBvh = 0;
+#endif  //BT_USE_DOUBLE_PRECISION
+
 			int sz = m_bvh->calculateSerializeBufferSizeNew();
-			btChunk* chunk = serializer->allocate(sz,1);
+			btChunk* chunk = serializer->allocate(sz, 1);
 			const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer);
-			serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,m_bvh);
+			serializer->finalizeChunk(chunk, structType, BT_QUANTIZED_BVH_CODE, m_bvh);
 		}
-	} else
+	}
+	else
 	{
 		trimeshData->m_quantizedFloatBvh = 0;
 		trimeshData->m_quantizedDoubleBvh = 0;
 	}
 
-	
-
-	if (m_triangleInfoMap && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_TRIANGLEINFOMAP))
+	if (m_triangleInfoMap && !(serializer->getSerializationFlags() & BT_SERIALIZE_NO_TRIANGLEINFOMAP))
 	{
 		void* chunk = serializer->findPointer(m_triangleInfoMap);
 		if (chunk)
 		{
 			trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)chunk;
-		} else
+		}
+		else
 		{
 			trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)serializer->getUniquePointer(m_triangleInfoMap);
 			int sz = m_triangleInfoMap->calculateSerializeBufferSize();
-			btChunk* chunk = serializer->allocate(sz,1);
+			btChunk* chunk = serializer->allocate(sz, 1);
 			const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer);
-			serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,m_triangleInfoMap);
+			serializer->finalizeChunk(chunk, structType, BT_TRIANLGE_INFO_MAP, m_triangleInfoMap);
 		}
-	} else
+	}
+	else
 	{
 		trimeshData->m_triangleInfoMap = 0;
 	}
 
+	// Fill padding with zeros to appease msan.
+	memset(trimeshData->m_pad3, 0, sizeof(trimeshData->m_pad3));
+
 	return "btTriangleMeshShapeData";
 }
 
-void	btBvhTriangleMeshShape::serializeSingleBvh(btSerializer* serializer) const
+void btBvhTriangleMeshShape::serializeSingleBvh(btSerializer* serializer) const
 {
 	if (m_bvh)
 	{
-		int len = m_bvh->calculateSerializeBufferSizeNew(); //make sure not to use calculateSerializeBufferSize because it is used for in-place
-		btChunk* chunk = serializer->allocate(len,1);
+		int len = m_bvh->calculateSerializeBufferSizeNew();  //make sure not to use calculateSerializeBufferSize because it is used for in-place
+		btChunk* chunk = serializer->allocate(len, 1);
 		const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer);
-		serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,(void*)m_bvh);
+		serializer->finalizeChunk(chunk, structType, BT_QUANTIZED_BVH_CODE, (void*)m_bvh);
 	}
 }
 
-void	btBvhTriangleMeshShape::serializeSingleTriangleInfoMap(btSerializer* serializer) const
+void btBvhTriangleMeshShape::serializeSingleTriangleInfoMap(btSerializer* serializer) const
 {
 	if (m_triangleInfoMap)
 	{
 		int len = m_triangleInfoMap->calculateSerializeBufferSize();
-		btChunk* chunk = serializer->allocate(len,1);
+		btChunk* chunk = serializer->allocate(len, 1);
 		const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer);
-		serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,(void*)m_triangleInfoMap);
+		serializer->finalizeChunk(chunk, structType, BT_TRIANLGE_INFO_MAP, (void*)m_triangleInfoMap);
 	}
 }
-
-
-
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h
index 1fa4995d160..8b2f2ee85e8 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h
@@ -23,103 +23,99 @@ subject to the following restrictions:
 
 ///The btBvhTriangleMeshShape is a static-triangle mesh shape, it can only be used for fixed/non-moving objects.
 ///If you required moving concave triangle meshes, it is recommended to perform convex decomposition
-///using HACD, see Bullet/Demos/ConvexDecompositionDemo. 
+///using HACD, see Bullet/Demos/ConvexDecompositionDemo.
 ///Alternatively, you can use btGimpactMeshShape for moving concave triangle meshes.
-///btBvhTriangleMeshShape has several optimizations, such as bounding volume hierarchy and 
-///cache friendly traversal for PlayStation 3 Cell SPU. 
+///btBvhTriangleMeshShape has several optimizations, such as bounding volume hierarchy and
+///cache friendly traversal for PlayStation 3 Cell SPU.
 ///It is recommended to enable useQuantizedAabbCompression for better memory usage.
 ///It takes a triangle mesh as input, for example a btTriangleMesh or btTriangleIndexVertexArray. The btBvhTriangleMeshShape class allows for triangle mesh deformations by a refit or partialRefit method.
 ///Instead of building the bounding volume hierarchy acceleration structure, it is also possible to serialize (save) and deserialize (load) the structure from disk.
 ///See Demos\ConcaveDemo\ConcavePhysicsDemo.cpp for an example.
-ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape
+ATTRIBUTE_ALIGNED16(class)
+btBvhTriangleMeshShape : public btTriangleMeshShape
 {
-
-	btOptimizedBvh*	m_bvh;
-	btTriangleInfoMap*	m_triangleInfoMap;
+	btOptimizedBvh* m_bvh;
+	btTriangleInfoMap* m_triangleInfoMap;
 
 	bool m_useQuantizedAabbCompression;
 	bool m_ownsBvh;
 #ifdef __clang__
-	bool m_pad[11] __attribute__((unused));////need padding due to alignment
+	bool m_pad[11] __attribute__((unused));  ////need padding due to alignment
 #else
-	bool m_pad[11];////need padding due to alignment
+	bool m_pad[11];  ////need padding due to alignment
 #endif
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	
-	btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true);
+	btBvhTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true);
 
 	///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb
-	btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true);
-	
+	btBvhTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, bool buildBvh = true);
+
 	virtual ~btBvhTriangleMeshShape();
 
-	bool getOwnsBvh () const
+	bool getOwnsBvh() const
 	{
 		return m_ownsBvh;
 	}
 
+	void performRaycast(btTriangleCallback * callback, const btVector3& raySource, const btVector3& rayTarget);
+	void performConvexcast(btTriangleCallback * callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax);
 
-	
-	void performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget);
-	void performConvexcast (btTriangleCallback* callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax);
-
-	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+	virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const;
 
-	void	refitTree(const btVector3& aabbMin,const btVector3& aabbMax);
+	void refitTree(const btVector3& aabbMin, const btVector3& aabbMax);
 
 	///for a fast incremental refit of parts of the tree. Note: the entire AABB of the tree will become more conservative, it never shrinks
-	void	partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax);
+	void partialRefitTree(const btVector3& aabbMin, const btVector3& aabbMax);
 
 	//debugging
-	virtual const char*	getName()const {return "BVHTRIANGLEMESH";}
+	virtual const char* getName() const { return "BVHTRIANGLEMESH"; }
 
+	virtual void setLocalScaling(const btVector3& scaling);
 
-	virtual void	setLocalScaling(const btVector3& scaling);
-	
-	btOptimizedBvh*	getOptimizedBvh()
+	btOptimizedBvh* getOptimizedBvh()
 	{
 		return m_bvh;
 	}
 
-	void	setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& localScaling=btVector3(1,1,1));
+	void setOptimizedBvh(btOptimizedBvh * bvh, const btVector3& localScaling = btVector3(1, 1, 1));
 
-	void    buildOptimizedBvh();
+	void buildOptimizedBvh();
 
-	bool	usesQuantizedAabbCompression() const
+	bool usesQuantizedAabbCompression() const
 	{
-		return	m_useQuantizedAabbCompression;
+		return m_useQuantizedAabbCompression;
 	}
 
-	void	setTriangleInfoMap(btTriangleInfoMap* triangleInfoMap)
+	void setTriangleInfoMap(btTriangleInfoMap * triangleInfoMap)
 	{
 		m_triangleInfoMap = triangleInfoMap;
 	}
 
-	const btTriangleInfoMap*	getTriangleInfoMap() const
+	const btTriangleInfoMap* getTriangleInfoMap() const
 	{
 		return m_triangleInfoMap;
 	}
-	
-	btTriangleInfoMap*	getTriangleInfoMap()
+
+	btTriangleInfoMap* getTriangleInfoMap()
 	{
 		return m_triangleInfoMap;
 	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
-	virtual void	serializeSingleBvh(btSerializer* serializer) const;
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 
-	virtual void	serializeSingleTriangleInfoMap(btSerializer* serializer) const;
+	virtual void serializeSingleBvh(btSerializer * serializer) const;
 
+	virtual void serializeSingleTriangleInfoMap(btSerializer * serializer) const;
 };
 
+// clang-format off
+
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btTriangleMeshShapeData
 {
@@ -138,12 +134,11 @@ struct	btTriangleMeshShapeData
 	
 };
 
+// clang-format on
 
-SIMD_FORCE_INLINE	int	btBvhTriangleMeshShape::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btBvhTriangleMeshShape::calculateSerializeBufferSize() const
 {
 	return sizeof(btTriangleMeshShapeData);
 }
 
-
-
-#endif //BT_BVH_TRIANGLE_MESH_SHAPE_H
+#endif  //BT_BVH_TRIANGLE_MESH_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp
index 864df26e931..7c337742841 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp
@@ -13,24 +13,21 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btCapsuleShape.h"
 
-#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
 #include "LinearMath/btQuaternion.h"
 
-btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape ()
+btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape()
 {
+	m_collisionMargin = radius;
 	m_shapeType = CAPSULE_SHAPE_PROXYTYPE;
 	m_upAxis = 1;
-	m_implicitShapeDimensions.setValue(radius,0.5f*height,radius);
+	m_implicitShapeDimensions.setValue(radius, 0.5f * height, radius);
 }
 
- 
- btVector3	btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const
 {
-
-	btVector3 supVec(0,0,0);
+	btVector3 supVec(0, 0, 0);
 
 	btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
 
@@ -38,24 +35,22 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
 	btScalar lenSqr = vec.length2();
 	if (lenSqr < btScalar(0.0001))
 	{
-		vec.setValue(1,0,0);
-	} else
+		vec.setValue(1, 0, 0);
+	}
+	else
 	{
-		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+		btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
 		vec *= rlen;
 	}
 
 	btVector3 vtx;
 	btScalar newDot;
-	
-	btScalar radius = getRadius();
-
 
 	{
-		btVector3 pos(0,0,0);
+		btVector3 pos(0, 0, 0);
 		pos[getUpAxis()] = getHalfHeight();
 
-		vtx = pos +vec*(radius) - vec * getMargin();
+		vtx = pos;
 		newDot = vec.dot(vtx);
 		if (newDot > maxDot)
 		{
@@ -64,10 +59,10 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
 		}
 	}
 	{
-		btVector3 pos(0,0,0);
+		btVector3 pos(0, 0, 0);
 		pos[getUpAxis()] = -getHalfHeight();
 
-		vtx = pos +vec*(radius) - vec * getMargin();
+		vtx = pos;
 		newDot = vec.dot(vtx);
 		if (newDot > maxDot)
 		{
@@ -77,16 +72,11 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
 	}
 
 	return supVec;
-
 }
 
- void	btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 {
-
-	
-	btScalar radius = getRadius();
-
-	for (int j=0;j<numVectors;j++)
+	for (int j = 0; j < numVectors; j++)
 	{
 		btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
 		const btVector3& vec = vectors[j];
@@ -94,9 +84,9 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
 		btVector3 vtx;
 		btScalar newDot;
 		{
-			btVector3 pos(0,0,0);
+			btVector3 pos(0, 0, 0);
 			pos[getUpAxis()] = getHalfHeight();
-			vtx = pos +vec*(radius) - vec * getMargin();
+			vtx = pos;
 			newDot = vec.dot(vtx);
 			if (newDot > maxDot)
 			{
@@ -105,9 +95,9 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
 			}
 		}
 		{
-			btVector3 pos(0,0,0);
+			btVector3 pos(0, 0, 0);
 			pos[getUpAxis()] = -getHalfHeight();
-			vtx = pos +vec*(radius) - vec * getMargin();
+			vtx = pos;
 			newDot = vec.dot(vtx);
 			if (newDot > maxDot)
 			{
@@ -115,57 +105,44 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
 				supportVerticesOut[j] = vtx;
 			}
 		}
-		
 	}
 }
 
-
-void	btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btCapsuleShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
 	//as an approximation, take the inertia of the box that bounds the spheres
 
 	btTransform ident;
 	ident.setIdentity();
 
-	
 	btScalar radius = getRadius();
 
-	btVector3 halfExtents(radius,radius,radius);
-	halfExtents[getUpAxis()]+=getHalfHeight();
-
-	btScalar margin = CONVEX_DISTANCE_MARGIN;
+	btVector3 halfExtents(radius, radius, radius);
+	halfExtents[getUpAxis()] += getHalfHeight();
 
-	btScalar lx=btScalar(2.)*(halfExtents[0]+margin);
-	btScalar ly=btScalar(2.)*(halfExtents[1]+margin);
-	btScalar lz=btScalar(2.)*(halfExtents[2]+margin);
-	const btScalar x2 = lx*lx;
-	const btScalar y2 = ly*ly;
-	const btScalar z2 = lz*lz;
+	btScalar lx = btScalar(2.) * (halfExtents[0]);
+	btScalar ly = btScalar(2.) * (halfExtents[1]);
+	btScalar lz = btScalar(2.) * (halfExtents[2]);
+	const btScalar x2 = lx * lx;
+	const btScalar y2 = ly * ly;
+	const btScalar z2 = lz * lz;
 	const btScalar scaledmass = mass * btScalar(.08333333);
 
-	inertia[0] = scaledmass * (y2+z2);
-	inertia[1] = scaledmass * (x2+z2);
-	inertia[2] = scaledmass * (x2+y2);
-
+	inertia[0] = scaledmass * (y2 + z2);
+	inertia[1] = scaledmass * (x2 + z2);
+	inertia[2] = scaledmass * (x2 + y2);
 }
 
-btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height)
+btCapsuleShapeX::btCapsuleShapeX(btScalar radius, btScalar height)
 {
+	m_collisionMargin = radius;
 	m_upAxis = 0;
-	m_implicitShapeDimensions.setValue(0.5f*height, radius,radius);
+	m_implicitShapeDimensions.setValue(0.5f * height, radius, radius);
 }
 
-
-
-
-
-
-btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height)
+btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius, btScalar height)
 {
+	m_collisionMargin = radius;
 	m_upAxis = 2;
-	m_implicitShapeDimensions.setValue(radius,radius,0.5f*height);
+	m_implicitShapeDimensions.setValue(radius, radius, 0.5f * height);
 }
-
-
-
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h
index f8c55ace4eb..138d0c0f7c1 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h
@@ -17,108 +17,96 @@ subject to the following restrictions:
 #define BT_CAPSULE_SHAPE_H
 
 #include "btConvexInternalShape.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
-
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"  // for the types
 
 ///The btCapsuleShape represents a capsule around the Y axis, there is also the btCapsuleShapeX aligned around the X axis and btCapsuleShapeZ around the Z axis.
 ///The total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps.
 ///The btCapsuleShape is a convex hull of two spheres. The btMultiSphereShape is a more general collision shape that takes the convex hull of multiple sphere, so it can also represent a capsule when just using two spheres.
-ATTRIBUTE_ALIGNED16(class) btCapsuleShape : public btConvexInternalShape
+ATTRIBUTE_ALIGNED16(class)
+btCapsuleShape : public btConvexInternalShape
 {
 protected:
-	int	m_upAxis;
+	int m_upAxis;
 
 protected:
 	///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses.
-	btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;};
+	btCapsuleShape() : btConvexInternalShape() { m_shapeType = CAPSULE_SHAPE_PROXYTYPE; };
 
 public:
-	
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-	btCapsuleShape(btScalar radius,btScalar height);
+
+	btCapsuleShape(btScalar radius, btScalar height);
 
 	///CollisionShape Interface
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
 
 	/// btConvexShape Interface
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
+
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
 
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
-	
 	virtual void setMargin(btScalar collisionMargin)
 	{
-		//correct the m_implicitShapeDimensions for the margin
-		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
-		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
-		
-		btConvexInternalShape::setMargin(collisionMargin);
-		btVector3 newMargin(getMargin(),getMargin(),getMargin());
-		m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
-
+		//don't override the margin for capsules, their entire radius == margin
+		(void)collisionMargin;
 	}
 
-	virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 	{
-			btVector3 halfExtents(getRadius(),getRadius(),getRadius());
-			halfExtents[m_upAxis] = getRadius() + getHalfHeight();
-			halfExtents += btVector3(getMargin(),getMargin(),getMargin());
-			btMatrix3x3 abs_b = t.getBasis().absolute();  
-			btVector3 center = t.getOrigin();
-            btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
-        
-			aabbMin = center - extent;
-			aabbMax = center + extent;
+		btVector3 halfExtents(getRadius(), getRadius(), getRadius());
+		halfExtents[m_upAxis] = getRadius() + getHalfHeight();
+		btMatrix3x3 abs_b = t.getBasis().absolute();
+		btVector3 center = t.getOrigin();
+		btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
+
+		aabbMin = center - extent;
+		aabbMax = center + extent;
 	}
 
-	virtual const char*	getName()const 
+	virtual const char* getName() const
 	{
 		return "CapsuleShape";
 	}
 
-	int	getUpAxis() const
+	int getUpAxis() const
 	{
 		return m_upAxis;
 	}
 
-	btScalar	getRadius() const
+	btScalar getRadius() const
 	{
-		int radiusAxis = (m_upAxis+2)%3;
+		int radiusAxis = (m_upAxis + 2) % 3;
 		return m_implicitShapeDimensions[radiusAxis];
 	}
 
-	btScalar	getHalfHeight() const
+	btScalar getHalfHeight() const
 	{
 		return m_implicitShapeDimensions[m_upAxis];
 	}
 
-	virtual void	setLocalScaling(const btVector3& scaling)
+	virtual void setLocalScaling(const btVector3& scaling)
 	{
-		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
-		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
-		btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
-
+		btVector3 unScaledImplicitShapeDimensions = m_implicitShapeDimensions / m_localScaling;
 		btConvexInternalShape::setLocalScaling(scaling);
-
-		m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
-
+		m_implicitShapeDimensions = (unScaledImplicitShapeDimensions * scaling);
+		//update m_collisionMargin, since entire radius==margin
+		int radiusAxis = (m_upAxis + 2) % 3;
+		m_collisionMargin = m_implicitShapeDimensions[radiusAxis];
 	}
 
-	virtual btVector3	getAnisotropicRollingFrictionDirection() const
+	virtual btVector3 getAnisotropicRollingFrictionDirection() const
 	{
-		btVector3 aniDir(0,0,0);
-		aniDir[getUpAxis()]=1;
+		btVector3 aniDir(0, 0, 0);
+		aniDir[getUpAxis()] = 1;
 		return aniDir;
 	}
 
-
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
-	SIMD_FORCE_INLINE	void	deSerializeFloat(struct btCapsuleShapeData* dataBuffer);
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 
+	SIMD_FORCE_INLINE void deSerializeFloat(struct btCapsuleShapeData * dataBuffer);
 };
 
 ///btCapsuleShapeX represents a capsule around the Z axis
@@ -126,17 +114,13 @@ public:
 class btCapsuleShapeX : public btCapsuleShape
 {
 public:
+	btCapsuleShapeX(btScalar radius, btScalar height);
 
-	btCapsuleShapeX(btScalar radius,btScalar height);
-		
 	//debugging
-	virtual const char*	getName()const
+	virtual const char* getName() const
 	{
 		return "CapsuleX";
 	}
-
-	
-
 };
 
 ///btCapsuleShapeZ represents a capsule around the Z axis
@@ -144,45 +128,49 @@ public:
 class btCapsuleShapeZ : public btCapsuleShape
 {
 public:
-	btCapsuleShapeZ(btScalar radius,btScalar height);
+	btCapsuleShapeZ(btScalar radius, btScalar height);
 
-		//debugging
-	virtual const char*	getName()const
+	//debugging
+	virtual const char* getName() const
 	{
 		return "CapsuleZ";
 	}
-
-	
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btCapsuleShapeData
+struct btCapsuleShapeData
 {
-	btConvexInternalShapeData	m_convexInternalShapeData;
+	btConvexInternalShapeData m_convexInternalShapeData;
 
-	int	m_upAxis;
+	int m_upAxis;
 
-	char	m_padding[4];
+	char m_padding[4];
 };
 
-SIMD_FORCE_INLINE	int	btCapsuleShape::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btCapsuleShape::calculateSerializeBufferSize() const
 {
 	return sizeof(btCapsuleShapeData);
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btCapsuleShape::serialize(void* dataBuffer, btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char* btCapsuleShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btCapsuleShapeData* shapeData = (btCapsuleShapeData*) dataBuffer;
-	
-	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
+	btCapsuleShapeData* shapeData = (btCapsuleShapeData*)dataBuffer;
+
+	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
 
 	shapeData->m_upAxis = m_upAxis;
-	
+
+	// Fill padding with zeros to appease msan.
+	shapeData->m_padding[0] = 0;
+	shapeData->m_padding[1] = 0;
+	shapeData->m_padding[2] = 0;
+	shapeData->m_padding[3] = 0;
+
 	return "btCapsuleShapeData";
 }
 
-SIMD_FORCE_INLINE	void	btCapsuleShape::deSerializeFloat(btCapsuleShapeData* dataBuffer)
+SIMD_FORCE_INLINE void btCapsuleShape::deSerializeFloat(btCapsuleShapeData* dataBuffer)
 {
 	m_implicitShapeDimensions.deSerializeFloat(dataBuffer->m_convexInternalShapeData.m_implicitShapeDimensions);
 	m_collisionMargin = dataBuffer->m_convexInternalShapeData.m_collisionMargin;
@@ -191,4 +179,4 @@ SIMD_FORCE_INLINE	void	btCapsuleShape::deSerializeFloat(btCapsuleShapeData* data
 	m_upAxis = dataBuffer->m_upAxis;
 }
 
-#endif //BT_CAPSULE_SHAPE_H
+#endif  //BT_CAPSULE_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h
index 474bf1fb499..abd8ab3eb5f 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h
@@ -19,9 +19,6 @@ subject to the following restrictions:
 ///The CONVEX_DISTANCE_MARGIN is a default collision margin for convex collision shapes derived from btConvexInternalShape.
 ///This collision margin is used by Gjk and some other algorithms
 ///Note that when creating small objects, you need to make sure to set a smaller collision margin, using the 'setMargin' API
-#define CONVEX_DISTANCE_MARGIN btScalar(0.04)// btScalar(0.1)//;//btScalar(0.01)
-
-
-
-#endif //BT_COLLISION_MARGIN_H
+#define CONVEX_DISTANCE_MARGIN btScalar(0.04)  // btScalar(0.1)//;//btScalar(0.01)
 
+#endif  //BT_COLLISION_MARGIN_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp
index 39ee21cad73..0b3640a65be 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp
@@ -20,47 +20,44 @@ subject to the following restrictions:
   can be used by probes that are checking whether the
   library is actually installed.
 */
-extern "C" 
+extern "C"
 {
-void btBulletCollisionProbe ();
+	void btBulletCollisionProbe();
 
-void btBulletCollisionProbe () {}
+	void btBulletCollisionProbe() {}
 }
 
-
-
-void	btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) const
+void btCollisionShape::getBoundingSphere(btVector3& center, btScalar& radius) const
 {
 	btTransform tr;
 	tr.setIdentity();
-	btVector3 aabbMin,aabbMax;
+	btVector3 aabbMin, aabbMax;
 
-	getAabb(tr,aabbMin,aabbMax);
+	getAabb(tr, aabbMin, aabbMax);
 
-	radius = (aabbMax-aabbMin).length()*btScalar(0.5);
-	center = (aabbMin+aabbMax)*btScalar(0.5);
+	radius = (aabbMax - aabbMin).length() * btScalar(0.5);
+	center = (aabbMin + aabbMax) * btScalar(0.5);
 }
 
-
-btScalar	btCollisionShape::getContactBreakingThreshold(btScalar defaultContactThreshold) const
+btScalar btCollisionShape::getContactBreakingThreshold(btScalar defaultContactThreshold) const
 {
 	return getAngularMotionDisc() * defaultContactThreshold;
 }
 
-btScalar	btCollisionShape::getAngularMotionDisc() const
+btScalar btCollisionShape::getAngularMotionDisc() const
 {
 	///@todo cache this value, to improve performance
-	btVector3	center;
+	btVector3 center;
 	btScalar disc;
-	getBoundingSphere(center,disc);
+	getBoundingSphere(center, disc);
 	disc += (center).length();
 	return disc;
 }
 
-void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const
+void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans, const btVector3& linvel, const btVector3& angvel, btScalar timeStep, btVector3& temporalAabbMin, btVector3& temporalAabbMax) const
 {
 	//start with static aabb
-	getAabb(curTrans,temporalAabbMin,temporalAabbMax);
+	getAabb(curTrans, temporalAabbMin, temporalAabbMax);
 
 	btScalar temporalAabbMaxx = temporalAabbMax.getX();
 	btScalar temporalAabbMaxy = temporalAabbMax.getY();
@@ -70,50 +67,53 @@ void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const b
 	btScalar temporalAabbMinz = temporalAabbMin.getZ();
 
 	// add linear motion
-	btVector3 linMotion = linvel*timeStep;
+	btVector3 linMotion = linvel * timeStep;
 	///@todo: simd would have a vector max/min operation, instead of per-element access
 	if (linMotion.x() > btScalar(0.))
-		temporalAabbMaxx += linMotion.x(); 
+		temporalAabbMaxx += linMotion.x();
 	else
 		temporalAabbMinx += linMotion.x();
 	if (linMotion.y() > btScalar(0.))
-		temporalAabbMaxy += linMotion.y(); 
+		temporalAabbMaxy += linMotion.y();
 	else
 		temporalAabbMiny += linMotion.y();
 	if (linMotion.z() > btScalar(0.))
-		temporalAabbMaxz += linMotion.z(); 
+		temporalAabbMaxz += linMotion.z();
 	else
 		temporalAabbMinz += linMotion.z();
 
 	//add conservative angular motion
 	btScalar angularMotion = angvel.length() * getAngularMotionDisc() * timeStep;
-	btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion);
-	temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
-	temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);
+	btVector3 angularMotion3d(angularMotion, angularMotion, angularMotion);
+	temporalAabbMin = btVector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz);
+	temporalAabbMax = btVector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz);
 
 	temporalAabbMin -= angularMotion3d;
 	temporalAabbMax += angularMotion3d;
 }
 
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btCollisionShape::serialize(void* dataBuffer, btSerializer* serializer) const
+const char* btCollisionShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btCollisionShapeData* shapeData = (btCollisionShapeData*) dataBuffer;
-	char* name = (char*) serializer->findNameForPointer(this);
+	btCollisionShapeData* shapeData = (btCollisionShapeData*)dataBuffer;
+	char* name = (char*)serializer->findNameForPointer(this);
 	shapeData->m_name = (char*)serializer->getUniquePointer(name);
 	if (shapeData->m_name)
 	{
 		serializer->serializeName(name);
 	}
 	shapeData->m_shapeType = m_shapeType;
-	//shapeData->m_padding//??
+
+	// Fill padding with zeros to appease msan.
+	memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding));
+
 	return "btCollisionShapeData";
 }
 
-void	btCollisionShape::serializeSingleShape(btSerializer* serializer) const
+void btCollisionShape::serializeSingleShape(btSerializer* serializer) const
 {
 	int len = calculateSerializeBufferSize();
-	btChunk* chunk = serializer->allocate(len,1);
+	btChunk* chunk = serializer->allocate(len, 1);
 	const char* structType = serialize(chunk->m_oldPtr, serializer);
-	serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,(void*)this);
+	serializer->finalizeChunk(chunk, structType, BT_SHAPE_CODE, (void*)this);
 }
\ No newline at end of file
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h
index 5e86568005e..c80e105a4d8 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h
@@ -19,12 +19,12 @@ subject to the following restrictions:
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btMatrix3x3.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"  //for the shape types
 class btSerializer;
 
-
 ///The btCollisionShape class provides an interface for collision shapes that can be shared among btCollisionObjects.
-ATTRIBUTE_ALIGNED16(class) btCollisionShape
+ATTRIBUTE_ALIGNED16(class)
+btCollisionShape
 {
 protected:
 	int m_shapeType;
@@ -32,10 +32,9 @@ protected:
 	int m_userIndex;
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1)
+	btCollisionShape() : m_shapeType(INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1)
 	{
 	}
 
@@ -44,50 +43,47 @@ public:
 	}
 
 	///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0;
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const = 0;
 
-	virtual void	getBoundingSphere(btVector3& center,btScalar& radius) const;
+	virtual void getBoundingSphere(btVector3 & center, btScalar & radius) const;
 
-	///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations.
-	virtual btScalar	getAngularMotionDisc() const;
-
-	virtual btScalar	getContactBreakingThreshold(btScalar defaultContactThresholdFactor) const;
+	///getAngularMotionDisc returns the maximum radius needed for Conservative Advancement to handle time-of-impact with rotations.
+	virtual btScalar getAngularMotionDisc() const;
 
+	virtual btScalar getContactBreakingThreshold(btScalar defaultContactThresholdFactor) const;
 
 	///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
 	///result is conservative
-	void calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const;
-
+	void calculateTemporalAabb(const btTransform& curTrans, const btVector3& linvel, const btVector3& angvel, btScalar timeStep, btVector3& temporalAabbMin, btVector3& temporalAabbMax) const;
 
-
-	SIMD_FORCE_INLINE bool	isPolyhedral() const
+	SIMD_FORCE_INLINE bool isPolyhedral() const
 	{
 		return btBroadphaseProxy::isPolyhedral(getShapeType());
 	}
 
-	SIMD_FORCE_INLINE bool	isConvex2d() const
+	SIMD_FORCE_INLINE bool isConvex2d() const
 	{
 		return btBroadphaseProxy::isConvex2d(getShapeType());
 	}
 
-	SIMD_FORCE_INLINE bool	isConvex() const
+	SIMD_FORCE_INLINE bool isConvex() const
 	{
 		return btBroadphaseProxy::isConvex(getShapeType());
 	}
-	SIMD_FORCE_INLINE bool	isNonMoving() const
+	SIMD_FORCE_INLINE bool isNonMoving() const
 	{
 		return btBroadphaseProxy::isNonMoving(getShapeType());
 	}
-	SIMD_FORCE_INLINE bool	isConcave() const
+	SIMD_FORCE_INLINE bool isConcave() const
 	{
 		return btBroadphaseProxy::isConcave(getShapeType());
 	}
-	SIMD_FORCE_INLINE bool	isCompound() const
+	SIMD_FORCE_INLINE bool isCompound() const
 	{
 		return btBroadphaseProxy::isCompound(getShapeType());
 	}
 
-	SIMD_FORCE_INLINE bool	isSoftBody() const
+	SIMD_FORCE_INLINE bool isSoftBody() const
 	{
 		return btBroadphaseProxy::isSoftBody(getShapeType());
 	}
@@ -99,35 +95,35 @@ public:
 	}
 
 #ifndef __SPU__
-	virtual void	setLocalScaling(const btVector3& scaling) =0;
-	virtual const btVector3& getLocalScaling() const =0;
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const = 0;
-
+	virtual void setLocalScaling(const btVector3& scaling) = 0;
+	virtual const btVector3& getLocalScaling() const = 0;
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const = 0;
 
-//debugging support
-	virtual const char*	getName()const =0 ;
-#endif //__SPU__
+	//debugging support
+	virtual const char* getName() const = 0;
+#endif  //__SPU__
 
-	
-	int		getShapeType() const { return m_shapeType; }
+	int getShapeType() const
+	{
+		return m_shapeType;
+	}
 
 	///the getAnisotropicRollingFrictionDirection can be used in combination with setAnisotropicFriction
 	///See Bullet/Demos/RollingFrictionDemo for an example
-	virtual btVector3	getAnisotropicRollingFrictionDirection() const
+	virtual btVector3 getAnisotropicRollingFrictionDirection() const
 	{
-		return btVector3(1,1,1);
+		return btVector3(1, 1, 1);
 	}
-	virtual void	setMargin(btScalar margin) = 0;
-	virtual btScalar	getMargin() const = 0;
+	virtual void setMargin(btScalar margin) = 0;
+	virtual btScalar getMargin() const = 0;
 
-	
 	///optional user data pointer
-	void	setUserPointer(void*  userPtr)
+	void setUserPointer(void* userPtr)
 	{
 		m_userPointer = userPtr;
 	}
 
-	void*	getUserPointer() const
+	void* getUserPointer() const
 	{
 		return m_userPointer;
 	}
@@ -141,16 +137,16 @@ public:
 		return m_userIndex;
 	}
 
-
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
-	virtual void	serializeSingleShape(btSerializer* serializer) const;
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 
-};	
+	virtual void serializeSingleShape(btSerializer * serializer) const;
+};
 
+// clang-format off
+// parser needs * with the name
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btCollisionShapeData
 {
@@ -158,13 +154,10 @@ struct	btCollisionShapeData
 	int		m_shapeType;
 	char	m_padding[4];
 };
-
-SIMD_FORCE_INLINE	int	btCollisionShape::calculateSerializeBufferSize() const
+// clang-format on
+SIMD_FORCE_INLINE int btCollisionShape::calculateSerializeBufferSize() const
 {
 	return sizeof(btCollisionShapeData);
 }
 
-
-
-#endif //BT_COLLISION_SHAPE_H
-
+#endif  //BT_COLLISION_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp
index 1e7f36e0a17..fd7828b1047 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp
@@ -19,26 +19,25 @@ subject to the following restrictions:
 #include "LinearMath/btSerializer.h"
 
 btCompoundShape::btCompoundShape(bool enableDynamicAabbTree, const int initialChildCapacity)
-: m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)),
-m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)),
-m_dynamicAabbTree(0),
-m_updateRevision(1),
-m_collisionMargin(btScalar(0.)),
-m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
+	: m_localAabbMin(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)),
+	  m_localAabbMax(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)),
+	  m_dynamicAabbTree(0),
+	  m_updateRevision(1),
+	  m_collisionMargin(btScalar(0.)),
+	  m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.))
 {
 	m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
 
 	if (enableDynamicAabbTree)
 	{
-		void* mem = btAlignedAlloc(sizeof(btDbvt),16);
-		m_dynamicAabbTree = new(mem) btDbvt();
-		btAssert(mem==m_dynamicAabbTree);
+		void* mem = btAlignedAlloc(sizeof(btDbvt), 16);
+		m_dynamicAabbTree = new (mem) btDbvt();
+		btAssert(mem == m_dynamicAabbTree);
 	}
 
 	m_children.reserve(initialChildCapacity);
 }
 
-
 btCompoundShape::~btCompoundShape()
 {
 	if (m_dynamicAabbTree)
@@ -48,7 +47,7 @@ btCompoundShape::~btCompoundShape()
 	}
 }
 
-void	btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape)
+void btCompoundShape::addChildShape(const btTransform& localTransform, btCollisionShape* shape)
 {
 	m_updateRevision++;
 	//m_childTransforms.push_back(localTransform);
@@ -60,11 +59,10 @@ void	btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio
 	child.m_childShapeType = shape->getShapeType();
 	child.m_childMargin = shape->getMargin();
 
-	
 	//extend the local aabbMin/aabbMax
-	btVector3 localAabbMin,localAabbMax;
-	shape->getAabb(localTransform,localAabbMin,localAabbMax);
-	for (int i=0;i<3;i++)
+	btVector3 localAabbMin, localAabbMax;
+	shape->getAabb(localTransform, localAabbMin, localAabbMax);
+	for (int i = 0; i < 3; i++)
 	{
 		if (m_localAabbMin[i] > localAabbMin[i])
 		{
@@ -74,31 +72,30 @@ void	btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio
 		{
 			m_localAabbMax[i] = localAabbMax[i];
 		}
-
 	}
 	if (m_dynamicAabbTree)
 	{
-		const btDbvtVolume	bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+		const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
 		size_t index = m_children.size();
-		child.m_node = m_dynamicAabbTree->insert(bounds,reinterpret_cast<void*>(index) );
+		child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index));
 	}
 
 	m_children.push_back(child);
-
 }
 
-void	btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform,bool shouldRecalculateLocalAabb)
+void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb)
 {
 	m_children[childIndex].m_transform = newChildTransform;
 
 	if (m_dynamicAabbTree)
 	{
 		///update the dynamic aabb tree
-		btVector3 localAabbMin,localAabbMax;
-		m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax);
-		ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+		btVector3 localAabbMin, localAabbMax;
+		m_children[childIndex].m_childShape->getAabb(newChildTransform, localAabbMin, localAabbMax);
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)
+		bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
 		//int index = m_children.size()-1;
-		m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds);
+		m_dynamicAabbTree->update(m_children[childIndex].m_node, bounds);
 	}
 
 	if (shouldRecalculateLocalAabb)
@@ -110,35 +107,30 @@ void	btCompoundShape::updateChildTransform(int childIndex, const btTransform& ne
 void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
 {
 	m_updateRevision++;
-	btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size());
+	btAssert(childShapeIndex >= 0 && childShapeIndex < m_children.size());
 	if (m_dynamicAabbTree)
 	{
 		m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node);
 	}
-	m_children.swap(childShapeIndex,m_children.size()-1);
-    if (m_dynamicAabbTree) 
+	m_children.swap(childShapeIndex, m_children.size() - 1);
+	if (m_dynamicAabbTree)
 		m_children[childShapeIndex].m_node->dataAsInt = childShapeIndex;
 	m_children.pop_back();
-
 }
 
-
-
 void btCompoundShape::removeChildShape(btCollisionShape* shape)
 {
 	m_updateRevision++;
 	// Find the children containing the shape specified, and remove those children.
 	//note: there might be multiple children using the same shape!
-	for(int i = m_children.size()-1; i >= 0 ; i--)
+	for (int i = m_children.size() - 1; i >= 0; i--)
 	{
-		if(m_children[i].m_childShape == shape)
+		if (m_children[i].m_childShape == shape)
 		{
 			removeChildShapeByIndex(i);
 		}
 	}
 
-
-
 	recalculateLocalAabb();
 }
 
@@ -147,15 +139,15 @@ void btCompoundShape::recalculateLocalAabb()
 	// Recalculate the local aabb
 	// Brute force, it iterates over all the shapes left.
 
-	m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-	m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+	m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+	m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
 
 	//extend the local aabbMin/aabbMax
 	for (int j = 0; j < m_children.size(); j++)
 	{
-		btVector3 localAabbMin,localAabbMax;
+		btVector3 localAabbMin, localAabbMax;
 		m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax);
-		for (int i=0;i<3;i++)
+		for (int i = 0; i < 3; i++)
 		{
 			if (m_localAabbMin[i] > localAabbMin[i])
 				m_localAabbMin[i] = localAabbMin[i];
@@ -166,54 +158,48 @@ void btCompoundShape::recalculateLocalAabb()
 }
 
 ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
-void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+void btCompoundShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const
 {
-	btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
-	btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
-	
+	btVector3 localHalfExtents = btScalar(0.5) * (m_localAabbMax - m_localAabbMin);
+	btVector3 localCenter = btScalar(0.5) * (m_localAabbMax + m_localAabbMin);
+
 	//avoid an illegal AABB when there are no children
 	if (!m_children.size())
 	{
-		localHalfExtents.setValue(0,0,0);
-		localCenter.setValue(0,0,0);
+		localHalfExtents.setValue(0, 0, 0);
+		localCenter.setValue(0, 0, 0);
 	}
-	localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
-		
+	localHalfExtents += btVector3(getMargin(), getMargin(), getMargin());
 
 	btMatrix3x3 abs_b = trans.getBasis().absolute();
 
 	btVector3 center = trans(localCenter);
 
-    btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
-	aabbMin = center-extent;
-	aabbMax = center+extent;
-	
+	btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
+	aabbMin = center - extent;
+	aabbMax = center + extent;
 }
 
-void	btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btCompoundShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
 	//approximation: take the inertia from the aabb for now
 	btTransform ident;
 	ident.setIdentity();
-	btVector3 aabbMin,aabbMax;
-	getAabb(ident,aabbMin,aabbMax);
+	btVector3 aabbMin, aabbMax;
+	getAabb(ident, aabbMin, aabbMax);
 
-	btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
+	btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
 
-	btScalar lx=btScalar(2.)*(halfExtents.x());
-	btScalar ly=btScalar(2.)*(halfExtents.y());
-	btScalar lz=btScalar(2.)*(halfExtents.z());
-
-	inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz);
-	inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz);
-	inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly);
+	btScalar lx = btScalar(2.) * (halfExtents.x());
+	btScalar ly = btScalar(2.) * (halfExtents.y());
+	btScalar lz = btScalar(2.) * (halfExtents.z());
 
+	inertia[0] = mass / (btScalar(12.0)) * (ly * ly + lz * lz);
+	inertia[1] = mass / (btScalar(12.0)) * (lx * lx + lz * lz);
+	inertia[2] = mass / (btScalar(12.0)) * (lx * lx + ly * ly);
 }
 
-
-
-
-void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
+void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const
 {
 	int n = m_children.size();
 
@@ -223,18 +209,18 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf
 
 	for (k = 0; k < n; k++)
 	{
-		btAssert(masses[k]>0);
+		btAssert(masses[k] > 0);
 		center += m_children[k].m_transform.getOrigin() * masses[k];
 		totalMass += masses[k];
 	}
 
-	btAssert(totalMass>0);
+	btAssert(totalMass > 0);
 
 	center /= totalMass;
 	principal.setOrigin(center);
 
 	btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
-	for ( k = 0; k < n; k++)
+	for (k = 0; k < n; k++)
 	{
 		btVector3 i;
 		m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
@@ -259,8 +245,8 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf
 		j[0].setValue(o2, 0, 0);
 		j[1].setValue(0, o2, 0);
 		j[2].setValue(0, 0, o2);
-		j[0] += o * -o.x(); 
-		j[1] += o * -o.y(); 
+		j[0] += o * -o.x();
+		j[1] += o * -o.y();
 		j[2] += o * -o.z();
 
 		//add inertia tensor of pointmass
@@ -273,59 +259,50 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf
 	inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
 }
 
-
-
-
-
 void btCompoundShape::setLocalScaling(const btVector3& scaling)
 {
-
-	for(int i = 0; i < m_children.size(); i++)
+	for (int i = 0; i < m_children.size(); i++)
 	{
 		btTransform childTrans = getChildTransform(i);
 		btVector3 childScale = m_children[i].m_childShape->getLocalScaling();
-//		childScale = childScale * (childTrans.getBasis() * scaling);
+		//		childScale = childScale * (childTrans.getBasis() * scaling);
 		childScale = childScale * scaling / m_localScaling;
 		m_children[i].m_childShape->setLocalScaling(childScale);
 		childTrans.setOrigin((childTrans.getOrigin()) * scaling / m_localScaling);
-		updateChildTransform(i, childTrans,false);
+		updateChildTransform(i, childTrans, false);
 	}
-	
+
 	m_localScaling = scaling;
 	recalculateLocalAabb();
-
 }
 
-
 void btCompoundShape::createAabbTreeFromChildren()
 {
-    if ( !m_dynamicAabbTree )
-    {
-        void* mem = btAlignedAlloc(sizeof(btDbvt),16);
-        m_dynamicAabbTree = new(mem) btDbvt();
-        btAssert(mem==m_dynamicAabbTree);
+	if (!m_dynamicAabbTree)
+	{
+		void* mem = btAlignedAlloc(sizeof(btDbvt), 16);
+		m_dynamicAabbTree = new (mem) btDbvt();
+		btAssert(mem == m_dynamicAabbTree);
 
-        for ( int index = 0; index < m_children.size(); index++ )
-        {
-            btCompoundShapeChild &child = m_children[index];
+		for (int index = 0; index < m_children.size(); index++)
+		{
+			btCompoundShapeChild& child = m_children[index];
 
-            //extend the local aabbMin/aabbMax
-            btVector3 localAabbMin,localAabbMax;
-            child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax);
+			//extend the local aabbMin/aabbMax
+			btVector3 localAabbMin, localAabbMax;
+			child.m_childShape->getAabb(child.m_transform, localAabbMin, localAabbMax);
 
-            const btDbvtVolume  bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+			const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
 			size_t index2 = index;
-            child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2) );
-        }
-    }
+			child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2));
+		}
+	}
 }
 
-
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const
+const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-
-	btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer;
+	btCompoundShapeData* shapeData = (btCompoundShapeData*)dataBuffer;
 	btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer);
 
 	shapeData->m_collisionMargin = float(m_collisionMargin);
@@ -333,27 +310,26 @@ const char*	btCompoundShape::serialize(void* dataBuffer, btSerializer* serialize
 	shapeData->m_childShapePtr = 0;
 	if (shapeData->m_numChildShapes)
 	{
-		btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData),shapeData->m_numChildShapes);
+		btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData), shapeData->m_numChildShapes);
 		btCompoundShapeChildData* memPtr = (btCompoundShapeChildData*)chunk->m_oldPtr;
 		shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr);
 
-		for (int i=0;i<shapeData->m_numChildShapes;i++,memPtr++)
+		for (int i = 0; i < shapeData->m_numChildShapes; i++, memPtr++)
 		{
 			memPtr->m_childMargin = float(m_children[i].m_childMargin);
 			memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape);
 			//don't serialize shapes that already have been serialized
 			if (!serializer->findPointer(m_children[i].m_childShape))
 			{
-				btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(),1);
-				const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr,serializer);
-				serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,m_children[i].m_childShape);
-			} 
+				btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(), 1);
+				const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr, serializer);
+				serializer->finalizeChunk(chunk, structType, BT_SHAPE_CODE, m_children[i].m_childShape);
+			}
 
 			memPtr->m_childShapeType = m_children[i].m_childShapeType;
 			m_children[i].m_transform.serializeFloat(memPtr->m_transform);
 		}
-		serializer->finalizeChunk(chunk,"btCompoundShapeChildData",BT_ARRAY_CODE,chunk->m_oldPtr);
+		serializer->finalizeChunk(chunk, "btCompoundShapeChildData", BT_ARRAY_CODE, chunk->m_oldPtr);
 	}
 	return "btCompoundShapeData";
 }
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h
index 4eef8dba306..7e2d0eb8170 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h
@@ -27,45 +27,47 @@ subject to the following restrictions:
 //class btOptimizedBvh;
 struct btDbvt;
 
-ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild
+ATTRIBUTE_ALIGNED16(struct)
+btCompoundShapeChild
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btTransform			m_transform;
-	btCollisionShape*	m_childShape;
-	int					m_childShapeType;
-	btScalar			m_childMargin;
-	struct btDbvtNode*	m_node;
+	btTransform m_transform;
+	btCollisionShape* m_childShape;
+	int m_childShapeType;
+	btScalar m_childMargin;
+	struct btDbvtNode* m_node;
 };
 
 SIMD_FORCE_INLINE bool operator==(const btCompoundShapeChild& c1, const btCompoundShapeChild& c2)
 {
-	return  ( c1.m_transform      == c2.m_transform &&
-		c1.m_childShape     == c2.m_childShape &&
-		c1.m_childShapeType == c2.m_childShapeType &&
-		c1.m_childMargin    == c2.m_childMargin );
+	return (c1.m_transform == c2.m_transform &&
+			c1.m_childShape == c2.m_childShape &&
+			c1.m_childShapeType == c2.m_childShapeType &&
+			c1.m_childMargin == c2.m_childMargin);
 }
 
 /// The btCompoundShape allows to store multiple other btCollisionShapes
 /// This allows for moving concave collision objects. This is more general then the static concave btBvhTriangleMeshShape.
-/// It has an (optional) dynamic aabb tree to accelerate early rejection tests. 
+/// It has an (optional) dynamic aabb tree to accelerate early rejection tests.
 /// @todo: This aabb tree can also be use to speed up ray tests on btCompoundShape, see http://code.google.com/p/bullet/issues/detail?id=25
 /// Currently, removal of child shapes is only supported when disabling the aabb tree (pass 'false' in the constructor of btCompoundShape)
-ATTRIBUTE_ALIGNED16(class) btCompoundShape	: public btCollisionShape
+ATTRIBUTE_ALIGNED16(class)
+btCompoundShape : public btCollisionShape
 {
 protected:
 	btAlignedObjectArray<btCompoundShapeChild> m_children;
-	btVector3						m_localAabbMin;
-	btVector3						m_localAabbMax;
+	btVector3 m_localAabbMin;
+	btVector3 m_localAabbMax;
 
-	btDbvt*							m_dynamicAabbTree;
+	btDbvt* m_dynamicAabbTree;
 
 	///increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be updated
-	int								m_updateRevision;
+	int m_updateRevision;
 
-	btScalar	m_collisionMargin;
+	btScalar m_collisionMargin;
 
-	btVector3	m_localScaling;
+	btVector3 m_localScaling;
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -74,17 +76,16 @@ public:
 
 	virtual ~btCompoundShape();
 
-	void	addChildShape(const btTransform& localTransform,btCollisionShape* shape);
+	void addChildShape(const btTransform& localTransform, btCollisionShape* shape);
 
 	/// Remove all children shapes that contain the specified shape
-	virtual void removeChildShape(btCollisionShape* shape);
+	virtual void removeChildShape(btCollisionShape * shape);
 
 	void removeChildShapeByIndex(int childShapeindex);
 
-
-	int		getNumChildShapes() const
+	int getNumChildShapes() const
 	{
-		return int (m_children.size());
+		return int(m_children.size());
 	}
 
 	btCollisionShape* getChildShape(int index)
@@ -96,18 +97,17 @@ public:
 		return m_children[index].m_childShape;
 	}
 
-	btTransform&	getChildTransform(int index)
+	btTransform& getChildTransform(int index)
 	{
 		return m_children[index].m_transform;
 	}
-	const btTransform&	getChildTransform(int index) const
+	const btTransform& getChildTransform(int index) const
 	{
 		return m_children[index].m_transform;
 	}
 
 	///set a new transform for a child, and update internal data structures (local aabb and dynamic tree)
-	void	updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb = true);
-
+	void updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb = true);
 
 	btCompoundShapeChild* getChildList()
 	{
@@ -115,40 +115,40 @@ public:
 	}
 
 	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
-	virtual	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
 	/** Re-calculate the local Aabb. Is called at the end of removeChildShapes. 
 	Use this yourself if you modify the children or their transforms. */
-	virtual void recalculateLocalAabb(); 
+	virtual void recalculateLocalAabb();
 
-	virtual void	setLocalScaling(const btVector3& scaling);
+	virtual void setLocalScaling(const btVector3& scaling);
 
-	virtual const btVector3& getLocalScaling() const 
+	virtual const btVector3& getLocalScaling() const
 	{
 		return m_localScaling;
 	}
 
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
 
-	virtual void	setMargin(btScalar margin)
+	virtual void setMargin(btScalar margin)
 	{
 		m_collisionMargin = margin;
 	}
-	virtual btScalar	getMargin() const
+	virtual btScalar getMargin() const
 	{
 		return m_collisionMargin;
 	}
-	virtual const char*	getName()const
+	virtual const char* getName() const
 	{
 		return "Compound";
 	}
 
-	const btDbvt*	getDynamicAabbTree() const
+	const btDbvt* getDynamicAabbTree() const
 	{
 		return m_dynamicAabbTree;
 	}
-	
-	btDbvt*	getDynamicAabbTree()
+
+	btDbvt* getDynamicAabbTree()
 	{
 		return m_dynamicAabbTree;
 	}
@@ -160,21 +160,21 @@ public:
 	///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound
 	///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
 	///of the collision object by the principal transform.
-	void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const;
+	void calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const;
 
-	int	getUpdateRevision() const
+	int getUpdateRevision() const
 	{
 		return m_updateRevision;
 	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
+// clang-format off
+
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct btCompoundShapeChildData
 {
@@ -197,16 +197,11 @@ struct	btCompoundShapeData
 
 };
 
+// clang-format on
 
-SIMD_FORCE_INLINE	int	btCompoundShape::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btCompoundShape::calculateSerializeBufferSize() const
 {
 	return sizeof(btCompoundShapeData);
 }
 
-
-
-
-
-
-
-#endif //BT_COMPOUND_SHAPE_H
+#endif  //BT_COMPOUND_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp
index 58ff84a5b02..5d396844dd8 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp
@@ -13,15 +13,12 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btConcaveShape.h"
 
 btConcaveShape::btConcaveShape() : m_collisionMargin(btScalar(0.))
 {
-
 }
 
 btConcaveShape::~btConcaveShape()
 {
-
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h
index 2917cc5b60f..716624e182b 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h
@@ -17,12 +17,13 @@ subject to the following restrictions:
 #define BT_CONCAVE_SHAPE_H
 
 #include "btCollisionShape.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"  // for the types
 #include "btTriangleCallback.h"
 
 /// PHY_ScalarType enumerates possible scalar types.
 /// See the btStridingMeshInterface or btHeightfieldTerrainShape for its use
-typedef enum PHY_ScalarType {
+typedef enum PHY_ScalarType
+{
 	PHY_FLOAT,
 	PHY_DOUBLE,
 	PHY_INTEGER,
@@ -33,30 +34,29 @@ typedef enum PHY_ScalarType {
 
 ///The btConcaveShape class provides an interface for non-moving (static) concave shapes.
 ///It has been implemented by the btStaticPlaneShape, btBvhTriangleMeshShape and btHeightfieldTerrainShape.
-ATTRIBUTE_ALIGNED16(class) btConcaveShape : public btCollisionShape
+ATTRIBUTE_ALIGNED16(class)
+btConcaveShape : public btCollisionShape
 {
 protected:
 	btScalar m_collisionMargin;
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	btConcaveShape();
 
 	virtual ~btConcaveShape();
 
-	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const = 0;
+	virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const = 0;
 
-	virtual btScalar getMargin() const {
+	virtual btScalar getMargin() const
+	{
 		return m_collisionMargin;
 	}
 	virtual void setMargin(btScalar collisionMargin)
 	{
 		m_collisionMargin = collisionMargin;
 	}
-
-
-
 };
 
-#endif //BT_CONCAVE_SHAPE_H
+#endif  //BT_CONCAVE_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp
index 2d83c8bfbac..64a6f272ca5 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp
@@ -15,11 +15,9 @@ subject to the following restrictions:
 
 #include "btConeShape.h"
 
-
-
-btConeShape::btConeShape (btScalar radius,btScalar height): btConvexInternalShape (),
-m_radius (radius),
-m_height(height)
+btConeShape::btConeShape(btScalar radius, btScalar height) : btConvexInternalShape(),
+															 m_radius(radius),
+															 m_height(height)
 {
 	m_shapeType = CONE_SHAPE_PROXYTYPE;
 	setConeUpIndex(1);
@@ -27,42 +25,40 @@ m_height(height)
 	m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height));
 }
 
-btConeShapeZ::btConeShapeZ (btScalar radius,btScalar height):
-btConeShape(radius,height)
+btConeShapeZ::btConeShapeZ(btScalar radius, btScalar height) : btConeShape(radius, height)
 {
 	setConeUpIndex(2);
 }
 
-btConeShapeX::btConeShapeX (btScalar radius,btScalar height):
-btConeShape(radius,height)
+btConeShapeX::btConeShapeX(btScalar radius, btScalar height) : btConeShape(radius, height)
 {
 	setConeUpIndex(0);
 }
 
 ///choose upAxis index
-void	btConeShape::setConeUpIndex(int upIndex)
+void btConeShape::setConeUpIndex(int upIndex)
 {
 	switch (upIndex)
 	{
-	case 0:
+		case 0:
 			m_coneIndices[0] = 1;
 			m_coneIndices[1] = 0;
 			m_coneIndices[2] = 2;
-		break;
-	case 1:
+			break;
+		case 1:
 			m_coneIndices[0] = 0;
 			m_coneIndices[1] = 1;
 			m_coneIndices[2] = 2;
-		break;
-	case 2:
+			break;
+		case 2:
 			m_coneIndices[0] = 0;
 			m_coneIndices[1] = 2;
 			m_coneIndices[2] = 1;
-		break;
-	default:
-		btAssert(0);
+			break;
+		default:
+			btAssert(0);
 	};
-	
+
 	m_implicitShapeDimensions[m_coneIndices[0]] = m_radius;
 	m_implicitShapeDimensions[m_coneIndices[1]] = m_height;
 	m_implicitShapeDimensions[m_coneIndices[2]] = m_radius;
@@ -70,72 +66,71 @@ void	btConeShape::setConeUpIndex(int upIndex)
 
 btVector3 btConeShape::coneLocalSupport(const btVector3& v) const
 {
-	
 	btScalar halfHeight = m_height * btScalar(0.5);
 
- if (v[m_coneIndices[1]] > v.length() * m_sinAngle)
- {
-	btVector3 tmp;
-
-	tmp[m_coneIndices[0]] = btScalar(0.);
-	tmp[m_coneIndices[1]] = halfHeight;
-	tmp[m_coneIndices[2]] = btScalar(0.);
-	return tmp;
- }
-  else {
-    btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]);
-    if (s > SIMD_EPSILON) {
-      btScalar d = m_radius / s;
-	  btVector3 tmp;
-	  tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d;
-	  tmp[m_coneIndices[1]] = -halfHeight;
-	  tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d;
-	  return tmp;
-    }
-    else  {
+	if (v[m_coneIndices[1]] > v.length() * m_sinAngle)
+	{
 		btVector3 tmp;
+
 		tmp[m_coneIndices[0]] = btScalar(0.);
-		tmp[m_coneIndices[1]] = -halfHeight;
+		tmp[m_coneIndices[1]] = halfHeight;
 		tmp[m_coneIndices[2]] = btScalar(0.);
 		return tmp;
 	}
-  }
-
+	else
+	{
+		btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]);
+		if (s > SIMD_EPSILON)
+		{
+			btScalar d = m_radius / s;
+			btVector3 tmp;
+			tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d;
+			tmp[m_coneIndices[1]] = -halfHeight;
+			tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d;
+			return tmp;
+		}
+		else
+		{
+			btVector3 tmp;
+			tmp[m_coneIndices[0]] = btScalar(0.);
+			tmp[m_coneIndices[1]] = -halfHeight;
+			tmp[m_coneIndices[2]] = btScalar(0.);
+			return tmp;
+		}
+	}
 }
 
-btVector3	btConeShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
+btVector3 btConeShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 {
-		return coneLocalSupport(vec);
+	return coneLocalSupport(vec);
 }
 
-void	btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 {
-	for (int i=0;i<numVectors;i++)
+	for (int i = 0; i < numVectors; i++)
 	{
 		const btVector3& vec = vectors[i];
 		supportVerticesOut[i] = coneLocalSupport(vec);
 	}
 }
 
-
-btVector3	btConeShape::localGetSupportingVertex(const btVector3& vec)  const
+btVector3 btConeShape::localGetSupportingVertex(const btVector3& vec) const
 {
 	btVector3 supVertex = coneLocalSupport(vec);
-	if ( getMargin()!=btScalar(0.) )
+	if (getMargin() != btScalar(0.))
 	{
 		btVector3 vecnorm = vec;
-		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+		if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON))
 		{
-			vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
-		} 
+			vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.));
+		}
 		vecnorm.normalize();
-		supVertex+= getMargin() * vecnorm;
+		supVertex += getMargin() * vecnorm;
 	}
 	return supVertex;
 }
 
-
-void	btConeShape::setLocalScaling(const btVector3& scaling)
+void btConeShape::setLocalScaling(const btVector3& scaling)
 {
 	int axis = m_coneIndices[1];
 	int r1 = m_coneIndices[0];
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h
index 4a0df0d558b..ee6786c8074 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h
@@ -17,155 +17,157 @@ subject to the following restrictions:
 #define BT_CONE_MINKOWSKI_H
 
 #include "btConvexInternalShape.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"  // for the types
 
 ///The btConeShape implements a cone shape primitive, centered around the origin and aligned with the Y axis. The btConeShapeX is aligned around the X axis and btConeShapeZ around the Z axis.
-ATTRIBUTE_ALIGNED16(class) btConeShape : public btConvexInternalShape
+ATTRIBUTE_ALIGNED16(class)
+btConeShape : public btConvexInternalShape
 
 {
-
 	btScalar m_sinAngle;
 	btScalar m_radius;
 	btScalar m_height;
-	int		m_coneIndices[3];
+	int m_coneIndices[3];
 	btVector3 coneLocalSupport(const btVector3& v) const;
 
-
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-	btConeShape (btScalar radius,btScalar height);
-	
-	virtual btVector3	localGetSupportingVertex(const btVector3& vec) const;
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
 
-	btScalar getRadius() const { return m_radius;}
-	btScalar getHeight() const { return m_height;}
+	btConeShape(btScalar radius, btScalar height);
 
+	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
 
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const
+	btScalar getRadius() const { return m_radius; }
+	btScalar getHeight() const { return m_height; }
+
+	void setRadius(const btScalar radius)
+	{
+		m_radius = radius;
+	}
+	void setHeight(const btScalar height)
+	{
+		m_height = height;
+	}
+
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const
 	{
 		btTransform identity;
 		identity.setIdentity();
-		btVector3 aabbMin,aabbMax;
-		getAabb(identity,aabbMin,aabbMax);
-
-		btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
+		btVector3 aabbMin, aabbMax;
 
-		btScalar margin = getMargin();
+		getAabb(identity, aabbMin, aabbMax);  // This already contains the margin
+		btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
 
-		btScalar lx=btScalar(2.)*(halfExtents.x()+margin);
-		btScalar ly=btScalar(2.)*(halfExtents.y()+margin);
-		btScalar lz=btScalar(2.)*(halfExtents.z()+margin);
-		const btScalar x2 = lx*lx;
-		const btScalar y2 = ly*ly;
-		const btScalar z2 = lz*lz;
+		btScalar lx = btScalar(2.) * (halfExtents.x());
+		btScalar ly = btScalar(2.) * (halfExtents.y());
+		btScalar lz = btScalar(2.) * (halfExtents.z());
+		const btScalar x2 = lx * lx;
+		const btScalar y2 = ly * ly;
+		const btScalar z2 = lz * lz;
 		const btScalar scaledmass = mass * btScalar(0.08333333);
 
-		inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+		inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2));
+
+		//		inertia.x() = scaledmass * (y2+z2);
+		//		inertia.y() = scaledmass * (x2+z2);
+		//		inertia.z() = scaledmass * (x2+y2);
+	}
 
-//		inertia.x() = scaledmass * (y2+z2);
-//		inertia.y() = scaledmass * (x2+z2);
-//		inertia.z() = scaledmass * (x2+y2);
+	virtual const char* getName() const
+	{
+		return "Cone";
 	}
 
+	///choose upAxis index
+	void setConeUpIndex(int upIndex);
 
-		virtual const char*	getName()const 
-		{
-			return "Cone";
-		}
-		
-		///choose upAxis index
-		void	setConeUpIndex(int upIndex);
-		
-		int	getConeUpIndex() const
-		{
-			return m_coneIndices[1];
-		}
-
-	virtual btVector3	getAnisotropicRollingFrictionDirection() const
+	int getConeUpIndex() const
 	{
-		return btVector3 (0,1,0);
+		return m_coneIndices[1];
 	}
 
-	virtual void	setLocalScaling(const btVector3& scaling);
-	
-	
-	virtual	int	calculateSerializeBufferSize() const;
-	
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
-	
+	virtual btVector3 getAnisotropicRollingFrictionDirection() const
+	{
+		return btVector3(0, 1, 0);
+	}
+
+	virtual void setLocalScaling(const btVector3& scaling);
+
+	virtual int calculateSerializeBufferSize() const;
 
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
 ///btConeShape implements a Cone shape, around the X axis
 class btConeShapeX : public btConeShape
 {
-	public:
-		btConeShapeX(btScalar radius,btScalar height);
+public:
+	btConeShapeX(btScalar radius, btScalar height);
 
-	virtual btVector3	getAnisotropicRollingFrictionDirection() const
+	virtual btVector3 getAnisotropicRollingFrictionDirection() const
 	{
-		return btVector3 (1,0,0);
+		return btVector3(1, 0, 0);
 	}
 
 	//debugging
-	virtual const char*	getName()const
+	virtual const char* getName() const
 	{
 		return "ConeX";
 	}
-	
-	
 };
 
 ///btConeShapeZ implements a Cone shape, around the Z axis
 class btConeShapeZ : public btConeShape
 {
 public:
-	btConeShapeZ(btScalar radius,btScalar height);
+	btConeShapeZ(btScalar radius, btScalar height);
 
-	virtual btVector3	getAnisotropicRollingFrictionDirection() const
+	virtual btVector3 getAnisotropicRollingFrictionDirection() const
 	{
-		return btVector3 (0,0,1);
+		return btVector3(0, 0, 1);
 	}
 
 	//debugging
-	virtual const char*	getName()const
+	virtual const char* getName() const
 	{
 		return "ConeZ";
 	}
-	
-	
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btConeShapeData
+struct btConeShapeData
 {
-	btConvexInternalShapeData	m_convexInternalShapeData;
-	
-	int	m_upIndex;
-	
-	char	m_padding[4];
+	btConvexInternalShapeData m_convexInternalShapeData;
+
+	int m_upIndex;
+
+	char m_padding[4];
 };
 
-SIMD_FORCE_INLINE	int	btConeShape::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btConeShape::calculateSerializeBufferSize() const
 {
 	return sizeof(btConeShapeData);
 }
 
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btConeShape::serialize(void* dataBuffer, btSerializer* serializer) const
+SIMD_FORCE_INLINE const char* btConeShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btConeShapeData* shapeData = (btConeShapeData*) dataBuffer;
-	
-	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
-	
+	btConeShapeData* shapeData = (btConeShapeData*)dataBuffer;
+
+	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
+
 	shapeData->m_upIndex = m_coneIndices[1];
-	
+
+	// Fill padding with zeros to appease msan.
+	shapeData->m_padding[0] = 0;
+	shapeData->m_padding[1] = 0;
+	shapeData->m_padding[2] = 0;
+	shapeData->m_padding[3] = 0;
+
 	return "btConeShapeData";
 }
 
-#endif //BT_CONE_MINKOWSKI_H
-
+#endif  //BT_CONE_MINKOWSKI_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp
index 10ea3e981af..7d3d1d362fb 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp
@@ -15,54 +15,48 @@ subject to the following restrictions:
 
 #include "btConvex2dShape.h"
 
-btConvex2dShape::btConvex2dShape(	btConvexShape* convexChildShape):
-btConvexShape (), m_childConvexShape(convexChildShape)
+btConvex2dShape::btConvex2dShape(btConvexShape* convexChildShape) : btConvexShape(), m_childConvexShape(convexChildShape)
 {
 	m_shapeType = CONVEX_2D_SHAPE_PROXYTYPE;
 }
-	
+
 btConvex2dShape::~btConvex2dShape()
 {
 }
 
-	
-
-btVector3	btConvex2dShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+btVector3 btConvex2dShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 {
 	return m_childConvexShape->localGetSupportingVertexWithoutMargin(vec);
 }
 
-void	btConvex2dShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btConvex2dShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 {
-	m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors);
+	m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors, supportVerticesOut, numVectors);
 }
 
-
-btVector3	btConvex2dShape::localGetSupportingVertex(const btVector3& vec)const
+btVector3 btConvex2dShape::localGetSupportingVertex(const btVector3& vec) const
 {
 	return m_childConvexShape->localGetSupportingVertex(vec);
 }
 
-
-void	btConvex2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btConvex2dShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
 	///this linear upscaling is not realistic, but we don't deal with large mass ratios...
-	m_childConvexShape->calculateLocalInertia(mass,inertia);
+	m_childConvexShape->calculateLocalInertia(mass, inertia);
 }
 
-
-	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
-void btConvex2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+void btConvex2dShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 {
-	m_childConvexShape->getAabb(t,aabbMin,aabbMax);
+	m_childConvexShape->getAabb(t, aabbMin, aabbMax);
 }
 
-void btConvex2dShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+void btConvex2dShape::getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 {
-	m_childConvexShape->getAabbSlow(t,aabbMin,aabbMax);
+	m_childConvexShape->getAabbSlow(t, aabbMin, aabbMax);
 }
 
-void	btConvex2dShape::setLocalScaling(const btVector3& scaling) 
+void btConvex2dShape::setLocalScaling(const btVector3& scaling)
 {
 	m_childConvexShape->setLocalScaling(scaling);
 }
@@ -72,21 +66,21 @@ const btVector3& btConvex2dShape::getLocalScaling() const
 	return m_childConvexShape->getLocalScaling();
 }
 
-void	btConvex2dShape::setMargin(btScalar margin)
+void btConvex2dShape::setMargin(btScalar margin)
 {
 	m_childConvexShape->setMargin(margin);
 }
-btScalar	btConvex2dShape::getMargin() const
+btScalar btConvex2dShape::getMargin() const
 {
 	return m_childConvexShape->getMargin();
 }
 
-int		btConvex2dShape::getNumPreferredPenetrationDirections() const
+int btConvex2dShape::getNumPreferredPenetrationDirections() const
 {
 	return m_childConvexShape->getNumPreferredPenetrationDirections();
 }
-	
-void	btConvex2dShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+
+void btConvex2dShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
 {
-	m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector);
+	m_childConvexShape->getPreferredPenetrationDirection(index, penetrationVector);
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h
index bbd1caf42fb..cd4f1ef7b89 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h
@@ -17,66 +17,61 @@ subject to the following restrictions:
 #define BT_CONVEX_2D_SHAPE_H
 
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"  // for the types
 
 ///The btConvex2dShape allows to use arbitrary convex shapes as 2d convex shapes, with the Z component assumed to be 0.
 ///For 2d boxes, the btBox2dShape is recommended.
-ATTRIBUTE_ALIGNED16(class) btConvex2dShape : public btConvexShape
+ATTRIBUTE_ALIGNED16(class)
+btConvex2dShape : public btConvexShape
 {
-	btConvexShape*	m_childConvexShape;
+	btConvexShape* m_childConvexShape;
 
-	public:
-	
+public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-	btConvex2dShape(	btConvexShape* convexChildShape);
-	
+
+	btConvex2dShape(btConvexShape * convexChildShape);
+
 	virtual ~btConvex2dShape();
-	
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
 
-	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
+
+	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
 
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
 
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
 
-	btConvexShape*	getChildShape() 
+	btConvexShape* getChildShape()
 	{
 		return m_childConvexShape;
 	}
 
-	const btConvexShape*	getChildShape() const
+	const btConvexShape* getChildShape() const
 	{
 		return m_childConvexShape;
 	}
 
-	virtual const char*	getName()const 
+	virtual const char* getName() const
 	{
 		return "Convex2dShape";
 	}
-	
-
 
 	///////////////////////////
 
-
 	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
-	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
-
-	virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+	void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
-	virtual void	setLocalScaling(const btVector3& scaling) ;
-	virtual const btVector3& getLocalScaling() const ;
+	virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
-	virtual void	setMargin(btScalar margin);
-	virtual btScalar	getMargin() const;
+	virtual void setLocalScaling(const btVector3& scaling);
+	virtual const btVector3& getLocalScaling() const;
 
-	virtual int		getNumPreferredPenetrationDirections() const;
-	
-	virtual void	getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const;
+	virtual void setMargin(btScalar margin);
+	virtual btScalar getMargin() const;
 
+	virtual int getNumPreferredPenetrationDirections() const;
 
+	virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const;
 };
 
-#endif //BT_CONVEX_2D_SHAPE_H
+#endif  //BT_CONVEX_2D_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
index 02ea5033b15..703de45922e 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
@@ -13,24 +13,26 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-//#if defined (_WIN32) || defined (__i386__)
-//#define BT_USE_SSE_IN_API
-//#endif
+#if defined(_WIN32) || defined(__i386__)
+#define BT_USE_SSE_IN_API
+#endif
 
 #include "btConvexHullShape.h"
 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
 
 #include "LinearMath/btQuaternion.h"
 #include "LinearMath/btSerializer.h"
+#include "btConvexPolyhedron.h"
+#include "LinearMath/btConvexHullComputer.h"
 
-btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexAabbCachingShape ()
+btConvexHullShape ::btConvexHullShape(const btScalar* points, int numPoints, int stride) : btPolyhedralConvexAabbCachingShape()
 {
 	m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE;
 	m_unscaledPoints.resize(numPoints);
 
 	unsigned char* pointsAddress = (unsigned char*)points;
 
-	for (int i=0;i<numPoints;i++)
+	for (int i = 0; i < numPoints; i++)
 	{
 		btScalar* point = (btScalar*)pointsAddress;
 		m_unscaledPoints[i] = btVector3(point[0], point[1], point[2]);
@@ -38,11 +40,8 @@ btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int
 	}
 
 	recalcLocalAabb();
-
 }
 
-
-
 void btConvexHullShape::setLocalScaling(const btVector3& scaling)
 {
 	m_localScaling = scaling;
@@ -54,83 +53,81 @@ void btConvexHullShape::addPoint(const btVector3& point, bool recalculateLocalAa
 	m_unscaledPoints.push_back(point);
 	if (recalculateLocalAabb)
 		recalcLocalAabb();
-
 }
 
-btVector3	btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 {
-	btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
+	btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.));
 	btScalar maxDot = btScalar(-BT_LARGE_FLOAT);
 
-    // Here we take advantage of dot(a, b*c) = dot(a*b, c).  Note: This is true mathematically, but not numerically. 
-    if( 0 < m_unscaledPoints.size() )
-    {
-        btVector3 scaled = vec * m_localScaling;
-        int index = (int) scaled.maxDot( &m_unscaledPoints[0], m_unscaledPoints.size(), maxDot); // FIXME: may violate encapsulation of m_unscaledPoints
-        return m_unscaledPoints[index] * m_localScaling;
-    }
+	// Here we take advantage of dot(a, b*c) = dot(a*b, c).  Note: This is true mathematically, but not numerically.
+	if (0 < m_unscaledPoints.size())
+	{
+		btVector3 scaled = vec * m_localScaling;
+		int index = (int)scaled.maxDot(&m_unscaledPoints[0], m_unscaledPoints.size(), maxDot);  // FIXME: may violate encapsulation of m_unscaledPoints
+		return m_unscaledPoints[index] * m_localScaling;
+	}
 
-    return supVec;
+	return supVec;
 }
 
-void	btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 {
 	btScalar newDot;
 	//use 'w' component of supportVerticesOut?
 	{
-		for (int i=0;i<numVectors;i++)
+		for (int i = 0; i < numVectors; i++)
 		{
 			supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
 		}
 	}
 
-    for (int j=0;j<numVectors;j++)
-    {
-        btVector3 vec = vectors[j] * m_localScaling;        // dot(a*b,c) = dot(a,b*c)
-        if( 0 <  m_unscaledPoints.size() )
-        {
-            int i = (int) vec.maxDot( &m_unscaledPoints[0], m_unscaledPoints.size(), newDot);
-            supportVerticesOut[j] = getScaledPoint(i);
-            supportVerticesOut[j][3] = newDot;        
-        }
-        else
-            supportVerticesOut[j][3] = -BT_LARGE_FLOAT;
-    }
-
-
-
+	for (int j = 0; j < numVectors; j++)
+	{
+		btVector3 vec = vectors[j] * m_localScaling;  // dot(a*b,c) = dot(a,b*c)
+		if (0 < m_unscaledPoints.size())
+		{
+			int i = (int)vec.maxDot(&m_unscaledPoints[0], m_unscaledPoints.size(), newDot);
+			supportVerticesOut[j] = getScaledPoint(i);
+			supportVerticesOut[j][3] = newDot;
+		}
+		else
+			supportVerticesOut[j][3] = -BT_LARGE_FLOAT;
+	}
 }
-	
 
-
-btVector3	btConvexHullShape::localGetSupportingVertex(const btVector3& vec)const
+btVector3 btConvexHullShape::localGetSupportingVertex(const btVector3& vec) const
 {
 	btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
 
-	if ( getMargin()!=btScalar(0.) )
+	if (getMargin() != btScalar(0.))
 	{
 		btVector3 vecnorm = vec;
-		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+		if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON))
 		{
-			vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
-		} 
+			vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.));
+		}
 		vecnorm.normalize();
-		supVertex+= getMargin() * vecnorm;
+		supVertex += getMargin() * vecnorm;
 	}
 	return supVertex;
 }
 
-
-
-
-
-
-
-
+void btConvexHullShape::optimizeConvexHull()
+{
+	btConvexHullComputer conv;
+	conv.compute(&m_unscaledPoints[0].getX(), sizeof(btVector3), m_unscaledPoints.size(), 0.f, 0.f);
+	int numVerts = conv.vertices.size();
+	m_unscaledPoints.resize(0);
+	for (int i = 0; i < numVerts; i++)
+	{
+		m_unscaledPoints.push_back(conv.vertices[i]);
+	}
+}
 
 //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
 //Please note that you can debug-draw btConvexHullShape with the Raytracer Demo
-int	btConvexHullShape::getNumVertices() const
+int btConvexHullShape::getNumVertices() const
 {
 	return m_unscaledPoints.size();
 }
@@ -140,111 +137,108 @@ int btConvexHullShape::getNumEdges() const
 	return m_unscaledPoints.size();
 }
 
-void btConvexHullShape::getEdge(int i,btVector3& pa,btVector3& pb) const
+void btConvexHullShape::getEdge(int i, btVector3& pa, btVector3& pb) const
 {
-
-	int index0 = i%m_unscaledPoints.size();
-	int index1 = (i+1)%m_unscaledPoints.size();
+	int index0 = i % m_unscaledPoints.size();
+	int index1 = (i + 1) % m_unscaledPoints.size();
 	pa = getScaledPoint(index0);
 	pb = getScaledPoint(index1);
 }
 
-void btConvexHullShape::getVertex(int i,btVector3& vtx) const
+void btConvexHullShape::getVertex(int i, btVector3& vtx) const
 {
 	vtx = getScaledPoint(i);
 }
 
-int	btConvexHullShape::getNumPlanes() const
+int btConvexHullShape::getNumPlanes() const
 {
 	return 0;
 }
 
-void btConvexHullShape::getPlane(btVector3& ,btVector3& ,int ) const
+void btConvexHullShape::getPlane(btVector3&, btVector3&, int) const
 {
-
 	btAssert(0);
 }
 
 //not yet
-bool btConvexHullShape::isInside(const btVector3& ,btScalar ) const
+bool btConvexHullShape::isInside(const btVector3&, btScalar) const
 {
 	btAssert(0);
 	return false;
 }
 
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btConvexHullShape::serialize(void* dataBuffer, btSerializer* serializer) const
+const char* btConvexHullShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	//int szc = sizeof(btConvexHullShapeData);
-	btConvexHullShapeData* shapeData = (btConvexHullShapeData*) dataBuffer;
+	btConvexHullShapeData* shapeData = (btConvexHullShapeData*)dataBuffer;
 	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
 
 	int numElem = m_unscaledPoints.size();
 	shapeData->m_numUnscaledPoints = numElem;
 #ifdef BT_USE_DOUBLE_PRECISION
 	shapeData->m_unscaledPointsFloatPtr = 0;
-	shapeData->m_unscaledPointsDoublePtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]):  0;
+	shapeData->m_unscaledPointsDoublePtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]) : 0;
 #else
-	shapeData->m_unscaledPointsFloatPtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]):  0;
+	shapeData->m_unscaledPointsFloatPtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]) : 0;
 	shapeData->m_unscaledPointsDoublePtr = 0;
 #endif
-	
+
 	if (numElem)
 	{
 		int sz = sizeof(btVector3Data);
-	//	int sz2 = sizeof(btVector3DoubleData);
-	//	int sz3 = sizeof(btVector3FloatData);
-		btChunk* chunk = serializer->allocate(sz,numElem);
+		//	int sz2 = sizeof(btVector3DoubleData);
+		//	int sz3 = sizeof(btVector3FloatData);
+		btChunk* chunk = serializer->allocate(sz, numElem);
 		btVector3Data* memPtr = (btVector3Data*)chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
 			m_unscaledPoints[i].serialize(*memPtr);
 		}
-		serializer->finalizeChunk(chunk,btVector3DataName,BT_ARRAY_CODE,(void*)&m_unscaledPoints[0]);
+		serializer->finalizeChunk(chunk, btVector3DataName, BT_ARRAY_CODE, (void*)&m_unscaledPoints[0]);
 	}
-	
+
+	// Fill padding with zeros to appease msan.
+	memset(shapeData->m_padding3, 0, sizeof(shapeData->m_padding3));
+
 	return "btConvexHullShapeData";
 }
 
-void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
+void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const
 {
 #if 1
 	minProj = FLT_MAX;
 	maxProj = -FLT_MAX;
 
 	int numVerts = m_unscaledPoints.size();
-	for(int i=0;i<numVerts;i++)
+	for (int i = 0; i < numVerts; i++)
 	{
 		btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
 		btVector3 pt = trans * vtx;
 		btScalar dp = pt.dot(dir);
-		if(dp < minProj)	
+		if (dp < minProj)
 		{
 			minProj = dp;
 			witnesPtMin = pt;
 		}
-		if(dp > maxProj)	
+		if (dp > maxProj)
 		{
 			maxProj = dp;
-			witnesPtMax=pt;
+			witnesPtMax = pt;
 		}
 	}
 #else
-	btVector3 localAxis = dir*trans.getBasis();
-	witnesPtMin  = trans(localGetSupportingVertex(localAxis));
+	btVector3 localAxis = dir * trans.getBasis();
+	witnesPtMin = trans(localGetSupportingVertex(localAxis));
 	witnesPtMax = trans(localGetSupportingVertex(-localAxis));
 
 	minProj = witnesPtMin.dot(dir);
 	maxProj = witnesPtMax.dot(dir);
 #endif
 
-	if(minProj>maxProj)
+	if (minProj > maxProj)
 	{
-		btSwap(minProj,maxProj);
-		btSwap(witnesPtMin,witnesPtMax);
+		btSwap(minProj, maxProj);
+		btSwap(witnesPtMin, witnesPtMax);
 	}
-
-
 }
-
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h
index 3bd598ec4e9..96136d7dd72 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h
@@ -17,28 +17,26 @@ subject to the following restrictions:
 #define BT_CONVEX_HULL_SHAPE_H
 
 #include "btPolyhedralConvexShape.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"  // for the types
 #include "LinearMath/btAlignedObjectArray.h"
 
-
 ///The btConvexHullShape implements an implicit convex hull of an array of vertices.
 ///Bullet provides a general and fast collision detector for convex shapes based on GJK and EPA using localGetSupportingVertex.
-ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexAabbCachingShape
+ATTRIBUTE_ALIGNED16(class)
+btConvexHullShape : public btPolyhedralConvexAabbCachingShape
 {
-	btAlignedObjectArray<btVector3>	m_unscaledPoints;
+	btAlignedObjectArray<btVector3> m_unscaledPoints;
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	
 	///this constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive btScalar (x,y,z), the striding defines the number of bytes between each point, in memory.
 	///It is easier to not pass any points in the constructor, and just add one point at a time, using addPoint.
 	///btConvexHullShape make an internal copy of the points.
-	btConvexHullShape(const btScalar* points=0,int numPoints=0, int stride=sizeof(btVector3));
+	btConvexHullShape(const btScalar* points = 0, int numPoints = 0, int stride = sizeof(btVector3));
 
 	void addPoint(const btVector3& point, bool recalculateLocalAabb = true);
 
-	
 	btVector3* getUnscaledPoints()
 	{
 		return &m_unscaledPoints[0];
@@ -55,49 +53,46 @@ public:
 		return getUnscaledPoints();
 	}
 
-	
-
+	void optimizeConvexHull();
 
-	SIMD_FORCE_INLINE	btVector3 getScaledPoint(int i) const
+	SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const
 	{
 		return m_unscaledPoints[i] * m_localScaling;
 	}
 
-	SIMD_FORCE_INLINE	int getNumPoints() const 
+	SIMD_FORCE_INLINE int getNumPoints() const
 	{
 		return m_unscaledPoints.size();
 	}
 
-	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
-	
-
-	virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
+	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
 
+	virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const;
 
 	//debugging
-	virtual const char*	getName()const {return "Convex";}
+	virtual const char* getName() const { return "Convex"; }
 
-	
-	virtual int	getNumVertices() const;
+	virtual int getNumVertices() const;
 	virtual int getNumEdges() const;
-	virtual void getEdge(int i,btVector3& pa,btVector3& pb) const;
-	virtual void getVertex(int i,btVector3& vtx) const;
-	virtual int	getNumPlanes() const;
-	virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const;
-	virtual	bool isInside(const btVector3& pt,btScalar tolerance) const;
+	virtual void getEdge(int i, btVector3& pa, btVector3& pb) const;
+	virtual void getVertex(int i, btVector3& vtx) const;
+	virtual int getNumPlanes() const;
+	virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const;
+	virtual bool isInside(const btVector3& pt, btScalar tolerance) const;
 
 	///in case we receive negative scaling
-	virtual void	setLocalScaling(const btVector3& scaling);
+	virtual void setLocalScaling(const btVector3& scaling);
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
+// clang-format off
+
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btConvexHullShapeData
 {
@@ -111,12 +106,11 @@ struct	btConvexHullShapeData
 
 };
 
+// clang-format on
 
-SIMD_FORCE_INLINE	int	btConvexHullShape::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btConvexHullShape::calculateSerializeBufferSize() const
 {
 	return sizeof(btConvexHullShapeData);
 }
 
-
-#endif //BT_CONVEX_HULL_SHAPE_H
-
+#endif  //BT_CONVEX_HULL_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp
index 083d60b1b1e..b847f8f40f6 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp
@@ -13,139 +13,125 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btConvexInternalShape.h"
 
-
-
 btConvexInternalShape::btConvexInternalShape()
-: m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
-m_collisionMargin(CONVEX_DISTANCE_MARGIN)
+	: m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.)),
+	  m_collisionMargin(CONVEX_DISTANCE_MARGIN)
 {
 }
 
-
-void	btConvexInternalShape::setLocalScaling(const btVector3& scaling)
+void btConvexInternalShape::setLocalScaling(const btVector3& scaling)
 {
 	m_localScaling = scaling.absolute();
 }
 
-
-
-void	btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const
+void btConvexInternalShape::getAabbSlow(const btTransform& trans, btVector3& minAabb, btVector3& maxAabb) const
 {
 #ifndef __SPU__
 	//use localGetSupportingVertexWithoutMargin?
 	btScalar margin = getMargin();
-	for (int i=0;i<3;i++)
+	for (int i = 0; i < 3; i++)
 	{
-		btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
+		btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.));
 		vec[i] = btScalar(1.);
 
-		btVector3 sv = localGetSupportingVertex(vec*trans.getBasis());
+		btVector3 sv = localGetSupportingVertex(vec * trans.getBasis());
 
 		btVector3 tmp = trans(sv);
-		maxAabb[i] = tmp[i]+margin;
+		maxAabb[i] = tmp[i] + margin;
 		vec[i] = btScalar(-1.);
-		tmp = trans(localGetSupportingVertex(vec*trans.getBasis()));
-		minAabb[i] = tmp[i]-margin;
+		tmp = trans(localGetSupportingVertex(vec * trans.getBasis()));
+		minAabb[i] = tmp[i] - margin;
 	}
 #endif
 }
 
-
-
-btVector3	btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)const
+btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec) const
 {
 #ifndef __SPU__
 
-	 btVector3	supVertex = localGetSupportingVertexWithoutMargin(vec);
+	btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
 
-	if ( getMargin()!=btScalar(0.) )
+	if (getMargin() != btScalar(0.))
 	{
 		btVector3 vecnorm = vec;
-		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+		if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON))
 		{
-			vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
-		} 
+			vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.));
+		}
 		vecnorm.normalize();
-		supVertex+= getMargin() * vecnorm;
+		supVertex += getMargin() * vecnorm;
 	}
 	return supVertex;
 
 #else
 	btAssert(0);
-	return btVector3(0,0,0);
-#endif //__SPU__
-
- }
-
+	return btVector3(0, 0, 0);
+#endif  //__SPU__
+}
 
 btConvexInternalAabbCachingShape::btConvexInternalAabbCachingShape()
-	:	btConvexInternalShape(),
-m_localAabbMin(1,1,1),
-m_localAabbMax(-1,-1,-1),
-m_isLocalAabbValid(false)
+	: btConvexInternalShape(),
+	  m_localAabbMin(1, 1, 1),
+	  m_localAabbMax(-1, -1, -1),
+	  m_isLocalAabbValid(false)
 {
 }
 
-
-void btConvexInternalAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+void btConvexInternalAabbCachingShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const
 {
-	getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin());
+	getNonvirtualAabb(trans, aabbMin, aabbMax, getMargin());
 }
 
-void	btConvexInternalAabbCachingShape::setLocalScaling(const btVector3& scaling)
+void btConvexInternalAabbCachingShape::setLocalScaling(const btVector3& scaling)
 {
 	btConvexInternalShape::setLocalScaling(scaling);
 	recalcLocalAabb();
 }
 
-
-void	btConvexInternalAabbCachingShape::recalcLocalAabb()
+void btConvexInternalAabbCachingShape::recalcLocalAabb()
 {
 	m_isLocalAabbValid = true;
-	
-	#if 1
+
+#if 1
 	static const btVector3 _directions[] =
-	{
-		btVector3( 1.,  0.,  0.),
-		btVector3( 0.,  1.,  0.),
-		btVector3( 0.,  0.,  1.),
-		btVector3( -1., 0.,  0.),
-		btVector3( 0., -1.,  0.),
-		btVector3( 0.,  0., -1.)
-	};
-	
+		{
+			btVector3(1., 0., 0.),
+			btVector3(0., 1., 0.),
+			btVector3(0., 0., 1.),
+			btVector3(-1., 0., 0.),
+			btVector3(0., -1., 0.),
+			btVector3(0., 0., -1.)};
+
 	btVector3 _supporting[] =
-	{
-		btVector3( 0., 0., 0.),
-		btVector3( 0., 0., 0.),
-		btVector3( 0., 0., 0.),
-		btVector3( 0., 0., 0.),
-		btVector3( 0., 0., 0.),
-		btVector3( 0., 0., 0.)
-	};
-	
+		{
+			btVector3(0., 0., 0.),
+			btVector3(0., 0., 0.),
+			btVector3(0., 0., 0.),
+			btVector3(0., 0., 0.),
+			btVector3(0., 0., 0.),
+			btVector3(0., 0., 0.)};
+
 	batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
-	
-	for ( int i = 0; i < 3; ++i )
+
+	for (int i = 0; i < 3; ++i)
 	{
-		m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin;
-		m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin;
+		m_localAabbMax[i] = _supporting[i][i];
+		m_localAabbMin[i] = _supporting[i + 3][i];
 	}
-	
-	#else
 
-	for (int i=0;i<3;i++)
+#else
+
+	for (int i = 0; i < 3; i++)
 	{
-		btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
+		btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.));
 		vec[i] = btScalar(1.);
 		btVector3 tmp = localGetSupportingVertex(vec);
-		m_localAabbMax[i] = tmp[i]+m_collisionMargin;
+		m_localAabbMax[i] = tmp[i];
 		vec[i] = btScalar(-1.);
 		tmp = localGetSupportingVertex(vec);
-		m_localAabbMin[i] = tmp[i]-m_collisionMargin;
+		m_localAabbMin[i] = tmp[i];
 	}
-	#endif
+#endif
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h
index 37e04f5fc81..a28c57de4b8 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h
@@ -19,39 +19,35 @@ subject to the following restrictions:
 #include "btConvexShape.h"
 #include "LinearMath/btAabbUtil2.h"
 
-
 ///The btConvexInternalShape is an internal base class, shared by most convex shape implementations.
 ///The btConvexInternalShape uses a default collision margin set to CONVEX_DISTANCE_MARGIN.
 ///This collision margin used by Gjk and some other algorithms, see also btCollisionMargin.h
-///Note that when creating small shapes (derived from btConvexInternalShape), 
+///Note that when creating small shapes (derived from btConvexInternalShape),
 ///you need to make sure to set a smaller collision margin, using the 'setMargin' API
 ///There is a automatic mechanism 'setSafeMargin' used by btBoxShape and btCylinderShape
-ATTRIBUTE_ALIGNED16(class) btConvexInternalShape : public btConvexShape
+ATTRIBUTE_ALIGNED16(class)
+btConvexInternalShape : public btConvexShape
 {
-
-	protected:
-
+protected:
 	//local scaling. collisionMargin is not scaled !
-	btVector3	m_localScaling;
+	btVector3 m_localScaling;
+
+	btVector3 m_implicitShapeDimensions;
 
-	btVector3	m_implicitShapeDimensions;
-	
-	btScalar	m_collisionMargin;
+	btScalar m_collisionMargin;
 
-	btScalar	m_padding;
+	btScalar m_padding;
 
 	btConvexInternalShape();
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	virtual ~btConvexInternalShape()
 	{
-
 	}
 
-	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;
+	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
 
 	const btVector3& getImplicitShapeDimensions() const
 	{
@@ -62,163 +58,151 @@ public:
 	///changing a collision shape while the body is in the world is not recommended,
 	///it is best to remove the body from the world, then make the change, and re-add it
 	///alternatively flush the contact points, see documentation for 'cleanProxyFromPairs'
-	void	setImplicitShapeDimensions(const btVector3& dimensions)
+	void setImplicitShapeDimensions(const btVector3& dimensions)
 	{
 		m_implicitShapeDimensions = dimensions;
 	}
 
-	void	setSafeMargin(btScalar minDimension, btScalar defaultMarginMultiplier = 0.1f)
+	void setSafeMargin(btScalar minDimension, btScalar defaultMarginMultiplier = 0.1f)
 	{
-		btScalar safeMargin = defaultMarginMultiplier*minDimension;
+		btScalar safeMargin = defaultMarginMultiplier * minDimension;
 		if (safeMargin < getMargin())
 		{
 			setMargin(safeMargin);
 		}
 	}
-	void	setSafeMargin(const btVector3& halfExtents, btScalar defaultMarginMultiplier = 0.1f)
+	void setSafeMargin(const btVector3& halfExtents, btScalar defaultMarginMultiplier = 0.1f)
 	{
 		//see http://code.google.com/p/bullet/issues/detail?id=349
 		//this margin check could could be added to other collision shapes too,
 		//or add some assert/warning somewhere
-		btScalar minDimension=halfExtents[halfExtents.minAxis()]; 		
+		btScalar minDimension = halfExtents[halfExtents.minAxis()];
 		setSafeMargin(minDimension, defaultMarginMultiplier);
 	}
 
 	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
-	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+	void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 	{
-		getAabbSlow(t,aabbMin,aabbMax);
+		getAabbSlow(t, aabbMin, aabbMax);
 	}
 
+	virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
-	
-	virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
-
-
-	virtual void	setLocalScaling(const btVector3& scaling);
-	virtual const btVector3& getLocalScaling() const 
+	virtual void setLocalScaling(const btVector3& scaling);
+	virtual const btVector3& getLocalScaling() const
 	{
 		return m_localScaling;
 	}
 
-	const btVector3& getLocalScalingNV() const 
+	const btVector3& getLocalScalingNV() const
 	{
 		return m_localScaling;
 	}
 
-	virtual void	setMargin(btScalar margin)
+	virtual void setMargin(btScalar margin)
 	{
 		m_collisionMargin = margin;
 	}
-	virtual btScalar	getMargin() const
+	virtual btScalar getMargin() const
 	{
 		return m_collisionMargin;
 	}
 
-	btScalar	getMarginNV() const
+	btScalar getMarginNV() const
 	{
 		return m_collisionMargin;
 	}
 
-	virtual int		getNumPreferredPenetrationDirections() const
+	virtual int getNumPreferredPenetrationDirections() const
 	{
 		return 0;
 	}
-	
-	virtual void	getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+
+	virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
 	{
 		(void)penetrationVector;
 		(void)index;
 		btAssert(0);
 	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
-	
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btConvexInternalShapeData
+struct btConvexInternalShapeData
 {
-	btCollisionShapeData	m_collisionShapeData;
+	btCollisionShapeData m_collisionShapeData;
 
-	btVector3FloatData	m_localScaling;
+	btVector3FloatData m_localScaling;
 
-	btVector3FloatData	m_implicitShapeDimensions;
-	
-	float			m_collisionMargin;
+	btVector3FloatData m_implicitShapeDimensions;
 
-	int	m_padding;
+	float m_collisionMargin;
 
+	int m_padding;
 };
 
-
-
-SIMD_FORCE_INLINE	int	btConvexInternalShape::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btConvexInternalShape::calculateSerializeBufferSize() const
 {
 	return sizeof(btConvexInternalShapeData);
 }
 
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btConvexInternalShape::serialize(void* dataBuffer, btSerializer* serializer) const
+SIMD_FORCE_INLINE const char* btConvexInternalShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btConvexInternalShapeData* shapeData = (btConvexInternalShapeData*) dataBuffer;
+	btConvexInternalShapeData* shapeData = (btConvexInternalShapeData*)dataBuffer;
 	btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer);
 
 	m_implicitShapeDimensions.serializeFloat(shapeData->m_implicitShapeDimensions);
 	m_localScaling.serializeFloat(shapeData->m_localScaling);
 	shapeData->m_collisionMargin = float(m_collisionMargin);
 
+	// Fill padding with zeros to appease msan.
+	shapeData->m_padding = 0;
+
 	return "btConvexInternalShapeData";
 }
 
-
-
-
 ///btConvexInternalAabbCachingShape adds local aabb caching for convex shapes, to avoid expensive bounding box calculations
 class btConvexInternalAabbCachingShape : public btConvexInternalShape
 {
-	btVector3	m_localAabbMin;
-	btVector3	m_localAabbMax;
-	bool		m_isLocalAabbValid;
-	
+	btVector3 m_localAabbMin;
+	btVector3 m_localAabbMax;
+	bool m_isLocalAabbValid;
+
 protected:
-					
 	btConvexInternalAabbCachingShape();
-	
-	void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax)
+
+	void setCachedLocalAabb(const btVector3& aabbMin, const btVector3& aabbMax)
 	{
 		m_isLocalAabbValid = true;
 		m_localAabbMin = aabbMin;
 		m_localAabbMax = aabbMax;
 	}
 
-	inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const
+	inline void getCachedLocalAabb(btVector3& aabbMin, btVector3& aabbMax) const
 	{
 		btAssert(m_isLocalAabbValid);
 		aabbMin = m_localAabbMin;
 		aabbMax = m_localAabbMax;
 	}
 
-	inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const
+	inline void getNonvirtualAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax, btScalar margin) const
 	{
-
 		//lazy evaluation of local aabb
 		btAssert(m_isLocalAabbValid);
-		btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax);
+		btTransformAabb(m_localAabbMin, m_localAabbMax, margin, trans, aabbMin, aabbMax);
 	}
-		
-public:
-		
-	virtual void	setLocalScaling(const btVector3& scaling);
 
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+public:
+	virtual void setLocalScaling(const btVector3& scaling);
 
-	void	recalcLocalAabb();
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
+	void recalcLocalAabb();
 };
 
-#endif //BT_CONVEX_INTERNAL_SHAPE_H
+#endif  //BT_CONVEX_INTERNAL_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp
index ad1d1bf78f8..f00a440fa35 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp
@@ -25,81 +25,73 @@ void btConvexPointCloudShape::setLocalScaling(const btVector3& scaling)
 }
 
 #ifndef __SPU__
-btVector3	btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const
 {
-	btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
+	btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.));
 	btScalar maxDot = btScalar(-BT_LARGE_FLOAT);
 
 	btVector3 vec = vec0;
 	btScalar lenSqr = vec.length2();
 	if (lenSqr < btScalar(0.0001))
 	{
-		vec.setValue(1,0,0);
-	} else
+		vec.setValue(1, 0, 0);
+	}
+	else
 	{
-		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+		btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
 		vec *= rlen;
 	}
-    
-    if( m_numPoints > 0 )
-    {
-        // Here we take advantage of dot(a*b, c) = dot( a, b*c) to do less work. Note this transformation is true mathematically, not numerically.
-    //    btVector3 scaled = vec * m_localScaling;
-        int index = (int) vec.maxDot( &m_unscaledPoints[0], m_numPoints, maxDot);   //FIXME: may violate encapsulation of m_unscaledPoints
-        return getScaledPoint(index);
-    }
+
+	if (m_numPoints > 0)
+	{
+		// Here we take advantage of dot(a*b, c) = dot( a, b*c) to do less work. Note this transformation is true mathematically, not numerically.
+		//    btVector3 scaled = vec * m_localScaling;
+		int index = (int)vec.maxDot(&m_unscaledPoints[0], m_numPoints, maxDot);  //FIXME: may violate encapsulation of m_unscaledPoints
+		return getScaledPoint(index);
+	}
 
 	return supVec;
 }
 
-void	btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 {
-    for( int j = 0; j < numVectors; j++ )
-    {
-        const btVector3& vec = vectors[j] * m_localScaling;  // dot( a*c, b) = dot(a, b*c)
-        btScalar maxDot;
-        int index = (int) vec.maxDot( &m_unscaledPoints[0], m_numPoints, maxDot);
-        supportVerticesOut[j][3] = btScalar(-BT_LARGE_FLOAT);
-        if( 0 <= index )
-        {
-            //WARNING: don't swap next lines, the w component would get overwritten!
-            supportVerticesOut[j] = getScaledPoint(index);
-            supportVerticesOut[j][3] = maxDot;
-        }
-    }
-
+	for (int j = 0; j < numVectors; j++)
+	{
+		const btVector3& vec = vectors[j] * m_localScaling;  // dot( a*c, b) = dot(a, b*c)
+		btScalar maxDot;
+		int index = (int)vec.maxDot(&m_unscaledPoints[0], m_numPoints, maxDot);
+		supportVerticesOut[j][3] = btScalar(-BT_LARGE_FLOAT);
+		if (0 <= index)
+		{
+			//WARNING: don't swap next lines, the w component would get overwritten!
+			supportVerticesOut[j] = getScaledPoint(index);
+			supportVerticesOut[j][3] = maxDot;
+		}
+	}
 }
-	
-
 
-btVector3	btConvexPointCloudShape::localGetSupportingVertex(const btVector3& vec)const
+btVector3 btConvexPointCloudShape::localGetSupportingVertex(const btVector3& vec) const
 {
 	btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
 
-	if ( getMargin()!=btScalar(0.) )
+	if (getMargin() != btScalar(0.))
 	{
 		btVector3 vecnorm = vec;
-		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+		if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON))
 		{
-			vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
-		} 
+			vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.));
+		}
 		vecnorm.normalize();
-		supVertex+= getMargin() * vecnorm;
+		supVertex += getMargin() * vecnorm;
 	}
 	return supVertex;
 }
 
-
 #endif
 
-
-
-
-
-
 //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
 //Please note that you can debug-draw btConvexHullShape with the Raytracer Demo
-int	btConvexPointCloudShape::getNumVertices() const
+int btConvexPointCloudShape::getNumVertices() const
 {
 	return m_numPoints;
 }
@@ -109,31 +101,29 @@ int btConvexPointCloudShape::getNumEdges() const
 	return 0;
 }
 
-void btConvexPointCloudShape::getEdge(int i,btVector3& pa,btVector3& pb) const
+void btConvexPointCloudShape::getEdge(int i, btVector3& pa, btVector3& pb) const
 {
-	btAssert (0);
+	btAssert(0);
 }
 
-void btConvexPointCloudShape::getVertex(int i,btVector3& vtx) const
+void btConvexPointCloudShape::getVertex(int i, btVector3& vtx) const
 {
-	vtx = m_unscaledPoints[i]*m_localScaling;
+	vtx = m_unscaledPoints[i] * m_localScaling;
 }
 
-int	btConvexPointCloudShape::getNumPlanes() const
+int btConvexPointCloudShape::getNumPlanes() const
 {
 	return 0;
 }
 
-void btConvexPointCloudShape::getPlane(btVector3& ,btVector3& ,int ) const
+void btConvexPointCloudShape::getPlane(btVector3&, btVector3&, int) const
 {
-
 	btAssert(0);
 }
 
 //not yet
-bool btConvexPointCloudShape::isInside(const btVector3& ,btScalar ) const
+bool btConvexPointCloudShape::isInside(const btVector3&, btScalar) const
 {
 	btAssert(0);
 	return false;
 }
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h
index 54b5afac3ec..c7d554a4d3d 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h
@@ -17,11 +17,12 @@ subject to the following restrictions:
 #define BT_CONVEX_POINT_CLOUD_SHAPE_H
 
 #include "btPolyhedralConvexShape.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"  // for the types
 #include "LinearMath/btAlignedObjectArray.h"
 
 ///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices.
-ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexAabbCachingShape
+ATTRIBUTE_ALIGNED16(class)
+btConvexPointCloudShape : public btPolyhedralConvexAabbCachingShape
 {
 	btVector3* m_unscaledPoints;
 	int m_numPoints;
@@ -31,13 +32,13 @@ public:
 
 	btConvexPointCloudShape()
 	{
-		m_localScaling.setValue(1.f,1.f,1.f);
+		m_localScaling.setValue(1.f, 1.f, 1.f);
 		m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE;
 		m_unscaledPoints = 0;
 		m_numPoints = 0;
 	}
 
-	btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true)
+	btConvexPointCloudShape(btVector3 * points, int numPoints, const btVector3& localScaling, bool computeAabb = true)
 	{
 		m_localScaling = localScaling;
 		m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE;
@@ -48,7 +49,7 @@ public:
 			recalcLocalAabb();
 	}
 
-	void setPoints (btVector3* points, int numPoints, bool computeAabb = true,const btVector3& localScaling=btVector3(1.f,1.f,1.f))
+	void setPoints(btVector3 * points, int numPoints, bool computeAabb = true, const btVector3& localScaling = btVector3(1.f, 1.f, 1.f))
 	{
 		m_unscaledPoints = points;
 		m_numPoints = numPoints;
@@ -58,48 +59,45 @@ public:
 			recalcLocalAabb();
 	}
 
-	SIMD_FORCE_INLINE	btVector3* getUnscaledPoints()
+	SIMD_FORCE_INLINE btVector3* getUnscaledPoints()
 	{
 		return m_unscaledPoints;
 	}
 
-	SIMD_FORCE_INLINE	const btVector3* getUnscaledPoints() const
+	SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const
 	{
 		return m_unscaledPoints;
 	}
 
-	SIMD_FORCE_INLINE	int getNumPoints() const 
+	SIMD_FORCE_INLINE int getNumPoints() const
 	{
 		return m_numPoints;
 	}
 
-	SIMD_FORCE_INLINE	btVector3	getScaledPoint( int index) const
+	SIMD_FORCE_INLINE btVector3 getScaledPoint(int index) const
 	{
 		return m_unscaledPoints[index] * m_localScaling;
 	}
 
 #ifndef __SPU__
-	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
 #endif
 
-
 	//debugging
-	virtual const char*	getName()const {return "ConvexPointCloud";}
+	virtual const char* getName() const { return "ConvexPointCloud"; }
 
-	virtual int	getNumVertices() const;
+	virtual int getNumVertices() const;
 	virtual int getNumEdges() const;
-	virtual void getEdge(int i,btVector3& pa,btVector3& pb) const;
-	virtual void getVertex(int i,btVector3& vtx) const;
-	virtual int	getNumPlanes() const;
-	virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const;
-	virtual	bool isInside(const btVector3& pt,btScalar tolerance) const;
+	virtual void getEdge(int i, btVector3& pa, btVector3& pb) const;
+	virtual void getVertex(int i, btVector3& vtx) const;
+	virtual int getNumPlanes() const;
+	virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const;
+	virtual bool isInside(const btVector3& pt, btScalar tolerance) const;
 
 	///in case we receive negative scaling
-	virtual void	setLocalScaling(const btVector3& scaling);
+	virtual void setLocalScaling(const btVector3& scaling);
 };
 
-
-#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H
-
+#endif  //BT_CONVEX_POINT_CLOUD_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
index 4f45319a83a..9694f4ddb3d 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 ///This file was written by Erwin Coumans
 ///Separating axis rest based on work from Pierre Terdiman, see
 ///And contact clipping based on work from Simon Hobbs
@@ -21,49 +20,45 @@ subject to the following restrictions:
 #include "btConvexPolyhedron.h"
 #include "LinearMath/btHashMap.h"
 
-
 btConvexPolyhedron::btConvexPolyhedron()
 {
-
 }
 btConvexPolyhedron::~btConvexPolyhedron()
 {
-
 }
 
-
-inline bool IsAlmostZero(const btVector3& v)
+inline bool IsAlmostZero1(const btVector3& v)
 {
-	if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6)	return false;
+	if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false;
 	return true;
 }
 
 struct btInternalVertexPair
 {
-	btInternalVertexPair(short int v0,short int v1)
-		:m_v0(v0),
-		m_v1(v1)
+	btInternalVertexPair(short int v0, short int v1)
+		: m_v0(v0),
+		  m_v1(v1)
 	{
-		if (m_v1>m_v0)
-			btSwap(m_v0,m_v1);
+		if (m_v1 > m_v0)
+			btSwap(m_v0, m_v1);
 	}
 	short int m_v0;
 	short int m_v1;
 	int getHash() const
 	{
-		return m_v0+(m_v1<<16);
+		return m_v0 + (m_v1 << 16);
 	}
 	bool equals(const btInternalVertexPair& other) const
 	{
-		return m_v0==other.m_v0 && m_v1==other.m_v1;
+		return m_v0 == other.m_v0 && m_v1 == other.m_v1;
 	}
 };
 
 struct btInternalEdge
 {
 	btInternalEdge()
-		:m_face0(-1),
-		m_face1(-1)
+		: m_face0(-1),
+		  m_face1(-1)
 	{
 	}
 	short int m_face0;
@@ -75,23 +70,31 @@ struct btInternalEdge
 #ifdef TEST_INTERNAL_OBJECTS
 bool btConvexPolyhedron::testContainment() const
 {
-	for(int p=0;p<8;p++)
+	for (int p = 0; p < 8; p++)
 	{
 		btVector3 LocalPt;
-		if(p==0)		LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]);
-		else if(p==1)	LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]);
-		else if(p==2)	LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]);
-		else if(p==3)	LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]);
-		else if(p==4)	LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]);
-		else if(p==5)	LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]);
-		else if(p==6)	LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]);
-		else if(p==7)	LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]);
-
-		for(int i=0;i<m_faces.size();i++)
+		if (p == 0)
+			LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]);
+		else if (p == 1)
+			LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]);
+		else if (p == 2)
+			LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]);
+		else if (p == 3)
+			LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]);
+		else if (p == 4)
+			LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]);
+		else if (p == 5)
+			LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]);
+		else if (p == 6)
+			LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]);
+		else if (p == 7)
+			LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]);
+
+		for (int i = 0; i < m_faces.size(); i++)
 		{
 			const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
 			const btScalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
-			if(d>0.0f)
+			if (d > 0.0f)
 				return false;
 		}
 	}
@@ -99,33 +102,28 @@ bool btConvexPolyhedron::testContainment() const
 }
 #endif
 
-void	btConvexPolyhedron::initialize()
+void btConvexPolyhedron::initialize()
 {
+	btHashMap<btInternalVertexPair, btInternalEdge> edges;
 
-	btHashMap<btInternalVertexPair,btInternalEdge> edges;
-
-	btScalar TotalArea = 0.0f;
-	
-	m_localCenter.setValue(0, 0, 0);
-	for(int i=0;i<m_faces.size();i++)
+	for (int i = 0; i < m_faces.size(); i++)
 	{
 		int numVertices = m_faces[i].m_indices.size();
 		int NbTris = numVertices;
-		for(int j=0;j<NbTris;j++)
+		for (int j = 0; j < NbTris; j++)
 		{
-			int k = (j+1)%numVertices;
-			btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
+			int k = (j + 1) % numVertices;
+			btInternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
 			btInternalEdge* edptr = edges.find(vp);
-			btVector3 edge = m_vertices[vp.m_v1]-m_vertices[vp.m_v0];
+			btVector3 edge = m_vertices[vp.m_v1] - m_vertices[vp.m_v0];
 			edge.normalize();
 
 			bool found = false;
 
-			for (int p=0;p<m_uniqueEdges.size();p++)
+			for (int p = 0; p < m_uniqueEdges.size(); p++)
 			{
-				
-				if (IsAlmostZero(m_uniqueEdges[p]-edge) || 
-					IsAlmostZero(m_uniqueEdges[p]+edge))
+				if (IsAlmostZero1(m_uniqueEdges[p] - edge) ||
+					IsAlmostZero1(m_uniqueEdges[p] + edge))
 				{
 					found = true;
 					break;
@@ -139,105 +137,107 @@ void	btConvexPolyhedron::initialize()
 
 			if (edptr)
 			{
-				btAssert(edptr->m_face0>=0);
-				btAssert(edptr->m_face1<0);
+				btAssert(edptr->m_face0 >= 0);
+				btAssert(edptr->m_face1 < 0);
 				edptr->m_face1 = i;
-			} else
+			}
+			else
 			{
 				btInternalEdge ed;
 				ed.m_face0 = i;
-				edges.insert(vp,ed);
+				edges.insert(vp, ed);
 			}
 		}
 	}
 
 #ifdef USE_CONNECTED_FACES
-	for(int i=0;i<m_faces.size();i++)
+	for (int i = 0; i < m_faces.size(); i++)
 	{
 		int numVertices = m_faces[i].m_indices.size();
 		m_faces[i].m_connectedFaces.resize(numVertices);
 
-		for(int j=0;j<numVertices;j++)
+		for (int j = 0; j < numVertices; j++)
 		{
-			int k = (j+1)%numVertices;
-			btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
+			int k = (j + 1) % numVertices;
+			btInternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
 			btInternalEdge* edptr = edges.find(vp);
 			btAssert(edptr);
-			btAssert(edptr->m_face0>=0);
-			btAssert(edptr->m_face1>=0);
+			btAssert(edptr->m_face0 >= 0);
+			btAssert(edptr->m_face1 >= 0);
 
-			int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0;
+			int connectedFace = (edptr->m_face0 == i) ? edptr->m_face1 : edptr->m_face0;
 			m_faces[i].m_connectedFaces[j] = connectedFace;
 		}
 	}
-#endif//USE_CONNECTED_FACES
+#endif  //USE_CONNECTED_FACES
+
+	initialize2();
+}
 
-	for(int i=0;i<m_faces.size();i++)
+void btConvexPolyhedron::initialize2()
+{
+	m_localCenter.setValue(0, 0, 0);
+	btScalar TotalArea = 0.0f;
+	for (int i = 0; i < m_faces.size(); i++)
 	{
 		int numVertices = m_faces[i].m_indices.size();
-		int NbTris = numVertices-2;
-		
+		int NbTris = numVertices - 2;
+
 		const btVector3& p0 = m_vertices[m_faces[i].m_indices[0]];
-		for(int j=1;j<=NbTris;j++)
+		for (int j = 1; j <= NbTris; j++)
 		{
-			int k = (j+1)%numVertices;
+			int k = (j + 1) % numVertices;
 			const btVector3& p1 = m_vertices[m_faces[i].m_indices[j]];
 			const btVector3& p2 = m_vertices[m_faces[i].m_indices[k]];
 			btScalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
-			btVector3 Center = (p0+p1+p2)/3.0f;
+			btVector3 Center = (p0 + p1 + p2) / 3.0f;
 			m_localCenter += Area * Center;
 			TotalArea += Area;
 		}
 	}
 	m_localCenter /= TotalArea;
 
-
-
-
 #ifdef TEST_INTERNAL_OBJECTS
-	if(1)
+	if (1)
 	{
 		m_radius = FLT_MAX;
-		for(int i=0;i<m_faces.size();i++)
+		for (int i = 0; i < m_faces.size(); i++)
 		{
 			const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
 			const btScalar dist = btFabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
-			if(dist<m_radius)
+			if (dist < m_radius)
 				m_radius = dist;
 		}
 
-	
 		btScalar MinX = FLT_MAX;
 		btScalar MinY = FLT_MAX;
 		btScalar MinZ = FLT_MAX;
 		btScalar MaxX = -FLT_MAX;
 		btScalar MaxY = -FLT_MAX;
 		btScalar MaxZ = -FLT_MAX;
-		for(int i=0; i<m_vertices.size(); i++)
+		for (int i = 0; i < m_vertices.size(); i++)
 		{
 			const btVector3& pt = m_vertices[i];
-			if(pt.x()<MinX)	MinX = pt.x();
-			if(pt.x()>MaxX)	MaxX = pt.x();
-			if(pt.y()<MinY)	MinY = pt.y();
-			if(pt.y()>MaxY)	MaxY = pt.y();
-			if(pt.z()<MinZ)	MinZ = pt.z();
-			if(pt.z()>MaxZ)	MaxZ = pt.z();
+			if (pt.x() < MinX) MinX = pt.x();
+			if (pt.x() > MaxX) MaxX = pt.x();
+			if (pt.y() < MinY) MinY = pt.y();
+			if (pt.y() > MaxY) MaxY = pt.y();
+			if (pt.z() < MinZ) MinZ = pt.z();
+			if (pt.z() > MaxZ) MaxZ = pt.z();
 		}
-		mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ);
-		mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ);
-
+		mC.setValue(MaxX + MinX, MaxY + MinY, MaxZ + MinZ);
+		mE.setValue(MaxX - MinX, MaxY - MinY, MaxZ - MinZ);
 
-
-//		const btScalar r = m_radius / sqrtf(2.0f);
+		//		const btScalar r = m_radius / sqrtf(2.0f);
 		const btScalar r = m_radius / sqrtf(3.0f);
 		const int LargestExtent = mE.maxAxis();
-		const btScalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f;
+		const btScalar Step = (mE[LargestExtent] * 0.5f - r) / 1024.0f;
 		m_extents[0] = m_extents[1] = m_extents[2] = r;
-		m_extents[LargestExtent] = mE[LargestExtent]*0.5f;
+		m_extents[LargestExtent] = mE[LargestExtent] * 0.5f;
 		bool FoundBox = false;
-		for(int j=0;j<1024;j++)
+		for (int j = 0; j < 1024; j++)
 		{
-			if(testContainment())
+			if (testContainment())
 			{
 				FoundBox = true;
 				break;
@@ -245,25 +245,25 @@ void	btConvexPolyhedron::initialize()
 
 			m_extents[LargestExtent] -= Step;
 		}
-		if(!FoundBox)
+		if (!FoundBox)
 		{
 			m_extents[0] = m_extents[1] = m_extents[2] = r;
 		}
 		else
 		{
 			// Refine the box
-			const btScalar Step = (m_radius - r)/1024.0f;
-			const int e0 = (1<<LargestExtent) & 3;
-			const int e1 = (1<<e0) & 3;
+			const btScalar Step = (m_radius - r) / 1024.0f;
+			const int e0 = (1 << LargestExtent) & 3;
+			const int e1 = (1 << e0) & 3;
 
-			for(int j=0;j<1024;j++)
+			for (int j = 0; j < 1024; j++)
 			{
 				const btScalar Saved0 = m_extents[e0];
 				const btScalar Saved1 = m_extents[e1];
 				m_extents[e0] += Step;
 				m_extents[e1] += Step;
 
-				if(!testContainment())
+				if (!testContainment())
 				{
 					m_extents[e0] = Saved0;
 					m_extents[e1] = Saved1;
@@ -274,30 +274,29 @@ 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;
 	int numVerts = m_vertices.size();
-	for(int i=0;i<numVerts;i++)
+	for (int i = 0; i < numVerts; i++)
 	{
 		btVector3 pt = trans * m_vertices[i];
 		btScalar dp = pt.dot(dir);
-		if(dp < minProj)
+		if (dp < minProj)
 		{
 			minProj = dp;
 			witnesPtMin = pt;
 		}
-		if(dp > maxProj)
+		if (dp > maxProj)
 		{
 			maxProj = dp;
 			witnesPtMax = pt;
 		}
 	}
-	if(minProj>maxProj)
+	if (minProj > maxProj)
 	{
-		btSwap(minProj,maxProj);
-		btSwap(witnesPtMin,witnesPtMax);
+		btSwap(minProj, maxProj);
+		btSwap(witnesPtMin, witnesPtMax);
 	}
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h
index d3cd066ac8d..638aa9b3df0 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h
@@ -13,10 +13,8 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 ///This file was written by Erwin Coumans
 
-
 #ifndef _BT_POLYHEDRAL_FEATURES_H
 #define _BT_POLYHEDRAL_FEATURES_H
 
@@ -25,41 +23,37 @@ subject to the following restrictions:
 
 #define TEST_INTERNAL_OBJECTS 1
 
-
 struct btFace
 {
-	btAlignedObjectArray<int>	m_indices;
-//	btAlignedObjectArray<int>	m_connectedFaces;
-	btScalar	m_plane[4];
+	btAlignedObjectArray<int> m_indices;
+	//	btAlignedObjectArray<int>	m_connectedFaces;
+	btScalar m_plane[4];
 };
 
-
-ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron
+ATTRIBUTE_ALIGNED16(class)
+btConvexPolyhedron
 {
-	public:
-		
+public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-		
+
 	btConvexPolyhedron();
-	virtual	~btConvexPolyhedron();
+	virtual ~btConvexPolyhedron();
 
-	btAlignedObjectArray<btVector3>	m_vertices;
-	btAlignedObjectArray<btFace>	m_faces;
+	btAlignedObjectArray<btVector3> m_vertices;
+	btAlignedObjectArray<btFace> m_faces;
 	btAlignedObjectArray<btVector3> m_uniqueEdges;
 
-	btVector3		m_localCenter;
-	btVector3		m_extents;
-	btScalar		m_radius;
-	btVector3		mC;
-	btVector3		mE;
+	btVector3 m_localCenter;
+	btVector3 m_extents;
+	btScalar m_radius;
+	btVector3 mC;
+	btVector3 mE;
 
-	void	initialize();
+	void initialize();
+	void initialize2();
 	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;
 };
 
-	
-#endif //_BT_POLYHEDRAL_FEATURES_H
-
-
+#endif  //_BT_POLYHEDRAL_FEATURES_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp
index 88018b4c624..f8fb0aa9fda 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp
@@ -13,9 +13,9 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-//#if defined (_WIN32) || defined (__i386__)
-//#define BT_USE_SSE_IN_API
-//#endif
+#if defined(_WIN32) || defined(__i386__)
+#define BT_USE_SSE_IN_API
+#endif
 
 #include "btConvexShape.h"
 #include "btTriangleShape.h"
@@ -27,30 +27,28 @@ subject to the following restrictions:
 #include "btConvexPointCloudShape.h"
 
 ///not supported on IBM SDK, until we fix the alignment of btVector3
-#if defined (__CELLOS_LV2__) && defined (__SPU__)
+#if defined(__CELLOS_LV2__) && defined(__SPU__)
 #include <spu_intrinsics.h>
-static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 )
+static inline vec_float4 vec_dot3(vec_float4 vec0, vec_float4 vec1)
 {
-    vec_float4 result;
-    result = spu_mul( vec0, vec1 );
-    result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result );
-    return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result );
+	vec_float4 result;
+	result = spu_mul(vec0, vec1);
+	result = spu_madd(spu_rlqwbyte(vec0, 4), spu_rlqwbyte(vec1, 4), result);
+	return spu_madd(spu_rlqwbyte(vec0, 8), spu_rlqwbyte(vec1, 8), result);
 }
-#endif //__SPU__
+#endif  //__SPU__
 
-btConvexShape::btConvexShape ()
+btConvexShape::btConvexShape()
 {
 }
 
 btConvexShape::~btConvexShape()
 {
-
 }
 
-
-void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max, btVector3& witnesPtMin,btVector3& witnesPtMax) const
+void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max, btVector3& witnesPtMin, btVector3& witnesPtMax) const
 {
-	btVector3 localAxis = dir*trans.getBasis();
+	btVector3 localAxis = dir * trans.getBasis();
 	btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
 	btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis));
 
@@ -58,8 +56,8 @@ void btConvexShape::project(const btTransform& trans, const btVector3& dir, btSc
 	max = vtx2.dot(dir);
 	witnesPtMax = vtx2;
 	witnesPtMin = vtx1;
-	
-	if(min>max)
+
+	if (min > max)
 	{
 		btScalar tmp = min;
 		min = max;
@@ -69,391 +67,392 @@ void btConvexShape::project(const btTransform& trans, const btVector3& dir, btSc
 	}
 }
 
-
-static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling)
-{	
-
+static btVector3 convexHullSupport(const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling)
+{
 	btVector3 vec = localDirOrg * localScaling;
 
-#if defined (__CELLOS_LV2__) && defined (__SPU__)
+#if defined(__CELLOS_LV2__) && defined(__SPU__)
 
 	btVector3 localDir = vec;
 
-	vec_float4 v_distMax = {-FLT_MAX,0,0,0};
-	vec_int4 v_idxMax = {-999,0,0,0};
-	int v=0;
+	vec_float4 v_distMax = {-FLT_MAX, 0, 0, 0};
+	vec_int4 v_idxMax = {-999, 0, 0, 0};
+	int v = 0;
 	int numverts = numPoints;
 
-	for(;v<(int)numverts-4;v+=4) {
-		vec_float4 p0 = vec_dot3(points[v  ].get128(),localDir.get128());
-		vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128());
-		vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128());
-		vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128());
-		const vec_int4 i0 = {v  ,0,0,0};
-		const vec_int4 i1 = {v+1,0,0,0};
-		const vec_int4 i2 = {v+2,0,0,0};
-		const vec_int4 i3 = {v+3,0,0,0};
-		vec_uint4  retGt01 = spu_cmpgt(p0,p1);
-		vec_float4 pmax01 = spu_sel(p1,p0,retGt01);
-		vec_int4   imax01 = spu_sel(i1,i0,retGt01);
-		vec_uint4  retGt23 = spu_cmpgt(p2,p3);
-		vec_float4 pmax23 = spu_sel(p3,p2,retGt23);
-		vec_int4   imax23 = spu_sel(i3,i2,retGt23);
-		vec_uint4  retGt0123 = spu_cmpgt(pmax01,pmax23);
-		vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123);
-		vec_int4   imax0123 = spu_sel(imax23,imax01,retGt0123);
-		vec_uint4  retGtMax = spu_cmpgt(v_distMax,pmax0123);
-		v_distMax = spu_sel(pmax0123,v_distMax,retGtMax);
-		v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax);
+	for (; v < (int)numverts - 4; v += 4)
+	{
+		vec_float4 p0 = vec_dot3(points[v].get128(), localDir.get128());
+		vec_float4 p1 = vec_dot3(points[v + 1].get128(), localDir.get128());
+		vec_float4 p2 = vec_dot3(points[v + 2].get128(), localDir.get128());
+		vec_float4 p3 = vec_dot3(points[v + 3].get128(), localDir.get128());
+		const vec_int4 i0 = {v, 0, 0, 0};
+		const vec_int4 i1 = {v + 1, 0, 0, 0};
+		const vec_int4 i2 = {v + 2, 0, 0, 0};
+		const vec_int4 i3 = {v + 3, 0, 0, 0};
+		vec_uint4 retGt01 = spu_cmpgt(p0, p1);
+		vec_float4 pmax01 = spu_sel(p1, p0, retGt01);
+		vec_int4 imax01 = spu_sel(i1, i0, retGt01);
+		vec_uint4 retGt23 = spu_cmpgt(p2, p3);
+		vec_float4 pmax23 = spu_sel(p3, p2, retGt23);
+		vec_int4 imax23 = spu_sel(i3, i2, retGt23);
+		vec_uint4 retGt0123 = spu_cmpgt(pmax01, pmax23);
+		vec_float4 pmax0123 = spu_sel(pmax23, pmax01, retGt0123);
+		vec_int4 imax0123 = spu_sel(imax23, imax01, retGt0123);
+		vec_uint4 retGtMax = spu_cmpgt(v_distMax, pmax0123);
+		v_distMax = spu_sel(pmax0123, v_distMax, retGtMax);
+		v_idxMax = spu_sel(imax0123, v_idxMax, retGtMax);
 	}
-	for(;v<(int)numverts;v++) {
-		vec_float4 p = vec_dot3(points[v].get128(),localDir.get128());
-		const vec_int4 i = {v,0,0,0};
-		vec_uint4  retGtMax = spu_cmpgt(v_distMax,p);
-		v_distMax = spu_sel(p,v_distMax,retGtMax);
-		v_idxMax = spu_sel(i,v_idxMax,retGtMax);
+	for (; v < (int)numverts; v++)
+	{
+		vec_float4 p = vec_dot3(points[v].get128(), localDir.get128());
+		const vec_int4 i = {v, 0, 0, 0};
+		vec_uint4 retGtMax = spu_cmpgt(v_distMax, p);
+		v_distMax = spu_sel(p, v_distMax, retGtMax);
+		v_idxMax = spu_sel(i, v_idxMax, retGtMax);
 	}
-	int ptIndex = spu_extract(v_idxMax,0);
-	const btVector3& supVec= points[ptIndex] * localScaling;
+	int ptIndex = spu_extract(v_idxMax, 0);
+	const btVector3& supVec = points[ptIndex] * localScaling;
 	return supVec;
 #else
 
-    btScalar maxDot;
-    long ptIndex = vec.maxDot( points, numPoints, maxDot);
+	btScalar maxDot;
+	long ptIndex = vec.maxDot(points, numPoints, maxDot);
 	btAssert(ptIndex >= 0);
+	if (ptIndex < 0)
+	{
+		ptIndex = 0;
+	}
 	btVector3 supVec = points[ptIndex] * localScaling;
 	return supVec;
-#endif //__SPU__
+#endif  //__SPU__
 }
 
-btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const
+btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual(const btVector3& localDir) const
 {
 	switch (m_shapeType)
 	{
-    case SPHERE_SHAPE_PROXYTYPE:
-	{
-		return btVector3(0,0,0);
-    }
-	case BOX_SHAPE_PROXYTYPE:
-	{
-		btBoxShape* convexShape = (btBoxShape*)this;
-		const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
-
-#if defined( __APPLE__ ) && (defined( BT_USE_SSE )||defined( BT_USE_NEON ))
-    #if defined( BT_USE_SSE )
-            return btVector3( _mm_xor_ps( _mm_and_ps( localDir.mVec128, (__m128){-0.0f, -0.0f, -0.0f, -0.0f }), halfExtents.mVec128 ));
-    #elif defined( BT_USE_NEON )
-            return btVector3( (float32x4_t) (((uint32x4_t) localDir.mVec128 & (uint32x4_t){ 0x80000000, 0x80000000, 0x80000000, 0x80000000}) ^ (uint32x4_t) halfExtents.mVec128 ));
-    #else
-        #error unknown vector arch
-    #endif
-#else
-		return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()),
-			btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()),
-			btFsels(localDir.z(), halfExtents.z(), -halfExtents.z()));
-#endif
-	}
-	case TRIANGLE_SHAPE_PROXYTYPE:
-	{
-		btTriangleShape* triangleShape = (btTriangleShape*)this;
-		btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
-		btVector3* vertices = &triangleShape->m_vertices1[0];
-        btVector3 dots = dir.dot3(vertices[0], vertices[1], vertices[2]);
-		btVector3 sup = vertices[dots.maxAxis()];
-		return btVector3(sup.getX(),sup.getY(),sup.getZ());
-	}
-	case CYLINDER_SHAPE_PROXYTYPE:
-	{
-		btCylinderShape* cylShape = (btCylinderShape*)this;
-		//mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
-
-		btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
-		btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
-		int cylinderUpAxis = cylShape->getUpAxis();
-		int XX(1),YY(0),ZZ(2);
-
-		switch (cylinderUpAxis)
+		case SPHERE_SHAPE_PROXYTYPE:
 		{
-		case 0:
-		{
-			XX = 1;
-			YY = 0;
-			ZZ = 2;
+			return btVector3(0, 0, 0);
 		}
-		break;
-		case 1:
+		case BOX_SHAPE_PROXYTYPE:
 		{
-			XX = 0;
-			YY = 1;
-			ZZ = 2;	
+			btBoxShape* convexShape = (btBoxShape*)this;
+			const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
+
+#if defined(__APPLE__) && (defined(BT_USE_SSE) || defined(BT_USE_NEON))
+#if defined(BT_USE_SSE)
+			return btVector3(_mm_xor_ps(_mm_and_ps(localDir.mVec128, (__m128){-0.0f, -0.0f, -0.0f, -0.0f}), halfExtents.mVec128));
+#elif defined(BT_USE_NEON)
+			return btVector3((float32x4_t)(((uint32x4_t)localDir.mVec128 & (uint32x4_t){0x80000000, 0x80000000, 0x80000000, 0x80000000}) ^ (uint32x4_t)halfExtents.mVec128));
+#else
+#error unknown vector arch
+#endif
+#else
+			return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()),
+							 btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()),
+							 btFsels(localDir.z(), halfExtents.z(), -halfExtents.z()));
+#endif
 		}
-		break;
-		case 2:
+		case TRIANGLE_SHAPE_PROXYTYPE:
 		{
-			XX = 0;
-			YY = 2;
-			ZZ = 1;
-			
+			btTriangleShape* triangleShape = (btTriangleShape*)this;
+			btVector3 dir(localDir.getX(), localDir.getY(), localDir.getZ());
+			btVector3* vertices = &triangleShape->m_vertices1[0];
+			btVector3 dots = dir.dot3(vertices[0], vertices[1], vertices[2]);
+			btVector3 sup = vertices[dots.maxAxis()];
+			return btVector3(sup.getX(), sup.getY(), sup.getZ());
 		}
-		break;
-		default:
-			btAssert(0);
-		break;
-		};
-
-		btScalar radius = halfExtents[XX];
-		btScalar halfHeight = halfExtents[cylinderUpAxis];
+		case CYLINDER_SHAPE_PROXYTYPE:
+		{
+			btCylinderShape* cylShape = (btCylinderShape*)this;
+			//mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
 
-		btVector3 tmp;
-		btScalar d ;
+			btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
+			btVector3 v(localDir.getX(), localDir.getY(), localDir.getZ());
+			int cylinderUpAxis = cylShape->getUpAxis();
+			int XX(1), YY(0), ZZ(2);
 
-		btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
-		if (s != btScalar(0.0))
-		{
-			d = radius / s;  
-			tmp[XX] = v[XX] * d;
-			tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
-			tmp[ZZ] = v[ZZ] * d;
-			return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
-		} else {
-			tmp[XX] = radius;
-			tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
-			tmp[ZZ] = btScalar(0.0);
-			return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
+			switch (cylinderUpAxis)
+			{
+				case 0:
+				{
+					XX = 1;
+					YY = 0;
+					ZZ = 2;
+				}
+				break;
+				case 1:
+				{
+					XX = 0;
+					YY = 1;
+					ZZ = 2;
+				}
+				break;
+				case 2:
+				{
+					XX = 0;
+					YY = 2;
+					ZZ = 1;
+				}
+				break;
+				default:
+					btAssert(0);
+					break;
+			};
+
+			btScalar radius = halfExtents[XX];
+			btScalar halfHeight = halfExtents[cylinderUpAxis];
+
+			btVector3 tmp;
+			btScalar d;
+
+			btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
+			if (s != btScalar(0.0))
+			{
+				d = radius / s;
+				tmp[XX] = v[XX] * d;
+				tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+				tmp[ZZ] = v[ZZ] * d;
+				return btVector3(tmp.getX(), tmp.getY(), tmp.getZ());
+			}
+			else
+			{
+				tmp[XX] = radius;
+				tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+				tmp[ZZ] = btScalar(0.0);
+				return btVector3(tmp.getX(), tmp.getY(), tmp.getZ());
+			}
 		}
-	}
-	case CAPSULE_SHAPE_PROXYTYPE:
-	{
-		btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
+		case CAPSULE_SHAPE_PROXYTYPE:
+		{
+			btVector3 vec0(localDir.getX(), localDir.getY(), localDir.getZ());
 
-		btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
-		btScalar halfHeight = capsuleShape->getHalfHeight();
-		int capsuleUpAxis = capsuleShape->getUpAxis();
+			btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
+			btScalar halfHeight = capsuleShape->getHalfHeight();
+			int capsuleUpAxis = capsuleShape->getUpAxis();
 
-		btScalar radius = capsuleShape->getRadius();
-		btVector3 supVec(0,0,0);
+			btVector3 supVec(0, 0, 0);
 
-		btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
+			btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
 
-		btVector3 vec = vec0;
-		btScalar lenSqr = vec.length2();
-		if (lenSqr < btScalar(0.0001))
-		{
-			vec.setValue(1,0,0);
-		} else
-		{
-			btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
-			vec *= rlen;
-		}
-		btVector3 vtx;
-		btScalar newDot;
-		{
-			btVector3 pos(0,0,0);
-			pos[capsuleUpAxis] = halfHeight;
+			btVector3 vec = vec0;
+			btScalar lenSqr = vec.length2();
+			if (lenSqr < SIMD_EPSILON * SIMD_EPSILON)
+			{
+				vec.setValue(1, 0, 0);
+			}
+			else
+			{
+				btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
+				vec *= rlen;
+			}
+			btVector3 vtx;
+			btScalar newDot;
+			{
+				btVector3 pos(0, 0, 0);
+				pos[capsuleUpAxis] = halfHeight;
 
-			//vtx = pos +vec*(radius);
-			vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV();
-			newDot = vec.dot(vtx);
-			
+				vtx = pos;
+				newDot = vec.dot(vtx);
 
-			if (newDot > maxDot)
+				if (newDot > maxDot)
+				{
+					maxDot = newDot;
+					supVec = vtx;
+				}
+			}
 			{
-				maxDot = newDot;
-				supVec = vtx;
+				btVector3 pos(0, 0, 0);
+				pos[capsuleUpAxis] = -halfHeight;
+
+				vtx = pos;
+				newDot = vec.dot(vtx);
+				if (newDot > maxDot)
+				{
+					maxDot = newDot;
+					supVec = vtx;
+				}
 			}
+			return btVector3(supVec.getX(), supVec.getY(), supVec.getZ());
 		}
+		case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
 		{
-			btVector3 pos(0,0,0);
-			pos[capsuleUpAxis] = -halfHeight;
-
-			//vtx = pos +vec*(radius);
-			vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV();
-			newDot = vec.dot(vtx);
-			if (newDot > maxDot)
-			{
-				maxDot = newDot;
-				supVec = vtx;
-			}
+			btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
+			btVector3* points = convexPointCloudShape->getUnscaledPoints();
+			int numPoints = convexPointCloudShape->getNumPoints();
+			return convexHullSupport(localDir, points, numPoints, convexPointCloudShape->getLocalScalingNV());
 		}
-		return btVector3(supVec.getX(),supVec.getY(),supVec.getZ());	
-	}
-	case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
-	{
-		btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
-		btVector3* points = convexPointCloudShape->getUnscaledPoints ();
-		int numPoints = convexPointCloudShape->getNumPoints ();
-		return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV());
-	}
-	case CONVEX_HULL_SHAPE_PROXYTYPE:
-	{
-		btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
-		btVector3* points = convexHullShape->getUnscaledPoints();
-		int numPoints = convexHullShape->getNumPoints ();
-		return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV());
-	}
-    default:
+		case CONVEX_HULL_SHAPE_PROXYTYPE:
+		{
+			btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
+			btVector3* points = convexHullShape->getUnscaledPoints();
+			int numPoints = convexHullShape->getNumPoints();
+			return convexHullSupport(localDir, points, numPoints, convexHullShape->getLocalScalingNV());
+		}
+		default:
 #ifndef __SPU__
-		return this->localGetSupportingVertexWithoutMargin (localDir);
+			return this->localGetSupportingVertexWithoutMargin(localDir);
 #else
-		btAssert (0);
+			btAssert(0);
 #endif
 	}
 
 	// should never reach here
-	btAssert (0);
-	return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
+	btAssert(0);
+	return btVector3(btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
 }
 
-btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const
+btVector3 btConvexShape::localGetSupportVertexNonVirtual(const btVector3& localDir) const
 {
 	btVector3 localDirNorm = localDir;
-	if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+	if (localDirNorm.length2() < (SIMD_EPSILON * SIMD_EPSILON))
 	{
-		localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
+		localDirNorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.));
 	}
-	localDirNorm.normalize ();
+	localDirNorm.normalize();
 
-	return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm;
+	return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm) + getMarginNonVirtual() * localDirNorm;
 }
 
 /* TODO: This should be bumped up to btCollisionShape () */
-btScalar btConvexShape::getMarginNonVirtual () const
+btScalar btConvexShape::getMarginNonVirtual() const
 {
 	switch (m_shapeType)
 	{
-    case SPHERE_SHAPE_PROXYTYPE:
-	{
-		btSphereShape* sphereShape = (btSphereShape*)this;
-		return sphereShape->getRadius ();
-	}
-	case BOX_SHAPE_PROXYTYPE:
-	{
-		btBoxShape* convexShape = (btBoxShape*)this;
-		return convexShape->getMarginNV ();
-	}
-	case TRIANGLE_SHAPE_PROXYTYPE:
-	{
-		btTriangleShape* triangleShape = (btTriangleShape*)this;
-		return triangleShape->getMarginNV ();
-	}
-	case CYLINDER_SHAPE_PROXYTYPE:
-	{
-		btCylinderShape* cylShape = (btCylinderShape*)this;
-		return cylShape->getMarginNV();
-	}
-	case CONE_SHAPE_PROXYTYPE:
-	{
-		btConeShape* conShape = (btConeShape*)this;
-		return conShape->getMarginNV();
-	}
-	case CAPSULE_SHAPE_PROXYTYPE:
-	{
-		btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
-		return capsuleShape->getMarginNV();
-	}
-	case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
-	/* fall through */
-	case CONVEX_HULL_SHAPE_PROXYTYPE:
-	{
-		btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this;
-		return convexHullShape->getMarginNV();
-	}
-    default:
+		case SPHERE_SHAPE_PROXYTYPE:
+		{
+			btSphereShape* sphereShape = (btSphereShape*)this;
+			return sphereShape->getRadius();
+		}
+		case BOX_SHAPE_PROXYTYPE:
+		{
+			btBoxShape* convexShape = (btBoxShape*)this;
+			return convexShape->getMarginNV();
+		}
+		case TRIANGLE_SHAPE_PROXYTYPE:
+		{
+			btTriangleShape* triangleShape = (btTriangleShape*)this;
+			return triangleShape->getMarginNV();
+		}
+		case CYLINDER_SHAPE_PROXYTYPE:
+		{
+			btCylinderShape* cylShape = (btCylinderShape*)this;
+			return cylShape->getMarginNV();
+		}
+		case CONE_SHAPE_PROXYTYPE:
+		{
+			btConeShape* conShape = (btConeShape*)this;
+			return conShape->getMarginNV();
+		}
+		case CAPSULE_SHAPE_PROXYTYPE:
+		{
+			btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
+			return capsuleShape->getMarginNV();
+		}
+		case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
+		/* fall through */
+		case CONVEX_HULL_SHAPE_PROXYTYPE:
+		{
+			btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this;
+			return convexHullShape->getMarginNV();
+		}
+		default:
 #ifndef __SPU__
-		return this->getMargin ();
+			return this->getMargin();
 #else
-		btAssert (0);
+			btAssert(0);
 #endif
 	}
 
 	// should never reach here
-	btAssert (0);
+	btAssert(0);
 	return btScalar(0.0f);
 }
 #ifndef __SPU__
-void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
+void btConvexShape::getAabbNonVirtual(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 {
 	switch (m_shapeType)
 	{
-    case SPHERE_SHAPE_PROXYTYPE:
-	{
-		btSphereShape* sphereShape = (btSphereShape*)this;
-		btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
-		btScalar margin = radius + sphereShape->getMarginNonVirtual();
-		const btVector3& center = t.getOrigin();
-		btVector3 extent(margin,margin,margin);
-		aabbMin = center - extent;
-		aabbMax = center + extent;
-    }
-	break;
-	case CYLINDER_SHAPE_PROXYTYPE:
-	/* fall through */
-	case BOX_SHAPE_PROXYTYPE:
-	{
-		btBoxShape* convexShape = (btBoxShape*)this;
-		btScalar margin=convexShape->getMarginNonVirtual();
-		btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
-		halfExtents += btVector3(margin,margin,margin);
-		btMatrix3x3 abs_b = t.getBasis().absolute();  
-		btVector3 center = t.getOrigin();
-        btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);    
-        
-		aabbMin = center - extent;
-		aabbMax = center + extent;
+		case SPHERE_SHAPE_PROXYTYPE:
+		{
+			btSphereShape* sphereShape = (btSphereShape*)this;
+			btScalar radius = sphereShape->getImplicitShapeDimensions().getX();  // * convexShape->getLocalScaling().getX();
+			btScalar margin = radius + sphereShape->getMarginNonVirtual();
+			const btVector3& center = t.getOrigin();
+			btVector3 extent(margin, margin, margin);
+			aabbMin = center - extent;
+			aabbMax = center + extent;
+		}
 		break;
-	}
-	case TRIANGLE_SHAPE_PROXYTYPE:
-	{
-		btTriangleShape* triangleShape = (btTriangleShape*)this;
-		btScalar margin = triangleShape->getMarginNonVirtual();
-		for (int i=0;i<3;i++)
+		case CYLINDER_SHAPE_PROXYTYPE:
+		/* fall through */
+		case BOX_SHAPE_PROXYTYPE:
 		{
-			btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
-			vec[i] = btScalar(1.);
+			btBoxShape* convexShape = (btBoxShape*)this;
+			btScalar margin = convexShape->getMarginNonVirtual();
+			btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
+			halfExtents += btVector3(margin, margin, margin);
+			btMatrix3x3 abs_b = t.getBasis().absolute();
+			btVector3 center = t.getOrigin();
+			btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
+
+			aabbMin = center - extent;
+			aabbMax = center + extent;
+			break;
+		}
+		case TRIANGLE_SHAPE_PROXYTYPE:
+		{
+			btTriangleShape* triangleShape = (btTriangleShape*)this;
+			btScalar margin = triangleShape->getMarginNonVirtual();
+			for (int i = 0; i < 3; i++)
+			{
+				btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.));
+				vec[i] = btScalar(1.);
 
-			btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis());
+				btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec * t.getBasis());
 
-			btVector3 tmp = t(sv);
-			aabbMax[i] = tmp[i]+margin;
-			vec[i] = btScalar(-1.);
-			tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()));
-			aabbMin[i] = tmp[i]-margin;
-		}	
-	}
-	break;
-	case CAPSULE_SHAPE_PROXYTYPE:
-	{
-		btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
-		btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius());
-		int m_upAxis = capsuleShape->getUpAxis();
-		halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight();
-		halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual());
-		btMatrix3x3 abs_b = t.getBasis().absolute();  
-		btVector3 center = t.getOrigin();
-        btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);    
-		aabbMin = center - extent;
-		aabbMax = center + extent;
-	}
-	break;
-	case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
-	case CONVEX_HULL_SHAPE_PROXYTYPE:
-	{
-		btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this;
-		btScalar margin = convexHullShape->getMarginNonVirtual();
-		convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin);
-	}
-	break;
-    default:
+				btVector3 tmp = t(sv);
+				aabbMax[i] = tmp[i] + margin;
+				vec[i] = btScalar(-1.);
+				tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec * t.getBasis()));
+				aabbMin[i] = tmp[i] - margin;
+			}
+		}
+		break;
+		case CAPSULE_SHAPE_PROXYTYPE:
+		{
+			btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
+			btVector3 halfExtents(capsuleShape->getRadius(), capsuleShape->getRadius(), capsuleShape->getRadius());
+			int m_upAxis = capsuleShape->getUpAxis();
+			halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight();
+			btMatrix3x3 abs_b = t.getBasis().absolute();
+			btVector3 center = t.getOrigin();
+			btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
+			aabbMin = center - extent;
+			aabbMax = center + extent;
+		}
+		break;
+		case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
+		case CONVEX_HULL_SHAPE_PROXYTYPE:
+		{
+			btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this;
+			btScalar margin = convexHullShape->getMarginNonVirtual();
+			convexHullShape->getNonvirtualAabb(t, aabbMin, aabbMax, margin);
+		}
+		break;
+		default:
 #ifndef __SPU__
-		this->getAabb (t, aabbMin, aabbMax);
+			this->getAabb(t, aabbMin, aabbMax);
 #else
-		btAssert (0);
+			btAssert(0);
 #endif
-	break;
+			break;
 	}
 
 	// should never reach here
-	btAssert (0);
+	btAssert(0);
 }
 
-#endif //__SPU__
+#endif  //__SPU__
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h
index 875f2ac195a..d3b3ed816ec 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h
@@ -28,58 +28,48 @@ subject to the following restrictions:
 
 /// The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape, btConvexHullShape etc.
 /// It describes general convex shapes using the localGetSupportingVertex interface, used by collision detectors such as btGjkPairDetector.
-ATTRIBUTE_ALIGNED16(class) btConvexShape : public btCollisionShape
+ATTRIBUTE_ALIGNED16(class)
+btConvexShape : public btCollisionShape
 {
-
-
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btConvexShape ();
+	btConvexShape();
 
 	virtual ~btConvexShape();
 
-	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const = 0;
-
-	////////
-	#ifndef __SPU__
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec) const=0;
-	#endif //#ifndef __SPU__
+	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const = 0;
 
-	btVector3 localGetSupportVertexWithoutMarginNonVirtual (const btVector3& vec) const;
-	btVector3 localGetSupportVertexNonVirtual (const btVector3& vec) const;
-	btScalar getMarginNonVirtual () const;
-	void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
+////////
+#ifndef __SPU__
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const = 0;
+#endif  //#ifndef __SPU__
 
+	btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3& vec) const;
+	btVector3 localGetSupportVertexNonVirtual(const btVector3& vec) const;
+	btScalar getMarginNonVirtual() const;
+	void getAabbNonVirtual(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
-	virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
+	virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const;
 
-	
 	//notice that the vectors should be unit length
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0;
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const = 0;
 
 	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
-	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0;
+	void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const = 0;
 
-	virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0;
+	virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const = 0;
 
-	virtual void	setLocalScaling(const btVector3& scaling) =0;
-	virtual const btVector3& getLocalScaling() const =0;
+	virtual void setLocalScaling(const btVector3& scaling) = 0;
+	virtual const btVector3& getLocalScaling() const = 0;
 
-	virtual void	setMargin(btScalar margin)=0;
+	virtual void setMargin(btScalar margin) = 0;
 
-	virtual btScalar	getMargin() const=0;
+	virtual btScalar getMargin() const = 0;
 
-	virtual int		getNumPreferredPenetrationDirections() const=0;
-	
-	virtual void	getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const=0;
+	virtual int getNumPreferredPenetrationDirections() const = 0;
 
-
-	
-	
+	virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const = 0;
 };
 
-
-
-#endif //BT_CONVEX_SHAPE_INTERFACE1
+#endif  //BT_CONVEX_SHAPE_INTERFACE1
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp
index 0f9ced554b8..f6987cc760c 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp
@@ -19,42 +19,37 @@ subject to the following restrictions:
 #include "LinearMath/btQuaternion.h"
 #include "BulletCollision/CollisionShapes/btStridingMeshInterface.h"
 
-
-btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb)
-: btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface)
+btConvexTriangleMeshShape ::btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface, bool calcAabb)
+	: btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface)
 {
 	m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE;
-	if ( calcAabb )
+	if (calcAabb)
 		recalcLocalAabb();
 }
 
-
-
-
 ///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once
 ///but then we are duplicating
-class LocalSupportVertexCallback: public btInternalTriangleIndexCallback
+class LocalSupportVertexCallback : public btInternalTriangleIndexCallback
 {
-
 	btVector3 m_supportVertexLocal;
-public:
 
+public:
 	btScalar m_maxDot;
 	btVector3 m_supportVecLocal;
 
 	LocalSupportVertexCallback(const btVector3& supportVecLocal)
-		: m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)),
-		m_maxDot(btScalar(-BT_LARGE_FLOAT)),
-                m_supportVecLocal(supportVecLocal)
+		: m_supportVertexLocal(btScalar(0.), btScalar(0.), btScalar(0.)),
+		  m_maxDot(btScalar(-BT_LARGE_FLOAT)),
+		  m_supportVecLocal(supportVecLocal)
 	{
 	}
 
-	virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+	virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
 	{
 		(void)triangleIndex;
 		(void)partId;
 
-		for (int i=0;i<3;i++)
+		for (int i = 0; i < 3; i++)
 		{
 			btScalar dot = m_supportVecLocal.dot(triangle[i]);
 			if (dot > m_maxDot)
@@ -64,99 +59,82 @@ public:
 			}
 		}
 	}
-	
-	btVector3	GetSupportVertexLocal()
+
+	btVector3 GetSupportVertexLocal()
 	{
 		return m_supportVertexLocal;
 	}
-
 };
 
-
-
-
-
-btVector3	btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const
 {
-	btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
+	btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.));
 
 	btVector3 vec = vec0;
 	btScalar lenSqr = vec.length2();
 	if (lenSqr < btScalar(0.0001))
 	{
-		vec.setValue(1,0,0);
-	} else
+		vec.setValue(1, 0, 0);
+	}
+	else
 	{
-		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+		btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
 		vec *= rlen;
 	}
 
-	LocalSupportVertexCallback	supportCallback(vec);
-	btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-	m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
+	LocalSupportVertexCallback supportCallback(vec);
+	btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+	m_stridingMesh->InternalProcessAllTriangles(&supportCallback, -aabbMax, aabbMax);
 	supVec = supportCallback.GetSupportVertexLocal();
 
 	return supVec;
 }
 
-void	btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 {
 	//use 'w' component of supportVerticesOut?
 	{
-		for (int i=0;i<numVectors;i++)
+		for (int i = 0; i < numVectors; i++)
 		{
 			supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
 		}
 	}
-	
-	///@todo: could do the batch inside the callback!
 
+	///@todo: could do the batch inside the callback!
 
-	for (int j=0;j<numVectors;j++)
+	for (int j = 0; j < numVectors; j++)
 	{
 		const btVector3& vec = vectors[j];
-		LocalSupportVertexCallback	supportCallback(vec);
-		btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-		m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
+		LocalSupportVertexCallback supportCallback(vec);
+		btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+		m_stridingMesh->InternalProcessAllTriangles(&supportCallback, -aabbMax, aabbMax);
 		supportVerticesOut[j] = supportCallback.GetSupportVertexLocal();
 	}
-	
 }
-	
 
-
-btVector3	btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const
+btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) const
 {
 	btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
 
-	if ( getMargin()!=btScalar(0.) )
+	if (getMargin() != btScalar(0.))
 	{
 		btVector3 vecnorm = vec;
-		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+		if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON))
 		{
-			vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
-		} 
+			vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.));
+		}
 		vecnorm.normalize();
-		supVertex+= getMargin() * vecnorm;
+		supVertex += getMargin() * vecnorm;
 	}
 	return supVertex;
 }
 
-
-
-
-
-
-
-
-
 //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
 //Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo
-int	btConvexTriangleMeshShape::getNumVertices() const
+int btConvexTriangleMeshShape::getNumVertices() const
 {
 	//cache this?
 	return 0;
-	
 }
 
 int btConvexTriangleMeshShape::getNumEdges() const
@@ -164,44 +142,40 @@ int btConvexTriangleMeshShape::getNumEdges() const
 	return 0;
 }
 
-void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const
+void btConvexTriangleMeshShape::getEdge(int, btVector3&, btVector3&) const
 {
-	btAssert(0);	
+	btAssert(0);
 }
 
-void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const
+void btConvexTriangleMeshShape::getVertex(int, btVector3&) const
 {
 	btAssert(0);
 }
 
-int	btConvexTriangleMeshShape::getNumPlanes() const
+int btConvexTriangleMeshShape::getNumPlanes() const
 {
 	return 0;
 }
 
-void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int  ) const
+void btConvexTriangleMeshShape::getPlane(btVector3&, btVector3&, int) const
 {
 	btAssert(0);
 }
 
 //not yet
-bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const
+bool btConvexTriangleMeshShape::isInside(const btVector3&, btScalar) const
 {
 	btAssert(0);
 	return false;
 }
 
-
-
-void	btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling)
+void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling)
 {
 	m_stridingMesh->setScaling(scaling);
-	
+
 	recalcLocalAabb();
-	
 }
 
-
 const btVector3& btConvexTriangleMeshShape::getLocalScaling() const
 {
 	return m_stridingMesh->getScaling();
@@ -209,107 +183,101 @@ const btVector3& btConvexTriangleMeshShape::getLocalScaling() const
 
 void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const
 {
-   class CenterCallback: public btInternalTriangleIndexCallback
-   {
-      bool first;
-      btVector3 ref;
-      btVector3 sum;
-      btScalar volume;
-
-   public:
-
-      CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0)
-      {
-      }
-
-      virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
-      {
-         (void) triangleIndex;
-         (void) partId;
-         if (first)
-         {
-            ref = triangle[0];
-            first = false;
-         }
-         else
-         {
-            btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref));
-            sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
-            volume += vol;
-         }
-      }
-      
-      btVector3 getCenter()
-      {
-         return (volume > 0) ? sum / volume : ref;
-      }
-
-      btScalar getVolume()
-      {
-         return volume * btScalar(1. / 6);
-      }
-
-   };
-
-   class InertiaCallback: public btInternalTriangleIndexCallback
-   {
-      btMatrix3x3 sum;
-      btVector3 center;
-
-   public:
-
-      InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center)
-      {
-      }
-
-      virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
-      {
-         (void) triangleIndex;
-         (void) partId;
-         btMatrix3x3 i;
-         btVector3 a = triangle[0] - center;
-         btVector3 b = triangle[1] - center;
-         btVector3 c = triangle[2] - center;
-         btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
-         for (int j = 0; j < 3; j++)
-         {
-            for (int k = 0; k <= j; k++)
-            {
-               i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k])
-                  + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
-            }
-         }
-         btScalar i00 = -i[0][0];
-         btScalar i11 = -i[1][1];
-         btScalar i22 = -i[2][2];
-         i[0][0] = i11 + i22; 
-         i[1][1] = i22 + i00; 
-         i[2][2] = i00 + i11;
-         sum[0] += i[0];
-         sum[1] += i[1];
-         sum[2] += i[2];
-      }
-      
-      btMatrix3x3& getInertia()
-      {
-         return sum;
-      }
-
-   };
-
-   CenterCallback centerCallback;
-   btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-   m_stridingMesh->InternalProcessAllTriangles(&centerCallback, -aabbMax, aabbMax);
-   btVector3 center = centerCallback.getCenter();
-   principal.setOrigin(center);
-   volume = centerCallback.getVolume();
-
-   InertiaCallback inertiaCallback(center);
-   m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax);
-
-   btMatrix3x3& i = inertiaCallback.getInertia();
-   i.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
-   inertia.setValue(i[0][0], i[1][1], i[2][2]);
-   inertia /= volume;
-}
+	class CenterCallback : public btInternalTriangleIndexCallback
+	{
+		bool first;
+		btVector3 ref;
+		btVector3 sum;
+		btScalar volume;
+
+	public:
+		CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0)
+		{
+		}
+
+		virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
+		{
+			(void)triangleIndex;
+			(void)partId;
+			if (first)
+			{
+				ref = triangle[0];
+				first = false;
+			}
+			else
+			{
+				btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref));
+				sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
+				volume += vol;
+			}
+		}
 
+		btVector3 getCenter()
+		{
+			return (volume > 0) ? sum / volume : ref;
+		}
+
+		btScalar getVolume()
+		{
+			return volume * btScalar(1. / 6);
+		}
+	};
+
+	class InertiaCallback : public btInternalTriangleIndexCallback
+	{
+		btMatrix3x3 sum;
+		btVector3 center;
+
+	public:
+		InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center)
+		{
+		}
+
+		virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
+		{
+			(void)triangleIndex;
+			(void)partId;
+			btMatrix3x3 i;
+			btVector3 a = triangle[0] - center;
+			btVector3 b = triangle[1] - center;
+			btVector3 c = triangle[2] - center;
+			btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
+			for (int j = 0; j < 3; j++)
+			{
+				for (int k = 0; k <= j; k++)
+				{
+					i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
+				}
+			}
+			btScalar i00 = -i[0][0];
+			btScalar i11 = -i[1][1];
+			btScalar i22 = -i[2][2];
+			i[0][0] = i11 + i22;
+			i[1][1] = i22 + i00;
+			i[2][2] = i00 + i11;
+			sum[0] += i[0];
+			sum[1] += i[1];
+			sum[2] += i[2];
+		}
+
+		btMatrix3x3& getInertia()
+		{
+			return sum;
+		}
+	};
+
+	CenterCallback centerCallback;
+	btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+	m_stridingMesh->InternalProcessAllTriangles(&centerCallback, -aabbMax, aabbMax);
+	btVector3 center = centerCallback.getCenter();
+	principal.setOrigin(center);
+	volume = centerCallback.getVolume();
+
+	InertiaCallback inertiaCallback(center);
+	m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax);
+
+	btMatrix3x3& i = inertiaCallback.getInertia();
+	i.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
+	inertia.setValue(i[0][0], i[1][1], i[2][2]);
+	inertia /= volume;
+}
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h
index f338865ca13..6dac9fff040 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h
@@ -15,24 +15,22 @@ subject to the following restrictions:
 #ifndef BT_CONVEX_TRIANGLEMESH_SHAPE_H
 #define BT_CONVEX_TRIANGLEMESH_SHAPE_H
 
-
 #include "btPolyhedralConvexShape.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
-
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"  // for the types
 
 /// The btConvexTriangleMeshShape is a convex hull of a triangle mesh, but the performance is not as good as btConvexHullShape.
 /// A small benefit of this class is that it uses the btStridingMeshInterface, so you can avoid the duplication of the triangle mesh data. Nevertheless, most users should use the much better performing btConvexHullShape instead.
-ATTRIBUTE_ALIGNED16(class) btConvexTriangleMeshShape : public btPolyhedralConvexAabbCachingShape
+ATTRIBUTE_ALIGNED16(class)
+btConvexTriangleMeshShape : public btPolyhedralConvexAabbCachingShape
 {
-
-	class btStridingMeshInterface*	m_stridingMesh;
+	class btStridingMeshInterface* m_stridingMesh;
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-	btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface, bool calcAabb = true);
 
-	class btStridingMeshInterface*	getMeshInterface()
+	btConvexTriangleMeshShape(btStridingMeshInterface * meshInterface, bool calcAabb = true);
+
+	class btStridingMeshInterface* getMeshInterface()
 	{
 		return m_stridingMesh;
 	}
@@ -40,24 +38,23 @@ public:
 	{
 		return m_stridingMesh;
 	}
-	
-	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
-	
+
+	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
+
 	//debugging
-	virtual const char*	getName()const {return "ConvexTrimesh";}
-	
-	virtual int	getNumVertices() const;
+	virtual const char* getName() const { return "ConvexTrimesh"; }
+
+	virtual int getNumVertices() const;
 	virtual int getNumEdges() const;
-	virtual void getEdge(int i,btVector3& pa,btVector3& pb) const;
-	virtual void getVertex(int i,btVector3& vtx) const;
-	virtual int	getNumPlanes() const;
-	virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const;
-	virtual	bool isInside(const btVector3& pt,btScalar tolerance) const;
+	virtual void getEdge(int i, btVector3& pa, btVector3& pb) const;
+	virtual void getVertex(int i, btVector3& vtx) const;
+	virtual int getNumPlanes() const;
+	virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const;
+	virtual bool isInside(const btVector3& pt, btScalar tolerance) const;
 
-	
-	virtual void	setLocalScaling(const btVector3& scaling);
+	virtual void setLocalScaling(const btVector3& scaling);
 	virtual const btVector3& getLocalScaling() const;
 
 	///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
@@ -65,13 +62,7 @@ public:
 	///by the mass. The resulting transform "principal" has to be applied inversely to the mesh in order for the local coordinate system of the
 	///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
 	///of the collision object by the principal transform. This method also computes the volume of the convex mesh.
-	void calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const;
-
+	void calculatePrincipalAxisTransform(btTransform & principal, btVector3 & inertia, btScalar & volume) const;
 };
 
-
-
-#endif //BT_CONVEX_TRIANGLEMESH_SHAPE_H
-
-
-
+#endif  //BT_CONVEX_TRIANGLEMESH_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp
index 6cfe43be4da..66dbb8e53de 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp
@@ -15,41 +15,37 @@ subject to the following restrictions:
 
 #include "btCylinderShape.h"
 
-btCylinderShape::btCylinderShape (const btVector3& halfExtents)
-:btConvexInternalShape(),
-m_upAxis(1)
+btCylinderShape::btCylinderShape(const btVector3& halfExtents)
+	: btConvexInternalShape(),
+	  m_upAxis(1)
 {
+	btVector3 margin(getMargin(), getMargin(), getMargin());
+	m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin;
+
 	setSafeMargin(halfExtents);
 
-	btVector3 margin(getMargin(),getMargin(),getMargin());
-	m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin;
 	m_shapeType = CYLINDER_SHAPE_PROXYTYPE;
 }
 
-
-btCylinderShapeX::btCylinderShapeX (const btVector3& halfExtents)
-:btCylinderShape(halfExtents)
+btCylinderShapeX::btCylinderShapeX(const btVector3& halfExtents)
+	: btCylinderShape(halfExtents)
 {
 	m_upAxis = 0;
-
 }
 
-
-btCylinderShapeZ::btCylinderShapeZ (const btVector3& halfExtents)
-:btCylinderShape(halfExtents)
+btCylinderShapeZ::btCylinderShapeZ(const btVector3& halfExtents)
+	: btCylinderShape(halfExtents)
 {
 	m_upAxis = 2;
-
 }
 
-void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+void btCylinderShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 {
-	btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
+	btTransformAabb(getHalfExtentsWithoutMargin(), getMargin(), t, aabbMin, aabbMax);
 }
 
-void	btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btCylinderShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
-
 //Until Bullet 2.77 a box approximation was used, so uncomment this if you need backwards compatibility
 //#define USE_BOX_INERTIA_APPROXIMATION 1
 #ifndef USE_BOX_INERTIA_APPROXIMATION
@@ -63,25 +59,25 @@ void	btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co
 	*
 	*/
 
-	btScalar radius2;	// square of cylinder radius
-	btScalar height2;	// square of cylinder height
-	btVector3 halfExtents = getHalfExtentsWithMargin();	// get cylinder dimension
+	btScalar radius2;                                    // square of cylinder radius
+	btScalar height2;                                    // square of cylinder height
+	btVector3 halfExtents = getHalfExtentsWithMargin();  // get cylinder dimension
 	btScalar div12 = mass / 12.f;
 	btScalar div4 = mass / 4.f;
 	btScalar div2 = mass / 2.f;
 	int idxRadius, idxHeight;
 
-	switch (m_upAxis)	// get indices of radius and height of cylinder
+	switch (m_upAxis)  // get indices of radius and height of cylinder
 	{
-		case 0:		// cylinder is aligned along x
+		case 0:  // cylinder is aligned along x
 			idxRadius = 1;
 			idxHeight = 0;
 			break;
-		case 2:		// cylinder is aligned along z
+		case 2:  // cylinder is aligned along z
 			idxRadius = 0;
 			idxHeight = 2;
 			break;
-		default:	// cylinder is aligned along y
+		default:  // cylinder is aligned along y
 			idxRadius = 0;
 			idxHeight = 1;
 	}
@@ -94,188 +90,164 @@ void	btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co
 	btScalar t1 = div12 * height2 + div4 * radius2;
 	btScalar t2 = div2 * radius2;
 
-	switch (m_upAxis)	// set diagonal elements of inertia tensor
+	switch (m_upAxis)  // set diagonal elements of inertia tensor
 	{
-		case 0:		// cylinder is aligned along x
-			inertia.setValue(t2,t1,t1);
+		case 0:  // cylinder is aligned along x
+			inertia.setValue(t2, t1, t1);
 			break;
-		case 2:		// cylinder is aligned along z
-			inertia.setValue(t1,t1,t2);
+		case 2:  // cylinder is aligned along z
+			inertia.setValue(t1, t1, t2);
 			break;
-		default:	// cylinder is aligned along y
-			inertia.setValue(t1,t2,t1);
+		default:  // cylinder is aligned along y
+			inertia.setValue(t1, t2, t1);
 	}
-#else //USE_BOX_INERTIA_APPROXIMATION
+#else   //USE_BOX_INERTIA_APPROXIMATION
 	//approximation of box shape
 	btVector3 halfExtents = getHalfExtentsWithMargin();
 
-	btScalar lx=btScalar(2.)*(halfExtents.x());
-	btScalar ly=btScalar(2.)*(halfExtents.y());
-	btScalar lz=btScalar(2.)*(halfExtents.z());
+	btScalar lx = btScalar(2.) * (halfExtents.x());
+	btScalar ly = btScalar(2.) * (halfExtents.y());
+	btScalar lz = btScalar(2.) * (halfExtents.z());
 
-	inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
-					mass/(btScalar(12.0)) * (lx*lx + lz*lz),
-					mass/(btScalar(12.0)) * (lx*lx + ly*ly));
-#endif //USE_BOX_INERTIA_APPROXIMATION
+	inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz),
+					 mass / (btScalar(12.0)) * (lx * lx + lz * lz),
+					 mass / (btScalar(12.0)) * (lx * lx + ly * ly));
+#endif  //USE_BOX_INERTIA_APPROXIMATION
 }
 
-
-SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v) 
+SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents, const btVector3& v)
 {
-const int cylinderUpAxis = 0;
-const int XX = 1;
-const int YY = 0;
-const int ZZ = 2;
+	const int cylinderUpAxis = 0;
+	const int XX = 1;
+	const int YY = 0;
+	const int ZZ = 2;
 
 	//mapping depends on how cylinder local orientation is
 	// extents of the cylinder is: X,Y is for radius, and Z for height
 
-
 	btScalar radius = halfExtents[XX];
 	btScalar halfHeight = halfExtents[cylinderUpAxis];
 
+	btVector3 tmp;
+	btScalar d;
 
-    btVector3 tmp;
-	btScalar d ;
-
-    btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
-    if (s != btScalar(0.0))
+	btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
+	if (s != btScalar(0.0))
 	{
-        d = radius / s;  
+		d = radius / s;
 		tmp[XX] = v[XX] * d;
 		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
 		tmp[ZZ] = v[ZZ] * d;
 		return tmp;
 	}
-    else
+	else
 	{
-	    tmp[XX] = radius;
+		tmp[XX] = radius;
 		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
 		tmp[ZZ] = btScalar(0.0);
 		return tmp;
-    }
-
-
+	}
 }
 
-
-
-
-
-
-inline  btVector3 CylinderLocalSupportY(const btVector3& halfExtents,const btVector3& v) 
+inline btVector3 CylinderLocalSupportY(const btVector3& halfExtents, const btVector3& v)
 {
-
-const int cylinderUpAxis = 1;
-const int XX = 0;
-const int YY = 1;
-const int ZZ = 2;
-
+	const int cylinderUpAxis = 1;
+	const int XX = 0;
+	const int YY = 1;
+	const int ZZ = 2;
 
 	btScalar radius = halfExtents[XX];
 	btScalar halfHeight = halfExtents[cylinderUpAxis];
 
+	btVector3 tmp;
+	btScalar d;
 
-    btVector3 tmp;
-	btScalar d ;
-
-    btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
-    if (s != btScalar(0.0))
+	btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
+	if (s != btScalar(0.0))
 	{
-        d = radius / s;  
+		d = radius / s;
 		tmp[XX] = v[XX] * d;
 		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
 		tmp[ZZ] = v[ZZ] * d;
 		return tmp;
 	}
-    else
+	else
 	{
-	    tmp[XX] = radius;
+		tmp[XX] = radius;
 		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
 		tmp[ZZ] = btScalar(0.0);
 		return tmp;
-    }
-
+	}
 }
 
-inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents,const btVector3& v) 
+inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents, const btVector3& v)
 {
-const int cylinderUpAxis = 2;
-const int XX = 0;
-const int YY = 2;
-const int ZZ = 1;
+	const int cylinderUpAxis = 2;
+	const int XX = 0;
+	const int YY = 2;
+	const int ZZ = 1;
 
 	//mapping depends on how cylinder local orientation is
 	// extents of the cylinder is: X,Y is for radius, and Z for height
 
-
 	btScalar radius = halfExtents[XX];
 	btScalar halfHeight = halfExtents[cylinderUpAxis];
 
+	btVector3 tmp;
+	btScalar d;
 
-    btVector3 tmp;
-	btScalar d ;
-
-    btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
-    if (s != btScalar(0.0))
+	btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
+	if (s != btScalar(0.0))
 	{
-        d = radius / s;  
+		d = radius / s;
 		tmp[XX] = v[XX] * d;
 		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
 		tmp[ZZ] = v[ZZ] * d;
 		return tmp;
 	}
-    else
+	else
 	{
-	    tmp[XX] = radius;
+		tmp[XX] = radius;
 		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
 		tmp[ZZ] = btScalar(0.0);
 		return tmp;
-    }
-
-
+	}
 }
 
-btVector3	btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+btVector3 btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 {
-	return CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vec);
+	return CylinderLocalSupportX(getHalfExtentsWithoutMargin(), vec);
 }
 
-
-btVector3	btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+btVector3 btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 {
-	return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vec);
+	return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(), vec);
 }
-btVector3	btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+btVector3 btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 {
-	return CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vec);
+	return CylinderLocalSupportY(getHalfExtentsWithoutMargin(), vec);
 }
 
-void	btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 {
-	for (int i=0;i<numVectors;i++)
+	for (int i = 0; i < numVectors; i++)
 	{
-		supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vectors[i]);
+		supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtentsWithoutMargin(), vectors[i]);
 	}
 }
 
-void	btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 {
-	for (int i=0;i<numVectors;i++)
+	for (int i = 0; i < numVectors; i++)
 	{
-		supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vectors[i]);
+		supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtentsWithoutMargin(), vectors[i]);
 	}
 }
 
-
-
-
-void	btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 {
-	for (int i=0;i<numVectors;i++)
+	for (int i = 0; i < numVectors; i++)
 	{
-		supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vectors[i]);
+		supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtentsWithoutMargin(), vectors[i]);
 	}
 }
-
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h
index 6f796950e1d..d3f64508bae 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h
@@ -17,90 +17,84 @@ subject to the following restrictions:
 #define BT_CYLINDER_MINKOWSKI_H
 
 #include "btBoxShape.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"  // for the types
 #include "LinearMath/btVector3.h"
 
 /// The btCylinderShape class implements a cylinder shape primitive, centered around the origin. Its central axis aligned with the Y axis. btCylinderShapeX is aligned with the X axis and btCylinderShapeZ around the Z axis.
-ATTRIBUTE_ALIGNED16(class) btCylinderShape : public btConvexInternalShape
+ATTRIBUTE_ALIGNED16(class)
+btCylinderShape : public btConvexInternalShape
 
 {
-
 protected:
-
-	int	m_upAxis;
+	int m_upAxis;
 
 public:
-
-BT_DECLARE_ALIGNED_ALLOCATOR();
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	btVector3 getHalfExtentsWithMargin() const
 	{
 		btVector3 halfExtents = getHalfExtentsWithoutMargin();
-		btVector3 margin(getMargin(),getMargin(),getMargin());
+		btVector3 margin(getMargin(), getMargin(), getMargin());
 		halfExtents += margin;
 		return halfExtents;
 	}
-	
+
 	const btVector3& getHalfExtentsWithoutMargin() const
 	{
-		return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included
+		return m_implicitShapeDimensions;  //changed in Bullet 2.63: assume the scaling and margin are included
 	}
 
-	btCylinderShape (const btVector3& halfExtents);
-	
-	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+	btCylinderShape(const btVector3& halfExtents);
+
+	void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
 
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
 
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
 
 	virtual void setMargin(btScalar collisionMargin)
 	{
 		//correct the m_implicitShapeDimensions for the margin
-		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
-		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
-		
+		btVector3 oldMargin(getMargin(), getMargin(), getMargin());
+		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin;
+
 		btConvexInternalShape::setMargin(collisionMargin);
-		btVector3 newMargin(getMargin(),getMargin(),getMargin());
+		btVector3 newMargin(getMargin(), getMargin(), getMargin());
 		m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
-
 	}
 
-	virtual btVector3	localGetSupportingVertex(const btVector3& vec) const
+	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
 	{
-
 		btVector3 supVertex;
 		supVertex = localGetSupportingVertexWithoutMargin(vec);
-		
-		if ( getMargin()!=btScalar(0.) )
+
+		if (getMargin() != btScalar(0.))
 		{
 			btVector3 vecnorm = vec;
-			if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+			if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON))
 			{
-				vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
-			} 
+				vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.));
+			}
 			vecnorm.normalize();
-			supVertex+= getMargin() * vecnorm;
+			supVertex += getMargin() * vecnorm;
 		}
 		return supVertex;
 	}
 
-
 	//use box inertia
 	//	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
 
-
-	int	getUpAxis() const
+	int getUpAxis() const
 	{
 		return m_upAxis;
 	}
 
-	virtual btVector3	getAnisotropicRollingFrictionDirection() const
+	virtual btVector3 getAnisotropicRollingFrictionDirection() const
 	{
-		btVector3 aniDir(0,0,0);
-		aniDir[getUpAxis()]=1;
+		btVector3 aniDir(0, 0, 0);
+		aniDir[getUpAxis()] = 1;
 		return aniDir;
 	}
 
@@ -109,43 +103,41 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 		return getHalfExtentsWithMargin().getX();
 	}
 
-	virtual void	setLocalScaling(const btVector3& scaling)
+	virtual void setLocalScaling(const btVector3& scaling)
 	{
-		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
-		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+		btVector3 oldMargin(getMargin(), getMargin(), getMargin());
+		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin;
 		btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
 
 		btConvexInternalShape::setLocalScaling(scaling);
 
 		m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
-
 	}
 
 	//debugging
-	virtual const char*	getName()const
+	virtual const char* getName() const
 	{
 		return "CylinderY";
 	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
 class btCylinderShapeX : public btCylinderShape
 {
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-	btCylinderShapeX (const btVector3& halfExtents);
-
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
-	
-		//debugging
-	virtual const char*	getName()const
+
+	btCylinderShapeX(const btVector3& halfExtents);
+
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
+
+	//debugging
+	virtual const char* getName() const
 	{
 		return "CylinderX";
 	}
@@ -154,21 +146,20 @@ public:
 	{
 		return getHalfExtentsWithMargin().getY();
 	}
-
 };
 
 class btCylinderShapeZ : public btCylinderShape
 {
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-	btCylinderShapeZ (const btVector3& halfExtents);
 
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+	btCylinderShapeZ(const btVector3& halfExtents);
 
-		//debugging
-	virtual const char*	getName()const
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
+
+	//debugging
+	virtual const char* getName() const
 	{
 		return "CylinderZ";
 	}
@@ -177,37 +168,39 @@ public:
 	{
 		return getHalfExtentsWithMargin().getX();
 	}
-
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btCylinderShapeData
+struct btCylinderShapeData
 {
-	btConvexInternalShapeData	m_convexInternalShapeData;
+	btConvexInternalShapeData m_convexInternalShapeData;
 
-	int	m_upAxis;
+	int m_upAxis;
 
-	char	m_padding[4];
+	char m_padding[4];
 };
 
-SIMD_FORCE_INLINE	int	btCylinderShape::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btCylinderShape::calculateSerializeBufferSize() const
 {
 	return sizeof(btCylinderShapeData);
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btCylinderShape::serialize(void* dataBuffer, btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char* btCylinderShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btCylinderShapeData* shapeData = (btCylinderShapeData*) dataBuffer;
-	
-	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
+	btCylinderShapeData* shapeData = (btCylinderShapeData*)dataBuffer;
 
-	shapeData->m_upAxis = m_upAxis;
-	
-	return "btCylinderShapeData";
-}
+	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
 
+	shapeData->m_upAxis = m_upAxis;
 
+	// Fill padding with zeros to appease msan.
+	shapeData->m_padding[0] = 0;
+	shapeData->m_padding[1] = 0;
+	shapeData->m_padding[2] = 0;
+	shapeData->m_padding[3] = 0;
 
-#endif //BT_CYLINDER_MINKOWSKI_H
+	return "btCylinderShapeData";
+}
 
+#endif  //BT_CYLINDER_MINKOWSKI_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp
index a9e6df5c58e..4699555bd87 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp
@@ -15,36 +15,28 @@ subject to the following restrictions:
 
 #include "btEmptyShape.h"
 
-
 #include "btCollisionShape.h"
 
-
-btEmptyShape::btEmptyShape() : btConcaveShape ()
+btEmptyShape::btEmptyShape() : btConcaveShape()
 {
 	m_shapeType = EMPTY_SHAPE_PROXYTYPE;
 }
 
-
 btEmptyShape::~btEmptyShape()
 {
 }
 
-
-	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
-void btEmptyShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+void btEmptyShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 {
-	btVector3 margin(getMargin(),getMargin(),getMargin());
+	btVector3 margin(getMargin(), getMargin(), getMargin());
 
 	aabbMin = t.getOrigin() - margin;
 
 	aabbMax = t.getOrigin() + margin;
-
 }
 
-void	btEmptyShape::calculateLocalInertia(btScalar ,btVector3& ) const
+void btEmptyShape::calculateLocalInertia(btScalar, btVector3&) const
 {
 	btAssert(0);
 }
-
-	
-	
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h
index 069a79402bf..d2e21173b21 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h
@@ -23,50 +23,43 @@ subject to the following restrictions:
 #include "LinearMath/btMatrix3x3.h"
 #include "btCollisionMargin.h"
 
-
-
-
 /// The btEmptyShape is a collision shape without actual collision detection shape, so most users should ignore this class.
 /// It can be replaced by another shape during runtime, but the inertia tensor should be recomputed.
-ATTRIBUTE_ALIGNED16(class) btEmptyShape	: public btConcaveShape
+ATTRIBUTE_ALIGNED16(class)
+btEmptyShape : public btConcaveShape
 {
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	btEmptyShape();
 
 	virtual ~btEmptyShape();
 
-
 	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
-	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+	void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
-
-	virtual void	setLocalScaling(const btVector3& scaling)
+	virtual void setLocalScaling(const btVector3& scaling)
 	{
 		m_localScaling = scaling;
 	}
-	virtual const btVector3& getLocalScaling() const 
+	virtual const btVector3& getLocalScaling() const
 	{
 		return m_localScaling;
 	}
 
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
-	
-	virtual const char*	getName()const
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
+
+	virtual const char* getName() const
 	{
 		return "Empty";
 	}
 
-	virtual void processAllTriangles(btTriangleCallback* ,const btVector3& ,const btVector3& ) const
+	virtual void processAllTriangles(btTriangleCallback*, const btVector3&, const btVector3&) const
 	{
 	}
 
 protected:
-	btVector3	m_localScaling;
-
+	btVector3 m_localScaling;
 };
 
-
-
-#endif //BT_EMPTY_SHAPE_H
+#endif  //BT_EMPTY_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
index 441a89c6bb6..34e7926f17c 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
@@ -17,23 +17,23 @@ subject to the following restrictions:
 
 #include "LinearMath/btTransformUtil.h"
 
-
-
-btHeightfieldTerrainShape::btHeightfieldTerrainShape
-(
-int heightStickWidth, int heightStickLength, const void* heightfieldData,
-btScalar heightScale, btScalar minHeight, btScalar maxHeight,int upAxis,
-PHY_ScalarType hdt, bool flipQuadEdges
-)
+btHeightfieldTerrainShape::btHeightfieldTerrainShape(
+	int heightStickWidth, int heightStickLength, const void* heightfieldData,
+	btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
+	PHY_ScalarType hdt, bool flipQuadEdges)
+	:m_userIndex2(-1),
+	m_userValue3(0),
+	m_triangleInfoMap(0)
 {
 	initialize(heightStickWidth, heightStickLength, heightfieldData,
-	           heightScale, minHeight, maxHeight, upAxis, hdt,
-	           flipQuadEdges);
+			   heightScale, minHeight, maxHeight, upAxis, hdt,
+			   flipQuadEdges);
 }
 
-
-
-btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,const void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges)
+btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges)
+	:m_userIndex2(-1),
+	m_userValue3(0),
+	m_triangleInfoMap(0)
 {
 	// legacy constructor: support only float or unsigned char,
 	// 	and min height is zero
@@ -45,27 +45,23 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int h
 	btScalar heightScale = maxHeight / 65535;
 
 	initialize(heightStickWidth, heightStickLength, heightfieldData,
-	           heightScale, minHeight, maxHeight, upAxis, hdt,
-	           flipQuadEdges);
+			   heightScale, minHeight, maxHeight, upAxis, hdt,
+			   flipQuadEdges);
 }
 
-
-
-void btHeightfieldTerrainShape::initialize
-(
-int heightStickWidth, int heightStickLength, const void* heightfieldData,
-btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
-PHY_ScalarType hdt, bool flipQuadEdges
-)
+void btHeightfieldTerrainShape::initialize(
+	int heightStickWidth, int heightStickLength, const void* heightfieldData,
+	btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
+	PHY_ScalarType hdt, bool flipQuadEdges)
 {
 	// validation
-	btAssert(heightStickWidth > 1);// && "bad width");
-	btAssert(heightStickLength > 1);// && "bad length");
-	btAssert(heightfieldData);// && "null heightfield data");
+	btAssert(heightStickWidth > 1);   // && "bad width");
+	btAssert(heightStickLength > 1);  // && "bad length");
+	btAssert(heightfieldData);        // && "null heightfield data");
 	// btAssert(heightScale) -- do we care?  Trust caller here
-	btAssert(minHeight <= maxHeight);// && "bad min/max height");
-	btAssert(upAxis >= 0 && upAxis < 3);// && "bad upAxis--should be in range [0,2]");
-	btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT);// && "Bad height data type enum");
+	btAssert(minHeight <= maxHeight);                                    // && "bad min/max height");
+	btAssert(upAxis >= 0 && upAxis < 3);                                 // && "bad upAxis--should be in range [0,2]");
+	btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT);  // && "Bad height data type enum");
 
 	// initialize member variables
 	m_shapeType = TERRAIN_SHAPE_PROXYTYPE;
@@ -73,42 +69,47 @@ PHY_ScalarType hdt, bool flipQuadEdges
 	m_heightStickLength = heightStickLength;
 	m_minHeight = minHeight;
 	m_maxHeight = maxHeight;
-	m_width = (btScalar) (heightStickWidth - 1);
-	m_length = (btScalar) (heightStickLength - 1);
+	m_width = (btScalar)(heightStickWidth - 1);
+	m_length = (btScalar)(heightStickLength - 1);
 	m_heightScale = heightScale;
 	m_heightfieldDataUnknown = heightfieldData;
 	m_heightDataType = hdt;
 	m_flipQuadEdges = flipQuadEdges;
 	m_useDiamondSubdivision = false;
 	m_useZigzagSubdivision = false;
+	m_flipTriangleWinding = false;
 	m_upAxis = upAxis;
 	m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.));
+	
+	m_vboundsChunkSize = 0;
+	m_vboundsGridWidth = 0;
+	m_vboundsGridLength = 0;
 
 	// determine min/max axis-aligned bounding box (aabb) values
 	switch (m_upAxis)
 	{
-	case 0:
+		case 0:
 		{
 			m_localAabbMin.setValue(m_minHeight, 0, 0);
 			m_localAabbMax.setValue(m_maxHeight, m_width, m_length);
 			break;
 		}
-	case 1:
+		case 1:
 		{
 			m_localAabbMin.setValue(0, m_minHeight, 0);
 			m_localAabbMax.setValue(m_width, m_maxHeight, m_length);
 			break;
 		};
-	case 2:
+		case 2:
 		{
 			m_localAabbMin.setValue(0, 0, m_minHeight);
 			m_localAabbMax.setValue(m_width, m_length, m_maxHeight);
 			break;
 		}
-	default:
+		default:
 		{
 			//need to get valid m_upAxis
-			btAssert(0);// && "Bad m_upAxis");
+			btAssert(0);  // && "Bad m_upAxis");
 		}
 	}
 
@@ -116,62 +117,58 @@ PHY_ScalarType hdt, bool flipQuadEdges
 	m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax);
 }
 
-
-
 btHeightfieldTerrainShape::~btHeightfieldTerrainShape()
 {
+	clearAccelerator();
 }
 
-
-
-void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+void btHeightfieldTerrainShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 {
-	btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5);
+	btVector3 halfExtents = (m_localAabbMax - m_localAabbMin) * m_localScaling * btScalar(0.5);
 
 	btVector3 localOrigin(0, 0, 0);
 	localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5);
 	localOrigin *= m_localScaling;
 
-	btMatrix3x3 abs_b = t.getBasis().absolute();  
+	btMatrix3x3 abs_b = t.getBasis().absolute();
 	btVector3 center = t.getOrigin();
-    btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
-	extent += btVector3(getMargin(),getMargin(),getMargin());
+	btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
+	extent += btVector3(getMargin(), getMargin(), getMargin());
 
 	aabbMin = center - extent;
 	aabbMax = center + extent;
 }
 
-
 /// This returns the "raw" (user's initial) height, not the actual height.
 /// The actual height needs to be adjusted to be relative to the center
 ///   of the heightfield's AABB.
 btScalar
-btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const
+btHeightfieldTerrainShape::getRawHeightFieldValue(int x, int y) const
 {
 	btScalar val = 0.f;
 	switch (m_heightDataType)
 	{
-	case PHY_FLOAT:
+		case PHY_FLOAT:
 		{
-			val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x];
+			val = m_heightfieldDataFloat[(y * m_heightStickWidth) + x];
 			break;
 		}
 
-	case PHY_UCHAR:
+		case PHY_UCHAR:
 		{
-			unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x];
+			unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y * m_heightStickWidth) + x];
 			val = heightFieldValue * m_heightScale;
 			break;
 		}
 
-	case PHY_SHORT:
+		case PHY_SHORT:
 		{
 			short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x];
 			val = hfValue * m_heightScale;
 			break;
 		}
 
-	default:
+		default:
 		{
 			btAssert(!"Bad m_heightDataType");
 		}
@@ -180,74 +177,63 @@ btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const
 	return val;
 }
 
-
-
-
 /// this returns the vertex in bullet-local coordinates
-void	btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const
+void btHeightfieldTerrainShape::getVertex(int x, int y, btVector3& vertex) const
 {
-	btAssert(x>=0);
-	btAssert(y>=0);
-	btAssert(x<m_heightStickWidth);
-	btAssert(y<m_heightStickLength);
+	btAssert(x >= 0);
+	btAssert(y >= 0);
+	btAssert(x < m_heightStickWidth);
+	btAssert(y < m_heightStickLength);
 
-	btScalar	height = getRawHeightFieldValue(x,y);
+	btScalar height = getRawHeightFieldValue(x, y);
 
 	switch (m_upAxis)
 	{
-	case 0:
+		case 0:
 		{
-		vertex.setValue(
-			height - m_localOrigin.getX(),
-			(-m_width/btScalar(2.0)) + x,
-			(-m_length/btScalar(2.0) ) + y
-			);
+			vertex.setValue(
+				height - m_localOrigin.getX(),
+				(-m_width / btScalar(2.0)) + x,
+				(-m_length / btScalar(2.0)) + y);
 			break;
 		}
-	case 1:
+		case 1:
 		{
 			vertex.setValue(
-			(-m_width/btScalar(2.0)) + x,
-			height - m_localOrigin.getY(),
-			(-m_length/btScalar(2.0)) + y
-			);
+				(-m_width / btScalar(2.0)) + x,
+				height - m_localOrigin.getY(),
+				(-m_length / btScalar(2.0)) + y);
 			break;
 		};
-	case 2:
+		case 2:
 		{
 			vertex.setValue(
-			(-m_width/btScalar(2.0)) + x,
-			(-m_length/btScalar(2.0)) + y,
-			height - m_localOrigin.getZ()
-			);
+				(-m_width / btScalar(2.0)) + x,
+				(-m_length / btScalar(2.0)) + y,
+				height - m_localOrigin.getZ());
 			break;
 		}
-	default:
+		default:
 		{
 			//need to get valid m_upAxis
 			btAssert(0);
 		}
 	}
 
-	vertex*=m_localScaling;
+	vertex *= m_localScaling;
 }
 
-
-
 static inline int
-getQuantized
-(
-btScalar x
-)
+getQuantized(
+	btScalar x)
 {
-	if (x < 0.0) {
-		return (int) (x - 0.5);
+	if (x < 0.0)
+	{
+		return (int)(x - 0.5);
 	}
-	return (int) (x + 0.5);
+	return (int)(x + 0.5);
 }
 
-
-
 /// given input vector, return quantized version
 /**
   This routine is basically determining the gridpoint indices for a given
@@ -257,7 +243,7 @@ btScalar x
   "with clamp" means that we restrict the point to be in the heightfield's
   axis-aligned bounding box.
  */
-void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point,int /*isMax*/) const
+void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point, int /*isMax*/) const
 {
 	btVector3 clampedPoint(point);
 	clampedPoint.setMax(m_localAabbMin);
@@ -266,11 +252,8 @@ void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& poi
 	out[0] = getQuantized(clampedPoint.getX());
 	out[1] = getQuantized(clampedPoint.getY());
 	out[2] = getQuantized(clampedPoint.getZ());
-		
 }
 
-
-
 /// process all triangles within the provided axis-aligned bounding box
 /**
   basic algorithm:
@@ -278,128 +261,132 @@ void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& poi
     - convert input aabb to a range of heightfield grid points (quantize)
     - iterate over all triangles in that subset of the grid
  */
-void	btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
 {
 	// scale down the input aabb's so they are in local (non-scaled) coordinates
-	btVector3	localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
-	btVector3	localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
+	btVector3 localAabbMin = aabbMin * btVector3(1.f / m_localScaling[0], 1.f / m_localScaling[1], 1.f / m_localScaling[2]);
+	btVector3 localAabbMax = aabbMax * btVector3(1.f / m_localScaling[0], 1.f / m_localScaling[1], 1.f / m_localScaling[2]);
 
 	// account for local origin
 	localAabbMin += m_localOrigin;
 	localAabbMax += m_localOrigin;
 
 	//quantize the aabbMin and aabbMax, and adjust the start/end ranges
-	int	quantizedAabbMin[3];
-	int	quantizedAabbMax[3];
-	quantizeWithClamp(quantizedAabbMin, localAabbMin,0);
-	quantizeWithClamp(quantizedAabbMax, localAabbMax,1);
-	
+	int quantizedAabbMin[3];
+	int quantizedAabbMax[3];
+	quantizeWithClamp(quantizedAabbMin, localAabbMin, 0);
+	quantizeWithClamp(quantizedAabbMax, localAabbMax, 1);
+
 	// expand the min/max quantized values
 	// this is to catch the case where the input aabb falls between grid points!
-	for (int i = 0; i < 3; ++i) {
+	for (int i = 0; i < 3; ++i)
+	{
 		quantizedAabbMin[i]--;
 		quantizedAabbMax[i]++;
-	}	
+	}
 
-	int startX=0;
-	int endX=m_heightStickWidth-1;
-	int startJ=0;
-	int endJ=m_heightStickLength-1;
+	int startX = 0;
+	int endX = m_heightStickWidth - 1;
+	int startJ = 0;
+	int endJ = m_heightStickLength - 1;
 
 	switch (m_upAxis)
 	{
-	case 0:
+		case 0:
 		{
-			if (quantizedAabbMin[1]>startX)
+			if (quantizedAabbMin[1] > startX)
 				startX = quantizedAabbMin[1];
-			if (quantizedAabbMax[1]<endX)
+			if (quantizedAabbMax[1] < endX)
 				endX = quantizedAabbMax[1];
-			if (quantizedAabbMin[2]>startJ)
+			if (quantizedAabbMin[2] > startJ)
 				startJ = quantizedAabbMin[2];
-			if (quantizedAabbMax[2]<endJ)
+			if (quantizedAabbMax[2] < endJ)
 				endJ = quantizedAabbMax[2];
 			break;
 		}
-	case 1:
+		case 1:
 		{
-			if (quantizedAabbMin[0]>startX)
+			if (quantizedAabbMin[0] > startX)
 				startX = quantizedAabbMin[0];
-			if (quantizedAabbMax[0]<endX)
+			if (quantizedAabbMax[0] < endX)
 				endX = quantizedAabbMax[0];
-			if (quantizedAabbMin[2]>startJ)
+			if (quantizedAabbMin[2] > startJ)
 				startJ = quantizedAabbMin[2];
-			if (quantizedAabbMax[2]<endJ)
+			if (quantizedAabbMax[2] < endJ)
 				endJ = quantizedAabbMax[2];
 			break;
 		};
-	case 2:
+		case 2:
 		{
-			if (quantizedAabbMin[0]>startX)
+			if (quantizedAabbMin[0] > startX)
 				startX = quantizedAabbMin[0];
-			if (quantizedAabbMax[0]<endX)
+			if (quantizedAabbMax[0] < endX)
 				endX = quantizedAabbMax[0];
-			if (quantizedAabbMin[1]>startJ)
+			if (quantizedAabbMin[1] > startJ)
 				startJ = quantizedAabbMin[1];
-			if (quantizedAabbMax[1]<endJ)
+			if (quantizedAabbMax[1] < endJ)
 				endJ = quantizedAabbMax[1];
 			break;
 		}
-	default:
+		default:
 		{
 			//need to get valid m_upAxis
 			btAssert(0);
 		}
 	}
 
-	
-  
+	// TODO If m_vboundsGrid is available, use it to determine if we really need to process this area
 
-	for(int j=startJ; j<endJ; j++)
+	for (int j = startJ; j < endJ; j++)
 	{
-		for(int x=startX; x<endX; x++)
+		for (int x = startX; x < endX; x++)
 		{
 			btVector3 vertices[3];
-			if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1))|| (m_useZigzagSubdivision  && !(j & 1)))
+			int indices[3] = { 0, 1, 2 };
+			if (m_flipTriangleWinding)
+			{
+				indices[0] = 2;
+				indices[2] = 0;
+			}
+
+			if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j + x) & 1)) || (m_useZigzagSubdivision && !(j & 1)))
 			{
-        //first triangle
-        getVertex(x,j,vertices[0]);
-		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 + 1, j, vertices[2]);
-		callback->processTriangle(vertices, x, j);
-
-			} else
+				//first triangle
+				getVertex(x, j, vertices[indices[0]]);
+				getVertex(x, j + 1, vertices[indices[1]]);
+				getVertex(x + 1, j + 1, vertices[indices[2]]);
+				callback->processTriangle(vertices, 2 * x, j);
+				//second triangle
+				//  getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman
+				getVertex(x + 1, j + 1, vertices[indices[1]]);
+				getVertex(x + 1, j, vertices[indices[2]]);
+				callback->processTriangle(vertices, 2 * x+1, j);
+			}
+			else
 			{
-        //first triangle
-        getVertex(x,j,vertices[0]);
-        getVertex(x,j+1,vertices[1]);
-        getVertex(x+1,j,vertices[2]);
-        callback->processTriangle(vertices,x,j);
-        //second triangle
-        getVertex(x+1,j,vertices[0]);
-        //getVertex(x,j+1,vertices[1]);
-        getVertex(x+1,j+1,vertices[2]);
-        callback->processTriangle(vertices,x,j);
+				//first triangle
+				getVertex(x, j, vertices[indices[0]]);
+				getVertex(x, j + 1, vertices[indices[1]]);
+				getVertex(x + 1, j, vertices[indices[2]]);
+				callback->processTriangle(vertices, 2 * x, j);
+				//second triangle
+				getVertex(x + 1, j, vertices[indices[0]]);
+				//getVertex(x,j+1,vertices[1]);
+				getVertex(x + 1, j + 1, vertices[indices[2]]);
+				callback->processTriangle(vertices, 2 * x+1, j);
 			}
 		}
 	}
-
-	
-
 }
 
-void	btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const
+void btHeightfieldTerrainShape::calculateLocalInertia(btScalar, btVector3& inertia) const
 {
 	//moving concave objects not supported
-	
-	inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+
+	inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 }
 
-void	btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling)
+void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling)
 {
 	m_localScaling = scaling;
 }
@@ -407,3 +394,458 @@ const btVector3& btHeightfieldTerrainShape::getLocalScaling() const
 {
 	return m_localScaling;
 }
+
+namespace
+{
+	struct GridRaycastState
+	{
+		int x;  // Next quad coords
+		int z;
+		int prev_x;  // Previous quad coords
+		int prev_z;
+		btScalar param;      // Exit param for previous quad
+		btScalar prevParam;  // Enter param for previous quad
+		btScalar maxDistanceFlat;
+		btScalar maxDistance3d;
+	};
+}
+
+// TODO Does it really need to take 3D vectors?
+/// Iterates through a virtual 2D grid of unit-sized square cells,
+/// and executes an action on each cell intersecting the given segment, ordered from begin to end.
+/// Initially inspired by http://www.cse.yorku.ca/~amana/research/grid.pdf
+template <typename Action_T>
+void gridRaycast(Action_T& quadAction, const btVector3& beginPos, const btVector3& endPos, int indices[3])
+{
+	GridRaycastState rs;
+	rs.maxDistance3d = beginPos.distance(endPos);
+	if (rs.maxDistance3d < 0.0001)
+	{
+		// Consider the ray is too small to hit anything
+		return;
+	}
+	
+
+	btScalar rayDirectionFlatX = endPos[indices[0]] - beginPos[indices[0]];
+	btScalar rayDirectionFlatZ = endPos[indices[2]] - beginPos[indices[2]];
+	rs.maxDistanceFlat = btSqrt(rayDirectionFlatX * rayDirectionFlatX + rayDirectionFlatZ * rayDirectionFlatZ);
+
+	if (rs.maxDistanceFlat < 0.0001)
+	{
+		// Consider the ray vertical
+		rayDirectionFlatX = 0;
+		rayDirectionFlatZ = 0;
+	}
+	else
+	{
+		rayDirectionFlatX /= rs.maxDistanceFlat;
+		rayDirectionFlatZ /= rs.maxDistanceFlat;
+	}
+
+	const int xiStep = rayDirectionFlatX > 0 ? 1 : rayDirectionFlatX < 0 ? -1 : 0;
+	const int ziStep = rayDirectionFlatZ > 0 ? 1 : rayDirectionFlatZ < 0 ? -1 : 0;
+
+	const float infinite = 9999999;
+	const btScalar paramDeltaX = xiStep != 0 ? 1.f / btFabs(rayDirectionFlatX) : infinite;
+	const btScalar paramDeltaZ = ziStep != 0 ? 1.f / btFabs(rayDirectionFlatZ) : infinite;
+
+	// pos = param * dir
+	btScalar paramCrossX;  // At which value of `param` we will cross a x-axis lane?
+	btScalar paramCrossZ;  // At which value of `param` we will cross a z-axis lane?
+
+	// paramCrossX and paramCrossZ are initialized as being the first cross
+	// X initialization
+	if (xiStep != 0)
+	{
+		if (xiStep == 1)
+		{
+			paramCrossX = (ceil(beginPos[indices[0]]) - beginPos[indices[0]]) * paramDeltaX;
+		}
+		else
+		{
+			paramCrossX = (beginPos[indices[0]] - floor(beginPos[indices[0]])) * paramDeltaX;
+		}
+	}
+	else
+	{
+		paramCrossX = infinite;  // Will never cross on X
+	}
+
+	// Z initialization
+	if (ziStep != 0)
+	{
+		if (ziStep == 1)
+		{
+			paramCrossZ = (ceil(beginPos[indices[2]]) - beginPos[indices[2]]) * paramDeltaZ;
+		}
+		else
+		{
+			paramCrossZ = (beginPos[indices[2]] - floor(beginPos[indices[2]])) * paramDeltaZ;
+		}
+	}
+	else
+	{
+		paramCrossZ = infinite;  // Will never cross on Z
+	}
+
+	rs.x = static_cast<int>(floor(beginPos[indices[0]]));
+	rs.z = static_cast<int>(floor(beginPos[indices[2]]));
+
+	// Workaround cases where the ray starts at an integer position
+	if (paramCrossX == 0.0)
+	{
+		paramCrossX += paramDeltaX;
+		// If going backwards, we should ignore the position we would get by the above flooring,
+		// because the ray is not heading in that direction
+		if (xiStep == -1)
+		{
+			rs.x -= 1;
+		}
+	}
+
+	if (paramCrossZ == 0.0)
+	{
+		paramCrossZ += paramDeltaZ;
+		if (ziStep == -1)
+			rs.z -= 1;
+	}
+
+	rs.prev_x = rs.x;
+	rs.prev_z = rs.z;
+	rs.param = 0;
+
+	while (true)
+	{
+		rs.prev_x = rs.x;
+		rs.prev_z = rs.z;
+		rs.prevParam = rs.param;
+
+		if (paramCrossX < paramCrossZ)
+		{
+			// X lane
+			rs.x += xiStep;
+			// Assign before advancing the param,
+			// to be in sync with the initialization step
+			rs.param = paramCrossX;
+			paramCrossX += paramDeltaX;
+		}
+		else
+		{
+			// Z lane
+			rs.z += ziStep;
+			rs.param = paramCrossZ;
+			paramCrossZ += paramDeltaZ;
+		}
+
+		if (rs.param > rs.maxDistanceFlat)
+		{
+			rs.param = rs.maxDistanceFlat;
+			quadAction(rs);
+			break;
+		}
+		else
+		{
+			quadAction(rs);
+		}
+	}
+}
+
+struct ProcessTrianglesAction
+{
+	const btHeightfieldTerrainShape* shape;
+	bool flipQuadEdges;
+	bool useDiamondSubdivision;
+	int width;
+	int length;
+	btTriangleCallback* callback;
+
+	void exec(int x, int z) const
+	{
+		if (x < 0 || z < 0 || x >= width || z >= length)
+		{
+			return;
+		}
+
+		btVector3 vertices[3];
+
+		// TODO Since this is for raycasts, we could greatly benefit from an early exit on the first hit
+
+		// Check quad
+		if (flipQuadEdges || (useDiamondSubdivision && (((z + x) & 1) > 0)))
+		{
+			// First triangle
+			shape->getVertex(x, z, vertices[0]);
+			shape->getVertex(x + 1, z, vertices[1]);
+			shape->getVertex(x + 1, z + 1, vertices[2]);
+			callback->processTriangle(vertices, x, z);
+
+			// Second triangle
+			shape->getVertex(x, z, vertices[0]);
+			shape->getVertex(x + 1, z + 1, vertices[1]);
+			shape->getVertex(x, z + 1, vertices[2]);
+			callback->processTriangle(vertices, x, z);
+		}
+		else
+		{
+			// First triangle
+			shape->getVertex(x, z, vertices[0]);
+			shape->getVertex(x, z + 1, vertices[1]);
+			shape->getVertex(x + 1, z, vertices[2]);
+			callback->processTriangle(vertices, x, z);
+
+			// Second triangle
+			shape->getVertex(x + 1, z, vertices[0]);
+			shape->getVertex(x, z + 1, vertices[1]);
+			shape->getVertex(x + 1, z + 1, vertices[2]);
+			callback->processTriangle(vertices, x, z);
+		}
+	}
+
+	void operator()(const GridRaycastState& bs) const
+	{
+		exec(bs.prev_x, bs.prev_z);
+	}
+};
+
+struct ProcessVBoundsAction
+{
+	const btAlignedObjectArray<btHeightfieldTerrainShape::Range>& vbounds;
+	int width;
+	int length;
+	int chunkSize;
+
+	btVector3 rayBegin;
+	btVector3 rayEnd;
+	btVector3 rayDir;
+
+	int* m_indices;
+	ProcessTrianglesAction processTriangles;
+
+	ProcessVBoundsAction(const btAlignedObjectArray<btHeightfieldTerrainShape::Range>& bnd, int* indices)
+		: vbounds(bnd),
+		m_indices(indices)
+	{
+	}
+	void operator()(const GridRaycastState& rs) const
+	{
+		int x = rs.prev_x;
+		int z = rs.prev_z;
+
+		if (x < 0 || z < 0 || x >= width || z >= length)
+		{
+			return;
+		}
+
+		const btHeightfieldTerrainShape::Range chunk = vbounds[x + z * width];
+
+		btVector3 enterPos;
+		btVector3 exitPos;
+
+		if (rs.maxDistanceFlat > 0.0001)
+		{
+			btScalar flatTo3d = chunkSize * rs.maxDistance3d / rs.maxDistanceFlat;
+			btScalar enterParam3d = rs.prevParam * flatTo3d;
+			btScalar exitParam3d = rs.param * flatTo3d;
+			enterPos = rayBegin + rayDir * enterParam3d;
+			exitPos = rayBegin + rayDir * exitParam3d;
+
+			// We did enter the flat projection of the AABB,
+			// but we have to check if we intersect it on the vertical axis
+			if (enterPos[1] > chunk.max && exitPos[m_indices[1]] > chunk.max)
+			{
+				return;
+			}
+			if (enterPos[1] < chunk.min && exitPos[m_indices[1]] < chunk.min)
+			{
+				return;
+			}
+		}
+		else
+		{
+			// Consider the ray vertical
+			// (though we shouldn't reach this often because there is an early check up-front)
+			enterPos = rayBegin;
+			exitPos = rayEnd;
+		}
+
+		gridRaycast(processTriangles, enterPos, exitPos, m_indices);
+		// Note: it could be possible to have more than one grid at different levels,
+		// to do this there would be a branch using a pointer to another ProcessVBoundsAction
+	}
+};
+
+// TODO How do I interrupt the ray when there is a hit? `callback` does not return any result
+/// Performs a raycast using a hierarchical Bresenham algorithm.
+/// Does not allocate any memory by itself.
+void btHeightfieldTerrainShape::performRaycast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) const
+{
+	// Transform to cell-local
+	btVector3 beginPos = raySource / m_localScaling;
+	btVector3 endPos = rayTarget / m_localScaling;
+	beginPos += m_localOrigin;
+	endPos += m_localOrigin;
+
+	ProcessTrianglesAction processTriangles;
+	processTriangles.shape = this;
+	processTriangles.flipQuadEdges = m_flipQuadEdges;
+	processTriangles.useDiamondSubdivision = m_useDiamondSubdivision;
+	processTriangles.callback = callback;
+	processTriangles.width = m_heightStickWidth - 1;
+	processTriangles.length = m_heightStickLength - 1;
+
+	// TODO Transform vectors to account for m_upAxis
+	int indices[3] = { 0, 1, 2 };
+	if (m_upAxis == 2)
+	{
+		indices[1] = 2;
+		indices[2] = 1;
+	}
+	int iBeginX = static_cast<int>(floor(beginPos[indices[0]]));
+	int iBeginZ = static_cast<int>(floor(beginPos[indices[2]]));
+	int iEndX = static_cast<int>(floor(endPos[indices[0]]));
+	int iEndZ = static_cast<int>(floor(endPos[indices[2]]));
+
+	if (iBeginX == iEndX && iBeginZ == iEndZ)
+	{
+		// The ray will never cross quads within the plane,
+		// so directly process triangles within one quad
+		// (typically, vertical rays should end up here)
+		processTriangles.exec(iBeginX, iEndZ);
+		return;
+	}
+
+	
+
+	if (m_vboundsGrid.size()==0)
+	{
+		// Process all quads intersecting the flat projection of the ray
+		gridRaycast(processTriangles, beginPos, endPos, &indices[0]);
+	}
+	else
+	{
+		btVector3 rayDiff = endPos - beginPos;
+		btScalar flatDistance2 = rayDiff[indices[0]] * rayDiff[indices[0]] + rayDiff[indices[2]] * rayDiff[indices[2]];
+		if (flatDistance2 < m_vboundsChunkSize * m_vboundsChunkSize)
+		{
+			// Don't use chunks, the ray is too short in the plane
+			gridRaycast(processTriangles, beginPos, endPos, &indices[0]);
+		}
+
+		ProcessVBoundsAction processVBounds(m_vboundsGrid, &indices[0]);
+		processVBounds.width = m_vboundsGridWidth;
+		processVBounds.length = m_vboundsGridLength;
+		processVBounds.rayBegin = beginPos;
+		processVBounds.rayEnd = endPos;
+		processVBounds.rayDir = rayDiff.normalized();
+		processVBounds.processTriangles = processTriangles;
+		processVBounds.chunkSize = m_vboundsChunkSize;
+		// The ray is long, run raycast on a higher-level grid
+		gridRaycast(processVBounds, beginPos / m_vboundsChunkSize, endPos / m_vboundsChunkSize, indices);
+	}
+}
+
+/// Builds a grid data structure storing the min and max heights of the terrain in chunks.
+/// if chunkSize is zero, that accelerator is removed.
+/// If you modify the heights, you need to rebuild this accelerator.
+void btHeightfieldTerrainShape::buildAccelerator(int chunkSize)
+{
+	if (chunkSize <= 0)
+	{
+		clearAccelerator();
+		return;
+	}
+
+	m_vboundsChunkSize = chunkSize;
+	int nChunksX = m_heightStickWidth / chunkSize;
+	int nChunksZ = m_heightStickLength / chunkSize;
+
+	if (m_heightStickWidth % chunkSize > 0)
+	{
+		++nChunksX;  // In case terrain size isn't dividable by chunk size
+	}
+	if (m_heightStickLength % chunkSize > 0)
+	{
+		++nChunksZ;
+	}
+
+	if (m_vboundsGridWidth != nChunksX || m_vboundsGridLength != nChunksZ)
+	{
+		clearAccelerator();
+		m_vboundsGridWidth = nChunksX;
+		m_vboundsGridLength = nChunksZ;
+	}
+
+	if (nChunksX == 0 || nChunksZ == 0)
+	{
+		return;
+	}
+
+	// This data structure is only reallocated if the required size changed
+	m_vboundsGrid.resize(nChunksX * nChunksZ);
+	
+	// Compute min and max height for all chunks
+	for (int cz = 0; cz < nChunksZ; ++cz)
+	{
+		int z0 = cz * chunkSize;
+
+		for (int cx = 0; cx < nChunksX; ++cx)
+		{
+			int x0 = cx * chunkSize;
+
+			Range r;
+
+			r.min = getRawHeightFieldValue(x0, z0);
+			r.max = r.min;
+
+			// Compute min and max height for this chunk.
+			// We have to include one extra cell to account for neighbors.
+			// Here is why:
+			// Say we have a flat terrain, and a plateau that fits a chunk perfectly.
+			//
+			//   Left        Right
+			// 0---0---0---1---1---1
+			// |   |   |   |   |   |
+			// 0---0---0---1---1---1
+			// |   |   |   |   |   |
+			// 0---0---0---1---1---1
+			//           x
+			//
+			// If the AABB for the Left chunk did not share vertices with the Right,
+			// then we would fail collision tests at x due to a gap.
+			//
+			for (int z = z0; z < z0 + chunkSize + 1; ++z)
+			{
+				if (z >= m_heightStickLength)
+				{
+					continue;
+				}
+
+				for (int x = x0; x < x0 + chunkSize + 1; ++x)
+				{
+					if (x >= m_heightStickWidth)
+					{
+						continue;
+					}
+
+					btScalar height = getRawHeightFieldValue(x, z);
+
+					if (height < r.min)
+					{
+						r.min = height;
+					}
+					else if (height > r.max)
+					{
+						r.max = height;
+					}
+				}
+			}
+
+			m_vboundsGrid[cx + cz * nChunksX] = r;
+		}
+	}
+}
+
+void btHeightfieldTerrainShape::clearAccelerator()
+{
+	m_vboundsGrid.clear();
+}
\ No newline at end of file
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
index 4a7a4a4bda4..8dea98fc6be 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
@@ -17,6 +17,7 @@ subject to the following restrictions:
 #define BT_HEIGHTFIELD_TERRAIN_SHAPE_H
 
 #include "btConcaveShape.h"
+#include "LinearMath/btAlignedObjectArray.h"
 
 ///btHeightfieldTerrainShape simulates a 2D heightfield terrain
 /**
@@ -68,43 +69,58 @@ subject to the following restrictions:
 
   For usage and testing see the TerrainDemo.
  */
-ATTRIBUTE_ALIGNED16(class) btHeightfieldTerrainShape : public btConcaveShape
+ATTRIBUTE_ALIGNED16(class)
+btHeightfieldTerrainShape : public btConcaveShape
 {
+public:
+	struct Range
+	{
+		btScalar min;
+		btScalar max;
+	};
+
 protected:
-	btVector3	m_localAabbMin;
-	btVector3	m_localAabbMax;
-	btVector3	m_localOrigin;
+	btVector3 m_localAabbMin;
+	btVector3 m_localAabbMax;
+	btVector3 m_localOrigin;
 
 	///terrain data
-	int	m_heightStickWidth;
+	int m_heightStickWidth;
 	int m_heightStickLength;
-	btScalar	m_minHeight;
-	btScalar	m_maxHeight;
+	btScalar m_minHeight;
+	btScalar m_maxHeight;
 	btScalar m_width;
 	btScalar m_length;
 	btScalar m_heightScale;
-	union
-	{
-		const unsigned char*	m_heightfieldDataUnsignedChar;
-		const short*		m_heightfieldDataShort;
-		const btScalar*			m_heightfieldDataFloat;
-		const void*	m_heightfieldDataUnknown;
+	union {
+		const unsigned char* m_heightfieldDataUnsignedChar;
+		const short* m_heightfieldDataShort;
+		const btScalar* m_heightfieldDataFloat;
+		const void* m_heightfieldDataUnknown;
 	};
 
-	PHY_ScalarType	m_heightDataType;	
-	bool	m_flipQuadEdges;
-  	bool  m_useDiamondSubdivision;
+	PHY_ScalarType m_heightDataType;
+	bool m_flipQuadEdges;
+	bool m_useDiamondSubdivision;
 	bool m_useZigzagSubdivision;
+	bool m_flipTriangleWinding;
+	int m_upAxis;
 
-	int	m_upAxis;
-	
-	btVector3	m_localScaling;
+	btVector3 m_localScaling;
+
+	// Accelerator
+	btAlignedObjectArray<Range> m_vboundsGrid;
+	int m_vboundsGridWidth;
+	int m_vboundsGridLength;
+	int m_vboundsChunkSize;
 
-	virtual btScalar	getRawHeightFieldValue(int x,int y) const;
-	void		quantizeWithClamp(int* out, const btVector3& point,int isMax) const;
-	void		getVertex(int x,int y,btVector3& vertex) const;
+	int m_userIndex2;
+	btScalar m_userValue3;
 
+	struct btTriangleInfoMap* m_triangleInfoMap;
 
+	virtual btScalar getRawHeightFieldValue(int x, int y) const;
+	void quantizeWithClamp(int* out, const btVector3& point, int isMax) const;
 
 	/// protected initialization
 	/**
@@ -112,25 +128,24 @@ protected:
 	  backwards-compatible without a lot of copy/paste.
 	 */
 	void initialize(int heightStickWidth, int heightStickLength,
-	                const void* heightfieldData, btScalar heightScale,
-	                btScalar minHeight, btScalar maxHeight, int upAxis,
-	                PHY_ScalarType heightDataType, bool flipQuadEdges);
+					const void* heightfieldData, btScalar heightScale,
+					btScalar minHeight, btScalar maxHeight, int upAxis,
+					PHY_ScalarType heightDataType, bool flipQuadEdges);
 
 public:
-	
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	/// preferred constructor
 	/**
 	  This constructor supports a range of heightfield
 	  data types, and allows for a non-zero minimum height value.
 	  heightScale is needed for any integer-based heightfield data types.
 	 */
-	btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,
-	                          const void* heightfieldData, btScalar heightScale,
-	                          btScalar minHeight, btScalar maxHeight,
-	                          int upAxis, PHY_ScalarType heightDataType,
-	                          bool flipQuadEdges);
+	btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,
+							  const void* heightfieldData, btScalar heightScale,
+							  btScalar minHeight, btScalar maxHeight,
+							  int upAxis, PHY_ScalarType heightDataType,
+							  bool flipQuadEdges);
 
 	/// legacy constructor
 	/**
@@ -139,29 +154,76 @@ public:
 	  compatibility reasons, heightScale is calculated as maxHeight / 65535 
 	  (and is only used when useFloatData = false).
  	 */
-	btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,const void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges);
+	btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges);
 
 	virtual ~btHeightfieldTerrainShape();
 
+	void setUseDiamondSubdivision(bool useDiamondSubdivision = true) { m_useDiamondSubdivision = useDiamondSubdivision; }
 
-	void setUseDiamondSubdivision(bool useDiamondSubdivision=true) { m_useDiamondSubdivision = useDiamondSubdivision;}
+	///could help compatibility with Ogre heightfields. See https://code.google.com/p/bullet/issues/detail?id=625
+	void setUseZigzagSubdivision(bool useZigzagSubdivision = true) { m_useZigzagSubdivision = useZigzagSubdivision; }
 
-	///could help compatibility with Ogre heightfields. See https://code.google.com/p/bullet/issues/detail?id=625	
-	void setUseZigzagSubdivision(bool useZigzagSubdivision=true) { m_useZigzagSubdivision = useZigzagSubdivision;}
+	void setFlipTriangleWinding(bool flipTriangleWinding)
+	{
+		m_flipTriangleWinding = flipTriangleWinding;
+	}
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+	virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const;
 
-	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
 
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+	virtual void setLocalScaling(const btVector3& scaling);
 
-	virtual void	setLocalScaling(const btVector3& scaling);
-	
 	virtual const btVector3& getLocalScaling() const;
-	
+
+	void getVertex(int x, int y, btVector3& vertex) const;
+
+	void performRaycast(btTriangleCallback * callback, const btVector3& raySource, const btVector3& rayTarget) const;
+
+	void buildAccelerator(int chunkSize = 16);
+	void clearAccelerator();
+
+	int getUpAxis() const
+	{
+		return m_upAxis;
+	}
 	//debugging
-	virtual const char*	getName()const {return "HEIGHTFIELD";}
+	virtual const char* getName() const { return "HEIGHTFIELD"; }
 
+	
+	void setUserIndex2(int index)
+	{
+		m_userIndex2 = index;
+	}
+	int getUserIndex2() const
+	{
+		return m_userIndex2;
+	}
+	void setUserValue3(btScalar value)
+	{
+		m_userValue3 = value;
+	}
+	btScalar getUserValue3() const
+	{
+		return m_userValue3;
+	}
+	const struct btTriangleInfoMap* getTriangleInfoMap() const
+	{
+		return m_triangleInfoMap;
+	}
+	struct btTriangleInfoMap* getTriangleInfoMap()
+	{
+		return m_triangleInfoMap;
+	}
+	void setTriangleInfoMap(btTriangleInfoMap* map)
+	{
+		m_triangleInfoMap = map;
+	}
+	const unsigned char* getHeightfieldRawData() const
+	{
+		return m_heightfieldDataUnsignedChar;
+	}
 };
 
-#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H
+#endif  //BT_HEIGHTFIELD_TERRAIN_SHAPE_H
\ No newline at end of file
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h
index 866f9b4da4d..c9a436bf26a 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h
@@ -21,15 +21,18 @@ subject to the following restrictions:
 // Material class to be used by btMultimaterialTriangleMeshShape to store triangle properties
 class btMaterial
 {
-    // public members so that materials can change due to world events
+	// public members so that materials can change due to world events
 public:
-    btScalar m_friction;
-    btScalar m_restitution;
-    int pad[2];
-
-    btMaterial(){}
-    btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; }
+	btScalar m_friction;
+	btScalar m_restitution;
+	int pad[2];
+
+	btMaterial() {}
+	btMaterial(btScalar fric, btScalar rest)
+	{
+		m_friction = fric;
+		m_restitution = rest;
+	}
 };
 
-#endif // BT_MATERIAL_H
-
+#endif  // BT_MATERIAL_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.cpp
new file mode 100644
index 00000000000..13c0a343f17
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.cpp
@@ -0,0 +1,522 @@
+#include "btMiniSDF.h"
+
+//
+//Based on code from DiscreGrid, https://github.com/InteractiveComputerGraphics/Discregrid
+//example:
+//GenerateSDF.exe -r "32 32 32" -d "-1.6 -1.6 -.6 1.6 1.6 .6" concave_box.obj
+//The MIT License (MIT)
+//
+//Copyright (c) 2017 Dan Koschier
+//
+
+#include <limits.h>
+#include <string.h>  //memcpy
+
+struct btSdfDataStream
+{
+	const char* m_data;
+	int m_size;
+
+	int m_currentOffset;
+
+	btSdfDataStream(const char* data, int size)
+		: m_data(data),
+		  m_size(size),
+		  m_currentOffset(0)
+	{
+	}
+
+	template <class T>
+	bool read(T& val)
+	{
+		int bytes = sizeof(T);
+		if (m_currentOffset + bytes <= m_size)
+		{
+			char* dest = (char*)&val;
+			memcpy(dest, &m_data[m_currentOffset], bytes);
+			m_currentOffset += bytes;
+			return true;
+		}
+		btAssert(0);
+		return false;
+	}
+};
+
+bool btMiniSDF::load(const char* data, int size)
+{
+	int fileSize = -1;
+
+	btSdfDataStream ds(data, size);
+	{
+		double buf[6];
+		ds.read(buf);
+		m_domain.m_min[0] = buf[0];
+		m_domain.m_min[1] = buf[1];
+		m_domain.m_min[2] = buf[2];
+		m_domain.m_min[3] = 0;
+		m_domain.m_max[0] = buf[3];
+		m_domain.m_max[1] = buf[4];
+		m_domain.m_max[2] = buf[5];
+		m_domain.m_max[3] = 0;
+	}
+	{
+		unsigned int buf2[3];
+		ds.read(buf2);
+		m_resolution[0] = buf2[0];
+		m_resolution[1] = buf2[1];
+		m_resolution[2] = buf2[2];
+	}
+	{
+		double buf[3];
+		ds.read(buf);
+		m_cell_size[0] = buf[0];
+		m_cell_size[1] = buf[1];
+		m_cell_size[2] = buf[2];
+	}
+	{
+		double buf[3];
+		ds.read(buf);
+		m_inv_cell_size[0] = buf[0];
+		m_inv_cell_size[1] = buf[1];
+		m_inv_cell_size[2] = buf[2];
+	}
+	{
+		unsigned long long int cells;
+		ds.read(cells);
+		m_n_cells = cells;
+	}
+	{
+		unsigned long long int fields;
+		ds.read(fields);
+		m_n_fields = fields;
+	}
+
+	unsigned long long int nodes0;
+	std::size_t n_nodes0;
+	ds.read(nodes0);
+	n_nodes0 = nodes0;
+	if (n_nodes0 > 1024 * 1024 * 1024)
+	{
+		return m_isValid;
+	}
+	m_nodes.resize(n_nodes0);
+	for (unsigned int i = 0; i < n_nodes0; i++)
+	{
+		unsigned long long int n_nodes1;
+		ds.read(n_nodes1);
+		btAlignedObjectArray<double>& nodes = m_nodes[i];
+		nodes.resize(n_nodes1);
+		for (int j = 0; j < nodes.size(); j++)
+		{
+			double& node = nodes[j];
+			ds.read(node);
+		}
+	}
+
+	unsigned long long int n_cells0;
+	ds.read(n_cells0);
+	m_cells.resize(n_cells0);
+	for (int i = 0; i < n_cells0; i++)
+	{
+		unsigned long long int n_cells1;
+		btAlignedObjectArray<btCell32>& cells = m_cells[i];
+		ds.read(n_cells1);
+		cells.resize(n_cells1);
+		for (int j = 0; j < n_cells1; j++)
+		{
+			btCell32& cell = cells[j];
+			ds.read(cell);
+		}
+	}
+
+	{
+		unsigned long long int n_cell_maps0;
+		ds.read(n_cell_maps0);
+
+		m_cell_map.resize(n_cell_maps0);
+		for (int i = 0; i < n_cell_maps0; i++)
+		{
+			unsigned long long int n_cell_maps1;
+			btAlignedObjectArray<unsigned int>& cell_maps = m_cell_map[i];
+			ds.read(n_cell_maps1);
+			cell_maps.resize(n_cell_maps1);
+			for (int j = 0; j < n_cell_maps1; j++)
+			{
+				unsigned int& cell_map = cell_maps[j];
+				ds.read(cell_map);
+			}
+		}
+	}
+
+	m_isValid = (ds.m_currentOffset == ds.m_size);
+	return m_isValid;
+}
+
+unsigned int btMiniSDF::multiToSingleIndex(btMultiIndex const& ijk) const
+{
+	return m_resolution[1] * m_resolution[0] * ijk.ijk[2] + m_resolution[0] * ijk.ijk[1] + ijk.ijk[0];
+}
+
+btAlignedBox3d
+btMiniSDF::subdomain(btMultiIndex const& ijk) const
+{
+	btAssert(m_isValid);
+	btVector3 tmp;
+	tmp.m_floats[0] = m_cell_size[0] * (double)ijk.ijk[0];
+	tmp.m_floats[1] = m_cell_size[1] * (double)ijk.ijk[1];
+	tmp.m_floats[2] = m_cell_size[2] * (double)ijk.ijk[2];
+
+	btVector3 origin = m_domain.min() + tmp;
+
+	btAlignedBox3d box = btAlignedBox3d(origin, origin + m_cell_size);
+	return box;
+}
+
+btMultiIndex
+btMiniSDF::singleToMultiIndex(unsigned int l) const
+{
+	btAssert(m_isValid);
+	unsigned int n01 = m_resolution[0] * m_resolution[1];
+	unsigned int k = l / n01;
+	unsigned int temp = l % n01;
+	unsigned int j = temp / m_resolution[0];
+	unsigned int i = temp % m_resolution[0];
+	btMultiIndex mi;
+	mi.ijk[0] = i;
+	mi.ijk[1] = j;
+	mi.ijk[2] = k;
+	return mi;
+}
+
+btAlignedBox3d
+btMiniSDF::subdomain(unsigned int l) const
+{
+	btAssert(m_isValid);
+	return subdomain(singleToMultiIndex(l));
+}
+
+btShapeMatrix
+btMiniSDF::shape_function_(btVector3 const& xi, btShapeGradients* gradient) const
+{
+	btAssert(m_isValid);
+	btShapeMatrix res;
+
+	btScalar x = xi[0];
+	btScalar y = xi[1];
+	btScalar z = xi[2];
+
+	btScalar x2 = x * x;
+	btScalar y2 = y * y;
+	btScalar z2 = z * z;
+
+	btScalar _1mx = 1.0 - x;
+	btScalar _1my = 1.0 - y;
+	btScalar _1mz = 1.0 - z;
+
+	btScalar _1px = 1.0 + x;
+	btScalar _1py = 1.0 + y;
+	btScalar _1pz = 1.0 + z;
+
+	btScalar _1m3x = 1.0 - 3.0 * x;
+	btScalar _1m3y = 1.0 - 3.0 * y;
+	btScalar _1m3z = 1.0 - 3.0 * z;
+
+	btScalar _1p3x = 1.0 + 3.0 * x;
+	btScalar _1p3y = 1.0 + 3.0 * y;
+	btScalar _1p3z = 1.0 + 3.0 * z;
+
+	btScalar _1mxt1my = _1mx * _1my;
+	btScalar _1mxt1py = _1mx * _1py;
+	btScalar _1pxt1my = _1px * _1my;
+	btScalar _1pxt1py = _1px * _1py;
+
+	btScalar _1mxt1mz = _1mx * _1mz;
+	btScalar _1mxt1pz = _1mx * _1pz;
+	btScalar _1pxt1mz = _1px * _1mz;
+	btScalar _1pxt1pz = _1px * _1pz;
+
+	btScalar _1myt1mz = _1my * _1mz;
+	btScalar _1myt1pz = _1my * _1pz;
+	btScalar _1pyt1mz = _1py * _1mz;
+	btScalar _1pyt1pz = _1py * _1pz;
+
+	btScalar _1mx2 = 1.0 - x2;
+	btScalar _1my2 = 1.0 - y2;
+	btScalar _1mz2 = 1.0 - z2;
+
+	// Corner nodes.
+	btScalar fac = 1.0 / 64.0 * (9.0 * (x2 + y2 + z2) - 19.0);
+	res[0] = fac * _1mxt1my * _1mz;
+	res[1] = fac * _1pxt1my * _1mz;
+	res[2] = fac * _1mxt1py * _1mz;
+	res[3] = fac * _1pxt1py * _1mz;
+	res[4] = fac * _1mxt1my * _1pz;
+	res[5] = fac * _1pxt1my * _1pz;
+	res[6] = fac * _1mxt1py * _1pz;
+	res[7] = fac * _1pxt1py * _1pz;
+
+	// Edge nodes.
+
+	fac = 9.0 / 64.0 * _1mx2;
+	btScalar fact1m3x = fac * _1m3x;
+	btScalar fact1p3x = fac * _1p3x;
+	res[8] = fact1m3x * _1myt1mz;
+	res[9] = fact1p3x * _1myt1mz;
+	res[10] = fact1m3x * _1myt1pz;
+	res[11] = fact1p3x * _1myt1pz;
+	res[12] = fact1m3x * _1pyt1mz;
+	res[13] = fact1p3x * _1pyt1mz;
+	res[14] = fact1m3x * _1pyt1pz;
+	res[15] = fact1p3x * _1pyt1pz;
+
+	fac = 9.0 / 64.0 * _1my2;
+	btScalar fact1m3y = fac * _1m3y;
+	btScalar fact1p3y = fac * _1p3y;
+	res[16] = fact1m3y * _1mxt1mz;
+	res[17] = fact1p3y * _1mxt1mz;
+	res[18] = fact1m3y * _1pxt1mz;
+	res[19] = fact1p3y * _1pxt1mz;
+	res[20] = fact1m3y * _1mxt1pz;
+	res[21] = fact1p3y * _1mxt1pz;
+	res[22] = fact1m3y * _1pxt1pz;
+	res[23] = fact1p3y * _1pxt1pz;
+
+	fac = 9.0 / 64.0 * _1mz2;
+	btScalar fact1m3z = fac * _1m3z;
+	btScalar fact1p3z = fac * _1p3z;
+	res[24] = fact1m3z * _1mxt1my;
+	res[25] = fact1p3z * _1mxt1my;
+	res[26] = fact1m3z * _1mxt1py;
+	res[27] = fact1p3z * _1mxt1py;
+	res[28] = fact1m3z * _1pxt1my;
+	res[29] = fact1p3z * _1pxt1my;
+	res[30] = fact1m3z * _1pxt1py;
+	res[31] = fact1p3z * _1pxt1py;
+
+	if (gradient)
+	{
+		btShapeGradients& dN = *gradient;
+
+		btScalar _9t3x2py2pz2m19 = 9.0 * (3.0 * x2 + y2 + z2) - 19.0;
+		btScalar _9tx2p3y2pz2m19 = 9.0 * (x2 + 3.0 * y2 + z2) - 19.0;
+		btScalar _9tx2py2p3z2m19 = 9.0 * (x2 + y2 + 3.0 * z2) - 19.0;
+		btScalar _18x = 18.0 * x;
+		btScalar _18y = 18.0 * y;
+		btScalar _18z = 18.0 * z;
+
+		btScalar _3m9x2 = 3.0 - 9.0 * x2;
+		btScalar _3m9y2 = 3.0 - 9.0 * y2;
+		btScalar _3m9z2 = 3.0 - 9.0 * z2;
+
+		btScalar _2x = 2.0 * x;
+		btScalar _2y = 2.0 * y;
+		btScalar _2z = 2.0 * z;
+
+		btScalar _18xm9t3x2py2pz2m19 = _18x - _9t3x2py2pz2m19;
+		btScalar _18xp9t3x2py2pz2m19 = _18x + _9t3x2py2pz2m19;
+		btScalar _18ym9tx2p3y2pz2m19 = _18y - _9tx2p3y2pz2m19;
+		btScalar _18yp9tx2p3y2pz2m19 = _18y + _9tx2p3y2pz2m19;
+		btScalar _18zm9tx2py2p3z2m19 = _18z - _9tx2py2p3z2m19;
+		btScalar _18zp9tx2py2p3z2m19 = _18z + _9tx2py2p3z2m19;
+
+		dN(0, 0) = _18xm9t3x2py2pz2m19 * _1myt1mz;
+		dN(0, 1) = _1mxt1mz * _18ym9tx2p3y2pz2m19;
+		dN(0, 2) = _1mxt1my * _18zm9tx2py2p3z2m19;
+		dN(1, 0) = _18xp9t3x2py2pz2m19 * _1myt1mz;
+		dN(1, 1) = _1pxt1mz * _18ym9tx2p3y2pz2m19;
+		dN(1, 2) = _1pxt1my * _18zm9tx2py2p3z2m19;
+		dN(2, 0) = _18xm9t3x2py2pz2m19 * _1pyt1mz;
+		dN(2, 1) = _1mxt1mz * _18yp9tx2p3y2pz2m19;
+		dN(2, 2) = _1mxt1py * _18zm9tx2py2p3z2m19;
+		dN(3, 0) = _18xp9t3x2py2pz2m19 * _1pyt1mz;
+		dN(3, 1) = _1pxt1mz * _18yp9tx2p3y2pz2m19;
+		dN(3, 2) = _1pxt1py * _18zm9tx2py2p3z2m19;
+		dN(4, 0) = _18xm9t3x2py2pz2m19 * _1myt1pz;
+		dN(4, 1) = _1mxt1pz * _18ym9tx2p3y2pz2m19;
+		dN(4, 2) = _1mxt1my * _18zp9tx2py2p3z2m19;
+		dN(5, 0) = _18xp9t3x2py2pz2m19 * _1myt1pz;
+		dN(5, 1) = _1pxt1pz * _18ym9tx2p3y2pz2m19;
+		dN(5, 2) = _1pxt1my * _18zp9tx2py2p3z2m19;
+		dN(6, 0) = _18xm9t3x2py2pz2m19 * _1pyt1pz;
+		dN(6, 1) = _1mxt1pz * _18yp9tx2p3y2pz2m19;
+		dN(6, 2) = _1mxt1py * _18zp9tx2py2p3z2m19;
+		dN(7, 0) = _18xp9t3x2py2pz2m19 * _1pyt1pz;
+		dN(7, 1) = _1pxt1pz * _18yp9tx2p3y2pz2m19;
+		dN(7, 2) = _1pxt1py * _18zp9tx2py2p3z2m19;
+
+		dN.topRowsDivide(8, 64.0);
+
+		btScalar _m3m9x2m2x = -_3m9x2 - _2x;
+		btScalar _p3m9x2m2x = _3m9x2 - _2x;
+		btScalar _1mx2t1m3x = _1mx2 * _1m3x;
+		btScalar _1mx2t1p3x = _1mx2 * _1p3x;
+		dN(8, 0) = _m3m9x2m2x * _1myt1mz,
+			  dN(8, 1) = -_1mx2t1m3x * _1mz,
+			  dN(8, 2) = -_1mx2t1m3x * _1my;
+		dN(9, 0) = _p3m9x2m2x * _1myt1mz,
+			  dN(9, 1) = -_1mx2t1p3x * _1mz,
+			  dN(9, 2) = -_1mx2t1p3x * _1my;
+		dN(10, 0) = _m3m9x2m2x * _1myt1pz,
+			   dN(10, 1) = -_1mx2t1m3x * _1pz,
+			   dN(10, 2) = _1mx2t1m3x * _1my;
+		dN(11, 0) = _p3m9x2m2x * _1myt1pz,
+			   dN(11, 1) = -_1mx2t1p3x * _1pz,
+			   dN(11, 2) = _1mx2t1p3x * _1my;
+		dN(12, 0) = _m3m9x2m2x * _1pyt1mz,
+			   dN(12, 1) = _1mx2t1m3x * _1mz,
+			   dN(12, 2) = -_1mx2t1m3x * _1py;
+		dN(13, 0) = _p3m9x2m2x * _1pyt1mz,
+			   dN(13, 1) = _1mx2t1p3x * _1mz,
+			   dN(13, 2) = -_1mx2t1p3x * _1py;
+		dN(14, 0) = _m3m9x2m2x * _1pyt1pz,
+			   dN(14, 1) = _1mx2t1m3x * _1pz,
+			   dN(14, 2) = _1mx2t1m3x * _1py;
+		dN(15, 0) = _p3m9x2m2x * _1pyt1pz,
+			   dN(15, 1) = _1mx2t1p3x * _1pz,
+			   dN(15, 2) = _1mx2t1p3x * _1py;
+
+		btScalar _m3m9y2m2y = -_3m9y2 - _2y;
+		btScalar _p3m9y2m2y = _3m9y2 - _2y;
+		btScalar _1my2t1m3y = _1my2 * _1m3y;
+		btScalar _1my2t1p3y = _1my2 * _1p3y;
+		dN(16, 0) = -_1my2t1m3y * _1mz,
+			   dN(16, 1) = _m3m9y2m2y * _1mxt1mz,
+			   dN(16, 2) = -_1my2t1m3y * _1mx;
+		dN(17, 0) = -_1my2t1p3y * _1mz,
+			   dN(17, 1) = _p3m9y2m2y * _1mxt1mz,
+			   dN(17, 2) = -_1my2t1p3y * _1mx;
+		dN(18, 0) = _1my2t1m3y * _1mz,
+			   dN(18, 1) = _m3m9y2m2y * _1pxt1mz,
+			   dN(18, 2) = -_1my2t1m3y * _1px;
+		dN(19, 0) = _1my2t1p3y * _1mz,
+			   dN(19, 1) = _p3m9y2m2y * _1pxt1mz,
+			   dN(19, 2) = -_1my2t1p3y * _1px;
+		dN(20, 0) = -_1my2t1m3y * _1pz,
+			   dN(20, 1) = _m3m9y2m2y * _1mxt1pz,
+			   dN(20, 2) = _1my2t1m3y * _1mx;
+		dN(21, 0) = -_1my2t1p3y * _1pz,
+			   dN(21, 1) = _p3m9y2m2y * _1mxt1pz,
+			   dN(21, 2) = _1my2t1p3y * _1mx;
+		dN(22, 0) = _1my2t1m3y * _1pz,
+			   dN(22, 1) = _m3m9y2m2y * _1pxt1pz,
+			   dN(22, 2) = _1my2t1m3y * _1px;
+		dN(23, 0) = _1my2t1p3y * _1pz,
+			   dN(23, 1) = _p3m9y2m2y * _1pxt1pz,
+			   dN(23, 2) = _1my2t1p3y * _1px;
+
+		btScalar _m3m9z2m2z = -_3m9z2 - _2z;
+		btScalar _p3m9z2m2z = _3m9z2 - _2z;
+		btScalar _1mz2t1m3z = _1mz2 * _1m3z;
+		btScalar _1mz2t1p3z = _1mz2 * _1p3z;
+		dN(24, 0) = -_1mz2t1m3z * _1my,
+			   dN(24, 1) = -_1mz2t1m3z * _1mx,
+			   dN(24, 2) = _m3m9z2m2z * _1mxt1my;
+		dN(25, 0) = -_1mz2t1p3z * _1my,
+			   dN(25, 1) = -_1mz2t1p3z * _1mx,
+			   dN(25, 2) = _p3m9z2m2z * _1mxt1my;
+		dN(26, 0) = -_1mz2t1m3z * _1py,
+			   dN(26, 1) = _1mz2t1m3z * _1mx,
+			   dN(26, 2) = _m3m9z2m2z * _1mxt1py;
+		dN(27, 0) = -_1mz2t1p3z * _1py,
+			   dN(27, 1) = _1mz2t1p3z * _1mx,
+			   dN(27, 2) = _p3m9z2m2z * _1mxt1py;
+		dN(28, 0) = _1mz2t1m3z * _1my,
+			   dN(28, 1) = -_1mz2t1m3z * _1px,
+			   dN(28, 2) = _m3m9z2m2z * _1pxt1my;
+		dN(29, 0) = _1mz2t1p3z * _1my,
+			   dN(29, 1) = -_1mz2t1p3z * _1px,
+			   dN(29, 2) = _p3m9z2m2z * _1pxt1my;
+		dN(30, 0) = _1mz2t1m3z * _1py,
+			   dN(30, 1) = _1mz2t1m3z * _1px,
+			   dN(30, 2) = _m3m9z2m2z * _1pxt1py;
+		dN(31, 0) = _1mz2t1p3z * _1py,
+			   dN(31, 1) = _1mz2t1p3z * _1px,
+			   dN(31, 2) = _p3m9z2m2z * _1pxt1py;
+
+		dN.bottomRowsMul(32u - 8u, 9.0 / 64.0);
+	}
+
+	return res;
+}
+
+bool btMiniSDF::interpolate(unsigned int field_id, double& dist, btVector3 const& x,
+							btVector3* gradient) const
+{
+	btAssert(m_isValid);
+	if (!m_isValid)
+		return false;
+
+	if (!m_domain.contains(x))
+		return false;
+
+	btVector3 tmpmi = ((x - m_domain.min()) * (m_inv_cell_size));  //.cast<unsigned int>().eval();
+	unsigned int mi[3] = {(unsigned int)tmpmi[0], (unsigned int)tmpmi[1], (unsigned int)tmpmi[2]};
+	if (mi[0] >= m_resolution[0])
+		mi[0] = m_resolution[0] - 1;
+	if (mi[1] >= m_resolution[1])
+		mi[1] = m_resolution[1] - 1;
+	if (mi[2] >= m_resolution[2])
+		mi[2] = m_resolution[2] - 1;
+	btMultiIndex mui;
+	mui.ijk[0] = mi[0];
+	mui.ijk[1] = mi[1];
+	mui.ijk[2] = mi[2];
+	int i = multiToSingleIndex(mui);
+	unsigned int i_ = m_cell_map[field_id][i];
+	if (i_ == UINT_MAX)
+		return false;
+
+	btAlignedBox3d sd = subdomain(i);
+	i = i_;
+	btVector3 d = sd.m_max - sd.m_min;  //.diagonal().eval();
+
+	btVector3 denom = (sd.max() - sd.min());
+	btVector3 c0 = btVector3(2.0, 2.0, 2.0) / denom;
+	btVector3 c1 = (sd.max() + sd.min()) / denom;
+	btVector3 xi = (c0 * x - c1);
+
+	btCell32 const& cell = m_cells[field_id][i];
+	if (!gradient)
+	{
+		//auto phi = m_coefficients[field_id][i].dot(shape_function_(xi, 0));
+		double phi = 0.0;
+		btShapeMatrix N = shape_function_(xi, 0);
+		for (unsigned int j = 0u; j < 32u; ++j)
+		{
+			unsigned int v = cell.m_cells[j];
+			double c = m_nodes[field_id][v];
+			if (c == DBL_MAX)
+			{
+				return false;
+				;
+			}
+			phi += c * N[j];
+		}
+
+		dist = phi;
+		return true;
+	}
+
+	btShapeGradients dN;
+	btShapeMatrix N = shape_function_(xi, &dN);
+
+	double phi = 0.0;
+	gradient->setZero();
+	for (unsigned int j = 0u; j < 32u; ++j)
+	{
+		unsigned int v = cell.m_cells[j];
+		double c = m_nodes[field_id][v];
+		if (c == DBL_MAX)
+		{
+			gradient->setZero();
+			return false;
+		}
+		phi += c * N[j];
+		(*gradient)[0] += c * dN(j, 0);
+		(*gradient)[1] += c * dN(j, 1);
+		(*gradient)[2] += c * dN(j, 2);
+	}
+	(*gradient) *= c0;
+	dist = phi;
+	return true;
+}
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.h
new file mode 100644
index 00000000000..b60fd102fdc
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.h
@@ -0,0 +1,127 @@
+#ifndef MINISDF_H
+#define MINISDF_H
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+struct btMultiIndex
+{
+	unsigned int ijk[3];
+};
+
+struct btAlignedBox3d
+{
+	btVector3 m_min;
+	btVector3 m_max;
+
+	const btVector3& min() const
+	{
+		return m_min;
+	}
+
+	const btVector3& max() const
+	{
+		return m_max;
+	}
+
+	bool contains(const btVector3& x) const
+	{
+		return TestPointAgainstAabb2(m_min, m_max, x);
+	}
+
+	btAlignedBox3d(const btVector3& mn, const btVector3& mx)
+		: m_min(mn),
+		  m_max(mx)
+	{
+	}
+
+	btAlignedBox3d()
+	{
+	}
+};
+
+struct btShapeMatrix
+{
+	double m_vec[32];
+
+	inline double& operator[](int i)
+	{
+		return m_vec[i];
+	}
+
+	inline const double& operator[](int i) const
+	{
+		return m_vec[i];
+	}
+};
+
+struct btShapeGradients
+{
+	btVector3 m_vec[32];
+
+	void topRowsDivide(int row, double denom)
+	{
+		for (int i = 0; i < row; i++)
+		{
+			m_vec[i] /= denom;
+		}
+	}
+
+	void bottomRowsMul(int row, double val)
+	{
+		for (int i = 32 - row; i < 32; i++)
+		{
+			m_vec[i] *= val;
+		}
+	}
+
+	inline btScalar& operator()(int i, int j)
+	{
+		return m_vec[i][j];
+	}
+};
+
+struct btCell32
+{
+	unsigned int m_cells[32];
+};
+
+struct btMiniSDF
+{
+	btAlignedBox3d m_domain;
+	unsigned int m_resolution[3];
+	btVector3 m_cell_size;
+	btVector3 m_inv_cell_size;
+	std::size_t m_n_cells;
+	std::size_t m_n_fields;
+	bool m_isValid;
+
+	btAlignedObjectArray<btAlignedObjectArray<double> > m_nodes;
+	btAlignedObjectArray<btAlignedObjectArray<btCell32> > m_cells;
+	btAlignedObjectArray<btAlignedObjectArray<unsigned int> > m_cell_map;
+
+	btMiniSDF()
+		: m_isValid(false)
+	{
+	}
+	bool load(const char* data, int size);
+	bool isValid() const
+	{
+		return m_isValid;
+	}
+	unsigned int multiToSingleIndex(btMultiIndex const& ijk) const;
+
+	btAlignedBox3d subdomain(btMultiIndex const& ijk) const;
+
+	btMultiIndex singleToMultiIndex(unsigned int l) const;
+
+	btAlignedBox3d subdomain(unsigned int l) const;
+
+	btShapeMatrix
+	shape_function_(btVector3 const& xi, btShapeGradients* gradient = 0) const;
+
+	bool interpolate(unsigned int field_id, double& dist, btVector3 const& x, btVector3* gradient) const;
+};
+
+#endif  //MINISDF_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp
index 06707e24e55..d4b6a651de0 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp
@@ -13,48 +13,59 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btMinkowskiSumShape.h"
 
-
-btMinkowskiSumShape::btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB)
-: btConvexInternalShape (),
-m_shapeA(shapeA),
-m_shapeB(shapeB)
+btMinkowskiSumShape::btMinkowskiSumShape(const btConvexShape* shapeA, const btConvexShape* shapeB)
+	: btConvexInternalShape(),
+	  m_shapeA(shapeA),
+	  m_shapeB(shapeB)
 {
 	m_shapeType = MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE;
 	m_transA.setIdentity();
 	m_transB.setIdentity();
 }
 
-btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 {
-	btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec*m_transA.getBasis()));
-	btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(-vec*m_transB.getBasis()));
-	return  supVertexA - supVertexB;
+	btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec * m_transA.getBasis()));
+	btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(-vec * m_transB.getBasis()));
+	return supVertexA - supVertexB;
 }
 
-void	btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 {
 	///@todo: could make recursive use of batching. probably this shape is not used frequently.
-	for (int i=0;i<numVectors;i++)
+	for (int i = 0; i < numVectors; i++)
 	{
 		supportVerticesOut[i] = localGetSupportingVertexWithoutMargin(vectors[i]);
 	}
-
 }
 
-
-
-btScalar	btMinkowskiSumShape::getMargin() const
+btScalar btMinkowskiSumShape::getMargin() const
 {
 	return m_shapeA->getMargin() + m_shapeB->getMargin();
 }
 
-
-void	btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btMinkowskiSumShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
 	(void)mass;
-	btAssert(0);
-	inertia.setValue(0,0,0);
+	//inertia of the AABB of the Minkowski sum
+	btTransform identity;
+	identity.setIdentity();
+	btVector3 aabbMin, aabbMax;
+	getAabb(identity, aabbMin, aabbMax);
+
+	btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
+
+	btScalar margin = getMargin();
+
+	btScalar lx = btScalar(2.) * (halfExtents.x() + margin);
+	btScalar ly = btScalar(2.) * (halfExtents.y() + margin);
+	btScalar lz = btScalar(2.) * (halfExtents.z() + margin);
+	const btScalar x2 = lx * lx;
+	const btScalar y2 = ly * ly;
+	const btScalar z2 = lz * lz;
+	const btScalar scaledmass = mass * btScalar(0.08333333);
+
+	inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2));
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h
index a3f9a472394..3b5150f6d50 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h
@@ -17,46 +17,43 @@ subject to the following restrictions:
 #define BT_MINKOWSKI_SUM_SHAPE_H
 
 #include "btConvexInternalShape.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"  // for the types
 
 /// The btMinkowskiSumShape is only for advanced users. This shape represents implicit based minkowski sum of two convex implicit shapes.
-ATTRIBUTE_ALIGNED16(class) btMinkowskiSumShape : public btConvexInternalShape
+ATTRIBUTE_ALIGNED16(class)
+btMinkowskiSumShape : public btConvexInternalShape
 {
-
-	btTransform	m_transA;
-	btTransform	m_transB;
-	const btConvexShape*	m_shapeA;
-	const btConvexShape*	m_shapeB;
+	btTransform m_transA;
+	btTransform m_transB;
+	const btConvexShape* m_shapeA;
+	const btConvexShape* m_shapeB;
 
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-BT_DECLARE_ALIGNED_ALLOCATOR();
-
-	btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB);
-
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
-
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+	btMinkowskiSumShape(const btConvexShape* shapeA, const btConvexShape* shapeB);
 
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
 
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
 
-	void	setTransformA(const btTransform&	transA) { m_transA = transA;}
-	void	setTransformB(const btTransform&	transB) { m_transB = transB;}
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
 
-	const btTransform& getTransformA()const  { return m_transA;}
-	const btTransform& GetTransformB()const  { return m_transB;}
+	void setTransformA(const btTransform& transA) { m_transA = transA; }
+	void setTransformB(const btTransform& transB) { m_transB = transB; }
 
+	const btTransform& getTransformA() const { return m_transA; }
+	const btTransform& GetTransformB() const { return m_transB; }
 
-	virtual btScalar	getMargin() const;
+	virtual btScalar getMargin() const;
 
-	const btConvexShape*	getShapeA() const { return m_shapeA;}
-	const btConvexShape*	getShapeB() const { return m_shapeB;}
+	const btConvexShape* getShapeA() const { return m_shapeA; }
+	const btConvexShape* getShapeB() const { return m_shapeB; }
 
-	virtual const char*	getName()const 
+	virtual const char* getName() const
 	{
 		return "MinkowskiSum";
 	}
 };
 
-#endif //BT_MINKOWSKI_SUM_SHAPE_H
+#endif  //BT_MINKOWSKI_SUM_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
index 6abfdffa677..c0cc55dfb08 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
@@ -13,52 +13,50 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-//#if defined (_WIN32) || defined (__i386__)
-//#define BT_USE_SSE_IN_API
-//#endif
+#if defined(_WIN32) || defined(__i386__)
+#define BT_USE_SSE_IN_API
+#endif
 
 #include "btMultiSphereShape.h"
 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
 #include "LinearMath/btQuaternion.h"
 #include "LinearMath/btSerializer.h"
 
-btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres)
-:btConvexInternalAabbCachingShape ()
+btMultiSphereShape::btMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres)
+	: btConvexInternalAabbCachingShape()
 {
 	m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE;
 	//btScalar startMargin = btScalar(BT_LARGE_FLOAT);
 
 	m_localPositionArray.resize(numSpheres);
 	m_radiArray.resize(numSpheres);
-	for (int i=0;i<numSpheres;i++)
+	for (int i = 0; i < numSpheres; i++)
 	{
 		m_localPositionArray[i] = positions[i];
 		m_radiArray[i] = radi[i];
-		
 	}
 
 	recalcLocalAabb();
-
 }
 
 #ifndef MIN
-	#define MIN( _a, _b)    ((_a) < (_b) ? (_a) : (_b))
+#define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b))
 #endif
- btVector3	btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const
 {
-	btVector3 supVec(0,0,0);
+	btVector3 supVec(0, 0, 0);
 
 	btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
 
-
 	btVector3 vec = vec0;
 	btScalar lenSqr = vec.length2();
-	if (lenSqr < (SIMD_EPSILON*SIMD_EPSILON))
+	if (lenSqr < (SIMD_EPSILON * SIMD_EPSILON))
 	{
-		vec.setValue(1,0,0);
-	} else
+		vec.setValue(1, 0, 0);
+	}
+	else
 	{
-		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+		btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
 		vec *= rlen;
 	}
 
@@ -69,32 +67,30 @@ btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScala
 	const btScalar* rad = &m_radiArray[0];
 	int numSpheres = m_localPositionArray.size();
 
-	for( int k = 0; k < numSpheres; k+= 128 )
+	for (int k = 0; k < numSpheres; k += 128)
 	{
 		btVector3 temp[128];
-		int inner_count = MIN( numSpheres - k, 128 );
-        for( long i = 0; i < inner_count; i++ )
-        {
-            temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
-            pos++;
-            rad++;
-        }
-        long i = vec.maxDot( temp, inner_count, newDot);
-        if( newDot > maxDot )
+		int inner_count = MIN(numSpheres - k, 128);
+		for (long i = 0; i < inner_count; i++)
+		{
+			temp[i] = (*pos) * m_localScaling + vec * m_localScaling * (*rad) - vec * getMargin();
+			pos++;
+			rad++;
+		}
+		long i = vec.maxDot(temp, inner_count, newDot);
+		if (newDot > maxDot)
 		{
 			maxDot = newDot;
 			supVec = temp[i];
 		}
-    }
+	}
 
 	return supVec;
-
 }
 
- void	btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 {
-
-	for (int j=0;j<numVectors;j++)
+	for (int j = 0; j < numVectors; j++)
 	{
 		btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
 
@@ -107,76 +103,67 @@ btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScala
 		const btScalar* rad = &m_radiArray[0];
 		int numSpheres = m_localPositionArray.size();
 
-        for( int k = 0; k < numSpheres; k+= 128 )
-        {
-            btVector3 temp[128];
-            int inner_count = MIN( numSpheres - k, 128 );
-            for( long i = 0; i < inner_count; i++ )
-            {
-                temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
-                pos++;
-                rad++;
-            }
-            long i = vec.maxDot( temp, inner_count, newDot);
-            if( newDot > maxDot )
-            {
-                maxDot = newDot;
-                supportVerticesOut[j] = temp[i];
-            }
-        }
-        
+		for (int k = 0; k < numSpheres; k += 128)
+		{
+			btVector3 temp[128];
+			int inner_count = MIN(numSpheres - k, 128);
+			for (long i = 0; i < inner_count; i++)
+			{
+				temp[i] = (*pos) * m_localScaling + vec * m_localScaling * (*rad) - vec * getMargin();
+				pos++;
+				rad++;
+			}
+			long i = vec.maxDot(temp, inner_count, newDot);
+			if (newDot > maxDot)
+			{
+				maxDot = newDot;
+				supportVerticesOut[j] = temp[i];
+			}
+		}
 	}
 }
 
-
-
-
-
-
-
-
-void	btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btMultiSphereShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
 	//as an approximation, take the inertia of the box that bounds the spheres
 
-	btVector3 localAabbMin,localAabbMax;
-	getCachedLocalAabb(localAabbMin,localAabbMax);
-	btVector3 halfExtents = (localAabbMax-localAabbMin)*btScalar(0.5);
-
-	btScalar lx=btScalar(2.)*(halfExtents.x());
-	btScalar ly=btScalar(2.)*(halfExtents.y());
-	btScalar lz=btScalar(2.)*(halfExtents.z());
+	btVector3 localAabbMin, localAabbMax;
+	getCachedLocalAabb(localAabbMin, localAabbMax);
+	btVector3 halfExtents = (localAabbMax - localAabbMin) * btScalar(0.5);
 
-	inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
-					mass/(btScalar(12.0)) * (lx*lx + lz*lz),
-					mass/(btScalar(12.0)) * (lx*lx + ly*ly));
+	btScalar lx = btScalar(2.) * (halfExtents.x());
+	btScalar ly = btScalar(2.) * (halfExtents.y());
+	btScalar lz = btScalar(2.) * (halfExtents.z());
 
+	inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz),
+					 mass / (btScalar(12.0)) * (lx * lx + lz * lz),
+					 mass / (btScalar(12.0)) * (lx * lx + ly * ly));
 }
 
-
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const
+const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*) dataBuffer;
+	btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*)dataBuffer;
 	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
 
 	int numElem = m_localPositionArray.size();
-	shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]):  0;
-	
+	shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]) : 0;
+
 	shapeData->m_localPositionArraySize = numElem;
 	if (numElem)
 	{
-		btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius),numElem);
+		btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius), numElem);
 		btPositionAndRadius* memPtr = (btPositionAndRadius*)chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
 			m_localPositionArray[i].serializeFloat(memPtr->m_pos);
 			memPtr->m_radius = float(m_radiArray[i]);
 		}
-		serializer->finalizeChunk(chunk,"btPositionAndRadius",BT_ARRAY_CODE,(void*)&m_localPositionArray[0]);
+		serializer->finalizeChunk(chunk, "btPositionAndRadius", BT_ARRAY_CODE, (void*)&m_localPositionArray[0]);
 	}
-	
-	return "btMultiSphereShapeData";
-}
 
+	// Fill padding with zeros to appease msan.
+	memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding));
 
+	return "btMultiSphereShapeData";
+}
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h
index 5d3b4026848..2d79c07ca42 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h
@@ -17,69 +17,65 @@ subject to the following restrictions:
 #define BT_MULTI_SPHERE_MINKOWSKI_H
 
 #include "btConvexInternalShape.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"  // for the types
 #include "LinearMath/btAlignedObjectArray.h"
 #include "LinearMath/btAabbUtil2.h"
 
-
-
 ///The btMultiSphereShape represents the convex hull of a collection of spheres. You can create special capsules or other smooth volumes.
 ///It is possible to animate the spheres for deformation, but call 'recalcLocalAabb' after changing any sphere position/radius
-ATTRIBUTE_ALIGNED16(class) btMultiSphereShape : public btConvexInternalAabbCachingShape
+ATTRIBUTE_ALIGNED16(class)
+btMultiSphereShape : public btConvexInternalAabbCachingShape
 {
-	
 	btAlignedObjectArray<btVector3> m_localPositionArray;
-	btAlignedObjectArray<btScalar>  m_radiArray;
-	
+	btAlignedObjectArray<btScalar> m_radiArray;
+
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-	btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres);
+
+	btMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres);
 
 	///CollisionShape Interface
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
 
 	/// btConvexShape Interface
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
 
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
-	
-	int	getSphereCount() const
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
+
+	int getSphereCount() const
 	{
 		return m_localPositionArray.size();
 	}
 
-	const btVector3&	getSpherePosition(int index) const
+	const btVector3& getSpherePosition(int index) const
 	{
 		return m_localPositionArray[index];
 	}
 
-	btScalar	getSphereRadius(int index) const
+	btScalar getSphereRadius(int index) const
 	{
 		return m_radiArray[index];
 	}
 
-
-	virtual const char*	getName()const 
+	virtual const char* getName() const
 	{
 		return "MultiSphere";
 	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
-
-struct	btPositionAndRadius
+struct btPositionAndRadius
 {
-	btVector3FloatData	m_pos;
-	float		m_radius;
+	btVector3FloatData m_pos;
+	float m_radius;
 };
 
+// clang-format off
+
 struct	btMultiSphereShapeData
 {
 	btConvexInternalShapeData	m_convexInternalShapeData;
@@ -89,13 +85,11 @@ struct	btMultiSphereShapeData
 	char	m_padding[4];
 };
 
+// clang-format on
 
-
-SIMD_FORCE_INLINE	int	btMultiSphereShape::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btMultiSphereShape::calculateSerializeBufferSize() const
 {
 	return sizeof(btMultiSphereShapeData);
 }
 
-
-
-#endif //BT_MULTI_SPHERE_MINKOWSKI_H
+#endif  //BT_MULTI_SPHERE_MINKOWSKI_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp
index 58799ac96ad..30108c9e7b4 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp
@@ -19,27 +19,25 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h"
 //#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
 
-
 ///Obtains the material for a specific triangle
-const btMaterial * btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex)
+const btMaterial *btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex)
 {
-    const unsigned char * materialBase = 0;
-    int numMaterials;
-    PHY_ScalarType materialType;
-    int materialStride;
-    const unsigned char * triangleMaterialBase = 0;
-    int numTriangles;
-    int triangleMaterialStride;
-    PHY_ScalarType triangleType;
-
-    ((btTriangleIndexVertexMaterialArray*)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride,
-        &triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID);
-
-    // return the pointer to the place with the friction for the triangle
-    // TODO: This depends on whether it's a moving mesh or not
-    // BUG IN GIMPACT
-    //return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]);
-    int * matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)]));
-    btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride]));
-    return (matVal);
+	const unsigned char *materialBase = 0;
+	int numMaterials;
+	PHY_ScalarType materialType;
+	int materialStride;
+	const unsigned char *triangleMaterialBase = 0;
+	int numTriangles;
+	int triangleMaterialStride;
+	PHY_ScalarType triangleType;
+
+	((btTriangleIndexVertexMaterialArray *)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride, &triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID);
+
+	// return the pointer to the place with the friction for the triangle
+	// TODO: This depends on whether it's a moving mesh or not
+	// BUG IN GIMPACT
+	//return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]);
+	int *matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)]));
+	btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride]));
+	return (matVal);
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h
index 5ebaede4a88..d1d42f8e049 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h
@@ -22,82 +22,80 @@ subject to the following restrictions:
 #include "btMaterial.h"
 
 ///The BvhTriangleMaterialMeshShape extends the btBvhTriangleMeshShape. Its main contribution is the interface into a material array, which allows per-triangle friction and restitution.
-ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape
+ATTRIBUTE_ALIGNED16(class)
+btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape
 {
-    btAlignedObjectArray <btMaterial*> m_materialList;
+	btAlignedObjectArray<btMaterial *> m_materialList;
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-    btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true):
-        btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh)
-        {
-            m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
-
-            const unsigned char *vertexbase;
-            int numverts;
-            PHY_ScalarType type;
-            int stride;
-            const unsigned char *indexbase;
-            int indexstride;
-            int numfaces;
-            PHY_ScalarType indicestype;
-
-            //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));
-
-            for(int i = 0; i < meshInterface->getNumSubParts(); i++)
-            {
-                m_meshInterface->getLockedReadOnlyVertexIndexBase(
-                    &vertexbase,
-                    numverts,
-                    type,
-                    stride,
-                    &indexbase,
-                    indexstride,
-                    numfaces,
-                    indicestype,
-                    i);
-                //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16));
-            }
-        }
+	btMultimaterialTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true) : btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh)
+	{
+		m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
+
+		const unsigned char *vertexbase;
+		int numverts;
+		PHY_ScalarType type;
+		int stride;
+		const unsigned char *indexbase;
+		int indexstride;
+		int numfaces;
+		PHY_ScalarType indicestype;
+
+		//m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));
+
+		for (int i = 0; i < meshInterface->getNumSubParts(); i++)
+		{
+			m_meshInterface->getLockedReadOnlyVertexIndexBase(
+				&vertexbase,
+				numverts,
+				type,
+				stride,
+				&indexbase,
+				indexstride,
+				numfaces,
+				indicestype,
+				i);
+			//m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16));
+		}
+	}
 
 	///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb
-	btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true):
-        btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh)
-        {
-            m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
-
-            const unsigned char *vertexbase;
-            int numverts;
-            PHY_ScalarType type;
-            int stride;
-            const unsigned char *indexbase;
-            int indexstride;
-            int numfaces;
-            PHY_ScalarType indicestype;
-
-            //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));
-
-            for(int i = 0; i < meshInterface->getNumSubParts(); i++)
-            {
-                m_meshInterface->getLockedReadOnlyVertexIndexBase(
-                    &vertexbase,
-                    numverts,
-                    type,
-                    stride,
-                    &indexbase,
-                    indexstride,
-                    numfaces,
-                    indicestype,
-                    i);
-                //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16));
-            }
-        }
-	
-    virtual ~btMultimaterialTriangleMeshShape()
-    {
-/*
+	btMultimaterialTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, const btVector3 &bvhAabbMin, const btVector3 &bvhAabbMax, bool buildBvh = true) : btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh)
+	{
+		m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
+
+		const unsigned char *vertexbase;
+		int numverts;
+		PHY_ScalarType type;
+		int stride;
+		const unsigned char *indexbase;
+		int indexstride;
+		int numfaces;
+		PHY_ScalarType indicestype;
+
+		//m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));
+
+		for (int i = 0; i < meshInterface->getNumSubParts(); i++)
+		{
+			m_meshInterface->getLockedReadOnlyVertexIndexBase(
+				&vertexbase,
+				numverts,
+				type,
+				stride,
+				&indexbase,
+				indexstride,
+				numfaces,
+				indicestype,
+				i);
+			//m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16));
+		}
+	}
+
+	virtual ~btMultimaterialTriangleMeshShape()
+	{
+		/*
         for(int i = 0; i < m_meshInterface->getNumSubParts(); i++)
         {
             btAlignedFree(m_materialValues[i]);
@@ -106,14 +104,12 @@ public:
         btAlignedFree(m_materialValues);
         m_materialLookup = NULL;
 */
-    }
+	}
 	//debugging
-	virtual const char*	getName()const {return "MULTIMATERIALTRIANGLEMESH";}
-
-    ///Obtains the material for a specific triangle
-    const btMaterial * getMaterialProperties(int partID, int triIndex);
+	virtual const char *getName() const { return "MULTIMATERIALTRIANGLEMESH"; }
 
-}
-;
+	///Obtains the material for a specific triangle
+	const btMaterial *getMaterialProperties(int partID, int triIndex);
+};
 
-#endif //BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
+#endif  //BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
index 6f36775f7c9..863ea6d6ac2 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
@@ -13,51 +13,46 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btOptimizedBvh.h"
 #include "btStridingMeshInterface.h"
 #include "LinearMath/btAabbUtil2.h"
 #include "LinearMath/btIDebugDraw.h"
 
-
 btOptimizedBvh::btOptimizedBvh()
-{ 
+{
 }
 
 btOptimizedBvh::~btOptimizedBvh()
 {
 }
 
-
 void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax)
 {
 	m_useQuantization = useQuantizedAabbCompression;
 
-
 	// NodeArray	triangleNodes;
 
-	struct	NodeTriangleCallback : public btInternalTriangleIndexCallback
+	struct NodeTriangleCallback : public btInternalTriangleIndexCallback
 	{
-
-		NodeArray&	m_triangleNodes;
+		NodeArray& m_triangleNodes;
 
 		NodeTriangleCallback& operator=(NodeTriangleCallback& other)
 		{
 			m_triangleNodes.copyFromArray(other.m_triangleNodes);
 			return *this;
 		}
-		
-		NodeTriangleCallback(NodeArray&	triangleNodes)
-			:m_triangleNodes(triangleNodes)
+
+		NodeTriangleCallback(NodeArray& triangleNodes)
+			: m_triangleNodes(triangleNodes)
 		{
 		}
 
-		virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+		virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
 		{
 			btOptimizedBvhNode node;
-			btVector3	aabbMin,aabbMax;
-			aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-			aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); 
+			btVector3 aabbMin, aabbMax;
+			aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+			aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
 			aabbMin.setMin(triangle[0]);
 			aabbMax.setMax(triangle[0]);
 			aabbMin.setMin(triangle[1]);
@@ -70,17 +65,17 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized
 			node.m_aabbMaxOrg = aabbMax;
 
 			node.m_escapeIndex = -1;
-	
+
 			//for child nodes
 			node.m_subPart = partId;
 			node.m_triangleIndex = triangleIndex;
 			m_triangleNodes.push_back(node);
 		}
 	};
-	struct	QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback
+	struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback
 	{
-		QuantizedNodeArray&	m_triangleNodes;
-		const btQuantizedBvh* m_optimizedTree; // for quantization
+		QuantizedNodeArray& m_triangleNodes;
+		const btQuantizedBvh* m_optimizedTree;  // for quantization
 
 		QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other)
 		{
@@ -89,23 +84,23 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized
 			return *this;
 		}
 
-		QuantizedNodeTriangleCallback(QuantizedNodeArray&	triangleNodes,const btQuantizedBvh* tree)
-			:m_triangleNodes(triangleNodes),m_optimizedTree(tree)
+		QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes, const btQuantizedBvh* tree)
+			: m_triangleNodes(triangleNodes), m_optimizedTree(tree)
 		{
 		}
 
-		virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+		virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
 		{
 			// The partId and triangle index must fit in the same (positive) integer
-			btAssert(partId < (1<<MAX_NUM_PARTS_IN_BITS));
-			btAssert(triangleIndex < (1<<(31-MAX_NUM_PARTS_IN_BITS)));
+			btAssert(partId < (1 << MAX_NUM_PARTS_IN_BITS));
+			btAssert(triangleIndex < (1 << (31 - MAX_NUM_PARTS_IN_BITS)));
 			//negative indices are reserved for escapeIndex
-			btAssert(triangleIndex>=0);
+			btAssert(triangleIndex >= 0);
 
 			btQuantizedBvhNode node;
-			btVector3	aabbMin,aabbMax;
-			aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-			aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); 
+			btVector3 aabbMin, aabbMax;
+			aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+			aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
 			aabbMin.setMin(triangle[0]);
 			aabbMax.setMax(triangle[0]);
 			aabbMin.setMin(triangle[1]);
@@ -132,59 +127,52 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized
 				aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION);
 			}
 
-			m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
-			m_optimizedTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
+			m_optimizedTree->quantize(&node.m_quantizedAabbMin[0], aabbMin, 0);
+			m_optimizedTree->quantize(&node.m_quantizedAabbMax[0], aabbMax, 1);
 
-			node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
+			node.m_escapeIndexOrTriangleIndex = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
 
 			m_triangleNodes.push_back(node);
 		}
 	};
-	
-
 
 	int numLeafNodes = 0;
 
-	
 	if (m_useQuantization)
 	{
-
 		//initialize quantization values
-		setQuantizationValues(bvhAabbMin,bvhAabbMax);
+		setQuantizationValues(bvhAabbMin, bvhAabbMax);
 
-		QuantizedNodeTriangleCallback	callback(m_quantizedLeafNodes,this);
+		QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes, this);
 
-	
-		triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax);
+		triangles->InternalProcessAllTriangles(&callback, m_bvhAabbMin, m_bvhAabbMax);
 
 		//now we have an array of leafnodes in m_leafNodes
 		numLeafNodes = m_quantizedLeafNodes.size();
 
-
-		m_quantizedContiguousNodes.resize(2*numLeafNodes);
-
-
-	} else
+		m_quantizedContiguousNodes.resize(2 * numLeafNodes);
+	}
+	else
 	{
-		NodeTriangleCallback	callback(m_leafNodes);
+		NodeTriangleCallback callback(m_leafNodes);
 
-		btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
-		btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+		btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
+		btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
 
-		triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
+		triangles->InternalProcessAllTriangles(&callback, aabbMin, aabbMax);
 
 		//now we have an array of leafnodes in m_leafNodes
 		numLeafNodes = m_leafNodes.size();
 
-		m_contiguousNodes.resize(2*numLeafNodes);
+		m_contiguousNodes.resize(2 * numLeafNodes);
 	}
 
 	m_curNodeIndex = 0;
 
-	buildTree(0,numLeafNodes);
+	buildTree(0, numLeafNodes);
 
 	///if the entire tree is small then subtree size, we need to create a header info for the tree
-	if(m_useQuantization && !m_SubtreeHeaders.size())
+	if (m_useQuantization && !m_SubtreeHeaders.size())
 	{
 		btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
 		subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]);
@@ -200,37 +188,29 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized
 	m_leafNodes.clear();
 }
 
-
-
-
-void	btOptimizedBvh::refit(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax)
+void btOptimizedBvh::refit(btStridingMeshInterface* meshInterface, const btVector3& aabbMin, const btVector3& aabbMax)
 {
 	if (m_useQuantization)
 	{
+		setQuantizationValues(aabbMin, aabbMax);
 
-		setQuantizationValues(aabbMin,aabbMax);
-
-		updateBvhNodes(meshInterface,0,m_curNodeIndex,0);
+		updateBvhNodes(meshInterface, 0, m_curNodeIndex, 0);
 
 		///now update all subtree headers
 
 		int i;
-		for (i=0;i<m_SubtreeHeaders.size();i++)
+		for (i = 0; i < m_SubtreeHeaders.size(); i++)
 		{
 			btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
 			subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
 		}
-
-	} else
+	}
+	else
 	{
-
 	}
 }
 
-
-
-
-void	btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax)
+void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface, const btVector3& aabbMin, const btVector3& aabbMax)
 {
 	//incrementally initialize quantization values
 	btAssert(m_useQuantization);
@@ -245,147 +225,140 @@ void	btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const b
 
 	///we should update all quantization values, using updateBvhNodes(meshInterface);
 	///but we only update chunks that overlap the given aabb
-	
-	unsigned short	quantizedQueryAabbMin[3];
-	unsigned short	quantizedQueryAabbMax[3];
 
-	quantize(&quantizedQueryAabbMin[0],aabbMin,0);
-	quantize(&quantizedQueryAabbMax[0],aabbMax,1);
+	unsigned short quantizedQueryAabbMin[3];
+	unsigned short quantizedQueryAabbMax[3];
+
+	quantize(&quantizedQueryAabbMin[0], aabbMin, 0);
+	quantize(&quantizedQueryAabbMax[0], aabbMax, 1);
 
 	int i;
-	for (i=0;i<this->m_SubtreeHeaders.size();i++)
+	for (i = 0; i < this->m_SubtreeHeaders.size(); i++)
 	{
 		btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
 
 		//PCK: unsigned instead of bool
-		unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+		unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, subtree.m_quantizedAabbMin, subtree.m_quantizedAabbMax);
 		if (overlap != 0)
 		{
-			updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i);
+			updateBvhNodes(meshInterface, subtree.m_rootNodeIndex, subtree.m_rootNodeIndex + subtree.m_subtreeSize, i);
 
 			subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
 		}
 	}
-	
 }
 
-void	btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index)
+void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface, int firstNode, int endNode, int index)
 {
 	(void)index;
 
 	btAssert(m_useQuantization);
 
-	int curNodeSubPart=-1;
+	int curNodeSubPart = -1;
 
 	//get access info to trianglemesh data
-		const unsigned char *vertexbase = 0;
-		int numverts = 0;
-		PHY_ScalarType type = PHY_INTEGER;
-		int stride = 0;
-		const unsigned char *indexbase = 0;
-		int indexstride = 0;
-		int numfaces = 0;
-		PHY_ScalarType indicestype = PHY_INTEGER;
-
-		btVector3	triangleVerts[3];
-		btVector3	aabbMin,aabbMax;
-		const btVector3& meshScaling = meshInterface->getScaling();
-		
-		int i;
-		for (i=endNode-1;i>=firstNode;i--)
+	const unsigned char* vertexbase = 0;
+	int numverts = 0;
+	PHY_ScalarType type = PHY_INTEGER;
+	int stride = 0;
+	const unsigned char* indexbase = 0;
+	int indexstride = 0;
+	int numfaces = 0;
+	PHY_ScalarType indicestype = PHY_INTEGER;
+
+	btVector3 triangleVerts[3];
+	btVector3 aabbMin, aabbMax;
+	const btVector3& meshScaling = meshInterface->getScaling();
+
+	int i;
+	for (i = endNode - 1; i >= firstNode; i--)
+	{
+		btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i];
+		if (curNode.isLeafNode())
 		{
+			//recalc aabb from triangle data
+			int nodeSubPart = curNode.getPartId();
+			int nodeTriangleIndex = curNode.getTriangleIndex();
+			if (nodeSubPart != curNodeSubPart)
+			{
+				if (curNodeSubPart >= 0)
+					meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
+				meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, nodeSubPart);
 
+				curNodeSubPart = nodeSubPart;
+			}
+			//triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
 
-			btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i];
-			if (curNode.isLeafNode())
+			unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride);
+
+			for (int j = 2; j >= 0; j--)
 			{
-				//recalc aabb from triangle data
-				int nodeSubPart = curNode.getPartId();
-				int nodeTriangleIndex = curNode.getTriangleIndex();
-				if (nodeSubPart != curNodeSubPart)
+				int graphicsindex;
+                                switch (indicestype) {
+                                        case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
+                                        case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
+                                        case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
+                                        default: btAssert(0);
+                                }
+				if (type == PHY_FLOAT)
 				{
-					if (curNodeSubPart >= 0)
-						meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
-					meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,	type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart);
-
-					curNodeSubPart = nodeSubPart;
-					btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
+					float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
+					triangleVerts[j] = btVector3(
+						graphicsbase[0] * meshScaling.getX(),
+						graphicsbase[1] * meshScaling.getY(),
+						graphicsbase[2] * meshScaling.getZ());
 				}
-				//triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
-
-				unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
-				
-				
-				for (int j=2;j>=0;j--)
+				else
 				{
-					
-					int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
-					if (type == PHY_FLOAT)
-					{
-						float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
-						triangleVerts[j] = btVector3(
-							graphicsbase[0]*meshScaling.getX(),
-							graphicsbase[1]*meshScaling.getY(),
-							graphicsbase[2]*meshScaling.getZ());
-					}
-					else
-					{
-						double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
-						triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
-					}
+					double* graphicsbase = (double*)(vertexbase + graphicsindex * stride);
+					triangleVerts[j] = btVector3(btScalar(graphicsbase[0] * meshScaling.getX()), btScalar(graphicsbase[1] * meshScaling.getY()), btScalar(graphicsbase[2] * meshScaling.getZ()));
 				}
+			}
 
+			aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+			aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
+			aabbMin.setMin(triangleVerts[0]);
+			aabbMax.setMax(triangleVerts[0]);
+			aabbMin.setMin(triangleVerts[1]);
+			aabbMax.setMax(triangleVerts[1]);
+			aabbMin.setMin(triangleVerts[2]);
+			aabbMax.setMax(triangleVerts[2]);
+
+			quantize(&curNode.m_quantizedAabbMin[0], aabbMin, 0);
+			quantize(&curNode.m_quantizedAabbMax[0], aabbMax, 1);
+		}
+		else
+		{
+			//combine aabb from both children
 
-				
-				aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-				aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); 
-				aabbMin.setMin(triangleVerts[0]);
-				aabbMax.setMax(triangleVerts[0]);
-				aabbMin.setMin(triangleVerts[1]);
-				aabbMax.setMax(triangleVerts[1]);
-				aabbMin.setMin(triangleVerts[2]);
-				aabbMax.setMax(triangleVerts[2]);
-
-				quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0);
-				quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1);
-				
-			} else
-			{
-				//combine aabb from both children
+			btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i + 1];
 
-				btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1];
-				
-				btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] :
-					&m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()];
-				
+			btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i + 2] : &m_quantizedContiguousNodes[i + 1 + leftChildNode->getEscapeIndex()];
 
+			{
+				for (int i = 0; i < 3; i++)
 				{
-					for (int i=0;i<3;i++)
-					{
-						curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i];
-						if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i])
-							curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i];
-
-						curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i];
-						if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i])
-							curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i];
-					}
+					curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i];
+					if (curNode.m_quantizedAabbMin[i] > rightChildNode->m_quantizedAabbMin[i])
+						curNode.m_quantizedAabbMin[i] = rightChildNode->m_quantizedAabbMin[i];
+
+					curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i];
+					if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i])
+						curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i];
 				}
 			}
-
 		}
+	}
 
-		if (curNodeSubPart >= 0)
-			meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
-
-		
+	if (curNodeSubPart >= 0)
+		meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
 }
 
 ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
-btOptimizedBvh* btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian)
+btOptimizedBvh* btOptimizedBvh::deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian)
 {
-	btQuantizedBvh* bvh = btQuantizedBvh::deSerializeInPlace(i_alignedDataBuffer,i_dataBufferSize,i_swapEndian);
-	
+	btQuantizedBvh* bvh = btQuantizedBvh::deSerializeInPlace(i_alignedDataBuffer, i_dataBufferSize, i_swapEndian);
+
 	//we don't add additional data so just do a static upcast
 	return static_cast<btOptimizedBvh*>(bvh);
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h
index 715961f5528..22f131c8b25 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h
@@ -22,44 +22,35 @@ subject to the following restrictions:
 
 class btStridingMeshInterface;
 
-
 ///The btOptimizedBvh extends the btQuantizedBvh to create AABB tree for triangle meshes, through the btStridingMeshInterface.
-ATTRIBUTE_ALIGNED16(class) btOptimizedBvh : public btQuantizedBvh
+ATTRIBUTE_ALIGNED16(class)
+btOptimizedBvh : public btQuantizedBvh
 {
-	
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 protected:
-
 public:
-
 	btOptimizedBvh();
 
 	virtual ~btOptimizedBvh();
 
-	void	build(btStridingMeshInterface* triangles,bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax);
+	void build(btStridingMeshInterface * triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax);
 
-	void	refit(btStridingMeshInterface* triangles,const btVector3& aabbMin,const btVector3& aabbMax);
+	void refit(btStridingMeshInterface * triangles, const btVector3& aabbMin, const btVector3& aabbMax);
 
-	void	refitPartial(btStridingMeshInterface* triangles,const btVector3& aabbMin, const btVector3& aabbMax);
+	void refitPartial(btStridingMeshInterface * triangles, const btVector3& aabbMin, const btVector3& aabbMax);
 
-	void	updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index);
+	void updateBvhNodes(btStridingMeshInterface * meshInterface, int firstNode, int endNode, int index);
 
 	/// Data buffer MUST be 16 byte aligned
-	virtual bool serializeInPlace(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const
+	virtual bool serializeInPlace(void* o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const
 	{
-		return btQuantizedBvh::serialize(o_alignedDataBuffer,i_dataBufferSize,i_swapEndian);
-
+		return btQuantizedBvh::serialize(o_alignedDataBuffer, i_dataBufferSize, i_swapEndian);
 	}
 
 	///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
-	static btOptimizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian);
-
-
+	static btOptimizedBvh* deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian);
 };
 
-
-#endif //BT_OPTIMIZED_BVH_H
-
-
+#endif  //BT_OPTIMIZED_BVH_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
index b831e20c2f9..e4bd7bb4d56 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
@@ -12,9 +12,9 @@ subject to the following restrictions:
 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.
 */
-//#if defined (_WIN32) || defined (__i386__)
-//#define BT_USE_SSE_IN_API
-//#endif
+#if defined(_WIN32) || defined(__i386__)
+#define BT_USE_SSE_IN_API
+#endif
 
 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
 #include "btConvexPolyhedron.h"
@@ -23,11 +23,9 @@ subject to the following restrictions:
 #include "LinearMath/btGeometryUtil.h"
 #include "LinearMath/btGrahamScan2dConvexHull.h"
 
-
-btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(),
-m_polyhedron(0)
+btPolyhedralConvexShape::btPolyhedralConvexShape() : btConvexInternalShape(),
+													 m_polyhedron(0)
 {
-
 }
 
 btPolyhedralConvexShape::~btPolyhedralConvexShape()
@@ -39,79 +37,148 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape()
 	}
 }
 
-
-bool	btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin)
+void btPolyhedralConvexShape::setPolyhedralFeatures(btConvexPolyhedron& polyhedron)
 {
+	if (m_polyhedron)
+	{
+		*m_polyhedron = polyhedron;
+	}
+	else
+	{
+		void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron), 16);
+		m_polyhedron = new (mem) btConvexPolyhedron(polyhedron);
+	}
+}
 
+bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin)
+{
 	if (m_polyhedron)
 	{
 		m_polyhedron->~btConvexPolyhedron();
 		btAlignedFree(m_polyhedron);
 	}
-	
-	void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
+
+	void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron), 16);
 	m_polyhedron = new (mem) btConvexPolyhedron;
 
 	btAlignedObjectArray<btVector3> orgVertices;
 
-	for (int i=0;i<getNumVertices();i++)
+	for (int i = 0; i < getNumVertices(); i++)
 	{
 		btVector3& newVertex = orgVertices.expand();
-		getVertex(i,newVertex);
+		getVertex(i, newVertex);
 	}
-	
+
 	btConvexHullComputer conv;
-	
+
 	if (shiftVerticesByMargin)
 	{
 		btAlignedObjectArray<btVector3> planeEquations;
-		btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations);
+		btGeometryUtil::getPlaneEquationsFromVertices(orgVertices, planeEquations);
 
 		btAlignedObjectArray<btVector3> shiftedPlaneEquations;
-		for (int p=0;p<planeEquations.size();p++)
+		for (int p = 0; p < planeEquations.size(); p++)
 		{
-			   btVector3 plane = planeEquations[p];
-		//	   btScalar margin = getMargin();
-			   plane[3] -= getMargin();
-			   shiftedPlaneEquations.push_back(plane);
+			btVector3 plane = planeEquations[p];
+			//	   btScalar margin = getMargin();
+			plane[3] -= getMargin();
+			shiftedPlaneEquations.push_back(plane);
 		}
 
 		btAlignedObjectArray<btVector3> tmpVertices;
 
-		btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices);
-	
-		conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
-	} else
+		btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations, tmpVertices);
+
+		conv.compute(&tmpVertices[0].getX(), sizeof(btVector3), tmpVertices.size(), 0.f, 0.f);
+	}
+	else
 	{
-		
-		conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
+		conv.compute(&orgVertices[0].getX(), sizeof(btVector3), orgVertices.size(), 0.f, 0.f);
 	}
 
+#ifndef BT_RECONSTRUCT_FACES
 
+	int numVertices = conv.vertices.size();
+	m_polyhedron->m_vertices.resize(numVertices);
+	for (int p = 0; p < numVertices; p++)
+	{
+		m_polyhedron->m_vertices[p] = conv.vertices[p];
+	}
+
+	int v0, v1;
+	for (int j = 0; j < conv.faces.size(); j++)
+	{
+		btVector3 edges[3];
+		int numEdges = 0;
+		btFace combinedFace;
+		const btConvexHullComputer::Edge* edge = &conv.edges[conv.faces[j]];
+		v0 = edge->getSourceVertex();
+		int prevVertex = v0;
+		combinedFace.m_indices.push_back(v0);
+		v1 = edge->getTargetVertex();
+		while (v1 != v0)
+		{
+			btVector3 wa = conv.vertices[prevVertex];
+			btVector3 wb = conv.vertices[v1];
+			btVector3 newEdge = wb - wa;
+			newEdge.normalize();
+			if (numEdges < 2)
+				edges[numEdges++] = newEdge;
+
+			//face->addIndex(v1);
+			combinedFace.m_indices.push_back(v1);
+			edge = edge->getNextEdgeOfFace();
+			prevVertex = v1;
+			int v01 = edge->getSourceVertex();
+			v1 = edge->getTargetVertex();
+		}
+
+		btAssert(combinedFace.m_indices.size() > 2);
+
+		btVector3 faceNormal = edges[0].cross(edges[1]);
+		faceNormal.normalize();
+
+		btScalar planeEq = 1e30f;
+
+		for (int v = 0; v < combinedFace.m_indices.size(); v++)
+		{
+			btScalar eq = m_polyhedron->m_vertices[combinedFace.m_indices[v]].dot(faceNormal);
+			if (planeEq > eq)
+			{
+				planeEq = eq;
+			}
+		}
+		combinedFace.m_plane[0] = faceNormal.getX();
+		combinedFace.m_plane[1] = faceNormal.getY();
+		combinedFace.m_plane[2] = faceNormal.getZ();
+		combinedFace.m_plane[3] = -planeEq;
+
+		m_polyhedron->m_faces.push_back(combinedFace);
+	}
+
+#else  //BT_RECONSTRUCT_FACES
 
 	btAlignedObjectArray<btVector3> faceNormals;
 	int numFaces = conv.faces.size();
 	faceNormals.resize(numFaces);
 	btConvexHullComputer* convexUtil = &conv;
 
-	
-	btAlignedObjectArray<btFace>	tmpFaces;
+	btAlignedObjectArray<btFace> tmpFaces;
 	tmpFaces.resize(numFaces);
 
 	int numVertices = convexUtil->vertices.size();
 	m_polyhedron->m_vertices.resize(numVertices);
-	for (int p=0;p<numVertices;p++)
+	for (int p = 0; p < numVertices; p++)
 	{
 		m_polyhedron->m_vertices[p] = convexUtil->vertices[p];
 	}
 
-
-	for (int i=0;i<numFaces;i++)
+	for (int i = 0; i < numFaces; i++)
 	{
 		int face = convexUtil->faces[i];
 		//printf("face=%d\n",face);
-		const btConvexHullComputer::Edge*  firstEdge = &convexUtil->edges[face];
-		const btConvexHullComputer::Edge*  edge = firstEdge;
+		const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
+		const btConvexHullComputer::Edge* edge = firstEdge;
 
 		btVector3 edges[3];
 		int numEdges = 0;
@@ -119,25 +186,23 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
 
 		do
 		{
-			
 			int src = edge->getSourceVertex();
 			tmpFaces[i].m_indices.push_back(src);
 			int targ = edge->getTargetVertex();
 			btVector3 wa = convexUtil->vertices[src];
 
 			btVector3 wb = convexUtil->vertices[targ];
-			btVector3 newEdge = wb-wa;
+			btVector3 newEdge = wb - wa;
 			newEdge.normalize();
-			if (numEdges<2)
+			if (numEdges < 2)
 				edges[numEdges++] = newEdge;
 
 			edge = edge->getNextEdgeOfFace();
-		} while (edge!=firstEdge);
+		} while (edge != firstEdge);
 
 		btScalar planeEq = 1e30f;
 
-		
-		if (numEdges==2)
+		if (numEdges == 2)
 		{
 			faceNormals[i] = edges[0].cross(edges[1]);
 			faceNormals[i].normalize();
@@ -145,20 +210,19 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
 			tmpFaces[i].m_plane[1] = faceNormals[i].getY();
 			tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
 			tmpFaces[i].m_plane[3] = planeEq;
-
 		}
 		else
 		{
-			btAssert(0);//degenerate?
+			btAssert(0);  //degenerate?
 			faceNormals[i].setZero();
 		}
 
-		for (int v=0;v<tmpFaces[i].m_indices.size();v++)
+		for (int v = 0; v < tmpFaces[i].m_indices.size(); v++)
 		{
 			btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
-			if (planeEq>eq)
+			if (planeEq > eq)
 			{
-				planeEq=eq;
+				planeEq = eq;
 			}
 		}
 		tmpFaces[i].m_plane[3] = -planeEq;
@@ -166,89 +230,86 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
 
 	//merge coplanar faces and copy them to m_polyhedron
 
-	btScalar faceWeldThreshold= 0.999f;
+	btScalar faceWeldThreshold = 0.999f;
 	btAlignedObjectArray<int> todoFaces;
-	for (int i=0;i<tmpFaces.size();i++)
+	for (int i = 0; i < tmpFaces.size(); i++)
 		todoFaces.push_back(i);
 
 	while (todoFaces.size())
 	{
 		btAlignedObjectArray<int> coplanarFaceGroup;
-		int refFace = todoFaces[todoFaces.size()-1];
+		int refFace = todoFaces[todoFaces.size() - 1];
 
 		coplanarFaceGroup.push_back(refFace);
 		btFace& faceA = tmpFaces[refFace];
 		todoFaces.pop_back();
 
-		btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);
-		for (int j=todoFaces.size()-1;j>=0;j--)
+		btVector3 faceNormalA(faceA.m_plane[0], faceA.m_plane[1], faceA.m_plane[2]);
+		for (int j = todoFaces.size() - 1; j >= 0; j--)
 		{
 			int i = todoFaces[j];
 			btFace& faceB = tmpFaces[i];
-			btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]);
-			if (faceNormalA.dot(faceNormalB)>faceWeldThreshold)
+			btVector3 faceNormalB(faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2]);
+			if (faceNormalA.dot(faceNormalB) > faceWeldThreshold)
 			{
 				coplanarFaceGroup.push_back(i);
 				todoFaces.remove(i);
 			}
 		}
 
-
 		bool did_merge = false;
-		if (coplanarFaceGroup.size()>1)
+		if (coplanarFaceGroup.size() > 1)
 		{
 			//do the merge: use Graham Scan 2d convex hull
 
 			btAlignedObjectArray<GrahamVector3> orgpoints;
-			btVector3 averageFaceNormal(0,0,0);
+			btVector3 averageFaceNormal(0, 0, 0);
 
-			for (int i=0;i<coplanarFaceGroup.size();i++)
+			for (int i = 0; i < coplanarFaceGroup.size(); i++)
 			{
-//				m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
+				//				m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
 
 				btFace& face = tmpFaces[coplanarFaceGroup[i]];
-				btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
-				averageFaceNormal+=faceNormal;
-				for (int f=0;f<face.m_indices.size();f++)
+				btVector3 faceNormal(face.m_plane[0], face.m_plane[1], face.m_plane[2]);
+				averageFaceNormal += faceNormal;
+				for (int f = 0; f < face.m_indices.size(); f++)
 				{
 					int orgIndex = face.m_indices[f];
 					btVector3 pt = m_polyhedron->m_vertices[orgIndex];
-					
+
 					bool found = false;
 
-					for (int i=0;i<orgpoints.size();i++)
+					for (int i = 0; i < orgpoints.size(); i++)
 					{
 						//if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
 						if (orgpoints[i].m_orgIndex == orgIndex)
 						{
-							found=true;
+							found = true;
 							break;
 						}
 					}
 					if (!found)
-						orgpoints.push_back(GrahamVector3(pt,orgIndex));
+						orgpoints.push_back(GrahamVector3(pt, orgIndex));
 				}
 			}
 
-			
-
 			btFace combinedFace;
-			for (int i=0;i<4;i++)
+			for (int i = 0; i < 4; i++)
 				combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
 
 			btAlignedObjectArray<GrahamVector3> hull;
 
 			averageFaceNormal.normalize();
-			GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal);
+			GrahamScanConvexHull2D(orgpoints, hull, averageFaceNormal);
 
-			for (int i=0;i<hull.size();i++)
+			for (int i = 0; i < hull.size(); i++)
 			{
 				combinedFace.m_indices.push_back(hull[i].m_orgIndex);
-				for(int k = 0; k < orgpoints.size(); k++) 
+				for (int k = 0; k < orgpoints.size(); k++)
 				{
-					if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex) 
+					if (orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
 					{
-						orgpoints[k].m_orgIndex = -1; // invalidate...
+						orgpoints[k].m_orgIndex = -1;  // invalidate...
 						break;
 					}
 				}
@@ -256,38 +317,41 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
 
 			// are there rejected vertices?
 			bool reject_merge = false;
-			
-
 
-			for(int i = 0; i < orgpoints.size(); i++) {
-				if(orgpoints[i].m_orgIndex == -1)
-					continue; // this is in the hull...
+			for (int i = 0; i < orgpoints.size(); i++)
+			{
+				if (orgpoints[i].m_orgIndex == -1)
+					continue;  // this is in the hull...
 				// this vertex is rejected -- is anybody else using this vertex?
-				for(int j = 0; j < tmpFaces.size(); j++) {
-					
+				for (int j = 0; j < tmpFaces.size(); j++)
+				{
 					btFace& face = tmpFaces[j];
 					// is this a face of the current coplanar group?
 					bool is_in_current_group = false;
-					for(int k = 0; k < coplanarFaceGroup.size(); k++) {
-						if(coplanarFaceGroup[k] == j) {
+					for (int k = 0; k < coplanarFaceGroup.size(); k++)
+					{
+						if (coplanarFaceGroup[k] == j)
+						{
 							is_in_current_group = true;
 							break;
 						}
 					}
-					if(is_in_current_group) // ignore this face...
+					if (is_in_current_group)  // ignore this face...
 						continue;
 					// does this face use this rejected vertex?
-					for(int v = 0; v < face.m_indices.size(); v++) {
-						if(face.m_indices[v] == orgpoints[i].m_orgIndex) {
+					for (int v = 0; v < face.m_indices.size(); v++)
+					{
+						if (face.m_indices[v] == orgpoints[i].m_orgIndex)
+						{
 							// this rejected vertex is used in another face -- reject merge
 							reject_merge = true;
 							break;
 						}
 					}
-					if(reject_merge)
+					if (reject_merge)
 						break;
 				}
-				if(reject_merge)
+				if (reject_merge)
 					break;
 			}
 
@@ -298,34 +362,30 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
 				m_polyhedron->m_faces.push_back(combinedFace);
 			}
 		}
-		if(!did_merge)
+		if (!did_merge)
 		{
-			for (int i=0;i<coplanarFaceGroup.size();i++)
+			for (int i = 0; i < coplanarFaceGroup.size(); i++)
 			{
 				btFace face = tmpFaces[coplanarFaceGroup[i]];
 				m_polyhedron->m_faces.push_back(face);
 			}
+		}
+	}
 
-		} 
-
-
+#endif  //BT_RECONSTRUCT_FACES
 
-	}
-	
 	m_polyhedron->initialize();
 
 	return true;
 }
 
 #ifndef MIN
-    #define MIN(_a, _b)     ((_a) < (_b) ? (_a) : (_b))
+#define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b))
 #endif
 
-btVector3	btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const
 {
-
-
-	btVector3 supVec(0,0,0);
+	btVector3 supVec(0, 0, 0);
 #ifndef __SPU__
 	int i;
 	btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
@@ -334,37 +394,36 @@ btVector3	btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const b
 	btScalar lenSqr = vec.length2();
 	if (lenSqr < btScalar(0.0001))
 	{
-		vec.setValue(1,0,0);
-	} else
+		vec.setValue(1, 0, 0);
+	}
+	else
 	{
-		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+		btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
 		vec *= rlen;
 	}
 
 	btVector3 vtx;
 	btScalar newDot;
 
-    for( int k = 0; k < getNumVertices(); k += 128 )
-    {
-        btVector3 temp[128];
-        int inner_count = MIN(getNumVertices() - k, 128);
-        for( i = 0; i < inner_count; i++ )
-            getVertex(i,temp[i]); 
-        i = (int) vec.maxDot( temp, inner_count, newDot);
+	for (int k = 0; k < getNumVertices(); k += 128)
+	{
+		btVector3 temp[128];
+		int inner_count = MIN(getNumVertices() - k, 128);
+		for (i = 0; i < inner_count; i++)
+			getVertex(i, temp[i]);
+		i = (int)vec.maxDot(temp, inner_count, newDot);
 		if (newDot > maxDot)
 		{
 			maxDot = newDot;
 			supVec = temp[i];
-		}        
-    }
-	
-#endif //__SPU__
+		}
+	}
+
+#endif  //__SPU__
 	return supVec;
 }
 
-
-
-void	btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 {
 #ifndef __SPU__
 	int i;
@@ -372,36 +431,34 @@ void	btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(
 	btVector3 vtx;
 	btScalar newDot;
 
-	for (i=0;i<numVectors;i++)
+	for (i = 0; i < numVectors; i++)
 	{
 		supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
 	}
 
-	for (int j=0;j<numVectors;j++)
+	for (int j = 0; j < numVectors; j++)
 	{
-        const btVector3& vec = vectors[j];
-        
-        for( int k = 0; k < getNumVertices(); k += 128 )
-        {
-            btVector3 temp[128];
-            int inner_count = MIN(getNumVertices() - k, 128);
-            for( i = 0; i < inner_count; i++ )
-                getVertex(i,temp[i]); 
-            i = (int) vec.maxDot( temp, inner_count, newDot);
-            if (newDot > supportVerticesOut[j][3])
-            {
+		const btVector3& vec = vectors[j];
+
+		for (int k = 0; k < getNumVertices(); k += 128)
+		{
+			btVector3 temp[128];
+			int inner_count = MIN(getNumVertices() - k, 128);
+			for (i = 0; i < inner_count; i++)
+				getVertex(i, temp[i]);
+			i = (int)vec.maxDot(temp, inner_count, newDot);
+			if (newDot > supportVerticesOut[j][3])
+			{
 				supportVerticesOut[j] = temp[i];
 				supportVerticesOut[j][3] = newDot;
-            }        
-        }
-    }
+			}
+		}
+	}
 
-#endif //__SPU__
+#endif  //__SPU__
 }
 
-
-
-void	btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
 #ifndef __SPU__
 	//not yet, return box inertia
@@ -410,81 +467,77 @@ void	btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& ine
 
 	btTransform ident;
 	ident.setIdentity();
-	btVector3 aabbMin,aabbMax;
-	getAabb(ident,aabbMin,aabbMax); // This already contains the margin
-	btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
-
-	btScalar lx=btScalar(2.)*(halfExtents.x());
-	btScalar ly=btScalar(2.)*(halfExtents.y());
-	btScalar lz=btScalar(2.)*(halfExtents.z());
-	const btScalar x2 = lx*lx;
-	const btScalar y2 = ly*ly;
-	const btScalar z2 = lz*lz;
+	btVector3 aabbMin, aabbMax;
+	getAabb(ident, aabbMin, aabbMax);  // This already contains the margin
+	btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
+
+	btScalar lx = btScalar(2.) * (halfExtents.x());
+	btScalar ly = btScalar(2.) * (halfExtents.y());
+	btScalar lz = btScalar(2.) * (halfExtents.z());
+	const btScalar x2 = lx * lx;
+	const btScalar y2 = ly * ly;
+	const btScalar z2 = lz * lz;
 	const btScalar scaledmass = mass * btScalar(0.08333333);
 
-	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
-#endif //__SPU__
+	inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2));
+#endif  //__SPU__
 }
 
-
-
-void	btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling)
+void btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling)
 {
 	btConvexInternalShape::setLocalScaling(scaling);
 	recalcLocalAabb();
 }
 
 btPolyhedralConvexAabbCachingShape::btPolyhedralConvexAabbCachingShape()
-:btPolyhedralConvexShape(),
-m_localAabbMin(1,1,1),
-m_localAabbMax(-1,-1,-1),
-m_isLocalAabbValid(false)
+	: btPolyhedralConvexShape(),
+	  m_localAabbMin(1, 1, 1),
+	  m_localAabbMax(-1, -1, -1),
+	  m_isLocalAabbValid(false)
 {
 }
 
-void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const
 {
-	getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin());
+	getNonvirtualAabb(trans, aabbMin, aabbMax, getMargin());
 }
 
-void	btPolyhedralConvexAabbCachingShape::recalcLocalAabb()
+void btPolyhedralConvexAabbCachingShape::recalcLocalAabb()
 {
 	m_isLocalAabbValid = true;
-	
-	#if 1
+
+#if 1
 	static const btVector3 _directions[] =
-	{
-		btVector3( 1.,  0.,  0.),
-		btVector3( 0.,  1.,  0.),
-		btVector3( 0.,  0.,  1.),
-		btVector3( -1., 0.,  0.),
-		btVector3( 0., -1.,  0.),
-		btVector3( 0.,  0., -1.)
-	};
-	
+		{
+			btVector3(1., 0., 0.),
+			btVector3(0., 1., 0.),
+			btVector3(0., 0., 1.),
+			btVector3(-1., 0., 0.),
+			btVector3(0., -1., 0.),
+			btVector3(0., 0., -1.)};
+
 	btVector3 _supporting[] =
-	{
-		btVector3( 0., 0., 0.),
-		btVector3( 0., 0., 0.),
-		btVector3( 0., 0., 0.),
-		btVector3( 0., 0., 0.),
-		btVector3( 0., 0., 0.),
-		btVector3( 0., 0., 0.)
-	};
-	
+		{
+			btVector3(0., 0., 0.),
+			btVector3(0., 0., 0.),
+			btVector3(0., 0., 0.),
+			btVector3(0., 0., 0.),
+			btVector3(0., 0., 0.),
+			btVector3(0., 0., 0.)};
+
 	batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
-	
-	for ( int i = 0; i < 3; ++i )
+
+	for (int i = 0; i < 3; ++i)
 	{
 		m_localAabbMax[i] = _supporting[i][i];
 		m_localAabbMin[i] = _supporting[i + 3][i];
 	}
 
-	#else
+#else
 
-	for (int i=0;i<3;i++)
+	for (int i = 0; i < 3; i++)
 	{
-		btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
+		btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.));
 		vec[i] = btScalar(1.);
 		btVector3 tmp = localGetSupportingVertex(vec);
 		m_localAabbMax[i] = tmp[i];
@@ -492,9 +545,5 @@ void	btPolyhedralConvexAabbCachingShape::recalcLocalAabb()
 		tmp = localGetSupportingVertex(vec);
 		m_localAabbMin[i] = tmp[i];
 	}
-	#endif
+#endif
 }
-
-
-
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
index 961d001a9dd..b3ffab7a23c 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
@@ -20,20 +20,15 @@ subject to the following restrictions:
 #include "btConvexInternalShape.h"
 class btConvexPolyhedron;
 
-
 ///The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes.
-ATTRIBUTE_ALIGNED16(class) btPolyhedralConvexShape : public btConvexInternalShape
+ATTRIBUTE_ALIGNED16(class)
+btPolyhedralConvexShape : public btConvexInternalShape
 {
-	
-
 protected:
-	
 	btConvexPolyhedron* m_polyhedron;
 
 public:
-	
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
 
 	btPolyhedralConvexShape();
 
@@ -41,76 +36,71 @@ public:
 
 	///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges)
 	///experimental/work-in-progress
-	virtual bool	initializePolyhedralFeatures(int shiftVerticesByMargin=0);
+	virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin = 0);
 
-	const btConvexPolyhedron*	getConvexPolyhedron() const
+	virtual void setPolyhedralFeatures(btConvexPolyhedron & polyhedron);
+
+	const btConvexPolyhedron* getConvexPolyhedron() const
 	{
 		return m_polyhedron;
 	}
 
 	//brute force implementations
 
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
-	
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
-	
-	
-	virtual int	getNumVertices() const = 0 ;
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
+
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
+
+	virtual int getNumVertices() const = 0;
 	virtual int getNumEdges() const = 0;
-	virtual void getEdge(int i,btVector3& pa,btVector3& pb) const = 0;
-	virtual void getVertex(int i,btVector3& vtx) const = 0;
-	virtual int	getNumPlanes() const = 0;
-	virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const = 0;
-//	virtual int getIndex(int i) const = 0 ; 
-
-	virtual	bool isInside(const btVector3& pt,btScalar tolerance) const = 0;
-	
-};
+	virtual void getEdge(int i, btVector3& pa, btVector3& pb) const = 0;
+	virtual void getVertex(int i, btVector3& vtx) const = 0;
+	virtual int getNumPlanes() const = 0;
+	virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const = 0;
+	//	virtual int getIndex(int i) const = 0 ;
 
+	virtual bool isInside(const btVector3& pt, btScalar tolerance) const = 0;
+};
 
 ///The btPolyhedralConvexAabbCachingShape adds aabb caching to the btPolyhedralConvexShape
 class btPolyhedralConvexAabbCachingShape : public btPolyhedralConvexShape
 {
+	btVector3 m_localAabbMin;
+	btVector3 m_localAabbMax;
+	bool m_isLocalAabbValid;
 
-	btVector3	m_localAabbMin;
-	btVector3	m_localAabbMax;
-	bool		m_isLocalAabbValid;
-		
 protected:
-
-	void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax)
+	void setCachedLocalAabb(const btVector3& aabbMin, const btVector3& aabbMax)
 	{
 		m_isLocalAabbValid = true;
 		m_localAabbMin = aabbMin;
 		m_localAabbMax = aabbMax;
 	}
 
-	inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const
+	inline void getCachedLocalAabb(btVector3& aabbMin, btVector3& aabbMax) const
 	{
 		btAssert(m_isLocalAabbValid);
 		aabbMin = m_localAabbMin;
 		aabbMax = m_localAabbMax;
 	}
 
-public:
-
+protected:
 	btPolyhedralConvexAabbCachingShape();
-	
-	inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const
-	{
 
+public:
+	inline void getNonvirtualAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax, btScalar margin) const
+	{
 		//lazy evaluation of local aabb
 		btAssert(m_isLocalAabbValid);
-		btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax);
+		btTransformAabb(m_localAabbMin, m_localAabbMax, margin, trans, aabbMin, aabbMax);
 	}
 
-	virtual void	setLocalScaling(const btVector3& scaling);
-
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+	virtual void setLocalScaling(const btVector3& scaling);
 
-	void	recalcLocalAabb();
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
+	void recalcLocalAabb();
 };
 
-#endif //BT_POLYHEDRAL_CONVEX_SHAPE_H
+#endif  //BT_POLYHEDRAL_CONVEX_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp
index 6a337c786c4..f4273199749 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp
@@ -13,11 +13,10 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btScaledBvhTriangleMeshShape.h"
 
-btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling)
-:m_localScaling(localScaling),m_bvhTriMeshShape(childShape)
+btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape, const btVector3& localScaling)
+	: m_localScaling(localScaling), m_bvhTriMeshShape(childShape)
 {
 	m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE;
 }
@@ -26,55 +25,51 @@ btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape()
 {
 }
 
-
 class btScaledTriangleCallback : public btTriangleCallback
 {
 	btTriangleCallback* m_originalCallback;
 
-	btVector3	m_localScaling;
+	btVector3 m_localScaling;
 
 public:
-
-	btScaledTriangleCallback(btTriangleCallback* originalCallback,const btVector3& localScaling)
-		:m_originalCallback(originalCallback),
-		m_localScaling(localScaling)
+	btScaledTriangleCallback(btTriangleCallback* originalCallback, const btVector3& localScaling)
+		: m_originalCallback(originalCallback),
+		  m_localScaling(localScaling)
 	{
 	}
 
 	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 	{
 		btVector3 newTriangle[3];
-		newTriangle[0] = triangle[0]*m_localScaling;
-		newTriangle[1] = triangle[1]*m_localScaling;
-		newTriangle[2] = triangle[2]*m_localScaling;
-		m_originalCallback->processTriangle(&newTriangle[0],partId,triangleIndex);
+		newTriangle[0] = triangle[0] * m_localScaling;
+		newTriangle[1] = triangle[1] * m_localScaling;
+		newTriangle[2] = triangle[2] * m_localScaling;
+		m_originalCallback->processTriangle(&newTriangle[0], partId, triangleIndex);
 	}
 };
 
-void	btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+void btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
 {
-	btScaledTriangleCallback scaledCallback(callback,m_localScaling);
-	
-	btVector3 invLocalScaling(1.f/m_localScaling.getX(),1.f/m_localScaling.getY(),1.f/m_localScaling.getZ());
-	btVector3 scaledAabbMin,scaledAabbMax;
+	btScaledTriangleCallback scaledCallback(callback, m_localScaling);
+
+	btVector3 invLocalScaling(1.f / m_localScaling.getX(), 1.f / m_localScaling.getY(), 1.f / m_localScaling.getZ());
+	btVector3 scaledAabbMin, scaledAabbMax;
 
 	///support negative scaling
 	scaledAabbMin[0] = m_localScaling.getX() >= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0];
 	scaledAabbMin[1] = m_localScaling.getY() >= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1];
 	scaledAabbMin[2] = m_localScaling.getZ() >= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2];
 	scaledAabbMin[3] = 0.f;
-	
+
 	scaledAabbMax[0] = m_localScaling.getX() <= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0];
 	scaledAabbMax[1] = m_localScaling.getY() <= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1];
 	scaledAabbMax[2] = m_localScaling.getZ() <= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2];
 	scaledAabbMax[3] = 0.f;
-	
-	
-	m_bvhTriMeshShape->processAllTriangles(&scaledCallback,scaledAabbMin,scaledAabbMax);
-}
 
+	m_bvhTriMeshShape->processAllTriangles(&scaledCallback, scaledAabbMin, scaledAabbMax);
+}
 
-void	btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const
 {
 	btVector3 localAabbMin = m_bvhTriMeshShape->getLocalAabbMin();
 	btVector3 localAabbMax = m_bvhTriMeshShape->getLocalAabbMax();
@@ -89,22 +84,21 @@ void	btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& a
 	localAabbMax[1] = (m_localScaling.getY() <= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1];
 	localAabbMax[2] = (m_localScaling.getZ() <= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2];
 
-	btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
+	btVector3 localHalfExtents = btScalar(0.5) * (localAabbMax - localAabbMin);
 	btScalar margin = m_bvhTriMeshShape->getMargin();
-	localHalfExtents += btVector3(margin,margin,margin);
-	btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
-	
-	btMatrix3x3 abs_b = trans.getBasis().absolute();  
+	localHalfExtents += btVector3(margin, margin, margin);
+	btVector3 localCenter = btScalar(0.5) * (localAabbMax + localAabbMin);
+
+	btMatrix3x3 abs_b = trans.getBasis().absolute();
 
 	btVector3 center = trans(localCenter);
 
-    btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
+	btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
 	aabbMin = center - extent;
 	aabbMax = center + extent;
-
 }
 
-void	btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling)
+void btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling)
 {
 	m_localScaling = scaling;
 }
@@ -114,8 +108,8 @@ const btVector3& btScaledBvhTriangleMeshShape::getLocalScaling() const
 	return m_localScaling;
 }
 
-void	btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
 	///don't make this a movable object!
-//	btAssert(0);
+	//	btAssert(0);
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h
index 39049eaf083..4d6feb61af5 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h
@@ -18,78 +18,69 @@ subject to the following restrictions:
 
 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
 
-
 ///The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMeshShape.
 ///Note that each btBvhTriangleMeshShape still can have its own local scaling, independent from this btScaledBvhTriangleMeshShape 'localScaling'
-ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape
+ATTRIBUTE_ALIGNED16(class)
+btScaledBvhTriangleMeshShape : public btConcaveShape
 {
-	
-	
-	btVector3	m_localScaling;
+	btVector3 m_localScaling;
 
-	btBvhTriangleMeshShape*	m_bvhTriMeshShape;
+	btBvhTriangleMeshShape* m_bvhTriMeshShape;
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-
-	btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling);
+	btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape * childShape, const btVector3& localScaling);
 
 	virtual ~btScaledBvhTriangleMeshShape();
 
-
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
-	virtual void	setLocalScaling(const btVector3& scaling);
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
+	virtual void setLocalScaling(const btVector3& scaling);
 	virtual const btVector3& getLocalScaling() const;
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
 
-	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+	virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const;
 
-	btBvhTriangleMeshShape*	getChildShape()
+	btBvhTriangleMeshShape* getChildShape()
 	{
 		return m_bvhTriMeshShape;
 	}
 
-	const btBvhTriangleMeshShape*	getChildShape() const
+	const btBvhTriangleMeshShape* getChildShape() const
 	{
 		return m_bvhTriMeshShape;
 	}
 
 	//debugging
-	virtual const char*	getName()const {return "SCALEDBVHTRIANGLEMESH";}
+	virtual const char* getName() const { return "SCALEDBVHTRIANGLEMESH"; }
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btScaledTriangleMeshShapeData
+struct btScaledTriangleMeshShapeData
 {
-	btTriangleMeshShapeData	m_trimeshShapeData;
+	btTriangleMeshShapeData m_trimeshShapeData;
 
-	btVector3FloatData	m_localScaling;
+	btVector3FloatData m_localScaling;
 };
 
-
-SIMD_FORCE_INLINE	int	btScaledBvhTriangleMeshShape::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btScaledBvhTriangleMeshShape::calculateSerializeBufferSize() const
 {
 	return sizeof(btScaledTriangleMeshShapeData);
 }
 
-
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btScaledBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const
+SIMD_FORCE_INLINE const char* btScaledBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btScaledTriangleMeshShapeData* scaledMeshData = (btScaledTriangleMeshShapeData*) dataBuffer;
-	m_bvhTriMeshShape->serialize(&scaledMeshData->m_trimeshShapeData,serializer);
+	btScaledTriangleMeshShapeData* scaledMeshData = (btScaledTriangleMeshShapeData*)dataBuffer;
+	m_bvhTriMeshShape->serialize(&scaledMeshData->m_trimeshShapeData, serializer);
 	scaledMeshData->m_trimeshShapeData.m_collisionShapeData.m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE;
 	m_localScaling.serializeFloat(scaledMeshData->m_localScaling);
 	return "btScaledTriangleMeshShapeData";
 }
 
-
-#endif //BT_SCALED_BVH_TRIANGLE_MESH_SHAPE_H
+#endif  //BT_SCALED_BVH_TRIANGLE_MESH_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp
new file mode 100644
index 00000000000..4a95dbea4f2
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp
@@ -0,0 +1,92 @@
+#include "btSdfCollisionShape.h"
+#include "btMiniSDF.h"
+#include "LinearMath/btAabbUtil2.h"
+
+struct btSdfCollisionShapeInternalData
+{
+	btVector3 m_localScaling;
+	btScalar m_margin;
+	btMiniSDF m_sdf;
+
+	btSdfCollisionShapeInternalData()
+		: m_localScaling(1, 1, 1),
+		  m_margin(0)
+	{
+	}
+};
+
+bool btSdfCollisionShape::initializeSDF(const char* sdfData, int sizeInBytes)
+{
+	bool valid = m_data->m_sdf.load(sdfData, sizeInBytes);
+	return valid;
+}
+btSdfCollisionShape::btSdfCollisionShape()
+{
+	m_shapeType = SDF_SHAPE_PROXYTYPE;
+	m_data = new btSdfCollisionShapeInternalData();
+
+	//"E:/develop/bullet3/data/toys/ground_hole64_64_8.cdf");//ground_cube.cdf");
+	/*unsigned int field_id=0;
+	Eigen::Vector3d x (1,10,1);
+	Eigen::Vector3d gradient;
+	double dist = m_data->m_sdf.interpolate(field_id, x, &gradient);
+	printf("dist=%g\n", dist);
+	*/
+}
+btSdfCollisionShape::~btSdfCollisionShape()
+{
+	delete m_data;
+}
+
+void btSdfCollisionShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
+{
+	btAssert(m_data->m_sdf.isValid());
+	btVector3 localAabbMin = m_data->m_sdf.m_domain.m_min;
+	btVector3 localAabbMax = m_data->m_sdf.m_domain.m_max;
+	btScalar margin(0);
+	btTransformAabb(localAabbMin, localAabbMax, margin, t, aabbMin, aabbMax);
+}
+
+void btSdfCollisionShape::setLocalScaling(const btVector3& scaling)
+{
+	m_data->m_localScaling = scaling;
+}
+const btVector3& btSdfCollisionShape::getLocalScaling() const
+{
+	return m_data->m_localScaling;
+}
+void btSdfCollisionShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
+{
+	inertia.setValue(0, 0, 0);
+}
+const char* btSdfCollisionShape::getName() const
+{
+	return "btSdfCollisionShape";
+}
+void btSdfCollisionShape::setMargin(btScalar margin)
+{
+	m_data->m_margin = margin;
+}
+btScalar btSdfCollisionShape::getMargin() const
+{
+	return m_data->m_margin;
+}
+
+void btSdfCollisionShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
+{
+	//not yet
+}
+
+bool btSdfCollisionShape::queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal)
+{
+	int field = 0;
+	btVector3 grad;
+	double dist;
+	bool hasResult = m_data->m_sdf.interpolate(field, dist, ptInSDF, &grad);
+	if (hasResult)
+	{
+		normal.setValue(grad[0], grad[1], grad[2]);
+		distOut = dist;
+	}
+	return hasResult;
+}
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h
new file mode 100644
index 00000000000..3989d6245ed
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h
@@ -0,0 +1,29 @@
+#ifndef BT_SDF_COLLISION_SHAPE_H
+#define BT_SDF_COLLISION_SHAPE_H
+
+#include "btConcaveShape.h"
+
+class btSdfCollisionShape : public btConcaveShape
+{
+	struct btSdfCollisionShapeInternalData* m_data;
+
+public:
+	btSdfCollisionShape();
+	virtual ~btSdfCollisionShape();
+
+	bool initializeSDF(const char* sdfData, int sizeInBytes);
+
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
+	virtual void setLocalScaling(const btVector3& scaling);
+	virtual const btVector3& getLocalScaling() const;
+	virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const;
+	virtual const char* getName() const;
+	virtual void setMargin(btScalar margin);
+	virtual btScalar getMargin() const;
+
+	virtual void processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const;
+
+	bool queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal);
+};
+
+#endif  //BT_SDF_COLLISION_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp
index 3beaf865801..a2c490faf9d 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp
@@ -15,51 +15,48 @@ subject to the following restrictions:
 
 //btShapeHull was implemented by John McCutchan.
 
-
 #include "btShapeHull.h"
 #include "LinearMath/btConvexHull.h"
 
 #define NUM_UNITSPHERE_POINTS 42
+#define NUM_UNITSPHERE_POINTS_HIGHRES 256
 
-btShapeHull::btShapeHull (const btConvexShape* shape)
+btShapeHull::btShapeHull(const btConvexShape* shape)
 {
 	m_shape = shape;
-	m_vertices.clear ();
+	m_vertices.clear();
 	m_indices.clear();
 	m_numIndices = 0;
 }
 
-btShapeHull::~btShapeHull ()
+btShapeHull::~btShapeHull()
 {
-	m_indices.clear();	
-	m_vertices.clear ();
+	m_indices.clear();
+	m_vertices.clear();
 }
 
-bool
-btShapeHull::buildHull (btScalar /*margin*/)
+bool btShapeHull::buildHull(btScalar /*margin*/, int highres)
 {
-	int numSampleDirections = NUM_UNITSPHERE_POINTS;
-	{
-		int numPDA = m_shape->getNumPreferredPenetrationDirections();
-		if (numPDA)
-		{
-			for (int i=0;i<numPDA;i++)
-			{
-				btVector3 norm;
-				m_shape->getPreferredPenetrationDirection(i,norm);
-				getUnitSpherePoints()[numSampleDirections] = norm;
-				numSampleDirections++;
-			}
-		}
-	}
-
-	btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	
+	int numSampleDirections = highres ? NUM_UNITSPHERE_POINTS_HIGHRES : NUM_UNITSPHERE_POINTS;
+	btVector3 supportPoints[NUM_UNITSPHERE_POINTS_HIGHRES + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
 	int i;
 	for (i = 0; i < numSampleDirections; i++)
 	{
-		supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints()[i]);
+		supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints(highres)[i]);
 	}
 
+	int numPDA = m_shape->getNumPreferredPenetrationDirections();
+	if (numPDA)
+	{
+		for (int s = 0; s < numPDA; s++)
+		{
+			btVector3 norm;
+			m_shape->getPreferredPenetrationDirection(s, norm);
+			supportPoints[i++] = m_shape->localGetSupportingVertex(norm);
+			numSampleDirections++;
+		}
+	}
 	HullDesc hd;
 	hd.mFlags = QF_TRIANGLES;
 	hd.mVcount = static_cast<unsigned int>(numSampleDirections);
@@ -69,18 +66,17 @@ btShapeHull::buildHull (btScalar /*margin*/)
 	hd.mVertexStride = sizeof(btVector3);
 #else
 	hd.mVertices = &supportPoints[0];
-	hd.mVertexStride = sizeof (btVector3);
+	hd.mVertexStride = sizeof(btVector3);
 #endif
 
 	HullLibrary hl;
 	HullResult hr;
-	if (hl.CreateConvexHull (hd, hr) == QE_FAIL)
+	if (hl.CreateConvexHull(hd, hr) == QE_FAIL)
 	{
 		return false;
 	}
 
-	m_vertices.resize (static_cast<int>(hr.mNumOutputVertices));
-
+	m_vertices.resize(static_cast<int>(hr.mNumOutputVertices));
 
 	for (i = 0; i < static_cast<int>(hr.mNumOutputVertices); i++)
 	{
@@ -94,77 +90,332 @@ btShapeHull::buildHull (btScalar /*margin*/)
 	}
 
 	// free temporary hull result that we just copied
-	hl.ReleaseResult (hr);
+	hl.ReleaseResult(hr);
 
 	return true;
 }
 
-int
-btShapeHull::numTriangles () const
+int btShapeHull::numTriangles() const
 {
 	return static_cast<int>(m_numIndices / 3);
 }
 
-int
-btShapeHull::numVertices () const
+int btShapeHull::numVertices() const
 {
-	return m_vertices.size ();
+	return m_vertices.size();
 }
 
-int
-btShapeHull::numIndices () const
+int btShapeHull::numIndices() const
 {
 	return static_cast<int>(m_numIndices);
 }
 
-
-btVector3* btShapeHull::getUnitSpherePoints()
+btVector3* btShapeHull::getUnitSpherePoints(int highres)
 {
-	static btVector3 sUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
-	{
-		btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
-		btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
-		btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
-		btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
-		btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
-		btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
-		btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
-		btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
-		btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
-		btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
-		btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
-		btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
-		btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
-		btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
-		btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
-		btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
-		btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
-		btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
-		btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
-		btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
-		btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
-		btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
-		btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
-		btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
-		btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
-		btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
-		btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
-		btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
-		btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
-		btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
-		btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
-		btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
-		btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
-		btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
-		btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
-		btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
-		btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
-		btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
-		btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
-		btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
-		btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
-		btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
-	};
+	static btVector3 sUnitSpherePointsHighres[NUM_UNITSPHERE_POINTS_HIGHRES + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] =
+		{
+			btVector3(btScalar(0.997604), btScalar(0.067004), btScalar(0.017144)),
+			btVector3(btScalar(0.984139), btScalar(-0.086784), btScalar(-0.154427)),
+			btVector3(btScalar(0.971065), btScalar(0.124164), btScalar(-0.203224)),
+			btVector3(btScalar(0.955844), btScalar(0.291173), btScalar(-0.037704)),
+			btVector3(btScalar(0.957405), btScalar(0.212238), btScalar(0.195157)),
+			btVector3(btScalar(0.971650), btScalar(-0.012709), btScalar(0.235561)),
+			btVector3(btScalar(0.984920), btScalar(-0.161831), btScalar(0.059695)),
+			btVector3(btScalar(0.946673), btScalar(-0.299288), btScalar(-0.117536)),
+			btVector3(btScalar(0.922670), btScalar(-0.219186), btScalar(-0.317019)),
+			btVector3(btScalar(0.928134), btScalar(-0.007265), btScalar(-0.371867)),
+			btVector3(btScalar(0.875642), btScalar(0.198434), btScalar(-0.439988)),
+			btVector3(btScalar(0.908035), btScalar(0.325975), btScalar(-0.262562)),
+			btVector3(btScalar(0.864519), btScalar(0.488706), btScalar(-0.116755)),
+			btVector3(btScalar(0.893009), btScalar(0.428046), btScalar(0.137185)),
+			btVector3(btScalar(0.857494), btScalar(0.362137), btScalar(0.364776)),
+			btVector3(btScalar(0.900815), btScalar(0.132524), btScalar(0.412987)),
+			btVector3(btScalar(0.934964), btScalar(-0.241739), btScalar(0.259179)),
+			btVector3(btScalar(0.894570), btScalar(-0.103504), btScalar(0.434263)),
+			btVector3(btScalar(0.922085), btScalar(-0.376668), btScalar(0.086241)),
+			btVector3(btScalar(0.862177), btScalar(-0.499154), btScalar(-0.085330)),
+			btVector3(btScalar(0.861982), btScalar(-0.420218), btScalar(-0.282861)),
+			btVector3(btScalar(0.818076), btScalar(-0.328256), btScalar(-0.471804)),
+			btVector3(btScalar(0.762657), btScalar(-0.179329), btScalar(-0.621124)),
+			btVector3(btScalar(0.826857), btScalar(0.019760), btScalar(-0.561786)),
+			btVector3(btScalar(0.731434), btScalar(0.206599), btScalar(-0.649817)),
+			btVector3(btScalar(0.769486), btScalar(0.379052), btScalar(-0.513770)),
+			btVector3(btScalar(0.796806), btScalar(0.507176), btScalar(-0.328145)),
+			btVector3(btScalar(0.679722), btScalar(0.684101), btScalar(-0.264123)),
+			btVector3(btScalar(0.786854), btScalar(0.614886), btScalar(0.050912)),
+			btVector3(btScalar(0.769486), btScalar(0.571141), btScalar(0.285139)),
+			btVector3(btScalar(0.707432), btScalar(0.492789), btScalar(0.506288)),
+			btVector3(btScalar(0.774560), btScalar(0.268037), btScalar(0.572652)),
+			btVector3(btScalar(0.796220), btScalar(0.031230), btScalar(0.604077)),
+			btVector3(btScalar(0.837395), btScalar(-0.320285), btScalar(0.442461)),
+			btVector3(btScalar(0.848127), btScalar(-0.450548), btScalar(0.278307)),
+			btVector3(btScalar(0.775536), btScalar(-0.206354), btScalar(0.596465)),
+			btVector3(btScalar(0.816320), btScalar(-0.567007), btScalar(0.109469)),
+			btVector3(btScalar(0.741191), btScalar(-0.668690), btScalar(-0.056832)),
+			btVector3(btScalar(0.755632), btScalar(-0.602975), btScalar(-0.254949)),
+			btVector3(btScalar(0.720311), btScalar(-0.521318), btScalar(-0.457165)),
+			btVector3(btScalar(0.670746), btScalar(-0.386583), btScalar(-0.632835)),
+			btVector3(btScalar(0.587031), btScalar(-0.219769), btScalar(-0.778836)),
+			btVector3(btScalar(0.676015), btScalar(-0.003182), btScalar(-0.736676)),
+			btVector3(btScalar(0.566932), btScalar(0.186963), btScalar(-0.802064)),
+			btVector3(btScalar(0.618254), btScalar(0.398105), btScalar(-0.677533)),
+			btVector3(btScalar(0.653964), btScalar(0.575224), btScalar(-0.490933)),
+			btVector3(btScalar(0.525367), btScalar(0.743205), btScalar(-0.414028)),
+			btVector3(btScalar(0.506439), btScalar(0.836528), btScalar(-0.208885)),
+			btVector3(btScalar(0.651427), btScalar(0.756426), btScalar(-0.056247)),
+			btVector3(btScalar(0.641670), btScalar(0.745149), btScalar(0.180908)),
+			btVector3(btScalar(0.602643), btScalar(0.687211), btScalar(0.405180)),
+			btVector3(btScalar(0.516586), btScalar(0.596999), btScalar(0.613447)),
+			btVector3(btScalar(0.602252), btScalar(0.387801), btScalar(0.697573)),
+			btVector3(btScalar(0.646549), btScalar(0.153911), btScalar(0.746956)),
+			btVector3(btScalar(0.650842), btScalar(-0.087756), btScalar(0.753983)),
+			btVector3(btScalar(0.740411), btScalar(-0.497404), btScalar(0.451830)),
+			btVector3(btScalar(0.726946), btScalar(-0.619890), btScalar(0.295093)),
+			btVector3(btScalar(0.637768), btScalar(-0.313092), btScalar(0.703624)),
+			btVector3(btScalar(0.678942), btScalar(-0.722934), btScalar(0.126645)),
+			btVector3(btScalar(0.489072), btScalar(-0.867195), btScalar(-0.092942)),
+			btVector3(btScalar(0.622742), btScalar(-0.757541), btScalar(-0.194636)),
+			btVector3(btScalar(0.596788), btScalar(-0.693576), btScalar(-0.403098)),
+			btVector3(btScalar(0.550150), btScalar(-0.582172), btScalar(-0.598287)),
+			btVector3(btScalar(0.474436), btScalar(-0.429745), btScalar(-0.768101)),
+			btVector3(btScalar(0.372574), btScalar(-0.246016), btScalar(-0.894583)),
+			btVector3(btScalar(0.480095), btScalar(-0.026513), btScalar(-0.876626)),
+			btVector3(btScalar(0.352474), btScalar(0.177242), btScalar(-0.918787)),
+			btVector3(btScalar(0.441848), btScalar(0.374386), btScalar(-0.814946)),
+			btVector3(btScalar(0.492389), btScalar(0.582223), btScalar(-0.646693)),
+			btVector3(btScalar(0.343498), btScalar(0.866080), btScalar(-0.362693)),
+			btVector3(btScalar(0.362036), btScalar(0.745149), btScalar(-0.559639)),
+			btVector3(btScalar(0.334131), btScalar(0.937044), btScalar(-0.099774)),
+			btVector3(btScalar(0.486925), btScalar(0.871718), btScalar(0.052473)),
+			btVector3(btScalar(0.452776), btScalar(0.845665), btScalar(0.281820)),
+			btVector3(btScalar(0.399503), btScalar(0.771785), btScalar(0.494576)),
+			btVector3(btScalar(0.296469), btScalar(0.673018), btScalar(0.677469)),
+			btVector3(btScalar(0.392088), btScalar(0.479179), btScalar(0.785213)),
+			btVector3(btScalar(0.452190), btScalar(0.252094), btScalar(0.855286)),
+			btVector3(btScalar(0.478339), btScalar(0.013149), btScalar(0.877928)),
+			btVector3(btScalar(0.481656), btScalar(-0.219380), btScalar(0.848259)),
+			btVector3(btScalar(0.615327), btScalar(-0.494293), btScalar(0.613837)),
+			btVector3(btScalar(0.594642), btScalar(-0.650414), btScalar(0.472325)),
+			btVector3(btScalar(0.562249), btScalar(-0.771345), btScalar(0.297631)),
+			btVector3(btScalar(0.467411), btScalar(-0.437133), btScalar(0.768231)),
+			btVector3(btScalar(0.519513), btScalar(-0.847947), btScalar(0.103808)),
+			btVector3(btScalar(0.297640), btScalar(-0.938159), btScalar(-0.176288)),
+			btVector3(btScalar(0.446727), btScalar(-0.838615), btScalar(-0.311359)),
+			btVector3(btScalar(0.331790), btScalar(-0.942437), btScalar(0.040762)),
+			btVector3(btScalar(0.413358), btScalar(-0.748403), btScalar(-0.518259)),
+			btVector3(btScalar(0.347596), btScalar(-0.621640), btScalar(-0.701737)),
+			btVector3(btScalar(0.249831), btScalar(-0.456186), btScalar(-0.853984)),
+			btVector3(btScalar(0.131772), btScalar(-0.262931), btScalar(-0.955678)),
+			btVector3(btScalar(0.247099), btScalar(-0.042261), btScalar(-0.967975)),
+			btVector3(btScalar(0.113624), btScalar(0.165965), btScalar(-0.979491)),
+			btVector3(btScalar(0.217438), btScalar(0.374580), btScalar(-0.901220)),
+			btVector3(btScalar(0.307983), btScalar(0.554615), btScalar(-0.772786)),
+			btVector3(btScalar(0.166702), btScalar(0.953181), btScalar(-0.252021)),
+			btVector3(btScalar(0.172751), btScalar(0.844499), btScalar(-0.506743)),
+			btVector3(btScalar(0.177630), btScalar(0.711125), btScalar(-0.679876)),
+			btVector3(btScalar(0.120064), btScalar(0.992260), btScalar(-0.030482)),
+			btVector3(btScalar(0.289640), btScalar(0.949098), btScalar(0.122546)),
+			btVector3(btScalar(0.239879), btScalar(0.909047), btScalar(0.340377)),
+			btVector3(btScalar(0.181142), btScalar(0.821363), btScalar(0.540641)),
+			btVector3(btScalar(0.066986), btScalar(0.719097), btScalar(0.691327)),
+			btVector3(btScalar(0.156750), btScalar(0.545478), btScalar(0.823079)),
+			btVector3(btScalar(0.236172), btScalar(0.342306), btScalar(0.909353)),
+			btVector3(btScalar(0.277541), btScalar(0.112693), btScalar(0.953856)),
+			btVector3(btScalar(0.295299), btScalar(-0.121974), btScalar(0.947415)),
+			btVector3(btScalar(0.287883), btScalar(-0.349254), btScalar(0.891591)),
+			btVector3(btScalar(0.437165), btScalar(-0.634666), btScalar(0.636869)),
+			btVector3(btScalar(0.407113), btScalar(-0.784954), btScalar(0.466664)),
+			btVector3(btScalar(0.375111), btScalar(-0.888193), btScalar(0.264839)),
+			btVector3(btScalar(0.275394), btScalar(-0.560591), btScalar(0.780723)),
+			btVector3(btScalar(0.122015), btScalar(-0.992209), btScalar(-0.024821)),
+			btVector3(btScalar(0.087866), btScalar(-0.966156), btScalar(-0.241676)),
+			btVector3(btScalar(0.239489), btScalar(-0.885665), btScalar(-0.397437)),
+			btVector3(btScalar(0.167287), btScalar(-0.965184), btScalar(0.200817)),
+			btVector3(btScalar(0.201632), btScalar(-0.776789), btScalar(-0.596335)),
+			btVector3(btScalar(0.122015), btScalar(-0.637971), btScalar(-0.760098)),
+			btVector3(btScalar(0.008054), btScalar(-0.464741), btScalar(-0.885214)),
+			btVector3(btScalar(-0.116054), btScalar(-0.271096), btScalar(-0.955482)),
+			btVector3(btScalar(-0.000727), btScalar(-0.056065), btScalar(-0.998424)),
+			btVector3(btScalar(-0.134007), btScalar(0.152939), btScalar(-0.978905)),
+			btVector3(btScalar(-0.025900), btScalar(0.366026), btScalar(-0.930108)),
+			btVector3(btScalar(0.081231), btScalar(0.557337), btScalar(-0.826072)),
+			btVector3(btScalar(-0.002874), btScalar(0.917213), btScalar(-0.398023)),
+			btVector3(btScalar(-0.050683), btScalar(0.981761), btScalar(-0.182534)),
+			btVector3(btScalar(-0.040536), btScalar(0.710153), btScalar(-0.702713)),
+			btVector3(btScalar(-0.139081), btScalar(0.827973), btScalar(-0.543048)),
+			btVector3(btScalar(-0.101029), btScalar(0.994010), btScalar(0.041152)),
+			btVector3(btScalar(0.069328), btScalar(0.978067), btScalar(0.196133)),
+			btVector3(btScalar(0.023860), btScalar(0.911380), btScalar(0.410645)),
+			btVector3(btScalar(-0.153521), btScalar(0.736789), btScalar(0.658145)),
+			btVector3(btScalar(-0.070002), btScalar(0.591750), btScalar(0.802780)),
+			btVector3(btScalar(0.002590), btScalar(0.312948), btScalar(0.949562)),
+			btVector3(btScalar(0.090988), btScalar(-0.020680), btScalar(0.995627)),
+			btVector3(btScalar(0.088842), btScalar(-0.250099), btScalar(0.964006)),
+			btVector3(btScalar(0.083378), btScalar(-0.470185), btScalar(0.878318)),
+			btVector3(btScalar(0.240074), btScalar(-0.749764), btScalar(0.616374)),
+			btVector3(btScalar(0.210803), btScalar(-0.885860), btScalar(0.412987)),
+			btVector3(btScalar(0.077524), btScalar(-0.660524), btScalar(0.746565)),
+			btVector3(btScalar(-0.096736), btScalar(-0.990070), btScalar(-0.100945)),
+			btVector3(btScalar(-0.052634), btScalar(-0.990264), btScalar(0.127426)),
+			btVector3(btScalar(-0.106102), btScalar(-0.938354), btScalar(-0.328340)),
+			btVector3(btScalar(0.013323), btScalar(-0.863112), btScalar(-0.504596)),
+			btVector3(btScalar(-0.002093), btScalar(-0.936993), btScalar(0.349161)),
+			btVector3(btScalar(-0.106297), btScalar(-0.636610), btScalar(-0.763612)),
+			btVector3(btScalar(-0.229430), btScalar(-0.463769), btScalar(-0.855546)),
+			btVector3(btScalar(-0.245236), btScalar(-0.066175), btScalar(-0.966999)),
+			btVector3(btScalar(-0.351587), btScalar(-0.270513), btScalar(-0.896145)),
+			btVector3(btScalar(-0.370906), btScalar(0.133108), btScalar(-0.918982)),
+			btVector3(btScalar(-0.264360), btScalar(0.346000), btScalar(-0.900049)),
+			btVector3(btScalar(-0.151375), btScalar(0.543728), btScalar(-0.825291)),
+			btVector3(btScalar(-0.218697), btScalar(0.912741), btScalar(-0.344346)),
+			btVector3(btScalar(-0.274507), btScalar(0.953764), btScalar(-0.121635)),
+			btVector3(btScalar(-0.259677), btScalar(0.692266), btScalar(-0.673044)),
+			btVector3(btScalar(-0.350416), btScalar(0.798810), btScalar(-0.488786)),
+			btVector3(btScalar(-0.320170), btScalar(0.941127), btScalar(0.108297)),
+			btVector3(btScalar(-0.147667), btScalar(0.952792), btScalar(0.265034)),
+			btVector3(btScalar(-0.188061), btScalar(0.860636), btScalar(0.472910)),
+			btVector3(btScalar(-0.370906), btScalar(0.739900), btScalar(0.560941)),
+			btVector3(btScalar(-0.297143), btScalar(0.585334), btScalar(0.754178)),
+			btVector3(btScalar(-0.189622), btScalar(0.428241), btScalar(0.883393)),
+			btVector3(btScalar(-0.091272), btScalar(0.098695), btScalar(0.990747)),
+			btVector3(btScalar(-0.256945), btScalar(0.228375), btScalar(0.938827)),
+			btVector3(btScalar(-0.111761), btScalar(-0.133251), btScalar(0.984696)),
+			btVector3(btScalar(-0.118006), btScalar(-0.356253), btScalar(0.926725)),
+			btVector3(btScalar(-0.119372), btScalar(-0.563896), btScalar(0.817029)),
+			btVector3(btScalar(0.041228), btScalar(-0.833949), btScalar(0.550010)),
+			btVector3(btScalar(-0.121909), btScalar(-0.736543), btScalar(0.665172)),
+			btVector3(btScalar(-0.307681), btScalar(-0.931160), btScalar(-0.195026)),
+			btVector3(btScalar(-0.283679), btScalar(-0.957990), btScalar(0.041348)),
+			btVector3(btScalar(-0.227284), btScalar(-0.935243), btScalar(0.270890)),
+			btVector3(btScalar(-0.293436), btScalar(-0.858252), btScalar(-0.420860)),
+			btVector3(btScalar(-0.175767), btScalar(-0.780677), btScalar(-0.599262)),
+			btVector3(btScalar(-0.170108), btScalar(-0.858835), btScalar(0.482865)),
+			btVector3(btScalar(-0.332854), btScalar(-0.635055), btScalar(-0.696857)),
+			btVector3(btScalar(-0.447791), btScalar(-0.445299), btScalar(-0.775128)),
+			btVector3(btScalar(-0.470622), btScalar(-0.074146), btScalar(-0.879164)),
+			btVector3(btScalar(-0.639417), btScalar(-0.340505), btScalar(-0.689049)),
+			btVector3(btScalar(-0.598438), btScalar(0.104722), btScalar(-0.794256)),
+			btVector3(btScalar(-0.488575), btScalar(0.307699), btScalar(-0.816313)),
+			btVector3(btScalar(-0.379882), btScalar(0.513592), btScalar(-0.769077)),
+			btVector3(btScalar(-0.425740), btScalar(0.862775), btScalar(-0.272516)),
+			btVector3(btScalar(-0.480769), btScalar(0.875412), btScalar(-0.048439)),
+			btVector3(btScalar(-0.467890), btScalar(0.648716), btScalar(-0.600043)),
+			btVector3(btScalar(-0.543799), btScalar(0.730956), btScalar(-0.411881)),
+			btVector3(btScalar(-0.516284), btScalar(0.838277), btScalar(0.174076)),
+			btVector3(btScalar(-0.353343), btScalar(0.876384), btScalar(0.326519)),
+			btVector3(btScalar(-0.572875), btScalar(0.614497), btScalar(0.542007)),
+			btVector3(btScalar(-0.503600), btScalar(0.497261), btScalar(0.706161)),
+			btVector3(btScalar(-0.530920), btScalar(0.754870), btScalar(0.384685)),
+			btVector3(btScalar(-0.395884), btScalar(0.366414), btScalar(0.841818)),
+			btVector3(btScalar(-0.300656), btScalar(0.001678), btScalar(0.953661)),
+			btVector3(btScalar(-0.461060), btScalar(0.146912), btScalar(0.875000)),
+			btVector3(btScalar(-0.315486), btScalar(-0.232212), btScalar(0.919893)),
+			btVector3(btScalar(-0.323682), btScalar(-0.449187), btScalar(0.832644)),
+			btVector3(btScalar(-0.318999), btScalar(-0.639527), btScalar(0.699134)),
+			btVector3(btScalar(-0.496771), btScalar(-0.866029), btScalar(-0.055271)),
+			btVector3(btScalar(-0.496771), btScalar(-0.816257), btScalar(-0.294377)),
+			btVector3(btScalar(-0.456377), btScalar(-0.869528), btScalar(0.188130)),
+			btVector3(btScalar(-0.380858), btScalar(-0.827144), btScalar(0.412792)),
+			btVector3(btScalar(-0.449352), btScalar(-0.727405), btScalar(-0.518259)),
+			btVector3(btScalar(-0.570533), btScalar(-0.551064), btScalar(-0.608632)),
+			btVector3(btScalar(-0.656394), btScalar(-0.118280), btScalar(-0.744874)),
+			btVector3(btScalar(-0.756696), btScalar(-0.438105), btScalar(-0.484882)),
+			btVector3(btScalar(-0.801773), btScalar(-0.204798), btScalar(-0.561005)),
+			btVector3(btScalar(-0.785186), btScalar(0.038618), btScalar(-0.617805)),
+			btVector3(btScalar(-0.709082), btScalar(0.262399), btScalar(-0.654306)),
+			btVector3(btScalar(-0.583412), btScalar(0.462265), btScalar(-0.667383)),
+			btVector3(btScalar(-0.616001), btScalar(0.761286), btScalar(-0.201272)),
+			btVector3(btScalar(-0.660687), btScalar(0.750204), btScalar(0.020072)),
+			btVector3(btScalar(-0.744987), btScalar(0.435823), btScalar(-0.504791)),
+			btVector3(btScalar(-0.713765), btScalar(0.605554), btScalar(-0.351373)),
+			btVector3(btScalar(-0.686251), btScalar(0.687600), btScalar(0.236927)),
+			btVector3(btScalar(-0.680201), btScalar(0.429407), btScalar(0.593732)),
+			btVector3(btScalar(-0.733474), btScalar(0.546450), btScalar(0.403814)),
+			btVector3(btScalar(-0.591023), btScalar(0.292923), btScalar(0.751445)),
+			btVector3(btScalar(-0.500283), btScalar(-0.080757), btScalar(0.861922)),
+			btVector3(btScalar(-0.643710), btScalar(0.070115), btScalar(0.761985)),
+			btVector3(btScalar(-0.506332), btScalar(-0.308425), btScalar(0.805122)),
+			btVector3(btScalar(-0.503015), btScalar(-0.509847), btScalar(0.697573)),
+			btVector3(btScalar(-0.482525), btScalar(-0.682105), btScalar(0.549229)),
+			btVector3(btScalar(-0.680396), btScalar(-0.716323), btScalar(-0.153451)),
+			btVector3(btScalar(-0.658346), btScalar(-0.746264), btScalar(0.097562)),
+			btVector3(btScalar(-0.653272), btScalar(-0.646915), btScalar(-0.392948)),
+			btVector3(btScalar(-0.590828), btScalar(-0.732655), btScalar(0.337645)),
+			btVector3(btScalar(-0.819140), btScalar(-0.518013), btScalar(-0.246166)),
+			btVector3(btScalar(-0.900513), btScalar(-0.282178), btScalar(-0.330487)),
+			btVector3(btScalar(-0.914953), btScalar(-0.028652), btScalar(-0.402122)),
+			btVector3(btScalar(-0.859924), btScalar(0.220209), btScalar(-0.459898)),
+			btVector3(btScalar(-0.777185), btScalar(0.613720), btScalar(-0.137836)),
+			btVector3(btScalar(-0.805285), btScalar(0.586889), btScalar(0.082728)),
+			btVector3(btScalar(-0.872413), btScalar(0.406077), btScalar(-0.271735)),
+			btVector3(btScalar(-0.859339), btScalar(0.448072), btScalar(0.246101)),
+			btVector3(btScalar(-0.757671), btScalar(0.216320), btScalar(0.615594)),
+			btVector3(btScalar(-0.826165), btScalar(0.348139), btScalar(0.442851)),
+			btVector3(btScalar(-0.671810), btScalar(-0.162803), btScalar(0.722557)),
+			btVector3(btScalar(-0.796504), btScalar(-0.004543), btScalar(0.604468)),
+			btVector3(btScalar(-0.676298), btScalar(-0.378223), btScalar(0.631794)),
+			btVector3(btScalar(-0.668883), btScalar(-0.558258), btScalar(0.490673)),
+			btVector3(btScalar(-0.821287), btScalar(-0.570118), btScalar(0.006994)),
+			btVector3(btScalar(-0.767428), btScalar(-0.587810), btScalar(0.255470)),
+			btVector3(btScalar(-0.933296), btScalar(-0.349837), btScalar(-0.079865)),
+			btVector3(btScalar(-0.982667), btScalar(-0.100393), btScalar(-0.155208)),
+			btVector3(btScalar(-0.961396), btScalar(0.160910), btScalar(-0.222938)),
+			btVector3(btScalar(-0.934858), btScalar(0.354555), btScalar(-0.006864)),
+			btVector3(btScalar(-0.941687), btScalar(0.229736), btScalar(0.245711)),
+			btVector3(btScalar(-0.884317), btScalar(0.131552), btScalar(0.447536)),
+			btVector3(btScalar(-0.810359), btScalar(-0.219769), btScalar(0.542788)),
+			btVector3(btScalar(-0.915929), btScalar(-0.210048), btScalar(0.341743)),
+			btVector3(btScalar(-0.816799), btScalar(-0.407192), btScalar(0.408303)),
+			btVector3(btScalar(-0.903050), btScalar(-0.392416), btScalar(0.174076)),
+			btVector3(btScalar(-0.980325), btScalar(-0.170969), btScalar(0.096586)),
+			btVector3(btScalar(-0.995936), btScalar(0.084891), btScalar(0.029441)),
+			btVector3(btScalar(-0.960031), btScalar(0.002650), btScalar(0.279283)),
+		};
+	static btVector3 sUnitSpherePoints[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] =
+		{
+			btVector3(btScalar(0.000000), btScalar(-0.000000), btScalar(-1.000000)),
+			btVector3(btScalar(0.723608), btScalar(-0.525725), btScalar(-0.447219)),
+			btVector3(btScalar(-0.276388), btScalar(-0.850649), btScalar(-0.447219)),
+			btVector3(btScalar(-0.894426), btScalar(-0.000000), btScalar(-0.447216)),
+			btVector3(btScalar(-0.276388), btScalar(0.850649), btScalar(-0.447220)),
+			btVector3(btScalar(0.723608), btScalar(0.525725), btScalar(-0.447219)),
+			btVector3(btScalar(0.276388), btScalar(-0.850649), btScalar(0.447220)),
+			btVector3(btScalar(-0.723608), btScalar(-0.525725), btScalar(0.447219)),
+			btVector3(btScalar(-0.723608), btScalar(0.525725), btScalar(0.447219)),
+			btVector3(btScalar(0.276388), btScalar(0.850649), btScalar(0.447219)),
+			btVector3(btScalar(0.894426), btScalar(0.000000), btScalar(0.447216)),
+			btVector3(btScalar(-0.000000), btScalar(0.000000), btScalar(1.000000)),
+			btVector3(btScalar(0.425323), btScalar(-0.309011), btScalar(-0.850654)),
+			btVector3(btScalar(-0.162456), btScalar(-0.499995), btScalar(-0.850654)),
+			btVector3(btScalar(0.262869), btScalar(-0.809012), btScalar(-0.525738)),
+			btVector3(btScalar(0.425323), btScalar(0.309011), btScalar(-0.850654)),
+			btVector3(btScalar(0.850648), btScalar(-0.000000), btScalar(-0.525736)),
+			btVector3(btScalar(-0.525730), btScalar(-0.000000), btScalar(-0.850652)),
+			btVector3(btScalar(-0.688190), btScalar(-0.499997), btScalar(-0.525736)),
+			btVector3(btScalar(-0.162456), btScalar(0.499995), btScalar(-0.850654)),
+			btVector3(btScalar(-0.688190), btScalar(0.499997), btScalar(-0.525736)),
+			btVector3(btScalar(0.262869), btScalar(0.809012), btScalar(-0.525738)),
+			btVector3(btScalar(0.951058), btScalar(0.309013), btScalar(0.000000)),
+			btVector3(btScalar(0.951058), btScalar(-0.309013), btScalar(0.000000)),
+			btVector3(btScalar(0.587786), btScalar(-0.809017), btScalar(0.000000)),
+			btVector3(btScalar(0.000000), btScalar(-1.000000), btScalar(0.000000)),
+			btVector3(btScalar(-0.587786), btScalar(-0.809017), btScalar(0.000000)),
+			btVector3(btScalar(-0.951058), btScalar(-0.309013), btScalar(-0.000000)),
+			btVector3(btScalar(-0.951058), btScalar(0.309013), btScalar(-0.000000)),
+			btVector3(btScalar(-0.587786), btScalar(0.809017), btScalar(-0.000000)),
+			btVector3(btScalar(-0.000000), btScalar(1.000000), btScalar(-0.000000)),
+			btVector3(btScalar(0.587786), btScalar(0.809017), btScalar(-0.000000)),
+			btVector3(btScalar(0.688190), btScalar(-0.499997), btScalar(0.525736)),
+			btVector3(btScalar(-0.262869), btScalar(-0.809012), btScalar(0.525738)),
+			btVector3(btScalar(-0.850648), btScalar(0.000000), btScalar(0.525736)),
+			btVector3(btScalar(-0.262869), btScalar(0.809012), btScalar(0.525738)),
+			btVector3(btScalar(0.688190), btScalar(0.499997), btScalar(0.525736)),
+			btVector3(btScalar(0.525730), btScalar(0.000000), btScalar(0.850652)),
+			btVector3(btScalar(0.162456), btScalar(-0.499995), btScalar(0.850654)),
+			btVector3(btScalar(-0.425323), btScalar(-0.309011), btScalar(0.850654)),
+			btVector3(btScalar(-0.425323), btScalar(0.309011), btScalar(0.850654)),
+			btVector3(btScalar(0.162456), btScalar(0.499995), btScalar(0.850654))};
+	if (highres)
+		return sUnitSpherePointsHighres;
 	return sUnitSpherePoints;
 }
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h
index e959f198b69..54439f9ca25 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h
@@ -21,32 +21,31 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 
-
 ///The btShapeHull class takes a btConvexShape, builds a simplified convex hull using btConvexHull and provides triangle indices and vertices.
 ///It can be useful for to simplify a complex convex object and for visualization of a non-polyhedral convex object.
 ///It approximates the convex hull using the supporting vertex of 42 directions.
-ATTRIBUTE_ALIGNED16(class) btShapeHull
+ATTRIBUTE_ALIGNED16(class)
+btShapeHull
 {
 protected:
-
 	btAlignedObjectArray<btVector3> m_vertices;
 	btAlignedObjectArray<unsigned int> m_indices;
 	unsigned int m_numIndices;
 	const btConvexShape* m_shape;
 
-	static btVector3* getUnitSpherePoints();
+	static btVector3* getUnitSpherePoints(int highres = 0);
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-	btShapeHull (const btConvexShape* shape);
-	~btShapeHull ();
 
-	bool buildHull (btScalar margin);
+	btShapeHull(const btConvexShape* shape);
+	~btShapeHull();
+
+	bool buildHull(btScalar margin, int highres = 0);
 
-	int numTriangles () const;
-	int numVertices () const;
-	int numIndices () const;
+	int numTriangles() const;
+	int numVertices() const;
+	int numIndices() const;
 
 	const btVector3* getVertexPointer() const
 	{
@@ -58,4 +57,4 @@ public:
 	}
 };
 
-#endif //BT_SHAPE_HULL_H
+#endif  //BT_SHAPE_HULL_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp
index b9a736c0fdd..027db2e1046 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp
@@ -18,54 +18,48 @@ subject to the following restrictions:
 
 #include "LinearMath/btQuaternion.h"
 
-btVector3	btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+btVector3 btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 {
 	(void)vec;
-	return btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
+	return btVector3(btScalar(0.), btScalar(0.), btScalar(0.));
 }
 
-void	btSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 {
 	(void)vectors;
 
-	for (int i=0;i<numVectors;i++)
+	for (int i = 0; i < numVectors; i++)
 	{
-		supportVerticesOut[i].setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+		supportVerticesOut[i].setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 	}
 }
 
-
-btVector3	btSphereShape::localGetSupportingVertex(const btVector3& vec)const
+btVector3 btSphereShape::localGetSupportingVertex(const btVector3& vec) const
 {
 	btVector3 supVertex;
 	supVertex = localGetSupportingVertexWithoutMargin(vec);
 
 	btVector3 vecnorm = vec;
-	if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+	if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON))
 	{
-		vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
-	} 
+		vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.));
+	}
 	vecnorm.normalize();
-	supVertex+= getMargin() * vecnorm;
+	supVertex += getMargin() * vecnorm;
 	return supVertex;
 }
 
-
 //broken due to scaling
-void btSphereShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+void btSphereShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 {
 	const btVector3& center = t.getOrigin();
-	btVector3 extent(getMargin(),getMargin(),getMargin());
+	btVector3 extent(getMargin(), getMargin(), getMargin());
 	aabbMin = center - extent;
 	aabbMax = center + extent;
 }
 
-
-
-void	btSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btSphereShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
-	btScalar elem = btScalar(0.4) * mass * getMargin()*getMargin();
-	inertia.setValue(elem,elem,elem);
-
+	btScalar elem = btScalar(0.4) * mass * getMargin() * getMargin();
+	inertia.setValue(elem, elem, elem);
 }
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h
index b192efeeb8d..75e4fd8e189 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h
@@ -16,58 +16,56 @@ subject to the following restrictions:
 #define BT_SPHERE_MINKOWSKI_H
 
 #include "btConvexInternalShape.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"  // for the types
 
 ///The btSphereShape implements an implicit sphere, centered around a local origin with radius.
-ATTRIBUTE_ALIGNED16(class) btSphereShape : public btConvexInternalShape
+ATTRIBUTE_ALIGNED16(class)
+btSphereShape : public btConvexInternalShape
 
 {
-	
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btSphereShape (btScalar radius) : btConvexInternalShape ()
+	btSphereShape(btScalar radius) : btConvexInternalShape()
 	{
 		m_shapeType = SPHERE_SHAPE_PROXYTYPE;
+		m_localScaling.setValue(1.0, 1.0, 1.0);
+		m_implicitShapeDimensions.setZero();
 		m_implicitShapeDimensions.setX(radius);
 		m_collisionMargin = radius;
+		m_padding = 0;
 	}
-	
-	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
-	//notice that the vectors should be unit length
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
-
 
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
+	//notice that the vectors should be unit length
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
 
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
 
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
-	btScalar	getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX();}
+	btScalar getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX(); }
 
-	void	setUnscaledRadius(btScalar	radius)
+	void setUnscaledRadius(btScalar radius)
 	{
 		m_implicitShapeDimensions.setX(radius);
 		btConvexInternalShape::setMargin(radius);
 	}
 
 	//debugging
-	virtual const char*	getName()const {return "SPHERE";}
+	virtual const char* getName() const { return "SPHERE"; }
 
-	virtual void	setMargin(btScalar margin)
+	virtual void setMargin(btScalar margin)
 	{
 		btConvexInternalShape::setMargin(margin);
 	}
-	virtual btScalar	getMargin() const
+	virtual btScalar getMargin() const
 	{
 		//to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case
 		//this means, non-uniform scaling is not supported anymore
 		return getRadius();
 	}
-
-
 };
 
-
-#endif //BT_SPHERE_MINKOWSKI_H
+#endif  //BT_SPHERE_MINKOWSKI_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp
index d17141e3f20..9238c919d51 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp
@@ -17,24 +17,18 @@ subject to the following restrictions:
 
 #include "LinearMath/btTransformUtil.h"
 
-
-btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant)
-: btConcaveShape (), m_planeNormal(planeNormal.normalized()),
-m_planeConstant(planeConstant),
-m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
+btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal, btScalar planeConstant)
+	: btConcaveShape(), m_planeNormal(planeNormal.normalized()), m_planeConstant(planeConstant), m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.))
 {
 	m_shapeType = STATIC_PLANE_PROXYTYPE;
 	//	btAssert( btFuzzyZero(m_planeNormal.length() - btScalar(1.)) );
 }
 
-
 btStaticPlaneShape::~btStaticPlaneShape()
 {
 }
 
-
-
-void btStaticPlaneShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+void btStaticPlaneShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 {
 	(void)t;
 	/*
@@ -47,57 +41,49 @@ void btStaticPlaneShape::getAabb(const btTransform& t,btVector3& aabbMin,btVecto
 	aabbMax.setMax(center - infvec*m_planeNormal); 
 	*/
 
-	aabbMin.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
-	aabbMax.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-
+	aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
+	aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
 }
 
-
-
-
-void	btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+void btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
 {
-
 	btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
 	btScalar radius = halfExtents.length();
 	btVector3 center = (aabbMax + aabbMin) * btScalar(0.5);
-	
+
 	//this is where the triangles are generated, given AABB and plane equation (normal/constant)
 
-	btVector3 tangentDir0,tangentDir1;
+	btVector3 tangentDir0, tangentDir1;
 
 	//tangentDir0/tangentDir1 can be precalculated
-	btPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1);
+	btPlaneSpace1(m_planeNormal, tangentDir0, tangentDir1);
 
-	btVector3 supVertex0,supVertex1;
+	btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant) * m_planeNormal;
 
-	btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal;
-	
 	btVector3 triangle[3];
-	triangle[0] = projectedCenter + tangentDir0*radius + tangentDir1*radius;
-	triangle[1] = projectedCenter + tangentDir0*radius - tangentDir1*radius;
-	triangle[2] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
-
-	callback->processTriangle(triangle,0,0);
+	triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius;
+	triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius;
+	triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius;
 
-	triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
-	triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius;
-	triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius;
+	callback->processTriangle(triangle, 0, 0);
 
-	callback->processTriangle(triangle,0,1);
+	triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius;
+	triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius;
+	triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius;
 
+	callback->processTriangle(triangle, 0, 1);
 }
 
-void	btStaticPlaneShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btStaticPlaneShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
 	(void)mass;
 
 	//moving concave objects not supported
-	
-	inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+
+	inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 }
 
-void	btStaticPlaneShape::setLocalScaling(const btVector3& scaling)
+void btStaticPlaneShape::setLocalScaling(const btVector3& scaling)
 {
 	m_localScaling = scaling;
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h
index e6e32883959..1cda8bbc75b 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h
@@ -18,88 +18,86 @@ subject to the following restrictions:
 
 #include "btConcaveShape.h"
 
-
 ///The btStaticPlaneShape simulates an infinite non-moving (static) collision plane.
-ATTRIBUTE_ALIGNED16(class) btStaticPlaneShape : public btConcaveShape
+ATTRIBUTE_ALIGNED16(class)
+btStaticPlaneShape : public btConcaveShape
 {
 protected:
-	btVector3	m_localAabbMin;
-	btVector3	m_localAabbMax;
-	
-	btVector3	m_planeNormal;
-	btScalar      m_planeConstant;
-	btVector3	m_localScaling;
+	btVector3 m_localAabbMin;
+	btVector3 m_localAabbMax;
+
+	btVector3 m_planeNormal;
+	btScalar m_planeConstant;
+	btVector3 m_localScaling;
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant);
+	btStaticPlaneShape(const btVector3& planeNormal, btScalar planeConstant);
 
 	virtual ~btStaticPlaneShape();
 
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+	virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const;
 
-	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
 
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
-
-	virtual void	setLocalScaling(const btVector3& scaling);
+	virtual void setLocalScaling(const btVector3& scaling);
 	virtual const btVector3& getLocalScaling() const;
-	
-	const btVector3&	getPlaneNormal() const
+
+	const btVector3& getPlaneNormal() const
 	{
-		return	m_planeNormal;
+		return m_planeNormal;
 	}
 
-	const btScalar&	getPlaneConstant() const
+	const btScalar& getPlaneConstant() const
 	{
-		return	m_planeConstant;
+		return m_planeConstant;
 	}
 
 	//debugging
-	virtual const char*	getName()const {return "STATICPLANE";}
+	virtual const char* getName() const { return "STATICPLANE"; }
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btStaticPlaneShapeData
+struct btStaticPlaneShapeData
 {
-	btCollisionShapeData	m_collisionShapeData;
+	btCollisionShapeData m_collisionShapeData;
 
-	btVector3FloatData	m_localScaling;
-	btVector3FloatData	m_planeNormal;
-	float			m_planeConstant;
-	char	m_pad[4];
+	btVector3FloatData m_localScaling;
+	btVector3FloatData m_planeNormal;
+	float m_planeConstant;
+	char m_pad[4];
 };
 
-
-SIMD_FORCE_INLINE	int	btStaticPlaneShape::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btStaticPlaneShape::calculateSerializeBufferSize() const
 {
 	return sizeof(btStaticPlaneShapeData);
 }
 
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btStaticPlaneShape::serialize(void* dataBuffer, btSerializer* serializer) const
+SIMD_FORCE_INLINE const char* btStaticPlaneShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*) dataBuffer;
-	btCollisionShape::serialize(&planeData->m_collisionShapeData,serializer);
+	btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*)dataBuffer;
+	btCollisionShape::serialize(&planeData->m_collisionShapeData, serializer);
 
 	m_localScaling.serializeFloat(planeData->m_localScaling);
 	m_planeNormal.serializeFloat(planeData->m_planeNormal);
 	planeData->m_planeConstant = float(m_planeConstant);
-		
-	return "btStaticPlaneShapeData";
-}
-
-
-#endif //BT_STATIC_PLANE_SHAPE_H
 
+	// Fill padding with zeros to appease msan.
+	planeData->m_pad[0] = 0;
+	planeData->m_pad[1] = 0;
+	planeData->m_pad[2] = 0;
+	planeData->m_pad[3] = 0;
 
+	return "btStaticPlaneShapeData";
+}
 
+#endif  //BT_STATIC_PLANE_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp
index b3d449676b9..eb288e99c91 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp
@@ -18,32 +18,30 @@ subject to the following restrictions:
 
 btStridingMeshInterface::~btStridingMeshInterface()
 {
-
 }
 
-
-void	btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
 {
 	(void)aabbMin;
 	(void)aabbMax;
 	int numtotalphysicsverts = 0;
-	int part,graphicssubparts = getNumSubParts();
-	const unsigned char * vertexbase;
-	const unsigned char * indexbase;
+	int part, graphicssubparts = getNumSubParts();
+	const unsigned char* vertexbase;
+	const unsigned char* indexbase;
 	int indexstride;
 	PHY_ScalarType type;
 	PHY_ScalarType gfxindextype;
-	int stride,numverts,numtriangles;
+	int stride, numverts, numtriangles;
 	int gfxindex;
 	btVector3 triangle[3];
 
 	btVector3 meshScaling = getScaling();
 
 	///if the number of parts is big, the performance might drop due to the innerloop switch on indextype
-	for (part=0;part<graphicssubparts ;part++)
+	for (part = 0; part < graphicssubparts; part++)
 	{
-		getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part);
-		numtotalphysicsverts+=numtriangles*3; //upper bound
+		getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numtriangles, gfxindextype, part);
+		numtotalphysicsverts += numtriangles * 3;  //upper bound
 
 		///unlike that developers want to pass in double-precision meshes in single-precision Bullet build
 		///so disable this feature by default
@@ -51,143 +49,141 @@ void	btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde
 
 		switch (type)
 		{
-		case PHY_FLOAT:
-		 {
-
-			 float* graphicsbase;
-
-			 switch (gfxindextype)
-			 {
-			 case PHY_INTEGER:
-				 {
-					 for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
-					 {
-						 unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride);
-						 graphicsbase = (float*)(vertexbase+tri_indices[0]*stride);
-						 triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
-						 graphicsbase = (float*)(vertexbase+tri_indices[1]*stride);
-						 triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
-						 graphicsbase = (float*)(vertexbase+tri_indices[2]*stride);
-						 triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
-						 callback->internalProcessTriangleIndex(triangle,part,gfxindex);
-					 }
-					 break;
-				 }
-			 case PHY_SHORT:
-				 {
-					 for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
-					 {
-						 unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride);
-						 graphicsbase = (float*)(vertexbase+tri_indices[0]*stride);
-						 triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
-						 graphicsbase = (float*)(vertexbase+tri_indices[1]*stride);
-						 triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
-						 graphicsbase = (float*)(vertexbase+tri_indices[2]*stride);
-						 triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
-						 callback->internalProcessTriangleIndex(triangle,part,gfxindex);
-					 }
-					 break;
-				 }
-			case PHY_UCHAR:
-				 {
-					 for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
-					 {
-						 unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride);
-						 graphicsbase = (float*)(vertexbase+tri_indices[0]*stride);
-						 triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
-						 graphicsbase = (float*)(vertexbase+tri_indices[1]*stride);
-						 triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
-						 graphicsbase = (float*)(vertexbase+tri_indices[2]*stride);
-						 triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
-						 callback->internalProcessTriangleIndex(triangle,part,gfxindex);
-					 }
-					 break;
-				 }
-			 default:
-				 btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT));
-			 }
-			 break;
-		 }
-
-		case PHY_DOUBLE:
+			case PHY_FLOAT:
+			{
+				float* graphicsbase;
+
+				switch (gfxindextype)
+				{
+					case PHY_INTEGER:
+					{
+						for (gfxindex = 0; gfxindex < numtriangles; gfxindex++)
+						{
+							unsigned int* tri_indices = (unsigned int*)(indexbase + gfxindex * indexstride);
+							graphicsbase = (float*)(vertexbase + tri_indices[0] * stride);
+							triangle[0].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ());
+							graphicsbase = (float*)(vertexbase + tri_indices[1] * stride);
+							triangle[1].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ());
+							graphicsbase = (float*)(vertexbase + tri_indices[2] * stride);
+							triangle[2].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ());
+							callback->internalProcessTriangleIndex(triangle, part, gfxindex);
+						}
+						break;
+					}
+					case PHY_SHORT:
+					{
+						for (gfxindex = 0; gfxindex < numtriangles; gfxindex++)
+						{
+							unsigned short int* tri_indices = (unsigned short int*)(indexbase + gfxindex * indexstride);
+							graphicsbase = (float*)(vertexbase + tri_indices[0] * stride);
+							triangle[0].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ());
+							graphicsbase = (float*)(vertexbase + tri_indices[1] * stride);
+							triangle[1].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ());
+							graphicsbase = (float*)(vertexbase + tri_indices[2] * stride);
+							triangle[2].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ());
+							callback->internalProcessTriangleIndex(triangle, part, gfxindex);
+						}
+						break;
+					}
+					case PHY_UCHAR:
+					{
+						for (gfxindex = 0; gfxindex < numtriangles; gfxindex++)
+						{
+							unsigned char* tri_indices = (unsigned char*)(indexbase + gfxindex * indexstride);
+							graphicsbase = (float*)(vertexbase + tri_indices[0] * stride);
+							triangle[0].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ());
+							graphicsbase = (float*)(vertexbase + tri_indices[1] * stride);
+							triangle[1].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ());
+							graphicsbase = (float*)(vertexbase + tri_indices[2] * stride);
+							triangle[2].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ());
+							callback->internalProcessTriangleIndex(triangle, part, gfxindex);
+						}
+						break;
+					}
+					default:
+						btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT));
+				}
+				break;
+			}
+
+			case PHY_DOUBLE:
 			{
 				double* graphicsbase;
 
 				switch (gfxindextype)
 				{
-				case PHY_INTEGER:
+					case PHY_INTEGER:
 					{
-						for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+						for (gfxindex = 0; gfxindex < numtriangles; gfxindex++)
 						{
-							unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride);
-							graphicsbase = (double*)(vertexbase+tri_indices[0]*stride);
-							triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ());
-							graphicsbase = (double*)(vertexbase+tri_indices[1]*stride);
-							triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),  (btScalar)graphicsbase[2]*meshScaling.getZ());
-							graphicsbase = (double*)(vertexbase+tri_indices[2]*stride);
-							triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),  (btScalar)graphicsbase[2]*meshScaling.getZ());
-							callback->internalProcessTriangleIndex(triangle,part,gfxindex);
+							unsigned int* tri_indices = (unsigned int*)(indexbase + gfxindex * indexstride);
+							graphicsbase = (double*)(vertexbase + tri_indices[0] * stride);
+							triangle[0].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ());
+							graphicsbase = (double*)(vertexbase + tri_indices[1] * stride);
+							triangle[1].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ());
+							graphicsbase = (double*)(vertexbase + tri_indices[2] * stride);
+							triangle[2].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ());
+							callback->internalProcessTriangleIndex(triangle, part, gfxindex);
 						}
 						break;
 					}
-				case PHY_SHORT:
+					case PHY_SHORT:
 					{
-						for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+						for (gfxindex = 0; gfxindex < numtriangles; gfxindex++)
 						{
-							unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride);
-							graphicsbase = (double*)(vertexbase+tri_indices[0]*stride);
-							triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ());
-							graphicsbase = (double*)(vertexbase+tri_indices[1]*stride);
-							triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),  (btScalar)graphicsbase[2]*meshScaling.getZ());
-							graphicsbase = (double*)(vertexbase+tri_indices[2]*stride);
-							triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),  (btScalar)graphicsbase[2]*meshScaling.getZ());
-							callback->internalProcessTriangleIndex(triangle,part,gfxindex);
+							unsigned short int* tri_indices = (unsigned short int*)(indexbase + gfxindex * indexstride);
+							graphicsbase = (double*)(vertexbase + tri_indices[0] * stride);
+							triangle[0].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ());
+							graphicsbase = (double*)(vertexbase + tri_indices[1] * stride);
+							triangle[1].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ());
+							graphicsbase = (double*)(vertexbase + tri_indices[2] * stride);
+							triangle[2].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ());
+							callback->internalProcessTriangleIndex(triangle, part, gfxindex);
 						}
 						break;
 					}
-				case PHY_UCHAR:
+					case PHY_UCHAR:
 					{
-						for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+						for (gfxindex = 0; gfxindex < numtriangles; gfxindex++)
 						{
-							unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride);
-							graphicsbase = (double*)(vertexbase+tri_indices[0]*stride);
-							triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ());
-							graphicsbase = (double*)(vertexbase+tri_indices[1]*stride);
-							triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),  (btScalar)graphicsbase[2]*meshScaling.getZ());
-							graphicsbase = (double*)(vertexbase+tri_indices[2]*stride);
-							triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),  (btScalar)graphicsbase[2]*meshScaling.getZ());
-							callback->internalProcessTriangleIndex(triangle,part,gfxindex);
+							unsigned char* tri_indices = (unsigned char*)(indexbase + gfxindex * indexstride);
+							graphicsbase = (double*)(vertexbase + tri_indices[0] * stride);
+							triangle[0].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ());
+							graphicsbase = (double*)(vertexbase + tri_indices[1] * stride);
+							triangle[1].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ());
+							graphicsbase = (double*)(vertexbase + tri_indices[2] * stride);
+							triangle[2].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ());
+							callback->internalProcessTriangleIndex(triangle, part, gfxindex);
 						}
 						break;
 					}
-				default:
-					btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT));
+					default:
+						btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT));
 				}
 				break;
 			}
-		default:
-			btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE));
+			default:
+				btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE));
 		}
 
 		unLockReadOnlyVertexBase(part);
 	}
 }
 
-void	btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax)
+void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin, btVector3& aabbMax)
 {
-
-	struct	AabbCalculationCallback : public btInternalTriangleIndexCallback
+	struct AabbCalculationCallback : public btInternalTriangleIndexCallback
 	{
-		btVector3	m_aabbMin;
-		btVector3	m_aabbMax;
+		btVector3 m_aabbMin;
+		btVector3 m_aabbMax;
 
 		AabbCalculationCallback()
 		{
-			m_aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-			m_aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+			m_aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+			m_aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
 		}
 
-		virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+		virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
 		{
 			(void)partId;
 			(void)triangleIndex;
@@ -202,21 +198,19 @@ void	btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVecto
 	};
 
 	//first calculate the total aabb for all triangles
-	AabbCalculationCallback	aabbCallback;
-	aabbMin.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
-	aabbMax.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-	InternalProcessAllTriangles(&aabbCallback,aabbMin,aabbMax);
+	AabbCalculationCallback aabbCallback;
+	aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
+	aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+	InternalProcessAllTriangles(&aabbCallback, aabbMin, aabbMax);
 
 	aabbMin = aabbCallback.m_aabbMin;
 	aabbMax = aabbCallback.m_aabbMax;
 }
 
-
-
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* serializer) const
+const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btStridingMeshInterfaceData* trimeshData = (btStridingMeshInterfaceData*) dataBuffer;
+	btStridingMeshInterfaceData* trimeshData = (btStridingMeshInterfaceData*)dataBuffer;
 
 	trimeshData->m_numMeshParts = getNumSubParts();
 
@@ -226,29 +220,28 @@ const char*	btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s
 
 	if (trimeshData->m_numMeshParts)
 	{
-		btChunk* chunk = serializer->allocate(sizeof(btMeshPartData),trimeshData->m_numMeshParts);
+		btChunk* chunk = serializer->allocate(sizeof(btMeshPartData), trimeshData->m_numMeshParts);
 		btMeshPartData* memPtr = (btMeshPartData*)chunk->m_oldPtr;
-		trimeshData->m_meshPartsPtr = (btMeshPartData *)serializer->getUniquePointer(memPtr);
-
+		trimeshData->m_meshPartsPtr = (btMeshPartData*)serializer->getUniquePointer(memPtr);
 
-	//	int numtotalphysicsverts = 0;
-		int part,graphicssubparts = getNumSubParts();
-		const unsigned char * vertexbase;
-		const unsigned char * indexbase;
+		//	int numtotalphysicsverts = 0;
+		int part, graphicssubparts = getNumSubParts();
+		const unsigned char* vertexbase;
+		const unsigned char* indexbase;
 		int indexstride;
 		PHY_ScalarType type;
 		PHY_ScalarType gfxindextype;
-		int stride,numverts,numtriangles;
+		int stride, numverts, numtriangles;
 		int gfxindex;
-	//	btVector3 triangle[3];
+		//	btVector3 triangle[3];
 
-	//	btVector3 meshScaling = getScaling();
+		//	btVector3 meshScaling = getScaling();
 
 		///if the number of parts is big, the performance might drop due to the innerloop switch on indextype
-		for (part=0;part<graphicssubparts ;part++,memPtr++)
+		for (part = 0; part < graphicssubparts; part++, memPtr++)
 		{
-			getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part);
-			memPtr->m_numTriangles = numtriangles;//indices = 3*numtriangles
+			getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numtriangles, gfxindextype, part);
+			memPtr->m_numTriangles = numtriangles;  //indices = 3*numtriangles
 			memPtr->m_numVertices = numverts;
 			memPtr->m_indices16 = 0;
 			memPtr->m_indices32 = 0;
@@ -257,44 +250,46 @@ const char*	btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s
 			memPtr->m_vertices3f = 0;
 			memPtr->m_vertices3d = 0;
 
-
 			switch (gfxindextype)
 			{
-			case PHY_INTEGER:
+				case PHY_INTEGER:
 				{
-					int numindices = numtriangles*3;
-				
+					int numindices = numtriangles * 3;
+
 					if (numindices)
 					{
-						btChunk* chunk = serializer->allocate(sizeof(btIntIndexData),numindices);
+						btChunk* chunk = serializer->allocate(sizeof(btIntIndexData), numindices);
 						btIntIndexData* tmpIndices = (btIntIndexData*)chunk->m_oldPtr;
 						memPtr->m_indices32 = (btIntIndexData*)serializer->getUniquePointer(tmpIndices);
-						for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+						for (gfxindex = 0; gfxindex < numtriangles; gfxindex++)
 						{
-							unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride);
-							tmpIndices[gfxindex*3].m_value = tri_indices[0];
-							tmpIndices[gfxindex*3+1].m_value = tri_indices[1];
-							tmpIndices[gfxindex*3+2].m_value = tri_indices[2];
+							unsigned int* tri_indices = (unsigned int*)(indexbase + gfxindex * indexstride);
+							tmpIndices[gfxindex * 3].m_value = tri_indices[0];
+							tmpIndices[gfxindex * 3 + 1].m_value = tri_indices[1];
+							tmpIndices[gfxindex * 3 + 2].m_value = tri_indices[2];
 						}
-						serializer->finalizeChunk(chunk,"btIntIndexData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr);
+						serializer->finalizeChunk(chunk, "btIntIndexData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr);
 					}
 					break;
 				}
-			case PHY_SHORT:
+				case PHY_SHORT:
 				{
 					if (numtriangles)
 					{
-						btChunk* chunk = serializer->allocate(sizeof(btShortIntIndexTripletData),numtriangles);
+						btChunk* chunk = serializer->allocate(sizeof(btShortIntIndexTripletData), numtriangles);
 						btShortIntIndexTripletData* tmpIndices = (btShortIntIndexTripletData*)chunk->m_oldPtr;
-						memPtr->m_3indices16 = (btShortIntIndexTripletData*) serializer->getUniquePointer(tmpIndices);
-						for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+						memPtr->m_3indices16 = (btShortIntIndexTripletData*)serializer->getUniquePointer(tmpIndices);
+						for (gfxindex = 0; gfxindex < numtriangles; gfxindex++)
 						{
-							unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride);
+							unsigned short int* tri_indices = (unsigned short int*)(indexbase + gfxindex * indexstride);
 							tmpIndices[gfxindex].m_values[0] = tri_indices[0];
 							tmpIndices[gfxindex].m_values[1] = tri_indices[1];
 							tmpIndices[gfxindex].m_values[2] = tri_indices[2];
+							// Fill padding with zeros to appease msan.
+							tmpIndices[gfxindex].m_pad[0] = 0;
+							tmpIndices[gfxindex].m_pad[1] = 0;
 						}
-						serializer->finalizeChunk(chunk,"btShortIntIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr);
+						serializer->finalizeChunk(chunk, "btShortIntIndexTripletData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr);
 					}
 					break;
 				}
@@ -302,21 +297,23 @@ const char*	btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s
 				{
 					if (numtriangles)
 					{
-						btChunk* chunk = serializer->allocate(sizeof(btCharIndexTripletData),numtriangles);
+						btChunk* chunk = serializer->allocate(sizeof(btCharIndexTripletData), numtriangles);
 						btCharIndexTripletData* tmpIndices = (btCharIndexTripletData*)chunk->m_oldPtr;
-						memPtr->m_3indices8 = (btCharIndexTripletData*) serializer->getUniquePointer(tmpIndices);
-						for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+						memPtr->m_3indices8 = (btCharIndexTripletData*)serializer->getUniquePointer(tmpIndices);
+						for (gfxindex = 0; gfxindex < numtriangles; gfxindex++)
 						{
-							unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride);
+							unsigned char* tri_indices = (unsigned char*)(indexbase + gfxindex * indexstride);
 							tmpIndices[gfxindex].m_values[0] = tri_indices[0];
 							tmpIndices[gfxindex].m_values[1] = tri_indices[1];
 							tmpIndices[gfxindex].m_values[2] = tri_indices[2];
+							// Fill padding with zeros to appease msan.
+							tmpIndices[gfxindex].m_pad = 0;
 						}
-						serializer->finalizeChunk(chunk,"btCharIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr);
+						serializer->finalizeChunk(chunk, "btCharIndexTripletData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr);
 					}
 					break;
 				}
-			default:
+				default:
 				{
 					btAssert(0);
 					//unknown index type
@@ -325,56 +322,58 @@ const char*	btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s
 
 			switch (type)
 			{
-			case PHY_FLOAT:
-			 {
-				 float* graphicsbase;
-
-				 if (numverts)
-				 {
-					 btChunk* chunk = serializer->allocate(sizeof(btVector3FloatData),numverts);
-					 btVector3FloatData* tmpVertices = (btVector3FloatData*) chunk->m_oldPtr;
-					 memPtr->m_vertices3f = (btVector3FloatData *)serializer->getUniquePointer(tmpVertices);
-					 for (int i=0;i<numverts;i++)
-					 {
-						 graphicsbase = (float*)(vertexbase+i*stride);
-						 tmpVertices[i].m_floats[0] = graphicsbase[0];
-						 tmpVertices[i].m_floats[1] = graphicsbase[1];
-						 tmpVertices[i].m_floats[2] = graphicsbase[2];
-					 }
-					 serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr);
-				 }
-				 break;
+				case PHY_FLOAT:
+				{
+					float* graphicsbase;
+
+					if (numverts)
+					{
+						btChunk* chunk = serializer->allocate(sizeof(btVector3FloatData), numverts);
+						btVector3FloatData* tmpVertices = (btVector3FloatData*)chunk->m_oldPtr;
+						memPtr->m_vertices3f = (btVector3FloatData*)serializer->getUniquePointer(tmpVertices);
+						for (int i = 0; i < numverts; i++)
+						{
+							graphicsbase = (float*)(vertexbase + i * stride);
+							tmpVertices[i].m_floats[0] = graphicsbase[0];
+							tmpVertices[i].m_floats[1] = graphicsbase[1];
+							tmpVertices[i].m_floats[2] = graphicsbase[2];
+						}
+						serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr);
+					}
+					break;
 				}
 
-			case PHY_DOUBLE:
+				case PHY_DOUBLE:
 				{
 					if (numverts)
 					{
-						btChunk* chunk = serializer->allocate(sizeof(btVector3DoubleData),numverts);
-						btVector3DoubleData* tmpVertices = (btVector3DoubleData*) chunk->m_oldPtr;
-						memPtr->m_vertices3d = (btVector3DoubleData *) serializer->getUniquePointer(tmpVertices);
-						for (int i=0;i<numverts;i++)
-					 {
-						 double* graphicsbase = (double*)(vertexbase+i*stride);//for now convert to float, might leave it at double
-						 tmpVertices[i].m_floats[0] = graphicsbase[0];
-						 tmpVertices[i].m_floats[1] = graphicsbase[1];
-						 tmpVertices[i].m_floats[2] = graphicsbase[2];
-					 }
-						serializer->finalizeChunk(chunk,"btVector3DoubleData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr);
+						btChunk* chunk = serializer->allocate(sizeof(btVector3DoubleData), numverts);
+						btVector3DoubleData* tmpVertices = (btVector3DoubleData*)chunk->m_oldPtr;
+						memPtr->m_vertices3d = (btVector3DoubleData*)serializer->getUniquePointer(tmpVertices);
+						for (int i = 0; i < numverts; i++)
+						{
+							double* graphicsbase = (double*)(vertexbase + i * stride);  //for now convert to float, might leave it at double
+							tmpVertices[i].m_floats[0] = graphicsbase[0];
+							tmpVertices[i].m_floats[1] = graphicsbase[1];
+							tmpVertices[i].m_floats[2] = graphicsbase[2];
+						}
+						serializer->finalizeChunk(chunk, "btVector3DoubleData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr);
 					}
 					break;
 				}
 
-			default:
-				btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE));
+				default:
+					btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE));
 			}
 
 			unLockReadOnlyVertexBase(part);
 		}
 
-		serializer->finalizeChunk(chunk,"btMeshPartData",BT_ARRAY_CODE,chunk->m_oldPtr);
+		serializer->finalizeChunk(chunk, "btMeshPartData", BT_ARRAY_CODE, chunk->m_oldPtr);
 	}
 
+	// Fill padding with zeros to appease msan.
+	memset(trimeshData->m_padding, 0, sizeof(trimeshData->m_padding));
 
 	m_scaling.serializeFloat(trimeshData->m_scaling);
 	return "btStridingMeshInterfaceData";
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h
index 9fbe139768d..68a41dfb450 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h
@@ -20,110 +20,102 @@ subject to the following restrictions:
 #include "btTriangleCallback.h"
 #include "btConcaveShape.h"
 
-
-
-
-
 ///	The btStridingMeshInterface is the interface class for high performance generic access to triangle meshes, used in combination with btBvhTriangleMeshShape and some other collision shapes.
 /// Using index striding of 3*sizeof(integer) it can use triangle arrays, using index striding of 1*sizeof(integer) it can handle triangle strips.
 /// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory.
-ATTRIBUTE_ALIGNED16(class ) btStridingMeshInterface
+ATTRIBUTE_ALIGNED16(class)
+btStridingMeshInterface
 {
-	protected:
-	
-		btVector3 m_scaling;
-
-	public:
-		BT_DECLARE_ALIGNED_ALLOCATOR();
-		
-		btStridingMeshInterface() :m_scaling(btScalar(1.),btScalar(1.),btScalar(1.))
-		{
-
-		}
-
-		virtual ~btStridingMeshInterface();
-
-
-
-		virtual void	InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
-
-		///brute force method to calculate aabb
-		void	calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax);
-
-		/// get read and write access to a subpart of a triangle mesh
-		/// this subpart has a continuous array of vertices and indices
-		/// in this way the mesh can be handled as chunks of memory with striding
-		/// very similar to OpenGL vertexarray support
-		/// make a call to unLockVertexBase when the read and write access is finished	
-		virtual void	getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0)=0;
-		
-		virtual void	getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const=0;
-	
-		/// unLockVertexBase finishes the access to a subpart of the triangle mesh
-		/// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
-		virtual void	unLockVertexBase(int subpart)=0;
-
-		virtual void	unLockReadOnlyVertexBase(int subpart) const=0;
-
-
-		/// getNumSubParts returns the number of seperate subparts
-		/// each subpart has a continuous array of vertices and indices
-		virtual int		getNumSubParts() const=0;
-
-		virtual void	preallocateVertices(int numverts)=0;
-		virtual void	preallocateIndices(int numindices)=0;
-
-		virtual bool	hasPremadeAabb() const { return false; }
-		virtual void	setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const
-                {
-                        (void) aabbMin;
-                        (void) aabbMax;
-                }
-		virtual void	getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const
-        {
-            (void) aabbMin;
-            (void) aabbMax;
-        }
-
-		const btVector3&	getScaling() const {
-			return m_scaling;
-		}
-		void	setScaling(const btVector3& scaling)
-		{
-			m_scaling = scaling;
-		}
-
-		virtual	int	calculateSerializeBufferSize() const;
-
-		///fills the dataBuffer and returns the struct name (and 0 on failure)
-		virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
-
-
+protected:
+	btVector3 m_scaling;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btStridingMeshInterface() : m_scaling(btScalar(1.), btScalar(1.), btScalar(1.))
+	{
+	}
+
+	virtual ~btStridingMeshInterface();
+
+	virtual void InternalProcessAllTriangles(btInternalTriangleIndexCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const;
+
+	///brute force method to calculate aabb
+	void calculateAabbBruteForce(btVector3 & aabbMin, btVector3 & aabbMax);
+
+	/// get read and write access to a subpart of a triangle mesh
+	/// this subpart has a continuous array of vertices and indices
+	/// in this way the mesh can be handled as chunks of memory with striding
+	/// very similar to OpenGL vertexarray support
+	/// make a call to unLockVertexBase when the read and write access is finished
+	virtual void getLockedVertexIndexBase(unsigned char** vertexbase, int& numverts, PHY_ScalarType& type, int& stride, unsigned char** indexbase, int& indexstride, int& numfaces, PHY_ScalarType& indicestype, int subpart = 0) = 0;
+
+	virtual void getLockedReadOnlyVertexIndexBase(const unsigned char** vertexbase, int& numverts, PHY_ScalarType& type, int& stride, const unsigned char** indexbase, int& indexstride, int& numfaces, PHY_ScalarType& indicestype, int subpart = 0) const = 0;
+
+	/// unLockVertexBase finishes the access to a subpart of the triangle mesh
+	/// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
+	virtual void unLockVertexBase(int subpart) = 0;
+
+	virtual void unLockReadOnlyVertexBase(int subpart) const = 0;
+
+	/// getNumSubParts returns the number of separate subparts
+	/// each subpart has a continuous array of vertices and indices
+	virtual int getNumSubParts() const = 0;
+
+	virtual void preallocateVertices(int numverts) = 0;
+	virtual void preallocateIndices(int numindices) = 0;
+
+	virtual bool hasPremadeAabb() const { return false; }
+	virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax) const
+	{
+		(void)aabbMin;
+		(void)aabbMax;
+	}
+	virtual void getPremadeAabb(btVector3 * aabbMin, btVector3 * aabbMax) const
+	{
+		(void)aabbMin;
+		(void)aabbMax;
+	}
+
+	const btVector3& getScaling() const
+	{
+		return m_scaling;
+	}
+	void setScaling(const btVector3& scaling)
+	{
+		m_scaling = scaling;
+	}
+
+	virtual int calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
-struct	btIntIndexData
+struct btIntIndexData
 {
-	int	m_value;
+	int m_value;
 };
 
-struct	btShortIntIndexData
+struct btShortIntIndexData
 {
 	short m_value;
 	char m_pad[2];
 };
 
-struct	btShortIntIndexTripletData
+struct btShortIntIndexTripletData
 {
-	short	m_values[3];
-	char	m_pad[2];
+	short m_values[3];
+	char m_pad[2];
 };
 
-struct	btCharIndexTripletData
+struct btCharIndexTripletData
 {
 	unsigned char m_values[3];
-	char	m_pad;
+	char m_pad;
 };
 
+// clang-format off
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btMeshPartData
@@ -151,14 +143,11 @@ struct	btStridingMeshInterfaceData
 	char m_padding[4];
 };
 
+// clang-format on
 
-
-
-SIMD_FORCE_INLINE	int	btStridingMeshInterface::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btStridingMeshInterface::calculateSerializeBufferSize() const
 {
 	return sizeof(btStridingMeshInterfaceData);
 }
 
-
-
-#endif //BT_STRIDING_MESHINTERFACE_H
+#endif  //BT_STRIDING_MESHINTERFACE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp
index 52f346bf726..c4d33c429f0 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp
@@ -16,29 +16,29 @@ subject to the following restrictions:
 #include "btTetrahedronShape.h"
 #include "LinearMath/btMatrix3x3.h"
 
-btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexAabbCachingShape (),
-m_numVertices(0)
+btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexAabbCachingShape(),
+									   m_numVertices(0)
 {
 	m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
 }
 
-btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0) : btPolyhedralConvexAabbCachingShape (),
-m_numVertices(0)
+btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0) : btPolyhedralConvexAabbCachingShape(),
+														   m_numVertices(0)
 {
 	m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
 	addVertex(pt0);
 }
 
-btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1) : btPolyhedralConvexAabbCachingShape (),
-m_numVertices(0)
+btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1) : btPolyhedralConvexAabbCachingShape(),
+																				 m_numVertices(0)
 {
 	m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
 	addVertex(pt0);
 	addVertex(pt1);
 }
 
-btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2) : btPolyhedralConvexAabbCachingShape (),
-m_numVertices(0)
+btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1, const btVector3& pt2) : btPolyhedralConvexAabbCachingShape(),
+																									   m_numVertices(0)
 {
 	m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
 	addVertex(pt0);
@@ -46,8 +46,8 @@ m_numVertices(0)
 	addVertex(pt2);
 }
 
-btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3) : btPolyhedralConvexAabbCachingShape (),
-m_numVertices(0)
+btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1, const btVector3& pt2, const btVector3& pt3) : btPolyhedralConvexAabbCachingShape(),
+																															 m_numVertices(0)
 {
 	m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
 	addVertex(pt0);
@@ -56,17 +56,16 @@ m_numVertices(0)
 	addVertex(pt3);
 }
 
-
-void btBU_Simplex1to4::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+void btBU_Simplex1to4::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 {
 #if 1
-	btPolyhedralConvexAabbCachingShape::getAabb(t,aabbMin,aabbMax);
+	btPolyhedralConvexAabbCachingShape::getAabb(t, aabbMin, aabbMax);
 #else
-	aabbMin.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
-	aabbMax.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
+	aabbMin.setValue(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
+	aabbMax.setValue(-BT_LARGE_FLOAT, -BT_LARGE_FLOAT, -BT_LARGE_FLOAT);
 
 	//just transform the vertices in worldspace, and take their AABB
-	for (int i=0;i<m_numVertices;i++)
+	for (int i = 0; i < m_numVertices; i++)
 	{
 		btVector3 worldVertex = t(m_vertices[i]);
 		aabbMin.setMin(worldVertex);
@@ -75,18 +74,13 @@ void btBU_Simplex1to4::getAabb(const btTransform& t,btVector3& aabbMin,btVector3
 #endif
 }
 
-
-
-
-
 void btBU_Simplex1to4::addVertex(const btVector3& pt)
 {
 	m_vertices[m_numVertices++] = pt;
 	recalcLocalAabb();
 }
 
-
-int	btBU_Simplex1to4::getNumVertices() const
+int btBU_Simplex1to4::getNumVertices() const
 {
 	return m_numVertices;
 }
@@ -97,122 +91,113 @@ int btBU_Simplex1to4::getNumEdges() const
 
 	switch (m_numVertices)
 	{
-	case 0:
-		return 0;
-	case 1: return 0;
-	case 2: return 1;
-	case 3: return 3;
-	case 4: return 6;
-
-
+		case 0:
+			return 0;
+		case 1:
+			return 0;
+		case 2:
+			return 1;
+		case 3:
+			return 3;
+		case 4:
+			return 6;
 	}
 
 	return 0;
 }
 
-void btBU_Simplex1to4::getEdge(int i,btVector3& pa,btVector3& pb) const
+void btBU_Simplex1to4::getEdge(int i, btVector3& pa, btVector3& pb) const
 {
-	
-    switch (m_numVertices)
+	switch (m_numVertices)
 	{
-
-	case 2: 
-		pa = m_vertices[0];
-		pb = m_vertices[1];
-		break;
-	case 3:  
-		switch (i)
-		{
-		case 0:
-			pa = m_vertices[0];
-			pb = m_vertices[1];
-			break;
-		case 1:
-			pa = m_vertices[1];
-			pb = m_vertices[2];
-			break;
 		case 2:
-			pa = m_vertices[2];
-			pb = m_vertices[0];
-			break;
-
-		}
-		break;
-	case 4: 
-		switch (i)
-		{
-		case 0:
 			pa = m_vertices[0];
 			pb = m_vertices[1];
 			break;
-		case 1:
-			pa = m_vertices[1];
-			pb = m_vertices[2];
-			break;
-		case 2:
-			pa = m_vertices[2];
-			pb = m_vertices[0];
-			break;
 		case 3:
-			pa = m_vertices[0];
-			pb = m_vertices[3];
+			switch (i)
+			{
+				case 0:
+					pa = m_vertices[0];
+					pb = m_vertices[1];
+					break;
+				case 1:
+					pa = m_vertices[1];
+					pb = m_vertices[2];
+					break;
+				case 2:
+					pa = m_vertices[2];
+					pb = m_vertices[0];
+					break;
+			}
 			break;
 		case 4:
-			pa = m_vertices[1];
-			pb = m_vertices[3];
-			break;
-		case 5:
-			pa = m_vertices[2];
-			pb = m_vertices[3];
-			break;
-		}
-
+			switch (i)
+			{
+				case 0:
+					pa = m_vertices[0];
+					pb = m_vertices[1];
+					break;
+				case 1:
+					pa = m_vertices[1];
+					pb = m_vertices[2];
+					break;
+				case 2:
+					pa = m_vertices[2];
+					pb = m_vertices[0];
+					break;
+				case 3:
+					pa = m_vertices[0];
+					pb = m_vertices[3];
+					break;
+				case 4:
+					pa = m_vertices[1];
+					pb = m_vertices[3];
+					break;
+				case 5:
+					pa = m_vertices[2];
+					pb = m_vertices[3];
+					break;
+			}
 	}
-
-
-
-
 }
 
-void btBU_Simplex1to4::getVertex(int i,btVector3& vtx) const
+void btBU_Simplex1to4::getVertex(int i, btVector3& vtx) const
 {
 	vtx = m_vertices[i];
 }
 
-int	btBU_Simplex1to4::getNumPlanes() const
+int btBU_Simplex1to4::getNumPlanes() const
 {
 	switch (m_numVertices)
 	{
-	case 0:
+		case 0:
 			return 0;
-	case 1:
+		case 1:
 			return 0;
-	case 2:
+		case 2:
 			return 0;
-	case 3:
+		case 3:
 			return 2;
-	case 4:
+		case 4:
 			return 4;
-	default:
+		default:
 		{
 		}
 	}
 	return 0;
 }
 
-
-void btBU_Simplex1to4::getPlane(btVector3&, btVector3& ,int ) const
+void btBU_Simplex1to4::getPlane(btVector3&, btVector3&, int) const
 {
-	
 }
 
-int btBU_Simplex1to4::getIndex(int ) const
+int btBU_Simplex1to4::getIndex(int) const
 {
 	return 0;
 }
 
-bool btBU_Simplex1to4::isInside(const btVector3& ,btScalar ) const
+bool btBU_Simplex1to4::isInside(const btVector3&, btScalar) const
 {
 	return false;
 }
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h
index b69209835ef..f5e2209ed09 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h
@@ -16,61 +16,56 @@ subject to the following restrictions:
 #ifndef BT_SIMPLEX_1TO4_SHAPE
 #define BT_SIMPLEX_1TO4_SHAPE
 
-
 #include "btPolyhedralConvexShape.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 
-
 ///The btBU_Simplex1to4 implements tetrahedron, triangle, line, vertex collision shapes. In most cases it is better to use btConvexHullShape instead.
-ATTRIBUTE_ALIGNED16(class) btBU_Simplex1to4 : public btPolyhedralConvexAabbCachingShape
+ATTRIBUTE_ALIGNED16(class)
+btBU_Simplex1to4 : public btPolyhedralConvexAabbCachingShape
 {
 protected:
-
-	int	m_numVertices;
-	btVector3	m_vertices[4];
+	int m_numVertices;
+	btVector3 m_vertices[4];
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	btBU_Simplex1to4();
 
 	btBU_Simplex1to4(const btVector3& pt0);
-	btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1);
-	btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2);
-	btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3);
+	btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1);
+	btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1, const btVector3& pt2);
+	btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1, const btVector3& pt2, const btVector3& pt3);
 
-    
-	void	reset()
+	void reset()
 	{
 		m_numVertices = 0;
 	}
-	
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
 	void addVertex(const btVector3& pt);
 
 	//PolyhedralConvexShape interface
 
-	virtual int	getNumVertices() const;
+	virtual int getNumVertices() const;
 
 	virtual int getNumEdges() const;
 
-	virtual void getEdge(int i,btVector3& pa,btVector3& pb) const;
-	
-	virtual void getVertex(int i,btVector3& vtx) const;
+	virtual void getEdge(int i, btVector3& pa, btVector3& pb) const;
 
-	virtual int	getNumPlanes() const;
+	virtual void getVertex(int i, btVector3& vtx) const;
 
-	virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i) const;
+	virtual int getNumPlanes() const;
 
-	virtual int getIndex(int i) const;
+	virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const;
 
-	virtual	bool isInside(const btVector3& pt,btScalar tolerance) const;
+	virtual int getIndex(int i) const;
 
+	virtual bool isInside(const btVector3& pt, btScalar tolerance) const;
 
 	///getName is for debugging
-	virtual const char*	getName()const { return "btBU_Simplex1to4";}
-
+	virtual const char* getName() const { return "btBU_Simplex1to4"; }
 };
 
-#endif //BT_SIMPLEX_1TO4_SHAPE
+#endif  //BT_SIMPLEX_1TO4_SHAPE
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp
index 3027e65b256..3b6db2b39fa 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp
@@ -15,21 +15,14 @@ subject to the following restrictions:
 
 #include "btTriangleBuffer.h"
 
-
-
-
-
-
-
-void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int  triangleIndex)
+void btTriangleBuffer::processTriangle(btVector3* triangle, int partId, int triangleIndex)
 {
-		btTriangle	tri;
-		tri.m_vertex0 = triangle[0];
-		tri.m_vertex1 = triangle[1];
-		tri.m_vertex2 = triangle[2];
-		tri.m_partId = partId;
-		tri.m_triangleIndex = triangleIndex;
-			
-		m_triangleBuffer.push_back(tri);
+	btTriangle tri;
+	tri.m_vertex0 = triangle[0];
+	tri.m_vertex1 = triangle[1];
+	tri.m_vertex2 = triangle[2];
+	tri.m_partId = partId;
+	tri.m_triangleIndex = triangleIndex;
+
+	m_triangleBuffer.push_back(tri);
 }
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h
index b71fc8b3746..a89b9cd8a48 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h
@@ -19,13 +19,13 @@ subject to the following restrictions:
 #include "btTriangleCallback.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
-struct	btTriangle
+struct btTriangle
 {
-	btVector3	m_vertex0;
-	btVector3	m_vertex1;
-	btVector3	m_vertex2;
-	int	m_partId;
-	int	m_triangleIndex;
+	btVector3 m_vertex0;
+	btVector3 m_vertex1;
+	btVector3 m_vertex2;
+	int m_partId;
+	int m_triangleIndex;
 };
 
 ///The btTriangleBuffer callback can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles'
@@ -39,31 +39,25 @@ struct	btTriangle
 ///			}
 class btTriangleBuffer : public btTriangleCallback
 {
+	btAlignedObjectArray<btTriangle> m_triangleBuffer;
 
-	btAlignedObjectArray<btTriangle>	m_triangleBuffer;
-	
 public:
-
-
 	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
-	
-	int	getNumTriangles() const
+
+	int getNumTriangles() const
 	{
 		return int(m_triangleBuffer.size());
 	}
-	
-	const btTriangle&	getTriangle(int index) const
+
+	const btTriangle& getTriangle(int index) const
 	{
 		return m_triangleBuffer[index];
 	}
 
-	void	clearBuffer()
+	void clearBuffer()
 	{
 		m_triangleBuffer.clear();
 	}
-	
 };
 
-
-#endif //BT_TRIANGLE_BUFFER_H
-
+#endif  //BT_TRIANGLE_BUFFER_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp
index f558bf6d241..5bd2c595fed 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp
@@ -17,12 +17,8 @@ subject to the following restrictions:
 
 btTriangleCallback::~btTriangleCallback()
 {
-
 }
 
-
 btInternalTriangleIndexCallback::~btInternalTriangleIndexCallback()
 {
-
 }
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h
index 461c57f8773..d3644891ee6 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h
@@ -18,13 +18,11 @@ subject to the following restrictions:
 
 #include "LinearMath/btVector3.h"
 
-
 ///The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTriangles.
 ///This callback is called by processAllTriangles for all btConcaveShape derived class, such as  btBvhTriangleMeshShape, btStaticPlaneShape and btHeightfieldTerrainShape.
 class btTriangleCallback
 {
 public:
-
 	virtual ~btTriangleCallback();
 	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) = 0;
 };
@@ -32,11 +30,8 @@ public:
 class btInternalTriangleIndexCallback
 {
 public:
-
 	virtual ~btInternalTriangleIndexCallback();
-	virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex) = 0;
+	virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) = 0;
 };
 
-
-
-#endif //BT_TRIANGLE_CALLBACK_H
+#endif  //BT_TRIANGLE_CALLBACK_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp
index a665024cb61..dae42551941 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp
@@ -15,81 +15,76 @@ subject to the following restrictions:
 
 #include "btTriangleIndexVertexArray.h"
 
-btTriangleIndexVertexArray::btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride)
-: m_hasAabb(0)
+btTriangleIndexVertexArray::btTriangleIndexVertexArray(int numTriangles, int* triangleIndexBase, int triangleIndexStride, int numVertices, btScalar* vertexBase, int vertexStride)
+	: m_hasAabb(0)
 {
 	btIndexedMesh mesh;
 
 	mesh.m_numTriangles = numTriangles;
-	mesh.m_triangleIndexBase = (const unsigned char *)triangleIndexBase;
+	mesh.m_triangleIndexBase = (const unsigned char*)triangleIndexBase;
 	mesh.m_triangleIndexStride = triangleIndexStride;
 	mesh.m_numVertices = numVertices;
-	mesh.m_vertexBase = (const unsigned char *)vertexBase;
+	mesh.m_vertexBase = (const unsigned char*)vertexBase;
 	mesh.m_vertexStride = vertexStride;
 
 	addIndexedMesh(mesh);
-
 }
 
 btTriangleIndexVertexArray::~btTriangleIndexVertexArray()
 {
-
 }
 
-void	btTriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart)
+void btTriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char** vertexbase, int& numverts, PHY_ScalarType& type, int& vertexStride, unsigned char** indexbase, int& indexstride, int& numfaces, PHY_ScalarType& indicestype, int subpart)
 {
-	btAssert(subpart< getNumSubParts() );
+	btAssert(subpart < getNumSubParts());
 
 	btIndexedMesh& mesh = m_indexedMeshes[subpart];
 
 	numverts = mesh.m_numVertices;
-	(*vertexbase) = (unsigned char *) mesh.m_vertexBase;
+	(*vertexbase) = (unsigned char*)mesh.m_vertexBase;
 
-   type = mesh.m_vertexType;
+	type = mesh.m_vertexType;
 
 	vertexStride = mesh.m_vertexStride;
 
 	numfaces = mesh.m_numTriangles;
 
-	(*indexbase) = (unsigned char *)mesh.m_triangleIndexBase;
+	(*indexbase) = (unsigned char*)mesh.m_triangleIndexBase;
 	indexstride = mesh.m_triangleIndexStride;
 	indicestype = mesh.m_indexType;
 }
 
-void	btTriangleIndexVertexArray::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const
+void btTriangleIndexVertexArray::getLockedReadOnlyVertexIndexBase(const unsigned char** vertexbase, int& numverts, PHY_ScalarType& type, int& vertexStride, const unsigned char** indexbase, int& indexstride, int& numfaces, PHY_ScalarType& indicestype, int subpart) const
 {
 	const btIndexedMesh& mesh = m_indexedMeshes[subpart];
 
 	numverts = mesh.m_numVertices;
-	(*vertexbase) = (const unsigned char *)mesh.m_vertexBase;
+	(*vertexbase) = (const unsigned char*)mesh.m_vertexBase;
+
+	type = mesh.m_vertexType;
 
-   type = mesh.m_vertexType;
-   
 	vertexStride = mesh.m_vertexStride;
 
 	numfaces = mesh.m_numTriangles;
-	(*indexbase) = (const unsigned char *)mesh.m_triangleIndexBase;
+	(*indexbase) = (const unsigned char*)mesh.m_triangleIndexBase;
 	indexstride = mesh.m_triangleIndexStride;
 	indicestype = mesh.m_indexType;
 }
 
-bool	btTriangleIndexVertexArray::hasPremadeAabb() const
+bool btTriangleIndexVertexArray::hasPremadeAabb() const
 {
 	return (m_hasAabb == 1);
 }
 
-
-void	btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const
+void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax) const
 {
 	m_aabbMin = aabbMin;
 	m_aabbMax = aabbMax;
-	m_hasAabb = 1; // this is intentionally an int see notes in header
+	m_hasAabb = 1;  // this is intentionally an int see notes in header
 }
 
-void	btTriangleIndexVertexArray::getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const
+void btTriangleIndexVertexArray::getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax) const
 {
 	*aabbMin = m_aabbMin;
 	*aabbMax = m_aabbMax;
 }
-
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h
index 9e1544e87a4..556aa3fef4d 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h
@@ -20,62 +20,59 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 #include "LinearMath/btScalar.h"
 
-
 ///The btIndexedMesh indexes a single vertex and index array. Multiple btIndexedMesh objects can be passed into a btTriangleIndexVertexArray using addIndexedMesh.
 ///Instead of the number of indices, we pass the number of triangles.
-ATTRIBUTE_ALIGNED16( struct)	btIndexedMesh
+ATTRIBUTE_ALIGNED16(struct)
+btIndexedMesh
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-   int                     m_numTriangles;
-   const unsigned char *   m_triangleIndexBase;
-   // Size in byte of the indices for one triangle (3*sizeof(index_type) if the indices are tightly packed)
-   int                     m_triangleIndexStride;
-   int                     m_numVertices;
-   const unsigned char *   m_vertexBase;
-   // Size of a vertex, in bytes
-   int                     m_vertexStride;
-
-   // The index type is set when adding an indexed mesh to the
-   // btTriangleIndexVertexArray, do not set it manually
-   PHY_ScalarType m_indexType;
-
-   // The vertex type has a default type similar to Bullet's precision mode (float or double)
-   // but can be set manually if you for example run Bullet with double precision but have
-   // mesh data in single precision..
-   PHY_ScalarType m_vertexType;
-
-
-   btIndexedMesh()
-	   :m_indexType(PHY_INTEGER),
+	int m_numTriangles;
+	const unsigned char* m_triangleIndexBase;
+	// Size in byte of the indices for one triangle (3*sizeof(index_type) if the indices are tightly packed)
+	int m_triangleIndexStride;
+	int m_numVertices;
+	const unsigned char* m_vertexBase;
+	// Size of a vertex, in bytes
+	int m_vertexStride;
+
+	// The index type is set when adding an indexed mesh to the
+	// btTriangleIndexVertexArray, do not set it manually
+	PHY_ScalarType m_indexType;
+
+	// The vertex type has a default type similar to Bullet's precision mode (float or double)
+	// but can be set manually if you for example run Bullet with double precision but have
+	// mesh data in single precision..
+	PHY_ScalarType m_vertexType;
+
+	btIndexedMesh()
+		: m_indexType(PHY_INTEGER),
 #ifdef BT_USE_DOUBLE_PRECISION
-      m_vertexType(PHY_DOUBLE)
-#else // BT_USE_DOUBLE_PRECISION
-      m_vertexType(PHY_FLOAT)
-#endif // BT_USE_DOUBLE_PRECISION
-      {
-      }
-}
-;
-
+		  m_vertexType(PHY_DOUBLE)
+#else   // BT_USE_DOUBLE_PRECISION
+		  m_vertexType(PHY_FLOAT)
+#endif  // BT_USE_DOUBLE_PRECISION
+	{
+	}
+};
 
-typedef btAlignedObjectArray<btIndexedMesh>	IndexedMeshArray;
+typedef btAlignedObjectArray<btIndexedMesh> IndexedMeshArray;
 
 ///The btTriangleIndexVertexArray allows to access multiple triangle meshes, by indexing into existing triangle/index arrays.
 ///Additional meshes can be added using addIndexedMesh
-///No duplcate is made of the vertex/index data, it only indexes into external vertex/index arrays.
+///No duplicate is made of the vertex/index data, it only indexes into external vertex/index arrays.
 ///So keep those arrays around during the lifetime of this btTriangleIndexVertexArray.
-ATTRIBUTE_ALIGNED16( class) btTriangleIndexVertexArray : public btStridingMeshInterface
+ATTRIBUTE_ALIGNED16(class)
+btTriangleIndexVertexArray : public btStridingMeshInterface
 {
 protected:
-	IndexedMeshArray	m_indexedMeshes;
+	IndexedMeshArray m_indexedMeshes;
 	int m_pad[2];
-	mutable int m_hasAabb; // using int instead of bool to maintain alignment
+	mutable int m_hasAabb;  // using int instead of bool to maintain alignment
 	mutable btVector3 m_aabbMin;
 	mutable btVector3 m_aabbMax;
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	btTriangleIndexVertexArray() : m_hasAabb(0)
@@ -85,49 +82,47 @@ public:
 	virtual ~btTriangleIndexVertexArray();
 
 	//just to be backwards compatible
-	btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride);
-	
-	void	addIndexedMesh(const btIndexedMesh& mesh, PHY_ScalarType indexType = PHY_INTEGER)
+	btTriangleIndexVertexArray(int numTriangles, int* triangleIndexBase, int triangleIndexStride, int numVertices, btScalar* vertexBase, int vertexStride);
+
+	void addIndexedMesh(const btIndexedMesh& mesh, PHY_ScalarType indexType = PHY_INTEGER)
 	{
 		m_indexedMeshes.push_back(mesh);
-		m_indexedMeshes[m_indexedMeshes.size()-1].m_indexType = indexType;
+		m_indexedMeshes[m_indexedMeshes.size() - 1].m_indexType = indexType;
 	}
-	
-	
-	virtual void	getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0);
 
-	virtual void	getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const;
+	virtual void getLockedVertexIndexBase(unsigned char** vertexbase, int& numverts, PHY_ScalarType& type, int& vertexStride, unsigned char** indexbase, int& indexstride, int& numfaces, PHY_ScalarType& indicestype, int subpart = 0);
+
+	virtual void getLockedReadOnlyVertexIndexBase(const unsigned char** vertexbase, int& numverts, PHY_ScalarType& type, int& vertexStride, const unsigned char** indexbase, int& indexstride, int& numfaces, PHY_ScalarType& indicestype, int subpart = 0) const;
 
 	/// unLockVertexBase finishes the access to a subpart of the triangle mesh
 	/// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
-	virtual void	unLockVertexBase(int subpart) {(void)subpart;}
+	virtual void unLockVertexBase(int subpart) { (void)subpart; }
 
-	virtual void	unLockReadOnlyVertexBase(int subpart) const {(void)subpart;}
+	virtual void unLockReadOnlyVertexBase(int subpart) const { (void)subpart; }
 
-	/// getNumSubParts returns the number of seperate subparts
+	/// getNumSubParts returns the number of separate subparts
 	/// each subpart has a continuous array of vertices and indices
-	virtual int		getNumSubParts() const { 
+	virtual int getNumSubParts() const
+	{
 		return (int)m_indexedMeshes.size();
 	}
 
-	IndexedMeshArray&	getIndexedMeshArray()
+	IndexedMeshArray& getIndexedMeshArray()
 	{
 		return m_indexedMeshes;
 	}
 
-	const IndexedMeshArray&	getIndexedMeshArray() const
+	const IndexedMeshArray& getIndexedMeshArray() const
 	{
 		return m_indexedMeshes;
 	}
 
-	virtual void	preallocateVertices(int numverts){(void) numverts;}
-	virtual void	preallocateIndices(int numindices){(void) numindices;}
-
-	virtual bool	hasPremadeAabb() const;
-	virtual void	setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const;
-	virtual void	getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const;
+	virtual void preallocateVertices(int numverts) { (void)numverts; }
+	virtual void preallocateIndices(int numindices) { (void)numindices; }
 
-}
-;
+	virtual bool hasPremadeAabb() const;
+	virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax) const;
+	virtual void getPremadeAabb(btVector3 * aabbMin, btVector3 * aabbMax) const;
+};
 
-#endif //BT_TRIANGLE_INDEX_VERTEX_ARRAY_H
+#endif  //BT_TRIANGLE_INDEX_VERTEX_ARRAY_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp
index dc562941ad6..4bf133d7ac7 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp
@@ -17,70 +17,68 @@ subject to the following restrictions:
 
 #include "btTriangleIndexVertexMaterialArray.h"
 
-btTriangleIndexVertexMaterialArray::btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,
-                                   int numVertices,btScalar* vertexBase,int vertexStride,
-                                   int numMaterials, unsigned char* materialBase, int materialStride,
-                                   int* triangleMaterialsBase, int materialIndexStride) :
-btTriangleIndexVertexArray(numTriangles, triangleIndexBase, triangleIndexStride, numVertices, vertexBase, vertexStride)
+btTriangleIndexVertexMaterialArray::btTriangleIndexVertexMaterialArray(int numTriangles, int* triangleIndexBase, int triangleIndexStride,
+																	   int numVertices, btScalar* vertexBase, int vertexStride,
+																	   int numMaterials, unsigned char* materialBase, int materialStride,
+																	   int* triangleMaterialsBase, int materialIndexStride) : btTriangleIndexVertexArray(numTriangles, triangleIndexBase, triangleIndexStride, numVertices, vertexBase, vertexStride)
 {
-    btMaterialProperties mat;
+	btMaterialProperties mat;
 
-    mat.m_numMaterials = numMaterials;
-    mat.m_materialBase = materialBase;
-    mat.m_materialStride = materialStride;
+	mat.m_numMaterials = numMaterials;
+	mat.m_materialBase = materialBase;
+	mat.m_materialStride = materialStride;
 #ifdef BT_USE_DOUBLE_PRECISION
-    mat.m_materialType = PHY_DOUBLE;
+	mat.m_materialType = PHY_DOUBLE;
 #else
-    mat.m_materialType = PHY_FLOAT;
+	mat.m_materialType = PHY_FLOAT;
 #endif
 
-    mat.m_numTriangles = numTriangles;
-    mat.m_triangleMaterialsBase = (unsigned char *)triangleMaterialsBase;
-    mat.m_triangleMaterialStride = materialIndexStride;
-    mat.m_triangleType = PHY_INTEGER;
+	mat.m_numTriangles = numTriangles;
+	mat.m_triangleMaterialsBase = (unsigned char*)triangleMaterialsBase;
+	mat.m_triangleMaterialStride = materialIndexStride;
+	mat.m_triangleType = PHY_INTEGER;
 
-    addMaterialProperties(mat);
+	addMaterialProperties(mat);
 }
 
-
-void btTriangleIndexVertexMaterialArray::getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
-                                   unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart)
+void btTriangleIndexVertexMaterialArray::getLockedMaterialBase(unsigned char** materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
+															   unsigned char** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart)
 {
-    btAssert(subpart< getNumSubParts() );
+	btAssert(subpart < getNumSubParts());
 
-    btMaterialProperties& mats = m_materials[subpart];
+	btMaterialProperties& mats = m_materials[subpart];
 
-    numMaterials = mats.m_numMaterials;
-    (*materialBase) = (unsigned char *) mats.m_materialBase;
+	numMaterials = mats.m_numMaterials;
+	(*materialBase) = (unsigned char*)mats.m_materialBase;
 #ifdef BT_USE_DOUBLE_PRECISION
-    materialType = PHY_DOUBLE;
+	materialType = PHY_DOUBLE;
 #else
-    materialType = PHY_FLOAT;
+	materialType = PHY_FLOAT;
 #endif
-    materialStride = mats.m_materialStride;
+	materialStride = mats.m_materialStride;
 
-    numTriangles = mats.m_numTriangles;
-    (*triangleMaterialBase) = (unsigned char *)mats.m_triangleMaterialsBase;
-    triangleMaterialStride = mats.m_triangleMaterialStride;
-    triangleType = mats.m_triangleType;
+	numTriangles = mats.m_numTriangles;
+	(*triangleMaterialBase) = (unsigned char*)mats.m_triangleMaterialsBase;
+	triangleMaterialStride = mats.m_triangleMaterialStride;
+	triangleType = mats.m_triangleType;
 }
 
-void btTriangleIndexVertexMaterialArray::getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
-                                           const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart)
+void btTriangleIndexVertexMaterialArray::getLockedReadOnlyMaterialBase(const unsigned char** materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
+																	   const unsigned char** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart)
 {
-    btMaterialProperties& mats = m_materials[subpart];
+	btMaterialProperties& mats = m_materials[subpart];
 
-    numMaterials = mats.m_numMaterials;
-    (*materialBase) = (const unsigned char *) mats.m_materialBase;
+	numMaterials = mats.m_numMaterials;
+	(*materialBase) = (const unsigned char*)mats.m_materialBase;
 #ifdef BT_USE_DOUBLE_PRECISION
-    materialType = PHY_DOUBLE;
+	materialType = PHY_DOUBLE;
 #else
-    materialType = PHY_FLOAT;
+	materialType = PHY_FLOAT;
 #endif
-    materialStride = mats.m_materialStride;
+	materialStride = mats.m_materialStride;
 
-    numTriangles = mats.m_numTriangles;
-    (*triangleMaterialBase) = (const unsigned char *)mats.m_triangleMaterialsBase;
-    triangleMaterialStride = mats.m_triangleMaterialStride;
-    triangleType = mats.m_triangleType;
+	numTriangles = mats.m_numTriangles;
+	(*triangleMaterialBase) = (const unsigned char*)mats.m_triangleMaterialsBase;
+	triangleMaterialStride = mats.m_triangleMaterialStride;
+	triangleType = mats.m_triangleType;
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h
index ba4f7b46076..315b1e21f35 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h
@@ -20,26 +20,26 @@ subject to the following restrictions:
 
 #include "btTriangleIndexVertexArray.h"
 
-
-ATTRIBUTE_ALIGNED16( struct)	btMaterialProperties
+ATTRIBUTE_ALIGNED16(struct)
+btMaterialProperties
 {
-    ///m_materialBase ==========> 2 btScalar values make up one material, friction then restitution
-    int m_numMaterials;
-    const unsigned char * m_materialBase;
-    int m_materialStride;
-    PHY_ScalarType m_materialType;
-    ///m_numTriangles <=========== This exists in the btIndexedMesh object for the same subpart, but since we're
-    ///                           padding the structure, it can be reproduced at no real cost
-    ///m_triangleMaterials =====> 1 integer value makes up one entry
-    ///                           eg: m_triangleMaterials[1] = 5; // This will set triangle 2 to use material 5
-    int m_numTriangles; 
-    const unsigned char * m_triangleMaterialsBase;
-    int m_triangleMaterialStride;
-    ///m_triangleType <========== Automatically set in addMaterialProperties
-    PHY_ScalarType m_triangleType;
+	///m_materialBase ==========> 2 btScalar values make up one material, friction then restitution
+	int m_numMaterials;
+	const unsigned char* m_materialBase;
+	int m_materialStride;
+	PHY_ScalarType m_materialType;
+	///m_numTriangles <=========== This exists in the btIndexedMesh object for the same subpart, but since we're
+	///                           padding the structure, it can be reproduced at no real cost
+	///m_triangleMaterials =====> 1 integer value makes up one entry
+	///                           eg: m_triangleMaterials[1] = 5; // This will set triangle 2 to use material 5
+	int m_numTriangles;
+	const unsigned char* m_triangleMaterialsBase;
+	int m_triangleMaterialStride;
+	///m_triangleType <========== Automatically set in addMaterialProperties
+	PHY_ScalarType m_triangleType;
 };
 
-typedef btAlignedObjectArray<btMaterialProperties>	MaterialArray;
+typedef btAlignedObjectArray<btMaterialProperties> MaterialArray;
 
 ///Teh btTriangleIndexVertexMaterialArray is built on TriangleIndexVertexArray
 ///The addition of a material array allows for the utilization of the partID and
@@ -47,38 +47,37 @@ typedef btAlignedObjectArray<btMaterialProperties>	MaterialArray;
 ///TriangleIndexVertexArray, no duplicate is made of the material data, so it
 ///is the users responsibility to maintain the array during the lifetime of the
 ///TriangleIndexVertexMaterialArray.
-ATTRIBUTE_ALIGNED16(class) btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray
+ATTRIBUTE_ALIGNED16(class)
+btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray
 {
 protected:
-    MaterialArray       m_materials;
-		
+	MaterialArray m_materials;
+
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-    btTriangleIndexVertexMaterialArray()
+	btTriangleIndexVertexMaterialArray()
 	{
 	}
 
-    btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,
-        int numVertices,btScalar* vertexBase,int vertexStride,
-        int numMaterials, unsigned char* materialBase, int materialStride,
-        int* triangleMaterialsBase, int materialIndexStride);
-
-    virtual ~btTriangleIndexVertexMaterialArray() {}
+	btTriangleIndexVertexMaterialArray(int numTriangles, int* triangleIndexBase, int triangleIndexStride,
+									   int numVertices, btScalar* vertexBase, int vertexStride,
+									   int numMaterials, unsigned char* materialBase, int materialStride,
+									   int* triangleMaterialsBase, int materialIndexStride);
 
-    void	addMaterialProperties(const btMaterialProperties& mat, PHY_ScalarType triangleType = PHY_INTEGER)
-    {
-        m_materials.push_back(mat);
-        m_materials[m_materials.size()-1].m_triangleType = triangleType;
-    }
+	virtual ~btTriangleIndexVertexMaterialArray() {}
 
-    virtual void getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
-        unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType ,int subpart = 0);
+	void addMaterialProperties(const btMaterialProperties& mat, PHY_ScalarType triangleType = PHY_INTEGER)
+	{
+		m_materials.push_back(mat);
+		m_materials[m_materials.size() - 1].m_triangleType = triangleType;
+	}
 
-    virtual void getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
-        const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0);
+	virtual void getLockedMaterialBase(unsigned char** materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
+									   unsigned char** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0);
 
-}
-;
+	virtual void getLockedReadOnlyMaterialBase(const unsigned char** materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
+											   const unsigned char** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0);
+};
 
-#endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H
+#endif  //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h
index 17deef89d37..8ee35ef5faf 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h
@@ -16,11 +16,9 @@ subject to the following restrictions:
 #ifndef _BT_TRIANGLE_INFO_MAP_H
 #define _BT_TRIANGLE_INFO_MAP_H
 
-
 #include "LinearMath/btHashMap.h"
 #include "LinearMath/btSerializer.h"
 
-
 ///for btTriangleInfo m_flags
 #define TRI_INFO_V0V1_CONVEX 1
 #define TRI_INFO_V1V2_CONVEX 2
@@ -30,61 +28,58 @@ subject to the following restrictions:
 #define TRI_INFO_V1V2_SWAP_NORMALB 16
 #define TRI_INFO_V2V0_SWAP_NORMALB 32
 
-
 ///The btTriangleInfo structure stores information to adjust collision normals to avoid collisions against internal edges
-///it can be generated using 
-struct	btTriangleInfo
+///it can be generated using
+struct btTriangleInfo
 {
 	btTriangleInfo()
 	{
 		m_edgeV0V1Angle = SIMD_2_PI;
 		m_edgeV1V2Angle = SIMD_2_PI;
 		m_edgeV2V0Angle = SIMD_2_PI;
-		m_flags=0;
+		m_flags = 0;
 	}
 
-	int			m_flags;
-
-	btScalar	m_edgeV0V1Angle;
-	btScalar	m_edgeV1V2Angle;
-	btScalar	m_edgeV2V0Angle;
+	int m_flags;
 
+	btScalar m_edgeV0V1Angle;
+	btScalar m_edgeV1V2Angle;
+	btScalar m_edgeV2V0Angle;
 };
 
-typedef btHashMap<btHashInt,btTriangleInfo> btInternalTriangleInfoMap;
-
+typedef btHashMap<btHashInt, btTriangleInfo> btInternalTriangleInfoMap;
 
 ///The btTriangleInfoMap stores edge angle information for some triangles. You can compute this information yourself or using btGenerateInternalEdgeInfo.
-struct	btTriangleInfoMap : public btInternalTriangleInfoMap
+struct btTriangleInfoMap : public btInternalTriangleInfoMap
 {
-	btScalar	m_convexEpsilon;///used to determine if an edge or contact normal is convex, using the dot product
-	btScalar	m_planarEpsilon; ///used to determine if a triangle edge is planar with zero angle
-	btScalar	m_equalVertexThreshold; ///used to compute connectivity: if the distance between two vertices is smaller than m_equalVertexThreshold, they are considered to be 'shared'
-	btScalar	m_edgeDistanceThreshold; ///used to determine edge contacts: if the closest distance between a contact point and an edge is smaller than this distance threshold it is considered to "hit the edge"
-	btScalar	m_maxEdgeAngleThreshold; //ignore edges that connect triangles at an angle larger than this m_maxEdgeAngleThreshold
-	btScalar	m_zeroAreaThreshold; ///used to determine if a triangle is degenerate (length squared of cross product of 2 triangle edges < threshold)
-	
-	
+	btScalar m_convexEpsilon;          ///used to determine if an edge or contact normal is convex, using the dot product
+	btScalar m_planarEpsilon;          ///used to determine if a triangle edge is planar with zero angle
+	btScalar m_equalVertexThreshold;   ///used to compute connectivity: if the distance between two vertices is smaller than m_equalVertexThreshold, they are considered to be 'shared'
+	btScalar m_edgeDistanceThreshold;  ///used to determine edge contacts: if the closest distance between a contact point and an edge is smaller than this distance threshold it is considered to "hit the edge"
+	btScalar m_maxEdgeAngleThreshold;  //ignore edges that connect triangles at an angle larger than this m_maxEdgeAngleThreshold
+	btScalar m_zeroAreaThreshold;      ///used to determine if a triangle is degenerate (length squared of cross product of 2 triangle edges < threshold)
+
 	btTriangleInfoMap()
 	{
 		m_convexEpsilon = 0.00f;
 		m_planarEpsilon = 0.0001f;
-		m_equalVertexThreshold = btScalar(0.0001)*btScalar(0.0001);
+		m_equalVertexThreshold = btScalar(0.0001) * btScalar(0.0001);
 		m_edgeDistanceThreshold = btScalar(0.1);
-		m_zeroAreaThreshold = btScalar(0.0001)*btScalar(0.0001);
+		m_zeroAreaThreshold = btScalar(0.0001) * btScalar(0.0001);
 		m_maxEdgeAngleThreshold = SIMD_2_PI;
 	}
 	virtual ~btTriangleInfoMap() {}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
-	void	deSerialize(struct btTriangleInfoMapData& data);
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 
+	void deSerialize(struct btTriangleInfoMapData& data);
 };
 
+// clang-format off
+
 ///those fields have to be float and not btScalar for the serialization to work properly
 struct	btTriangleInfoData
 {
@@ -114,128 +109,130 @@ struct	btTriangleInfoMapData
 	char	m_padding[4];
 };
 
-SIMD_FORCE_INLINE	int	btTriangleInfoMap::calculateSerializeBufferSize() const
+// clang-format on
+
+SIMD_FORCE_INLINE int btTriangleInfoMap::calculateSerializeBufferSize() const
 {
 	return sizeof(btTriangleInfoMapData);
 }
 
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btTriangleInfoMap::serialize(void* dataBuffer, btSerializer* serializer) const
+SIMD_FORCE_INLINE const char* btTriangleInfoMap::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btTriangleInfoMapData* tmapData = (btTriangleInfoMapData*) dataBuffer;
+	btTriangleInfoMapData* tmapData = (btTriangleInfoMapData*)dataBuffer;
 	tmapData->m_convexEpsilon = (float)m_convexEpsilon;
 	tmapData->m_planarEpsilon = (float)m_planarEpsilon;
-	tmapData->m_equalVertexThreshold =(float) m_equalVertexThreshold;
+	tmapData->m_equalVertexThreshold = (float)m_equalVertexThreshold;
 	tmapData->m_edgeDistanceThreshold = (float)m_edgeDistanceThreshold;
 	tmapData->m_zeroAreaThreshold = (float)m_zeroAreaThreshold;
-	
+
 	tmapData->m_hashTableSize = m_hashTable.size();
 
 	tmapData->m_hashTablePtr = tmapData->m_hashTableSize ? (int*)serializer->getUniquePointer((void*)&m_hashTable[0]) : 0;
 	if (tmapData->m_hashTablePtr)
-	{ 
+	{
 		//serialize an int buffer
 		int sz = sizeof(int);
 		int numElem = tmapData->m_hashTableSize;
-		btChunk* chunk = serializer->allocate(sz,numElem);
+		btChunk* chunk = serializer->allocate(sz, numElem);
 		int* memPtr = (int*)chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
 			*memPtr = m_hashTable[i];
 		}
-		serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_hashTable[0]);
-
+		serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_hashTable[0]);
 	}
 
 	tmapData->m_nextSize = m_next.size();
-	tmapData->m_nextPtr = tmapData->m_nextSize? (int*)serializer->getUniquePointer((void*)&m_next[0]): 0;
+	tmapData->m_nextPtr = tmapData->m_nextSize ? (int*)serializer->getUniquePointer((void*)&m_next[0]) : 0;
 	if (tmapData->m_nextPtr)
 	{
 		int sz = sizeof(int);
 		int numElem = tmapData->m_nextSize;
-		btChunk* chunk = serializer->allocate(sz,numElem);
+		btChunk* chunk = serializer->allocate(sz, numElem);
 		int* memPtr = (int*)chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
 			*memPtr = m_next[i];
 		}
-		serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_next[0]);
+		serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_next[0]);
 	}
-	
+
 	tmapData->m_numValues = m_valueArray.size();
-	tmapData->m_valueArrayPtr = tmapData->m_numValues ? (btTriangleInfoData*)serializer->getUniquePointer((void*)&m_valueArray[0]): 0;
+	tmapData->m_valueArrayPtr = tmapData->m_numValues ? (btTriangleInfoData*)serializer->getUniquePointer((void*)&m_valueArray[0]) : 0;
 	if (tmapData->m_valueArrayPtr)
 	{
 		int sz = sizeof(btTriangleInfoData);
 		int numElem = tmapData->m_numValues;
-		btChunk* chunk = serializer->allocate(sz,numElem);
+		btChunk* chunk = serializer->allocate(sz, numElem);
 		btTriangleInfoData* memPtr = (btTriangleInfoData*)chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
 			memPtr->m_edgeV0V1Angle = (float)m_valueArray[i].m_edgeV0V1Angle;
 			memPtr->m_edgeV1V2Angle = (float)m_valueArray[i].m_edgeV1V2Angle;
 			memPtr->m_edgeV2V0Angle = (float)m_valueArray[i].m_edgeV2V0Angle;
 			memPtr->m_flags = m_valueArray[i].m_flags;
 		}
-		serializer->finalizeChunk(chunk,"btTriangleInfoData",BT_ARRAY_CODE,(void*) &m_valueArray[0]);
+		serializer->finalizeChunk(chunk, "btTriangleInfoData", BT_ARRAY_CODE, (void*)&m_valueArray[0]);
 	}
-	
+
 	tmapData->m_numKeys = m_keyArray.size();
 	tmapData->m_keyArrayPtr = tmapData->m_numKeys ? (int*)serializer->getUniquePointer((void*)&m_keyArray[0]) : 0;
 	if (tmapData->m_keyArrayPtr)
 	{
 		int sz = sizeof(int);
 		int numElem = tmapData->m_numValues;
-		btChunk* chunk = serializer->allocate(sz,numElem);
+		btChunk* chunk = serializer->allocate(sz, numElem);
 		int* memPtr = (int*)chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
 			*memPtr = m_keyArray[i].getUid1();
 		}
-		serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*) &m_keyArray[0]);
-
+		serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_keyArray[0]);
 	}
-	return "btTriangleInfoMapData";
-}
 
+	// Fill padding with zeros to appease msan.
+	tmapData->m_padding[0] = 0;
+	tmapData->m_padding[1] = 0;
+	tmapData->m_padding[2] = 0;
+	tmapData->m_padding[3] = 0;
 
+	return "btTriangleInfoMapData";
+}
 
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	void	btTriangleInfoMap::deSerialize(btTriangleInfoMapData& tmapData )
+SIMD_FORCE_INLINE void btTriangleInfoMap::deSerialize(btTriangleInfoMapData& tmapData)
 {
-
-
 	m_convexEpsilon = tmapData.m_convexEpsilon;
 	m_planarEpsilon = tmapData.m_planarEpsilon;
 	m_equalVertexThreshold = tmapData.m_equalVertexThreshold;
 	m_edgeDistanceThreshold = tmapData.m_edgeDistanceThreshold;
 	m_zeroAreaThreshold = tmapData.m_zeroAreaThreshold;
 	m_hashTable.resize(tmapData.m_hashTableSize);
-	int i =0;
-	for (i=0;i<tmapData.m_hashTableSize;i++)
+	int i = 0;
+	for (i = 0; i < tmapData.m_hashTableSize; i++)
 	{
 		m_hashTable[i] = tmapData.m_hashTablePtr[i];
 	}
 	m_next.resize(tmapData.m_nextSize);
-	for (i=0;i<tmapData.m_nextSize;i++)
+	for (i = 0; i < tmapData.m_nextSize; i++)
 	{
 		m_next[i] = tmapData.m_nextPtr[i];
 	}
 	m_valueArray.resize(tmapData.m_numValues);
-	for (i=0;i<tmapData.m_numValues;i++)
+	for (i = 0; i < tmapData.m_numValues; i++)
 	{
 		m_valueArray[i].m_edgeV0V1Angle = tmapData.m_valueArrayPtr[i].m_edgeV0V1Angle;
 		m_valueArray[i].m_edgeV1V2Angle = tmapData.m_valueArrayPtr[i].m_edgeV1V2Angle;
 		m_valueArray[i].m_edgeV2V0Angle = tmapData.m_valueArrayPtr[i].m_edgeV2V0Angle;
 		m_valueArray[i].m_flags = tmapData.m_valueArrayPtr[i].m_flags;
 	}
-	
-	m_keyArray.resize(tmapData.m_numKeys,btHashInt(0));
-	for (i=0;i<tmapData.m_numKeys;i++)
+
+	m_keyArray.resize(tmapData.m_numKeys, btHashInt(0));
+	for (i = 0; i < tmapData.m_numKeys; i++)
 	{
 		m_keyArray[i].setUid1(tmapData.m_keyArrayPtr[i]);
 	}
 }
 
-
-#endif //_BT_TRIANGLE_INFO_MAP_H
+#endif  //_BT_TRIANGLE_INFO_MAP_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp
index e4de7320930..abd8c227866 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp
@@ -13,38 +13,36 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btTriangleMesh.h"
 
-
-
-btTriangleMesh::btTriangleMesh (bool use32bitIndices,bool use4componentVertices)
-:m_use32bitIndices(use32bitIndices),
-m_use4componentVertices(use4componentVertices),
-m_weldingThreshold(0.0)
+btTriangleMesh::btTriangleMesh(bool use32bitIndices, bool use4componentVertices)
+	: m_use32bitIndices(use32bitIndices),
+	  m_use4componentVertices(use4componentVertices),
+	  m_weldingThreshold(0.0)
 {
 	btIndexedMesh meshIndex;
 	meshIndex.m_numTriangles = 0;
 	meshIndex.m_numVertices = 0;
 	meshIndex.m_indexType = PHY_INTEGER;
 	meshIndex.m_triangleIndexBase = 0;
-	meshIndex.m_triangleIndexStride = 3*sizeof(int);
+	meshIndex.m_triangleIndexStride = 3 * sizeof(int);
 	meshIndex.m_vertexBase = 0;
 	meshIndex.m_vertexStride = sizeof(btVector3);
 	m_indexedMeshes.push_back(meshIndex);
 
 	if (m_use32bitIndices)
 	{
-		m_indexedMeshes[0].m_numTriangles = m_32bitIndices.size()/3;
+		m_indexedMeshes[0].m_numTriangles = m_32bitIndices.size() / 3;
 		m_indexedMeshes[0].m_triangleIndexBase = 0;
 		m_indexedMeshes[0].m_indexType = PHY_INTEGER;
-		m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(int);
-	} else
+		m_indexedMeshes[0].m_triangleIndexStride = 3 * sizeof(int);
+	}
+	else
 	{
-		m_indexedMeshes[0].m_numTriangles = m_16bitIndices.size()/3;
+		m_indexedMeshes[0].m_numTriangles = m_16bitIndices.size() / 3;
 		m_indexedMeshes[0].m_triangleIndexBase = 0;
 		m_indexedMeshes[0].m_indexType = PHY_SHORT;
-		m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(short int);
+		m_indexedMeshes[0].m_triangleIndexStride = 3 * sizeof(short int);
 	}
 
 	if (m_use4componentVertices)
@@ -52,48 +50,48 @@ m_weldingThreshold(0.0)
 		m_indexedMeshes[0].m_numVertices = m_4componentVertices.size();
 		m_indexedMeshes[0].m_vertexBase = 0;
 		m_indexedMeshes[0].m_vertexStride = sizeof(btVector3);
-	} else
+	}
+	else
 	{
-		m_indexedMeshes[0].m_numVertices = m_3componentVertices.size()/3;
+		m_indexedMeshes[0].m_numVertices = m_3componentVertices.size() / 3;
 		m_indexedMeshes[0].m_vertexBase = 0;
-		m_indexedMeshes[0].m_vertexStride = 3*sizeof(btScalar);
+		m_indexedMeshes[0].m_vertexStride = 3 * sizeof(btScalar);
 	}
-
-
 }
 
-void	btTriangleMesh::addIndex(int index)
+void btTriangleMesh::addIndex(int index)
 {
 	if (m_use32bitIndices)
 	{
 		m_32bitIndices.push_back(index);
-		m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_32bitIndices[0];
-	} else
+		m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*)&m_32bitIndices[0];
+	}
+	else
 	{
 		m_16bitIndices.push_back(index);
-		m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_16bitIndices[0];
+		m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*)&m_16bitIndices[0];
 	}
 }
 
-void	btTriangleMesh::addTriangleIndices(int index1, int index2, int index3 )
+void btTriangleMesh::addTriangleIndices(int index1, int index2, int index3)
 {
 	m_indexedMeshes[0].m_numTriangles++;
-	addIndex( index1 );
-	addIndex( index2 );
-	addIndex( index3 );
+	addIndex(index1);
+	addIndex(index2);
+	addIndex(index3);
 }
 
-int	btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices)
+int btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices)
 {
 	//return index of new/existing vertex
 	///@todo: could use acceleration structure for this
 	if (m_use4componentVertices)
 	{
 		if (removeDuplicateVertices)
+		{
+			for (int i = 0; i < m_4componentVertices.size(); i++)
 			{
-			for (int i=0;i< m_4componentVertices.size();i++)
-			{
-				if ((m_4componentVertices[i]-vertex).length2() <= m_weldingThreshold)
+				if ((m_4componentVertices[i] - vertex).length2() <= m_weldingThreshold)
 				{
 					return i;
 				}
@@ -103,19 +101,18 @@ int	btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicat
 		m_4componentVertices.push_back(vertex);
 		m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_4componentVertices[0];
 
-		return m_4componentVertices.size()-1;
-		
-	} else
+		return m_4componentVertices.size() - 1;
+	}
+	else
 	{
-		
 		if (removeDuplicateVertices)
 		{
-			for (int i=0;i< m_3componentVertices.size();i+=3)
+			for (int i = 0; i < m_3componentVertices.size(); i += 3)
 			{
-				btVector3 vtx(m_3componentVertices[i],m_3componentVertices[i+1],m_3componentVertices[i+2]);
-				if ((vtx-vertex).length2() <= m_weldingThreshold)
+				btVector3 vtx(m_3componentVertices[i], m_3componentVertices[i + 1], m_3componentVertices[i + 2]);
+				if ((vtx - vertex).length2() <= m_weldingThreshold)
 				{
-					return i/3;
+					return i / 3;
 				}
 			}
 		}
@@ -124,17 +121,16 @@ int	btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicat
 		m_3componentVertices.push_back(vertex.getZ());
 		m_indexedMeshes[0].m_numVertices++;
 		m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0];
-		return (m_3componentVertices.size()/3)-1;
+		return (m_3componentVertices.size() / 3) - 1;
 	}
-
 }
-		
-void	btTriangleMesh::addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2,bool removeDuplicateVertices)
+
+void btTriangleMesh::addTriangle(const btVector3& vertex0, const btVector3& vertex1, const btVector3& vertex2, bool removeDuplicateVertices)
 {
 	m_indexedMeshes[0].m_numTriangles++;
-	addIndex(findOrAddVertex(vertex0,removeDuplicateVertices));
-	addIndex(findOrAddVertex(vertex1,removeDuplicateVertices));
-	addIndex(findOrAddVertex(vertex2,removeDuplicateVertices));
+	addIndex(findOrAddVertex(vertex0, removeDuplicateVertices));
+	addIndex(findOrAddVertex(vertex1, removeDuplicateVertices));
+	addIndex(findOrAddVertex(vertex2, removeDuplicateVertices));
 }
 
 int btTriangleMesh::getNumTriangles() const
@@ -151,7 +147,8 @@ void btTriangleMesh::preallocateVertices(int numverts)
 	if (m_use4componentVertices)
 	{
 		m_4componentVertices.reserve(numverts);
-	} else
+	}
+	else
 	{
 		m_3componentVertices.reserve(numverts);
 	}
@@ -162,7 +159,8 @@ void btTriangleMesh::preallocateIndices(int numindices)
 	if (m_use32bitIndices)
 	{
 		m_32bitIndices.reserve(numindices);
-	} else
+	}
+	else
 	{
 		m_16bitIndices.reserve(numindices);
 	}
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h
index ac4afa7f6b2..a8a362355cc 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h
@@ -26,47 +26,44 @@ subject to the following restrictions:
 ///Performance of btTriangleMesh and btTriangleIndexVertexArray used in a btBvhTriangleMeshShape is the same.
 class btTriangleMesh : public btTriangleIndexVertexArray
 {
-	btAlignedObjectArray<btVector3>	m_4componentVertices;
-	btAlignedObjectArray<btScalar>	m_3componentVertices;
+	btAlignedObjectArray<btVector3> m_4componentVertices;
+	btAlignedObjectArray<btScalar> m_3componentVertices;
 
-	btAlignedObjectArray<unsigned int>		m_32bitIndices;
-	btAlignedObjectArray<unsigned short int>		m_16bitIndices;
-	bool	m_use32bitIndices;
-	bool	m_use4componentVertices;
-	
+	btAlignedObjectArray<unsigned int> m_32bitIndices;
+	btAlignedObjectArray<unsigned short int> m_16bitIndices;
+	bool m_use32bitIndices;
+	bool m_use4componentVertices;
 
-	public:
-		btScalar	m_weldingThreshold;
+public:
+	btScalar m_weldingThreshold;
 
-		btTriangleMesh (bool use32bitIndices=true,bool use4componentVertices=true);
+	btTriangleMesh(bool use32bitIndices = true, bool use4componentVertices = true);
 
-		bool	getUse32bitIndices() const
-		{
-			return m_use32bitIndices;
-		}
+	bool getUse32bitIndices() const
+	{
+		return m_use32bitIndices;
+	}
 
-		bool	getUse4componentVertices() const
-		{
-			return m_use4componentVertices;
-		}
-		///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);
+	bool getUse4componentVertices() const
+	{
+		return m_use4componentVertices;
+	}
+	///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;
+	///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);
 
-		virtual void	preallocateVertices(int numverts);
-		virtual void	preallocateIndices(int numindices);
+	int getNumTriangles() const;
 
-		///findOrAddVertex is an internal method, use addTriangle instead
-		int		findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices);
-		///addIndex is an internal method, use addTriangle instead
-		void	addIndex(int index);
-		
-};
+	virtual void preallocateVertices(int numverts);
+	virtual void preallocateIndices(int numindices);
 
-#endif //BT_TRIANGLE_MESH_H
+	///findOrAddVertex is an internal method, use addTriangle instead
+	int findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices);
+	///addIndex is an internal method, use addTriangle instead
+	void addIndex(int index);
+};
 
+#endif  //BT_TRIANGLE_MESH_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp
index 0e17951405c..aec239063cd 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp
@@ -20,12 +20,11 @@ subject to the following restrictions:
 #include "LinearMath/btAabbUtil2.h"
 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
 
-
 btTriangleMeshShape::btTriangleMeshShape(btStridingMeshInterface* meshInterface)
-: btConcaveShape (), m_meshInterface(meshInterface)
+	: btConcaveShape(), m_meshInterface(meshInterface)
 {
 	m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
-	if(meshInterface->hasPremadeAabb())
+	if (meshInterface->hasPremadeAabb())
 	{
 		meshInterface->getPremadeAabb(&m_localAabbMin, &m_localAabbMax);
 	}
@@ -35,69 +34,60 @@ btTriangleMeshShape::btTriangleMeshShape(btStridingMeshInterface* meshInterface)
 	}
 }
 
-
 btTriangleMeshShape::~btTriangleMeshShape()
 {
-		
 }
 
-
-
-
-void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+void btTriangleMeshShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const
 {
+	btVector3 localHalfExtents = btScalar(0.5) * (m_localAabbMax - m_localAabbMin);
+	localHalfExtents += btVector3(getMargin(), getMargin(), getMargin());
+	btVector3 localCenter = btScalar(0.5) * (m_localAabbMax + m_localAabbMin);
 
-	btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
-	localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
-	btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
-	
-	btMatrix3x3 abs_b = trans.getBasis().absolute();  
+	btMatrix3x3 abs_b = trans.getBasis().absolute();
 
 	btVector3 center = trans(localCenter);
 
-    btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
+	btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
 	aabbMin = center - extent;
 	aabbMax = center + extent;
 }
 
-void	btTriangleMeshShape::recalcLocalAabb()
+void btTriangleMeshShape::recalcLocalAabb()
 {
-	for (int i=0;i<3;i++)
+	for (int i = 0; i < 3; i++)
 	{
-		btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
+		btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.));
 		vec[i] = btScalar(1.);
 		btVector3 tmp = localGetSupportingVertex(vec);
-		m_localAabbMax[i] = tmp[i]+m_collisionMargin;
+		m_localAabbMax[i] = tmp[i] + m_collisionMargin;
 		vec[i] = btScalar(-1.);
 		tmp = localGetSupportingVertex(vec);
-		m_localAabbMin[i] = tmp[i]-m_collisionMargin;
+		m_localAabbMin[i] = tmp[i] - m_collisionMargin;
 	}
 }
 
-
-
 class SupportVertexCallback : public btTriangleCallback
 {
-
 	btVector3 m_supportVertexLocal;
-public:
 
-	btTransform	m_worldTrans;
+public:
+	btTransform m_worldTrans;
 	btScalar m_maxDot;
 	btVector3 m_supportVecLocal;
 
-	SupportVertexCallback(const btVector3& supportVecWorld,const btTransform& trans)
-		: m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), m_worldTrans(trans) ,m_maxDot(btScalar(-BT_LARGE_FLOAT))
-		
+	SupportVertexCallback(const btVector3& supportVecWorld, const btTransform& trans)
+		: m_supportVertexLocal(btScalar(0.), btScalar(0.), btScalar(0.)), m_worldTrans(trans), m_maxDot(btScalar(-BT_LARGE_FLOAT))
+
 	{
 		m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis();
 	}
 
-	virtual void processTriangle( btVector3* triangle,int partId, int triangleIndex)
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 	{
 		(void)partId;
 		(void)triangleIndex;
-		for (int i=0;i<3;i++)
+		for (int i = 0; i < 3; i++)
 		{
 			btScalar dot = m_supportVecLocal.dot(triangle[i]);
 			if (dot > m_maxDot)
@@ -113,14 +103,12 @@ public:
 		return m_worldTrans(m_supportVertexLocal);
 	}
 
-	btVector3	GetSupportVertexLocal()
+	btVector3 GetSupportVertexLocal()
 	{
 		return m_supportVertexLocal;
 	}
-
 };
 
-	
 void btTriangleMeshShape::setLocalScaling(const btVector3& scaling)
 {
 	m_meshInterface->setScaling(scaling);
@@ -132,60 +120,46 @@ const btVector3& btTriangleMeshShape::getLocalScaling() const
 	return m_meshInterface->getScaling();
 }
 
-
-
-
-
-
 //#define DEBUG_TRIANGLE_MESH
 
-
-
-void	btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+void btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
 {
-		struct FilteredCallback : public btInternalTriangleIndexCallback
+	struct FilteredCallback : public btInternalTriangleIndexCallback
 	{
 		btTriangleCallback* m_callback;
 		btVector3 m_aabbMin;
 		btVector3 m_aabbMax;
 
-		FilteredCallback(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax)
-			:m_callback(callback),
-			m_aabbMin(aabbMin),
-			m_aabbMax(aabbMax)
+		FilteredCallback(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax)
+			: m_callback(callback),
+			  m_aabbMin(aabbMin),
+			  m_aabbMax(aabbMax)
 		{
 		}
 
-		virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
+		virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
 		{
-			if (TestTriangleAgainstAabb2(&triangle[0],m_aabbMin,m_aabbMax))
+			if (TestTriangleAgainstAabb2(&triangle[0], m_aabbMin, m_aabbMax))
 			{
 				//check aabb in triangle-space, before doing this
-				m_callback->processTriangle(triangle,partId,triangleIndex);
+				m_callback->processTriangle(triangle, partId, triangleIndex);
 			}
-			
 		}
-
 	};
 
-	FilteredCallback filterCallback(callback,aabbMin,aabbMax);
+	FilteredCallback filterCallback(callback, aabbMin, aabbMax);
 
-	m_meshInterface->InternalProcessAllTriangles(&filterCallback,aabbMin,aabbMax);
+	m_meshInterface->InternalProcessAllTriangles(&filterCallback, aabbMin, aabbMax);
 }
 
-
-
-
-
-void	btTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btTriangleMeshShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
 	(void)mass;
 	//moving concave objects not supported
 	btAssert(0);
-	inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+	inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 }
 
-
 btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) const
 {
 	btVector3 supportVertex;
@@ -193,15 +167,13 @@ btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) co
 	btTransform ident;
 	ident.setIdentity();
 
-	SupportVertexCallback supportCallback(vec,ident);
+	SupportVertexCallback supportCallback(vec, ident);
+
+	btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+
+	processAllTriangles(&supportCallback, -aabbMax, aabbMax);
 
-	btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-	
-	processAllTriangles(&supportCallback,-aabbMax,aabbMax);
-		
 	supportVertex = supportCallback.GetSupportVertexLocal();
 
 	return supportVertex;
 }
-
-
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h
index 453e58005a4..4a70e283fae 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h
@@ -19,18 +19,18 @@ subject to the following restrictions:
 #include "btConcaveShape.h"
 #include "btStridingMeshInterface.h"
 
-
 ///The btTriangleMeshShape is an internal concave triangle mesh interface. Don't use this class directly, use btBvhTriangleMeshShape instead.
-ATTRIBUTE_ALIGNED16(class) btTriangleMeshShape : public btConcaveShape
+ATTRIBUTE_ALIGNED16(class)
+btTriangleMeshShape : public btConcaveShape
 {
 protected:
-	btVector3	m_localAabbMin;
-	btVector3	m_localAabbMax;
+	btVector3 m_localAabbMin;
+	btVector3 m_localAabbMax;
 	btStridingMeshInterface* m_meshInterface;
 
 	///btTriangleMeshShape constructor has been disabled/protected, so that users will not mistakenly use this class.
 	///Don't use btTriangleMeshShape but use btBvhTriangleMeshShape instead!
-	btTriangleMeshShape(btStridingMeshInterface* meshInterface);
+	btTriangleMeshShape(btStridingMeshInterface * meshInterface);
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -39,23 +39,23 @@ public:
 
 	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
 
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 	{
 		btAssert(0);
 		return localGetSupportingVertex(vec);
 	}
 
-	void	recalcLocalAabb();
+	void recalcLocalAabb();
 
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
-	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+	virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const;
 
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
 
-	virtual void	setLocalScaling(const btVector3& scaling);
+	virtual void setLocalScaling(const btVector3& scaling);
 	virtual const btVector3& getLocalScaling() const;
-	
+
 	btStridingMeshInterface* getMeshInterface()
 	{
 		return m_meshInterface;
@@ -75,16 +75,8 @@ public:
 		return m_localAabbMax;
 	}
 
-
-
 	//debugging
-	virtual const char*	getName()const {return "TRIANGLEMESH";}
-
-	
-
+	virtual const char* getName() const { return "TRIANGLEMESH"; }
 };
 
-
-
-
-#endif //BT_TRIANGLE_MESH_SHAPE_H
+#endif  //BT_TRIANGLE_MESH_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h
index a8a80f82fe2..190cbdae69e 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h
@@ -19,15 +19,13 @@ subject to the following restrictions:
 #include "btConvexShape.h"
 #include "btBoxShape.h"
 
-ATTRIBUTE_ALIGNED16(class) btTriangleShape : public btPolyhedralConvexShape
+ATTRIBUTE_ALIGNED16(class)
+btTriangleShape : public btPolyhedralConvexShape
 {
-
-
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-BT_DECLARE_ALIGNED_ALLOCATOR();
-
-	btVector3	m_vertices1[3];
+	btVector3 m_vertices1[3];
 
 	virtual int getNumVertices() const
 	{
@@ -43,7 +41,7 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 	{
 		return m_vertices1[index];
 	}
-	virtual void getVertex(int index,btVector3& vert) const
+	virtual void getVertex(int index, btVector3& vert) const
 	{
 		vert = m_vertices1[index];
 	}
@@ -52,83 +50,79 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 	{
 		return 3;
 	}
-	
-	virtual void getEdge(int i,btVector3& pa,btVector3& pb) const
+
+	virtual void getEdge(int i, btVector3& pa, btVector3& pb) const
 	{
-		getVertex(i,pa);
-		getVertex((i+1)%3,pb);
+		getVertex(i, pa);
+		getVertex((i + 1) % 3, pb);
 	}
 
-
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const 
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 	{
-//		btAssert(0);
-		getAabbSlow(t,aabbMin,aabbMax);
+		//		btAssert(0);
+		getAabbSlow(t, aabbMin, aabbMax);
 	}
 
-	btVector3 localGetSupportingVertexWithoutMargin(const btVector3& dir)const 
+	btVector3 localGetSupportingVertexWithoutMargin(const btVector3& dir) const
 	{
-        btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]);
-	  	return m_vertices1[dots.maxAxis()];
-
+		btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]);
+		return m_vertices1[dots.maxAxis()];
 	}
 
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 	{
-		for (int i=0;i<numVectors;i++)
+		for (int i = 0; i < numVectors; i++)
 		{
 			const btVector3& dir = vectors[i];
-            btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]);
-  			supportVerticesOut[i] = m_vertices1[dots.maxAxis()];
+			btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]);
+			supportVerticesOut[i] = m_vertices1[dots.maxAxis()];
 		}
-
 	}
 
-	btTriangleShape() : btPolyhedralConvexShape ()
-    {
+	btTriangleShape() : btPolyhedralConvexShape()
+	{
 		m_shapeType = TRIANGLE_SHAPE_PROXYTYPE;
 	}
 
-	btTriangleShape(const btVector3& p0,const btVector3& p1,const btVector3& p2) : btPolyhedralConvexShape ()
-    {
+	btTriangleShape(const btVector3& p0, const btVector3& p1, const btVector3& p2) : btPolyhedralConvexShape()
+	{
 		m_shapeType = TRIANGLE_SHAPE_PROXYTYPE;
-        m_vertices1[0] = p0;
-        m_vertices1[1] = p1;
-        m_vertices1[2] = p2;
-    }
-
+		m_vertices1[0] = p0;
+		m_vertices1[1] = p1;
+		m_vertices1[2] = p2;
+	}
 
-	virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i) const
+	virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const
 	{
-		getPlaneEquation(i,planeNormal,planeSupport);
+		getPlaneEquation(i, planeNormal, planeSupport);
 	}
 
-	virtual int	getNumPlanes() const
+	virtual int getNumPlanes() const
 	{
 		return 1;
 	}
 
-	void calcNormal(btVector3& normal) const
+	void calcNormal(btVector3 & normal) const
 	{
-		normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]);
+		normal = (m_vertices1[1] - m_vertices1[0]).cross(m_vertices1[2] - m_vertices1[0]);
 		normal.normalize();
 	}
 
-	virtual void getPlaneEquation(int i, btVector3& planeNormal,btVector3& planeSupport) const
+	virtual void getPlaneEquation(int i, btVector3& planeNormal, btVector3& planeSupport) const
 	{
 		(void)i;
 		calcNormal(planeNormal);
 		planeSupport = m_vertices1[0];
 	}
 
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const
 	{
 		(void)mass;
 		btAssert(0);
-		inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+		inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 	}
 
-		virtual	bool isInside(const btVector3& pt,btScalar tolerance) const
+	virtual bool isInside(const btVector3& pt, btScalar tolerance) const
 	{
 		btVector3 normal;
 		calcNormal(normal);
@@ -140,45 +134,42 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 		{
 			//inside check on edge-planes
 			int i;
-			for (i=0;i<3;i++)
+			for (i = 0; i < 3; i++)
 			{
-				btVector3 pa,pb;
-				getEdge(i,pa,pb);
-				btVector3 edge = pb-pa;
+				btVector3 pa, pb;
+				getEdge(i, pa, pb);
+				btVector3 edge = pb - pa;
 				btVector3 edgeNormal = edge.cross(normal);
 				edgeNormal.normalize();
-				btScalar dist = pt.dot( edgeNormal);
+				btScalar dist = pt.dot(edgeNormal);
 				btScalar edgeConst = pa.dot(edgeNormal);
 				dist -= edgeConst;
 				if (dist < -tolerance)
 					return false;
 			}
-			
+
 			return true;
 		}
 
 		return false;
 	}
-		//debugging
-		virtual const char*	getName()const
-		{
-			return "Triangle";
-		}
-
-		virtual int		getNumPreferredPenetrationDirections() const
-		{
-			return 2;
-		}
-		
-		virtual void	getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
-		{
-			calcNormal(penetrationVector);
-			if (index)
-				penetrationVector *= btScalar(-1.);
-		}
+	//debugging
+	virtual const char* getName() const
+	{
+		return "Triangle";
+	}
 
+	virtual int getNumPreferredPenetrationDirections() const
+	{
+		return 2;
+	}
 
+	virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+	{
+		calcNormal(penetrationVector);
+		if (index)
+			penetrationVector *= btScalar(-1.);
+	}
 };
 
-#endif //BT_OBB_TRIANGLE_MINKOWSKI_H
-
+#endif  //BT_OBB_TRIANGLE_MINKOWSKI_H
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp
index b148bbd99a5..ed3cd2d2597 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp
@@ -15,122 +15,112 @@ subject to the following restrictions:
 
 #include "btUniformScalingShape.h"
 
-btUniformScalingShape::btUniformScalingShape(	btConvexShape* convexChildShape,btScalar uniformScalingFactor):
-btConvexShape (), m_childConvexShape(convexChildShape),
-m_uniformScalingFactor(uniformScalingFactor)
+btUniformScalingShape::btUniformScalingShape(btConvexShape* convexChildShape, btScalar uniformScalingFactor) : btConvexShape(), m_childConvexShape(convexChildShape), m_uniformScalingFactor(uniformScalingFactor)
 {
 	m_shapeType = UNIFORM_SCALING_SHAPE_PROXYTYPE;
 }
-	
+
 btUniformScalingShape::~btUniformScalingShape()
 {
 }
-	
 
-btVector3	btUniformScalingShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+btVector3 btUniformScalingShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 {
 	btVector3 tmpVertex;
 	tmpVertex = m_childConvexShape->localGetSupportingVertexWithoutMargin(vec);
-	return tmpVertex*m_uniformScalingFactor;
+	return tmpVertex * m_uniformScalingFactor;
 }
 
-void	btUniformScalingShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btUniformScalingShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 {
-	m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors);
+	m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors, supportVerticesOut, numVectors);
 	int i;
-	for (i=0;i<numVectors;i++)
+	for (i = 0; i < numVectors; i++)
 	{
 		supportVerticesOut[i] = supportVerticesOut[i] * m_uniformScalingFactor;
 	}
 }
 
-
-btVector3	btUniformScalingShape::localGetSupportingVertex(const btVector3& vec)const
+btVector3 btUniformScalingShape::localGetSupportingVertex(const btVector3& vec) const
 {
 	btVector3 tmpVertex;
 	tmpVertex = m_childConvexShape->localGetSupportingVertex(vec);
-	return tmpVertex*m_uniformScalingFactor;
+	return tmpVertex * m_uniformScalingFactor;
 }
 
-
-void	btUniformScalingShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btUniformScalingShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
-
 	///this linear upscaling is not realistic, but we don't deal with large mass ratios...
 	btVector3 tmpInertia;
-	m_childConvexShape->calculateLocalInertia(mass,tmpInertia);
+	m_childConvexShape->calculateLocalInertia(mass, tmpInertia);
 	inertia = tmpInertia * m_uniformScalingFactor;
 }
 
-
-	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
-void btUniformScalingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+void btUniformScalingShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const
 {
-	getAabbSlow(trans,aabbMin,aabbMax);
-
+	getAabbSlow(trans, aabbMin, aabbMax);
 }
 
-void btUniformScalingShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+void btUniformScalingShape::getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 {
 #if 1
 	btVector3 _directions[] =
-	{
-		btVector3( 1.,  0.,  0.),
-		btVector3( 0.,  1.,  0.),
-		btVector3( 0.,  0.,  1.),
-		btVector3( -1., 0.,  0.),
-		btVector3( 0., -1.,  0.),
-		btVector3( 0.,  0., -1.)
-	};
-	
+		{
+			btVector3(1., 0., 0.),
+			btVector3(0., 1., 0.),
+			btVector3(0., 0., 1.),
+			btVector3(-1., 0., 0.),
+			btVector3(0., -1., 0.),
+			btVector3(0., 0., -1.)};
+
 	btVector3 _supporting[] =
+		{
+			btVector3(0., 0., 0.),
+			btVector3(0., 0., 0.),
+			btVector3(0., 0., 0.),
+			btVector3(0., 0., 0.),
+			btVector3(0., 0., 0.),
+			btVector3(0., 0., 0.)};
+
+	for (int i = 0; i < 6; i++)
 	{
-		btVector3( 0., 0., 0.),
-		btVector3( 0., 0., 0.),
-		btVector3( 0., 0., 0.),
-		btVector3( 0., 0., 0.),
-		btVector3( 0., 0., 0.),
-		btVector3( 0., 0., 0.)
-	};
-
-	for (int i=0;i<6;i++)
-	{
-		_directions[i] = _directions[i]*t.getBasis();
+		_directions[i] = _directions[i] * t.getBasis();
 	}
-	
+
 	batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
-	
-	btVector3 aabbMin1(0,0,0),aabbMax1(0,0,0);
 
-	for ( int i = 0; i < 3; ++i )
+	btVector3 aabbMin1(0, 0, 0), aabbMax1(0, 0, 0);
+
+	for (int i = 0; i < 3; ++i)
 	{
 		aabbMax1[i] = t(_supporting[i])[i];
 		aabbMin1[i] = t(_supporting[i + 3])[i];
 	}
-	btVector3 marginVec(getMargin(),getMargin(),getMargin());
-	aabbMin = aabbMin1-marginVec;
-	aabbMax = aabbMax1+marginVec;
-	
+	btVector3 marginVec(getMargin(), getMargin(), getMargin());
+	aabbMin = aabbMin1 - marginVec;
+	aabbMax = aabbMax1 + marginVec;
+
 #else
 
 	btScalar margin = getMargin();
-	for (int i=0;i<3;i++)
+	for (int i = 0; i < 3; i++)
 	{
-		btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
+		btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.));
 		vec[i] = btScalar(1.);
-		btVector3 sv = localGetSupportingVertex(vec*t.getBasis());
+		btVector3 sv = localGetSupportingVertex(vec * t.getBasis());
 		btVector3 tmp = t(sv);
-		aabbMax[i] = tmp[i]+margin;
+		aabbMax[i] = tmp[i] + margin;
 		vec[i] = btScalar(-1.);
-		sv = localGetSupportingVertex(vec*t.getBasis());
+		sv = localGetSupportingVertex(vec * t.getBasis());
 		tmp = t(sv);
-		aabbMin[i] = tmp[i]-margin;
+		aabbMin[i] = tmp[i] - margin;
 	}
 
 #endif
 }
 
-void	btUniformScalingShape::setLocalScaling(const btVector3& scaling) 
+void btUniformScalingShape::setLocalScaling(const btVector3& scaling)
 {
 	m_childConvexShape->setLocalScaling(scaling);
 }
@@ -140,21 +130,21 @@ const btVector3& btUniformScalingShape::getLocalScaling() const
 	return m_childConvexShape->getLocalScaling();
 }
 
-void	btUniformScalingShape::setMargin(btScalar margin)
+void btUniformScalingShape::setMargin(btScalar margin)
 {
 	m_childConvexShape->setMargin(margin);
 }
-btScalar	btUniformScalingShape::getMargin() const
+btScalar btUniformScalingShape::getMargin() const
 {
 	return m_childConvexShape->getMargin() * m_uniformScalingFactor;
 }
 
-int		btUniformScalingShape::getNumPreferredPenetrationDirections() const
+int btUniformScalingShape::getNumPreferredPenetrationDirections() const
 {
 	return m_childConvexShape->getNumPreferredPenetrationDirections();
 }
-	
-void	btUniformScalingShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+
+void btUniformScalingShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
 {
-	m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector);
+	m_childConvexShape->getPreferredPenetrationDirection(index, penetrationVector);
 }
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h
index a10f58d2429..4dfe34efbdc 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h
@@ -17,73 +17,68 @@ subject to the following restrictions:
 #define BT_UNIFORM_SCALING_SHAPE_H
 
 #include "btConvexShape.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"  // for the types
 
 ///The btUniformScalingShape allows to re-use uniform scaled instances of btConvexShape in a memory efficient way.
 ///Istead of using btUniformScalingShape, it is better to use the non-uniform setLocalScaling method on convex shapes that implement it.
-ATTRIBUTE_ALIGNED16(class) btUniformScalingShape : public btConvexShape
+ATTRIBUTE_ALIGNED16(class)
+btUniformScalingShape : public btConvexShape
 {
-	btConvexShape*	m_childConvexShape;
+	btConvexShape* m_childConvexShape;
 
-	btScalar	m_uniformScalingFactor;
-	
-	public:
-	
+	btScalar m_uniformScalingFactor;
+
+public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-	btUniformScalingShape(	btConvexShape* convexChildShape, btScalar uniformScalingFactor);
-	
+
+	btUniformScalingShape(btConvexShape * convexChildShape, btScalar uniformScalingFactor);
+
 	virtual ~btUniformScalingShape();
-	
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
 
-	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
+
+	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
 
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const;
 
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
 
-	btScalar	getUniformScalingFactor() const
+	btScalar getUniformScalingFactor() const
 	{
 		return m_uniformScalingFactor;
 	}
 
-	btConvexShape*	getChildShape() 
+	btConvexShape* getChildShape()
 	{
 		return m_childConvexShape;
 	}
 
-	const btConvexShape*	getChildShape() const
+	const btConvexShape* getChildShape() const
 	{
 		return m_childConvexShape;
 	}
 
-	virtual const char*	getName()const 
+	virtual const char* getName() const
 	{
 		return "UniformScalingShape";
 	}
-	
-
 
 	///////////////////////////
 
-
 	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
-	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
-
-	virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+	void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
-	virtual void	setLocalScaling(const btVector3& scaling) ;
-	virtual const btVector3& getLocalScaling() const ;
+	virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
-	virtual void	setMargin(btScalar margin);
-	virtual btScalar	getMargin() const;
+	virtual void setLocalScaling(const btVector3& scaling);
+	virtual const btVector3& getLocalScaling() const;
 
-	virtual int		getNumPreferredPenetrationDirections() const;
-	
-	virtual void	getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const;
+	virtual void setMargin(btScalar margin);
+	virtual btScalar getMargin() const;
 
+	virtual int getNumPreferredPenetrationDirections() const;
 
+	virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const;
 };
 
-#endif //BT_UNIFORM_SCALING_SHAPE_H
+#endif  //BT_UNIFORM_SCALING_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h b/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h
index 0a0357e5a81..182835c3b44 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h
@@ -26,27 +26,21 @@ subject to the following restrictions:
 
 #include "LinearMath/btTransform.h"
 
-
 ///Swap numbers
-#define BT_SWAP_NUMBERS(a,b){ \
-    a = a+b; \
-    b = a-b; \
-    a = a-b; \
-}\
-
-
-#define BT_MAX(a,b) (a<b?b:a)
-#define BT_MIN(a,b) (a>b?b:a)
-
-#define BT_GREATER(x, y)	btFabs(x) > (y)
-
-#define BT_MAX3(a,b,c) BT_MAX(a,BT_MAX(b,c))
-#define BT_MIN3(a,b,c) BT_MIN(a,BT_MIN(b,c))
-
-
+#define BT_SWAP_NUMBERS(a, b) \
+	{                         \
+		a = a + b;            \
+		b = a - b;            \
+		a = a - b;            \
+	}
 
+#define BT_MAX(a, b) (a < b ? b : a)
+#define BT_MIN(a, b) (a > b ? b : a)
 
+#define BT_GREATER(x, y) btFabs(x) > (y)
 
+#define BT_MAX3(a, b, c) BT_MAX(a, BT_MAX(b, c))
+#define BT_MIN3(a, b, c) BT_MIN(a, BT_MIN(b, c))
 
 enum eBT_PLANE_INTERSECTION_TYPE
 {
@@ -115,152 +109,144 @@ enum eBT_PLANE_INTERSECTION_TYPE
 //	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1);
 //}
 
+#define TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, i_dir_0, i_dir_1, i_comp_0, i_comp_1) \
+	{                                                                                                               \
+		const btScalar dir0 = -edge[i_dir_0];                                                                       \
+		const btScalar dir1 = edge[i_dir_1];                                                                        \
+		btScalar pmin = pointa[i_comp_0] * dir0 + pointa[i_comp_1] * dir1;                                          \
+		btScalar pmax = pointb[i_comp_0] * dir0 + pointb[i_comp_1] * dir1;                                          \
+		if (pmin > pmax)                                                                                            \
+		{                                                                                                           \
+			BT_SWAP_NUMBERS(pmin, pmax);                                                                            \
+		}                                                                                                           \
+		const btScalar abs_dir0 = absolute_edge[i_dir_0];                                                           \
+		const btScalar abs_dir1 = absolute_edge[i_dir_1];                                                           \
+		const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;                           \
+		if (pmin > rad || -rad > pmax) return false;                                                                \
+	}
 
-#define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
-{\
-	const btScalar dir0 = -edge[i_dir_0];\
-	const btScalar dir1 = edge[i_dir_1];\
-	btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
-	btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
-	if(pmin>pmax)\
-	{\
-		BT_SWAP_NUMBERS(pmin,pmax); \
-	}\
-	const btScalar abs_dir0 = absolute_edge[i_dir_0];\
-	const btScalar abs_dir1 = absolute_edge[i_dir_1];\
-	const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
-	if(pmin>rad || -rad>pmax) return false;\
-}\
-
-
-#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
-{\
-	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
-}\
-
-#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
-{\
-	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
-}\
+#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend)       \
+	{                                                                                      \
+		TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 2, 1, 1, 2); \
+	}
 
-#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
-{\
-	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
-}\
+#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend)       \
+	{                                                                                      \
+		TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 0, 2, 2, 0); \
+	}
 
+#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend)       \
+	{                                                                                      \
+		TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 1, 0, 0, 1); \
+	}
 
 //! Returns the dot product between a vec3f and the col of a matrix
 SIMD_FORCE_INLINE btScalar bt_mat3_dot_col(
-const btMatrix3x3 & mat, const btVector3 & vec3, int colindex)
+	const btMatrix3x3 &mat, const btVector3 &vec3, int colindex)
 {
-	return vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex];
+	return vec3[0] * mat[0][colindex] + vec3[1] * mat[1][colindex] + vec3[2] * mat[2][colindex];
 }
 
-
 //!  Class for transforming a model1 to the space of model0
-ATTRIBUTE_ALIGNED16	(class) BT_BOX_BOX_TRANSFORM_CACHE
+ATTRIBUTE_ALIGNED16(class)
+BT_BOX_BOX_TRANSFORM_CACHE
 {
 public:
-    btVector3  m_T1to0;//!< Transforms translation of model1 to model 0
-	btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal  to R0' * R1
-	btMatrix3x3 m_AR;//!< Absolute value of m_R1to0
+	btVector3 m_T1to0;    //!< Transforms translation of model1 to model 0
+	btMatrix3x3 m_R1to0;  //!< Transforms Rotation of model1 to model 0, equal  to R0' * R1
+	btMatrix3x3 m_AR;     //!< Absolute value of m_R1to0
 
 	SIMD_FORCE_INLINE void calc_absolute_matrix()
 	{
-//		static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
-//		m_AR[0] = vepsi + m_R1to0[0].absolute();
-//		m_AR[1] = vepsi + m_R1to0[1].absolute();
-//		m_AR[2] = vepsi + m_R1to0[2].absolute();
-
-		int i,j;
+		//		static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
+		//		m_AR[0] = vepsi + m_R1to0[0].absolute();
+		//		m_AR[1] = vepsi + m_R1to0[1].absolute();
+		//		m_AR[2] = vepsi + m_R1to0[2].absolute();
 
-        for(i=0;i<3;i++)
-        {
-            for(j=0;j<3;j++ )
-            {
-            	m_AR[i][j] = 1e-6f + btFabs(m_R1to0[i][j]);
-            }
-        }
+		int i, j;
 
+		for (i = 0; i < 3; i++)
+		{
+			for (j = 0; j < 3; j++)
+			{
+				m_AR[i][j] = 1e-6f + btFabs(m_R1to0[i][j]);
+			}
+		}
 	}
 
 	BT_BOX_BOX_TRANSFORM_CACHE()
 	{
 	}
 
-
-
 	//! Calc the transformation relative  1 to 0. Inverts matrics by transposing
-	SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
+	SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform &trans0, const btTransform &trans1)
 	{
-
 		btTransform temp_trans = trans0.inverse();
 		temp_trans = temp_trans * trans1;
 
 		m_T1to0 = temp_trans.getOrigin();
 		m_R1to0 = temp_trans.getBasis();
 
-
 		calc_absolute_matrix();
 	}
 
 	//! Calcs the full invertion of the matrices. Useful for scaling matrices
-	SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
+	SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform &trans0, const btTransform &trans1)
 	{
 		m_R1to0 = trans0.getBasis().inverse();
 		m_T1to0 = m_R1to0 * (-trans0.getOrigin());
 
-		m_T1to0 += m_R1to0*trans1.getOrigin();
+		m_T1to0 += m_R1to0 * trans1.getOrigin();
 		m_R1to0 *= trans1.getBasis();
 
 		calc_absolute_matrix();
 	}
 
-	SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) const
+	SIMD_FORCE_INLINE btVector3 transform(const btVector3 &point) const
 	{
-        return point.dot3( m_R1to0[0], m_R1to0[1], m_R1to0[2] ) + m_T1to0;
+		return point.dot3(m_R1to0[0], m_R1to0[1], m_R1to0[2]) + m_T1to0;
 	}
 };
 
-
 #define BOX_PLANE_EPSILON 0.000001f
 
 //! Axis aligned box
-ATTRIBUTE_ALIGNED16	(class) btAABB
+ATTRIBUTE_ALIGNED16(class)
+btAABB
 {
 public:
 	btVector3 m_min;
 	btVector3 m_max;
 
 	btAABB()
-	{}
-
+	{
+	}
 
-	btAABB(const btVector3 & V1,
-			 const btVector3 & V2,
-			 const btVector3 & V3)
+	btAABB(const btVector3 &V1,
+		   const btVector3 &V2,
+		   const btVector3 &V3)
 	{
-		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
-		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
-		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+		m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]);
+		m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]);
+		m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]);
 
-		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
-		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
-		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+		m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]);
+		m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]);
+		m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]);
 	}
 
-	btAABB(const btVector3 & V1,
-			 const btVector3 & V2,
-			 const btVector3 & V3,
-			 btScalar margin)
+	btAABB(const btVector3 &V1,
+		   const btVector3 &V2,
+		   const btVector3 &V3,
+		   btScalar margin)
 	{
-		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
-		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
-		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+		m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]);
+		m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]);
+		m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]);
 
-		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
-		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
-		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+		m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]);
+		m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]);
+		m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]);
 
 		m_min[0] -= margin;
 		m_min[1] -= margin;
@@ -270,13 +256,11 @@ public:
 		m_max[2] += margin;
 	}
 
-	btAABB(const btAABB &other):
-		m_min(other.m_min),m_max(other.m_max)
+	btAABB(const btAABB &other) : m_min(other.m_min), m_max(other.m_max)
 	{
 	}
 
-	btAABB(const btAABB &other,btScalar margin ):
-		m_min(other.m_min),m_max(other.m_max)
+	btAABB(const btAABB &other, btScalar margin) : m_min(other.m_min), m_max(other.m_max)
 	{
 		m_min[0] -= margin;
 		m_min[1] -= margin;
@@ -317,34 +301,34 @@ public:
 		m_max[2] = other.m_max[2] + margin;
 	}
 
-	template<typename CLASS_POINT>
+	template <typename CLASS_POINT>
 	SIMD_FORCE_INLINE void calc_from_triangle(
-							const CLASS_POINT & V1,
-							const CLASS_POINT & V2,
-							const CLASS_POINT & V3)
+		const CLASS_POINT &V1,
+		const CLASS_POINT &V2,
+		const CLASS_POINT &V3)
 	{
-		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
-		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
-		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+		m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]);
+		m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]);
+		m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]);
 
-		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
-		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
-		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+		m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]);
+		m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]);
+		m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]);
 	}
 
-	template<typename CLASS_POINT>
+	template <typename CLASS_POINT>
 	SIMD_FORCE_INLINE void calc_from_triangle_margin(
-							const CLASS_POINT & V1,
-							const CLASS_POINT & V2,
-							const CLASS_POINT & V3, btScalar margin)
+		const CLASS_POINT &V1,
+		const CLASS_POINT &V2,
+		const CLASS_POINT &V3, btScalar margin)
 	{
-		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
-		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
-		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+		m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]);
+		m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]);
+		m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]);
 
-		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
-		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
-		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+		m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]);
+		m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]);
+		m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]);
 
 		m_min[0] -= margin;
 		m_min[1] -= margin;
@@ -355,91 +339,89 @@ public:
 	}
 
 	//! Apply a transform to an AABB
-	SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
+	SIMD_FORCE_INLINE void appy_transform(const btTransform &trans)
 	{
-		btVector3 center = (m_max+m_min)*0.5f;
+		btVector3 center = (m_max + m_min) * 0.5f;
 		btVector3 extends = m_max - center;
 		// Compute new center
 		center = trans(center);
 
-        btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(), 
-                                          trans.getBasis().getRow(1).absolute(), 
-                                          trans.getBasis().getRow(2).absolute());
+		btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(),
+										  trans.getBasis().getRow(1).absolute(),
+										  trans.getBasis().getRow(2).absolute());
 
 		m_min = center - textends;
 		m_max = center + textends;
 	}
 
-
 	//! Apply a transform to an AABB
-	SIMD_FORCE_INLINE void appy_transform_trans_cache(const BT_BOX_BOX_TRANSFORM_CACHE & trans)
+	SIMD_FORCE_INLINE void appy_transform_trans_cache(const BT_BOX_BOX_TRANSFORM_CACHE &trans)
 	{
-		btVector3 center = (m_max+m_min)*0.5f;
+		btVector3 center = (m_max + m_min) * 0.5f;
 		btVector3 extends = m_max - center;
 		// Compute new center
 		center = trans.transform(center);
 
-        btVector3 textends = extends.dot3(trans.m_R1to0.getRow(0).absolute(), 
-                                          trans.m_R1to0.getRow(1).absolute(), 
-                                          trans.m_R1to0.getRow(2).absolute());
-        
+		btVector3 textends = extends.dot3(trans.m_R1to0.getRow(0).absolute(),
+										  trans.m_R1to0.getRow(1).absolute(),
+										  trans.m_R1to0.getRow(2).absolute());
+
 		m_min = center - textends;
 		m_max = center + textends;
 	}
 
 	//! Merges a Box
-	SIMD_FORCE_INLINE void merge(const btAABB & box)
+	SIMD_FORCE_INLINE void merge(const btAABB &box)
 	{
-		m_min[0] = BT_MIN(m_min[0],box.m_min[0]);
-		m_min[1] = BT_MIN(m_min[1],box.m_min[1]);
-		m_min[2] = BT_MIN(m_min[2],box.m_min[2]);
+		m_min[0] = BT_MIN(m_min[0], box.m_min[0]);
+		m_min[1] = BT_MIN(m_min[1], box.m_min[1]);
+		m_min[2] = BT_MIN(m_min[2], box.m_min[2]);
 
-		m_max[0] = BT_MAX(m_max[0],box.m_max[0]);
-		m_max[1] = BT_MAX(m_max[1],box.m_max[1]);
-		m_max[2] = BT_MAX(m_max[2],box.m_max[2]);
+		m_max[0] = BT_MAX(m_max[0], box.m_max[0]);
+		m_max[1] = BT_MAX(m_max[1], box.m_max[1]);
+		m_max[2] = BT_MAX(m_max[2], box.m_max[2]);
 	}
 
 	//! Merges a point
-	template<typename CLASS_POINT>
-	SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
+	template <typename CLASS_POINT>
+	SIMD_FORCE_INLINE void merge_point(const CLASS_POINT &point)
 	{
-		m_min[0] = BT_MIN(m_min[0],point[0]);
-		m_min[1] = BT_MIN(m_min[1],point[1]);
-		m_min[2] = BT_MIN(m_min[2],point[2]);
+		m_min[0] = BT_MIN(m_min[0], point[0]);
+		m_min[1] = BT_MIN(m_min[1], point[1]);
+		m_min[2] = BT_MIN(m_min[2], point[2]);
 
-		m_max[0] = BT_MAX(m_max[0],point[0]);
-		m_max[1] = BT_MAX(m_max[1],point[1]);
-		m_max[2] = BT_MAX(m_max[2],point[2]);
+		m_max[0] = BT_MAX(m_max[0], point[0]);
+		m_max[1] = BT_MAX(m_max[1], point[1]);
+		m_max[2] = BT_MAX(m_max[2], point[2]);
 	}
 
 	//! Gets the extend and center
-	SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend)  const
+	SIMD_FORCE_INLINE void get_center_extend(btVector3 & center, btVector3 & extend) const
 	{
-		center = (m_max+m_min)*0.5f;
+		center = (m_max + m_min) * 0.5f;
 		extend = m_max - center;
 	}
 
 	//! Finds the intersecting box between this box and the other.
-	SIMD_FORCE_INLINE void find_intersection(const btAABB & other, btAABB & intersection)  const
+	SIMD_FORCE_INLINE void find_intersection(const btAABB &other, btAABB &intersection) const
 	{
-		intersection.m_min[0] = BT_MAX(other.m_min[0],m_min[0]);
-		intersection.m_min[1] = BT_MAX(other.m_min[1],m_min[1]);
-		intersection.m_min[2] = BT_MAX(other.m_min[2],m_min[2]);
+		intersection.m_min[0] = BT_MAX(other.m_min[0], m_min[0]);
+		intersection.m_min[1] = BT_MAX(other.m_min[1], m_min[1]);
+		intersection.m_min[2] = BT_MAX(other.m_min[2], m_min[2]);
 
-		intersection.m_max[0] = BT_MIN(other.m_max[0],m_max[0]);
-		intersection.m_max[1] = BT_MIN(other.m_max[1],m_max[1]);
-		intersection.m_max[2] = BT_MIN(other.m_max[2],m_max[2]);
+		intersection.m_max[0] = BT_MIN(other.m_max[0], m_max[0]);
+		intersection.m_max[1] = BT_MIN(other.m_max[1], m_max[1]);
+		intersection.m_max[2] = BT_MIN(other.m_max[2], m_max[2]);
 	}
 
-
-	SIMD_FORCE_INLINE bool has_collision(const btAABB & other) const
+	SIMD_FORCE_INLINE bool has_collision(const btAABB &other) const
 	{
-		if(m_min[0] > other.m_max[0] ||
-		   m_max[0] < other.m_min[0] ||
-		   m_min[1] > other.m_max[1] ||
-		   m_max[1] < other.m_min[1] ||
-		   m_min[2] > other.m_max[2] ||
-		   m_max[2] < other.m_min[2])
+		if (m_min[0] > other.m_max[0] ||
+			m_max[0] < other.m_min[0] ||
+			m_min[1] > other.m_max[1] ||
+			m_max[1] < other.m_min[1] ||
+			m_min[2] > other.m_max[2] ||
+			m_max[2] < other.m_min[2])
 		{
 			return false;
 		}
@@ -451,35 +433,34 @@ public:
 	\param vorigin A vec3f with the origin of the ray
 	\param vdir A vec3f with the direction of the ray
 	*/
-	SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir)  const
+	SIMD_FORCE_INLINE bool collide_ray(const btVector3 &vorigin, const btVector3 &vdir) const
 	{
-		btVector3 extents,center;
-		this->get_center_extend(center,extents);;
+		btVector3 extents, center;
+		this->get_center_extend(center, extents);
+		;
 
 		btScalar Dx = vorigin[0] - center[0];
-		if(BT_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f)	return false;
+		if (BT_GREATER(Dx, extents[0]) && Dx * vdir[0] >= 0.0f) return false;
 		btScalar Dy = vorigin[1] - center[1];
-		if(BT_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f)	return false;
+		if (BT_GREATER(Dy, extents[1]) && Dy * vdir[1] >= 0.0f) return false;
 		btScalar Dz = vorigin[2] - center[2];
-		if(BT_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f)	return false;
-
+		if (BT_GREATER(Dz, extents[2]) && Dz * vdir[2] >= 0.0f) return false;
 
 		btScalar f = vdir[1] * Dz - vdir[2] * Dy;
-		if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
+		if (btFabs(f) > extents[1] * btFabs(vdir[2]) + extents[2] * btFabs(vdir[1])) return false;
 		f = vdir[2] * Dx - vdir[0] * Dz;
-		if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
+		if (btFabs(f) > extents[0] * btFabs(vdir[2]) + extents[2] * btFabs(vdir[0])) return false;
 		f = vdir[0] * Dy - vdir[1] * Dx;
-		if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
+		if (btFabs(f) > extents[0] * btFabs(vdir[1]) + extents[1] * btFabs(vdir[0])) return false;
 		return true;
 	}
 
-
-	SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
+	SIMD_FORCE_INLINE void projection_interval(const btVector3 &direction, btScalar &vmin, btScalar &vmax) const
 	{
-		btVector3 center = (m_max+m_min)*0.5f;
-		btVector3 extend = m_max-center;
+		btVector3 center = (m_max + m_min) * 0.5f;
+		btVector3 extend = m_max - center;
 
-		btScalar _fOrigin =  direction.dot(center);
+		btScalar _fOrigin = direction.dot(center);
 		btScalar _fMaximumExtent = extend.dot(direction.absolute());
 		vmin = _fOrigin - _fMaximumExtent;
 		vmax = _fOrigin + _fMaximumExtent;
@@ -487,30 +468,30 @@ public:
 
 	SIMD_FORCE_INLINE eBT_PLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
 	{
-		btScalar _fmin,_fmax;
-		this->projection_interval(plane,_fmin,_fmax);
+		btScalar _fmin, _fmax;
+		this->projection_interval(plane, _fmin, _fmax);
 
-		if(plane[3] > _fmax + BOX_PLANE_EPSILON)
+		if (plane[3] > _fmax + BOX_PLANE_EPSILON)
 		{
-			return BT_CONST_BACK_PLANE; // 0
+			return BT_CONST_BACK_PLANE;  // 0
 		}
 
-		if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
+		if (plane[3] + BOX_PLANE_EPSILON >= _fmin)
 		{
-			return BT_CONST_COLLIDE_PLANE; //1
+			return BT_CONST_COLLIDE_PLANE;  //1
 		}
-		return BT_CONST_FRONT_PLANE;//2
+		return BT_CONST_FRONT_PLANE;  //2
 	}
 
-	SIMD_FORCE_INLINE bool overlapping_trans_conservative(const btAABB & box, btTransform & trans1_to_0) const
+	SIMD_FORCE_INLINE bool overlapping_trans_conservative(const btAABB &box, btTransform &trans1_to_0) const
 	{
 		btAABB tbox = box;
 		tbox.appy_transform(trans1_to_0);
 		return has_collision(tbox);
 	}
 
-	SIMD_FORCE_INLINE bool overlapping_trans_conservative2(const btAABB & box,
-		const BT_BOX_BOX_TRANSFORM_CACHE & trans1_to_0) const
+	SIMD_FORCE_INLINE bool overlapping_trans_conservative2(const btAABB &box,
+														   const BT_BOX_BOX_TRANSFORM_CACHE &trans1_to_0) const
 	{
 		btAABB tbox = box;
 		tbox.appy_transform_trans_cache(trans1_to_0);
@@ -519,52 +500,50 @@ public:
 
 	//! transcache is the transformation cache from box to this AABB
 	SIMD_FORCE_INLINE bool overlapping_trans_cache(
-		const btAABB & box,const BT_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest) const
+		const btAABB &box, const BT_BOX_BOX_TRANSFORM_CACHE &transcache, bool fulltest) const
 	{
-
 		//Taken from OPCODE
-		btVector3 ea,eb;//extends
-		btVector3 ca,cb;//extends
-		get_center_extend(ca,ea);
-		box.get_center_extend(cb,eb);
-
+		btVector3 ea, eb;  //extends
+		btVector3 ca, cb;  //extends
+		get_center_extend(ca, ea);
+		box.get_center_extend(cb, eb);
 
 		btVector3 T;
-		btScalar t,t2;
+		btScalar t, t2;
 		int i;
 
 		// Class I : A's basis vectors
-		for(i=0;i<3;i++)
+		for (i = 0; i < 3; i++)
 		{
-			T[i] =  transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
+			T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
 			t = transcache.m_AR[i].dot(eb) + ea[i];
-			if(BT_GREATER(T[i], t))	return false;
+			if (BT_GREATER(T[i], t)) return false;
 		}
 		// Class II : B's basis vectors
-		for(i=0;i<3;i++)
+		for (i = 0; i < 3; i++)
 		{
-			t = bt_mat3_dot_col(transcache.m_R1to0,T,i);
-			t2 = bt_mat3_dot_col(transcache.m_AR,ea,i) + eb[i];
-			if(BT_GREATER(t,t2))	return false;
+			t = bt_mat3_dot_col(transcache.m_R1to0, T, i);
+			t2 = bt_mat3_dot_col(transcache.m_AR, ea, i) + eb[i];
+			if (BT_GREATER(t, t2)) return false;
 		}
 		// Class III : 9 cross products
-		if(fulltest)
+		if (fulltest)
 		{
-			int j,m,n,o,p,q,r;
-			for(i=0;i<3;i++)
+			int j, m, n, o, p, q, r;
+			for (i = 0; i < 3; i++)
 			{
-				m = (i+1)%3;
-				n = (i+2)%3;
-				o = i==0?1:0;
-				p = i==2?1:2;
-				for(j=0;j<3;j++)
+				m = (i + 1) % 3;
+				n = (i + 2) % 3;
+				o = i == 0 ? 1 : 0;
+				p = i == 2 ? 1 : 2;
+				for (j = 0; j < 3; j++)
 				{
-					q = j==2?1:2;
-					r = j==0?1:0;
-					t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
-					t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
-						eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
-					if(BT_GREATER(t,t2))	return false;
+					q = j == 2 ? 1 : 2;
+					r = j == 0 ? 1 : 0;
+					t = T[n] * transcache.m_R1to0[m][j] - T[m] * transcache.m_R1to0[n][j];
+					t2 = ea[o] * transcache.m_AR[p][j] + ea[p] * transcache.m_AR[o][j] +
+						 eb[r] * transcache.m_AR[i][q] + eb[q] * transcache.m_AR[i][r];
+					if (BT_GREATER(t, t2)) return false;
 				}
 			}
 		}
@@ -573,7 +552,7 @@ public:
 
 	//! Simple test for planes.
 	SIMD_FORCE_INLINE bool collide_plane(
-		const btVector4 & plane) const
+		const btVector4 &plane) const
 	{
 		eBT_PLANE_INTERSECTION_TYPE classify = plane_classify(plane);
 		return (classify == BT_CONST_COLLIDE_PLANE);
@@ -581,15 +560,15 @@ public:
 
 	//! test for a triangle, with edges
 	SIMD_FORCE_INLINE bool collide_triangle_exact(
-		const btVector3 & p1,
-		const btVector3 & p2,
-		const btVector3 & p3,
-		const btVector4 & triangle_plane) const
+		const btVector3 &p1,
+		const btVector3 &p2,
+		const btVector3 &p3,
+		const btVector4 &triangle_plane) const
 	{
-		if(!collide_plane(triangle_plane)) return false;
+		if (!collide_plane(triangle_plane)) return false;
 
-		btVector3 center,extends;
-		this->get_center_extend(center,extends);
+		btVector3 center, extends;
+		this->get_center_extend(center, extends);
 
 		const btVector3 v1(p1 - center);
 		const btVector3 v2(p2 - center);
@@ -599,47 +578,43 @@ public:
 		btVector3 diff(v2 - v1);
 		btVector3 abs_diff = diff.absolute();
 		//Test With X axis
-		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v1, v3, extends);
 		//Test With Y axis
-		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v1, v3, extends);
 		//Test With Z axis
-		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);
-
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v1, v3, extends);
 
 		diff = v3 - v2;
 		abs_diff = diff.absolute();
 		//Test With X axis
-		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v2, v1, extends);
 		//Test With Y axis
-		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v2, v1, extends);
 		//Test With Z axis
-		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v2, v1, extends);
 
 		diff = v1 - v3;
 		abs_diff = diff.absolute();
 		//Test With X axis
-		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v3, v2, extends);
 		//Test With Y axis
-		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v3, v2, extends);
 		//Test With Z axis
-		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v3, v2, extends);
 
 		return true;
 	}
 };
 
-
 //! Compairison of transformation objects
-SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
+SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform &t1, const btTransform &t2)
 {
-	if(!(t1.getOrigin() == t2.getOrigin()) ) return false;
+	if (!(t1.getOrigin() == t2.getOrigin())) return false;
 
-	if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
-	if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
-	if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
+	if (!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0))) return false;
+	if (!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1))) return false;
+	if (!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2))) return false;
 	return true;
 }
 
-
-
-#endif // GIM_BOX_COLLISION_H_INCLUDED
+#endif  // GIM_BOX_COLLISION_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h b/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h
index de0a5231baf..38c23e222de 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h
@@ -27,77 +27,74 @@ subject to the following restrictions:
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btGeometryUtil.h"
 
-
-SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 & plane,const btVector3 &point)
+SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 &plane, const btVector3 &point)
 {
 	return point.dot(plane) - plane[3];
 }
 
 /*! Vector blending
 Takes two vectors a, b, blends them together*/
-SIMD_FORCE_INLINE void bt_vec_blend(btVector3 &vr, const btVector3 &va,const btVector3 &vb, btScalar blend_factor)
+SIMD_FORCE_INLINE void bt_vec_blend(btVector3 &vr, const btVector3 &va, const btVector3 &vb, btScalar blend_factor)
 {
-	vr = (1-blend_factor)*va + blend_factor*vb;
+	vr = (1 - blend_factor) * va + blend_factor * vb;
 }
 
 //! This function calcs the distance from a 3D plane
 SIMD_FORCE_INLINE void bt_plane_clip_polygon_collect(
-						const btVector3 & point0,
-						const btVector3 & point1,
-						btScalar dist0,
-						btScalar dist1,
-						btVector3 * clipped,
-						int & clipped_count)
+	const btVector3 &point0,
+	const btVector3 &point1,
+	btScalar dist0,
+	btScalar dist1,
+	btVector3 *clipped,
+	int &clipped_count)
 {
-	bool _prevclassif = (dist0>SIMD_EPSILON);
-	bool _classif = (dist1>SIMD_EPSILON);
-	if(_classif!=_prevclassif)
+	bool _prevclassif = (dist0 > SIMD_EPSILON);
+	bool _classif = (dist1 > SIMD_EPSILON);
+	if (_classif != _prevclassif)
 	{
-		btScalar blendfactor = -dist0/(dist1-dist0);
-		bt_vec_blend(clipped[clipped_count],point0,point1,blendfactor);
+		btScalar blendfactor = -dist0 / (dist1 - dist0);
+		bt_vec_blend(clipped[clipped_count], point0, point1, blendfactor);
 		clipped_count++;
 	}
-	if(!_classif)
+	if (!_classif)
 	{
 		clipped[clipped_count] = point1;
 		clipped_count++;
 	}
 }
 
-
 //! Clips a polygon by a plane
 /*!
 *\return The count of the clipped counts
 */
 SIMD_FORCE_INLINE int bt_plane_clip_polygon(
-						const btVector4 & plane,
-						const btVector3 * polygon_points,
-						int polygon_point_count,
-						btVector3 * clipped)
+	const btVector4 &plane,
+	const btVector3 *polygon_points,
+	int polygon_point_count,
+	btVector3 *clipped)
 {
-    int clipped_count = 0;
-
+	int clipped_count = 0;
 
-    //clip first point
-	btScalar firstdist = bt_distance_point_plane(plane,polygon_points[0]);;
-	if(!(firstdist>SIMD_EPSILON))
+	//clip first point
+	btScalar firstdist = bt_distance_point_plane(plane, polygon_points[0]);
+	;
+	if (!(firstdist > SIMD_EPSILON))
 	{
 		clipped[clipped_count] = polygon_points[0];
 		clipped_count++;
 	}
 
 	btScalar olddist = firstdist;
-	for(int i=1;i<polygon_point_count;i++)
+	for (int i = 1; i < polygon_point_count; i++)
 	{
-		btScalar dist = bt_distance_point_plane(plane,polygon_points[i]);
+		btScalar dist = bt_distance_point_plane(plane, polygon_points[i]);
 
 		bt_plane_clip_polygon_collect(
-						polygon_points[i-1],polygon_points[i],
-						olddist,
-						dist,
-						clipped,
-						clipped_count);
-
+			polygon_points[i - 1], polygon_points[i],
+			olddist,
+			dist,
+			clipped,
+			clipped_count);
 
 		olddist = dist;
 	}
@@ -105,11 +102,11 @@ SIMD_FORCE_INLINE int bt_plane_clip_polygon(
 	//RETURN TO FIRST  point
 
 	bt_plane_clip_polygon_collect(
-					polygon_points[polygon_point_count-1],polygon_points[0],
-					olddist,
-					firstdist,
-					clipped,
-					clipped_count);
+		polygon_points[polygon_point_count - 1], polygon_points[0],
+		olddist,
+		firstdist,
+		clipped,
+		clipped_count);
 
 	return clipped_count;
 }
@@ -120,18 +117,19 @@ SIMD_FORCE_INLINE int bt_plane_clip_polygon(
 *\return The count of the clipped counts
 */
 SIMD_FORCE_INLINE int bt_plane_clip_triangle(
-						const btVector4 & plane,
-						const btVector3 & point0,
-						const btVector3 & point1,
-						const btVector3& point2,
-						btVector3 * clipped // an allocated array of 16 points at least
-						)
+	const btVector4 &plane,
+	const btVector3 &point0,
+	const btVector3 &point1,
+	const btVector3 &point2,
+	btVector3 *clipped  // an allocated array of 16 points at least
+)
 {
-    int clipped_count = 0;
+	int clipped_count = 0;
 
-    //clip first point0
-	btScalar firstdist = bt_distance_point_plane(plane,point0);;
-	if(!(firstdist>SIMD_EPSILON))
+	//clip first point0
+	btScalar firstdist = bt_distance_point_plane(plane, point0);
+	;
+	if (!(firstdist > SIMD_EPSILON))
 	{
 		clipped[clipped_count] = point0;
 		clipped_count++;
@@ -139,44 +137,37 @@ SIMD_FORCE_INLINE int bt_plane_clip_triangle(
 
 	// point 1
 	btScalar olddist = firstdist;
-	btScalar dist = bt_distance_point_plane(plane,point1);
+	btScalar dist = bt_distance_point_plane(plane, point1);
 
 	bt_plane_clip_polygon_collect(
-					point0,point1,
-					olddist,
-					dist,
-					clipped,
-					clipped_count);
+		point0, point1,
+		olddist,
+		dist,
+		clipped,
+		clipped_count);
 
 	olddist = dist;
 
-
 	// point 2
-	dist = bt_distance_point_plane(plane,point2);
+	dist = bt_distance_point_plane(plane, point2);
 
 	bt_plane_clip_polygon_collect(
-					point1,point2,
-					olddist,
-					dist,
-					clipped,
-					clipped_count);
+		point1, point2,
+		olddist,
+		dist,
+		clipped,
+		clipped_count);
 	olddist = dist;
 
-
-
 	//RETURN TO FIRST  point0
 	bt_plane_clip_polygon_collect(
-					point2,point0,
-					olddist,
-					firstdist,
-					clipped,
-					clipped_count);
+		point2, point0,
+		olddist,
+		firstdist,
+		clipped,
+		clipped_count);
 
 	return clipped_count;
 }
 
-
-
-
-
-#endif // GIM_TRI_COLLISION_H_INCLUDED
+#endif  // GIM_TRI_COLLISION_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btCompoundFromGimpact.h b/extern/bullet2/src/BulletCollision/Gimpact/btCompoundFromGimpact.h
index 02f8b678a45..ede59e8a57f 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btCompoundFromGimpact.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btCompoundFromGimpact.h
@@ -5,89 +5,101 @@
 #include "btGImpactShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
 
+ATTRIBUTE_ALIGNED16(class)
+btCompoundFromGimpactShape : public btCompoundShape
+{
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	virtual ~btCompoundFromGimpactShape()
+	{
+		/*delete all the btBU_Simplex1to4 ChildShapes*/
+		for (int i = 0; i < m_children.size(); i++)
+		{
+			delete m_children[i].m_childShape;
+		}
+	}
+};
+
 struct MyCallback : public btTriangleRaycastCallback
+{
+	int m_ignorePart;
+	int m_ignoreTriangleIndex;
+
+	MyCallback(const btVector3& from, const btVector3& to, int ignorePart, int ignoreTriangleIndex)
+		: btTriangleRaycastCallback(from, to),
+		  m_ignorePart(ignorePart),
+		  m_ignoreTriangleIndex(ignoreTriangleIndex)
+	{
+	}
+	virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex)
+	{
+		if (partId != m_ignorePart || triangleIndex != m_ignoreTriangleIndex)
 		{
-			int	m_ignorePart;
-			int	m_ignoreTriangleIndex;
-			
-
-			MyCallback(const btVector3& from, const btVector3& to, int ignorePart, int ignoreTriangleIndex)
-			:btTriangleRaycastCallback(from,to),
-			m_ignorePart(ignorePart),
-			m_ignoreTriangleIndex(ignoreTriangleIndex)
-			{
-				
-			}
-			virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex)
-			{
-				if (partId!=m_ignorePart || triangleIndex!=m_ignoreTriangleIndex)
-				{
-					if (hitFraction < m_hitFraction)
-						return hitFraction;
-				}
-
-				return m_hitFraction;
-			}
-		};
-		struct MyInternalTriangleIndexCallback :public btInternalTriangleIndexCallback
+			if (hitFraction < m_hitFraction)
+				return hitFraction;
+		}
+
+		return m_hitFraction;
+	}
+};
+struct MyInternalTriangleIndexCallback : public btInternalTriangleIndexCallback
+{
+	const btGImpactMeshShape* m_gimpactShape;
+	btCompoundShape* m_colShape;
+	btScalar m_depth;
+
+	MyInternalTriangleIndexCallback(btCompoundShape* colShape, const btGImpactMeshShape* meshShape, btScalar depth)
+		: m_colShape(colShape),
+		  m_gimpactShape(meshShape),
+		  m_depth(depth)
+	{
+	}
+
+	virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
+	{
+		btVector3 scale = m_gimpactShape->getLocalScaling();
+		btVector3 v0 = triangle[0] * scale;
+		btVector3 v1 = triangle[1] * scale;
+		btVector3 v2 = triangle[2] * scale;
+
+		btVector3 centroid = (v0 + v1 + v2) / 3;
+		btVector3 normal = (v1 - v0).cross(v2 - v0);
+		normal.normalize();
+		btVector3 rayFrom = centroid;
+		btVector3 rayTo = centroid - normal * m_depth;
+
+		MyCallback cb(rayFrom, rayTo, partId, triangleIndex);
+
+		m_gimpactShape->processAllTrianglesRay(&cb, rayFrom, rayTo);
+		if (cb.m_hitFraction < 1)
 		{
-			const btGImpactMeshShape*		m_gimpactShape;
-			btCompoundShape*			m_colShape;
-			btScalar	m_depth;
-
-			MyInternalTriangleIndexCallback (btCompoundShape* colShape, const btGImpactMeshShape* meshShape, btScalar depth)
-			:m_colShape(colShape),
-			m_gimpactShape(meshShape),
-			m_depth(depth)
-			{
-			}
-			
-			virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
-			{
-				btVector3 scale = m_gimpactShape->getLocalScaling();
-				btVector3 v0=triangle[0]*scale;
-				btVector3 v1=triangle[1]*scale;
-				btVector3 v2=triangle[2]*scale;
-				
-				btVector3 centroid = (v0+v1+v2)/3;
-				btVector3 normal = (v1-v0).cross(v2-v0);
-				normal.normalize();
-				btVector3 rayFrom = centroid;
-				btVector3 rayTo = centroid-normal*m_depth;
-				
-				MyCallback cb(rayFrom,rayTo,partId,triangleIndex);
-				
-				m_gimpactShape->processAllTrianglesRay(&cb,rayFrom, rayTo);
-				if (cb.m_hitFraction<1)
-				{
-					rayTo.setInterpolate3(cb.m_from,cb.m_to,cb.m_hitFraction);
-					//rayTo = cb.m_from;
-					//rayTo = rayTo.lerp(cb.m_to,cb.m_hitFraction);
-					//gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0));
-				}
-				
-
-				
-				btBU_Simplex1to4* tet = new btBU_Simplex1to4(v0,v1,v2,rayTo);
-				btTransform ident;
-				ident.setIdentity();
-				m_colShape->addChildShape(ident,tet);
-			}
-		};
-		
-btCompoundShape*	btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth)
+			rayTo.setInterpolate3(cb.m_from, cb.m_to, cb.m_hitFraction);
+			//rayTo = cb.m_from;
+			//rayTo = rayTo.lerp(cb.m_to,cb.m_hitFraction);
+			//gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0));
+		}
+
+		btBU_Simplex1to4* tet = new btBU_Simplex1to4(v0, v1, v2, rayTo);
+		btTransform ident;
+		ident.setIdentity();
+		m_colShape->addChildShape(ident, tet);
+	}
+};
+
+btCompoundShape* btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth)
 {
-	btCompoundShape* colShape = new btCompoundShape();
-		
-		btTransform tr;
-		tr.setIdentity();
-		
-		MyInternalTriangleIndexCallback cb(colShape,gimpactMesh, depth);
-		btVector3 aabbMin,aabbMax;
-		gimpactMesh->getAabb(tr,aabbMin,aabbMax);
-		gimpactMesh->getMeshInterface()->InternalProcessAllTriangles(&cb,aabbMin,aabbMax);
-
-	return colShape;	
-}	
-
-#endif //BT_COMPOUND_FROM_GIMPACT
\ No newline at end of file
+	btCompoundShape* colShape = new btCompoundFromGimpactShape();
+
+	btTransform tr;
+	tr.setIdentity();
+
+	MyInternalTriangleIndexCallback cb(colShape, gimpactMesh, depth);
+	btVector3 aabbMin, aabbMax;
+	gimpactMesh->getAabb(tr, aabbMin, aabbMax);
+	gimpactMesh->getMeshInterface()->InternalProcessAllTriangles(&cb, aabbMin, aabbMax);
+
+	return colShape;
+}
+
+#endif  //BT_COMPOUND_FROM_GIMPACT
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp
index eed31d839f8..f2e3e18d610 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp
@@ -27,54 +27,50 @@ struct CONTACT_KEY_TOKEN
 	unsigned int m_key;
 	int m_value;
 	CONTACT_KEY_TOKEN()
-    {
-    }
-
-    CONTACT_KEY_TOKEN(unsigned int key,int token)
-    {
-    	m_key = key;
-    	m_value =  token;
-    }
-
-    CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken)
-    {
-    	m_key = rtoken.m_key;
-    	m_value = rtoken.m_value;
-    }
-
-    inline bool operator <(const CONTACT_KEY_TOKEN& other) const
+	{
+	}
+
+	CONTACT_KEY_TOKEN(unsigned int key, int token)
+	{
+		m_key = key;
+		m_value = token;
+	}
+
+	CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken)
+	{
+		m_key = rtoken.m_key;
+		m_value = rtoken.m_value;
+	}
+
+	inline bool operator<(const CONTACT_KEY_TOKEN& other) const
 	{
 		return (m_key < other.m_key);
 	}
 
-	inline bool operator >(const CONTACT_KEY_TOKEN& other) const
+	inline bool operator>(const CONTACT_KEY_TOKEN& other) const
 	{
 		return (m_key > other.m_key);
 	}
-
 };
 
 class CONTACT_KEY_TOKEN_COMP
 {
-	public:
-
-		bool operator() ( const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b ) const
-		{
-			return ( a < b );
-		}
+public:
+	bool operator()(const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b) const
+	{
+		return (a < b);
+	}
 };
 
-
 void btContactArray::merge_contacts(
-	const btContactArray & contacts, bool normal_contact_average)
+	const btContactArray& contacts, bool normal_contact_average)
 {
 	clear();
 
 	int i;
-	if(contacts.size()==0) return;
-
+	if (contacts.size() == 0) return;
 
-	if(contacts.size()==1)
+	if (contacts.size() == 1)
 	{
 		push_back(contacts[0]);
 		return;
@@ -86,16 +82,16 @@ void btContactArray::merge_contacts(
 
 	//fill key contacts
 
-	for ( i = 0;i<contacts.size() ;i++ )
+	for (i = 0; i < contacts.size(); i++)
 	{
-		keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(),i));
+		keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(), i));
 	}
 
 	//sort keys
 	keycontacts.quickSort(CONTACT_KEY_TOKEN_COMP());
 
 	// Merge contacts
-	int coincident_count=0;
+	int coincident_count = 0;
 	btVector3 coincident_normals[MAX_COINCIDENT];
 
 	unsigned int last_key = keycontacts[0].m_key;
@@ -103,56 +99,56 @@ void btContactArray::merge_contacts(
 
 	push_back(contacts[keycontacts[0].m_value]);
 
-	GIM_CONTACT * pcontact = &(*this)[0];
+	GIM_CONTACT* pcontact = &(*this)[0];
 
-	for( i=1;i<keycontacts.size();i++)
+	for (i = 1; i < keycontacts.size(); i++)
 	{
-	    key = keycontacts[i].m_key;
-		const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];
+		key = keycontacts[i].m_key;
+		const GIM_CONTACT* scontact = &contacts[keycontacts[i].m_value];
 
-		if(last_key ==  key)//same points
+		if (last_key == key)  //same points
 		{
 			//merge contact
-			if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
+			if (pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)  //)
 			{
 				*pcontact = *scontact;
-                coincident_count = 0;
+				coincident_count = 0;
 			}
-			else if(normal_contact_average)
+			else if (normal_contact_average)
 			{
-				if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
-                {
-                    if(coincident_count<MAX_COINCIDENT)
-                    {
-                    	coincident_normals[coincident_count] = scontact->m_normal;
-                        coincident_count++;
-                    }
-                }
+				if (btFabs(pcontact->m_depth - scontact->m_depth) < CONTACT_DIFF_EPSILON)
+				{
+					if (coincident_count < MAX_COINCIDENT)
+					{
+						coincident_normals[coincident_count] = scontact->m_normal;
+						coincident_count++;
+					}
+				}
 			}
 		}
 		else
-		{//add new contact
+		{  //add new contact
 
-		    if(normal_contact_average && coincident_count>0)
-		    {
-		    	pcontact->interpolate_normals(coincident_normals,coincident_count);
-		        coincident_count = 0;
-		    }
+			if (normal_contact_average && coincident_count > 0)
+			{
+				pcontact->interpolate_normals(coincident_normals, coincident_count);
+				coincident_count = 0;
+			}
 
-		    push_back(*scontact);
-		    pcontact = &(*this)[this->size()-1];
-        }
+			push_back(*scontact);
+			pcontact = &(*this)[this->size() - 1];
+		}
 		last_key = key;
 	}
 }
 
-void btContactArray::merge_contacts_unique(const btContactArray & contacts)
+void btContactArray::merge_contacts_unique(const btContactArray& contacts)
 {
 	clear();
 
-	if(contacts.size()==0) return;
+	if (contacts.size() == 0) return;
 
-	if(contacts.size()==1)
+	if (contacts.size() == 1)
 	{
 		push_back(contacts[0]);
 		return;
@@ -160,14 +156,14 @@ void btContactArray::merge_contacts_unique(const btContactArray & contacts)
 
 	GIM_CONTACT average_contact = contacts[0];
 
-	for (int i=1;i<contacts.size() ;i++ )
+	for (int i = 1; i < contacts.size(); i++)
 	{
 		average_contact.m_point += contacts[i].m_point;
 		average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
 	}
 
 	//divide
-	btScalar divide_average = 1.0f/((btScalar)contacts.size());
+	btScalar divide_average = 1.0f / ((btScalar)contacts.size());
 
 	average_contact.m_point *= divide_average;
 
@@ -176,6 +172,4 @@ void btContactArray::merge_contacts_unique(const btContactArray & contacts)
 	average_contact.m_depth = average_contact.m_normal.length();
 
 	average_contact.m_normal /= average_contact.m_depth;
-
 }
-
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h
index 0c66f8e106c..4ff09d7cddf 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.h
@@ -27,88 +27,9 @@ subject to the following restrictions:
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btAlignedObjectArray.h"
 #include "btTriangleShapeEx.h"
+#include "btContactProcessingStructs.h"
 
-
-
-/**
-Configuration var for applying interpolation of  contact normals
-*/
-#define NORMAL_CONTACT_AVERAGE 1
-
-#define CONTACT_DIFF_EPSILON 0.00001f
-
-///The GIM_CONTACT is an internal GIMPACT structure, similar to btManifoldPoint.
-///@todo: remove and replace GIM_CONTACT by btManifoldPoint.
-class GIM_CONTACT
-{
-public:
-    btVector3 m_point;
-    btVector3 m_normal;
-    btScalar m_depth;//Positive value indicates interpenetration
-    btScalar m_distance;//Padding not for use
-    int m_feature1;//Face number
-    int m_feature2;//Face number
-public:
-    GIM_CONTACT()
-    {
-    }
-
-    GIM_CONTACT(const GIM_CONTACT & contact):
-				m_point(contact.m_point),
-				m_normal(contact.m_normal),
-				m_depth(contact.m_depth),
-				m_feature1(contact.m_feature1),
-				m_feature2(contact.m_feature2)
-    {
-    }
-
-    GIM_CONTACT(const btVector3 &point,const btVector3 & normal,
-    	 			btScalar depth, int feature1, int feature2):
-				m_point(point),
-				m_normal(normal),
-				m_depth(depth),
-				m_feature1(feature1),
-				m_feature2(feature2)
-    {
-    }
-
-	//! Calcs key for coord classification
-    SIMD_FORCE_INLINE unsigned int calc_key_contact() const
-    {
-    	int _coords[] = {
-    		(int)(m_point[0]*1000.0f+1.0f),
-    		(int)(m_point[1]*1333.0f),
-    		(int)(m_point[2]*2133.0f+3.0f)};
-		unsigned int _hash=0;
-		unsigned int *_uitmp = (unsigned int *)(&_coords[0]);
-		_hash = *_uitmp;
-		_uitmp++;
-		_hash += (*_uitmp)<<4;
-		_uitmp++;
-		_hash += (*_uitmp)<<8;
-		return _hash;
-    }
-
-    SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,int normal_count)
-    {
-    	btVector3 vec_sum(m_normal);
-		for(int i=0;i<normal_count;i++)
-		{
-			vec_sum += normals[i];
-		}
-
-		btScalar vec_sum_len = vec_sum.length2();
-		if(vec_sum_len <CONTACT_DIFF_EPSILON) return;
-
-		//GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len)
-
-		m_normal = vec_sum/btSqrt(vec_sum_len);
-    }
-
-};
-
-
-class btContactArray:public btAlignedObjectArray<GIM_CONTACT>
+class btContactArray : public btAlignedObjectArray<GIM_CONTACT>
 {
 public:
 	btContactArray()
@@ -117,29 +38,28 @@ public:
 	}
 
 	SIMD_FORCE_INLINE void push_contact(
-		const btVector3 &point,const btVector3 & normal,
+		const btVector3 &point, const btVector3 &normal,
 		btScalar depth, int feature1, int feature2)
 	{
-		push_back( GIM_CONTACT(point,normal,depth,feature1,feature2) );
+		push_back(GIM_CONTACT(point, normal, depth, feature1, feature2));
 	}
 
 	SIMD_FORCE_INLINE void push_triangle_contacts(
-		const GIM_TRIANGLE_CONTACT & tricontact,
-		int feature1,int feature2)
+		const GIM_TRIANGLE_CONTACT &tricontact,
+		int feature1, int feature2)
 	{
-		for(int i = 0;i<tricontact.m_point_count ;i++ )
+		for (int i = 0; i < tricontact.m_point_count; i++)
 		{
 			push_contact(
 				tricontact.m_points[i],
 				tricontact.m_separating_normal,
-				tricontact.m_penetration_depth,feature1,feature2);
+				tricontact.m_penetration_depth, feature1, feature2);
 		}
 	}
 
-	void merge_contacts(const btContactArray & contacts, bool normal_contact_average = true);
+	void merge_contacts(const btContactArray &contacts, bool normal_contact_average = true);
 
-	void merge_contacts_unique(const btContactArray & contacts);
+	void merge_contacts_unique(const btContactArray &contacts);
 };
 
-
-#endif // GIM_CONTACT_H_INCLUDED
+#endif  // GIM_CONTACT_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessingStructs.h b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessingStructs.h
new file mode 100644
index 00000000000..bc8a709246f
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessingStructs.h
@@ -0,0 +1,105 @@
+#ifndef BT_CONTACT_H_STRUCTS_INCLUDED
+#define BT_CONTACT_H_STRUCTS_INCLUDED
+
+/*! \file gim_contact.h
+\author Francisco Leon Najera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+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 "LinearMath/btTransform.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btTriangleShapeEx.h"
+
+/**
+Configuration var for applying interpolation of  contact normals
+*/
+#define NORMAL_CONTACT_AVERAGE 1
+
+#define CONTACT_DIFF_EPSILON 0.00001f
+
+///The GIM_CONTACT is an internal GIMPACT structure, similar to btManifoldPoint.
+///@todo: remove and replace GIM_CONTACT by btManifoldPoint.
+class GIM_CONTACT
+{
+public:
+	btVector3 m_point;
+	btVector3 m_normal;
+	btScalar m_depth;     //Positive value indicates interpenetration
+	btScalar m_distance;  //Padding not for use
+	int m_feature1;       //Face number
+	int m_feature2;       //Face number
+public:
+	GIM_CONTACT()
+	{
+	}
+
+	GIM_CONTACT(const GIM_CONTACT &contact) : m_point(contact.m_point),
+											  m_normal(contact.m_normal),
+											  m_depth(contact.m_depth),
+											  m_feature1(contact.m_feature1),
+											  m_feature2(contact.m_feature2)
+	{
+	}
+
+	GIM_CONTACT(const btVector3 &point, const btVector3 &normal,
+				btScalar depth, int feature1, int feature2) : m_point(point),
+															  m_normal(normal),
+															  m_depth(depth),
+															  m_feature1(feature1),
+															  m_feature2(feature2)
+	{
+	}
+
+	//! Calcs key for coord classification
+	SIMD_FORCE_INLINE unsigned int calc_key_contact() const
+	{
+		int _coords[] = {
+			(int)(m_point[0] * 1000.0f + 1.0f),
+			(int)(m_point[1] * 1333.0f),
+			(int)(m_point[2] * 2133.0f + 3.0f)};
+		unsigned int _hash = 0;
+		unsigned int *_uitmp = (unsigned int *)(&_coords[0]);
+		_hash = *_uitmp;
+		_uitmp++;
+		_hash += (*_uitmp) << 4;
+		_uitmp++;
+		_hash += (*_uitmp) << 8;
+		return _hash;
+	}
+
+	SIMD_FORCE_INLINE void interpolate_normals(btVector3 *normals, int normal_count)
+	{
+		btVector3 vec_sum(m_normal);
+		for (int i = 0; i < normal_count; i++)
+		{
+			vec_sum += normals[i];
+		}
+
+		btScalar vec_sum_len = vec_sum.length2();
+		if (vec_sum_len < CONTACT_DIFF_EPSILON) return;
+
+		//GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len)
+
+		m_normal = vec_sum / btSqrt(vec_sum_len);
+	}
+};
+
+#endif  // BT_CONTACT_H_STRUCTS_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp
index 863233163a6..bb520e061d0 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.cpp
@@ -30,7 +30,6 @@ btClock g_tree_clock;
 float g_accum_tree_collision_time = 0;
 int g_count_traversing = 0;
 
-
 void bt_begin_gim02_tree_time()
 {
 	g_tree_clock.reset();
@@ -45,7 +44,7 @@ void bt_end_gim02_tree_time()
 //! Gets the average time in miliseconds of tree collisions
 float btGImpactBvh::getAverageTreeCollisionTime()
 {
-	if(g_count_traversing == 0) return 0;
+	if (g_count_traversing == 0) return 0;
 
 	float avgtime = g_accum_tree_collision_time;
 	avgtime /= (float)g_count_traversing;
@@ -54,80 +53,76 @@ float btGImpactBvh::getAverageTreeCollisionTime()
 	g_count_traversing = 0;
 	return avgtime;
 
-//	float avgtime = g_count_traversing;
-//	g_count_traversing = 0;
-//	return avgtime;
-
+	//	float avgtime = g_count_traversing;
+	//	g_count_traversing = 0;
+	//	return avgtime;
 }
 
-#endif //TRI_COLLISION_PROFILING
+#endif  //TRI_COLLISION_PROFILING
 
 /////////////////////// btBvhTree /////////////////////////////////
 
 int btBvhTree::_calc_splitting_axis(
-	GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
+	GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex)
 {
-
 	int i;
 
-	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
-	btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
-	int numIndices = endIndex-startIndex;
+	btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
+	btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.));
+	int numIndices = endIndex - startIndex;
 
-	for (i=startIndex;i<endIndex;i++)
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
-					 primitive_boxes[i].m_bound.m_min);
-		means+=center;
+		btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
+											primitive_boxes[i].m_bound.m_min);
+		means += center;
 	}
-	means *= (btScalar(1.)/(btScalar)numIndices);
+	means *= (btScalar(1.) / (btScalar)numIndices);
 
-	for (i=startIndex;i<endIndex;i++)
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
-					 primitive_boxes[i].m_bound.m_min);
-		btVector3 diff2 = center-means;
+		btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
+											primitive_boxes[i].m_bound.m_min);
+		btVector3 diff2 = center - means;
 		diff2 = diff2 * diff2;
 		variance += diff2;
 	}
-	variance *= (btScalar(1.)/	((btScalar)numIndices-1)	);
+	variance *= (btScalar(1.) / ((btScalar)numIndices - 1));
 
 	return variance.maxAxis();
 }
 
-
 int btBvhTree::_sort_and_calc_splitting_index(
-	GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
+	GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex,
 	int endIndex, int splitAxis)
 {
 	int i;
-	int splitIndex =startIndex;
+	int splitIndex = startIndex;
 	int numIndices = endIndex - startIndex;
 
 	// average of centers
 	btScalar splitValue = 0.0f;
 
-	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
-	for (i=startIndex;i<endIndex;i++)
+	btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
-					 primitive_boxes[i].m_bound.m_min);
-		means+=center;
+		btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
+											primitive_boxes[i].m_bound.m_min);
+		means += center;
 	}
-	means *= (btScalar(1.)/(btScalar)numIndices);
+	means *= (btScalar(1.) / (btScalar)numIndices);
 
 	splitValue = means[splitAxis];
 
-
 	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
-	for (i=startIndex;i<endIndex;i++)
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
-					 primitive_boxes[i].m_bound.m_min);
+		btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
+											primitive_boxes[i].m_bound.m_min);
 		if (center[splitAxis] > splitValue)
 		{
 			//swap
-			primitive_boxes.swap(i,splitIndex);
+			primitive_boxes.swap(i, splitIndex);
 			//swapLeafNodes(i,splitIndex);
 			splitIndex++;
 		}
@@ -142,32 +137,30 @@ int btBvhTree::_sort_and_calc_splitting_index(
 	//bool unbalanced2 = true;
 
 	//this should be safe too:
-	int rangeBalancedIndices = numIndices/3;
-	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+	int rangeBalancedIndices = numIndices / 3;
+	bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices)));
 
 	if (unbalanced)
 	{
-		splitIndex = startIndex+ (numIndices>>1);
+		splitIndex = startIndex + (numIndices >> 1);
 	}
 
-	btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
+	btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex))));
 
 	return splitIndex;
-
 }
 
-
-void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
+void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex)
 {
 	int curIndex = m_num_nodes;
 	m_num_nodes++;
 
-	btAssert((endIndex-startIndex)>0);
+	btAssert((endIndex - startIndex) > 0);
 
-	if ((endIndex-startIndex)==1)
+	if ((endIndex - startIndex) == 1)
 	{
-	    //We have a leaf node
-	    setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
+		//We have a leaf node
+		setNodeBound(curIndex, primitive_boxes[startIndex].m_bound);
 		m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
 
 		return;
@@ -175,47 +168,42 @@ void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startI
 	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
 
 	//split axis
-	int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
+	int splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex);
 
 	splitIndex = _sort_and_calc_splitting_index(
-			primitive_boxes,startIndex,endIndex,
-			splitIndex//split axis
-			);
-
+		primitive_boxes, startIndex, endIndex,
+		splitIndex  //split axis
+	);
 
 	//calc this node bounding box
 
 	btAABB node_bound;
 	node_bound.invalidate();
 
-	for (int i=startIndex;i<endIndex;i++)
+	for (int i = startIndex; i < endIndex; i++)
 	{
 		node_bound.merge(primitive_boxes[i].m_bound);
 	}
 
-	setNodeBound(curIndex,node_bound);
-
+	setNodeBound(curIndex, node_bound);
 
 	//build left branch
-	_build_sub_tree(primitive_boxes, startIndex, splitIndex );
-
+	_build_sub_tree(primitive_boxes, startIndex, splitIndex);
 
 	//build right branch
-	 _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
+	_build_sub_tree(primitive_boxes, splitIndex, endIndex);
 
 	m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
-
-
 }
 
 //! stackless build tree
 void btBvhTree::build_tree(
-	GIM_BVH_DATA_ARRAY & primitive_boxes)
+	GIM_BVH_DATA_ARRAY& primitive_boxes)
 {
 	// initialize node count to 0
 	m_num_nodes = 0;
 	// allocate nodes
-	m_node_array.resize(primitive_boxes.size()*2);
+	m_node_array.resize(primitive_boxes.size() * 2);
 
 	_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
 }
@@ -225,13 +213,13 @@ void btBvhTree::build_tree(
 void btGImpactBvh::refit()
 {
 	int nodecount = getNodeCount();
-	while(nodecount--)
+	while (nodecount--)
 	{
-		if(isLeafNode(nodecount))
+		if (isLeafNode(nodecount))
 		{
 			btAABB leafbox;
-			m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
-			setNodeBound(nodecount,leafbox);
+			m_primitive_manager->get_primitive_box(getNodeData(nodecount), leafbox);
+			setNodeBound(nodecount, leafbox);
 		}
 		else
 		{
@@ -243,20 +231,20 @@ void btGImpactBvh::refit()
 			btAABB temp_box;
 
 			int child_node = getLeftNode(nodecount);
-			if(child_node)
+			if (child_node)
 			{
-				getNodeBound(child_node,temp_box);
+				getNodeBound(child_node, temp_box);
 				bound.merge(temp_box);
 			}
 
 			child_node = getRightNode(nodecount);
-			if(child_node)
+			if (child_node)
 			{
-				getNodeBound(child_node,temp_box);
+				getNodeBound(child_node, temp_box);
 				bound.merge(temp_box);
 			}
 
-			setNodeBound(nodecount,bound);
+			setNodeBound(nodecount, bound);
 		}
 	}
 }
@@ -268,17 +256,17 @@ void btGImpactBvh::buildSet()
 	GIM_BVH_DATA_ARRAY primitive_boxes;
 	primitive_boxes.resize(m_primitive_manager->get_primitive_count());
 
-	for (int i = 0;i<primitive_boxes.size() ;i++ )
+	for (int i = 0; i < primitive_boxes.size(); i++)
 	{
-		 m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
-		 primitive_boxes[i].m_data = i;
+		m_primitive_manager->get_primitive_box(i, primitive_boxes[i].m_bound);
+		primitive_boxes[i].m_data = i;
 	}
 
 	m_box_tree.build_tree(primitive_boxes);
 }
 
 //! returns the indices of the primitives in the m_primitive_manager
-bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
+bool btGImpactBvh::boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const
 {
 	int curIndex = 0;
 	int numNodes = getNodeCount();
@@ -286,7 +274,7 @@ bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & coll
 	while (curIndex < numNodes)
 	{
 		btAABB bound;
-		getNodeBound(curIndex,bound);
+		getNodeBound(curIndex, bound);
 
 		//catch bugs in tree data
 
@@ -306,19 +294,17 @@ bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & coll
 		else
 		{
 			//skip node
-			curIndex+= getEscapeNodeIndex(curIndex);
+			curIndex += getEscapeNodeIndex(curIndex);
 		}
 	}
-	if(collided_results.size()>0) return true;
+	if (collided_results.size() > 0) return true;
 	return false;
 }
 
-
-
 //! returns the indices of the primitives in the m_primitive_manager
 bool btGImpactBvh::rayQuery(
-	const btVector3 & ray_dir,const btVector3 & ray_origin ,
-	btAlignedObjectArray<int> & collided_results) const
+	const btVector3& ray_dir, const btVector3& ray_origin,
+	btAlignedObjectArray<int>& collided_results) const
 {
 	int curIndex = 0;
 	int numNodes = getNodeCount();
@@ -326,16 +312,16 @@ bool btGImpactBvh::rayQuery(
 	while (curIndex < numNodes)
 	{
 		btAABB bound;
-		getNodeBound(curIndex,bound);
+		getNodeBound(curIndex, bound);
 
 		//catch bugs in tree data
 
-		bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
+		bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir);
 		bool isleafnode = isLeafNode(curIndex);
 
 		if (isleafnode && aabbOverlap)
 		{
-			collided_results.push_back(getNodeData( curIndex));
+			collided_results.push_back(getNodeData(curIndex));
 		}
 
 		if (aabbOverlap || isleafnode)
@@ -346,153 +332,133 @@ bool btGImpactBvh::rayQuery(
 		else
 		{
 			//skip node
-			curIndex+= getEscapeNodeIndex(curIndex);
+			curIndex += getEscapeNodeIndex(curIndex);
 		}
 	}
-	if(collided_results.size()>0) return true;
+	if (collided_results.size() > 0) return true;
 	return false;
 }
 
-
 SIMD_FORCE_INLINE bool _node_collision(
-	btGImpactBvh * boxset0, btGImpactBvh * boxset1,
-	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
-	int node0 ,int node1, bool complete_primitive_tests)
+	btGImpactBvh* boxset0, btGImpactBvh* boxset1,
+	const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0,
+	int node0, int node1, bool complete_primitive_tests)
 {
 	btAABB box0;
-	boxset0->getNodeBound(node0,box0);
+	boxset0->getNodeBound(node0, box0);
 	btAABB box1;
-	boxset1->getNodeBound(node1,box1);
-
-	return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
-//	box1.appy_transform_trans_cache(trans_cache_1to0);
-//	return box0.has_collision(box1);
+	boxset1->getNodeBound(node1, box1);
 
+	return box0.overlapping_trans_cache(box1, trans_cache_1to0, complete_primitive_tests);
+	//	box1.appy_transform_trans_cache(trans_cache_1to0);
+	//	return box0.has_collision(box1);
 }
 
-
 //stackless recursive collision routine
 static void _find_collision_pairs_recursive(
-	btGImpactBvh * boxset0, btGImpactBvh * boxset1,
-	btPairSet * collision_pairs,
-	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+	btGImpactBvh* boxset0, btGImpactBvh* boxset1,
+	btPairSet* collision_pairs,
+	const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0,
 	int node0, int node1, bool complete_primitive_tests)
 {
+	if (_node_collision(
+			boxset0, boxset1, trans_cache_1to0,
+			node0, node1, complete_primitive_tests) == false) return;  //avoid colliding internal nodes
 
-
-
-	if( _node_collision(
-		boxset0,boxset1,trans_cache_1to0,
-		node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
-
-	if(boxset0->isLeafNode(node0))
+	if (boxset0->isLeafNode(node0))
 	{
-		if(boxset1->isLeafNode(node1))
+		if (boxset1->isLeafNode(node1))
 		{
 			// collision result
 			collision_pairs->push_pair(
-				boxset0->getNodeData(node0),boxset1->getNodeData(node1));
+				boxset0->getNodeData(node0), boxset1->getNodeData(node1));
 			return;
 		}
 		else
 		{
-
 			//collide left recursive
 
 			_find_collision_pairs_recursive(
-								boxset0,boxset1,
-								collision_pairs,trans_cache_1to0,
-								node0,boxset1->getLeftNode(node1),false);
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				node0, boxset1->getLeftNode(node1), false);
 
 			//collide right recursive
 			_find_collision_pairs_recursive(
-								boxset0,boxset1,
-								collision_pairs,trans_cache_1to0,
-								node0,boxset1->getRightNode(node1),false);
-
-
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				node0, boxset1->getRightNode(node1), false);
 		}
 	}
 	else
 	{
-		if(boxset1->isLeafNode(node1))
+		if (boxset1->isLeafNode(node1))
 		{
-
 			//collide left recursive
 			_find_collision_pairs_recursive(
-								boxset0,boxset1,
-								collision_pairs,trans_cache_1to0,
-								boxset0->getLeftNode(node0),node1,false);
-
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getLeftNode(node0), node1, false);
 
 			//collide right recursive
 
 			_find_collision_pairs_recursive(
-								boxset0,boxset1,
-								collision_pairs,trans_cache_1to0,
-								boxset0->getRightNode(node0),node1,false);
-
-
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getRightNode(node0), node1, false);
 		}
 		else
 		{
 			//collide left0 left1
 
-
-
 			_find_collision_pairs_recursive(
-				boxset0,boxset1,
-				collision_pairs,trans_cache_1to0,
-				boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getLeftNode(node0), boxset1->getLeftNode(node1), false);
 
 			//collide left0 right1
 
 			_find_collision_pairs_recursive(
-				boxset0,boxset1,
-				collision_pairs,trans_cache_1to0,
-				boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
-
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getLeftNode(node0), boxset1->getRightNode(node1), false);
 
 			//collide right0 left1
 
 			_find_collision_pairs_recursive(
-				boxset0,boxset1,
-				collision_pairs,trans_cache_1to0,
-				boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getRightNode(node0), boxset1->getLeftNode(node1), false);
 
 			//collide right0 right1
 
 			_find_collision_pairs_recursive(
-				boxset0,boxset1,
-				collision_pairs,trans_cache_1to0,
-				boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getRightNode(node0), boxset1->getRightNode(node1), false);
 
-		}// else if node1 is not a leaf
-	}// else if node0 is not a leaf
+		}  // else if node1 is not a leaf
+	}      // else if node0 is not a leaf
 }
 
-
-void btGImpactBvh::find_collision(btGImpactBvh * boxset0, const btTransform & trans0,
-		btGImpactBvh * boxset1, const btTransform & trans1,
-		btPairSet & collision_pairs)
+void btGImpactBvh::find_collision(btGImpactBvh* boxset0, const btTransform& trans0,
+								  btGImpactBvh* boxset1, const btTransform& trans1,
+								  btPairSet& collision_pairs)
 {
-
-	if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
+	if (boxset0->getNodeCount() == 0 || boxset1->getNodeCount() == 0) return;
 
 	BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
 
-	trans_cache_1to0.calc_from_homogenic(trans0,trans1);
+	trans_cache_1to0.calc_from_homogenic(trans0, trans1);
 
 #ifdef TRI_COLLISION_PROFILING
 	bt_begin_gim02_tree_time();
-#endif //TRI_COLLISION_PROFILING
+#endif  //TRI_COLLISION_PROFILING
 
 	_find_collision_pairs_recursive(
-		boxset0,boxset1,
-		&collision_pairs,trans_cache_1to0,0,0,true);
+		boxset0, boxset1,
+		&collision_pairs, trans_cache_1to0, 0, 0, true);
 #ifdef TRI_COLLISION_PROFILING
 	bt_end_gim02_tree_time();
-#endif //TRI_COLLISION_PROFILING
-
+#endif  //TRI_COLLISION_PROFILING
 }
-
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h
index 6174ae97a97..3cd8fa24e77 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvh.h
@@ -1,5 +1,5 @@
-#ifndef GIM_BOX_SET_H_INCLUDED
-#define GIM_BOX_SET_H_INCLUDED
+#ifndef BT_GIMPACT_BVH_H_INCLUDED
+#define BT_GIMPACT_BVH_H_INCLUDED
 
 /*! \file gim_box_set.h
 \author Francisco Leon Najera
@@ -24,135 +24,55 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "LinearMath/btAlignedObjectArray.h"
 
 #include "btBoxCollision.h"
 #include "btTriangleShapeEx.h"
-
-
-
-
-
-//! Overlapping pair
-struct GIM_PAIR
-{
-    int m_index1;
-    int m_index2;
-    GIM_PAIR()
-    {}
-
-    GIM_PAIR(const GIM_PAIR & p)
-    {
-    	m_index1 = p.m_index1;
-    	m_index2 = p.m_index2;
-	}
-
-	GIM_PAIR(int index1, int index2)
-    {
-    	m_index1 = index1;
-    	m_index2 = index2;
-	}
-};
+#include "btGImpactBvhStructs.h"
 
 //! A pairset array
-class btPairSet: public btAlignedObjectArray<GIM_PAIR>
+class btPairSet : public btAlignedObjectArray<GIM_PAIR>
 {
 public:
 	btPairSet()
 	{
 		reserve(32);
 	}
-	inline void push_pair(int index1,int index2)
+	inline void push_pair(int index1, int index2)
 	{
-		push_back(GIM_PAIR(index1,index2));
+		push_back(GIM_PAIR(index1, index2));
 	}
 
-	inline void push_pair_inv(int index1,int index2)
+	inline void push_pair_inv(int index1, int index2)
 	{
-		push_back(GIM_PAIR(index2,index1));
+		push_back(GIM_PAIR(index2, index1));
 	}
 };
 
-
-///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box
-struct GIM_BVH_DATA
+class GIM_BVH_DATA_ARRAY : public btAlignedObjectArray<GIM_BVH_DATA>
 {
-	btAABB m_bound;
-	int m_data;
 };
 
-//! Node Structure for trees
-class GIM_BVH_TREE_NODE
-{
-public:
-	btAABB m_bound;
-protected:
-	int	m_escapeIndexOrDataIndex;
-public:
-	GIM_BVH_TREE_NODE()
-	{
-		m_escapeIndexOrDataIndex = 0;
-	}
-
-	SIMD_FORCE_INLINE bool isLeafNode() const
-	{
-		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
-		return (m_escapeIndexOrDataIndex>=0);
-	}
-
-	SIMD_FORCE_INLINE int getEscapeIndex() const
-	{
-		//btAssert(m_escapeIndexOrDataIndex < 0);
-		return -m_escapeIndexOrDataIndex;
-	}
-
-	SIMD_FORCE_INLINE void setEscapeIndex(int index)
-	{
-		m_escapeIndexOrDataIndex = -index;
-	}
-
-	SIMD_FORCE_INLINE int getDataIndex() const
-	{
-		//btAssert(m_escapeIndexOrDataIndex >= 0);
-
-		return m_escapeIndexOrDataIndex;
-	}
-
-	SIMD_FORCE_INLINE void setDataIndex(int index)
-	{
-		m_escapeIndexOrDataIndex = index;
-	}
-
-};
-
-
-class GIM_BVH_DATA_ARRAY:public btAlignedObjectArray<GIM_BVH_DATA>
-{
-};
-
-
-class GIM_BVH_TREE_NODE_ARRAY:public btAlignedObjectArray<GIM_BVH_TREE_NODE>
+class GIM_BVH_TREE_NODE_ARRAY : public btAlignedObjectArray<GIM_BVH_TREE_NODE>
 {
 };
 
-
-
-
 //! Basic Box tree structure
 class btBvhTree
 {
 protected:
 	int m_num_nodes;
 	GIM_BVH_TREE_NODE_ARRAY m_node_array;
+
 protected:
 	int _sort_and_calc_splitting_index(
-		GIM_BVH_DATA_ARRAY & primitive_boxes,
-		 int startIndex,  int endIndex, int splitAxis);
+		GIM_BVH_DATA_ARRAY& primitive_boxes,
+		int startIndex, int endIndex, int splitAxis);
 
-	int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);
+	int _calc_splitting_axis(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex);
+
+	void _build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex);
 
-	void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);
 public:
 	btBvhTree()
 	{
@@ -161,7 +81,7 @@ public:
 
 	//! prototype functions for box tree management
 	//!@{
-	void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes);
+	void build_tree(GIM_BVH_DATA_ARRAY& primitive_boxes);
 
 	SIMD_FORCE_INLINE void clearNodes()
 	{
@@ -186,25 +106,25 @@ public:
 		return m_node_array[nodeindex].getDataIndex();
 	}
 
-	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const
+	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const
 	{
 		bound = m_node_array[nodeindex].m_bound;
 	}
 
-	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound)
 	{
 		m_node_array[nodeindex].m_bound = bound;
 	}
 
 	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
 	{
-		return nodeindex+1;
+		return nodeindex + 1;
 	}
 
 	SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
 	{
-		if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2;
-		return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex();
+		if (m_node_array[nodeindex + 1].isLeafNode()) return nodeindex + 2;
+		return nodeindex + 1 + m_node_array[nodeindex + 1].getEscapeIndex();
 	}
 
 	SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
@@ -212,7 +132,7 @@ public:
 		return m_node_array[nodeindex].getEscapeIndex();
 	}
 
-	SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const
+	SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE* get_node_pointer(int index = 0) const
 	{
 		return &m_node_array[index];
 	}
@@ -220,7 +140,6 @@ public:
 	//!@}
 };
 
-
 //! Prototype Base class for primitive classification
 /*!
 This class is a wrapper for primitive collections.
@@ -230,18 +149,16 @@ This class can manage Compound shapes and trimeshes, and if it is managing trime
 class btPrimitiveManagerBase
 {
 public:
-
 	virtual ~btPrimitiveManagerBase() {}
 
 	//! determines if this manager consist on only triangles, which special case will be optimized
 	virtual bool is_trimesh() const = 0;
 	virtual int get_primitive_count() const = 0;
-	virtual void get_primitive_box(int prim_index ,btAABB & primbox) const = 0;
+	virtual void get_primitive_box(int prim_index, btAABB& primbox) const = 0;
 	//! retrieves only the points of the triangle, and the collision margin
-	virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const= 0;
+	virtual void get_primitive_triangle(int prim_index, btPrimitiveTriangle& triangle) const = 0;
 };
 
-
 //! Structure for containing Boxes
 /*!
 This class offers an structure for managing a box tree of primitives.
@@ -251,13 +168,13 @@ class btGImpactBvh
 {
 protected:
 	btBvhTree m_box_tree;
-	btPrimitiveManagerBase * m_primitive_manager;
+	btPrimitiveManagerBase* m_primitive_manager;
 
 protected:
 	//stackless refit
 	void refit();
-public:
 
+public:
 	//! this constructor doesn't build the tree. you must call	buildSet
 	btGImpactBvh()
 	{
@@ -265,31 +182,30 @@ public:
 	}
 
 	//! this constructor doesn't build the tree. you must call	buildSet
-	btGImpactBvh(btPrimitiveManagerBase * primitive_manager)
+	btGImpactBvh(btPrimitiveManagerBase* primitive_manager)
 	{
 		m_primitive_manager = primitive_manager;
 	}
 
-	SIMD_FORCE_INLINE btAABB getGlobalBox()  const
+	SIMD_FORCE_INLINE btAABB getGlobalBox() const
 	{
 		btAABB totalbox;
 		getNodeBound(0, totalbox);
 		return totalbox;
 	}
 
-	SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager)
+	SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase* primitive_manager)
 	{
 		m_primitive_manager = primitive_manager;
 	}
 
-	SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const
+	SIMD_FORCE_INLINE btPrimitiveManagerBase* getPrimitiveManager() const
 	{
 		return m_primitive_manager;
 	}
 
-
-//! node manager prototype functions
-///@{
+	//! node manager prototype functions
+	///@{
 
 	//! this attemps to refit the box set.
 	SIMD_FORCE_INLINE void update()
@@ -301,21 +217,21 @@ public:
 	void buildSet();
 
 	//! returns the indices of the primitives in the m_primitive_manager
-	bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const;
+	bool boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const;
 
 	//! returns the indices of the primitives in the m_primitive_manager
-	SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box,
-		 const btTransform & transform, btAlignedObjectArray<int> & collided_results) const
+	SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB& box,
+										 const btTransform& transform, btAlignedObjectArray<int>& collided_results) const
 	{
-		btAABB transbox=box;
+		btAABB transbox = box;
 		transbox.appy_transform(transform);
-		return boxQuery(transbox,collided_results);
+		return boxQuery(transbox, collided_results);
 	}
 
 	//! returns the indices of the primitives in the m_primitive_manager
 	bool rayQuery(
-		const btVector3 & ray_dir,const btVector3 & ray_origin ,
-		btAlignedObjectArray<int> & collided_results) const;
+		const btVector3& ray_dir, const btVector3& ray_origin,
+		btAlignedObjectArray<int>& collided_results) const;
 
 	//! tells if this set has hierarcht
 	SIMD_FORCE_INLINE bool hasHierarchy() const
@@ -324,7 +240,7 @@ public:
 	}
 
 	//! tells if this set is a trimesh
-	SIMD_FORCE_INLINE bool isTrimesh()  const
+	SIMD_FORCE_INLINE bool isTrimesh() const
 	{
 		return m_primitive_manager->is_trimesh();
 	}
@@ -346,17 +262,16 @@ public:
 		return m_box_tree.getNodeData(nodeindex);
 	}
 
-	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound)  const
+	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const
 	{
 		m_box_tree.getNodeBound(nodeindex, bound);
 	}
 
-	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound)
 	{
 		m_box_tree.setNodeBound(nodeindex, bound);
 	}
 
-
 	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
 	{
 		return m_box_tree.getLeftNode(nodeindex);
@@ -372,25 +287,23 @@ public:
 		return m_box_tree.getEscapeNodeIndex(nodeindex);
 	}
 
-	SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const
+	SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex, btPrimitiveTriangle& triangle) const
 	{
-		m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle);
+		m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex), triangle);
 	}
 
-
-	SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const
+	SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE* get_node_pointer(int index = 0) const
 	{
 		return m_box_tree.get_node_pointer(index);
 	}
 
 #ifdef TRI_COLLISION_PROFILING
 	static float getAverageTreeCollisionTime();
-#endif //TRI_COLLISION_PROFILING
+#endif  //TRI_COLLISION_PROFILING
 
-	static void find_collision(btGImpactBvh * boxset1, const btTransform & trans1,
-		btGImpactBvh * boxset2, const btTransform & trans2,
-		btPairSet & collision_pairs);
+	static void find_collision(btGImpactBvh* boxset1, const btTransform& trans1,
+							   btGImpactBvh* boxset2, const btTransform& trans2,
+							   btPairSet& collision_pairs);
 };
 
-
-#endif // GIM_BOXPRUNING_H_INCLUDED
+#endif  // BT_GIMPACT_BVH_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvhStructs.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvhStructs.h
new file mode 100644
index 00000000000..8f78c234b4d
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactBvhStructs.h
@@ -0,0 +1,85 @@
+#ifndef GIM_BOX_SET_STRUCT_H_INCLUDED
+#define GIM_BOX_SET_STRUCT_H_INCLUDED
+
+/*! \file gim_box_set.h
+\author Francisco Leon Najera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+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 "LinearMath/btAlignedObjectArray.h"
+
+#include "btBoxCollision.h"
+#include "btTriangleShapeEx.h"
+#include "gim_pair.h" //for GIM_PAIR
+
+///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box
+struct GIM_BVH_DATA
+{
+	btAABB m_bound;
+	int m_data;
+};
+
+//! Node Structure for trees
+class GIM_BVH_TREE_NODE
+{
+public:
+	btAABB m_bound;
+
+protected:
+	int m_escapeIndexOrDataIndex;
+
+public:
+	GIM_BVH_TREE_NODE()
+	{
+		m_escapeIndexOrDataIndex = 0;
+	}
+
+	SIMD_FORCE_INLINE bool isLeafNode() const
+	{
+		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+		return (m_escapeIndexOrDataIndex >= 0);
+	}
+
+	SIMD_FORCE_INLINE int getEscapeIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex < 0);
+		return -m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setEscapeIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = -index;
+	}
+
+	SIMD_FORCE_INLINE int getDataIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex >= 0);
+
+		return m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setDataIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = index;
+	}
+};
+
+#endif  // GIM_BOXPRUNING_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp
index 2e87475e393..73e3db10107 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp
@@ -18,7 +18,7 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 /*
-Author: Francisco Len Nßjera
+Author: Francisco Leon Najera
 Concave-Concave Collision
 
 */
@@ -31,18 +31,16 @@ Concave-Concave Collision
 #include "btContactProcessing.h"
 #include "LinearMath/btQuickprof.h"
 
-
 //! Class for accessing the plane equation
 class btPlaneShape : public btStaticPlaneShape
 {
 public:
-
 	btPlaneShape(const btVector3& v, float f)
-		:btStaticPlaneShape(v,f)
+		: btStaticPlaneShape(v, f)
 	{
 	}
 
-	void get_plane_equation(btVector4 &equation)
+	void get_plane_equation(btVector4& equation)
 	{
 		equation[0] = m_planeNormal[0];
 		equation[1] = m_planeNormal[1];
@@ -50,18 +48,16 @@ public:
 		equation[3] = m_planeConstant;
 	}
 
-
-	void get_plane_equation_transformed(const btTransform & trans,btVector4 &equation) const 
+	void get_plane_equation_transformed(const btTransform& trans, btVector4& equation) const
 	{
-		equation[0] = trans.getBasis().getRow(0).dot(m_planeNormal);
-		equation[1] = trans.getBasis().getRow(1).dot(m_planeNormal);
-		equation[2] = trans.getBasis().getRow(2).dot(m_planeNormal);
-		equation[3] = trans.getOrigin().dot(m_planeNormal) + m_planeConstant;
+		const btVector3 normal = trans.getBasis() * m_planeNormal;
+		equation[0] = normal[0];
+		equation[1] = normal[1];
+		equation[2] = normal[2];
+		equation[3] = normal.dot(trans * (m_planeConstant * m_planeNormal));
 	}
 };
 
-
-
 //////////////////////////////////////////////////////////////////////////////////////////////
 #ifdef TRI_COLLISION_PROFILING
 
@@ -80,7 +76,7 @@ void bt_end_gim02_tri_time()
 	g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds();
 	g_count_triangle_collision++;
 }
-#endif //TRI_COLLISION_PROFILING
+#endif  //TRI_COLLISION_PROFILING
 //! Retrieving shapes shapes
 /*!
 Declared here due of insuficent space on Pool allocators
@@ -89,7 +85,7 @@ Declared here due of insuficent space on Pool allocators
 class GIM_ShapeRetriever
 {
 public:
-	const btGImpactShapeInterface * m_gim_shape;
+	const btGImpactShapeInterface* m_gim_shape;
 	btTriangleShapeEx m_trishape;
 	btTetrahedronShapeEx m_tetrashape;
 
@@ -97,51 +93,50 @@ public:
 	class ChildShapeRetriever
 	{
 	public:
-		GIM_ShapeRetriever * m_parent;
-		virtual const btCollisionShape * getChildShape(int index)
+		GIM_ShapeRetriever* m_parent;
+		virtual const btCollisionShape* getChildShape(int index)
 		{
 			return m_parent->m_gim_shape->getChildShape(index);
 		}
 		virtual ~ChildShapeRetriever() {}
 	};
 
-	class TriangleShapeRetriever:public ChildShapeRetriever
+	class TriangleShapeRetriever : public ChildShapeRetriever
 	{
 	public:
-
-		virtual btCollisionShape * getChildShape(int index)
+		virtual btCollisionShape* getChildShape(int index)
 		{
-			m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape);
+			m_parent->m_gim_shape->getBulletTriangle(index, m_parent->m_trishape);
 			return &m_parent->m_trishape;
 		}
 		virtual ~TriangleShapeRetriever() {}
 	};
 
-	class TetraShapeRetriever:public ChildShapeRetriever
+	class TetraShapeRetriever : public ChildShapeRetriever
 	{
 	public:
-
-		virtual btCollisionShape * getChildShape(int index)
+		virtual btCollisionShape* getChildShape(int index)
 		{
-			m_parent->m_gim_shape->getBulletTetrahedron(index,m_parent->m_tetrashape);
+			m_parent->m_gim_shape->getBulletTetrahedron(index, m_parent->m_tetrashape);
 			return &m_parent->m_tetrashape;
 		}
 	};
+
 public:
 	ChildShapeRetriever m_child_retriever;
 	TriangleShapeRetriever m_tri_retriever;
-	TetraShapeRetriever  m_tetra_retriever;
-	ChildShapeRetriever * m_current_retriever;
+	TetraShapeRetriever m_tetra_retriever;
+	ChildShapeRetriever* m_current_retriever;
 
-	GIM_ShapeRetriever(const btGImpactShapeInterface * gim_shape)
+	GIM_ShapeRetriever(const btGImpactShapeInterface* gim_shape)
 	{
 		m_gim_shape = gim_shape;
 		//select retriever
-		if(m_gim_shape->needsRetrieveTriangles())
+		if (m_gim_shape->needsRetrieveTriangles())
 		{
 			m_current_retriever = &m_tri_retriever;
 		}
-		else if(m_gim_shape->needsRetrieveTetrahedrons())
+		else if (m_gim_shape->needsRetrieveTetrahedrons())
 		{
 			m_current_retriever = &m_tetra_retriever;
 		}
@@ -153,32 +148,26 @@ public:
 		m_current_retriever->m_parent = this;
 	}
 
-	const btCollisionShape * getChildShape(int index)
+	const btCollisionShape* getChildShape(int index)
 	{
 		return m_current_retriever->getChildShape(index);
 	}
-
-
 };
 
-
-
 //!@}
 
-
 #ifdef TRI_COLLISION_PROFILING
 
 //! Gets the average time in miliseconds of tree collisions
 float btGImpactCollisionAlgorithm::getAverageTreeCollisionTime()
 {
 	return btGImpactBoxSet::getAverageTreeCollisionTime();
-
 }
 
 //! Gets the average time in miliseconds of triangle collisions
 float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime()
 {
-	if(g_count_triangle_collision == 0) return 0;
+	if (g_count_triangle_collision == 0) return 0;
 
 	float avgtime = g_accum_triangle_collision_time;
 	avgtime /= (float)g_count_triangle_collision;
@@ -189,12 +178,10 @@ float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime()
 	return avgtime;
 }
 
-#endif //TRI_COLLISION_PROFILING
-
+#endif  //TRI_COLLISION_PROFILING
 
-
-btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
-: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap)
+btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
+	: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap)
 {
 	m_manifoldPtr = NULL;
 	m_convex_algorithm = NULL;
@@ -205,77 +192,62 @@ btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm()
 	clearCache();
 }
 
-
-
-
-
-void btGImpactCollisionAlgorithm::addContactPoint(const btCollisionObjectWrapper * body0Wrap,
-				const btCollisionObjectWrapper * body1Wrap,
-				const btVector3 & point,
-				const btVector3 & normal,
-				btScalar distance)
+void btGImpactCollisionAlgorithm::addContactPoint(const btCollisionObjectWrapper* body0Wrap,
+												  const btCollisionObjectWrapper* body1Wrap,
+												  const btVector3& point,
+												  const btVector3& normal,
+												  btScalar distance)
 {
-	m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
-	m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
-	checkManifold(body0Wrap,body1Wrap);
-	m_resultOut->addContactPoint(normal,point,distance);
+	m_resultOut->setShapeIdentifiersA(m_part0, m_triface0);
+	m_resultOut->setShapeIdentifiersB(m_part1, m_triface1);
+	checkManifold(body0Wrap, body1Wrap);
+	m_resultOut->addContactPoint(normal, point, distance);
 }
 
-
 void btGImpactCollisionAlgorithm::shape_vs_shape_collision(
-					  const btCollisionObjectWrapper * body0Wrap,
-					  const btCollisionObjectWrapper* body1Wrap,
-					  const btCollisionShape * shape0,
-					  const btCollisionShape * shape1)
+	const btCollisionObjectWrapper* body0Wrap,
+	const btCollisionObjectWrapper* body1Wrap,
+	const btCollisionShape* shape0,
+	const btCollisionShape* shape1)
 {
-
-
 	{
-		
-		btCollisionAlgorithm* algor = newAlgorithm(body0Wrap,body1Wrap);
+		btCollisionAlgorithm* algor = newAlgorithm(body0Wrap, body1Wrap);
 		// post :	checkManifold is called
 
-		m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
-		m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
-		
-		algor->processCollision(body0Wrap,body1Wrap,*m_dispatchInfo,m_resultOut);
-		
+		m_resultOut->setShapeIdentifiersA(m_part0, m_triface0);
+		m_resultOut->setShapeIdentifiersB(m_part1, m_triface1);
+
+		algor->processCollision(body0Wrap, body1Wrap, *m_dispatchInfo, m_resultOut);
+
 		algor->~btCollisionAlgorithm();
 		m_dispatcher->freeCollisionAlgorithm(algor);
 	}
-
 }
 
 void btGImpactCollisionAlgorithm::convex_vs_convex_collision(
-					  const btCollisionObjectWrapper* body0Wrap,
-					  const btCollisionObjectWrapper* body1Wrap,
-					  const btCollisionShape* shape0,
-					  const btCollisionShape* shape1)
+	const btCollisionObjectWrapper* body0Wrap,
+	const btCollisionObjectWrapper* body1Wrap,
+	const btCollisionShape* shape0,
+	const btCollisionShape* shape1)
 {
+	m_resultOut->setShapeIdentifiersA(m_part0, m_triface0);
+	m_resultOut->setShapeIdentifiersB(m_part1, m_triface1);
 
-	m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
-	m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
-
-	btCollisionObjectWrapper ob0(body0Wrap,shape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0);
-	btCollisionObjectWrapper ob1(body1Wrap,shape1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),m_part1,m_triface1);
-	checkConvexAlgorithm(&ob0,&ob1);
-	m_convex_algorithm->processCollision(&ob0,&ob1,*m_dispatchInfo,m_resultOut);
-
-
+	btCollisionObjectWrapper ob0(body0Wrap, shape0, body0Wrap->getCollisionObject(), body0Wrap->getWorldTransform(), m_part0, m_triface0);
+	btCollisionObjectWrapper ob1(body1Wrap, shape1, body1Wrap->getCollisionObject(), body1Wrap->getWorldTransform(), m_part1, m_triface1);
+	checkConvexAlgorithm(&ob0, &ob1);
+	m_convex_algorithm->processCollision(&ob0, &ob1, *m_dispatchInfo, m_resultOut);
 }
 
-
-
-
 void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs(
-					  const btTransform & trans0,
-					  const btTransform & trans1,
-					  const btGImpactShapeInterface * shape0,
-					  const btGImpactShapeInterface * shape1,btPairSet & pairset)
+	const btTransform& trans0,
+	const btTransform& trans1,
+	const btGImpactShapeInterface* shape0,
+	const btGImpactShapeInterface* shape1, btPairSet& pairset)
 {
-	if(shape0->hasBoxSet() && shape1->hasBoxSet())
+	if (shape0->hasBoxSet() && shape1->hasBoxSet())
 	{
-		btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset);
+		btGImpactBoxSet::find_collision(shape0->getBoxSet(), trans0, shape1->getBoxSet(), trans1, pairset);
 	}
 	else
 	{
@@ -283,74 +255,66 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs(
 		btAABB boxshape1;
 		int i = shape0->getNumChildShapes();
 
-		while(i--)
+		while (i--)
 		{
-			shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
+			shape0->getChildAabb(i, trans0, boxshape0.m_min, boxshape0.m_max);
 
 			int j = shape1->getNumChildShapes();
-			while(j--)
+			while (j--)
 			{
-				shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max);
+				shape1->getChildAabb(i, trans1, boxshape1.m_min, boxshape1.m_max);
 
-				if(boxshape1.has_collision(boxshape0))
+				if (boxshape1.has_collision(boxshape0))
 				{
-					pairset.push_pair(i,j);
+					pairset.push_pair(i, j);
 				}
 			}
 		}
 	}
-
-
 }
 
-
 void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs(
-					  const btTransform & trans0,
-					  const btTransform & trans1,
-					  const btGImpactShapeInterface * shape0,
-					  const btCollisionShape * shape1,
-					  btAlignedObjectArray<int> & collided_primitives)
+	const btTransform& trans0,
+	const btTransform& trans1,
+	const btGImpactShapeInterface* shape0,
+	const btCollisionShape* shape1,
+	btAlignedObjectArray<int>& collided_primitives)
 {
-
 	btAABB boxshape;
 
-
-	if(shape0->hasBoxSet())
+	if (shape0->hasBoxSet())
 	{
 		btTransform trans1to0 = trans0.inverse();
 		trans1to0 *= trans1;
 
-		shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max);
+		shape1->getAabb(trans1to0, boxshape.m_min, boxshape.m_max);
 
 		shape0->getBoxSet()->boxQuery(boxshape, collided_primitives);
 	}
 	else
 	{
-		shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max);
+		shape1->getAabb(trans1, boxshape.m_min, boxshape.m_max);
 
 		btAABB boxshape0;
 		int i = shape0->getNumChildShapes();
 
-		while(i--)
+		while (i--)
 		{
-			shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
+			shape0->getChildAabb(i, trans0, boxshape0.m_min, boxshape0.m_max);
 
-			if(boxshape.has_collision(boxshape0))
+			if (boxshape.has_collision(boxshape0))
 			{
 				collided_primitives.push_back(i);
 			}
 		}
-
 	}
-
 }
 
-
-void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectWrapper * body0Wrap,
-				  const btCollisionObjectWrapper * body1Wrap,
-				  const btGImpactMeshShapePart * shape0,
-				  const btGImpactMeshShapePart * shape1,
-				  const int * pairs, int pair_count)
+void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectWrapper* body0Wrap,
+														const btCollisionObjectWrapper* body1Wrap,
+														const btGImpactMeshShapePart* shape0,
+														const btGImpactMeshShapePart* shape1,
+														const int* pairs, int pair_count)
 {
 	btTriangleShapeEx tri0;
 	btTriangleShapeEx tri1;
@@ -358,27 +322,22 @@ void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectW
 	shape0->lockChildShapes();
 	shape1->lockChildShapes();
 
-	const int * pair_pointer = pairs;
+	const int* pair_pointer = pairs;
 
-	while(pair_count--)
+	while (pair_count--)
 	{
-
 		m_triface0 = *(pair_pointer);
-		m_triface1 = *(pair_pointer+1);
-		pair_pointer+=2;
-
-
-
-		shape0->getBulletTriangle(m_triface0,tri0);
-		shape1->getBulletTriangle(m_triface1,tri1);
+		m_triface1 = *(pair_pointer + 1);
+		pair_pointer += 2;
 
+		shape0->getBulletTriangle(m_triface0, tri0);
+		shape1->getBulletTriangle(m_triface1, tri1);
 
 		//collide two convex shapes
-		if(tri0.overlap_test_conservative(tri1))
+		if (tri0.overlap_test_conservative(tri1))
 		{
-			convex_vs_convex_collision(body0Wrap,body1Wrap,&tri0,&tri1);
+			convex_vs_convex_collision(body0Wrap, body1Wrap, &tri0, &tri1);
 		}
-
 	}
 
 	shape0->unlockChildShapes();
@@ -386,10 +345,10 @@ void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectW
 }
 
 void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap,
-					  const btCollisionObjectWrapper* body1Wrap,
-					  const btGImpactMeshShapePart * shape0,
-					  const btGImpactMeshShapePart * shape1,
-					  const int * pairs, int pair_count)
+														const btCollisionObjectWrapper* body1Wrap,
+														const btGImpactMeshShapePart* shape0,
+														const btGImpactMeshShapePart* shape1,
+														const int* pairs, int pair_count)
 {
 	btTransform orgtrans0 = body0Wrap->getWorldTransform();
 	btTransform orgtrans1 = body1Wrap->getWorldTransform();
@@ -401,119 +360,105 @@ void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectW
 	shape0->lockChildShapes();
 	shape1->lockChildShapes();
 
-	const int * pair_pointer = pairs;
+	const int* pair_pointer = pairs;
 
-	while(pair_count--)
+	while (pair_count--)
 	{
-
 		m_triface0 = *(pair_pointer);
-		m_triface1 = *(pair_pointer+1);
-		pair_pointer+=2;
-
+		m_triface1 = *(pair_pointer + 1);
+		pair_pointer += 2;
 
-		shape0->getPrimitiveTriangle(m_triface0,ptri0);
-		shape1->getPrimitiveTriangle(m_triface1,ptri1);
+		shape0->getPrimitiveTriangle(m_triface0, ptri0);
+		shape1->getPrimitiveTriangle(m_triface1, ptri1);
 
-		#ifdef TRI_COLLISION_PROFILING
+#ifdef TRI_COLLISION_PROFILING
 		bt_begin_gim02_tri_time();
-		#endif
+#endif
 
 		ptri0.applyTransform(orgtrans0);
 		ptri1.applyTransform(orgtrans1);
 
-
 		//build planes
 		ptri0.buildTriPlane();
 		ptri1.buildTriPlane();
 		// test conservative
 
-
-
-		if(ptri0.overlap_test_conservative(ptri1))
+		if (ptri0.overlap_test_conservative(ptri1))
 		{
-			if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data))
+			if (ptri0.find_triangle_collision_clip_method(ptri1, contact_data))
 			{
-
 				int j = contact_data.m_point_count;
-				while(j--)
+				while (j--)
 				{
-
 					addContactPoint(body0Wrap, body1Wrap,
-								contact_data.m_points[j],
-								contact_data.m_separating_normal,
-								-contact_data.m_penetration_depth);
+									contact_data.m_points[j],
+									contact_data.m_separating_normal,
+									-contact_data.m_penetration_depth);
 				}
 			}
 		}
 
-		#ifdef TRI_COLLISION_PROFILING
+#ifdef TRI_COLLISION_PROFILING
 		bt_end_gim02_tri_time();
-		#endif
-
+#endif
 	}
 
 	shape0->unlockChildShapes();
 	shape1->unlockChildShapes();
-
 }
 
-
 void btGImpactCollisionAlgorithm::gimpact_vs_gimpact(
-						const btCollisionObjectWrapper* body0Wrap,
-					   	const btCollisionObjectWrapper * body1Wrap,
-					  	const btGImpactShapeInterface * shape0,
-					  	const btGImpactShapeInterface * shape1)
+	const btCollisionObjectWrapper* body0Wrap,
+	const btCollisionObjectWrapper* body1Wrap,
+	const btGImpactShapeInterface* shape0,
+	const btGImpactShapeInterface* shape1)
 {
-
-	if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
+	if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE)
 	{
-		const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0);
+		const btGImpactMeshShape* meshshape0 = static_cast<const btGImpactMeshShape*>(shape0);
 		m_part0 = meshshape0->getMeshPartCount();
 
-		while(m_part0--)
+		while (m_part0--)
 		{
-			gimpact_vs_gimpact(body0Wrap,body1Wrap,meshshape0->getMeshPart(m_part0),shape1);
+			gimpact_vs_gimpact(body0Wrap, body1Wrap, meshshape0->getMeshPart(m_part0), shape1);
 		}
 
 		return;
 	}
 
-	if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
+	if (shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE)
 	{
-		const btGImpactMeshShape * meshshape1 = static_cast<const btGImpactMeshShape *>(shape1);
+		const btGImpactMeshShape* meshshape1 = static_cast<const btGImpactMeshShape*>(shape1);
 		m_part1 = meshshape1->getMeshPartCount();
 
-		while(m_part1--)
+		while (m_part1--)
 		{
-
-			gimpact_vs_gimpact(body0Wrap,body1Wrap,shape0,meshshape1->getMeshPart(m_part1));
-
+			gimpact_vs_gimpact(body0Wrap, body1Wrap, shape0, meshshape1->getMeshPart(m_part1));
 		}
 
 		return;
 	}
 
-
 	btTransform orgtrans0 = body0Wrap->getWorldTransform();
 	btTransform orgtrans1 = body1Wrap->getWorldTransform();
 
 	btPairSet pairset;
 
-	gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset);
+	gimpact_vs_gimpact_find_pairs(orgtrans0, orgtrans1, shape0, shape1, pairset);
 
-	if(pairset.size()== 0) return;
+	if (pairset.size() == 0) return;
 
-	if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
+	if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
 		shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART)
 	{
-		const btGImpactMeshShapePart * shapepart0 = static_cast<const btGImpactMeshShapePart * >(shape0);
-		const btGImpactMeshShapePart * shapepart1 = static_cast<const btGImpactMeshShapePart * >(shape1);
-		//specialized function
-		#ifdef BULLET_TRIANGLE_COLLISION
-		collide_gjk_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
-		#else
-		collide_sat_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
-		#endif
+		const btGImpactMeshShapePart* shapepart0 = static_cast<const btGImpactMeshShapePart*>(shape0);
+		const btGImpactMeshShapePart* shapepart1 = static_cast<const btGImpactMeshShapePart*>(shape1);
+//specialized function
+#ifdef BULLET_TRIANGLE_COLLISION
+		collide_gjk_triangles(body0Wrap, body1Wrap, shapepart0, shapepart1, &pairset[0].m_index1, pairset.size());
+#else
+		collide_sat_triangles(body0Wrap, body1Wrap, shapepart0, shapepart1, &pairset[0].m_index1, pairset.size());
+#endif
 
 		return;
 	}
@@ -530,32 +475,32 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact(
 	bool child_has_transform1 = shape1->childrenHasTransform();
 
 	int i = pairset.size();
-	while(i--)
+	while (i--)
 	{
-		GIM_PAIR * pair = &pairset[i];
+		GIM_PAIR* pair = &pairset[i];
 		m_triface0 = pair->m_index1;
 		m_triface1 = pair->m_index2;
-		const btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0);
-		const btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1);
+		const btCollisionShape* colshape0 = retriever0.getChildShape(m_triface0);
+		const btCollisionShape* colshape1 = retriever1.getChildShape(m_triface1);
 
 		btTransform tr0 = body0Wrap->getWorldTransform();
 		btTransform tr1 = body1Wrap->getWorldTransform();
 
-		if(child_has_transform0)
+		if (child_has_transform0)
 		{
-			tr0 = orgtrans0*shape0->getChildTransform(m_triface0);
+			tr0 = orgtrans0 * shape0->getChildTransform(m_triface0);
 		}
 
-		if(child_has_transform1)
+		if (child_has_transform1)
 		{
-			tr1 = orgtrans1*shape1->getChildTransform(m_triface1);
+			tr1 = orgtrans1 * shape1->getChildTransform(m_triface1);
 		}
 
-		btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),tr0,m_part0,m_triface0);
-		btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),tr1,m_part1,m_triface1);
+		btCollisionObjectWrapper ob0(body0Wrap, colshape0, body0Wrap->getCollisionObject(), tr0, m_part0, m_triface0);
+		btCollisionObjectWrapper ob1(body1Wrap, colshape1, body1Wrap->getCollisionObject(), tr1, m_part1, m_triface1);
 
 		//collide two convex shapes
-		convex_vs_convex_collision(&ob0,&ob1,colshape0,colshape1);
+		convex_vs_convex_collision(&ob0, &ob1, colshape0, colshape1);
 	}
 
 	shape0->unlockChildShapes();
@@ -563,159 +508,159 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact(
 }
 
 void btGImpactCollisionAlgorithm::gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap,
-				  const btCollisionObjectWrapper * body1Wrap,
-				  const btGImpactShapeInterface * shape0,
-				  const btCollisionShape * shape1,bool swapped)
+												   const btCollisionObjectWrapper* body1Wrap,
+												   const btGImpactShapeInterface* shape0,
+												   const btCollisionShape* shape1, bool swapped)
 {
-	if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
+	if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE)
 	{
-		const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0);
+		const btGImpactMeshShape* meshshape0 = static_cast<const btGImpactMeshShape*>(shape0);
 		int& part = swapped ? m_part1 : m_part0;
 		part = meshshape0->getMeshPartCount();
 
-		while(part--)
+		while (part--)
 		{
-
 			gimpact_vs_shape(body0Wrap,
-				  body1Wrap,
-				  meshshape0->getMeshPart(part),
-				  shape1,swapped);
-
+							 body1Wrap,
+							 meshshape0->getMeshPart(part),
+							 shape1, swapped);
 		}
 
 		return;
 	}
 
-	#ifdef GIMPACT_VS_PLANE_COLLISION
-	if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
+#ifdef GIMPACT_VS_PLANE_COLLISION
+	if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
 		shape1->getShapeType() == STATIC_PLANE_PROXYTYPE)
 	{
-		const btGImpactMeshShapePart * shapepart = static_cast<const btGImpactMeshShapePart *>(shape0);
-		const btStaticPlaneShape * planeshape = static_cast<const btStaticPlaneShape * >(shape1);
-		gimpacttrimeshpart_vs_plane_collision(body0Wrap,body1Wrap,shapepart,planeshape,swapped);
+		const btGImpactMeshShapePart* shapepart = static_cast<const btGImpactMeshShapePart*>(shape0);
+		const btStaticPlaneShape* planeshape = static_cast<const btStaticPlaneShape*>(shape1);
+		gimpacttrimeshpart_vs_plane_collision(body0Wrap, body1Wrap, shapepart, planeshape, swapped);
 		return;
 	}
 
-	#endif
-
+#endif
 
-
-	if(shape1->isCompound())
+	if (shape1->isCompound())
 	{
-		const btCompoundShape * compoundshape = static_cast<const btCompoundShape *>(shape1);
-		gimpact_vs_compoundshape(body0Wrap,body1Wrap,shape0,compoundshape,swapped);
+		const btCompoundShape* compoundshape = static_cast<const btCompoundShape*>(shape1);
+		gimpact_vs_compoundshape(body0Wrap, body1Wrap, shape0, compoundshape, swapped);
 		return;
 	}
-	else if(shape1->isConcave())
+	else if (shape1->isConcave())
 	{
-		const btConcaveShape * concaveshape = static_cast<const btConcaveShape *>(shape1);
-		gimpact_vs_concave(body0Wrap,body1Wrap,shape0,concaveshape,swapped);
+		const btConcaveShape* concaveshape = static_cast<const btConcaveShape*>(shape1);
+		gimpact_vs_concave(body0Wrap, body1Wrap, shape0, concaveshape, swapped);
 		return;
 	}
 
-
 	btTransform orgtrans0 = body0Wrap->getWorldTransform();
 
 	btTransform orgtrans1 = body1Wrap->getWorldTransform();
 
 	btAlignedObjectArray<int> collided_results;
 
-	gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results);
-
-	if(collided_results.size() == 0) return;
+	gimpact_vs_shape_find_pairs(orgtrans0, orgtrans1, shape0, shape1, collided_results);
 
+	if (collided_results.size() == 0) return;
 
 	shape0->lockChildShapes();
 
 	GIM_ShapeRetriever retriever0(shape0);
 
-
 	bool child_has_transform0 = shape0->childrenHasTransform();
 
-
 	int i = collided_results.size();
 
-	while(i--)
+	while (i--)
 	{
 		int child_index = collided_results[i];
-        if(swapped)
-    		m_triface1 = child_index;
-        else
-            m_triface0 = child_index;
+		if (swapped)
+			m_triface1 = child_index;
+		else
+			m_triface0 = child_index;
 
-		const btCollisionShape * colshape0 = retriever0.getChildShape(child_index);
+		const btCollisionShape* colshape0 = retriever0.getChildShape(child_index);
 
 		btTransform tr0 = body0Wrap->getWorldTransform();
 
-		if(child_has_transform0)
+		if (child_has_transform0)
 		{
-			tr0 = orgtrans0*shape0->getChildTransform(child_index);
+			tr0 = orgtrans0 * shape0->getChildTransform(child_index);
 		}
 
-		btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0);
-		const btCollisionObjectWrapper* prevObj0 = m_resultOut->getBody0Wrap();
-		
-		if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob0.getCollisionObject())
+		btCollisionObjectWrapper ob0(body0Wrap, colshape0, body0Wrap->getCollisionObject(), body0Wrap->getWorldTransform(), m_part0, m_triface0);
+		const btCollisionObjectWrapper* prevObj;
+
+		if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob0.getCollisionObject())
 		{
+			prevObj = m_resultOut->getBody0Wrap();
 			m_resultOut->setBody0Wrap(&ob0);
-		} else
+		}
+		else
 		{
+			prevObj = m_resultOut->getBody1Wrap();
 			m_resultOut->setBody1Wrap(&ob0);
 		}
 
 		//collide two shapes
-		if(swapped)
+		if (swapped)
 		{
-			
-			shape_vs_shape_collision(body1Wrap,&ob0,shape1,colshape0);
+			shape_vs_shape_collision(body1Wrap, &ob0, shape1, colshape0);
 		}
 		else
 		{
-			
-			shape_vs_shape_collision(&ob0,body1Wrap,colshape0,shape1);
+			shape_vs_shape_collision(&ob0, body1Wrap, colshape0, shape1);
 		}
-		m_resultOut->setBody0Wrap(prevObj0);
 
+		if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob0.getCollisionObject())
+		{
+			m_resultOut->setBody0Wrap(prevObj);
+		}
+		else
+		{
+			m_resultOut->setBody1Wrap(prevObj);
+		}
 	}
 
 	shape0->unlockChildShapes();
-
 }
 
 void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap,
-				  const btCollisionObjectWrapper* body1Wrap,
-				  const btGImpactShapeInterface * shape0,
-				  const btCompoundShape * shape1,bool swapped)
+														   const btCollisionObjectWrapper* body1Wrap,
+														   const btGImpactShapeInterface* shape0,
+														   const btCompoundShape* shape1, bool swapped)
 {
 	btTransform orgtrans1 = body1Wrap->getWorldTransform();
 
 	int i = shape1->getNumChildShapes();
-	while(i--)
+	while (i--)
 	{
+		const btCollisionShape* colshape1 = shape1->getChildShape(i);
+		btTransform childtrans1 = orgtrans1 * shape1->getChildTransform(i);
 
-		const btCollisionShape * colshape1 = shape1->getChildShape(i);
-		btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i);
+		btCollisionObjectWrapper ob1(body1Wrap, colshape1, body1Wrap->getCollisionObject(), childtrans1, -1, i);
 
-		btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),childtrans1,-1,i);
-		
 		const btCollisionObjectWrapper* tmp = 0;
-		if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject())
+		if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob1.getCollisionObject())
 		{
 			tmp = m_resultOut->getBody0Wrap();
 			m_resultOut->setBody0Wrap(&ob1);
-		} else
+		}
+		else
 		{
 			tmp = m_resultOut->getBody1Wrap();
 			m_resultOut->setBody1Wrap(&ob1);
 		}
 		//collide child shape
 		gimpact_vs_shape(body0Wrap, &ob1,
-					  shape0,colshape1,swapped);
+						 shape0, colshape1, swapped);
 
-		if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject())
+		if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob1.getCollisionObject())
 		{
 			m_resultOut->setBody0Wrap(tmp);
-		} else
+		}
+		else
 		{
 			m_resultOut->setBody1Wrap(tmp);
 		}
@@ -723,27 +668,25 @@ void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObje
 }
 
 void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision(
-					  const btCollisionObjectWrapper * body0Wrap,
-					  const btCollisionObjectWrapper * body1Wrap,
-					  const btGImpactMeshShapePart * shape0,
-					  const btStaticPlaneShape * shape1,bool swapped)
+	const btCollisionObjectWrapper* body0Wrap,
+	const btCollisionObjectWrapper* body1Wrap,
+	const btGImpactMeshShapePart* shape0,
+	const btStaticPlaneShape* shape1, bool swapped)
 {
-
-
 	btTransform orgtrans0 = body0Wrap->getWorldTransform();
 	btTransform orgtrans1 = body1Wrap->getWorldTransform();
 
-	const btPlaneShape * planeshape = static_cast<const btPlaneShape *>(shape1);
+	const btPlaneShape* planeshape = static_cast<const btPlaneShape*>(shape1);
 	btVector4 plane;
-	planeshape->get_plane_equation_transformed(orgtrans1,plane);
+	planeshape->get_plane_equation_transformed(orgtrans1, plane);
 
 	//test box against plane
 
 	btAABB tribox;
-	shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max);
+	shape0->getAabb(orgtrans0, tribox.m_min, tribox.m_max);
 	tribox.increment_margin(planeshape->getMargin());
 
-	if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return;
+	if (tribox.plane_classify(plane) != BT_CONST_COLLIDE_PLANE) return;
 
 	shape0->lockChildShapes();
 
@@ -751,28 +694,28 @@ void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision(
 
 	btVector3 vertex;
 	int vi = shape0->getVertexCount();
-	while(vi--)
+	while (vi--)
 	{
-		shape0->getVertex(vi,vertex);
+		shape0->getVertex(vi, vertex);
 		vertex = orgtrans0(vertex);
 
 		btScalar distance = vertex.dot(plane) - plane[3] - margin;
 
-		if(distance<0.0)//add contact
+		if (distance < 0.0)  //add contact
 		{
-			if(swapped)
+			if (swapped)
 			{
 				addContactPoint(body1Wrap, body0Wrap,
-					vertex,
-					-plane,
-					distance);
+								vertex,
+								-plane,
+								distance);
 			}
 			else
 			{
 				addContactPoint(body0Wrap, body1Wrap,
-					vertex,
-					plane,
-					distance);
+								vertex,
+								plane,
+								distance);
 			}
 		}
 	}
@@ -780,69 +723,64 @@ void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision(
 	shape0->unlockChildShapes();
 }
 
-
-
-
-class btGImpactTriangleCallback: public btTriangleCallback
+class btGImpactTriangleCallback : public btTriangleCallback
 {
 public:
-	btGImpactCollisionAlgorithm * algorithm;
-	const btCollisionObjectWrapper * body0Wrap;
-	const btCollisionObjectWrapper * body1Wrap;
-	const btGImpactShapeInterface * gimpactshape0;
+	btGImpactCollisionAlgorithm* algorithm;
+	const btCollisionObjectWrapper* body0Wrap;
+	const btCollisionObjectWrapper* body1Wrap;
+	const btGImpactShapeInterface* gimpactshape0;
 	bool swapped;
 	btScalar margin;
 
 	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 	{
-		btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]);
+		btTriangleShapeEx tri1(triangle[0], triangle[1], triangle[2]);
 		tri1.setMargin(margin);
-        if(swapped)
-        {
-            algorithm->setPart0(partId);
-            algorithm->setFace0(triangleIndex);
-        }
-        else
-        {
-            algorithm->setPart1(partId);
-            algorithm->setFace1(triangleIndex);
-        }
-
-		btCollisionObjectWrapper ob1Wrap(body1Wrap,&tri1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),partId,triangleIndex);
-		const btCollisionObjectWrapper * tmp = 0;
-
-		if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject())
+		if (swapped)
+		{
+			algorithm->setPart0(partId);
+			algorithm->setFace0(triangleIndex);
+		}
+		else
+		{
+			algorithm->setPart1(partId);
+			algorithm->setFace1(triangleIndex);
+		}
+
+		btCollisionObjectWrapper ob1Wrap(body1Wrap, &tri1, body1Wrap->getCollisionObject(), body1Wrap->getWorldTransform(), partId, triangleIndex);
+		const btCollisionObjectWrapper* tmp = 0;
+
+		if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject() == ob1Wrap.getCollisionObject())
 		{
 			tmp = algorithm->internalGetResultOut()->getBody0Wrap();
 			algorithm->internalGetResultOut()->setBody0Wrap(&ob1Wrap);
-		} else
+		}
+		else
 		{
 			tmp = algorithm->internalGetResultOut()->getBody1Wrap();
 			algorithm->internalGetResultOut()->setBody1Wrap(&ob1Wrap);
 		}
-		
+
 		algorithm->gimpact_vs_shape(
-							body0Wrap,&ob1Wrap,gimpactshape0,&tri1,swapped);
+			body0Wrap, &ob1Wrap, gimpactshape0, &tri1, swapped);
 
-		if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject())
+		if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject() == ob1Wrap.getCollisionObject())
 		{
 			algorithm->internalGetResultOut()->setBody0Wrap(tmp);
-		} else
+		}
+		else
 		{
 			algorithm->internalGetResultOut()->setBody1Wrap(tmp);
 		}
-
 	}
 };
 
-
-
-
 void btGImpactCollisionAlgorithm::gimpact_vs_concave(
-				  const btCollisionObjectWrapper* body0Wrap,
-				  const btCollisionObjectWrapper * body1Wrap,
-				  const btGImpactShapeInterface * shape0,
-				  const btConcaveShape * shape1,bool swapped)
+	const btCollisionObjectWrapper* body0Wrap,
+	const btCollisionObjectWrapper* body1Wrap,
+	const btGImpactShapeInterface* shape0,
+	const btConcaveShape* shape1, bool swapped)
 {
 	//create the callback
 	btGImpactTriangleCallback tricallback;
@@ -858,75 +796,71 @@ void btGImpactCollisionAlgorithm::gimpact_vs_concave(
 
 	gimpactInConcaveSpace = body1Wrap->getWorldTransform().inverse() * body0Wrap->getWorldTransform();
 
-	btVector3 minAABB,maxAABB;
-	shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB);
-
-	shape1->processAllTriangles(&tricallback,minAABB,maxAABB);
+	btVector3 minAABB, maxAABB;
+	shape0->getAabb(gimpactInConcaveSpace, minAABB, maxAABB);
 
+	shape1->processAllTriangles(&tricallback, minAABB, maxAABB);
 }
 
-
-
-void btGImpactCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btGImpactCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
-    clearCache();
+	clearCache();
 
-    m_resultOut = resultOut;
+	m_resultOut = resultOut;
 	m_dispatchInfo = &dispatchInfo;
-    const btGImpactShapeInterface * gimpactshape0;
-    const btGImpactShapeInterface * gimpactshape1;
+	const btGImpactShapeInterface* gimpactshape0;
+	const btGImpactShapeInterface* gimpactshape1;
 
-	if (body0Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE)
+	if (body0Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
 	{
-		gimpactshape0 = static_cast<const btGImpactShapeInterface *>(body0Wrap->getCollisionShape());
+		gimpactshape0 = static_cast<const btGImpactShapeInterface*>(body0Wrap->getCollisionShape());
 
-		if( body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
+		if (body1Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
 		{
-			gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape());
+			gimpactshape1 = static_cast<const btGImpactShapeInterface*>(body1Wrap->getCollisionShape());
 
-			gimpact_vs_gimpact(body0Wrap,body1Wrap,gimpactshape0,gimpactshape1);
+			gimpact_vs_gimpact(body0Wrap, body1Wrap, gimpactshape0, gimpactshape1);
 		}
 		else
 		{
-			gimpact_vs_shape(body0Wrap,body1Wrap,gimpactshape0,body1Wrap->getCollisionShape(),false);
+			gimpact_vs_shape(body0Wrap, body1Wrap, gimpactshape0, body1Wrap->getCollisionShape(), false);
 		}
-
 	}
-	else if (body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
+	else if (body1Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
 	{
-		gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape());
+		gimpactshape1 = static_cast<const btGImpactShapeInterface*>(body1Wrap->getCollisionShape());
 
-		gimpact_vs_shape(body1Wrap,body0Wrap,gimpactshape1,body0Wrap->getCollisionShape(),true);
+		gimpact_vs_shape(body1Wrap, body0Wrap, gimpactshape1, body0Wrap->getCollisionShape(), true);
 	}
-}
 
+	// Ensure that gContactProcessedCallback is called for concave shapes.
+	if (getLastManifold())
+	{
+		m_resultOut->refreshContactPoints();
+	}
+}
 
-btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	return 1.f;
-
 }
 
 ///////////////////////////////////// REGISTERING ALGORITHM //////////////////////////////////////////////
 
-
-
 //! Use this function for register the algorithm externally
-void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher)
+void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher* dispatcher)
 {
-
 	static btGImpactCollisionAlgorithm::CreateFunc s_gimpact_cf;
 
 	int i;
 
-	for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
+	for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++)
 	{
-		dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&s_gimpact_cf);
+		dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE, i, &s_gimpact_cf);
 	}
 
-	for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
+	for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++)
 	{
-		dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&s_gimpact_cf);
+		dispatcher->registerCollisionCreateFunc(i, GIMPACT_SHAPE_PROXYTYPE, &s_gimpact_cf);
 	}
-
 }
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h
index f85a94cb4c7..a368c8a0c0b 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h
@@ -42,7 +42,6 @@ class btDispatcher;
 #include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 
-
 //! Collision Algorithm for GImpact Shapes
 /*!
 For register this algorithm in Bullet, proceed as following:
@@ -54,36 +53,35 @@ btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher);
 class btGImpactCollisionAlgorithm : public btActivatingCollisionAlgorithm
 {
 protected:
-	btCollisionAlgorithm * m_convex_algorithm;
-    btPersistentManifold * m_manifoldPtr;
+	btCollisionAlgorithm* m_convex_algorithm;
+	btPersistentManifold* m_manifoldPtr;
 	btManifoldResult* m_resultOut;
-	const btDispatcherInfo * m_dispatchInfo;
+	const btDispatcherInfo* m_dispatchInfo;
 	int m_triface0;
 	int m_part0;
 	int m_triface1;
 	int m_part1;
 
-
 	//! Creates a new contact point
-	SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(const btCollisionObject* body0,const btCollisionObject* body1)
+	SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(const btCollisionObject* body0, const btCollisionObject* body1)
 	{
-		m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0, body1);
 		return m_manifoldPtr;
 	}
 
 	SIMD_FORCE_INLINE void destroyConvexAlgorithm()
 	{
-		if(m_convex_algorithm)
+		if (m_convex_algorithm)
 		{
 			m_convex_algorithm->~btCollisionAlgorithm();
-			m_dispatcher->freeCollisionAlgorithm( m_convex_algorithm);
+			m_dispatcher->freeCollisionAlgorithm(m_convex_algorithm);
 			m_convex_algorithm = NULL;
 		}
 	}
 
 	SIMD_FORCE_INLINE void destroyContactManifolds()
 	{
-		if(m_manifoldPtr == NULL) return;
+		if (m_manifoldPtr == NULL) return;
 		m_dispatcher->releaseManifold(m_manifoldPtr);
 		m_manifoldPtr = NULL;
 	}
@@ -104,207 +102,187 @@ protected:
 		return m_manifoldPtr;
 	}
 
-
 	// Call before process collision
-	SIMD_FORCE_INLINE void checkManifold(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+	SIMD_FORCE_INLINE void checkManifold(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 	{
-		if(getLastManifold() == 0)
+		if (getLastManifold() == 0)
 		{
-			newContactManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
+			newContactManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
 		}
 
 		m_resultOut->setPersistentManifold(getLastManifold());
 	}
 
 	// Call before process collision
-	SIMD_FORCE_INLINE btCollisionAlgorithm * newAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+	SIMD_FORCE_INLINE btCollisionAlgorithm* newAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 	{
-		checkManifold(body0Wrap,body1Wrap);
+		checkManifold(body0Wrap, body1Wrap);
 
-		btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm(
-				body0Wrap,body1Wrap,getLastManifold());
-		return convex_algorithm ;
+		btCollisionAlgorithm* convex_algorithm = m_dispatcher->findAlgorithm(
+			body0Wrap, body1Wrap, getLastManifold(), BT_CONTACT_POINT_ALGORITHMS);
+		return convex_algorithm;
 	}
 
 	// Call before process collision
-	SIMD_FORCE_INLINE void checkConvexAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+	SIMD_FORCE_INLINE void checkConvexAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 	{
-		if(m_convex_algorithm) return;
-		m_convex_algorithm = newAlgorithm(body0Wrap,body1Wrap);
+		if (m_convex_algorithm) return;
+		m_convex_algorithm = newAlgorithm(body0Wrap, body1Wrap);
 	}
 
+	void addContactPoint(const btCollisionObjectWrapper* body0Wrap,
+						 const btCollisionObjectWrapper* body1Wrap,
+						 const btVector3& point,
+						 const btVector3& normal,
+						 btScalar distance);
 
-
-
-	void addContactPoint(const btCollisionObjectWrapper * body0Wrap,
-					const btCollisionObjectWrapper * body1Wrap,
-					const btVector3 & point,
-					const btVector3 & normal,
-					btScalar distance);
-
-//! Collision routines
-//!@{
+	//! Collision routines
+	//!@{
 
 	void collide_gjk_triangles(const btCollisionObjectWrapper* body0Wrap,
-				  const btCollisionObjectWrapper* body1Wrap,
-				  const btGImpactMeshShapePart * shape0,
-				  const btGImpactMeshShapePart * shape1,
-				  const int * pairs, int pair_count);
+							   const btCollisionObjectWrapper* body1Wrap,
+							   const btGImpactMeshShapePart* shape0,
+							   const btGImpactMeshShapePart* shape1,
+							   const int* pairs, int pair_count);
 
 	void collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap,
-					  const btCollisionObjectWrapper* body1Wrap,
-					  const btGImpactMeshShapePart * shape0,
-					  const btGImpactMeshShapePart * shape1,
-					  const int * pairs, int pair_count);
-
-
-
+							   const btCollisionObjectWrapper* body1Wrap,
+							   const btGImpactMeshShapePart* shape0,
+							   const btGImpactMeshShapePart* shape1,
+							   const int* pairs, int pair_count);
 
 	void shape_vs_shape_collision(
-					  const btCollisionObjectWrapper* body0,
-					  const btCollisionObjectWrapper* body1,
-					  const btCollisionShape * shape0,
-					  const btCollisionShape * shape1);
+		const btCollisionObjectWrapper* body0,
+		const btCollisionObjectWrapper* body1,
+		const btCollisionShape* shape0,
+		const btCollisionShape* shape1);
 
 	void convex_vs_convex_collision(const btCollisionObjectWrapper* body0Wrap,
-					  const btCollisionObjectWrapper* body1Wrap,
-					  const btCollisionShape* shape0,
-					  const btCollisionShape* shape1);
-
-
+									const btCollisionObjectWrapper* body1Wrap,
+									const btCollisionShape* shape0,
+									const btCollisionShape* shape1);
 
 	void gimpact_vs_gimpact_find_pairs(
-					  const btTransform & trans0,
-					  const btTransform & trans1,
-					  const btGImpactShapeInterface * shape0,
-					  const btGImpactShapeInterface * shape1,btPairSet & pairset);
+		const btTransform& trans0,
+		const btTransform& trans1,
+		const btGImpactShapeInterface* shape0,
+		const btGImpactShapeInterface* shape1, btPairSet& pairset);
 
 	void gimpact_vs_shape_find_pairs(
-					  const btTransform & trans0,
-					  const btTransform & trans1,
-					  const btGImpactShapeInterface * shape0,
-					  const btCollisionShape * shape1,
-					  btAlignedObjectArray<int> & collided_primitives);
-
+		const btTransform& trans0,
+		const btTransform& trans1,
+		const btGImpactShapeInterface* shape0,
+		const btCollisionShape* shape1,
+		btAlignedObjectArray<int>& collided_primitives);
 
 	void gimpacttrimeshpart_vs_plane_collision(
-					  const btCollisionObjectWrapper * body0Wrap,
-					  const btCollisionObjectWrapper * body1Wrap,
-					  const btGImpactMeshShapePart * shape0,
-					  const btStaticPlaneShape * shape1,bool swapped);
-
+		const btCollisionObjectWrapper* body0Wrap,
+		const btCollisionObjectWrapper* body1Wrap,
+		const btGImpactMeshShapePart* shape0,
+		const btStaticPlaneShape* shape1, bool swapped);
 
 public:
-
-	btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+	btGImpactCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap);
 
 	virtual ~btGImpactCollisionAlgorithm();
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
 	{
 		if (m_manifoldPtr)
 			manifoldArray.push_back(m_manifoldPtr);
 	}
 
-	btManifoldResult*	internalGetResultOut()
+	btManifoldResult* internalGetResultOut()
 	{
 		return m_resultOut;
 	}
 
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btGImpactCollisionAlgorithm));
-			return new(mem) btGImpactCollisionAlgorithm(ci,body0Wrap,body1Wrap);
+			return new (mem) btGImpactCollisionAlgorithm(ci, body0Wrap, body1Wrap);
 		}
 	};
 
 	//! Use this function for register the algorithm externally
-	static void registerAlgorithm(btCollisionDispatcher * dispatcher);
+	static void registerAlgorithm(btCollisionDispatcher* dispatcher);
 #ifdef TRI_COLLISION_PROFILING
 	//! Gets the average time in miliseconds of tree collisions
 	static float getAverageTreeCollisionTime();
 
 	//! Gets the average time in miliseconds of triangle collisions
 	static float getAverageTriangleCollisionTime();
-#endif //TRI_COLLISION_PROFILING
+#endif  //TRI_COLLISION_PROFILING
 
 	//! Collides two gimpact shapes
 	/*!
 	\pre shape0 and shape1 couldn't be btGImpactMeshShape objects
 	*/
 
-
 	void gimpact_vs_gimpact(const btCollisionObjectWrapper* body0Wrap,
-					  const btCollisionObjectWrapper * body1Wrap,
-					  const btGImpactShapeInterface * shape0,
-					  const btGImpactShapeInterface * shape1);
+							const btCollisionObjectWrapper* body1Wrap,
+							const btGImpactShapeInterface* shape0,
+							const btGImpactShapeInterface* shape1);
 
 	void gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap,
-					  const btCollisionObjectWrapper* body1Wrap,
-					  const btGImpactShapeInterface * shape0,
-					  const btCollisionShape * shape1,bool swapped);
+						  const btCollisionObjectWrapper* body1Wrap,
+						  const btGImpactShapeInterface* shape0,
+						  const btCollisionShape* shape1, bool swapped);
 
-	void gimpact_vs_compoundshape(const btCollisionObjectWrapper * body0Wrap,
-					  const btCollisionObjectWrapper * body1Wrap,
-					  const btGImpactShapeInterface * shape0,
-					  const btCompoundShape * shape1,bool swapped);
+	void gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap,
+								  const btCollisionObjectWrapper* body1Wrap,
+								  const btGImpactShapeInterface* shape0,
+								  const btCompoundShape* shape1, bool swapped);
 
 	void gimpact_vs_concave(
-					  const btCollisionObjectWrapper * body0Wrap,
-					  const btCollisionObjectWrapper * body1Wrap,
-					  const btGImpactShapeInterface * shape0,
-					  const btConcaveShape * shape1,bool swapped);
-
-
-
-
-		/// Accessor/Mutator pairs for Part and triangleID
-    void 	setFace0(int value) 
-    { 
-    	m_triface0 = value; 
-    }
-    int getFace0() 
-    { 
-    	return m_triface0; 
-    }
-    void setFace1(int value) 
-    { 
-    	m_triface1 = value; 
-    }
-    int getFace1() 
-    { 
-    	return m_triface1; 
-    }
-    void setPart0(int value) 
-    { 
-    	m_part0 = value; 
-    }
-    int getPart0() 
-    { 
-    	return m_part0; 
-    }
-    void setPart1(int value) 
-    { 
-    	m_part1 = value; 
-		}
-    int getPart1() 
-    { 
-    	return m_part1; 
-    }
+		const btCollisionObjectWrapper* body0Wrap,
+		const btCollisionObjectWrapper* body1Wrap,
+		const btGImpactShapeInterface* shape0,
+		const btConcaveShape* shape1, bool swapped);
 
+	/// Accessor/Mutator pairs for Part and triangleID
+	void setFace0(int value)
+	{
+		m_triface0 = value;
+	}
+	int getFace0()
+	{
+		return m_triface0;
+	}
+	void setFace1(int value)
+	{
+		m_triface1 = value;
+	}
+	int getFace1()
+	{
+		return m_triface1;
+	}
+	void setPart0(int value)
+	{
+		m_part0 = value;
+	}
+	int getPart0()
+	{
+		return m_part0;
+	}
+	void setPart1(int value)
+	{
+		m_part1 = value;
+	}
+	int getPart1()
+	{
+		return m_part1;
+	}
 };
 
-
 //algorithm details
 //#define BULLET_TRIANGLE_COLLISION 1
 #define GIMPACT_VS_PLANE_COLLISION 1
 
-
-
-#endif //BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H
+#endif  //BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h
index 2543aefcfc6..1cde46ed8ba 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h
@@ -21,40 +21,36 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef GIMPACT_MASS_UTIL_H
 #define GIMPACT_MASS_UTIL_H
 
 #include "LinearMath/btTransform.h"
 
-
-
 SIMD_FORCE_INLINE btVector3 gim_inertia_add_transformed(
-	const btVector3 & source_inertia, const btVector3 & added_inertia, const btTransform & transform)
+	const btVector3& source_inertia, const btVector3& added_inertia, const btTransform& transform)
 {
-	btMatrix3x3  rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose();
+	btMatrix3x3 rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose();
 
 	btScalar x2 = transform.getOrigin()[0];
-	x2*= x2;
+	x2 *= x2;
 	btScalar y2 = transform.getOrigin()[1];
-	y2*= y2;
+	y2 *= y2;
 	btScalar z2 = transform.getOrigin()[2];
-	z2*= z2;
+	z2 *= z2;
 
-	btScalar ix = rotatedTensor[0][0]*(y2+z2);
-	btScalar iy = rotatedTensor[1][1]*(x2+z2);
-	btScalar iz = rotatedTensor[2][2]*(x2+y2);
+	btScalar ix = rotatedTensor[0][0] * (y2 + z2);
+	btScalar iy = rotatedTensor[1][1] * (x2 + z2);
+	btScalar iz = rotatedTensor[2][2] * (x2 + y2);
 
-	return btVector3(source_inertia[0]+ix,source_inertia[1]+iy,source_inertia[2] + iz);
+	return btVector3(source_inertia[0] + ix, source_inertia[1] + iy, source_inertia[2] + iz);
 }
 
-SIMD_FORCE_INLINE btVector3 gim_get_point_inertia(const btVector3 & point, btScalar mass)
+SIMD_FORCE_INLINE btVector3 gim_get_point_inertia(const btVector3& point, btScalar mass)
 {
-	btScalar x2 = point[0]*point[0];
-	btScalar y2 = point[1]*point[1];
-	btScalar z2 = point[2]*point[2];
-	return btVector3(mass*(y2+z2),mass*(x2+z2),mass*(x2+y2));
+	btScalar x2 = point[0] * point[0];
+	btScalar y2 = point[1] * point[1];
+	btScalar z2 = point[2] * point[2];
+	return btVector3(mass * (y2 + z2), mass * (x2 + z2), mass * (x2 + y2));
 }
 
-
-#endif //GIMPACT_MESH_SHAPE_H
+#endif  //GIMPACT_MESH_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
index 4528758c370..b81fc970449 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
@@ -27,11 +27,9 @@ subject to the following restrictions:
 #ifdef TRI_COLLISION_PROFILING
 btClock g_q_tree_clock;
 
-
 float g_q_accum_tree_collision_time = 0;
 int g_q_count_traversing = 0;
 
-
 void bt_begin_gim02_q_tree_time()
 {
 	g_q_tree_clock.reset();
@@ -43,11 +41,10 @@ void bt_end_gim02_q_tree_time()
 	g_q_count_traversing++;
 }
 
-
 //! Gets the average time in miliseconds of tree collisions
 float btGImpactQuantizedBvh::getAverageTreeCollisionTime()
 {
-	if(g_q_count_traversing == 0) return 0;
+	if (g_q_count_traversing == 0) return 0;
 
 	float avgtime = g_q_accum_tree_collision_time;
 	avgtime /= (float)g_q_count_traversing;
@@ -56,99 +53,92 @@ float btGImpactQuantizedBvh::getAverageTreeCollisionTime()
 	g_q_count_traversing = 0;
 	return avgtime;
 
-//	float avgtime = g_q_count_traversing;
-//	g_q_count_traversing = 0;
-//	return avgtime;
-
+	//	float avgtime = g_q_count_traversing;
+	//	g_q_count_traversing = 0;
+	//	return avgtime;
 }
 
-#endif //TRI_COLLISION_PROFILING
+#endif  //TRI_COLLISION_PROFILING
 
 /////////////////////// btQuantizedBvhTree /////////////////////////////////
 
 void btQuantizedBvhTree::calc_quantization(
-	GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin)
+	GIM_BVH_DATA_ARRAY& primitive_boxes, btScalar boundMargin)
 {
 	//calc globa box
 	btAABB global_bound;
 	global_bound.invalidate();
 
-	for (int i=0;i<primitive_boxes.size() ;i++ )
+	for (int i = 0; i < primitive_boxes.size(); i++)
 	{
 		global_bound.merge(primitive_boxes[i].m_bound);
 	}
 
 	bt_calc_quantization_parameters(
-		m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization,global_bound.m_min,global_bound.m_max,boundMargin);
-
+		m_global_bound.m_min, m_global_bound.m_max, m_bvhQuantization, global_bound.m_min, global_bound.m_max, boundMargin);
 }
 
-
-
 int btQuantizedBvhTree::_calc_splitting_axis(
-	GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
+	GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex)
 {
-
 	int i;
 
-	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
-	btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
-	int numIndices = endIndex-startIndex;
+	btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
+	btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.));
+	int numIndices = endIndex - startIndex;
 
-	for (i=startIndex;i<endIndex;i++)
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
-					 primitive_boxes[i].m_bound.m_min);
-		means+=center;
+		btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
+											primitive_boxes[i].m_bound.m_min);
+		means += center;
 	}
-	means *= (btScalar(1.)/(btScalar)numIndices);
+	means *= (btScalar(1.) / (btScalar)numIndices);
 
-	for (i=startIndex;i<endIndex;i++)
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
-					 primitive_boxes[i].m_bound.m_min);
-		btVector3 diff2 = center-means;
+		btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
+											primitive_boxes[i].m_bound.m_min);
+		btVector3 diff2 = center - means;
 		diff2 = diff2 * diff2;
 		variance += diff2;
 	}
-	variance *= (btScalar(1.)/	((btScalar)numIndices-1)	);
+	variance *= (btScalar(1.) / ((btScalar)numIndices - 1));
 
 	return variance.maxAxis();
 }
 
-
 int btQuantizedBvhTree::_sort_and_calc_splitting_index(
-	GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
+	GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex,
 	int endIndex, int splitAxis)
 {
 	int i;
-	int splitIndex =startIndex;
+	int splitIndex = startIndex;
 	int numIndices = endIndex - startIndex;
 
 	// average of centers
 	btScalar splitValue = 0.0f;
 
-	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
-	for (i=startIndex;i<endIndex;i++)
+	btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
-					 primitive_boxes[i].m_bound.m_min);
-		means+=center;
+		btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
+											primitive_boxes[i].m_bound.m_min);
+		means += center;
 	}
-	means *= (btScalar(1.)/(btScalar)numIndices);
+	means *= (btScalar(1.) / (btScalar)numIndices);
 
 	splitValue = means[splitAxis];
 
-
 	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
-	for (i=startIndex;i<endIndex;i++)
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
-					 primitive_boxes[i].m_bound.m_min);
+		btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
+											primitive_boxes[i].m_bound.m_min);
 		if (center[splitAxis] > splitValue)
 		{
 			//swap
-			primitive_boxes.swap(i,splitIndex);
+			primitive_boxes.swap(i, splitIndex);
 			//swapLeafNodes(i,splitIndex);
 			splitIndex++;
 		}
@@ -163,32 +153,30 @@ int btQuantizedBvhTree::_sort_and_calc_splitting_index(
 	//bool unbalanced2 = true;
 
 	//this should be safe too:
-	int rangeBalancedIndices = numIndices/3;
-	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+	int rangeBalancedIndices = numIndices / 3;
+	bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices)));
 
 	if (unbalanced)
 	{
-		splitIndex = startIndex+ (numIndices>>1);
+		splitIndex = startIndex + (numIndices >> 1);
 	}
 
-	btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
+	btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex))));
 
 	return splitIndex;
-
 }
 
-
-void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
+void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex)
 {
 	int curIndex = m_num_nodes;
 	m_num_nodes++;
 
-	btAssert((endIndex-startIndex)>0);
+	btAssert((endIndex - startIndex) > 0);
 
-	if ((endIndex-startIndex)==1)
+	if ((endIndex - startIndex) == 1)
 	{
-	    //We have a leaf node
-	    setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
+		//We have a leaf node
+		setNodeBound(curIndex, primitive_boxes[startIndex].m_bound);
 		m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
 
 		return;
@@ -196,48 +184,43 @@ void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, i
 	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
 
 	//split axis
-	int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
+	int splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex);
 
 	splitIndex = _sort_and_calc_splitting_index(
-			primitive_boxes,startIndex,endIndex,
-			splitIndex//split axis
-			);
-
+		primitive_boxes, startIndex, endIndex,
+		splitIndex  //split axis
+	);
 
 	//calc this node bounding box
 
 	btAABB node_bound;
 	node_bound.invalidate();
 
-	for (int i=startIndex;i<endIndex;i++)
+	for (int i = startIndex; i < endIndex; i++)
 	{
 		node_bound.merge(primitive_boxes[i].m_bound);
 	}
 
-	setNodeBound(curIndex,node_bound);
-
+	setNodeBound(curIndex, node_bound);
 
 	//build left branch
-	_build_sub_tree(primitive_boxes, startIndex, splitIndex );
-
+	_build_sub_tree(primitive_boxes, startIndex, splitIndex);
 
 	//build right branch
-	 _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
+	_build_sub_tree(primitive_boxes, splitIndex, endIndex);
 
 	m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
-
-
 }
 
 //! stackless build tree
 void btQuantizedBvhTree::build_tree(
-	GIM_BVH_DATA_ARRAY & primitive_boxes)
+	GIM_BVH_DATA_ARRAY& primitive_boxes)
 {
 	calc_quantization(primitive_boxes);
 	// initialize node count to 0
 	m_num_nodes = 0;
 	// allocate nodes
-	m_node_array.resize(primitive_boxes.size()*2);
+	m_node_array.resize(primitive_boxes.size() * 2);
 
 	_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
 }
@@ -247,13 +230,13 @@ void btQuantizedBvhTree::build_tree(
 void btGImpactQuantizedBvh::refit()
 {
 	int nodecount = getNodeCount();
-	while(nodecount--)
+	while (nodecount--)
 	{
-		if(isLeafNode(nodecount))
+		if (isLeafNode(nodecount))
 		{
 			btAABB leafbox;
-			m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
-			setNodeBound(nodecount,leafbox);
+			m_primitive_manager->get_primitive_box(getNodeData(nodecount), leafbox);
+			setNodeBound(nodecount, leafbox);
 		}
 		else
 		{
@@ -265,20 +248,20 @@ void btGImpactQuantizedBvh::refit()
 			btAABB temp_box;
 
 			int child_node = getLeftNode(nodecount);
-			if(child_node)
+			if (child_node)
 			{
-				getNodeBound(child_node,temp_box);
+				getNodeBound(child_node, temp_box);
 				bound.merge(temp_box);
 			}
 
 			child_node = getRightNode(nodecount);
-			if(child_node)
+			if (child_node)
 			{
-				getNodeBound(child_node,temp_box);
+				getNodeBound(child_node, temp_box);
 				bound.merge(temp_box);
 			}
 
-			setNodeBound(nodecount,bound);
+			setNodeBound(nodecount, bound);
 		}
 	}
 }
@@ -290,17 +273,17 @@ void btGImpactQuantizedBvh::buildSet()
 	GIM_BVH_DATA_ARRAY primitive_boxes;
 	primitive_boxes.resize(m_primitive_manager->get_primitive_count());
 
-	for (int i = 0;i<primitive_boxes.size() ;i++ )
+	for (int i = 0; i < primitive_boxes.size(); i++)
 	{
-		 m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
-		 primitive_boxes[i].m_data = i;
+		m_primitive_manager->get_primitive_box(i, primitive_boxes[i].m_bound);
+		primitive_boxes[i].m_data = i;
 	}
 
 	m_box_tree.build_tree(primitive_boxes);
 }
 
 //! returns the indices of the primitives in the m_primitive_manager
-bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
+bool btGImpactQuantizedBvh::boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const
 {
 	int curIndex = 0;
 	int numNodes = getNodeCount();
@@ -310,16 +293,14 @@ bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<in
 	unsigned short quantizedMin[3];
 	unsigned short quantizedMax[3];
 
-	m_box_tree.quantizePoint(quantizedMin,box.m_min);
-	m_box_tree.quantizePoint(quantizedMax,box.m_max);
-
+	m_box_tree.quantizePoint(quantizedMin, box.m_min);
+	m_box_tree.quantizePoint(quantizedMax, box.m_max);
 
 	while (curIndex < numNodes)
 	{
-
 		//catch bugs in tree data
 
-		bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin,quantizedMax);
+		bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin, quantizedMax);
 		bool isleafnode = isLeafNode(curIndex);
 
 		if (isleafnode && aabbOverlap)
@@ -335,19 +316,17 @@ bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<in
 		else
 		{
 			//skip node
-			curIndex+= getEscapeNodeIndex(curIndex);
+			curIndex += getEscapeNodeIndex(curIndex);
 		}
 	}
-	if(collided_results.size()>0) return true;
+	if (collided_results.size() > 0) return true;
 	return false;
 }
 
-
-
 //! returns the indices of the primitives in the m_primitive_manager
 bool btGImpactQuantizedBvh::rayQuery(
-	const btVector3 & ray_dir,const btVector3 & ray_origin ,
-	btAlignedObjectArray<int> & collided_results) const
+	const btVector3& ray_dir, const btVector3& ray_origin,
+	btAlignedObjectArray<int>& collided_results) const
 {
 	int curIndex = 0;
 	int numNodes = getNodeCount();
@@ -355,16 +334,16 @@ bool btGImpactQuantizedBvh::rayQuery(
 	while (curIndex < numNodes)
 	{
 		btAABB bound;
-		getNodeBound(curIndex,bound);
+		getNodeBound(curIndex, bound);
 
 		//catch bugs in tree data
 
-		bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
+		bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir);
 		bool isleafnode = isLeafNode(curIndex);
 
 		if (isleafnode && aabbOverlap)
 		{
-			collided_results.push_back(getNodeData( curIndex));
+			collided_results.push_back(getNodeData(curIndex));
 		}
 
 		if (aabbOverlap || isleafnode)
@@ -375,154 +354,133 @@ bool btGImpactQuantizedBvh::rayQuery(
 		else
 		{
 			//skip node
-			curIndex+= getEscapeNodeIndex(curIndex);
+			curIndex += getEscapeNodeIndex(curIndex);
 		}
 	}
-	if(collided_results.size()>0) return true;
+	if (collided_results.size() > 0) return true;
 	return false;
 }
 
-
 SIMD_FORCE_INLINE bool _quantized_node_collision(
-	const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1,
-	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
-	int node0 ,int node1, bool complete_primitive_tests)
+	const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1,
+	const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0,
+	int node0, int node1, bool complete_primitive_tests)
 {
 	btAABB box0;
-	boxset0->getNodeBound(node0,box0);
+	boxset0->getNodeBound(node0, box0);
 	btAABB box1;
-	boxset1->getNodeBound(node1,box1);
-
-	return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
-//	box1.appy_transform_trans_cache(trans_cache_1to0);
-//	return box0.has_collision(box1);
+	boxset1->getNodeBound(node1, box1);
 
+	return box0.overlapping_trans_cache(box1, trans_cache_1to0, complete_primitive_tests);
+	//	box1.appy_transform_trans_cache(trans_cache_1to0);
+	//	return box0.has_collision(box1);
 }
 
-
 //stackless recursive collision routine
 static void _find_quantized_collision_pairs_recursive(
-	const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1,
-	btPairSet * collision_pairs,
-	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+	const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1,
+	btPairSet* collision_pairs,
+	const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0,
 	int node0, int node1, bool complete_primitive_tests)
 {
+	if (_quantized_node_collision(
+			boxset0, boxset1, trans_cache_1to0,
+			node0, node1, complete_primitive_tests) == false) return;  //avoid colliding internal nodes
 
-
-
-	if( _quantized_node_collision(
-		boxset0,boxset1,trans_cache_1to0,
-		node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
-
-	if(boxset0->isLeafNode(node0))
+	if (boxset0->isLeafNode(node0))
 	{
-		if(boxset1->isLeafNode(node1))
+		if (boxset1->isLeafNode(node1))
 		{
 			// collision result
 			collision_pairs->push_pair(
-				boxset0->getNodeData(node0),boxset1->getNodeData(node1));
+				boxset0->getNodeData(node0), boxset1->getNodeData(node1));
 			return;
 		}
 		else
 		{
-
 			//collide left recursive
 
 			_find_quantized_collision_pairs_recursive(
-								boxset0,boxset1,
-								collision_pairs,trans_cache_1to0,
-								node0,boxset1->getLeftNode(node1),false);
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				node0, boxset1->getLeftNode(node1), false);
 
 			//collide right recursive
 			_find_quantized_collision_pairs_recursive(
-								boxset0,boxset1,
-								collision_pairs,trans_cache_1to0,
-								node0,boxset1->getRightNode(node1),false);
-
-
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				node0, boxset1->getRightNode(node1), false);
 		}
 	}
 	else
 	{
-		if(boxset1->isLeafNode(node1))
+		if (boxset1->isLeafNode(node1))
 		{
-
 			//collide left recursive
 			_find_quantized_collision_pairs_recursive(
-								boxset0,boxset1,
-								collision_pairs,trans_cache_1to0,
-								boxset0->getLeftNode(node0),node1,false);
-
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getLeftNode(node0), node1, false);
 
 			//collide right recursive
 
 			_find_quantized_collision_pairs_recursive(
-								boxset0,boxset1,
-								collision_pairs,trans_cache_1to0,
-								boxset0->getRightNode(node0),node1,false);
-
-
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getRightNode(node0), node1, false);
 		}
 		else
 		{
 			//collide left0 left1
 
-
-
 			_find_quantized_collision_pairs_recursive(
-				boxset0,boxset1,
-				collision_pairs,trans_cache_1to0,
-				boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getLeftNode(node0), boxset1->getLeftNode(node1), false);
 
 			//collide left0 right1
 
 			_find_quantized_collision_pairs_recursive(
-				boxset0,boxset1,
-				collision_pairs,trans_cache_1to0,
-				boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
-
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getLeftNode(node0), boxset1->getRightNode(node1), false);
 
 			//collide right0 left1
 
 			_find_quantized_collision_pairs_recursive(
-				boxset0,boxset1,
-				collision_pairs,trans_cache_1to0,
-				boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getRightNode(node0), boxset1->getLeftNode(node1), false);
 
 			//collide right0 right1
 
 			_find_quantized_collision_pairs_recursive(
-				boxset0,boxset1,
-				collision_pairs,trans_cache_1to0,
-				boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getRightNode(node0), boxset1->getRightNode(node1), false);
 
-		}// else if node1 is not a leaf
-	}// else if node0 is not a leaf
+		}  // else if node1 is not a leaf
+	}      // else if node0 is not a leaf
 }
 
-
-void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh * boxset0, const btTransform & trans0,
-		const btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
-		btPairSet & collision_pairs)
+void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh* boxset0, const btTransform& trans0,
+										   const btGImpactQuantizedBvh* boxset1, const btTransform& trans1,
+										   btPairSet& collision_pairs)
 {
-
-	if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
+	if (boxset0->getNodeCount() == 0 || boxset1->getNodeCount() == 0) return;
 
 	BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
 
-	trans_cache_1to0.calc_from_homogenic(trans0,trans1);
+	trans_cache_1to0.calc_from_homogenic(trans0, trans1);
 
 #ifdef TRI_COLLISION_PROFILING
 	bt_begin_gim02_q_tree_time();
-#endif //TRI_COLLISION_PROFILING
+#endif  //TRI_COLLISION_PROFILING
 
 	_find_quantized_collision_pairs_recursive(
-		boxset0,boxset1,
-		&collision_pairs,trans_cache_1to0,0,0,true);
+		boxset0, boxset1,
+		&collision_pairs, trans_cache_1to0, 0, 0, true);
 #ifdef TRI_COLLISION_PROFILING
 	bt_end_gim02_q_tree_time();
-#endif //TRI_COLLISION_PROFILING
-
+#endif  //TRI_COLLISION_PROFILING
 }
-
-
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h
index e6e52fff4c0..b231c1e8327 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h
@@ -26,81 +26,12 @@ subject to the following restrictions:
 
 #include "btGImpactBvh.h"
 #include "btQuantization.h"
+#include "btGImpactQuantizedBvhStructs.h"
 
-
-
-
-
-///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
-///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
-ATTRIBUTE_ALIGNED16	(struct) BT_QUANTIZED_BVH_NODE
-{
-	//12 bytes
-	unsigned short int	m_quantizedAabbMin[3];
-	unsigned short int	m_quantizedAabbMax[3];
-	//4 bytes
-	int	m_escapeIndexOrDataIndex;
-
-	BT_QUANTIZED_BVH_NODE()
-	{
-		m_escapeIndexOrDataIndex = 0;
-	}
-
-	SIMD_FORCE_INLINE bool isLeafNode() const
-	{
-		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
-		return (m_escapeIndexOrDataIndex>=0);
-	}
-
-	SIMD_FORCE_INLINE int getEscapeIndex() const
-	{
-		//btAssert(m_escapeIndexOrDataIndex < 0);
-		return -m_escapeIndexOrDataIndex;
-	}
-
-	SIMD_FORCE_INLINE void setEscapeIndex(int index)
-	{
-		m_escapeIndexOrDataIndex = -index;
-	}
-
-	SIMD_FORCE_INLINE int getDataIndex() const
-	{
-		//btAssert(m_escapeIndexOrDataIndex >= 0);
-
-		return m_escapeIndexOrDataIndex;
-	}
-
-	SIMD_FORCE_INLINE void setDataIndex(int index)
-	{
-		m_escapeIndexOrDataIndex = index;
-	}
-
-	SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(
-		unsigned short * quantizedMin,unsigned short * quantizedMax) const
-	{
-		if(m_quantizedAabbMin[0] > quantizedMax[0] ||
-		   m_quantizedAabbMax[0] < quantizedMin[0] ||
-		   m_quantizedAabbMin[1] > quantizedMax[1] ||
-		   m_quantizedAabbMax[1] < quantizedMin[1] ||
-		   m_quantizedAabbMin[2] > quantizedMax[2] ||
-		   m_quantizedAabbMax[2] < quantizedMin[2])
-		{
-			return false;
-		}
-		return true;
-	}
-
-};
-
-
-
-class GIM_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE>
+class GIM_QUANTIZED_BVH_NODE_ARRAY : public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE>
 {
 };
 
-
-
-
 //! Basic Box tree structure
 class btQuantizedBvhTree
 {
@@ -109,16 +40,18 @@ protected:
 	GIM_QUANTIZED_BVH_NODE_ARRAY m_node_array;
 	btAABB m_global_bound;
 	btVector3 m_bvhQuantization;
+
 protected:
-	void calc_quantization(GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin = btScalar(1.0) );
+	void calc_quantization(GIM_BVH_DATA_ARRAY& primitive_boxes, btScalar boundMargin = btScalar(1.0));
 
 	int _sort_and_calc_splitting_index(
-		GIM_BVH_DATA_ARRAY & primitive_boxes,
-		 int startIndex,  int endIndex, int splitAxis);
+		GIM_BVH_DATA_ARRAY& primitive_boxes,
+		int startIndex, int endIndex, int splitAxis);
+
+	int _calc_splitting_axis(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex);
 
-	int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);
+	void _build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex);
 
-	void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);
 public:
 	btQuantizedBvhTree()
 	{
@@ -127,20 +60,19 @@ public:
 
 	//! prototype functions for box tree management
 	//!@{
-	void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes);
+	void build_tree(GIM_BVH_DATA_ARRAY& primitive_boxes);
 
 	SIMD_FORCE_INLINE void quantizePoint(
-		unsigned short * quantizedpoint, const btVector3 & point) const
+		unsigned short* quantizedpoint, const btVector3& point) const
 	{
-		bt_quantize_clamp(quantizedpoint,point,m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization);
+		bt_quantize_clamp(quantizedpoint, point, m_global_bound.m_min, m_global_bound.m_max, m_bvhQuantization);
 	}
 
-
 	SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(
 		int node_index,
-		unsigned short * quantizedMin,unsigned short * quantizedMax) const
+		unsigned short* quantizedMin, unsigned short* quantizedMax) const
 	{
-		return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin,quantizedMax);
+		return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin, quantizedMax);
 	}
 
 	SIMD_FORCE_INLINE void clearNodes()
@@ -166,41 +98,41 @@ public:
 		return m_node_array[nodeindex].getDataIndex();
 	}
 
-	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const
+	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const
 	{
 		bound.m_min = bt_unquantize(
 			m_node_array[nodeindex].m_quantizedAabbMin,
-			m_global_bound.m_min,m_bvhQuantization);
+			m_global_bound.m_min, m_bvhQuantization);
 
 		bound.m_max = bt_unquantize(
 			m_node_array[nodeindex].m_quantizedAabbMax,
-			m_global_bound.m_min,m_bvhQuantization);
+			m_global_bound.m_min, m_bvhQuantization);
 	}
 
-	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound)
 	{
-		bt_quantize_clamp(	m_node_array[nodeindex].m_quantizedAabbMin,
-							bound.m_min,
-							m_global_bound.m_min,
-							m_global_bound.m_max,
-							m_bvhQuantization);
+		bt_quantize_clamp(m_node_array[nodeindex].m_quantizedAabbMin,
+						  bound.m_min,
+						  m_global_bound.m_min,
+						  m_global_bound.m_max,
+						  m_bvhQuantization);
 
-		bt_quantize_clamp(	m_node_array[nodeindex].m_quantizedAabbMax,
-							bound.m_max,
-							m_global_bound.m_min,
-							m_global_bound.m_max,
-							m_bvhQuantization);
+		bt_quantize_clamp(m_node_array[nodeindex].m_quantizedAabbMax,
+						  bound.m_max,
+						  m_global_bound.m_min,
+						  m_global_bound.m_max,
+						  m_bvhQuantization);
 	}
 
 	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
 	{
-		return nodeindex+1;
+		return nodeindex + 1;
 	}
 
 	SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
 	{
-		if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2;
-		return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex();
+		if (m_node_array[nodeindex + 1].isLeafNode()) return nodeindex + 2;
+		return nodeindex + 1 + m_node_array[nodeindex + 1].getEscapeIndex();
 	}
 
 	SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
@@ -208,7 +140,7 @@ public:
 		return m_node_array[nodeindex].getEscapeIndex();
 	}
 
-	SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const
+	SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE* get_node_pointer(int index = 0) const
 	{
 		return &m_node_array[index];
 	}
@@ -216,8 +148,6 @@ public:
 	//!@}
 };
 
-
-
 //! Structure for containing Boxes
 /*!
 This class offers an structure for managing a box tree of primitives.
@@ -227,13 +157,13 @@ class btGImpactQuantizedBvh
 {
 protected:
 	btQuantizedBvhTree m_box_tree;
-	btPrimitiveManagerBase * m_primitive_manager;
+	btPrimitiveManagerBase* m_primitive_manager;
 
 protected:
 	//stackless refit
 	void refit();
-public:
 
+public:
 	//! this constructor doesn't build the tree. you must call	buildSet
 	btGImpactQuantizedBvh()
 	{
@@ -241,31 +171,30 @@ public:
 	}
 
 	//! this constructor doesn't build the tree. you must call	buildSet
-	btGImpactQuantizedBvh(btPrimitiveManagerBase * primitive_manager)
+	btGImpactQuantizedBvh(btPrimitiveManagerBase* primitive_manager)
 	{
 		m_primitive_manager = primitive_manager;
 	}
 
-	SIMD_FORCE_INLINE btAABB getGlobalBox()  const
+	SIMD_FORCE_INLINE btAABB getGlobalBox() const
 	{
 		btAABB totalbox;
 		getNodeBound(0, totalbox);
 		return totalbox;
 	}
 
-	SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager)
+	SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase* primitive_manager)
 	{
 		m_primitive_manager = primitive_manager;
 	}
 
-	SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const
+	SIMD_FORCE_INLINE btPrimitiveManagerBase* getPrimitiveManager() const
 	{
 		return m_primitive_manager;
 	}
 
-
-//! node manager prototype functions
-///@{
+	//! node manager prototype functions
+	///@{
 
 	//! this attemps to refit the box set.
 	SIMD_FORCE_INLINE void update()
@@ -277,21 +206,21 @@ public:
 	void buildSet();
 
 	//! returns the indices of the primitives in the m_primitive_manager
-	bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const;
+	bool boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const;
 
 	//! returns the indices of the primitives in the m_primitive_manager
-	SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box,
-		 const btTransform & transform, btAlignedObjectArray<int> & collided_results) const
+	SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB& box,
+										 const btTransform& transform, btAlignedObjectArray<int>& collided_results) const
 	{
-		btAABB transbox=box;
+		btAABB transbox = box;
 		transbox.appy_transform(transform);
-		return boxQuery(transbox,collided_results);
+		return boxQuery(transbox, collided_results);
 	}
 
 	//! returns the indices of the primitives in the m_primitive_manager
 	bool rayQuery(
-		const btVector3 & ray_dir,const btVector3 & ray_origin ,
-		btAlignedObjectArray<int> & collided_results) const;
+		const btVector3& ray_dir, const btVector3& ray_origin,
+		btAlignedObjectArray<int>& collided_results) const;
 
 	//! tells if this set has hierarcht
 	SIMD_FORCE_INLINE bool hasHierarchy() const
@@ -300,7 +229,7 @@ public:
 	}
 
 	//! tells if this set is a trimesh
-	SIMD_FORCE_INLINE bool isTrimesh()  const
+	SIMD_FORCE_INLINE bool isTrimesh() const
 	{
 		return m_primitive_manager->is_trimesh();
 	}
@@ -322,17 +251,16 @@ public:
 		return m_box_tree.getNodeData(nodeindex);
 	}
 
-	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound)  const
+	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const
 	{
 		m_box_tree.getNodeBound(nodeindex, bound);
 	}
 
-	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound)
 	{
 		m_box_tree.setNodeBound(nodeindex, bound);
 	}
 
-
 	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
 	{
 		return m_box_tree.getLeftNode(nodeindex);
@@ -348,25 +276,23 @@ public:
 		return m_box_tree.getEscapeNodeIndex(nodeindex);
 	}
 
-	SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const
+	SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex, btPrimitiveTriangle& triangle) const
 	{
-		m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle);
+		m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex), triangle);
 	}
 
-
-	SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const
+	SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE* get_node_pointer(int index = 0) const
 	{
 		return m_box_tree.get_node_pointer(index);
 	}
 
 #ifdef TRI_COLLISION_PROFILING
 	static float getAverageTreeCollisionTime();
-#endif //TRI_COLLISION_PROFILING
+#endif  //TRI_COLLISION_PROFILING
 
-	static void find_collision(const btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
-		const btGImpactQuantizedBvh * boxset2, const btTransform & trans2,
-		btPairSet & collision_pairs);
+	static void find_collision(const btGImpactQuantizedBvh* boxset1, const btTransform& trans1,
+							   const btGImpactQuantizedBvh* boxset2, const btTransform& trans2,
+							   btPairSet& collision_pairs);
 };
 
-
-#endif // GIM_BOXPRUNING_H_INCLUDED
+#endif  // GIM_BOXPRUNING_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h
new file mode 100644
index 00000000000..bd50cb5b87e
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h
@@ -0,0 +1,91 @@
+#ifndef GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED
+#define GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED
+
+/*! \file btGImpactQuantizedBvh.h
+\author Francisco Leon Najera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+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 "btGImpactBvh.h"
+#include "btQuantization.h"
+
+///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
+///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
+ATTRIBUTE_ALIGNED16(struct)
+BT_QUANTIZED_BVH_NODE
+{
+	//12 bytes
+	unsigned short int m_quantizedAabbMin[3];
+	unsigned short int m_quantizedAabbMax[3];
+	//4 bytes
+	int m_escapeIndexOrDataIndex;
+
+	BT_QUANTIZED_BVH_NODE()
+	{
+		m_escapeIndexOrDataIndex = 0;
+	}
+
+	SIMD_FORCE_INLINE bool isLeafNode() const
+	{
+		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+		return (m_escapeIndexOrDataIndex >= 0);
+	}
+
+	SIMD_FORCE_INLINE int getEscapeIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex < 0);
+		return -m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setEscapeIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = -index;
+	}
+
+	SIMD_FORCE_INLINE int getDataIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex >= 0);
+
+		return m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setDataIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = index;
+	}
+
+	SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(
+		unsigned short* quantizedMin, unsigned short* quantizedMax) const
+	{
+		if (m_quantizedAabbMin[0] > quantizedMax[0] ||
+			m_quantizedAabbMax[0] < quantizedMin[0] ||
+			m_quantizedAabbMin[1] > quantizedMax[1] ||
+			m_quantizedAabbMax[1] < quantizedMin[1] ||
+			m_quantizedAabbMin[2] > quantizedMax[2] ||
+			m_quantizedAabbMax[2] < quantizedMin[2])
+		{
+			return false;
+		}
+		return true;
+	}
+};
+
+#endif  // GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp
index ac8efdf3833..34c229a3aba 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp
@@ -18,125 +18,169 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btGImpactShape.h"
 #include "btGImpactMassUtil.h"
 
+btGImpactMeshShapePart::btGImpactMeshShapePart(btStridingMeshInterface* meshInterface, int part)
+{
+	// moved from .h to .cpp because of conditional compilation
+	// (The setting of BT_THREADSAFE may differ between various cpp files, so it is best to
+	// avoid using it in h files)
+	m_primitive_manager.m_meshInterface = meshInterface;
+	m_primitive_manager.m_part = part;
+	m_box_set.setPrimitiveManager(&m_primitive_manager);
+#if BT_THREADSAFE
+	// If threadsafe is requested, this object uses a different lock/unlock
+	//  model with the btStridingMeshInterface -- lock once when the object is constructed
+	//  and unlock once in the destructor.
+	// The other way of locking and unlocking for each collision check in the narrowphase
+	// is not threadsafe.  Note these are not thread-locks, they are calls to the meshInterface's
+	// getLockedReadOnlyVertexIndexBase virtual function, which by default just returns a couple of
+	// pointers.  In theory a client could override the lock function to do all sorts of
+	// things like reading data from GPU memory, or decompressing data on the fly, but such things
+	// do not seem all that likely or useful, given the performance cost.
+	m_primitive_manager.lock();
+#endif
+}
 
-#define CALC_EXACT_INERTIA 1
+btGImpactMeshShapePart::~btGImpactMeshShapePart()
+{
+	// moved from .h to .cpp because of conditional compilation
+#if BT_THREADSAFE
+	m_primitive_manager.unlock();
+#endif
+}
 
+void btGImpactMeshShapePart::lockChildShapes() const
+{
+	// moved from .h to .cpp because of conditional compilation
+#if !BT_THREADSAFE
+	// called in the narrowphase -- not threadsafe!
+	void* dummy = (void*)(m_box_set.getPrimitiveManager());
+	TrimeshPrimitiveManager* dummymanager = static_cast<TrimeshPrimitiveManager*>(dummy);
+	dummymanager->lock();
+#endif
+}
 
-void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btGImpactMeshShapePart::unlockChildShapes() const
+{
+	// moved from .h to .cpp because of conditional compilation
+#if !BT_THREADSAFE
+	// called in the narrowphase -- not threadsafe!
+	void* dummy = (void*)(m_box_set.getPrimitiveManager());
+	TrimeshPrimitiveManager* dummymanager = static_cast<TrimeshPrimitiveManager*>(dummy);
+	dummymanager->unlock();
+#endif
+}
+
+#define CALC_EXACT_INERTIA 1
+
+void btGImpactCompoundShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
 	lockChildShapes();
 #ifdef CALC_EXACT_INERTIA
-	inertia.setValue(0.f,0.f,0.f);
+	inertia.setValue(0.f, 0.f, 0.f);
 
 	int i = this->getNumChildShapes();
-	btScalar shapemass = mass/btScalar(i);
+	btScalar shapemass = mass / btScalar(i);
 
-	while(i--)
+	while (i--)
 	{
 		btVector3 temp_inertia;
-		m_childShapes[i]->calculateLocalInertia(shapemass,temp_inertia);
-		if(childrenHasTransform())
+		m_childShapes[i]->calculateLocalInertia(shapemass, temp_inertia);
+		if (childrenHasTransform())
 		{
-			inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]);
+			inertia = gim_inertia_add_transformed(inertia, temp_inertia, m_childTransforms[i]);
 		}
 		else
 		{
-			inertia = gim_inertia_add_transformed( inertia,temp_inertia,btTransform::getIdentity());
+			inertia = gim_inertia_add_transformed(inertia, temp_inertia, btTransform::getIdentity());
 		}
-
 	}
 
 #else
 
 	// Calc box inertia
 
-	btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
-	btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
-	btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
-	const btScalar x2 = lx*lx;
-	const btScalar y2 = ly*ly;
-	const btScalar z2 = lz*lz;
+	btScalar lx = m_localAABB.m_max[0] - m_localAABB.m_min[0];
+	btScalar ly = m_localAABB.m_max[1] - m_localAABB.m_min[1];
+	btScalar lz = m_localAABB.m_max[2] - m_localAABB.m_min[2];
+	const btScalar x2 = lx * lx;
+	const btScalar y2 = ly * ly;
+	const btScalar z2 = lz * lz;
 	const btScalar scaledmass = mass * btScalar(0.08333333);
 
-	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+	inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2));
 
 #endif
 	unlockChildShapes();
 }
 
-
-
-void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
 	lockChildShapes();
 
-
 #ifdef CALC_EXACT_INERTIA
-	inertia.setValue(0.f,0.f,0.f);
+	inertia.setValue(0.f, 0.f, 0.f);
 
 	int i = this->getVertexCount();
-	btScalar pointmass = mass/btScalar(i);
+	btScalar pointmass = mass / btScalar(i);
 
-	while(i--)
+	while (i--)
 	{
 		btVector3 pointintertia;
-		this->getVertex(i,pointintertia);
-		pointintertia = gim_get_point_inertia(pointintertia,pointmass);
-		inertia+=pointintertia;
+		this->getVertex(i, pointintertia);
+		pointintertia = gim_get_point_inertia(pointintertia, pointmass);
+		inertia += pointintertia;
 	}
 
 #else
 
 	// Calc box inertia
 
-	btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
-	btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
-	btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
-	const btScalar x2 = lx*lx;
-	const btScalar y2 = ly*ly;
-	const btScalar z2 = lz*lz;
+	btScalar lx = m_localAABB.m_max[0] - m_localAABB.m_min[0];
+	btScalar ly = m_localAABB.m_max[1] - m_localAABB.m_min[1];
+	btScalar lz = m_localAABB.m_max[2] - m_localAABB.m_min[2];
+	const btScalar x2 = lx * lx;
+	const btScalar y2 = ly * ly;
+	const btScalar z2 = lz * lz;
 	const btScalar scaledmass = mass * btScalar(0.08333333);
 
-	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+	inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2));
 
 #endif
 
 	unlockChildShapes();
 }
 
-void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+void btGImpactMeshShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
 {
-
 #ifdef CALC_EXACT_INERTIA
-	inertia.setValue(0.f,0.f,0.f);
+	inertia.setValue(0.f, 0.f, 0.f);
 
 	int i = this->getMeshPartCount();
-	btScalar partmass = mass/btScalar(i);
+	btScalar partmass = mass / btScalar(i);
 
-	while(i--)
+	while (i--)
 	{
 		btVector3 partinertia;
-		getMeshPart(i)->calculateLocalInertia(partmass,partinertia);
-		inertia+=partinertia;
+		getMeshPart(i)->calculateLocalInertia(partmass, partinertia);
+		inertia += partinertia;
 	}
 
 #else
 
 	// Calc box inertia
 
-	btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
-	btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
-	btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
-	const btScalar x2 = lx*lx;
-	const btScalar y2 = ly*ly;
-	const btScalar z2 = lz*lz;
+	btScalar lx = m_localAABB.m_max[0] - m_localAABB.m_min[0];
+	btScalar ly = m_localAABB.m_max[1] - m_localAABB.m_min[1];
+	btScalar lz = m_localAABB.m_max[2] - m_localAABB.m_min[2];
+	const btScalar x2 = lx * lx;
+	const btScalar y2 = ly * ly;
+	const btScalar z2 = lz * lz;
 	const btScalar scaledmass = mass * btScalar(0.08333333);
 
-	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+	inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2));
 
 #endif
 }
@@ -145,7 +189,7 @@ void btGImpactMeshShape::rayTest(const btVector3& rayFrom, const btVector3& rayT
 {
 }
 
-void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom, const btVector3& rayTo) const
+void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const
 {
 	lockChildShapes();
 
@@ -154,7 +198,7 @@ void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback
 	rayDir.normalize();
 	m_box_set.rayQuery(rayDir, rayFrom, collided);
 
-	if(collided.size()==0)
+	if (collided.size() == 0)
 	{
 		unlockChildShapes();
 		return;
@@ -163,15 +207,15 @@ void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback
 	int part = (int)getPart();
 	btPrimitiveTriangle triangle;
 	int i = collided.size();
-	while(i--)
+	while (i--)
 	{
-		getPrimitiveTriangle(collided[i],triangle);
-		callback->processTriangle(triangle.m_vertices,part,collided[i]);
+		getPrimitiveTriangle(collided[i], triangle);
+		callback->processTriangle(triangle.m_vertices, part, collided[i]);
 	}
 	unlockChildShapes();
 }
 
-void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
 {
 	lockChildShapes();
 	btAABB box;
@@ -179,9 +223,9 @@ void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,co
 	box.m_max = aabbMax;
 
 	btAlignedObjectArray<int> collided;
-	m_box_set.boxQuery(box,collided);
+	m_box_set.boxQuery(box, collided);
 
-	if(collided.size()==0)
+	if (collided.size() == 0)
 	{
 		unlockChildShapes();
 		return;
@@ -190,40 +234,38 @@ void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,co
 	int part = (int)getPart();
 	btPrimitiveTriangle triangle;
 	int i = collided.size();
-	while(i--)
+	while (i--)
 	{
-		this->getPrimitiveTriangle(collided[i],triangle);
-		callback->processTriangle(triangle.m_vertices,part,collided[i]);
+		this->getPrimitiveTriangle(collided[i], triangle);
+		callback->processTriangle(triangle.m_vertices, part, collided[i]);
 	}
 	unlockChildShapes();
-
 }
 
-void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
 {
 	int i = m_mesh_parts.size();
-	while(i--)
+	while (i--)
 	{
-		m_mesh_parts[i]->processAllTriangles(callback,aabbMin,aabbMax);
+		m_mesh_parts[i]->processAllTriangles(callback, aabbMin, aabbMax);
 	}
 }
 
-void btGImpactMeshShape::processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom, const btVector3& rayTo) const
+void btGImpactMeshShape::processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const
 {
 	int i = m_mesh_parts.size();
-	while(i--)
+	while (i--)
 	{
 		m_mesh_parts[i]->processAllTrianglesRay(callback, rayFrom, rayTo);
 	}
 }
 
-
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const
+const char* btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btGImpactMeshShapeData* trimeshData = (btGImpactMeshShapeData*) dataBuffer;
+	btGImpactMeshShapeData* trimeshData = (btGImpactMeshShapeData*)dataBuffer;
 
-	btCollisionShape::serialize(&trimeshData->m_collisionShapeData,serializer);
+	btCollisionShape::serialize(&trimeshData->m_collisionShapeData, serializer);
 
 	m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer);
 
@@ -235,4 +277,3 @@ const char*	btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serial
 
 	return "btGImpactMeshShapeData";
 }
-
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h
index 3d1f48d4776..cc91079579d 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h
@@ -21,7 +21,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef GIMPACT_SHAPE_H
 #define GIMPACT_SHAPE_H
 
@@ -37,8 +36,7 @@ subject to the following restrictions:
 #include "LinearMath/btMatrix3x3.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
-#include "btGImpactQuantizedBvh.h" // box tree class
-
+#include "btGImpactQuantizedBvh.h"  // box tree class
 
 //! declare Quantized trees, (you can change to float based trees)
 typedef btGImpactQuantizedBvh btGImpactBoxSet;
@@ -50,10 +48,8 @@ enum eGIMPACT_SHAPE_TYPE
 	CONST_GIMPACT_TRIMESH_SHAPE
 };
 
-
-
 //! Helper class for tetrahedrons
-class btTetrahedronShapeEx:public btBU_Simplex1to4
+class btTetrahedronShapeEx : public btBU_Simplex1to4
 {
 public:
 	btTetrahedronShapeEx()
@@ -61,10 +57,9 @@ public:
 		m_numVertices = 4;
 	}
 
-
 	SIMD_FORCE_INLINE void setVertices(
-		const btVector3 & v0,const btVector3 & v1,
-		const btVector3 & v2,const btVector3 & v3)
+		const btVector3& v0, const btVector3& v1,
+		const btVector3& v2, const btVector3& v3)
 	{
 		m_vertices[0] = v0;
 		m_vertices[1] = v1;
@@ -74,45 +69,42 @@ public:
 	}
 };
 
-
 //! Base class for gimpact shapes
 class btGImpactShapeInterface : public btConcaveShape
 {
 protected:
-    btAABB m_localAABB;
-    bool m_needs_update;
-    btVector3  localScaling;
-    btGImpactBoxSet m_box_set;// optionally boxset
+	btAABB m_localAABB;
+	bool m_needs_update;
+	btVector3 localScaling;
+	btGImpactBoxSet m_box_set;  // optionally boxset
 
 	//! use this function for perfofm refit in bounding boxes
-    //! use this function for perfofm refit in bounding boxes
-    virtual void calcLocalAABB()
-    {
+	//! use this function for perfofm refit in bounding boxes
+	virtual void calcLocalAABB()
+	{
 		lockChildShapes();
-    	if(m_box_set.getNodeCount() == 0)
-    	{
-    		m_box_set.buildSet();
-    	}
-    	else
-    	{
-    		m_box_set.update();
-    	}
-    	unlockChildShapes();
-
-    	m_localAABB = m_box_set.getGlobalBox();
-    }
+		if (m_box_set.getNodeCount() == 0)
+		{
+			m_box_set.buildSet();
+		}
+		else
+		{
+			m_box_set.update();
+		}
+		unlockChildShapes();
 
+		m_localAABB = m_box_set.getGlobalBox();
+	}
 
 public:
 	btGImpactShapeInterface()
 	{
-		m_shapeType=GIMPACT_SHAPE_PROXYTYPE;
+		m_shapeType = GIMPACT_SHAPE_PROXYTYPE;
 		m_localAABB.invalidate();
 		m_needs_update = true;
-		localScaling.setValue(1.f,1.f,1.f);
+		localScaling.setValue(1.f, 1.f, 1.f);
 	}
 
-
 	//! performs refit operation
 	/*!
 	Updates the entire Box set of this shape.
@@ -120,47 +112,46 @@ public:
 		will does nothing.
 	\post if m_needs_update == true, then it calls calcLocalAABB();
 	*/
-    SIMD_FORCE_INLINE void updateBound()
-    {
-    	if(!m_needs_update) return;
-    	calcLocalAABB();
-    	m_needs_update  = false;
-    }
-
-    //! If the Bounding box is not updated, then this class attemps to calculate it.
-    /*!
+	SIMD_FORCE_INLINE void updateBound()
+	{
+		if (!m_needs_update) return;
+		calcLocalAABB();
+		m_needs_update = false;
+	}
+
+	//! If the Bounding box is not updated, then this class attemps to calculate it.
+	/*!
     \post Calls updateBound() for update the box set.
     */
-    void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
-    {
-        btAABB transformedbox = m_localAABB;
-        transformedbox.appy_transform(t);
-        aabbMin = transformedbox.m_min;
-        aabbMax = transformedbox.m_max;
-    }
-
-    //! Tells to this object that is needed to refit the box set
-    virtual void postUpdate()
-    {
-    	m_needs_update = true;
-    }
+	void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
+	{
+		btAABB transformedbox = m_localAABB;
+		transformedbox.appy_transform(t);
+		aabbMin = transformedbox.m_min;
+		aabbMax = transformedbox.m_max;
+	}
+
+	//! Tells to this object that is needed to refit the box set
+	virtual void postUpdate()
+	{
+		m_needs_update = true;
+	}
 
 	//! Obtains the local box, which is the global calculated box of the total of subshapes
-	SIMD_FORCE_INLINE const btAABB & getLocalBox()
+	SIMD_FORCE_INLINE const btAABB& getLocalBox()
 	{
 		return m_localAABB;
 	}
 
+	virtual int getShapeType() const
+	{
+		return GIMPACT_SHAPE_PROXYTYPE;
+	}
 
-    virtual int	getShapeType() const
-    {
-        return GIMPACT_SHAPE_PROXYTYPE;
-    }
-
-    /*!
+	/*!
 	\post You must call updateBound() for update the box set.
 	*/
-	virtual void	setLocalScaling(const btVector3& scaling)
+	virtual void setLocalScaling(const btVector3& scaling)
 	{
 		localScaling = scaling;
 		postUpdate();
@@ -171,46 +162,43 @@ public:
 		return localScaling;
 	}
 
-
 	virtual void setMargin(btScalar margin)
-    {
-    	m_collisionMargin = margin;
-    	int i = getNumChildShapes();
-    	while(i--)
-    	{
+	{
+		m_collisionMargin = margin;
+		int i = getNumChildShapes();
+		while (i--)
+		{
 			btCollisionShape* child = getChildShape(i);
 			child->setMargin(margin);
-    	}
+		}
 
 		m_needs_update = true;
-    }
-
+	}
 
 	//! Subshape member functions
 	//!@{
 
 	//! Base method for determinig which kind of GIMPACT shape we get
-	virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const = 0 ;
+	virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const = 0;
 
 	//! gets boxset
-	SIMD_FORCE_INLINE const btGImpactBoxSet * getBoxSet() const
+	SIMD_FORCE_INLINE const btGImpactBoxSet* getBoxSet() const
 	{
 		return &m_box_set;
 	}
 
 	//! Determines if this class has a hierarchy structure for sorting its primitives
-	SIMD_FORCE_INLINE bool hasBoxSet()  const
+	SIMD_FORCE_INLINE bool hasBoxSet() const
 	{
-		if(m_box_set.getNodeCount() == 0) return false;
+		if (m_box_set.getNodeCount() == 0) return false;
 		return true;
 	}
 
 	//! Obtains the primitive manager
-	virtual const btPrimitiveManagerBase * getPrimitiveManager()  const = 0;
-
+	virtual const btPrimitiveManagerBase* getPrimitiveManager() const = 0;
 
 	//! Gets the number of children
-	virtual int	getNumChildShapes() const  = 0;
+	virtual int getNumChildShapes() const = 0;
 
 	//! if true, then its children must get transforms.
 	virtual bool childrenHasTransform() const = 0;
@@ -221,11 +209,9 @@ public:
 	//! Determines if this shape has tetrahedrons
 	virtual bool needsRetrieveTetrahedrons() const = 0;
 
-	virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const = 0;
-
-	virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const = 0;
-
+	virtual void getBulletTriangle(int prim_index, btTriangleShapeEx& triangle) const = 0;
 
+	virtual void getBulletTetrahedron(int prim_index, btTetrahedronShapeEx& tetrahedron) const = 0;
 
 	//! call when reading child shapes
 	virtual void lockChildShapes() const
@@ -237,94 +223,91 @@ public:
 	}
 
 	//! if this trimesh
-	SIMD_FORCE_INLINE void getPrimitiveTriangle(int index,btPrimitiveTriangle & triangle) const
+	SIMD_FORCE_INLINE void getPrimitiveTriangle(int index, btPrimitiveTriangle& triangle) const
 	{
-		getPrimitiveManager()->get_primitive_triangle(index,triangle);
+		getPrimitiveManager()->get_primitive_triangle(index, triangle);
 	}
 
-
 	//! Retrieves the bound from a child
-    /*!
+	/*!
     */
-    virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
-    {
-        btAABB child_aabb;
-        getPrimitiveManager()->get_primitive_box(child_index,child_aabb);
-        child_aabb.appy_transform(t);
-        aabbMin = child_aabb.m_min;
-        aabbMax = child_aabb.m_max;
-    }
+	virtual void getChildAabb(int child_index, const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
+	{
+		btAABB child_aabb;
+		getPrimitiveManager()->get_primitive_box(child_index, child_aabb);
+		child_aabb.appy_transform(t);
+		aabbMin = child_aabb.m_min;
+		aabbMax = child_aabb.m_max;
+	}
 
 	//! Gets the children
 	virtual btCollisionShape* getChildShape(int index) = 0;
 
-
 	//! Gets the child
 	virtual const btCollisionShape* getChildShape(int index) const = 0;
 
 	//! Gets the children transform
-	virtual btTransform	getChildTransform(int index) const = 0;
+	virtual btTransform getChildTransform(int index) const = 0;
 
 	//! Sets the children transform
 	/*!
 	\post You must call updateBound() for update the box set.
 	*/
-	virtual void setChildTransform(int index, const btTransform & transform) = 0;
+	virtual void setChildTransform(int index, const btTransform& transform) = 0;
 
 	//!@}
 
-
 	//! virtual method for ray collision
-	virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback)  const
+	virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const
 	{
-        (void) rayFrom; (void) rayTo; (void) resultCallback;
+		(void)rayFrom;
+		(void)rayTo;
+		(void)resultCallback;
 	}
 
 	//! Function for retrieve triangles.
 	/*!
 	It gives the triangles in local space
 	*/
-	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+	virtual void processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
 	{
-        (void) callback; (void) aabbMin; (void) aabbMax;
+		(void)callback;
+		(void)aabbMin;
+		(void)aabbMax;
 	}
 
 	//! Function for retrieve triangles.
 	/*!
 	It gives the triangles in local space
 	*/
-	virtual void processAllTrianglesRay(btTriangleCallback* /*callback*/,const btVector3& /*rayFrom*/, const btVector3& /*rayTo*/) const
+	virtual void processAllTrianglesRay(btTriangleCallback* /*callback*/, const btVector3& /*rayFrom*/, const btVector3& /*rayTo*/) const
 	{
-		
 	}
 
 	//!@}
-
 };
 
-
 //! btGImpactCompoundShape allows to handle multiple btCollisionShape objects at once
 /*!
 This class only can manage Convex subshapes
 */
-class btGImpactCompoundShape	: public btGImpactShapeInterface
+class btGImpactCompoundShape : public btGImpactShapeInterface
 {
 public:
 	//! compound primitive manager
-	class CompoundPrimitiveManager:public btPrimitiveManagerBase
+	class CompoundPrimitiveManager : public btPrimitiveManagerBase
 	{
 	public:
 		virtual ~CompoundPrimitiveManager() {}
-		btGImpactCompoundShape * m_compoundShape;
-
+		btGImpactCompoundShape* m_compoundShape;
 
 		CompoundPrimitiveManager(const CompoundPrimitiveManager& compound)
-            : btPrimitiveManagerBase()
+			: btPrimitiveManagerBase()
 		{
 			m_compoundShape = compound.m_compoundShape;
 		}
 
-		CompoundPrimitiveManager(btGImpactCompoundShape * compoundShape)
+		CompoundPrimitiveManager(btGImpactCompoundShape* compoundShape)
 		{
 			m_compoundShape = compoundShape;
 		}
@@ -341,13 +324,13 @@ public:
 
 		virtual int get_primitive_count() const
 		{
-			return (int )m_compoundShape->getNumChildShapes();
+			return (int)m_compoundShape->getNumChildShapes();
 		}
 
-		virtual void get_primitive_box(int prim_index ,btAABB & primbox) const
+		virtual void get_primitive_box(int prim_index, btAABB& primbox) const
 		{
 			btTransform prim_trans;
-			if(m_compoundShape->childrenHasTransform())
+			if (m_compoundShape->childrenHasTransform())
 			{
 				prim_trans = m_compoundShape->getChildTransform(prim_index);
 			}
@@ -356,30 +339,26 @@ public:
 				prim_trans.setIdentity();
 			}
 			const btCollisionShape* shape = m_compoundShape->getChildShape(prim_index);
-			shape->getAabb(prim_trans,primbox.m_min,primbox.m_max);
+			shape->getAabb(prim_trans, primbox.m_min, primbox.m_max);
 		}
 
-		virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const
+		virtual void get_primitive_triangle(int prim_index, btPrimitiveTriangle& triangle) const
 		{
 			btAssert(0);
-            (void) prim_index; (void) triangle;
+			(void)prim_index;
+			(void)triangle;
 		}
-
 	};
 
-
-
 protected:
 	CompoundPrimitiveManager m_primitive_manager;
-	btAlignedObjectArray<btTransform>		m_childTransforms;
-	btAlignedObjectArray<btCollisionShape*>	m_childShapes;
-
+	btAlignedObjectArray<btTransform> m_childTransforms;
+	btAlignedObjectArray<btCollisionShape*> m_childShapes;
 
 public:
-
 	btGImpactCompoundShape(bool children_has_transform = true)
 	{
-        (void) children_has_transform;
+		(void)children_has_transform;
 		m_primitive_manager.m_compoundShape = this;
 		m_box_set.setPrimitiveManager(&m_primitive_manager);
 	}
@@ -388,36 +367,33 @@ public:
 	{
 	}
 
-
 	//! if true, then its children must get transforms.
 	virtual bool childrenHasTransform() const
 	{
-		if(m_childTransforms.size()==0) return false;
+		if (m_childTransforms.size() == 0) return false;
 		return true;
 	}
 
-
 	//! Obtains the primitive manager
-	virtual const btPrimitiveManagerBase * getPrimitiveManager()  const
+	virtual const btPrimitiveManagerBase* getPrimitiveManager() const
 	{
 		return &m_primitive_manager;
 	}
 
 	//! Obtains the compopund primitive manager
-	SIMD_FORCE_INLINE CompoundPrimitiveManager * getCompoundPrimitiveManager()
+	SIMD_FORCE_INLINE CompoundPrimitiveManager* getCompoundPrimitiveManager()
 	{
 		return &m_primitive_manager;
 	}
 
 	//! Gets the number of children
-	virtual int	getNumChildShapes() const
+	virtual int getNumChildShapes() const
 	{
 		return m_childShapes.size();
 	}
 
-
 	//! Use this method for adding children. Only Convex shapes are allowed.
-	void addChildShape(const btTransform& localTransform,btCollisionShape* shape)
+	void addChildShape(const btTransform& localTransform, btCollisionShape* shape)
 	{
 		btAssert(shape->isConvex());
 		m_childTransforms.push_back(localTransform);
@@ -444,24 +420,22 @@ public:
 	}
 
 	//! Retrieves the bound from a child
-    /*!
+	/*!
     */
-    virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
-    {
-
-    	if(childrenHasTransform())
-    	{
-    		m_childShapes[child_index]->getAabb(t*m_childTransforms[child_index],aabbMin,aabbMax);
-    	}
-    	else
-    	{
-    		m_childShapes[child_index]->getAabb(t,aabbMin,aabbMax);
-    	}
-    }
-
+	virtual void getChildAabb(int child_index, const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
+	{
+		if (childrenHasTransform())
+		{
+			m_childShapes[child_index]->getAabb(t * m_childTransforms[child_index], aabbMin, aabbMax);
+		}
+		else
+		{
+			m_childShapes[child_index]->getAabb(t, aabbMin, aabbMax);
+		}
+	}
 
 	//! Gets the children transform
-	virtual btTransform	getChildTransform(int index) const
+	virtual btTransform getChildTransform(int index) const
 	{
 		btAssert(m_childTransforms.size() == m_childShapes.size());
 		return m_childTransforms[index];
@@ -471,7 +445,7 @@ public:
 	/*!
 	\post You must call updateBound() for update the box set.
 	*/
-	virtual void setChildTransform(int index, const btTransform & transform)
+	virtual void setChildTransform(int index, const btTransform& transform)
 	{
 		btAssert(m_childTransforms.size() == m_childShapes.size());
 		m_childTransforms[index] = transform;
@@ -490,24 +464,24 @@ public:
 		return false;
 	}
 
-
-	virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const
+	virtual void getBulletTriangle(int prim_index, btTriangleShapeEx& triangle) const
 	{
-        (void) prim_index; (void) triangle;
+		(void)prim_index;
+		(void)triangle;
 		btAssert(0);
 	}
 
-	virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const
+	virtual void getBulletTetrahedron(int prim_index, btTetrahedronShapeEx& tetrahedron) const
 	{
-        (void) prim_index; (void) tetrahedron;
+		(void)prim_index;
+		(void)tetrahedron;
 		btAssert(0);
 	}
 
-
 	//! Calculates the exact inertia tensor for this shape
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+	virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const;
 
-	virtual const char*	getName()const
+	virtual const char* getName() const
 	{
 		return "GImpactCompound";
 	}
@@ -516,11 +490,8 @@ public:
 	{
 		return CONST_GIMPACT_COMPOUND_SHAPE;
 	}
-
 };
 
-
-
 //! This class manages a sub part of a mesh supplied by the btStridingMeshInterface interface.
 /*!
 - Simply create this shape by passing the btStridingMeshInterface to the constructor btGImpactMeshShapePart, then you must call updateBound() after creating the mesh
@@ -535,21 +506,21 @@ public:
 	/*!
 	Manages the info from btStridingMeshInterface object and controls the Lock/Unlock mechanism
 	*/
-	class TrimeshPrimitiveManager:public btPrimitiveManagerBase
+	class TrimeshPrimitiveManager : public btPrimitiveManagerBase
 	{
 	public:
 		btScalar m_margin;
-		btStridingMeshInterface * m_meshInterface;
+		btStridingMeshInterface* m_meshInterface;
 		btVector3 m_scale;
 		int m_part;
 		int m_lock_count;
-		const unsigned char *vertexbase;
+		const unsigned char* vertexbase;
 		int numverts;
 		PHY_ScalarType type;
 		int stride;
-		const unsigned char *indexbase;
+		const unsigned char* indexbase;
 		int indexstride;
-		int  numfaces;
+		int numfaces;
 		PHY_ScalarType indicestype;
 
 		TrimeshPrimitiveManager()
@@ -557,7 +528,7 @@ public:
 			m_meshInterface = NULL;
 			m_part = 0;
 			m_margin = 0.01f;
-			m_scale = btVector3(1.f,1.f,1.f);
+			m_scale = btVector3(1.f, 1.f, 1.f);
 			m_lock_count = 0;
 			vertexbase = 0;
 			numverts = 0;
@@ -567,8 +538,8 @@ public:
 			numfaces = 0;
 		}
 
- 		TrimeshPrimitiveManager(const TrimeshPrimitiveManager & manager)
-            : btPrimitiveManagerBase()
+		TrimeshPrimitiveManager(const TrimeshPrimitiveManager& manager)
+			: btPrimitiveManagerBase()
 		{
 			m_meshInterface = manager.m_meshInterface;
 			m_part = manager.m_part;
@@ -581,11 +552,10 @@ public:
 			indexbase = 0;
 			indexstride = 0;
 			numfaces = 0;
-
 		}
 
 		TrimeshPrimitiveManager(
-			btStridingMeshInterface * meshInterface,	int part)
+			btStridingMeshInterface* meshInterface, int part)
 		{
 			m_meshInterface = meshInterface;
 			m_part = part;
@@ -598,29 +568,28 @@ public:
 			indexbase = 0;
 			indexstride = 0;
 			numfaces = 0;
-
 		}
 
 		virtual ~TrimeshPrimitiveManager() {}
 
 		void lock()
 		{
-			if(m_lock_count>0)
+			if (m_lock_count > 0)
 			{
 				m_lock_count++;
 				return;
 			}
 			m_meshInterface->getLockedReadOnlyVertexIndexBase(
-				&vertexbase,numverts,
-				type, stride,&indexbase, indexstride, numfaces,indicestype,m_part);
+				&vertexbase, numverts,
+				type, stride, &indexbase, indexstride, numfaces, indicestype, m_part);
 
 			m_lock_count = 1;
 		}
 
 		void unlock()
 		{
-			if(m_lock_count == 0) return;
-			if(m_lock_count>1)
+			if (m_lock_count == 0) return;
+			if (m_lock_count > 1)
 			{
 				--m_lock_count;
 				return;
@@ -637,102 +606,99 @@ public:
 
 		virtual int get_primitive_count() const
 		{
-			return (int )numfaces;
+			return (int)numfaces;
 		}
 
 		SIMD_FORCE_INLINE int get_vertex_count() const
 		{
-			return (int )numverts;
+			return (int)numverts;
 		}
 
-		SIMD_FORCE_INLINE void get_indices(int face_index,unsigned int &i0,unsigned int &i1,unsigned int &i2) const
+		SIMD_FORCE_INLINE void get_indices(int face_index, unsigned int& i0, unsigned int& i1, unsigned int& i2) const
 		{
-			if(indicestype == PHY_SHORT)
+			if (indicestype == PHY_SHORT)
 			{
-				unsigned short* s_indices = (unsigned short *)(indexbase + face_index * indexstride);
+				unsigned short* s_indices = (unsigned short*)(indexbase + face_index * indexstride);
 				i0 = s_indices[0];
 				i1 = s_indices[1];
 				i2 = s_indices[2];
 			}
+			else if (indicestype == PHY_INTEGER)
+			{
+				unsigned int* i_indices = (unsigned int*)(indexbase + face_index * indexstride);
+				i0 = i_indices[0];
+				i1 = i_indices[1];
+				i2 = i_indices[2];
+			}
 			else
 			{
-				unsigned int * i_indices = (unsigned int *)(indexbase + face_index*indexstride);
+				btAssert(indicestype == PHY_UCHAR);
+				unsigned char* i_indices = (unsigned char*)(indexbase + face_index * indexstride);
 				i0 = i_indices[0];
 				i1 = i_indices[1];
 				i2 = i_indices[2];
 			}
 		}
 
-		SIMD_FORCE_INLINE void get_vertex(unsigned int vertex_index, btVector3 & vertex) const
+		SIMD_FORCE_INLINE void get_vertex(unsigned int vertex_index, btVector3& vertex) const
 		{
-			if(type == PHY_DOUBLE)
+			if (type == PHY_DOUBLE)
 			{
-				double * dvertices = (double *)(vertexbase + vertex_index*stride);
-				vertex[0] = btScalar(dvertices[0]*m_scale[0]);
-				vertex[1] = btScalar(dvertices[1]*m_scale[1]);
-				vertex[2] = btScalar(dvertices[2]*m_scale[2]);
+				double* dvertices = (double*)(vertexbase + vertex_index * stride);
+				vertex[0] = btScalar(dvertices[0] * m_scale[0]);
+				vertex[1] = btScalar(dvertices[1] * m_scale[1]);
+				vertex[2] = btScalar(dvertices[2] * m_scale[2]);
 			}
 			else
 			{
-				float * svertices = (float *)(vertexbase + vertex_index*stride);
-				vertex[0] = svertices[0]*m_scale[0];
-				vertex[1] = svertices[1]*m_scale[1];
-				vertex[2] = svertices[2]*m_scale[2];
+				float* svertices = (float*)(vertexbase + vertex_index * stride);
+				vertex[0] = svertices[0] * m_scale[0];
+				vertex[1] = svertices[1] * m_scale[1];
+				vertex[2] = svertices[2] * m_scale[2];
 			}
 		}
 
-		virtual void get_primitive_box(int prim_index ,btAABB & primbox) const
+		virtual void get_primitive_box(int prim_index, btAABB& primbox) const
 		{
-			btPrimitiveTriangle  triangle;
-			get_primitive_triangle(prim_index,triangle);
+			btPrimitiveTriangle triangle;
+			get_primitive_triangle(prim_index, triangle);
 			primbox.calc_from_triangle_margin(
 				triangle.m_vertices[0],
-				triangle.m_vertices[1],triangle.m_vertices[2],triangle.m_margin);
+				triangle.m_vertices[1], triangle.m_vertices[2], triangle.m_margin);
 		}
 
-		virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const
+		virtual void get_primitive_triangle(int prim_index, btPrimitiveTriangle& triangle) const
 		{
 			unsigned int indices[3];
-			get_indices(prim_index,indices[0],indices[1],indices[2]);
-			get_vertex(indices[0],triangle.m_vertices[0]);
-			get_vertex(indices[1],triangle.m_vertices[1]);
-			get_vertex(indices[2],triangle.m_vertices[2]);
+			get_indices(prim_index, indices[0], indices[1], indices[2]);
+			get_vertex(indices[0], triangle.m_vertices[0]);
+			get_vertex(indices[1], triangle.m_vertices[1]);
+			get_vertex(indices[2], triangle.m_vertices[2]);
 			triangle.m_margin = m_margin;
 		}
 
-		SIMD_FORCE_INLINE void get_bullet_triangle(int prim_index,btTriangleShapeEx & triangle) const
+		SIMD_FORCE_INLINE void get_bullet_triangle(int prim_index, btTriangleShapeEx& triangle) const
 		{
 			unsigned int indices[3];
-			get_indices(prim_index,indices[0],indices[1],indices[2]);
-			get_vertex(indices[0],triangle.m_vertices1[0]);
-			get_vertex(indices[1],triangle.m_vertices1[1]);
-			get_vertex(indices[2],triangle.m_vertices1[2]);
+			get_indices(prim_index, indices[0], indices[1], indices[2]);
+			get_vertex(indices[0], triangle.m_vertices1[0]);
+			get_vertex(indices[1], triangle.m_vertices1[1]);
+			get_vertex(indices[2], triangle.m_vertices1[2]);
 			triangle.setMargin(m_margin);
 		}
-
 	};
 
-
 protected:
 	TrimeshPrimitiveManager m_primitive_manager;
-public:
 
+public:
 	btGImpactMeshShapePart()
 	{
 		m_box_set.setPrimitiveManager(&m_primitive_manager);
 	}
 
-
-	btGImpactMeshShapePart(btStridingMeshInterface * meshInterface,	int part)
-	{
-		m_primitive_manager.m_meshInterface = meshInterface;
-		m_primitive_manager.m_part = part;
-		m_box_set.setPrimitiveManager(&m_primitive_manager);
-	}
-
-	virtual ~btGImpactMeshShapePart()
-	{
-	}
+	btGImpactMeshShapePart(btStridingMeshInterface* meshInterface, int part);
+	virtual ~btGImpactMeshShapePart();
 
 	//! if true, then its children must get transforms.
 	virtual bool childrenHasTransform() const
@@ -740,51 +706,36 @@ public:
 		return false;
 	}
 
-
 	//! call when reading child shapes
-	virtual void lockChildShapes() const
-	{
-		void * dummy = (void*)(m_box_set.getPrimitiveManager());
-		TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>(dummy);
-		dummymanager->lock();
-	}
-
-	virtual void unlockChildShapes()  const
-	{
-		void * dummy = (void*)(m_box_set.getPrimitiveManager());
-		TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>(dummy);
-		dummymanager->unlock();
-	}
+	virtual void lockChildShapes() const;
+	virtual void unlockChildShapes() const;
 
 	//! Gets the number of children
-	virtual int	getNumChildShapes() const
+	virtual int getNumChildShapes() const
 	{
 		return m_primitive_manager.get_primitive_count();
 	}
 
-
 	//! Gets the children
 	virtual btCollisionShape* getChildShape(int index)
 	{
-        (void) index;
+		(void)index;
 		btAssert(0);
 		return NULL;
 	}
 
-
-
 	//! Gets the child
 	virtual const btCollisionShape* getChildShape(int index) const
 	{
-        (void) index;
+		(void)index;
 		btAssert(0);
 		return NULL;
 	}
 
 	//! Gets the children transform
-	virtual btTransform	getChildTransform(int index) const
+	virtual btTransform getChildTransform(int index) const
 	{
-        (void) index;
+		(void)index;
 		btAssert(0);
 		return btTransform();
 	}
@@ -793,35 +744,27 @@ public:
 	/*!
 	\post You must call updateBound() for update the box set.
 	*/
-	virtual void setChildTransform(int index, const btTransform & transform)
+	virtual void setChildTransform(int index, const btTransform& transform)
 	{
-        (void) index;
-        (void) transform;
+		(void)index;
+		(void)transform;
 		btAssert(0);
 	}
 
-
 	//! Obtains the primitive manager
-	virtual const btPrimitiveManagerBase * getPrimitiveManager()  const
+	virtual const btPrimitiveManagerBase* getPrimitiveManager() const
 	{
 		return &m_primitive_manager;
 	}
 
-	SIMD_FORCE_INLINE TrimeshPrimitiveManager * getTrimeshPrimitiveManager()
+	SIMD_FORCE_INLINE TrimeshPrimitiveManager* getTrimeshPrimitiveManager()
 	{
 		return &m_primitive_manager;
 	}
 
+	virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const;
 
-
-
-
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
-
-
-
-
-	virtual const char*	getName()const
+	virtual const char* getName() const
 	{
 		return "GImpactMeshShapePart";
 	}
@@ -843,61 +786,58 @@ public:
 		return false;
 	}
 
-	virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const
+	virtual void getBulletTriangle(int prim_index, btTriangleShapeEx& triangle) const
 	{
-		m_primitive_manager.get_bullet_triangle(prim_index,triangle);
+		m_primitive_manager.get_bullet_triangle(prim_index, triangle);
 	}
 
-	virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const
+	virtual void getBulletTetrahedron(int prim_index, btTetrahedronShapeEx& tetrahedron) const
 	{
-        (void) prim_index;
-        (void) tetrahedron;
+		(void)prim_index;
+		(void)tetrahedron;
 		btAssert(0);
 	}
 
-
-
 	SIMD_FORCE_INLINE int getVertexCount() const
 	{
 		return m_primitive_manager.get_vertex_count();
 	}
 
-	SIMD_FORCE_INLINE void getVertex(int vertex_index, btVector3 & vertex) const
+	SIMD_FORCE_INLINE void getVertex(int vertex_index, btVector3& vertex) const
 	{
-		m_primitive_manager.get_vertex(vertex_index,vertex);
+		m_primitive_manager.get_vertex(vertex_index, vertex);
 	}
 
 	SIMD_FORCE_INLINE void setMargin(btScalar margin)
-    {
-    	m_primitive_manager.m_margin = margin;
-    	postUpdate();
-    }
-
-    SIMD_FORCE_INLINE btScalar getMargin() const
-    {
-    	return m_primitive_manager.m_margin;
-    }
-
-    virtual void	setLocalScaling(const btVector3& scaling)
-    {
-    	m_primitive_manager.m_scale = scaling;
-    	postUpdate();
-    }
-
-    virtual const btVector3& getLocalScaling() const
-    {
-    	return m_primitive_manager.m_scale;
-    }
-
-    SIMD_FORCE_INLINE int getPart() const
-    {
-    	return (int)m_primitive_manager.m_part;
-    }
-
-	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
-	virtual void	processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom,const btVector3& rayTo) const;
-};
+	{
+		m_primitive_manager.m_margin = margin;
+		postUpdate();
+	}
+
+	SIMD_FORCE_INLINE btScalar getMargin() const
+	{
+		return m_primitive_manager.m_margin;
+	}
+
+	virtual void setLocalScaling(const btVector3& scaling)
+	{
+		m_primitive_manager.m_scale = scaling;
+		postUpdate();
+	}
+
+	virtual const btVector3& getLocalScaling() const
+	{
+		return m_primitive_manager.m_scale;
+	}
+
+	SIMD_FORCE_INLINE int getPart() const
+	{
+		return (int)m_primitive_manager.m_part;
+	}
 
+	virtual void processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const;
+	virtual void processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const;
+};
 
 //! This class manages a mesh supplied by the btStridingMeshInterface interface.
 /*!
@@ -913,29 +853,29 @@ class btGImpactMeshShape : public btGImpactShapeInterface
 
 protected:
 	btAlignedObjectArray<btGImpactMeshShapePart*> m_mesh_parts;
-	void buildMeshParts(btStridingMeshInterface * meshInterface)
+	void buildMeshParts(btStridingMeshInterface* meshInterface)
 	{
-		for (int i=0;i<meshInterface->getNumSubParts() ;++i )
+		for (int i = 0; i < meshInterface->getNumSubParts(); ++i)
 		{
-			btGImpactMeshShapePart * newpart = new btGImpactMeshShapePart(meshInterface,i);
+			btGImpactMeshShapePart* newpart = new btGImpactMeshShapePart(meshInterface, i);
 			m_mesh_parts.push_back(newpart);
 		}
 	}
 
 	//! use this function for perfofm refit in bounding boxes
-    virtual void calcLocalAABB()
-    {
-    	m_localAABB.invalidate();
-    	int i = m_mesh_parts.size();
-    	while(i--)
-    	{
-    		m_mesh_parts[i]->updateBound();
-    		m_localAABB.merge(m_mesh_parts[i]->getLocalBox());
-    	}
-    }
+	virtual void calcLocalAABB()
+	{
+		m_localAABB.invalidate();
+		int i = m_mesh_parts.size();
+		while (i--)
+		{
+			m_mesh_parts[i]->updateBound();
+			m_localAABB.merge(m_mesh_parts[i]->getLocalBox());
+		}
+	}
 
 public:
-	btGImpactMeshShape(btStridingMeshInterface * meshInterface)
+	btGImpactMeshShape(btStridingMeshInterface* meshInterface)
 	{
 		m_meshInterface = meshInterface;
 		buildMeshParts(meshInterface);
@@ -944,15 +884,14 @@ public:
 	virtual ~btGImpactMeshShape()
 	{
 		int i = m_mesh_parts.size();
-    	while(i--)
-    	{
-			btGImpactMeshShapePart * part = m_mesh_parts[i];
+		while (i--)
+		{
+			btGImpactMeshShapePart* part = m_mesh_parts[i];
 			delete part;
-    	}
+		}
 		m_mesh_parts.clear();
 	}
 
-
 	btStridingMeshInterface* getMeshInterface()
 	{
 		return m_meshInterface;
@@ -968,79 +907,73 @@ public:
 		return m_mesh_parts.size();
 	}
 
-	btGImpactMeshShapePart * getMeshPart(int index)
+	btGImpactMeshShapePart* getMeshPart(int index)
 	{
 		return m_mesh_parts[index];
 	}
 
-
-
-	const btGImpactMeshShapePart * getMeshPart(int index) const
+	const btGImpactMeshShapePart* getMeshPart(int index) const
 	{
 		return m_mesh_parts[index];
 	}
 
-
-	virtual void	setLocalScaling(const btVector3& scaling)
+	virtual void setLocalScaling(const btVector3& scaling)
 	{
 		localScaling = scaling;
 
 		int i = m_mesh_parts.size();
-    	while(i--)
-    	{
-			btGImpactMeshShapePart * part = m_mesh_parts[i];
+		while (i--)
+		{
+			btGImpactMeshShapePart* part = m_mesh_parts[i];
 			part->setLocalScaling(scaling);
-    	}
+		}
 
 		m_needs_update = true;
 	}
 
 	virtual void setMargin(btScalar margin)
-    {
-    	m_collisionMargin = margin;
+	{
+		m_collisionMargin = margin;
 
 		int i = m_mesh_parts.size();
-    	while(i--)
-    	{
-			btGImpactMeshShapePart * part = m_mesh_parts[i];
+		while (i--)
+		{
+			btGImpactMeshShapePart* part = m_mesh_parts[i];
 			part->setMargin(margin);
-    	}
+		}
 
 		m_needs_update = true;
-    }
+	}
 
 	//! Tells to this object that is needed to refit all the meshes
-    virtual void postUpdate()
-    {
+	virtual void postUpdate()
+	{
 		int i = m_mesh_parts.size();
-    	while(i--)
-    	{
-			btGImpactMeshShapePart * part = m_mesh_parts[i];
+		while (i--)
+		{
+			btGImpactMeshShapePart* part = m_mesh_parts[i];
 			part->postUpdate();
-    	}
-
-    	m_needs_update = true;
-    }
+		}
 
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+		m_needs_update = true;
+	}
 
+	virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const;
 
 	//! Obtains the primitive manager
-	virtual const btPrimitiveManagerBase * getPrimitiveManager()  const
+	virtual const btPrimitiveManagerBase* getPrimitiveManager() const
 	{
 		btAssert(0);
 		return NULL;
 	}
 
-
 	//! Gets the number of children
-	virtual int	getNumChildShapes() const
+	virtual int getNumChildShapes() const
 	{
 		btAssert(0);
 		return 0;
 	}
 
-
 	//! if true, then its children must get transforms.
 	virtual bool childrenHasTransform() const
 	{
@@ -1062,15 +995,17 @@ public:
 		return false;
 	}
 
-	virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const
+	virtual void getBulletTriangle(int prim_index, btTriangleShapeEx& triangle) const
 	{
-        (void) prim_index; (void) triangle;
+		(void)prim_index;
+		(void)triangle;
 		btAssert(0);
 	}
 
-	virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const
+	virtual void getBulletTetrahedron(int prim_index, btTetrahedronShapeEx& tetrahedron) const
 	{
-        (void) prim_index; (void) tetrahedron;
+		(void)prim_index;
+		(void)tetrahedron;
 		btAssert(0);
 	}
 
@@ -1085,39 +1020,38 @@ public:
 		btAssert(0);
 	}
 
-
-
-
 	//! Retrieves the bound from a child
-    /*!
+	/*!
     */
-    virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
-    {
-        (void) child_index; (void) t; (void) aabbMin; (void) aabbMax;
-        btAssert(0);
-    }
+	virtual void getChildAabb(int child_index, const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
+	{
+		(void)child_index;
+		(void)t;
+		(void)aabbMin;
+		(void)aabbMax;
+		btAssert(0);
+	}
 
 	//! Gets the children
 	virtual btCollisionShape* getChildShape(int index)
 	{
-        (void) index;
+		(void)index;
 		btAssert(0);
 		return NULL;
 	}
 
-
 	//! Gets the child
 	virtual const btCollisionShape* getChildShape(int index) const
 	{
-        (void) index;
+		(void)index;
 		btAssert(0);
 		return NULL;
 	}
 
 	//! Gets the children transform
-	virtual btTransform	getChildTransform(int index) const
+	virtual btTransform getChildTransform(int index) const
 	{
-        (void) index;
+		(void)index;
 		btAssert(0);
 		return btTransform();
 	}
@@ -1126,59 +1060,56 @@ public:
 	/*!
 	\post You must call updateBound() for update the box set.
 	*/
-	virtual void setChildTransform(int index, const btTransform & transform)
+	virtual void setChildTransform(int index, const btTransform& transform)
 	{
-        (void) index; (void) transform;
+		(void)index;
+		(void)transform;
 		btAssert(0);
 	}
 
-
 	virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const
 	{
 		return CONST_GIMPACT_TRIMESH_SHAPE;
 	}
 
-
-	virtual const char*	getName()const
+	virtual const char* getName() const
 	{
 		return "GImpactMesh";
 	}
 
-	virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback)  const;
+	virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const;
 
 	//! Function for retrieve triangles.
 	/*!
 	It gives the triangles in local space
 	*/
-	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+	virtual void processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const;
 
-	virtual void	processAllTrianglesRay (btTriangleCallback* callback,const btVector3& rayFrom,const btVector3& rayTo) const;
+	virtual void processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const;
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btGImpactMeshShapeData
+struct btGImpactMeshShapeData
 {
-	btCollisionShapeData	m_collisionShapeData;
+	btCollisionShapeData m_collisionShapeData;
 
 	btStridingMeshInterfaceData m_meshInterface;
 
-	btVector3FloatData	m_localScaling;
+	btVector3FloatData m_localScaling;
 
-	float	m_collisionMargin;
+	float m_collisionMargin;
 
-	int		m_gimpactSubType;
+	int m_gimpactSubType;
 };
 
-SIMD_FORCE_INLINE	int	btGImpactMeshShape::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btGImpactMeshShape::calculateSerializeBufferSize() const
 {
 	return sizeof(btGImpactMeshShapeData);
 }
 
-
-#endif //GIMPACT_MESH_SHAPE_H
+#endif  //GIMPACT_MESH_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp
index 5d07d1adb97..bfdb3db5d08 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp
@@ -20,48 +20,45 @@ subject to the following restrictions:
 
 #include "btGenericPoolAllocator.h"
 
-
-
 /// *************** btGenericMemoryPool ******************///////////
 
 size_t btGenericMemoryPool::allocate_from_free_nodes(size_t num_elements)
 {
 	size_t ptr = BT_UINT_MAX;
 
-	if(m_free_nodes_count == 0) return BT_UINT_MAX;
+	if (m_free_nodes_count == 0) return BT_UINT_MAX;
 	// find an avaliable free node with the correct size
 	size_t revindex = m_free_nodes_count;
 
-	while(revindex-- && ptr == BT_UINT_MAX)
+	while (revindex-- && ptr == BT_UINT_MAX)
 	{
-		if(m_allocated_sizes[m_free_nodes[revindex]]>=num_elements)
+		if (m_allocated_sizes[m_free_nodes[revindex]] >= num_elements)
 		{
 			ptr = revindex;
 		}
 	}
-	if(ptr == BT_UINT_MAX) return BT_UINT_MAX; // not found
-
+	if (ptr == BT_UINT_MAX) return BT_UINT_MAX;  // not found
 
 	revindex = ptr;
 	ptr = m_free_nodes[revindex];
 	// post: ptr contains the node index, and revindex the index in m_free_nodes
 
-	size_t  finalsize = m_allocated_sizes[ptr];
+	size_t finalsize = m_allocated_sizes[ptr];
 	finalsize -= num_elements;
 
 	m_allocated_sizes[ptr] = num_elements;
 
 	// post: finalsize>=0, m_allocated_sizes[ptr] has the requested size
 
-	if(finalsize>0) // preserve free node, there are some free memory
+	if (finalsize > 0)  // preserve free node, there are some free memory
 	{
 		m_free_nodes[revindex] = ptr + num_elements;
 		m_allocated_sizes[ptr + num_elements] = finalsize;
 	}
-	else // delete free node
+	else  // delete free node
 	{
 		// swap with end
-		m_free_nodes[revindex] = m_free_nodes[m_free_nodes_count-1];
+		m_free_nodes[revindex] = m_free_nodes[m_free_nodes_count - 1];
 		m_free_nodes_count--;
 	}
 
@@ -70,17 +67,16 @@ size_t btGenericMemoryPool::allocate_from_free_nodes(size_t num_elements)
 
 size_t btGenericMemoryPool::allocate_from_pool(size_t num_elements)
 {
-	if(m_allocated_count+num_elements>m_max_element_count) return BT_UINT_MAX;
+	if (m_allocated_count + num_elements > m_max_element_count) return BT_UINT_MAX;
 
 	size_t ptr = m_allocated_count;
 
 	m_allocated_sizes[m_allocated_count] = num_elements;
-	m_allocated_count+=num_elements;
+	m_allocated_count += num_elements;
 
 	return ptr;
 }
 
-
 void btGenericMemoryPool::init_pool(size_t element_size, size_t element_count)
 {
 	m_allocated_count = 0;
@@ -89,14 +85,11 @@ void btGenericMemoryPool::init_pool(size_t element_size, size_t element_count)
 	m_element_size = element_size;
 	m_max_element_count = element_count;
 
+	m_pool = (unsigned char *)btAlignedAlloc(m_element_size * m_max_element_count, 16);
+	m_free_nodes = (size_t *)btAlignedAlloc(sizeof(size_t) * m_max_element_count, 16);
+	m_allocated_sizes = (size_t *)btAlignedAlloc(sizeof(size_t) * m_max_element_count, 16);
 
-
-
-	m_pool = (unsigned char *) btAlignedAlloc(m_element_size*m_max_element_count,16);
-	m_free_nodes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16);
-	m_allocated_sizes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16);
-
-	for (size_t i = 0;i< m_max_element_count;i++ )
+	for (size_t i = 0; i < m_max_element_count; i++)
 	{
 		m_allocated_sizes[i] = 0;
 	}
@@ -111,150 +104,141 @@ void btGenericMemoryPool::end_pool()
 	m_free_nodes_count = 0;
 }
 
-
 //! Allocates memory in pool
 /*!
 \param size_bytes size in bytes of the buffer
 */
-void * btGenericMemoryPool::allocate(size_t size_bytes)
+void *btGenericMemoryPool::allocate(size_t size_bytes)
 {
-
-	size_t module = size_bytes%m_element_size;
-	size_t element_count = size_bytes/m_element_size;
-	if(module>0) element_count++;
+	size_t module = size_bytes % m_element_size;
+	size_t element_count = size_bytes / m_element_size;
+	if (module > 0) element_count++;
 
 	size_t alloc_pos = allocate_from_free_nodes(element_count);
 	// a free node is found
-	if(alloc_pos != BT_UINT_MAX)
+	if (alloc_pos != BT_UINT_MAX)
 	{
 		return get_element_data(alloc_pos);
 	}
 	// allocate directly on pool
 	alloc_pos = allocate_from_pool(element_count);
 
-	if(alloc_pos == BT_UINT_MAX) return NULL; // not space
+	if (alloc_pos == BT_UINT_MAX) return NULL;  // not space
 	return get_element_data(alloc_pos);
 }
 
-bool btGenericMemoryPool::freeMemory(void * pointer)
+bool btGenericMemoryPool::freeMemory(void *pointer)
 {
-	unsigned char * pointer_pos = (unsigned char *)pointer;
-	unsigned char * pool_pos = (unsigned char *)m_pool;
+	unsigned char *pointer_pos = (unsigned char *)pointer;
+	unsigned char *pool_pos = (unsigned char *)m_pool;
 	// calc offset
-	if(pointer_pos<pool_pos) return false;//other pool
+	if (pointer_pos < pool_pos) return false;  //other pool
 	size_t offset = size_t(pointer_pos - pool_pos);
-	if(offset>=get_pool_capacity()) return false;// far away
+	if (offset >= get_pool_capacity()) return false;  // far away
 
 	// find free position
-	m_free_nodes[m_free_nodes_count] = offset/m_element_size;
+	m_free_nodes[m_free_nodes_count] = offset / m_element_size;
 	m_free_nodes_count++;
 	return true;
 }
 
-
 /// *******************! btGenericPoolAllocator *******************!///
 
-
 btGenericPoolAllocator::~btGenericPoolAllocator()
 {
 	// destroy pools
 	size_t i;
-	for (i=0;i<m_pool_count;i++)
+	for (i = 0; i < m_pool_count; i++)
 	{
 		m_pools[i]->end_pool();
 		btAlignedFree(m_pools[i]);
 	}
 }
 
-
 // creates a pool
-btGenericMemoryPool * btGenericPoolAllocator::push_new_pool()
+btGenericMemoryPool *btGenericPoolAllocator::push_new_pool()
 {
-	if(m_pool_count >= BT_DEFAULT_MAX_POOLS) return NULL;
+	if (m_pool_count >= BT_DEFAULT_MAX_POOLS) return NULL;
 
-	btGenericMemoryPool * newptr = (btGenericMemoryPool *)btAlignedAlloc(sizeof(btGenericMemoryPool),16);
+	btGenericMemoryPool *newptr = (btGenericMemoryPool *)btAlignedAlloc(sizeof(btGenericMemoryPool), 16);
 
 	m_pools[m_pool_count] = newptr;
 
-	m_pools[m_pool_count]->init_pool(m_pool_element_size,m_pool_element_count);
+	m_pools[m_pool_count]->init_pool(m_pool_element_size, m_pool_element_count);
 
 	m_pool_count++;
 	return newptr;
 }
 
-void * btGenericPoolAllocator::failback_alloc(size_t size_bytes)
+void *btGenericPoolAllocator::failback_alloc(size_t size_bytes)
 {
+	btGenericMemoryPool *pool = NULL;
 
-	btGenericMemoryPool * pool = NULL;
-
-
-	if(size_bytes<=get_pool_capacity())
+	if (size_bytes <= get_pool_capacity())
 	{
-		pool = 	push_new_pool();
+		pool = push_new_pool();
 	}
 
-	if(pool==NULL) // failback
+	if (pool == NULL)  // failback
 	{
-		return btAlignedAlloc(size_bytes,16);
+		return btAlignedAlloc(size_bytes, 16);
 	}
 
 	return pool->allocate(size_bytes);
 }
 
-bool btGenericPoolAllocator::failback_free(void * pointer)
+bool btGenericPoolAllocator::failback_free(void *pointer)
 {
 	btAlignedFree(pointer);
 	return true;
 }
 
-
 //! Allocates memory in pool
 /*!
 \param size_bytes size in bytes of the buffer
 */
-void * btGenericPoolAllocator::allocate(size_t size_bytes)
+void *btGenericPoolAllocator::allocate(size_t size_bytes)
 {
-	void * ptr = NULL;
+	void *ptr = NULL;
 
 	size_t i = 0;
-	while(i<m_pool_count && ptr == NULL)
+	while (i < m_pool_count && ptr == NULL)
 	{
 		ptr = m_pools[i]->allocate(size_bytes);
 		++i;
 	}
 
-	if(ptr) return ptr;
+	if (ptr) return ptr;
 
 	return failback_alloc(size_bytes);
 }
 
-bool btGenericPoolAllocator::freeMemory(void * pointer)
+bool btGenericPoolAllocator::freeMemory(void *pointer)
 {
 	bool result = false;
 
 	size_t i = 0;
-	while(i<m_pool_count && result == false)
+	while (i < m_pool_count && result == false)
 	{
 		result = m_pools[i]->freeMemory(pointer);
 		++i;
 	}
 
-	if(result) return true;
+	if (result) return true;
 
 	return failback_free(pointer);
 }
 
 /// ************** STANDARD ALLOCATOR ***************************///
 
-
 #define BT_DEFAULT_POOL_SIZE 32768
 #define BT_DEFAULT_POOL_ELEMENT_SIZE 8
 
 // main allocator
-class GIM_STANDARD_ALLOCATOR: public btGenericPoolAllocator
+class GIM_STANDARD_ALLOCATOR : public btGenericPoolAllocator
 {
 public:
-	GIM_STANDARD_ALLOCATOR():btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE,BT_DEFAULT_POOL_SIZE)
+	GIM_STANDARD_ALLOCATOR() : btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE, BT_DEFAULT_POOL_SIZE)
 	{
 	}
 };
@@ -262,19 +246,18 @@ public:
 // global allocator
 GIM_STANDARD_ALLOCATOR g_main_allocator;
 
-
-void * btPoolAlloc(size_t size)
+void *btPoolAlloc(size_t size)
 {
 	return g_main_allocator.allocate(size);
 }
 
-void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize)
+void *btPoolRealloc(void *ptr, size_t oldsize, size_t newsize)
 {
-	void * newptr = btPoolAlloc(newsize);
-    size_t copysize = oldsize<newsize?oldsize:newsize;
-    memcpy(newptr,ptr,copysize);
-    btPoolFree(ptr);
-    return newptr;
+	void *newptr = btPoolAlloc(newsize);
+	size_t copysize = oldsize < newsize ? oldsize : newsize;
+	memcpy(newptr, ptr, copysize);
+	btPoolFree(ptr);
+	return newptr;
 }
 
 void btPoolFree(void *ptr)
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h
index b46d8516347..a535088e484 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h
@@ -29,16 +29,16 @@ subject to the following restrictions:
 #define BT_UINT_MAX UINT_MAX
 #define BT_DEFAULT_MAX_POOLS 16
 
-
 //! Generic Pool class
 class btGenericMemoryPool
 {
 public:
-	unsigned char * m_pool; //[m_element_size*m_max_element_count];
-	size_t * m_free_nodes; //[m_max_element_count];//! free nodes
-	size_t * m_allocated_sizes;//[m_max_element_count];//! Number of elements allocated per node
+	unsigned char *m_pool;      //[m_element_size*m_max_element_count];
+	size_t *m_free_nodes;       //[m_max_element_count];//! free nodes
+	size_t *m_allocated_sizes;  //[m_max_element_count];//! Number of elements allocated per node
 	size_t m_allocated_count;
 	size_t m_free_nodes_count;
+
 protected:
 	size_t m_element_size;
 	size_t m_max_element_count;
@@ -47,12 +47,10 @@ protected:
 	size_t allocate_from_pool(size_t num_elements);
 
 public:
-
 	void init_pool(size_t element_size, size_t element_count);
 
 	void end_pool();
 
-
 	btGenericMemoryPool(size_t element_size, size_t element_count)
 	{
 		init_pool(element_size, element_count);
@@ -63,10 +61,9 @@ public:
 		end_pool();
 	}
 
-
 	inline size_t get_pool_capacity()
 	{
-		return m_element_size*m_max_element_count;
+		return m_element_size * m_max_element_count;
 	}
 
 	inline size_t gem_element_size()
@@ -89,23 +86,20 @@ public:
 		return m_free_nodes_count;
 	}
 
-	inline void * get_element_data(size_t element_index)
+	inline void *get_element_data(size_t element_index)
 	{
-		return &m_pool[element_index*m_element_size];
+		return &m_pool[element_index * m_element_size];
 	}
 
 	//! Allocates memory in pool
 	/*!
 	\param size_bytes size in bytes of the buffer
 	*/
-	void * allocate(size_t size_bytes);
+	void *allocate(size_t size_bytes);
 
-	bool freeMemory(void * pointer);
+	bool freeMemory(void *pointer);
 };
 
-
-
-
 //! Generic Allocator with pools
 /*!
 General purpose Allocator which can create Memory Pools dynamiacally as needed.
@@ -115,26 +109,25 @@ class btGenericPoolAllocator
 protected:
 	size_t m_pool_element_size;
 	size_t m_pool_element_count;
+
 public:
-	btGenericMemoryPool * m_pools[BT_DEFAULT_MAX_POOLS];
+	btGenericMemoryPool *m_pools[BT_DEFAULT_MAX_POOLS];
 	size_t m_pool_count;
 
-
 	inline size_t get_pool_capacity()
 	{
-		return m_pool_element_size*m_pool_element_count;
+		return m_pool_element_size * m_pool_element_count;
 	}
 
-
 protected:
 	// creates a pool
-	btGenericMemoryPool * push_new_pool();
+	btGenericMemoryPool *push_new_pool();
 
-	void * failback_alloc(size_t size_bytes);
+	void *failback_alloc(size_t size_bytes);
 
-	bool failback_free(void * pointer);
-public:
+	bool failback_free(void *pointer);
 
+public:
 	btGenericPoolAllocator(size_t pool_element_size, size_t pool_element_count)
 	{
 		m_pool_count = 0;
@@ -148,16 +141,13 @@ public:
 	/*!
 	\param size_bytes size in bytes of the buffer
 	*/
-	void * allocate(size_t size_bytes);
+	void *allocate(size_t size_bytes);
 
-	bool freeMemory(void * pointer);
+	bool freeMemory(void *pointer);
 };
 
-
-
-void * btPoolAlloc(size_t size);
-void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize);
+void *btPoolAlloc(size_t size);
+void *btPoolRealloc(void *ptr, size_t oldsize, size_t newsize);
 void btPoolFree(void *ptr);
 
-
 #endif
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h b/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h
index 60f06510ad7..6a1ee6dcf91 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h
@@ -27,52 +27,44 @@ subject to the following restrictions:
 
 #include "btBoxCollision.h"
 
-
-
-
-
 #define PLANEDIREPSILON 0.0000001f
 #define PARALELENORMALS 0.000001f
 
-
-#define BT_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
+#define BT_CLAMP(number, minval, maxval) (number < minval ? minval : (number > maxval ? maxval : number))
 
 /// Calc a plane from a triangle edge an a normal. plane is a vec4f
-SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 & e1,const btVector3 &  e2, const btVector3 & normal,btVector4 & plane)
+SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 &e1, const btVector3 &e2, const btVector3 &normal, btVector4 &plane)
 {
-	btVector3 planenormal = (e2-e1).cross(normal);
+	btVector3 planenormal = (e2 - e1).cross(normal);
 	planenormal.normalize();
-	plane.setValue(planenormal[0],planenormal[1],planenormal[2],e2.dot(planenormal));
+	plane.setValue(planenormal[0], planenormal[1], planenormal[2], e2.dot(planenormal));
 }
 
-
-
 //***************** SEGMENT and LINE FUNCTIONS **********************************///
 
 /*! Finds the closest point(cp) to (v) on a segment (e1,e2)
  */
 SIMD_FORCE_INLINE void bt_closest_point_on_segment(
-	btVector3 & cp, const btVector3 & v,
-	const btVector3  &e1,const btVector3 &e2)
+	btVector3 &cp, const btVector3 &v,
+	const btVector3 &e1, const btVector3 &e2)
 {
-    btVector3 n = e2-e1;
-    cp = v - e1;
-	btScalar _scalar = cp.dot(n)/n.dot(n);
-	if(_scalar <0.0f)
+	btVector3 n = e2 - e1;
+	cp = v - e1;
+	btScalar _scalar = cp.dot(n) / n.dot(n);
+	if (_scalar < 0.0f)
 	{
-	    cp = e1;
+		cp = e1;
 	}
-	else if(_scalar >1.0f)
+	else if (_scalar > 1.0f)
 	{
-	    cp = e2;
+		cp = e2;
 	}
 	else
 	{
-		cp = _scalar*n + e1;
+		cp = _scalar * n + e1;
 	}
 }
 
-
 //! line plane collision
 /*!
 *\return
@@ -82,131 +74,125 @@ SIMD_FORCE_INLINE void bt_closest_point_on_segment(
 */
 
 SIMD_FORCE_INLINE int bt_line_plane_collision(
-	const btVector4 & plane,
-	const btVector3 & vDir,
-	const btVector3 & vPoint,
-	btVector3 & pout,
+	const btVector4 &plane,
+	const btVector3 &vDir,
+	const btVector3 &vPoint,
+	btVector3 &pout,
 	btScalar &tparam,
 	btScalar tmin, btScalar tmax)
 {
-
 	btScalar _dotdir = vDir.dot(plane);
 
-	if(btFabs(_dotdir)<PLANEDIREPSILON)
+	if (btFabs(_dotdir) < PLANEDIREPSILON)
 	{
 		tparam = tmax;
-	    return 0;
+		return 0;
 	}
 
-	btScalar _dis = bt_distance_point_plane(plane,vPoint);
-	char returnvalue = _dis<0.0f? 2:1;
-	tparam = -_dis/_dotdir;
+	btScalar _dis = bt_distance_point_plane(plane, vPoint);
+	char returnvalue = _dis < 0.0f ? 2 : 1;
+	tparam = -_dis / _dotdir;
 
-	if(tparam<tmin)
+	if (tparam < tmin)
 	{
 		returnvalue = 0;
 		tparam = tmin;
 	}
-	else if(tparam>tmax)
+	else if (tparam > tmax)
 	{
 		returnvalue = 0;
 		tparam = tmax;
 	}
-	pout = tparam*vDir + vPoint;
+	pout = tparam * vDir + vPoint;
 	return returnvalue;
 }
 
-
 //! Find closest points on segments
 SIMD_FORCE_INLINE void bt_segment_collision(
-	const btVector3 & vA1,
-	const btVector3 & vA2,
-	const btVector3 & vB1,
-	const btVector3 & vB2,
-	btVector3 & vPointA,
-	btVector3 & vPointB)
+	const btVector3 &vA1,
+	const btVector3 &vA2,
+	const btVector3 &vB1,
+	const btVector3 &vB2,
+	btVector3 &vPointA,
+	btVector3 &vPointB)
 {
-    btVector3 AD = vA2 - vA1;
-    btVector3 BD = vB2 - vB1;
-    btVector3 N = AD.cross(BD);
-    btScalar tp = N.length2();
-
-    btVector4 _M;//plane
-
-    if(tp<SIMD_EPSILON)//ARE PARALELE
-    {
-    	//project B over A
-    	bool invert_b_order = false;
-    	_M[0] = vB1.dot(AD);
-    	_M[1] = vB2.dot(AD);
-
-    	if(_M[0]>_M[1])
-    	{
-    		invert_b_order  = true;
-    		BT_SWAP_NUMBERS(_M[0],_M[1]);
-    	}
-    	_M[2] = vA1.dot(AD);
-    	_M[3] = vA2.dot(AD);
-    	//mid points
-    	N[0] = (_M[0]+_M[1])*0.5f;
-    	N[1] = (_M[2]+_M[3])*0.5f;
-
-    	if(N[0]<N[1])
-    	{
-    		if(_M[1]<_M[2])
-    		{
-    			vPointB = invert_b_order?vB1:vB2;
-    			vPointA = vA1;
-    		}
-    		else if(_M[1]<_M[3])
-    		{
-    			vPointB = invert_b_order?vB1:vB2;
-    			bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
-    		}
-    		else
-    		{
-    			vPointA = vA2;
-    			bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
-    		}
-    	}
-    	else
-    	{
-    		if(_M[3]<_M[0])
-    		{
-    			vPointB = invert_b_order?vB2:vB1;
-    			vPointA = vA2;
-    		}
-    		else if(_M[3]<_M[1])
-    		{
-    			vPointA = vA2;
-    			bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
-    		}
-    		else
-    		{
-    			vPointB = invert_b_order?vB1:vB2;
-    			bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
-    		}
-    	}
-    	return;
-    }
-
-    N = N.cross(BD);
-    _M.setValue(N[0],N[1],N[2],vB1.dot(N));
+	btVector3 AD = vA2 - vA1;
+	btVector3 BD = vB2 - vB1;
+	btVector3 N = AD.cross(BD);
+	btScalar tp = N.length2();
 
-	// get point A as the plane collision point
-    bt_line_plane_collision(_M,AD,vA1,vPointA,tp,btScalar(0), btScalar(1));
-
-    /*Closest point on segment*/
-    vPointB = vPointA - vB1;
-	tp = vPointB.dot(BD);
-	tp/= BD.dot(BD);
-	tp = BT_CLAMP(tp,0.0f,1.0f);
+	btVector4 _M;  //plane
 
-	vPointB = tp*BD + vB1;
-}
+	if (tp < SIMD_EPSILON)  //ARE PARALELE
+	{
+		//project B over A
+		bool invert_b_order = false;
+		_M[0] = vB1.dot(AD);
+		_M[1] = vB2.dot(AD);
+
+		if (_M[0] > _M[1])
+		{
+			invert_b_order = true;
+			BT_SWAP_NUMBERS(_M[0], _M[1]);
+		}
+		_M[2] = vA1.dot(AD);
+		_M[3] = vA2.dot(AD);
+		//mid points
+		N[0] = (_M[0] + _M[1]) * 0.5f;
+		N[1] = (_M[2] + _M[3]) * 0.5f;
+
+		if (N[0] < N[1])
+		{
+			if (_M[1] < _M[2])
+			{
+				vPointB = invert_b_order ? vB1 : vB2;
+				vPointA = vA1;
+			}
+			else if (_M[1] < _M[3])
+			{
+				vPointB = invert_b_order ? vB1 : vB2;
+				bt_closest_point_on_segment(vPointA, vPointB, vA1, vA2);
+			}
+			else
+			{
+				vPointA = vA2;
+				bt_closest_point_on_segment(vPointB, vPointA, vB1, vB2);
+			}
+		}
+		else
+		{
+			if (_M[3] < _M[0])
+			{
+				vPointB = invert_b_order ? vB2 : vB1;
+				vPointA = vA2;
+			}
+			else if (_M[3] < _M[1])
+			{
+				vPointA = vA2;
+				bt_closest_point_on_segment(vPointB, vPointA, vB1, vB2);
+			}
+			else
+			{
+				vPointB = invert_b_order ? vB1 : vB2;
+				bt_closest_point_on_segment(vPointA, vPointB, vA1, vA2);
+			}
+		}
+		return;
+	}
 
+	N = N.cross(BD);
+	_M.setValue(N[0], N[1], N[2], vB1.dot(N));
 
+	// get point A as the plane collision point
+	bt_line_plane_collision(_M, AD, vA1, vPointA, tp, btScalar(0), btScalar(1));
 
+	/*Closest point on segment*/
+	vPointB = vPointA - vB1;
+	tp = vPointB.dot(BD);
+	tp /= BD.dot(BD);
+	tp = BT_CLAMP(tp, 0.0f, 1.0f);
 
+	vPointB = tp * BD + vB1;
+}
 
-#endif // GIM_VECTOR_H_INCLUDED
+#endif  // GIM_VECTOR_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h b/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h
index bd2633cfc59..19a02a21772 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h
@@ -27,54 +27,47 @@ subject to the following restrictions:
 
 #include "LinearMath/btTransform.h"
 
-
-
-
-
-
 SIMD_FORCE_INLINE void bt_calc_quantization_parameters(
-	btVector3 & outMinBound,
-	btVector3 & outMaxBound,
-	btVector3 & bvhQuantization,
-	const btVector3& srcMinBound,const btVector3& srcMaxBound,
+	btVector3& outMinBound,
+	btVector3& outMaxBound,
+	btVector3& bvhQuantization,
+	const btVector3& srcMinBound, const btVector3& srcMaxBound,
 	btScalar quantizationMargin)
 {
 	//enlarge the AABB to avoid division by zero when initializing the quantization values
-	btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin);
+	btVector3 clampValue(quantizationMargin, quantizationMargin, quantizationMargin);
 	outMinBound = srcMinBound - clampValue;
 	outMaxBound = srcMaxBound + clampValue;
 	btVector3 aabbSize = outMaxBound - outMinBound;
 	bvhQuantization = btVector3(btScalar(65535.0),
 								btScalar(65535.0),
-								btScalar(65535.0)) / aabbSize;
+								btScalar(65535.0)) /
+					  aabbSize;
 }
 
-
 SIMD_FORCE_INLINE void bt_quantize_clamp(
 	unsigned short* out,
 	const btVector3& point,
-	const btVector3 & min_bound,
-	const btVector3 & max_bound,
-	const btVector3 & bvhQuantization)
+	const btVector3& min_bound,
+	const btVector3& max_bound,
+	const btVector3& bvhQuantization)
 {
-
 	btVector3 clampedPoint(point);
 	clampedPoint.setMax(min_bound);
 	clampedPoint.setMin(max_bound);
 
 	btVector3 v = (clampedPoint - min_bound) * bvhQuantization;
-	out[0] = (unsigned short)(v.getX()+0.5f);
-	out[1] = (unsigned short)(v.getY()+0.5f);
-	out[2] = (unsigned short)(v.getZ()+0.5f);
+	out[0] = (unsigned short)(v.getX() + 0.5f);
+	out[1] = (unsigned short)(v.getY() + 0.5f);
+	out[2] = (unsigned short)(v.getZ() + 0.5f);
 }
 
-
 SIMD_FORCE_INLINE btVector3 bt_unquantize(
 	const unsigned short* vecIn,
-	const btVector3 & offset,
-	const btVector3 & bvhQuantization)
+	const btVector3& offset,
+	const btVector3& bvhQuantization)
 {
-	btVector3	vecOut;
+	btVector3 vecOut;
 	vecOut.setValue(
 		(btScalar)(vecIn[0]) / (bvhQuantization.getX()),
 		(btScalar)(vecIn[1]) / (bvhQuantization.getY()),
@@ -83,6 +76,4 @@ SIMD_FORCE_INLINE btVector3 bt_unquantize(
 	return vecOut;
 }
 
-
-
-#endif // BT_GIMPACT_QUANTIZATION_H_INCLUDED
+#endif  // BT_GIMPACT_QUANTIZATION_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp
index ca76cc54a12..292ef8c1ff3 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp
@@ -23,196 +23,181 @@ subject to the following restrictions:
 
 #include "btTriangleShapeEx.h"
 
-
-
-void GIM_TRIANGLE_CONTACT::merge_points(const btVector4 & plane,
-                                       btScalar margin, const btVector3 * points, int point_count)
+void GIM_TRIANGLE_CONTACT::merge_points(const btVector4& plane,
+										btScalar margin, const btVector3* points, int point_count)
 {
-    m_point_count = 0;
-    m_penetration_depth= -1000.0f;
+	m_point_count = 0;
+	m_penetration_depth = -1000.0f;
 
-    int point_indices[MAX_TRI_CLIPPING];
+	int point_indices[MAX_TRI_CLIPPING];
 
 	int _k;
 
-    for ( _k=0;_k<point_count;_k++)
-    {
-        btScalar _dist = - bt_distance_point_plane(plane,points[_k]) + margin;
-
-        if (_dist>=0.0f)
-        {
-            if (_dist>m_penetration_depth)
-            {
-                m_penetration_depth = _dist;
-                point_indices[0] = _k;
-                m_point_count=1;
-            }
-            else if ((_dist+SIMD_EPSILON)>=m_penetration_depth)
-            {
-                point_indices[m_point_count] = _k;
-                m_point_count++;
-            }
-        }
-    }
-
-    for ( _k=0;_k<m_point_count;_k++)
-    {
-        m_points[_k] = points[point_indices[_k]];
-    }
+	for (_k = 0; _k < point_count; _k++)
+	{
+		btScalar _dist = -bt_distance_point_plane(plane, points[_k]) + margin;
+
+		if (_dist >= 0.0f)
+		{
+			if (_dist > m_penetration_depth)
+			{
+				m_penetration_depth = _dist;
+				point_indices[0] = _k;
+				m_point_count = 1;
+			}
+			else if ((_dist + SIMD_EPSILON) >= m_penetration_depth)
+			{
+				point_indices[m_point_count] = _k;
+				m_point_count++;
+			}
+		}
+	}
+
+	for (_k = 0; _k < m_point_count; _k++)
+	{
+		m_points[_k] = points[point_indices[_k]];
+	}
 }
 
 ///class btPrimitiveTriangle
 bool btPrimitiveTriangle::overlap_test_conservative(const btPrimitiveTriangle& other)
 {
-    btScalar total_margin = m_margin + other.m_margin;
-    // classify points on other triangle
-    btScalar dis0 = bt_distance_point_plane(m_plane,other.m_vertices[0]) - total_margin;
+	btScalar total_margin = m_margin + other.m_margin;
+	// classify points on other triangle
+	btScalar dis0 = bt_distance_point_plane(m_plane, other.m_vertices[0]) - total_margin;
 
-    btScalar dis1 = bt_distance_point_plane(m_plane,other.m_vertices[1]) - total_margin;
+	btScalar dis1 = bt_distance_point_plane(m_plane, other.m_vertices[1]) - total_margin;
 
-    btScalar dis2 = bt_distance_point_plane(m_plane,other.m_vertices[2]) - total_margin;
+	btScalar dis2 = bt_distance_point_plane(m_plane, other.m_vertices[2]) - total_margin;
 
-    if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+	if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false;
 
-    // classify points on this triangle
-    dis0 = bt_distance_point_plane(other.m_plane,m_vertices[0]) - total_margin;
+	// classify points on this triangle
+	dis0 = bt_distance_point_plane(other.m_plane, m_vertices[0]) - total_margin;
 
-    dis1 = bt_distance_point_plane(other.m_plane,m_vertices[1]) - total_margin;
+	dis1 = bt_distance_point_plane(other.m_plane, m_vertices[1]) - total_margin;
 
-    dis2 = bt_distance_point_plane(other.m_plane,m_vertices[2]) - total_margin;
+	dis2 = bt_distance_point_plane(other.m_plane, m_vertices[2]) - total_margin;
 
-    if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+	if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false;
 
-    return true;
+	return true;
 }
 
-int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points )
+int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle& other, btVector3* clipped_points)
 {
-    // edge 0
-
-    btVector3 temp_points[MAX_TRI_CLIPPING];
-
+	// edge 0
 
-    btVector4 edgeplane;
+	btVector3 temp_points[MAX_TRI_CLIPPING];
 
-    get_edge_plane(0,edgeplane);
+	btVector4 edgeplane;
 
+	get_edge_plane(0, edgeplane);
 
-    int clipped_count = bt_plane_clip_triangle(
-                            edgeplane,other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],temp_points);
+	int clipped_count = bt_plane_clip_triangle(
+		edgeplane, other.m_vertices[0], other.m_vertices[1], other.m_vertices[2], temp_points);
 
-    if (clipped_count == 0) return 0;
+	if (clipped_count == 0) return 0;
 
-    btVector3 temp_points1[MAX_TRI_CLIPPING];
+	btVector3 temp_points1[MAX_TRI_CLIPPING];
 
+	// edge 1
+	get_edge_plane(1, edgeplane);
 
-    // edge 1
-    get_edge_plane(1,edgeplane);
+	clipped_count = bt_plane_clip_polygon(edgeplane, temp_points, clipped_count, temp_points1);
 
+	if (clipped_count == 0) return 0;
 
-    clipped_count = bt_plane_clip_polygon(edgeplane,temp_points,clipped_count,temp_points1);
+	// edge 2
+	get_edge_plane(2, edgeplane);
 
-    if (clipped_count == 0) return 0;
+	clipped_count = bt_plane_clip_polygon(
+		edgeplane, temp_points1, clipped_count, clipped_points);
 
-    // edge 2
-    get_edge_plane(2,edgeplane);
-
-    clipped_count = bt_plane_clip_polygon(
-                        edgeplane,temp_points1,clipped_count,clipped_points);
-
-    return clipped_count;
+	return clipped_count;
 }
 
-bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts)
+bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle& other, GIM_TRIANGLE_CONTACT& contacts)
 {
-    btScalar margin = m_margin + other.m_margin;
-
-    btVector3 clipped_points[MAX_TRI_CLIPPING];
-    int clipped_count;
-    //create planes
-    // plane v vs U points
-
-    GIM_TRIANGLE_CONTACT contacts1;
-
-    contacts1.m_separating_normal = m_plane;
-
-
-    clipped_count = clip_triangle(other,clipped_points);
-
-    if (clipped_count == 0 )
-    {
-        return false;//Reject
-    }
-
-    //find most deep interval face1
-    contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
-    if (contacts1.m_point_count == 0) return false; // too far
-    //Normal pointing to this triangle
-    contacts1.m_separating_normal *= -1.f;
-
-
-    //Clip tri1 by tri2 edges
-    GIM_TRIANGLE_CONTACT contacts2;
-    contacts2.m_separating_normal = other.m_plane;
-
-    clipped_count = other.clip_triangle(*this,clipped_points);
-
-    if (clipped_count == 0 )
-    {
-        return false;//Reject
-    }
-
-    //find most deep interval face1
-    contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
-    if (contacts2.m_point_count == 0) return false; // too far
-
-
-
-
-    ////check most dir for contacts
-    if (contacts2.m_penetration_depth<contacts1.m_penetration_depth)
-    {
-        contacts.copy_from(contacts2);
-    }
-    else
-    {
-        contacts.copy_from(contacts1);
-    }
-    return true;
+	btScalar margin = m_margin + other.m_margin;
+
+	btVector3 clipped_points[MAX_TRI_CLIPPING];
+	int clipped_count;
+	//create planes
+	// plane v vs U points
+
+	GIM_TRIANGLE_CONTACT contacts1;
+
+	contacts1.m_separating_normal = m_plane;
+
+	clipped_count = clip_triangle(other, clipped_points);
+
+	if (clipped_count == 0)
+	{
+		return false;  //Reject
+	}
+
+	//find most deep interval face1
+	contacts1.merge_points(contacts1.m_separating_normal, margin, clipped_points, clipped_count);
+	if (contacts1.m_point_count == 0) return false;  // too far
+	//Normal pointing to this triangle
+	contacts1.m_separating_normal *= -1.f;
+
+	//Clip tri1 by tri2 edges
+	GIM_TRIANGLE_CONTACT contacts2;
+	contacts2.m_separating_normal = other.m_plane;
+
+	clipped_count = other.clip_triangle(*this, clipped_points);
+
+	if (clipped_count == 0)
+	{
+		return false;  //Reject
+	}
+
+	//find most deep interval face1
+	contacts2.merge_points(contacts2.m_separating_normal, margin, clipped_points, clipped_count);
+	if (contacts2.m_point_count == 0) return false;  // too far
+
+	////check most dir for contacts
+	if (contacts2.m_penetration_depth < contacts1.m_penetration_depth)
+	{
+		contacts.copy_from(contacts2);
+	}
+	else
+	{
+		contacts.copy_from(contacts1);
+	}
+	return true;
 }
 
-
-
 ///class btTriangleShapeEx: public btTriangleShape
 
 bool btTriangleShapeEx::overlap_test_conservative(const btTriangleShapeEx& other)
 {
-    btScalar total_margin = getMargin() + other.getMargin();
+	btScalar total_margin = getMargin() + other.getMargin();
 
-    btVector4 plane0;
-    buildTriPlane(plane0);
-    btVector4 plane1;
-    other.buildTriPlane(plane1);
+	btVector4 plane0;
+	buildTriPlane(plane0);
+	btVector4 plane1;
+	other.buildTriPlane(plane1);
 
-    // classify points on other triangle
-    btScalar dis0 = bt_distance_point_plane(plane0,other.m_vertices1[0]) - total_margin;
+	// classify points on other triangle
+	btScalar dis0 = bt_distance_point_plane(plane0, other.m_vertices1[0]) - total_margin;
 
-    btScalar dis1 = bt_distance_point_plane(plane0,other.m_vertices1[1]) - total_margin;
+	btScalar dis1 = bt_distance_point_plane(plane0, other.m_vertices1[1]) - total_margin;
 
-    btScalar dis2 = bt_distance_point_plane(plane0,other.m_vertices1[2]) - total_margin;
+	btScalar dis2 = bt_distance_point_plane(plane0, other.m_vertices1[2]) - total_margin;
 
-    if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+	if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false;
 
-    // classify points on this triangle
-    dis0 = bt_distance_point_plane(plane1,m_vertices1[0]) - total_margin;
+	// classify points on this triangle
+	dis0 = bt_distance_point_plane(plane1, m_vertices1[0]) - total_margin;
 
-    dis1 = bt_distance_point_plane(plane1,m_vertices1[1]) - total_margin;
+	dis1 = bt_distance_point_plane(plane1, m_vertices1[1]) - total_margin;
 
-    dis2 = bt_distance_point_plane(plane1,m_vertices1[2]) - total_margin;
+	dis2 = bt_distance_point_plane(plane1, m_vertices1[2]) - total_margin;
 
-    if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+	if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false;
 
-    return true;
+	return true;
 }
-
-
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h
index 973c2ed1277..568a1ce8116 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h
@@ -21,7 +21,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef GIMPACT_TRIANGLE_SHAPE_EX_H
 #define GIMPACT_TRIANGLE_SHAPE_EX_H
 
@@ -31,16 +30,15 @@ subject to the following restrictions:
 #include "btClipPolygon.h"
 #include "btGeometryOperations.h"
 
-
 #define MAX_TRI_CLIPPING 16
 
 //! Structure for collision
 struct GIM_TRIANGLE_CONTACT
 {
-    btScalar m_penetration_depth;
-    int m_point_count;
-    btVector4 m_separating_normal;
-    btVector3 m_points[MAX_TRI_CLIPPING];
+	btScalar m_penetration_depth;
+	int m_point_count;
+	btVector4 m_separating_normal;
+	btVector3 m_points[MAX_TRI_CLIPPING];
 
 	SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT& other)
 	{
@@ -48,7 +46,7 @@ struct GIM_TRIANGLE_CONTACT
 		m_separating_normal = other.m_separating_normal;
 		m_point_count = other.m_point_count;
 		int i = m_point_count;
-		while(i--)
+		while (i--)
 		{
 			m_points[i] = other.m_points[i];
 		}
@@ -63,14 +61,11 @@ struct GIM_TRIANGLE_CONTACT
 		copy_from(other);
 	}
 
-    //! classify points that are closer
-    void merge_points(const btVector4 & plane,
-    				btScalar margin, const btVector3 * points, int point_count);
-
+	//! classify points that are closer
+	void merge_points(const btVector4& plane,
+					  btScalar margin, const btVector3* points, int point_count);
 };
 
-
-
 class btPrimitiveTriangle
 {
 public:
@@ -78,17 +73,15 @@ public:
 	btVector4 m_plane;
 	btScalar m_margin;
 	btScalar m_dummy;
-	btPrimitiveTriangle():m_margin(0.01f)
+	btPrimitiveTriangle() : m_margin(0.01f)
 	{
-
 	}
 
-
 	SIMD_FORCE_INLINE void buildTriPlane()
 	{
-		btVector3 normal = (m_vertices[1]-m_vertices[0]).cross(m_vertices[2]-m_vertices[0]);
+		btVector3 normal = (m_vertices[1] - m_vertices[0]).cross(m_vertices[2] - m_vertices[0]);
 		normal.normalize();
-		m_plane.setValue(normal[0],normal[1],normal[2],m_vertices[0].dot(normal));
+		m_plane.setValue(normal[0], normal[1], normal[2], m_vertices[0].dot(normal));
 	}
 
 	//! Test if triangles could collide
@@ -98,14 +91,14 @@ public:
 	/*!
 	\pre this triangle must have its plane calculated.
 	*/
-	SIMD_FORCE_INLINE void get_edge_plane(int edge_index, btVector4 &plane)  const
-    {
-		const btVector3 & e0 = m_vertices[edge_index];
-		const btVector3 & e1 = m_vertices[(edge_index+1)%3];
-		bt_edge_plane(e0,e1,m_plane,plane);
-    }
-
-    void applyTransform(const btTransform& t)
+	SIMD_FORCE_INLINE void get_edge_plane(int edge_index, btVector4& plane) const
+	{
+		const btVector3& e0 = m_vertices[edge_index];
+		const btVector3& e1 = m_vertices[(edge_index + 1) % 3];
+		bt_edge_plane(e0, e1, m_plane, plane);
+	}
+
+	void applyTransform(const btTransform& t)
 	{
 		m_vertices[0] = t(m_vertices[0]);
 		m_vertices[1] = t(m_vertices[1]);
@@ -117,44 +110,41 @@ public:
 	\pre clipped_points must have MAX_TRI_CLIPPING size, and this triangle must have its plane calculated.
 	\return the number of clipped points
 	*/
-    int clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points );
+	int clip_triangle(btPrimitiveTriangle& other, btVector3* clipped_points);
 
 	//! Find collision using the clipping method
 	/*!
 	\pre this triangle and other must have their triangles calculated
 	*/
-    bool find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts);
+	bool find_triangle_collision_clip_method(btPrimitiveTriangle& other, GIM_TRIANGLE_CONTACT& contacts);
 };
 
-
-
 //! Helper class for colliding Bullet Triangle Shapes
 /*!
 This class implements a better getAabb method than the previous btTriangleShape class
 */
-class btTriangleShapeEx: public btTriangleShape
+class btTriangleShapeEx : public btTriangleShape
 {
 public:
-
-	btTriangleShapeEx():btTriangleShape(btVector3(0,0,0),btVector3(0,0,0),btVector3(0,0,0))
+	btTriangleShapeEx() : btTriangleShape(btVector3(0, 0, 0), btVector3(0, 0, 0), btVector3(0, 0, 0))
 	{
 	}
 
-	btTriangleShapeEx(const btVector3& p0,const btVector3& p1,const btVector3& p2):	btTriangleShape(p0,p1,p2)
+	btTriangleShapeEx(const btVector3& p0, const btVector3& p1, const btVector3& p2) : btTriangleShape(p0, p1, p2)
 	{
 	}
 
-	btTriangleShapeEx(const btTriangleShapeEx & other):	btTriangleShape(other.m_vertices1[0],other.m_vertices1[1],other.m_vertices1[2])
+	btTriangleShapeEx(const btTriangleShapeEx& other) : btTriangleShape(other.m_vertices1[0], other.m_vertices1[1], other.m_vertices1[2])
 	{
 	}
 
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 	{
 		btVector3 tv0 = t(m_vertices1[0]);
 		btVector3 tv1 = t(m_vertices1[1]);
 		btVector3 tv2 = t(m_vertices1[2]);
 
-		btAABB trianglebox(tv0,tv1,tv2,m_collisionMargin);
+		btAABB trianglebox(tv0, tv1, tv2, m_collisionMargin);
 		aabbMin = trianglebox.m_min;
 		aabbMax = trianglebox.m_max;
 	}
@@ -166,15 +156,14 @@ public:
 		m_vertices1[2] = t(m_vertices1[2]);
 	}
 
-	SIMD_FORCE_INLINE void buildTriPlane(btVector4 & plane) const
+	SIMD_FORCE_INLINE void buildTriPlane(btVector4& plane) const
 	{
-		btVector3 normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]);
+		btVector3 normal = (m_vertices1[1] - m_vertices1[0]).cross(m_vertices1[2] - m_vertices1[0]);
 		normal.normalize();
-		plane.setValue(normal[0],normal[1],normal[2],m_vertices1[0].dot(normal));
+		plane.setValue(normal[0], normal[1], normal[2], m_vertices1[0].dot(normal));
 	}
 
 	bool overlap_test_conservative(const btTriangleShapeEx& other);
 };
 
-
-#endif //GIMPACT_TRIANGLE_MESH_SHAPE_H
+#endif  //GIMPACT_TRIANGLE_MESH_SHAPE_H
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h
index 27e6f32fc8b..fc2dc38a330 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h
@@ -34,47 +34,46 @@ email: projectileman@yahoo.com
 
 #include "gim_memory.h"
 
-
 #define GIM_ARRAY_GROW_INCREMENT 2
 #define GIM_ARRAY_GROW_FACTOR 2
 
 //!	Very simple array container with fast access and simd memory
-template<typename T>
+template <typename T>
 class gim_array
 {
 public:
-//! properties
-//!@{
-    T *m_data;
-    GUINT m_size;
-    GUINT m_allocated_size;
-//!@}
-//! protected operations
-//!@{
-
-    inline void destroyData()
-	{
-	    m_allocated_size = 0;
-		if(m_data==NULL) return;
+	//! properties
+	//!@{
+	T* m_data;
+	GUINT m_size;
+	GUINT m_allocated_size;
+	//!@}
+	//! protected operations
+	//!@{
+
+	inline void destroyData()
+	{
+		m_allocated_size = 0;
+		if (m_data == NULL) return;
 		gim_free(m_data);
 		m_data = NULL;
 	}
 
 	inline bool resizeData(GUINT newsize)
 	{
-		if(newsize==0)
+		if (newsize == 0)
 		{
 			destroyData();
 			return true;
 		}
 
-		if(m_size>0)
+		if (m_size > 0)
 		{
-            m_data = (T*)gim_realloc(m_data,m_size*sizeof(T),newsize*sizeof(T));
+			m_data = (T*)gim_realloc(m_data, m_size * sizeof(T), newsize * sizeof(T));
 		}
 		else
 		{
-		    m_data = (T*)gim_alloc(newsize*sizeof(T));
+			m_data = (T*)gim_alloc(newsize * sizeof(T));
 		}
 		m_allocated_size = newsize;
 		return true;
@@ -82,243 +81,238 @@ public:
 
 	inline bool growingCheck()
 	{
-		if(m_allocated_size<=m_size)
+		if (m_allocated_size <= m_size)
 		{
-		    GUINT requestsize = m_size;
-		    m_size = m_allocated_size;
-			if(resizeData((requestsize+GIM_ARRAY_GROW_INCREMENT)*GIM_ARRAY_GROW_FACTOR)==false) return false;
+			GUINT requestsize = m_size;
+			m_size = m_allocated_size;
+			if (resizeData((requestsize + GIM_ARRAY_GROW_INCREMENT) * GIM_ARRAY_GROW_FACTOR) == false) return false;
 		}
 		return true;
 	}
 
-//!@}
-//! public operations
-//!@{
-    inline  bool reserve(GUINT size)
-    {
-        if(m_allocated_size>=size) return false;
-        return resizeData(size);
-    }
-
-    inline void clear_range(GUINT start_range)
-    {
-        while(m_size>start_range)
-        {
-            m_data[--m_size].~T();
-        }
-    }
-
-    inline void clear()
-    {
-        if(m_size==0)return;
-        clear_range(0);
-    }
-
-    inline void clear_memory()
-    {
-        clear();
-        destroyData();
-    }
-
-    gim_array()
-    {
-        m_data = 0;
-        m_size = 0;
-        m_allocated_size = 0;
-    }
-
-    gim_array(GUINT reservesize)
-    {
-        m_data = 0;
-        m_size = 0;
-
-        m_allocated_size = 0;
-        reserve(reservesize);
-    }
-
-    ~gim_array()
-    {
-        clear_memory();
-    }
-
-    inline GUINT size() const
-    {
-        return m_size;
-    }
-
-    inline GUINT max_size() const
-    {
-        return m_allocated_size;
-    }
-
-    inline T & operator[](size_t i)
+	//!@}
+	//! public operations
+	//!@{
+	inline bool reserve(GUINT size)
+	{
+		if (m_allocated_size >= size) return false;
+		return resizeData(size);
+	}
+
+	inline void clear_range(GUINT start_range)
+	{
+		while (m_size > start_range)
+		{
+			m_data[--m_size].~T();
+		}
+	}
+
+	inline void clear()
+	{
+		if (m_size == 0) return;
+		clear_range(0);
+	}
+
+	inline void clear_memory()
+	{
+		clear();
+		destroyData();
+	}
+
+	gim_array()
+	{
+		m_data = 0;
+		m_size = 0;
+		m_allocated_size = 0;
+	}
+
+	gim_array(GUINT reservesize)
+	{
+		m_data = 0;
+		m_size = 0;
+
+		m_allocated_size = 0;
+		reserve(reservesize);
+	}
+
+	~gim_array()
+	{
+		clear_memory();
+	}
+
+	inline GUINT size() const
+	{
+		return m_size;
+	}
+
+	inline GUINT max_size() const
+	{
+		return m_allocated_size;
+	}
+
+	inline T& operator[](size_t i)
 	{
 		return m_data[i];
 	}
-	inline  const T & operator[](size_t i) const
+	inline const T& operator[](size_t i) const
 	{
 		return m_data[i];
 	}
 
-    inline T * pointer(){ return m_data;}
-    inline const T * pointer() const
-    { return m_data;}
-
+	inline T* pointer() { return m_data; }
+	inline const T* pointer() const
+	{
+		return m_data;
+	}
 
-    inline T * get_pointer_at(GUINT i)
+	inline T* get_pointer_at(GUINT i)
 	{
 		return m_data + i;
 	}
 
-	inline const T * get_pointer_at(GUINT i) const
+	inline const T* get_pointer_at(GUINT i) const
 	{
 		return m_data + i;
 	}
 
-	inline T & at(GUINT i)
+	inline T& at(GUINT i)
 	{
 		return m_data[i];
 	}
 
-	inline const T & at(GUINT i) const
+	inline const T& at(GUINT i) const
 	{
 		return m_data[i];
 	}
 
-	inline T & front()
+	inline T& front()
 	{
 		return *m_data;
 	}
 
-	inline const T & front() const
+	inline const T& front() const
 	{
 		return *m_data;
 	}
 
-	inline T & back()
+	inline T& back()
 	{
-		return m_data[m_size-1];
+		return m_data[m_size - 1];
 	}
 
-	inline const T & back() const
+	inline const T& back() const
 	{
-		return m_data[m_size-1];
+		return m_data[m_size - 1];
 	}
 
-
 	inline void swap(GUINT i, GUINT j)
 	{
-	    gim_swap_elements(m_data,i,j);
+		gim_swap_elements(m_data, i, j);
 	}
 
-	inline void push_back(const T & obj)
+	inline void push_back(const T& obj)
 	{
-	    this->growingCheck();
-	    m_data[m_size] = obj;
-	    m_size++;
+		this->growingCheck();
+		m_data[m_size] = obj;
+		m_size++;
 	}
 
 	//!Simply increase the m_size, doesn't call the new element constructor
 	inline void push_back_mem()
 	{
-	    this->growingCheck();
-	    m_size++;
+		this->growingCheck();
+		m_size++;
 	}
 
-	inline void push_back_memcpy(const T & obj)
+	inline void push_back_memcpy(const T& obj)
 	{
-	    this->growingCheck();
-	    irr_simd_memcpy(&m_data[m_size],&obj,sizeof(T));
-	    m_size++;
+		this->growingCheck();
+		gim_simd_memcpy(&m_data[m_size], &obj, sizeof(T));
+		m_size++;
 	}
 
 	inline void pop_back()
 	{
-	    m_size--;
-        m_data[m_size].~T();
+		m_size--;
+		m_data[m_size].~T();
 	}
 
 	//!Simply decrease the m_size, doesn't call the deleted element destructor
 	inline void pop_back_mem()
 	{
-	    m_size--;
+		m_size--;
 	}
 
-    //! fast erase
+	//! fast erase
 	inline void erase(GUINT index)
 	{
-	    if(index<m_size-1)
-	    {
-	        swap(index,m_size-1);
-	    }
-	    pop_back();
+		if (index < m_size - 1)
+		{
+			swap(index, m_size - 1);
+		}
+		pop_back();
 	}
 
 	inline void erase_sorted_mem(GUINT index)
 	{
-	    m_size--;
-	    for(GUINT i = index;i<m_size;i++)
-	    {
-	        gim_simd_memcpy(m_data+i,m_data+i+1,sizeof(T));
-	    }
+		m_size--;
+		for (GUINT i = index; i < m_size; i++)
+		{
+			gim_simd_memcpy(m_data + i, m_data + i + 1, sizeof(T));
+		}
 	}
 
 	inline void erase_sorted(GUINT index)
 	{
-	    m_data[index].~T();
-	    erase_sorted_mem(index);
+		m_data[index].~T();
+		erase_sorted_mem(index);
 	}
 
 	inline void insert_mem(GUINT index)
 	{
-	    this->growingCheck();
-	    for(GUINT i = m_size;i>index;i--)
-	    {
-	        gim_simd_memcpy(m_data+i,m_data+i-1,sizeof(T));
-	    }
-	    m_size++;
-	}
-
-	inline void insert(const T & obj,GUINT index)
-	{
-	    insert_mem(index);
-	    m_data[index] = obj;
-	}
-
-	inline void resize(GUINT size, bool call_constructor = true, const T& fillData=T())
-	{
-	    if(size>m_size)
-	    {
-            reserve(size);
-            if(call_constructor)
-            {
-                while(m_size<size)
-                {
-                    m_data[m_size] = fillData;
-                    m_size++;
-                }
-            }
-            else
-            {
-            	m_size = size;
-            }
-	    }
-	    else if(size<m_size)
-	    {
-	        if(call_constructor) clear_range(size);
-	        m_size = size;
-	    }
+		this->growingCheck();
+		for (GUINT i = m_size; i > index; i--)
+		{
+			gim_simd_memcpy(m_data + i, m_data + i - 1, sizeof(T));
+		}
+		m_size++;
 	}
 
-	inline void refit()
+	inline void insert(const T& obj, GUINT index)
 	{
-	    resizeData(m_size);
+		insert_mem(index);
+		m_data[index] = obj;
 	}
 
-};
-
-
-
+	inline void resize(GUINT size, bool call_constructor = true, const T& fillData = T())
+	{
+		if (size > m_size)
+		{
+			reserve(size);
+			if (call_constructor)
+			{
+				while (m_size < size)
+				{
+					m_data[m_size] = fillData;
+					m_size++;
+				}
+			}
+			else
+			{
+				m_size = size;
+			}
+		}
+		else if (size < m_size)
+		{
+			if (call_constructor) clear_range(size);
+			m_size = size;
+		}
+	}
 
+	inline void refit()
+	{
+		resizeData(m_size);
+	}
+};
 
-#endif // GIM_CONTAINERS_H_INCLUDED
+#endif  // GIM_CONTAINERS_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h
index d98051da3d6..7ab783672d6 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h
@@ -35,88 +35,92 @@ email: projectileman@yahoo.com
 -----------------------------------------------------------------------------
 */
 
-
 #include "gim_linear_math.h"
 
-
-
-
-
+#ifndef PLANEDIREPSILON
 #define PLANEDIREPSILON 0.0000001f
-#define PARALELENORMALS 0.000001f
-
+#endif
 
-#define TRIANGLE_NORMAL(v1,v2,v3,n)\
-{\
-	vec3f _dif1,_dif2;\
-    VEC_DIFF(_dif1,v2,v1);\
-    VEC_DIFF(_dif2,v3,v1);\
-    VEC_CROSS(n,_dif1,_dif2);\
-    VEC_NORMALIZE(n);\
-}\
+#ifndef PARALELENORMALS
+#define PARALELENORMALS 0.000001f
+#endif
+
+#define TRIANGLE_NORMAL(v1, v2, v3, n) \
+	{                                  \
+		vec3f _dif1, _dif2;            \
+		VEC_DIFF(_dif1, v2, v1);       \
+		VEC_DIFF(_dif2, v3, v1);       \
+		VEC_CROSS(n, _dif1, _dif2);    \
+		VEC_NORMALIZE(n);              \
+	}
 
-#define TRIANGLE_NORMAL_FAST(v1,v2,v3,n){\
-    vec3f _dif1,_dif2; \
-    VEC_DIFF(_dif1,v2,v1); \
-    VEC_DIFF(_dif2,v3,v1); \
-    VEC_CROSS(n,_dif1,_dif2); \
-}\
+#define TRIANGLE_NORMAL_FAST(v1, v2, v3, n) \
+	{                                       \
+		vec3f _dif1, _dif2;                 \
+		VEC_DIFF(_dif1, v2, v1);            \
+		VEC_DIFF(_dif2, v3, v1);            \
+		VEC_CROSS(n, _dif1, _dif2);         \
+	}
 
 /// plane is a vec4f
-#define TRIANGLE_PLANE(v1,v2,v3,plane) {\
-    TRIANGLE_NORMAL(v1,v2,v3,plane);\
-    plane[3] = VEC_DOT(v1,plane);\
-}\
+#define TRIANGLE_PLANE(v1, v2, v3, plane)   \
+	{                                       \
+		TRIANGLE_NORMAL(v1, v2, v3, plane); \
+		plane[3] = VEC_DOT(v1, plane);      \
+	}
 
 /// plane is a vec4f
-#define TRIANGLE_PLANE_FAST(v1,v2,v3,plane) {\
-    TRIANGLE_NORMAL_FAST(v1,v2,v3,plane);\
-    plane[3] = VEC_DOT(v1,plane);\
-}\
+#define TRIANGLE_PLANE_FAST(v1, v2, v3, plane)   \
+	{                                            \
+		TRIANGLE_NORMAL_FAST(v1, v2, v3, plane); \
+		plane[3] = VEC_DOT(v1, plane);           \
+	}
 
 /// Calc a plane from an edge an a normal. plane is a vec4f
-#define EDGE_PLANE(e1,e2,n,plane) {\
-    vec3f _dif; \
-    VEC_DIFF(_dif,e2,e1); \
-    VEC_CROSS(plane,_dif,n); \
-    VEC_NORMALIZE(plane); \
-    plane[3] = VEC_DOT(e1,plane);\
-}\
-
-#define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3])
-
-#define PROJECT_POINT_PLANE(point,plane,projected) {\
-	GREAL _dis;\
-	_dis = DISTANCE_PLANE_POINT(plane,point);\
-	VEC_SCALE(projected,-_dis,plane);\
-	VEC_SUM(projected,projected,point);	\
-}\
+#define EDGE_PLANE(e1, e2, n, plane)   \
+	{                                  \
+		vec3f _dif;                    \
+		VEC_DIFF(_dif, e2, e1);        \
+		VEC_CROSS(plane, _dif, n);     \
+		VEC_NORMALIZE(plane);          \
+		plane[3] = VEC_DOT(e1, plane); \
+	}
+
+#define DISTANCE_PLANE_POINT(plane, point) (VEC_DOT(plane, point) - plane[3])
+
+#define PROJECT_POINT_PLANE(point, plane, projected) \
+	{                                                \
+		GREAL _dis;                                  \
+		_dis = DISTANCE_PLANE_POINT(plane, point);   \
+		VEC_SCALE(projected, -_dis, plane);          \
+		VEC_SUM(projected, projected, point);        \
+	}
 
 //! Verifies if a point is in the plane hull
-template<typename CLASS_POINT,typename CLASS_PLANE>
+template <typename CLASS_POINT, typename CLASS_PLANE>
 SIMD_FORCE_INLINE bool POINT_IN_HULL(
-	const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count)
+	const CLASS_POINT &point, const CLASS_PLANE *planes, GUINT plane_count)
 {
 	GREAL _dis;
-	for (GUINT _i = 0;_i< plane_count;++_i)
+	for (GUINT _i = 0; _i < plane_count; ++_i)
 	{
-		_dis = DISTANCE_PLANE_POINT(planes[_i],point);
-	    if(_dis>0.0f) return false;
+		_dis = DISTANCE_PLANE_POINT(planes[_i], point);
+		if (_dis > 0.0f) return false;
 	}
 	return true;
 }
 
-template<typename CLASS_POINT,typename CLASS_PLANE>
+template <typename CLASS_POINT, typename CLASS_PLANE>
 SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT(
-	const CLASS_POINT& s1,
-	const CLASS_POINT &s2,const CLASS_PLANE &plane,CLASS_POINT &clipped)
+	const CLASS_POINT &s1,
+	const CLASS_POINT &s2, const CLASS_PLANE &plane, CLASS_POINT &clipped)
 {
-	GREAL _dis1,_dis2;
-	_dis1 = DISTANCE_PLANE_POINT(plane,s1);
-	VEC_DIFF(clipped,s2,s1);
-	_dis2 = VEC_DOT(clipped,plane);
-	VEC_SCALE(clipped,-_dis1/_dis2,clipped);
-	VEC_SUM(clipped,clipped,s1);
+	GREAL _dis1, _dis2;
+	_dis1 = DISTANCE_PLANE_POINT(plane, s1);
+	VEC_DIFF(clipped, s2, s1);
+	_dis2 = VEC_DOT(clipped, plane);
+	VEC_SCALE(clipped, -_dis1 / _dis2, clipped);
+	VEC_SUM(clipped, clipped, s1);
 }
 
 enum ePLANE_INTERSECTION_TYPE
@@ -149,30 +153,30 @@ intersection type must have the following values
 </ul>
 */
 
-template<typename CLASS_POINT,typename CLASS_PLANE>
+template <typename CLASS_POINT, typename CLASS_PLANE>
 SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2(
-	const CLASS_POINT& s1,
+	const CLASS_POINT &s1,
 	const CLASS_POINT &s2,
-	const CLASS_PLANE &plane,CLASS_POINT &clipped)
+	const CLASS_PLANE &plane, CLASS_POINT &clipped)
 {
-	GREAL _dis1 = DISTANCE_PLANE_POINT(plane,s1);
-	GREAL _dis2 = DISTANCE_PLANE_POINT(plane,s2);
-	if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON)
+	GREAL _dis1 = DISTANCE_PLANE_POINT(plane, s1);
+	GREAL _dis2 = DISTANCE_PLANE_POINT(plane, s2);
+	if (_dis1 > -G_EPSILON && _dis2 > -G_EPSILON)
 	{
-	    if(_dis1<_dis2) return G_FRONT_PLANE_S1;
-	    return G_FRONT_PLANE_S2;
+		if (_dis1 < _dis2) return G_FRONT_PLANE_S1;
+		return G_FRONT_PLANE_S2;
 	}
-	else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON)
+	else if (_dis1 < G_EPSILON && _dis2 < G_EPSILON)
 	{
-	    if(_dis1>_dis2) return G_BACK_PLANE_S1;
-	    return G_BACK_PLANE_S2;
+		if (_dis1 > _dis2) return G_BACK_PLANE_S1;
+		return G_BACK_PLANE_S2;
 	}
 
-	VEC_DIFF(clipped,s2,s1);
-	_dis2 = VEC_DOT(clipped,plane);
-	VEC_SCALE(clipped,-_dis1/_dis2,clipped);
-	VEC_SUM(clipped,clipped,s1);
-	if(_dis1<_dis2) return G_COLLIDE_PLANE_S1;
+	VEC_DIFF(clipped, s2, s1);
+	_dis2 = VEC_DOT(clipped, plane);
+	VEC_SCALE(clipped, -_dis1 / _dis2, clipped);
+	VEC_SUM(clipped, clipped, s1);
+	if (_dis1 < _dis2) return G_COLLIDE_PLANE_S1;
 	return G_COLLIDE_PLANE_S2;
 }
 
@@ -191,43 +195,42 @@ intersection_type must have the following values
 <li> 5 : Segment collides plane, s2 in back
 </ul>
 */
-template<typename CLASS_POINT,typename CLASS_PLANE>
+template <typename CLASS_POINT, typename CLASS_PLANE>
 SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST(
-	const CLASS_POINT& s1,
+	const CLASS_POINT &s1,
 	const CLASS_POINT &s2,
 	const CLASS_PLANE &plane,
-	CLASS_POINT &clipped1,CLASS_POINT &clipped2)
+	CLASS_POINT &clipped1, CLASS_POINT &clipped2)
 {
-	eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1);
-	switch(intersection_type)
+	eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1, s2, plane, clipped1);
+	switch (intersection_type)
 	{
-	case G_FRONT_PLANE_S1:
-		VEC_COPY(clipped1,s1);
-	    VEC_COPY(clipped2,s2);
-		break;
-	case G_FRONT_PLANE_S2:
-		VEC_COPY(clipped1,s2);
-	    VEC_COPY(clipped2,s1);
-		break;
-	case G_BACK_PLANE_S1:
-		VEC_COPY(clipped1,s1);
-	    VEC_COPY(clipped2,s2);
-		break;
-	case G_BACK_PLANE_S2:
-		VEC_COPY(clipped1,s2);
-	    VEC_COPY(clipped2,s1);
-		break;
-	case G_COLLIDE_PLANE_S1:
-		VEC_COPY(clipped2,s1);
-		break;
-	case G_COLLIDE_PLANE_S2:
-		VEC_COPY(clipped2,s2);
-		break;
+		case G_FRONT_PLANE_S1:
+			VEC_COPY(clipped1, s1);
+			VEC_COPY(clipped2, s2);
+			break;
+		case G_FRONT_PLANE_S2:
+			VEC_COPY(clipped1, s2);
+			VEC_COPY(clipped2, s1);
+			break;
+		case G_BACK_PLANE_S1:
+			VEC_COPY(clipped1, s1);
+			VEC_COPY(clipped2, s2);
+			break;
+		case G_BACK_PLANE_S2:
+			VEC_COPY(clipped1, s2);
+			VEC_COPY(clipped2, s1);
+			break;
+		case G_COLLIDE_PLANE_S1:
+			VEC_COPY(clipped2, s1);
+			break;
+		case G_COLLIDE_PLANE_S2:
+			VEC_COPY(clipped2, s2);
+			break;
 	}
 	return intersection_type;
 }
 
-
 //! Finds the 2 smallest cartesian coordinates of a plane normal
 #define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1)
 
@@ -236,23 +239,23 @@ SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST(
 Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).<br/>
 It uses the PLANEDIREPSILON constant.
 */
-template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
+template <typename T, typename CLASS_POINT, typename CLASS_PLANE>
 SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION(
-	const CLASS_PLANE & plane,
-	const CLASS_POINT & vDir,
-	const CLASS_POINT & vPoint,
-	CLASS_POINT & pout,T &tparam)
+	const CLASS_PLANE &plane,
+	const CLASS_POINT &vDir,
+	const CLASS_POINT &vPoint,
+	CLASS_POINT &pout, T &tparam)
 {
-	GREAL _dis,_dotdir;
-	_dotdir = VEC_DOT(plane,vDir);
-	if(_dotdir<PLANEDIREPSILON)
+	GREAL _dis, _dotdir;
+	_dotdir = VEC_DOT(plane, vDir);
+	if (_dotdir < PLANEDIREPSILON)
 	{
-	    return false;
+		return false;
 	}
-	_dis = DISTANCE_PLANE_POINT(plane,vPoint);
-	tparam = -_dis/_dotdir;
-	VEC_SCALE(pout,tparam,vDir);
-	VEC_SUM(pout,vPoint,pout);
+	_dis = DISTANCE_PLANE_POINT(plane, vPoint);
+	tparam = -_dis / _dotdir;
+	VEC_SCALE(pout, tparam, vDir);
+	VEC_SUM(pout, vPoint, pout);
 	return true;
 }
 
@@ -263,39 +266,39 @@ SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION(
 	-1 if the ray collides in front
 	-2 if the ray collides in back
 */
-template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
+template <typename T, typename CLASS_POINT, typename CLASS_PLANE>
 SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION(
-	const CLASS_PLANE & plane,
-	const CLASS_POINT & vDir,
-	const CLASS_POINT & vPoint,
-	CLASS_POINT & pout,
+	const CLASS_PLANE &plane,
+	const CLASS_POINT &vDir,
+	const CLASS_POINT &vPoint,
+	CLASS_POINT &pout,
 	T &tparam,
 	T tmin, T tmax)
 {
-	GREAL _dis,_dotdir;
-	_dotdir = VEC_DOT(plane,vDir);
-	if(btFabs(_dotdir)<PLANEDIREPSILON)
+	GREAL _dis, _dotdir;
+	_dotdir = VEC_DOT(plane, vDir);
+	if (btFabs(_dotdir) < PLANEDIREPSILON)
 	{
 		tparam = tmax;
-	    return 0;
+		return 0;
 	}
-	_dis = DISTANCE_PLANE_POINT(plane,vPoint);
-	char returnvalue = _dis<0.0f?2:1;
-	tparam = -_dis/_dotdir;
+	_dis = DISTANCE_PLANE_POINT(plane, vPoint);
+	char returnvalue = _dis < 0.0f ? 2 : 1;
+	tparam = -_dis / _dotdir;
 
-	if(tparam<tmin)
+	if (tparam < tmin)
 	{
 		returnvalue = 0;
 		tparam = tmin;
 	}
-	else if(tparam>tmax)
+	else if (tparam > tmax)
 	{
 		returnvalue = 0;
 		tparam = tmax;
 	}
 
-	VEC_SCALE(pout,tparam,vDir);
-	VEC_SUM(pout,vPoint,pout);
+	VEC_SCALE(pout, tparam, vDir);
+	VEC_SUM(pout, vPoint, pout);
 	return returnvalue;
 }
 
@@ -309,24 +312,24 @@ SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION(
   \return true if the planes intersect, 0 if paralell.
 
 */
-template<typename CLASS_POINT,typename CLASS_PLANE>
+template <typename CLASS_POINT, typename CLASS_PLANE>
 SIMD_FORCE_INLINE bool INTERSECT_PLANES(
-		const CLASS_PLANE &p1,
-		const CLASS_PLANE &p2,
-		CLASS_POINT &p,
-		CLASS_POINT &d)
+	const CLASS_PLANE &p1,
+	const CLASS_PLANE &p2,
+	CLASS_POINT &p,
+	CLASS_POINT &d)
 {
-	VEC_CROSS(d,p1,p2);
-  	GREAL denom = VEC_DOT(d, d);
-  	if(GIM_IS_ZERO(denom)) return false;
+	VEC_CROSS(d, p1, p2);
+	GREAL denom = VEC_DOT(d, d);
+	if (GIM_IS_ZERO(denom)) return false;
 	vec3f _n;
-	_n[0]=p1[3]*p2[0] - p2[3]*p1[0];
-	_n[1]=p1[3]*p2[1] - p2[3]*p1[1];
-	_n[2]=p1[3]*p2[2] - p2[3]*p1[2];
-	VEC_CROSS(p,_n,d);
-	p[0]/=denom;
-	p[1]/=denom;
-	p[2]/=denom;
+	_n[0] = p1[3] * p2[0] - p2[3] * p1[0];
+	_n[1] = p1[3] * p2[1] - p2[3] * p1[1];
+	_n[2] = p1[3] * p2[2] - p2[3] * p1[2];
+	VEC_CROSS(p, _n, d);
+	p[0] /= denom;
+	p[1] /= denom;
+	p[2] /= denom;
 	return true;
 }
 
@@ -334,32 +337,31 @@ SIMD_FORCE_INLINE bool INTERSECT_PLANES(
 
 /*! Finds the closest point(cp) to (v) on a segment (e1,e2)
  */
-template<typename CLASS_POINT>
+template <typename CLASS_POINT>
 SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT(
-	CLASS_POINT & cp, const CLASS_POINT & v,
-	const CLASS_POINT &e1,const CLASS_POINT &e2)
+	CLASS_POINT &cp, const CLASS_POINT &v,
+	const CLASS_POINT &e1, const CLASS_POINT &e2)
 {
-    vec3f _n;
-    VEC_DIFF(_n,e2,e1);
-    VEC_DIFF(cp,v,e1);
+	vec3f _n;
+	VEC_DIFF(_n, e2, e1);
+	VEC_DIFF(cp, v, e1);
 	GREAL _scalar = VEC_DOT(cp, _n);
-	_scalar/= VEC_DOT(_n, _n);
-	if(_scalar <0.0f)
+	_scalar /= VEC_DOT(_n, _n);
+	if (_scalar < 0.0f)
 	{
-	    VEC_COPY(cp,e1);
+		VEC_COPY(cp, e1);
 	}
-	else if(_scalar >1.0f)
+	else if (_scalar > 1.0f)
 	{
-	    VEC_COPY(cp,e2);
+		VEC_COPY(cp, e2);
 	}
 	else
 	{
-        VEC_SCALE(cp,_scalar,_n);
-        VEC_SUM(cp,cp,e1);
+		VEC_SCALE(cp, _scalar, _n);
+		VEC_SUM(cp, cp, e1);
 	}
 }
 
-
 /*! \brief Finds the line params where these lines intersect.
 
 \param dir1 Direction of line 1
@@ -371,117 +373,113 @@ SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT(
 \param dointersect  0  if the lines won't intersect, else 1
 
 */
-template<typename T,typename CLASS_POINT>
+template <typename T, typename CLASS_POINT>
 SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS(
-	const CLASS_POINT & dir1,
-	CLASS_POINT & point1,
-	const CLASS_POINT & dir2,
-	CLASS_POINT &  point2,
-	T& t1,T& t2)
+	const CLASS_POINT &dir1,
+	CLASS_POINT &point1,
+	const CLASS_POINT &dir2,
+	CLASS_POINT &point2,
+	T &t1, T &t2)
 {
-    GREAL det;
-	GREAL e1e1 = VEC_DOT(dir1,dir1);
-	GREAL e1e2 = VEC_DOT(dir1,dir2);
-	GREAL e2e2 = VEC_DOT(dir2,dir2);
+	GREAL det;
+	GREAL e1e1 = VEC_DOT(dir1, dir1);
+	GREAL e1e2 = VEC_DOT(dir1, dir2);
+	GREAL e2e2 = VEC_DOT(dir2, dir2);
 	vec3f p1p2;
-    VEC_DIFF(p1p2,point1,point2);
-    GREAL p1p2e1 = VEC_DOT(p1p2,dir1);
-	GREAL p1p2e2 = VEC_DOT(p1p2,dir2);
-	det = e1e2*e1e2 - e1e1*e2e2;
-	if(GIM_IS_ZERO(det)) return false;
-	t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det;
-	t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det;
+	VEC_DIFF(p1p2, point1, point2);
+	GREAL p1p2e1 = VEC_DOT(p1p2, dir1);
+	GREAL p1p2e2 = VEC_DOT(p1p2, dir2);
+	det = e1e2 * e1e2 - e1e1 * e2e2;
+	if (GIM_IS_ZERO(det)) return false;
+	t1 = (e1e2 * p1p2e2 - e2e2 * p1p2e1) / det;
+	t2 = (e1e1 * p1p2e2 - e1e2 * p1p2e1) / det;
 	return true;
 }
 
 //! Find closest points on segments
-template<typename CLASS_POINT>
+template <typename CLASS_POINT>
 SIMD_FORCE_INLINE void SEGMENT_COLLISION(
-	const CLASS_POINT & vA1,
-	const CLASS_POINT & vA2,
-	const CLASS_POINT & vB1,
-	const CLASS_POINT & vB2,
-	CLASS_POINT & vPointA,
-	CLASS_POINT & vPointB)
+	const CLASS_POINT &vA1,
+	const CLASS_POINT &vA2,
+	const CLASS_POINT &vB1,
+	const CLASS_POINT &vB2,
+	CLASS_POINT &vPointA,
+	CLASS_POINT &vPointB)
 {
-    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);
-    if(_tp<G_EPSILON)//ARE PARALELE
-    {
-    	//project B over A
-    	bool invert_b_order = false;
-    	_M[0] = VEC_DOT(vB1,_AD);
-    	_M[1] = VEC_DOT(vB2,_AD);
-    	if(_M[0]>_M[1])
-    	{
-    		invert_b_order  = true;
-    		GIM_SWAP_NUMBERS(_M[0],_M[1]);
-    	}
-    	_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;
-
-    	if(n[0]<n[1])
-    	{
-    		if(_M[1]<_M[2])
-    		{
-    			vPointB = invert_b_order?vB1:vB2;
-    			vPointA = vA1;
-    		}
-    		else if(_M[1]<_M[3])
-    		{
-    			vPointB = invert_b_order?vB1:vB2;
-    			CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
-    		}
-    		else
-    		{
-    			vPointA = vA2;
-    			CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
-    		}
-    	}
-    	else
-    	{
-    		if(_M[3]<_M[0])
-    		{
-    			vPointB = invert_b_order?vB2:vB1;
-    			vPointA = vA2;
-    		}
-    		else if(_M[3]<_M[1])
-    		{
-    			vPointA = vA2;
-    			CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
-    		}
-    		else
-    		{
-    			vPointB = invert_b_order?vB1:vB2;
-    			CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
-    		}
-    	}
-    	return;
-    }
-
-
-    VEC_CROSS(_M,n,_BD);
-    _M[3] = VEC_DOT(_M,vB1);
-
-    LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1));
-    /*Closest point on segment*/
-    VEC_DIFF(vPointB,vPointA,vB1);
-	_tp = VEC_DOT(vPointB, _BD);
-	_tp/= VEC_DOT(_BD, _BD);
-	_tp = GIM_CLAMP(_tp,0.0f,1.0f);
-    VEC_SCALE(vPointB,_tp,_BD);
-    VEC_SUM(vPointB,vPointB,vB1);
-}
-
+	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);
+	if (_tp < G_EPSILON)  //ARE PARALELE
+	{
+		//project B over A
+		bool invert_b_order = false;
+		_M[0] = VEC_DOT(vB1, _AD);
+		_M[1] = VEC_DOT(vB2, _AD);
+		if (_M[0] > _M[1])
+		{
+			invert_b_order = true;
+			GIM_SWAP_NUMBERS(_M[0], _M[1]);
+		}
+		_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;
+
+		if (n[0] < n[1])
+		{
+			if (_M[1] < _M[2])
+			{
+				vPointB = invert_b_order ? vB1 : vB2;
+				vPointA = vA1;
+			}
+			else if (_M[1] < _M[3])
+			{
+				vPointB = invert_b_order ? vB1 : vB2;
+				CLOSEST_POINT_ON_SEGMENT(vPointA, vPointB, vA1, vA2);
+			}
+			else
+			{
+				vPointA = vA2;
+				CLOSEST_POINT_ON_SEGMENT(vPointB, vPointA, vB1, vB2);
+			}
+		}
+		else
+		{
+			if (_M[3] < _M[0])
+			{
+				vPointB = invert_b_order ? vB2 : vB1;
+				vPointA = vA2;
+			}
+			else if (_M[3] < _M[1])
+			{
+				vPointA = vA2;
+				CLOSEST_POINT_ON_SEGMENT(vPointB, vPointA, vB1, vB2);
+			}
+			else
+			{
+				vPointB = invert_b_order ? vB1 : vB2;
+				CLOSEST_POINT_ON_SEGMENT(vPointA, vPointB, vA1, vA2);
+			}
+		}
+		return;
+	}
 
+	VEC_CROSS(_M, n, _BD);
+	_M[3] = VEC_DOT(_M, vB1);
 
+	LINE_PLANE_COLLISION(_M, _AD, vA1, vPointA, _tp, btScalar(0), btScalar(1));
+	/*Closest point on segment*/
+	VEC_DIFF(vPointB, vPointA, vB1);
+	_tp = VEC_DOT(vPointB, _BD);
+	_tp /= VEC_DOT(_BD, _BD);
+	_tp = GIM_CLAMP(_tp, 0.0f, 1.0f);
+	VEC_SCALE(vPointB, _tp, _BD);
+	VEC_SUM(vPointB, vPointB, vB1);
+}
 
 //! Line box intersection in one dimension
 /*!
@@ -494,37 +492,36 @@ SIMD_FORCE_INLINE void SEGMENT_COLLISION(
 *\param tlast the maximum projection. Assign to INFINITY at first.
 *\return true if there is an intersection.
 */
-template<typename T>
-SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir,T bmin, T bmax, T & tfirst, T & tlast)
+template <typename T>
+SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir, T bmin, T bmax, T &tfirst, T &tlast)
 {
-	if(GIM_IS_ZERO(dir))
+	if (GIM_IS_ZERO(dir))
 	{
-        return !(pos < bmin || pos > bmax);
+		return !(pos < bmin || pos > bmax);
 	}
 	GREAL a0 = (bmin - pos) / dir;
 	GREAL a1 = (bmax - pos) / dir;
-	if(a0 > a1)   GIM_SWAP_NUMBERS(a0, a1);
+	if (a0 > a1) GIM_SWAP_NUMBERS(a0, a1);
 	tfirst = GIM_MAX(a0, tfirst);
 	tlast = GIM_MIN(a1, tlast);
 	if (tlast < tfirst) return false;
 	return true;
 }
 
-
 //! Sorts 3 componets
-template<typename T>
+template <typename T>
 SIMD_FORCE_INLINE void SORT_3_INDICES(
-		const T * values,
-		GUINT * order_indices)
+	const T *values,
+	GUINT *order_indices)
 {
 	//get minimum
 	order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2);
 
 	//get second and third
-	GUINT i0 = (order_indices[0] + 1)%3;
-	GUINT i1 = (i0 + 1)%3;
+	GUINT i0 = (order_indices[0] + 1) % 3;
+	GUINT i1 = (i0 + 1) % 3;
 
-	if(values[i0] < values[i1])
+	if (values[i0] < values[i1])
 	{
 		order_indices[1] = i0;
 		order_indices[2] = i1;
@@ -536,8 +533,4 @@ SIMD_FORCE_INLINE void SORT_3_INDICES(
 	}
 }
 
-
-
-
-
-#endif // GIM_VECTOR_H_INCLUDED
+#endif  // GIM_VECTOR_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h
index 7dee48a4c76..c1fb41a5c0c 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h
@@ -34,34 +34,32 @@ email: projectileman@yahoo.com
 
 #include "gim_array.h"
 
-
 #define GUINT_BIT_COUNT 32
 #define GUINT_EXPONENT 5
 
 class gim_bitset
 {
 public:
-    gim_array<GUINT> m_container;
-
-    gim_bitset()
-    {
+	gim_array<GUINT> m_container;
 
-    }
+	gim_bitset()
+	{
+	}
 
-    gim_bitset(GUINT bits_count)
-    {
-        resize(bits_count);
-    }
+	gim_bitset(GUINT bits_count)
+	{
+		resize(bits_count);
+	}
 
-    ~gim_bitset()
-    {
-    }
+	~gim_bitset()
+	{
+	}
 
 	inline bool resize(GUINT newsize)
 	{
 		GUINT oldsize = m_container.size();
-		m_container.resize(newsize/GUINT_BIT_COUNT + 1,false);
-		while(oldsize<m_container.size())
+		m_container.resize(newsize / GUINT_BIT_COUNT + 1, false);
+		while (oldsize < m_container.size())
 		{
 			m_container[oldsize] = 0;
 		}
@@ -70,12 +68,12 @@ public:
 
 	inline GUINT size()
 	{
-		return m_container.size()*GUINT_BIT_COUNT;
+		return m_container.size() * GUINT_BIT_COUNT;
 	}
 
 	inline void set_all()
 	{
-		for(GUINT i = 0;i<m_container.size();++i)
+		for (GUINT i = 0; i < m_container.size(); ++i)
 		{
 			m_container[i] = 0xffffffff;
 		}
@@ -83,7 +81,7 @@ public:
 
 	inline void clear_all()
 	{
-	    for(GUINT i = 0;i<m_container.size();++i)
+		for (GUINT i = 0; i < m_container.size(); ++i)
 		{
 			m_container[i] = 0;
 		}
@@ -91,33 +89,29 @@ public:
 
 	inline void set(GUINT bit_index)
 	{
-		if(bit_index>=size())
+		if (bit_index >= size())
 		{
 			resize(bit_index);
 		}
-		m_container[bit_index >> GUINT_EXPONENT] |= (1 << (bit_index & (GUINT_BIT_COUNT-1)));
+		m_container[bit_index >> GUINT_EXPONENT] |= (1 << (bit_index & (GUINT_BIT_COUNT - 1)));
 	}
 
 	///Return 0 or 1
 	inline char get(GUINT bit_index)
 	{
-		if(bit_index>=size())
+		if (bit_index >= size())
 		{
 			return 0;
 		}
 		char value = m_container[bit_index >> GUINT_EXPONENT] &
-					 (1 << (bit_index & (GUINT_BIT_COUNT-1)));
+					 (1 << (bit_index & (GUINT_BIT_COUNT - 1)));
 		return value;
 	}
 
 	inline void clear(GUINT bit_index)
 	{
-	    m_container[bit_index >> GUINT_EXPONENT] &= ~(1 << (bit_index & (GUINT_BIT_COUNT-1)));
+		m_container[bit_index >> GUINT_EXPONENT] &= ~(1 << (bit_index & (GUINT_BIT_COUNT - 1)));
 	}
 };
 
-
-
-
-
-#endif // GIM_CONTAINERS_H_INCLUDED
+#endif  // GIM_CONTAINERS_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h
index 9c572638acd..9f7cbe732fa 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h
@@ -35,8 +35,6 @@ email: projectileman@yahoo.com
 #include "gim_basic_geometry_operations.h"
 #include "LinearMath/btTransform.h"
 
-
-
 //SIMD_FORCE_INLINE bool test_cross_edge_box(
 //	const btVector3 & edge,
 //	const btVector3 & absolute_edge,
@@ -97,51 +95,52 @@ email: projectileman@yahoo.com
 //	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1);
 //}
 
-#define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
-{\
-	const btScalar dir0 = -edge[i_dir_0];\
-	const btScalar dir1 = edge[i_dir_1];\
-	btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
-	btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
-	if(pmin>pmax)\
-	{\
-		GIM_SWAP_NUMBERS(pmin,pmax); \
-	}\
-	const btScalar abs_dir0 = absolute_edge[i_dir_0];\
-	const btScalar abs_dir1 = absolute_edge[i_dir_1];\
-	const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
-	if(pmin>rad || -rad>pmax) return false;\
-}\
-
-
-#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
-{\
-	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
-}\
+#ifndef TEST_CROSS_EDGE_BOX_MCR
+
+#define TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, i_dir_0, i_dir_1, i_comp_0, i_comp_1) \
+	{                                                                                                               \
+		const btScalar dir0 = -edge[i_dir_0];                                                                       \
+		const btScalar dir1 = edge[i_dir_1];                                                                        \
+		btScalar pmin = pointa[i_comp_0] * dir0 + pointa[i_comp_1] * dir1;                                          \
+		btScalar pmax = pointb[i_comp_0] * dir0 + pointb[i_comp_1] * dir1;                                          \
+		if (pmin > pmax)                                                                                            \
+		{                                                                                                           \
+			GIM_SWAP_NUMBERS(pmin, pmax);                                                                           \
+		}                                                                                                           \
+		const btScalar abs_dir0 = absolute_edge[i_dir_0];                                                           \
+		const btScalar abs_dir1 = absolute_edge[i_dir_1];                                                           \
+		const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;                           \
+		if (pmin > rad || -rad > pmax) return false;                                                                \
+	}
 
-#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
-{\
-	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
-}\
+#endif
 
-#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
-{\
-	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
-}\
+#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend)       \
+	{                                                                                      \
+		TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 2, 1, 1, 2); \
+	}
 
+#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend)       \
+	{                                                                                      \
+		TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 0, 2, 2, 0); \
+	}
 
+#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend)       \
+	{                                                                                      \
+		TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 1, 0, 0, 1); \
+	}
 
 //!  Class for transforming a model1 to the space of model0
 class GIM_BOX_BOX_TRANSFORM_CACHE
 {
 public:
-    btVector3  m_T1to0;//!< Transforms translation of model1 to model 0
-	btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal  to R0' * R1
-	btMatrix3x3 m_AR;//!< Absolute value of m_R1to0
+	btVector3 m_T1to0;    //!< Transforms translation of model1 to model 0
+	btMatrix3x3 m_R1to0;  //!< Transforms Rotation of model1 to model 0, equal  to R0' * R1
+	btMatrix3x3 m_AR;     //!< Absolute value of m_R1to0
 
 	SIMD_FORCE_INLINE void calc_absolute_matrix()
 	{
-		static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
+		static const btVector3 vepsi(1e-6f, 1e-6f, 1e-6f);
 		m_AR[0] = vepsi + m_R1to0[0].absolute();
 		m_AR[1] = vepsi + m_R1to0[1].absolute();
 		m_AR[2] = vepsi + m_R1to0[2].absolute();
@@ -151,47 +150,46 @@ public:
 	{
 	}
 
-
-	GIM_BOX_BOX_TRANSFORM_CACHE(mat4f  trans1_to_0)
+	GIM_BOX_BOX_TRANSFORM_CACHE(mat4f trans1_to_0)
 	{
-		COPY_MATRIX_3X3(m_R1to0,trans1_to_0)
-        MAT_GET_TRANSLATION(trans1_to_0,m_T1to0)
+		COPY_MATRIX_3X3(m_R1to0, trans1_to_0)
+		MAT_GET_TRANSLATION(trans1_to_0, m_T1to0)
 		calc_absolute_matrix();
 	}
 
 	//! Calc the transformation relative  1 to 0. Inverts matrics by transposing
-	SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
+	SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform &trans0, const btTransform &trans1)
 	{
-
 		m_R1to0 = trans0.getBasis().transpose();
 		m_T1to0 = m_R1to0 * (-trans0.getOrigin());
 
-		m_T1to0 += m_R1to0*trans1.getOrigin();
+		m_T1to0 += m_R1to0 * trans1.getOrigin();
 		m_R1to0 *= trans1.getBasis();
 
 		calc_absolute_matrix();
 	}
 
 	//! Calcs the full invertion of the matrices. Useful for scaling matrices
-	SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
+	SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform &trans0, const btTransform &trans1)
 	{
 		m_R1to0 = trans0.getBasis().inverse();
 		m_T1to0 = m_R1to0 * (-trans0.getOrigin());
 
-		m_T1to0 += m_R1to0*trans1.getOrigin();
+		m_T1to0 += m_R1to0 * trans1.getOrigin();
 		m_R1to0 *= trans1.getBasis();
 
 		calc_absolute_matrix();
 	}
 
-	SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point)
+	SIMD_FORCE_INLINE btVector3 transform(const btVector3 &point)
 	{
-        return point.dot3(m_R1to0[0], m_R1to0[1], m_R1to0[2]) + m_T1to0;
+		return point.dot3(m_R1to0[0], m_R1to0[1], m_R1to0[2]) + m_T1to0;
 	}
 };
 
-
+#ifndef BOX_PLANE_EPSILON
 #define BOX_PLANE_EPSILON 0.000001f
+#endif
 
 //! Axis aligned box
 class GIM_AABB
@@ -201,34 +199,34 @@ public:
 	btVector3 m_max;
 
 	GIM_AABB()
-	{}
-
+	{
+	}
 
-	GIM_AABB(const btVector3 & V1,
-			 const btVector3 & V2,
-			 const btVector3 & V3)
+	GIM_AABB(const btVector3 &V1,
+			 const btVector3 &V2,
+			 const btVector3 &V3)
 	{
-		m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
-		m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
-		m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+		m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]);
+		m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]);
+		m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]);
 
-		m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
-		m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
-		m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+		m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]);
+		m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]);
+		m_max[2] = GIM_MAX3(V1[2], V2[2], V3[2]);
 	}
 
-	GIM_AABB(const btVector3 & V1,
-			 const btVector3 & V2,
-			 const btVector3 & V3,
+	GIM_AABB(const btVector3 &V1,
+			 const btVector3 &V2,
+			 const btVector3 &V3,
 			 GREAL margin)
 	{
-		m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
-		m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
-		m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+		m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]);
+		m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]);
+		m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]);
 
-		m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
-		m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
-		m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+		m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]);
+		m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]);
+		m_max[2] = GIM_MAX3(V1[2], V2[2], V3[2]);
 
 		m_min[0] -= margin;
 		m_min[1] -= margin;
@@ -238,13 +236,11 @@ public:
 		m_max[2] += margin;
 	}
 
-	GIM_AABB(const GIM_AABB &other):
-		m_min(other.m_min),m_max(other.m_max)
+	GIM_AABB(const GIM_AABB &other) : m_min(other.m_min), m_max(other.m_max)
 	{
 	}
 
-	GIM_AABB(const GIM_AABB &other,btScalar margin ):
-		m_min(other.m_min),m_max(other.m_max)
+	GIM_AABB(const GIM_AABB &other, btScalar margin) : m_min(other.m_min), m_max(other.m_max)
 	{
 		m_min[0] -= margin;
 		m_min[1] -= margin;
@@ -285,34 +281,34 @@ public:
 		m_max[2] = other.m_max[2] + margin;
 	}
 
-	template<typename CLASS_POINT>
+	template <typename CLASS_POINT>
 	SIMD_FORCE_INLINE void calc_from_triangle(
-							const CLASS_POINT & V1,
-							const CLASS_POINT & V2,
-							const CLASS_POINT & V3)
+		const CLASS_POINT &V1,
+		const CLASS_POINT &V2,
+		const CLASS_POINT &V3)
 	{
-		m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
-		m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
-		m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+		m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]);
+		m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]);
+		m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]);
 
-		m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
-		m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
-		m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+		m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]);
+		m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]);
+		m_max[2] = GIM_MAX3(V1[2], V2[2], V3[2]);
 	}
 
-	template<typename CLASS_POINT>
+	template <typename CLASS_POINT>
 	SIMD_FORCE_INLINE void calc_from_triangle_margin(
-							const CLASS_POINT & V1,
-							const CLASS_POINT & V2,
-							const CLASS_POINT & V3, btScalar margin)
+		const CLASS_POINT &V1,
+		const CLASS_POINT &V2,
+		const CLASS_POINT &V3, btScalar margin)
 	{
-		m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
-		m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
-		m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+		m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]);
+		m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]);
+		m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]);
 
-		m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
-		m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
-		m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+		m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]);
+		m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]);
+		m_max[2] = GIM_MAX3(V1[2], V2[2], V3[2]);
 
 		m_min[0] -= margin;
 		m_min[1] -= margin;
@@ -323,74 +319,73 @@ public:
 	}
 
 	//! Apply a transform to an AABB
-	SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
+	SIMD_FORCE_INLINE void appy_transform(const btTransform &trans)
 	{
-		btVector3 center = (m_max+m_min)*0.5f;
+		btVector3 center = (m_max + m_min) * 0.5f;
 		btVector3 extends = m_max - center;
 		// Compute new center
 		center = trans(center);
 
-        btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(), 
-                                          trans.getBasis().getRow(1).absolute(), 
-                                          trans.getBasis().getRow(2).absolute());
-        
+		btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(),
+										  trans.getBasis().getRow(1).absolute(),
+										  trans.getBasis().getRow(2).absolute());
+
 		m_min = center - textends;
 		m_max = center + textends;
 	}
 
 	//! Merges a Box
-	SIMD_FORCE_INLINE void merge(const GIM_AABB & box)
+	SIMD_FORCE_INLINE void merge(const GIM_AABB &box)
 	{
-		m_min[0] = GIM_MIN(m_min[0],box.m_min[0]);
-		m_min[1] = GIM_MIN(m_min[1],box.m_min[1]);
-		m_min[2] = GIM_MIN(m_min[2],box.m_min[2]);
+		m_min[0] = GIM_MIN(m_min[0], box.m_min[0]);
+		m_min[1] = GIM_MIN(m_min[1], box.m_min[1]);
+		m_min[2] = GIM_MIN(m_min[2], box.m_min[2]);
 
-		m_max[0] = GIM_MAX(m_max[0],box.m_max[0]);
-		m_max[1] = GIM_MAX(m_max[1],box.m_max[1]);
-		m_max[2] = GIM_MAX(m_max[2],box.m_max[2]);
+		m_max[0] = GIM_MAX(m_max[0], box.m_max[0]);
+		m_max[1] = GIM_MAX(m_max[1], box.m_max[1]);
+		m_max[2] = GIM_MAX(m_max[2], box.m_max[2]);
 	}
 
 	//! Merges a point
-	template<typename CLASS_POINT>
-	SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
+	template <typename CLASS_POINT>
+	SIMD_FORCE_INLINE void merge_point(const CLASS_POINT &point)
 	{
-		m_min[0] = GIM_MIN(m_min[0],point[0]);
-		m_min[1] = GIM_MIN(m_min[1],point[1]);
-		m_min[2] = GIM_MIN(m_min[2],point[2]);
+		m_min[0] = GIM_MIN(m_min[0], point[0]);
+		m_min[1] = GIM_MIN(m_min[1], point[1]);
+		m_min[2] = GIM_MIN(m_min[2], point[2]);
 
-		m_max[0] = GIM_MAX(m_max[0],point[0]);
-		m_max[1] = GIM_MAX(m_max[1],point[1]);
-		m_max[2] = GIM_MAX(m_max[2],point[2]);
+		m_max[0] = GIM_MAX(m_max[0], point[0]);
+		m_max[1] = GIM_MAX(m_max[1], point[1]);
+		m_max[2] = GIM_MAX(m_max[2], point[2]);
 	}
 
 	//! Gets the extend and center
-	SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend)  const
+	SIMD_FORCE_INLINE void get_center_extend(btVector3 &center, btVector3 &extend) const
 	{
-		center = (m_max+m_min)*0.5f;
+		center = (m_max + m_min) * 0.5f;
 		extend = m_max - center;
 	}
 
 	//! Finds the intersecting box between this box and the other.
-	SIMD_FORCE_INLINE void find_intersection(const GIM_AABB & other, GIM_AABB & intersection)  const
+	SIMD_FORCE_INLINE void find_intersection(const GIM_AABB &other, GIM_AABB &intersection) const
 	{
-		intersection.m_min[0] = GIM_MAX(other.m_min[0],m_min[0]);
-		intersection.m_min[1] = GIM_MAX(other.m_min[1],m_min[1]);
-		intersection.m_min[2] = GIM_MAX(other.m_min[2],m_min[2]);
+		intersection.m_min[0] = GIM_MAX(other.m_min[0], m_min[0]);
+		intersection.m_min[1] = GIM_MAX(other.m_min[1], m_min[1]);
+		intersection.m_min[2] = GIM_MAX(other.m_min[2], m_min[2]);
 
-		intersection.m_max[0] = GIM_MIN(other.m_max[0],m_max[0]);
-		intersection.m_max[1] = GIM_MIN(other.m_max[1],m_max[1]);
-		intersection.m_max[2] = GIM_MIN(other.m_max[2],m_max[2]);
+		intersection.m_max[0] = GIM_MIN(other.m_max[0], m_max[0]);
+		intersection.m_max[1] = GIM_MIN(other.m_max[1], m_max[1]);
+		intersection.m_max[2] = GIM_MIN(other.m_max[2], m_max[2]);
 	}
 
-
-	SIMD_FORCE_INLINE bool has_collision(const GIM_AABB & other) const
+	SIMD_FORCE_INLINE bool has_collision(const GIM_AABB &other) const
 	{
-		if(m_min[0] > other.m_max[0] ||
-		   m_max[0] < other.m_min[0] ||
-		   m_min[1] > other.m_max[1] ||
-		   m_max[1] < other.m_min[1] ||
-		   m_min[2] > other.m_max[2] ||
-		   m_max[2] < other.m_min[2])
+		if (m_min[0] > other.m_max[0] ||
+			m_max[0] < other.m_min[0] ||
+			m_min[1] > other.m_max[1] ||
+			m_max[1] < other.m_min[1] ||
+			m_min[2] > other.m_max[2] ||
+			m_max[2] < other.m_min[2])
 		{
 			return false;
 		}
@@ -402,35 +397,34 @@ public:
 	\param vorigin A vec3f with the origin of the ray
 	\param vdir A vec3f with the direction of the ray
 	*/
-	SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir)
+	SIMD_FORCE_INLINE bool collide_ray(const btVector3 &vorigin, const btVector3 &vdir)
 	{
-		btVector3 extents,center;
-		this->get_center_extend(center,extents);;
+		btVector3 extents, center;
+		this->get_center_extend(center, extents);
+		;
 
 		btScalar Dx = vorigin[0] - center[0];
-		if(GIM_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f)	return false;
+		if (GIM_GREATER(Dx, extents[0]) && Dx * vdir[0] >= 0.0f) return false;
 		btScalar Dy = vorigin[1] - center[1];
-		if(GIM_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f)	return false;
+		if (GIM_GREATER(Dy, extents[1]) && Dy * vdir[1] >= 0.0f) return false;
 		btScalar Dz = vorigin[2] - center[2];
-		if(GIM_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f)	return false;
-
+		if (GIM_GREATER(Dz, extents[2]) && Dz * vdir[2] >= 0.0f) return false;
 
 		btScalar f = vdir[1] * Dz - vdir[2] * Dy;
-		if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
+		if (btFabs(f) > extents[1] * btFabs(vdir[2]) + extents[2] * btFabs(vdir[1])) return false;
 		f = vdir[2] * Dx - vdir[0] * Dz;
-		if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
+		if (btFabs(f) > extents[0] * btFabs(vdir[2]) + extents[2] * btFabs(vdir[0])) return false;
 		f = vdir[0] * Dy - vdir[1] * Dx;
-		if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
+		if (btFabs(f) > extents[0] * btFabs(vdir[1]) + extents[1] * btFabs(vdir[0])) return false;
 		return true;
 	}
 
-
-	SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
+	SIMD_FORCE_INLINE void projection_interval(const btVector3 &direction, btScalar &vmin, btScalar &vmax) const
 	{
-		btVector3 center = (m_max+m_min)*0.5f;
-		btVector3 extend = m_max-center;
+		btVector3 center = (m_max + m_min) * 0.5f;
+		btVector3 extend = m_max - center;
 
-		btScalar _fOrigin =  direction.dot(center);
+		btScalar _fOrigin = direction.dot(center);
 		btScalar _fMaximumExtent = extend.dot(direction.absolute());
 		vmin = _fOrigin - _fMaximumExtent;
 		vmax = _fOrigin + _fMaximumExtent;
@@ -438,22 +432,22 @@ public:
 
 	SIMD_FORCE_INLINE ePLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
 	{
-		btScalar _fmin,_fmax;
-		this->projection_interval(plane,_fmin,_fmax);
+		btScalar _fmin, _fmax;
+		this->projection_interval(plane, _fmin, _fmax);
 
-		if(plane[3] > _fmax + BOX_PLANE_EPSILON)
+		if (plane[3] > _fmax + BOX_PLANE_EPSILON)
 		{
-			return G_BACK_PLANE; // 0
+			return G_BACK_PLANE;  // 0
 		}
 
-		if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
+		if (plane[3] + BOX_PLANE_EPSILON >= _fmin)
 		{
-			return G_COLLIDE_PLANE; //1
+			return G_COLLIDE_PLANE;  //1
 		}
-		return G_FRONT_PLANE;//2
+		return G_FRONT_PLANE;  //2
 	}
 
-	SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB & box, btTransform & trans1_to_0)
+	SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB &box, btTransform &trans1_to_0)
 	{
 		GIM_AABB tbox = box;
 		tbox.appy_transform(trans1_to_0);
@@ -462,52 +456,50 @@ public:
 
 	//! transcache is the transformation cache from box to this AABB
 	SIMD_FORCE_INLINE bool overlapping_trans_cache(
-		const GIM_AABB & box,const GIM_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest)
+		const GIM_AABB &box, const GIM_BOX_BOX_TRANSFORM_CACHE &transcache, bool fulltest)
 	{
-
 		//Taken from OPCODE
-		btVector3 ea,eb;//extends
-		btVector3 ca,cb;//extends
-		get_center_extend(ca,ea);
-		box.get_center_extend(cb,eb);
-
+		btVector3 ea, eb;  //extends
+		btVector3 ca, cb;  //extends
+		get_center_extend(ca, ea);
+		box.get_center_extend(cb, eb);
 
 		btVector3 T;
-		btScalar t,t2;
+		btScalar t, t2;
 		int i;
 
 		// Class I : A's basis vectors
-		for(i=0;i<3;i++)
+		for (i = 0; i < 3; i++)
 		{
-			T[i] =  transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
+			T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
 			t = transcache.m_AR[i].dot(eb) + ea[i];
-			if(GIM_GREATER(T[i], t))	return false;
+			if (GIM_GREATER(T[i], t)) return false;
 		}
 		// Class II : B's basis vectors
-		for(i=0;i<3;i++)
+		for (i = 0; i < 3; i++)
 		{
-			t = MAT_DOT_COL(transcache.m_R1to0,T,i);
-			t2 = MAT_DOT_COL(transcache.m_AR,ea,i) + eb[i];
-			if(GIM_GREATER(t,t2))	return false;
+			t = MAT_DOT_COL(transcache.m_R1to0, T, i);
+			t2 = MAT_DOT_COL(transcache.m_AR, ea, i) + eb[i];
+			if (GIM_GREATER(t, t2)) return false;
 		}
 		// Class III : 9 cross products
-		if(fulltest)
+		if (fulltest)
 		{
-			int j,m,n,o,p,q,r;
-			for(i=0;i<3;i++)
+			int j, m, n, o, p, q, r;
+			for (i = 0; i < 3; i++)
 			{
-				m = (i+1)%3;
-				n = (i+2)%3;
-				o = i==0?1:0;
-				p = i==2?1:2;
-				for(j=0;j<3;j++)
+				m = (i + 1) % 3;
+				n = (i + 2) % 3;
+				o = i == 0 ? 1 : 0;
+				p = i == 2 ? 1 : 2;
+				for (j = 0; j < 3; j++)
 				{
-					q = j==2?1:2;
-					r = j==0?1:0;
-					t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
-					t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
-						eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
-					if(GIM_GREATER(t,t2))	return false;
+					q = j == 2 ? 1 : 2;
+					r = j == 0 ? 1 : 0;
+					t = T[n] * transcache.m_R1to0[m][j] - T[m] * transcache.m_R1to0[n][j];
+					t2 = ea[o] * transcache.m_AR[p][j] + ea[p] * transcache.m_AR[o][j] +
+						 eb[r] * transcache.m_AR[i][q] + eb[q] * transcache.m_AR[i][r];
+					if (GIM_GREATER(t, t2)) return false;
 				}
 			}
 		}
@@ -516,7 +508,7 @@ public:
 
 	//! Simple test for planes.
 	SIMD_FORCE_INLINE bool collide_plane(
-		const btVector4 & plane)
+		const btVector4 &plane)
 	{
 		ePLANE_INTERSECTION_TYPE classify = plane_classify(plane);
 		return (classify == G_COLLIDE_PLANE);
@@ -524,15 +516,15 @@ public:
 
 	//! test for a triangle, with edges
 	SIMD_FORCE_INLINE bool collide_triangle_exact(
-		const btVector3 & p1,
-		const btVector3 & p2,
-		const btVector3 & p3,
-		const btVector4 & triangle_plane)
+		const btVector3 &p1,
+		const btVector3 &p2,
+		const btVector3 &p3,
+		const btVector4 &triangle_plane)
 	{
-		if(!collide_plane(triangle_plane)) return false;
+		if (!collide_plane(triangle_plane)) return false;
 
-		btVector3 center,extends;
-		this->get_center_extend(center,extends);
+		btVector3 center, extends;
+		this->get_center_extend(center, extends);
 
 		const btVector3 v1(p1 - center);
 		const btVector3 v2(p2 - center);
@@ -542,47 +534,45 @@ public:
 		btVector3 diff(v2 - v1);
 		btVector3 abs_diff = diff.absolute();
 		//Test With X axis
-		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v1, v3, extends);
 		//Test With Y axis
-		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v1, v3, extends);
 		//Test With Z axis
-		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);
-
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v1, v3, extends);
 
 		diff = v3 - v2;
 		abs_diff = diff.absolute();
 		//Test With X axis
-		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v2, v1, extends);
 		//Test With Y axis
-		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v2, v1, extends);
 		//Test With Z axis
-		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v2, v1, extends);
 
 		diff = v1 - v3;
 		abs_diff = diff.absolute();
 		//Test With X axis
-		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v3, v2, extends);
 		//Test With Y axis
-		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v3, v2, extends);
 		//Test With Z axis
-		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v3, v2, extends);
 
 		return true;
 	}
 };
 
-
+#ifndef BT_BOX_COLLISION_H_INCLUDED
 //! Compairison of transformation objects
-SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
+SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform &t1, const btTransform &t2)
 {
-	if(!(t1.getOrigin() == t2.getOrigin()) ) return false;
+	if (!(t1.getOrigin() == t2.getOrigin())) return false;
 
-	if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
-	if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
-	if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
+	if (!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0))) return false;
+	if (!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1))) return false;
+	if (!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2))) return false;
 	return true;
 }
+#endif
 
-
-
-#endif // GIM_BOX_COLLISION_H_INCLUDED
+#endif  // GIM_BOX_COLLISION_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp
index 0c3d7ba8db0..0c7a6b7fc1b 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp
@@ -28,67 +28,64 @@ email: projectileman@yahoo.com
 -----------------------------------------------------------------------------
 */
 
-
 #include "gim_box_set.h"
 
-
 GUINT GIM_BOX_TREE::_calc_splitting_axis(
-	gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex)
+	gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex)
 {
 	GUINT i;
 
-	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
-	btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
-	GUINT numIndices = endIndex-startIndex;
+	btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
+	btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.));
+	GUINT numIndices = endIndex - startIndex;
 
-	for (i=startIndex;i<endIndex;i++)
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
-					 primitive_boxes[i].m_bound.m_min);
-		means+=center;
+		btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
+											primitive_boxes[i].m_bound.m_min);
+		means += center;
 	}
-	means *= (btScalar(1.)/(btScalar)numIndices);
+	means *= (btScalar(1.) / (btScalar)numIndices);
 
-	for (i=startIndex;i<endIndex;i++)
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
-					 primitive_boxes[i].m_bound.m_min);
-		btVector3 diff2 = center-means;
+		btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
+											primitive_boxes[i].m_bound.m_min);
+		btVector3 diff2 = center - means;
 		diff2 = diff2 * diff2;
 		variance += diff2;
 	}
-	variance *= (btScalar(1.)/	((btScalar)numIndices-1)	);
+	variance *= (btScalar(1.) / ((btScalar)numIndices - 1));
 
 	return variance.maxAxis();
 }
 
-
 GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index(
-	gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,
+	gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex,
 	GUINT endIndex, GUINT splitAxis)
 {
 	GUINT i;
-	GUINT splitIndex =startIndex;
+	GUINT splitIndex = startIndex;
 	GUINT numIndices = endIndex - startIndex;
 
 	// average of centers
 	btScalar splitValue = 0.0f;
-	for (i=startIndex;i<endIndex;i++)
+	for (i = startIndex; i < endIndex; i++)
 	{
-		splitValue+= 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] +
-					 primitive_boxes[i].m_bound.m_min[splitAxis]);
+		splitValue += 0.5f * (primitive_boxes[i].m_bound.m_max[splitAxis] +
+							  primitive_boxes[i].m_bound.m_min[splitAxis]);
 	}
 	splitValue /= (btScalar)numIndices;
 
 	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
-	for (i=startIndex;i<endIndex;i++)
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btScalar center = 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] +
-					 primitive_boxes[i].m_bound.m_min[splitAxis]);
+		btScalar center = 0.5f * (primitive_boxes[i].m_bound.m_max[splitAxis] +
+								  primitive_boxes[i].m_bound.m_min[splitAxis]);
 		if (center > splitValue)
 		{
 			//swap
-			primitive_boxes.swap(i,splitIndex);
+			primitive_boxes.swap(i, splitIndex);
 			splitIndex++;
 		}
 	}
@@ -102,28 +99,27 @@ GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index(
 	//bool unbalanced2 = true;
 
 	//this should be safe too:
-	GUINT rangeBalancedIndices = numIndices/3;
-	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+	GUINT rangeBalancedIndices = numIndices / 3;
+	bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices)));
 
 	if (unbalanced)
 	{
-		splitIndex = startIndex+ (numIndices>>1);
+		splitIndex = startIndex + (numIndices >> 1);
 	}
 
-	btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
+	btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex))));
 
 	return splitIndex;
 }
 
-
-void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex)
+void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex)
 {
 	GUINT current_index = m_num_nodes++;
 
-	btAssert((endIndex-startIndex)>0);
+	btAssert((endIndex - startIndex) > 0);
 
-	if((endIndex-startIndex) == 1) //we got a leaf
-	{		
+	if ((endIndex - startIndex) == 1)  //we got a leaf
+	{
 		m_node_array[current_index].m_left = 0;
 		m_node_array[current_index].m_right = 0;
 		m_node_array[current_index].m_escapeIndex = 0;
@@ -138,8 +134,8 @@ void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, G
 	GUINT splitIndex;
 
 	//calc this node bounding box
-	m_node_array[current_index].m_bound.invalidate();	
-	for (splitIndex=startIndex;splitIndex<endIndex;splitIndex++)
+	m_node_array[current_index].m_bound.invalidate();
+	for (splitIndex = startIndex; splitIndex < endIndex; splitIndex++)
 	{
 		m_node_array[current_index].m_bound.merge(primitive_boxes[splitIndex].m_bound);
 	}
@@ -147,36 +143,34 @@ void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, G
 	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
 
 	//split axis
-	splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
+	splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex);
 
 	splitIndex = _sort_and_calc_splitting_index(
-			primitive_boxes,startIndex,endIndex,splitIndex);
+		primitive_boxes, startIndex, endIndex, splitIndex);
 
 	//configure this inner node : the left node index
 	m_node_array[current_index].m_left = m_num_nodes;
 	//build left child tree
-	_build_sub_tree(primitive_boxes, startIndex, splitIndex );
+	_build_sub_tree(primitive_boxes, startIndex, splitIndex);
 
 	//configure this inner node : the right node index
 	m_node_array[current_index].m_right = m_num_nodes;
 
 	//build right child tree
-	_build_sub_tree(primitive_boxes, splitIndex ,endIndex);
+	_build_sub_tree(primitive_boxes, splitIndex, endIndex);
 
 	//configure this inner node : the escape index
-	m_node_array[current_index].m_escapeIndex  = m_num_nodes - current_index;
+	m_node_array[current_index].m_escapeIndex = m_num_nodes - current_index;
 }
 
 //! stackless build tree
 void GIM_BOX_TREE::build_tree(
-	gim_array<GIM_AABB_DATA> & primitive_boxes)
+	gim_array<GIM_AABB_DATA>& primitive_boxes)
 {
 	// initialize node count to 0
 	m_num_nodes = 0;
 	// allocate nodes
-	m_node_array.resize(primitive_boxes.size()*2);
-	
+	m_node_array.resize(primitive_boxes.size() * 2);
+
 	_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
 }
-
-
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h
index 61d190a7dfe..afc591dac0d 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h
@@ -33,54 +33,30 @@ email: projectileman@yahoo.com
 -----------------------------------------------------------------------------
 */
 
-
 #include "gim_array.h"
 #include "gim_radixsort.h"
 #include "gim_box_collision.h"
 #include "gim_tri_collision.h"
-
-
-
-//! Overlapping pair
-struct GIM_PAIR
-{
-    GUINT m_index1;
-    GUINT m_index2;
-    GIM_PAIR()
-    {}
-
-    GIM_PAIR(const GIM_PAIR & p)
-    {
-    	m_index1 = p.m_index1;
-    	m_index2 = p.m_index2;
-	}
-
-	GIM_PAIR(GUINT index1, GUINT index2)
-    {
-    	m_index1 = index1;
-    	m_index2 = index2;
-	}
-};
+#include "gim_pair.h"
 
 //! A pairset array
-class gim_pair_set: public gim_array<GIM_PAIR>
+class gim_pair_set : public gim_array<GIM_PAIR>
 {
 public:
-	gim_pair_set():gim_array<GIM_PAIR>(32)
+	gim_pair_set() : gim_array<GIM_PAIR>(32)
 	{
 	}
-	inline void push_pair(GUINT index1,GUINT index2)
+	inline void push_pair(GUINT index1, GUINT index2)
 	{
-		push_back(GIM_PAIR(index1,index2));
+		push_back(GIM_PAIR(index1, index2));
 	}
 
-	inline void push_pair_inv(GUINT index1,GUINT index2)
+	inline void push_pair_inv(GUINT index1, GUINT index2)
 	{
-		push_back(GIM_PAIR(index2,index1));
+		push_back(GIM_PAIR(index2, index1));
 	}
 };
 
-
 //! Prototype Base class for primitive classification
 /*!
 This class is a wrapper for primitive collections.
@@ -90,16 +66,14 @@ This class can manage Compound shapes and trimeshes, and if it is managing trime
 class GIM_PRIMITIVE_MANAGER_PROTOTYPE
 {
 public:
-
 	virtual ~GIM_PRIMITIVE_MANAGER_PROTOTYPE() {}
 	//! determines if this manager consist on only triangles, which special case will be optimized
 	virtual bool is_trimesh() = 0;
 	virtual GUINT get_primitive_count() = 0;
-	virtual void get_primitive_box(GUINT prim_index ,GIM_AABB & primbox) = 0;
-	virtual void get_primitive_triangle(GUINT prim_index,GIM_TRIANGLE & triangle) = 0;
+	virtual void get_primitive_box(GUINT prim_index, GIM_AABB& primbox) = 0;
+	virtual void get_primitive_triangle(GUINT prim_index, GIM_TRIANGLE& triangle) = 0;
 };
 
-
 struct GIM_AABB_DATA
 {
 	GIM_AABB m_bound;
@@ -110,22 +84,22 @@ struct GIM_AABB_DATA
 struct GIM_BOX_TREE_NODE
 {
 	GIM_AABB m_bound;
-	GUINT m_left;//!< Left subtree
-	GUINT m_right;//!< Right subtree
-	GUINT m_escapeIndex;//!< Scape index for traversing
-	GUINT m_data;//!< primitive index if apply
+	GUINT m_left;         //!< Left subtree
+	GUINT m_right;        //!< Right subtree
+	GUINT m_escapeIndex;  //!< Scape index for traversing
+	GUINT m_data;         //!< primitive index if apply
 
 	GIM_BOX_TREE_NODE()
 	{
-	    m_left = 0;
-	    m_right = 0;
-	    m_escapeIndex = 0;
-	    m_data = 0;
+		m_left = 0;
+		m_right = 0;
+		m_escapeIndex = 0;
+		m_data = 0;
 	}
 
 	SIMD_FORCE_INLINE bool is_leaf_node() const
 	{
-	    return  (!m_left && !m_right);
+		return (!m_left && !m_right);
 	}
 };
 
@@ -135,14 +109,16 @@ class GIM_BOX_TREE
 protected:
 	GUINT m_num_nodes;
 	gim_array<GIM_BOX_TREE_NODE> m_node_array;
+
 protected:
 	GUINT _sort_and_calc_splitting_index(
-		gim_array<GIM_AABB_DATA> & primitive_boxes,
-		 GUINT startIndex,  GUINT endIndex, GUINT splitAxis);
+		gim_array<GIM_AABB_DATA>& primitive_boxes,
+		GUINT startIndex, GUINT endIndex, GUINT splitAxis);
 
-	GUINT _calc_splitting_axis(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex);
+	GUINT _calc_splitting_axis(gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex);
+
+	void _build_sub_tree(gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex);
 
-	void _build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex);
 public:
 	GIM_BOX_TREE()
 	{
@@ -151,7 +127,7 @@ public:
 
 	//! prototype functions for box tree management
 	//!@{
-	void build_tree(gim_array<GIM_AABB_DATA> & primitive_boxes);
+	void build_tree(gim_array<GIM_AABB_DATA>& primitive_boxes);
 
 	SIMD_FORCE_INLINE void clearNodes()
 	{
@@ -176,22 +152,22 @@ public:
 		return m_node_array[nodeindex].m_data;
 	}
 
-	SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const
+	SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB& bound) const
 	{
 		bound = m_node_array[nodeindex].m_bound;
 	}
 
-	SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound)
+	SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB& bound)
 	{
 		m_node_array[nodeindex].m_bound = bound;
 	}
 
-	SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex)  const
+	SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const
 	{
 		return m_node_array[nodeindex].m_left;
 	}
 
-	SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex)  const
+	SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const
 	{
 		return m_node_array[nodeindex].m_right;
 	}
@@ -204,78 +180,78 @@ public:
 	//!@}
 };
 
-
 //! Generic Box Tree Template
 /*!
 This class offers an structure for managing a box tree of primitives.
 Requires a Primitive prototype (like GIM_PRIMITIVE_MANAGER_PROTOTYPE ) and
 a Box tree structure ( like GIM_BOX_TREE).
 */
-template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE, typename _GIM_BOX_TREE_PROTOTYPE>
+template <typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE, typename _GIM_BOX_TREE_PROTOTYPE>
 class GIM_BOX_TREE_TEMPLATE_SET
 {
 protected:
 	_GIM_PRIMITIVE_MANAGER_PROTOTYPE m_primitive_manager;
 	_GIM_BOX_TREE_PROTOTYPE m_box_tree;
+
 protected:
 	//stackless refit
 	SIMD_FORCE_INLINE void refit()
 	{
 		GUINT nodecount = getNodeCount();
-		while(nodecount--)
+		while (nodecount--)
 		{
-			if(isLeafNode(nodecount))
+			if (isLeafNode(nodecount))
 			{
 				GIM_AABB leafbox;
-				m_primitive_manager.get_primitive_box(getNodeData(nodecount),leafbox);
-				setNodeBound(nodecount,leafbox);
+				m_primitive_manager.get_primitive_box(getNodeData(nodecount), leafbox);
+				setNodeBound(nodecount, leafbox);
 			}
 			else
 			{
 				//get left bound
 				GUINT childindex = getLeftNodeIndex(nodecount);
 				GIM_AABB bound;
-				getNodeBound(childindex,bound);
+				getNodeBound(childindex, bound);
 				//get right bound
 				childindex = getRightNodeIndex(nodecount);
 				GIM_AABB bound2;
-				getNodeBound(childindex,bound2);
+				getNodeBound(childindex, bound2);
 				bound.merge(bound2);
 
-				setNodeBound(nodecount,bound);
+				setNodeBound(nodecount, bound);
 			}
 		}
 	}
-public:
 
+public:
 	GIM_BOX_TREE_TEMPLATE_SET()
 	{
 	}
 
-	SIMD_FORCE_INLINE GIM_AABB getGlobalBox()  const
+	SIMD_FORCE_INLINE GIM_AABB getGlobalBox() const
 	{
 		GIM_AABB totalbox;
 		getNodeBound(0, totalbox);
 		return totalbox;
 	}
 
-	SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & primitive_manager)
+	SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE& primitive_manager)
 	{
 		m_primitive_manager = primitive_manager;
 	}
 
-	const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() const
+	const _GIM_PRIMITIVE_MANAGER_PROTOTYPE& getPrimitiveManager() const
 	{
 		return m_primitive_manager;
 	}
 
-	_GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager()
+	_GIM_PRIMITIVE_MANAGER_PROTOTYPE& getPrimitiveManager()
 	{
 		return m_primitive_manager;
 	}
 
-//! node manager prototype functions
-///@{
+	//! node manager prototype functions
+	///@{
 
 	//! this attemps to refit the box set.
 	SIMD_FORCE_INLINE void update()
@@ -288,19 +264,19 @@ public:
 	{
 		//obtain primitive boxes
 		gim_array<GIM_AABB_DATA> primitive_boxes;
-		primitive_boxes.resize(m_primitive_manager.get_primitive_count(),false);
+		primitive_boxes.resize(m_primitive_manager.get_primitive_count(), false);
 
-		for (GUINT i = 0;i<primitive_boxes.size() ;i++ )
+		for (GUINT i = 0; i < primitive_boxes.size(); i++)
 		{
-			 m_primitive_manager.get_primitive_box(i,primitive_boxes[i].m_bound);
-			 primitive_boxes[i].m_data = i;
+			m_primitive_manager.get_primitive_box(i, primitive_boxes[i].m_bound);
+			primitive_boxes[i].m_data = i;
 		}
 
 		m_box_tree.build_tree(primitive_boxes);
 	}
 
 	//! returns the indices of the primitives in the m_primitive_manager
-	SIMD_FORCE_INLINE bool boxQuery(const GIM_AABB & box, gim_array<GUINT> & collided_results) const
+	SIMD_FORCE_INLINE bool boxQuery(const GIM_AABB& box, gim_array<GUINT>& collided_results) const
 	{
 		GUINT curIndex = 0;
 		GUINT numNodes = getNodeCount();
@@ -308,7 +284,7 @@ public:
 		while (curIndex < numNodes)
 		{
 			GIM_AABB bound;
-			getNodeBound(curIndex,bound);
+			getNodeBound(curIndex, bound);
 
 			//catch bugs in tree data
 
@@ -328,26 +304,26 @@ public:
 			else
 			{
 				//skip node
-				curIndex+= getScapeNodeIndex(curIndex);
+				curIndex += getScapeNodeIndex(curIndex);
 			}
 		}
-		if(collided_results.size()>0) return true;
+		if (collided_results.size() > 0) return true;
 		return false;
 	}
 
 	//! returns the indices of the primitives in the m_primitive_manager
-	SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB & box,
-		 const btTransform & transform, gim_array<GUINT> & collided_results) const
+	SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB& box,
+										 const btTransform& transform, gim_array<GUINT>& collided_results) const
 	{
-		GIM_AABB transbox=box;
+		GIM_AABB transbox = box;
 		transbox.appy_transform(transform);
-		return boxQuery(transbox,collided_results);
+		return boxQuery(transbox, collided_results);
 	}
 
 	//! returns the indices of the primitives in the m_primitive_manager
 	SIMD_FORCE_INLINE bool rayQuery(
-		const btVector3 & ray_dir,const btVector3 & ray_origin ,
-		gim_array<GUINT> & collided_results) const
+		const btVector3& ray_dir, const btVector3& ray_origin,
+		gim_array<GUINT>& collided_results) const
 	{
 		GUINT curIndex = 0;
 		GUINT numNodes = getNodeCount();
@@ -355,16 +331,16 @@ public:
 		while (curIndex < numNodes)
 		{
 			GIM_AABB bound;
-			getNodeBound(curIndex,bound);
+			getNodeBound(curIndex, bound);
 
 			//catch bugs in tree data
 
-			bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
+			bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir);
 			bool isleafnode = isLeafNode(curIndex);
 
 			if (isleafnode && aabbOverlap)
 			{
-				collided_results.push_back(getNodeData( curIndex));
+				collided_results.push_back(getNodeData(curIndex));
 			}
 
 			if (aabbOverlap || isleafnode)
@@ -375,10 +351,10 @@ public:
 			else
 			{
 				//skip node
-				curIndex+= getScapeNodeIndex(curIndex);
+				curIndex += getScapeNodeIndex(curIndex);
 			}
 		}
-		if(collided_results.size()>0) return true;
+		if (collided_results.size() > 0) return true;
 		return false;
 	}
 
@@ -389,7 +365,7 @@ public:
 	}
 
 	//! tells if this set is a trimesh
-	SIMD_FORCE_INLINE bool isTrimesh()  const
+	SIMD_FORCE_INLINE bool isTrimesh() const
 	{
 		return m_primitive_manager.is_trimesh();
 	}
@@ -411,12 +387,12 @@ public:
 		return m_box_tree.getNodeData(nodeindex);
 	}
 
-	SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound)  const
+	SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB& bound) const
 	{
 		m_box_tree.getNodeBound(nodeindex, bound);
 	}
 
-	SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound)
+	SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB& bound)
 	{
 		m_box_tree.setNodeBound(nodeindex, bound);
 	}
@@ -436,36 +412,30 @@ public:
 		return m_box_tree.getScapeNodeIndex(nodeindex);
 	}
 
-	SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex,GIM_TRIANGLE & triangle) const
+	SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex, GIM_TRIANGLE& triangle) const
 	{
-		m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex),triangle);
+		m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex), triangle);
 	}
-
 };
 
 //! Class for Box Tree Sets
 /*!
 this has the GIM_BOX_TREE implementation for bounding boxes.
 */
-template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE>
-class GIM_BOX_TREE_SET: public GIM_BOX_TREE_TEMPLATE_SET< _GIM_PRIMITIVE_MANAGER_PROTOTYPE, GIM_BOX_TREE>
+template <typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE>
+class GIM_BOX_TREE_SET : public GIM_BOX_TREE_TEMPLATE_SET<_GIM_PRIMITIVE_MANAGER_PROTOTYPE, GIM_BOX_TREE>
 {
 public:
-
 };
 
-
-
-
-
 /// GIM_BOX_SET collision methods
-template<typename BOX_SET_CLASS0,typename BOX_SET_CLASS1>
+template <typename BOX_SET_CLASS0, typename BOX_SET_CLASS1>
 class GIM_TREE_TREE_COLLIDER
 {
 public:
-	gim_pair_set * m_collision_pairs;
-	BOX_SET_CLASS0 * m_boxset0;
-	BOX_SET_CLASS1 * m_boxset1;
+	gim_pair_set* m_collision_pairs;
+	BOX_SET_CLASS0* m_boxset0;
+	BOX_SET_CLASS1* m_boxset1;
 	GUINT current_node0;
 	GUINT current_node1;
 	bool node0_is_leaf;
@@ -483,18 +453,18 @@ public:
 	GIM_TRIANGLE m_tri1;
 	btVector4 m_tri1_plane;
 
-
 public:
 	GIM_TREE_TREE_COLLIDER()
 	{
 		current_node0 = G_UINT_INFINITY;
 		current_node1 = G_UINT_INFINITY;
 	}
+
 protected:
 	SIMD_FORCE_INLINE void retrieve_node0_triangle(GUINT node0)
 	{
-		if(node0_has_triangle) return;
-		m_boxset0->getNodeTriangle(node0,m_tri0);
+		if (node0_has_triangle) return;
+		m_boxset0->getNodeTriangle(node0, m_tri0);
 		//transform triangle
 		m_tri0.m_vertices[0] = trans_cache_0to1(m_tri0.m_vertices[0]);
 		m_tri0.m_vertices[1] = trans_cache_0to1(m_tri0.m_vertices[1]);
@@ -506,8 +476,8 @@ protected:
 
 	SIMD_FORCE_INLINE void retrieve_node1_triangle(GUINT node1)
 	{
-		if(node1_has_triangle) return;
-		m_boxset1->getNodeTriangle(node1,m_tri1);
+		if (node1_has_triangle) return;
+		m_boxset1->getNodeTriangle(node1, m_tri1);
 		//transform triangle
 		m_tri1.m_vertices[0] = trans_cache_1to0.transform(m_tri1.m_vertices[0]);
 		m_tri1.m_vertices[1] = trans_cache_1to0.transform(m_tri1.m_vertices[1]);
@@ -519,8 +489,8 @@ protected:
 
 	SIMD_FORCE_INLINE void retrieve_node0_info(GUINT node0)
 	{
-		if(node0 == current_node0) return;
-		m_boxset0->getNodeBound(node0,m_box0);
+		if (node0 == current_node0) return;
+		m_boxset0->getNodeBound(node0, m_box0);
 		node0_is_leaf = m_boxset0->isLeafNode(node0);
 		node0_has_triangle = false;
 		current_node0 = node0;
@@ -528,21 +498,21 @@ protected:
 
 	SIMD_FORCE_INLINE void retrieve_node1_info(GUINT node1)
 	{
-		if(node1 == current_node1) return;
-		m_boxset1->getNodeBound(node1,m_box1);
+		if (node1 == current_node1) return;
+		m_boxset1->getNodeBound(node1, m_box1);
 		node1_is_leaf = m_boxset1->isLeafNode(node1);
 		node1_has_triangle = false;
 		current_node1 = node1;
 	}
 
-	SIMD_FORCE_INLINE bool node_collision(GUINT node0 ,GUINT node1)
+	SIMD_FORCE_INLINE bool node_collision(GUINT node0, GUINT node1)
 	{
 		retrieve_node0_info(node0);
 		retrieve_node1_info(node1);
-		bool result = m_box0.overlapping_trans_cache(m_box1,trans_cache_1to0,true);
-		if(!result) return false;
+		bool result = m_box0.overlapping_trans_cache(m_box1, trans_cache_1to0, true);
+		if (!result) return false;
 
-		if(t0_is_trimesh && node0_is_leaf)
+		if (t0_is_trimesh && node0_is_leaf)
 		{
 			//perform primitive vs box collision
 			retrieve_node0_triangle(node0);
@@ -550,14 +520,14 @@ protected:
 			m_box1.increment_margin(m_tri0.m_margin);
 
 			result = m_box1.collide_triangle_exact(
-				m_tri0.m_vertices[0],m_tri0.m_vertices[1],m_tri0.m_vertices[2],m_tri0_plane);
+				m_tri0.m_vertices[0], m_tri0.m_vertices[1], m_tri0.m_vertices[2], m_tri0_plane);
 
 			m_box1.increment_margin(-m_tri0.m_margin);
 
-			if(!result) return false;
+			if (!result) return false;
 			return true;
 		}
-		else if(t1_is_trimesh && node1_is_leaf)
+		else if (t1_is_trimesh && node1_is_leaf)
 		{
 			//perform primitive vs box collision
 			retrieve_node1_triangle(node1);
@@ -565,11 +535,11 @@ protected:
 			m_box0.increment_margin(m_tri1.m_margin);
 
 			result = m_box0.collide_triangle_exact(
-				m_tri1.m_vertices[0],m_tri1.m_vertices[1],m_tri1.m_vertices[2],m_tri1_plane);
+				m_tri1.m_vertices[0], m_tri1.m_vertices[1], m_tri1.m_vertices[2], m_tri1_plane);
 
 			m_box0.increment_margin(-m_tri1.m_margin);
 
-			if(!result) return false;
+			if (!result) return false;
 			return true;
 		}
 		return true;
@@ -582,40 +552,39 @@ protected:
 		stack_collisions.reserve(32);
 
 		//add the first pair
-		stack_collisions.push_pair(0,0);
+		stack_collisions.push_pair(0, 0);
 
-
-		while(stack_collisions.size())
+		while (stack_collisions.size())
 		{
 			//retrieve the last pair and pop
 			GUINT node0 = stack_collisions.back().m_index1;
 			GUINT node1 = stack_collisions.back().m_index2;
 			stack_collisions.pop_back();
-			if(node_collision(node0,node1)) // a collision is found
+			if (node_collision(node0, node1))  // a collision is found
 			{
-				if(node0_is_leaf)
+				if (node0_is_leaf)
 				{
-					if(node1_is_leaf)
+					if (node1_is_leaf)
 					{
-						m_collision_pairs->push_pair(m_boxset0->getNodeData(node0),m_boxset1->getNodeData(node1));
+						m_collision_pairs->push_pair(m_boxset0->getNodeData(node0), m_boxset1->getNodeData(node1));
 					}
 					else
 					{
 						//collide left
-						stack_collisions.push_pair(node0,m_boxset1->getLeftNodeIndex(node1));
+						stack_collisions.push_pair(node0, m_boxset1->getLeftNodeIndex(node1));
 
 						//collide right
-						stack_collisions.push_pair(node0,m_boxset1->getRightNodeIndex(node1));
+						stack_collisions.push_pair(node0, m_boxset1->getRightNodeIndex(node1));
 					}
 				}
 				else
 				{
-					if(node1_is_leaf)
+					if (node1_is_leaf)
 					{
 						//collide left
-						stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0),node1);
+						stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0), node1);
 						//collide right
-						stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0),node1);
+						stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0), node1);
 					}
 					else
 					{
@@ -624,36 +593,36 @@ protected:
 						GUINT left1 = m_boxset1->getLeftNodeIndex(node1);
 						GUINT right1 = m_boxset1->getRightNodeIndex(node1);
 						//collide left
-						stack_collisions.push_pair(left0,left1);
+						stack_collisions.push_pair(left0, left1);
 						//collide right
-						stack_collisions.push_pair(left0,right1);
+						stack_collisions.push_pair(left0, right1);
 						//collide left
-						stack_collisions.push_pair(right0,left1);
+						stack_collisions.push_pair(right0, left1);
 						//collide right
-						stack_collisions.push_pair(right0,right1);
+						stack_collisions.push_pair(right0, right1);
 
-					}// else if node1 is not a leaf
-				}// else if node0 is not a leaf
+					}  // else if node1 is not a leaf
+				}      // else if node0 is not a leaf
 
-			}// if(node_collision(node0,node1))
-		}//while(stack_collisions.size())
+			}  // if(node_collision(node0,node1))
+		}      //while(stack_collisions.size())
 	}
+
 public:
-	void find_collision(BOX_SET_CLASS0 * boxset1, const btTransform & trans1,
-		BOX_SET_CLASS1 * boxset2, const btTransform & trans2,
-		gim_pair_set & collision_pairs, bool complete_primitive_tests = true)
+	void find_collision(BOX_SET_CLASS0* boxset1, const btTransform& trans1,
+						BOX_SET_CLASS1* boxset2, const btTransform& trans2,
+						gim_pair_set& collision_pairs, bool complete_primitive_tests = true)
 	{
 		m_collision_pairs = &collision_pairs;
 		m_boxset0 = boxset1;
 		m_boxset1 = boxset2;
 
-		trans_cache_1to0.calc_from_homogenic(trans1,trans2);
+		trans_cache_1to0.calc_from_homogenic(trans1, trans2);
 
-		trans_cache_0to1 =  trans2.inverse();
+		trans_cache_0to1 = trans2.inverse();
 		trans_cache_0to1 *= trans1;
 
-
-		if(complete_primitive_tests)
+		if (complete_primitive_tests)
 		{
 			t0_is_trimesh = boxset1->getPrimitiveManager().is_trimesh();
 			t1_is_trimesh = boxset2->getPrimitiveManager().is_trimesh();
@@ -668,7 +637,4 @@ public:
 	}
 };
 
-
-#endif // GIM_BOXPRUNING_H_INCLUDED
-
-
+#endif  // GIM_BOXPRUNING_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h
index e342459ce56..57b9c5c91ad 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h
@@ -33,91 +33,86 @@ email: projectileman@yahoo.com
 -----------------------------------------------------------------------------
 */
 
-
 //! This function calcs the distance from a 3D plane
 class DISTANCE_PLANE_3D_FUNC
 {
 public:
-	template<typename CLASS_POINT,typename CLASS_PLANE>
-	inline GREAL operator()(const CLASS_PLANE & plane, const CLASS_POINT & point)
+	template <typename CLASS_POINT, typename CLASS_PLANE>
+	inline GREAL operator()(const CLASS_PLANE& plane, const CLASS_POINT& point)
 	{
 		return DISTANCE_PLANE_POINT(plane, point);
 	}
 };
 
-
-
-template<typename CLASS_POINT>
+template <typename CLASS_POINT>
 SIMD_FORCE_INLINE void PLANE_CLIP_POLYGON_COLLECT(
-						const CLASS_POINT & point0,
-						const CLASS_POINT & point1,
-						GREAL dist0,
-						GREAL dist1,
-						CLASS_POINT * clipped,
-						GUINT & clipped_count)
+	const CLASS_POINT& point0,
+	const CLASS_POINT& point1,
+	GREAL dist0,
+	GREAL dist1,
+	CLASS_POINT* clipped,
+	GUINT& clipped_count)
 {
-	GUINT _prevclassif = (dist0>G_EPSILON);
-	GUINT _classif = (dist1>G_EPSILON);
-	if(_classif!=_prevclassif)
+	GUINT _prevclassif = (dist0 > G_EPSILON);
+	GUINT _classif = (dist1 > G_EPSILON);
+	if (_classif != _prevclassif)
 	{
-		GREAL blendfactor = -dist0/(dist1-dist0);
-		VEC_BLEND(clipped[clipped_count],point0,point1,blendfactor);
+		GREAL blendfactor = -dist0 / (dist1 - dist0);
+		VEC_BLEND(clipped[clipped_count], point0, point1, blendfactor);
 		clipped_count++;
 	}
-	if(!_classif)
+	if (!_classif)
 	{
-		VEC_COPY(clipped[clipped_count],point1);
+		VEC_COPY(clipped[clipped_count], point1);
 		clipped_count++;
 	}
 }
 
-
 //! Clips a polygon by a plane
 /*!
 *\return The count of the clipped counts
 */
-template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
+template <typename CLASS_POINT, typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
 SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON_GENERIC(
-						const CLASS_PLANE & plane,
-						const CLASS_POINT * polygon_points,
-						GUINT polygon_point_count,
-						CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func)
+	const CLASS_PLANE& plane,
+	const CLASS_POINT* polygon_points,
+	GUINT polygon_point_count,
+	CLASS_POINT* clipped, DISTANCE_PLANE_FUNC distance_func)
 {
-    GUINT clipped_count = 0;
+	GUINT clipped_count = 0;
 
-
-    //clip first point
-	GREAL firstdist = distance_func(plane,polygon_points[0]);;
-	if(!(firstdist>G_EPSILON))
+	//clip first point
+	GREAL firstdist = distance_func(plane, polygon_points[0]);
+	;
+	if (!(firstdist > G_EPSILON))
 	{
-		VEC_COPY(clipped[clipped_count],polygon_points[0]);
+		VEC_COPY(clipped[clipped_count], polygon_points[0]);
 		clipped_count++;
 	}
 
 	GREAL olddist = firstdist;
-	for(GUINT _i=1;_i<polygon_point_count;_i++)
-	{		
-		GREAL dist = distance_func(plane,polygon_points[_i]);
+	for (GUINT _i = 1; _i < polygon_point_count; _i++)
+	{
+		GREAL dist = distance_func(plane, polygon_points[_i]);
 
 		PLANE_CLIP_POLYGON_COLLECT(
-						polygon_points[_i-1],polygon_points[_i],
-						olddist,
-						dist,
-						clipped,
-						clipped_count);
+			polygon_points[_i - 1], polygon_points[_i],
+			olddist,
+			dist,
+			clipped,
+			clipped_count);
 
-
-		olddist = dist;		
+		olddist = dist;
 	}
 
-	//RETURN TO FIRST  point	
+	//RETURN TO FIRST  point
 
 	PLANE_CLIP_POLYGON_COLLECT(
-					polygon_points[polygon_point_count-1],polygon_points[0],
-					olddist,
-					firstdist,
-					clipped,
-					clipped_count);
+		polygon_points[polygon_point_count - 1], polygon_points[0],
+		olddist,
+		firstdist,
+		clipped,
+		clipped_count);
 
 	return clipped_count;
 }
@@ -126,85 +121,79 @@ SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON_GENERIC(
 /*!
 *\return The count of the clipped counts
 */
-template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
+template <typename CLASS_POINT, typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
 SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE_GENERIC(
-						const CLASS_PLANE & plane,
-						const CLASS_POINT & point0,
-						const CLASS_POINT & point1,
-						const CLASS_POINT & point2,
-						CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func)
+	const CLASS_PLANE& plane,
+	const CLASS_POINT& point0,
+	const CLASS_POINT& point1,
+	const CLASS_POINT& point2,
+	CLASS_POINT* clipped, DISTANCE_PLANE_FUNC distance_func)
 {
-    GUINT clipped_count = 0;
+	GUINT clipped_count = 0;
 
-    //clip first point
-	GREAL firstdist = distance_func(plane,point0);;
-	if(!(firstdist>G_EPSILON))
+	//clip first point
+	GREAL firstdist = distance_func(plane, point0);
+	;
+	if (!(firstdist > G_EPSILON))
 	{
-		VEC_COPY(clipped[clipped_count],point0);
+		VEC_COPY(clipped[clipped_count], point0);
 		clipped_count++;
 	}
 
 	// point 1
 	GREAL olddist = firstdist;
-	GREAL dist = distance_func(plane,point1);
+	GREAL dist = distance_func(plane, point1);
 
 	PLANE_CLIP_POLYGON_COLLECT(
-					point0,point1,
-					olddist,
-					dist,
-					clipped,
-					clipped_count);
+		point0, point1,
+		olddist,
+		dist,
+		clipped,
+		clipped_count);
 
 	olddist = dist;
 
-
 	// point 2
-	dist = distance_func(plane,point2);
+	dist = distance_func(plane, point2);
 
 	PLANE_CLIP_POLYGON_COLLECT(
-					point1,point2,
-					olddist,
-					dist,
-					clipped,
-					clipped_count);
+		point1, point2,
+		olddist,
+		dist,
+		clipped,
+		clipped_count);
 	olddist = dist;
 
-
-
 	//RETURN TO FIRST  point
 	PLANE_CLIP_POLYGON_COLLECT(
-					point2,point0,
-					olddist,
-					firstdist,
-					clipped,
-					clipped_count);
+		point2, point0,
+		olddist,
+		firstdist,
+		clipped,
+		clipped_count);
 
 	return clipped_count;
 }
 
-
-template<typename CLASS_POINT,typename CLASS_PLANE>
+template <typename CLASS_POINT, typename CLASS_PLANE>
 SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON3D(
-						const CLASS_PLANE & plane,
-						const CLASS_POINT * polygon_points,
-						GUINT polygon_point_count,
-						CLASS_POINT * clipped)
+	const CLASS_PLANE& plane,
+	const CLASS_POINT* polygon_points,
+	GUINT polygon_point_count,
+	CLASS_POINT* clipped)
 {
-	return PLANE_CLIP_POLYGON_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,polygon_points,polygon_point_count,clipped,DISTANCE_PLANE_3D_FUNC());
+	return PLANE_CLIP_POLYGON_GENERIC<CLASS_POINT, CLASS_PLANE>(plane, polygon_points, polygon_point_count, clipped, DISTANCE_PLANE_3D_FUNC());
 }
 
-
-template<typename CLASS_POINT,typename CLASS_PLANE>
+template <typename CLASS_POINT, typename CLASS_PLANE>
 SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE3D(
-						const CLASS_PLANE & plane,
-						const CLASS_POINT & point0,
-						const CLASS_POINT & point1,
-						const CLASS_POINT & point2,
-						CLASS_POINT * clipped)
+	const CLASS_PLANE& plane,
+	const CLASS_POINT& point0,
+	const CLASS_POINT& point1,
+	const CLASS_POINT& point2,
+	CLASS_POINT* clipped)
 {
-	return PLANE_CLIP_TRIANGLE_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,point0,point1,point2,clipped,DISTANCE_PLANE_3D_FUNC());
+	return PLANE_CLIP_TRIANGLE_GENERIC<CLASS_POINT, CLASS_PLANE>(plane, point0, point1, point2, clipped, DISTANCE_PLANE_3D_FUNC());
 }
 
-
-
-#endif // GIM_TRI_COLLISION_H_INCLUDED
+#endif  // GIM_TRI_COLLISION_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.cpp b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.cpp
index 20e41de089f..390225709e1 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.cpp
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.cpp
@@ -33,91 +33,89 @@ email: projectileman@yahoo.com
 #define MAX_COINCIDENT 8
 
 void gim_contact_array::merge_contacts(
-	const gim_contact_array & contacts, bool normal_contact_average)
+	const gim_contact_array& contacts, bool normal_contact_average)
 {
 	clear();
 
-	if(contacts.size()==1)
+	if (contacts.size() == 1)
 	{
 		push_back(contacts.back());
 		return;
 	}
 
 	gim_array<GIM_RSORT_TOKEN> keycontacts(contacts.size());
-	keycontacts.resize(contacts.size(),false);
+	keycontacts.resize(contacts.size(), false);
 
 	//fill key contacts
 
 	GUINT i;
 
-	for (i = 0;i<contacts.size() ;i++ )
+	for (i = 0; i < contacts.size(); i++)
 	{
 		keycontacts[i].m_key = contacts[i].calc_key_contact();
 		keycontacts[i].m_value = i;
 	}
 
 	//sort keys
-	gim_heap_sort(keycontacts.pointer(),keycontacts.size(),GIM_RSORT_TOKEN_COMPARATOR());
+	gim_heap_sort(keycontacts.pointer(), keycontacts.size(), GIM_RSORT_TOKEN_COMPARATOR());
 
 	// Merge contacts
 
-	GUINT coincident_count=0;
+	GUINT coincident_count = 0;
 	btVector3 coincident_normals[MAX_COINCIDENT];
 
 	GUINT last_key = keycontacts[0].m_key;
 	GUINT key = 0;
 
 	push_back(contacts[keycontacts[0].m_value]);
-	GIM_CONTACT * pcontact = &back();
+	GIM_CONTACT* pcontact = &back();
 
-
-
-	for( i=1;i<keycontacts.size();i++)
+	for (i = 1; i < keycontacts.size(); i++)
 	{
-	    key = keycontacts[i].m_key;
-		const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];
+		key = keycontacts[i].m_key;
+		const GIM_CONTACT* scontact = &contacts[keycontacts[i].m_value];
 
-		if(last_key ==  key)//same points
+		if (last_key == key)  //same points
 		{
 			//merge contact
-			if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
+			if (pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)  //)
 			{
 				*pcontact = *scontact;
-                coincident_count = 0;
+				coincident_count = 0;
 			}
-			else if(normal_contact_average)
+			else if (normal_contact_average)
 			{
-				if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
-                {
-                    if(coincident_count<MAX_COINCIDENT)
-                    {
-                    	coincident_normals[coincident_count] = scontact->m_normal;
-                        coincident_count++;
-                    }
-                }
+				if (btFabs(pcontact->m_depth - scontact->m_depth) < CONTACT_DIFF_EPSILON)
+				{
+					if (coincident_count < MAX_COINCIDENT)
+					{
+						coincident_normals[coincident_count] = scontact->m_normal;
+						coincident_count++;
+					}
+				}
 			}
 		}
 		else
-		{//add new contact
+		{  //add new contact
 
-		    if(normal_contact_average && coincident_count>0)
-		    {
-		    	pcontact->interpolate_normals(coincident_normals,coincident_count);
-		        coincident_count = 0;
-		    }
+			if (normal_contact_average && coincident_count > 0)
+			{
+				pcontact->interpolate_normals(coincident_normals, coincident_count);
+				coincident_count = 0;
+			}
 
-		    push_back(*scontact);
-		    pcontact = &back();
-        }
+			push_back(*scontact);
+			pcontact = &back();
+		}
 		last_key = key;
 	}
 }
 
-void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts)
+void gim_contact_array::merge_contacts_unique(const gim_contact_array& contacts)
 {
 	clear();
 
-	if(contacts.size()==1)
+	if (contacts.size() == 1)
 	{
 		push_back(contacts.back());
 		return;
@@ -125,14 +123,14 @@ void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts
 
 	GIM_CONTACT average_contact = contacts.back();
 
-	for (GUINT i=1;i<contacts.size() ;i++ )
+	for (GUINT i = 1; i < contacts.size(); i++)
 	{
 		average_contact.m_point += contacts[i].m_point;
 		average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
 	}
 
 	//divide
-	GREAL divide_average = 1.0f/((GREAL)contacts.size());
+	GREAL divide_average = 1.0f / ((GREAL)contacts.size());
 
 	average_contact.m_point *= divide_average;
 
@@ -141,6 +139,4 @@ void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts
 	average_contact.m_depth = average_contact.m_normal.length();
 
 	average_contact.m_normal /= average_contact.m_depth;
-
 }
-
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h
index 5d9f8ef8180..9deb28a26eb 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h
@@ -36,12 +36,18 @@ email: projectileman@yahoo.com
 #include "gim_radixsort.h"
 #include "gim_array.h"
 
-
 /**
 Configuration var for applying interpolation of  contact normals
 */
+#ifndef NORMAL_CONTACT_AVERAGE
 #define NORMAL_CONTACT_AVERAGE 1
+#endif
+
+#ifndef CONTACT_DIFF_EPSILON
 #define CONTACT_DIFF_EPSILON 0.00001f
+#endif
+
+#ifndef BT_CONTACT_H_STRUCTS_INCLUDED
 
 /// Structure for collision results
 ///Functions for managing and sorting contacts resulting from a collision query.
@@ -51,89 +57,87 @@ Configuration var for applying interpolation of  contact normals
 class GIM_CONTACT
 {
 public:
-    btVector3 m_point;
-    btVector3 m_normal;
-    GREAL m_depth;//Positive value indicates interpenetration
-    GREAL m_distance;//Padding not for use
-    GUINT m_feature1;//Face number
-    GUINT m_feature2;//Face number
+	btVector3 m_point;
+	btVector3 m_normal;
+	GREAL m_depth;     //Positive value indicates interpenetration
+	GREAL m_distance;  //Padding not for use
+	GUINT m_feature1;  //Face number
+	GUINT m_feature2;  //Face number
 public:
-    GIM_CONTACT()
-    {
-    }
-
-    GIM_CONTACT(const GIM_CONTACT & contact):
-				m_point(contact.m_point),
-				m_normal(contact.m_normal),
-				m_depth(contact.m_depth),
-				m_feature1(contact.m_feature1),
-				m_feature2(contact.m_feature2)
-    {
-    	m_point = contact.m_point;
-    	m_normal = contact.m_normal;
-    	m_depth = contact.m_depth;
-    	m_feature1 = contact.m_feature1;
-    	m_feature2 = contact.m_feature2;
-    }
-
-    GIM_CONTACT(const btVector3 &point,const btVector3 & normal,
-    	 			GREAL depth, GUINT feature1, GUINT feature2):
-				m_point(point),
-				m_normal(normal),
-				m_depth(depth),
-				m_feature1(feature1),
-				m_feature2(feature2)
-    {
-    }
+	GIM_CONTACT()
+	{
+	}
+
+	GIM_CONTACT(const GIM_CONTACT &contact) : m_point(contact.m_point),
+											  m_normal(contact.m_normal),
+											  m_depth(contact.m_depth),
+											  m_feature1(contact.m_feature1),
+											  m_feature2(contact.m_feature2)
+	{
+		m_point = contact.m_point;
+		m_normal = contact.m_normal;
+		m_depth = contact.m_depth;
+		m_feature1 = contact.m_feature1;
+		m_feature2 = contact.m_feature2;
+	}
+
+	GIM_CONTACT(const btVector3 &point, const btVector3 &normal,
+				GREAL depth, GUINT feature1, GUINT feature2) : m_point(point),
+															   m_normal(normal),
+															   m_depth(depth),
+															   m_feature1(feature1),
+															   m_feature2(feature2)
+	{
+	}
 
 	//! Calcs key for coord classification
-    SIMD_FORCE_INLINE GUINT calc_key_contact() const
-    {
-    	GINT _coords[] = {
-    		(GINT)(m_point[0]*1000.0f+1.0f),
-    		(GINT)(m_point[1]*1333.0f),
-    		(GINT)(m_point[2]*2133.0f+3.0f)};
-		GUINT _hash=0;
+	SIMD_FORCE_INLINE GUINT calc_key_contact() const
+	{
+		GINT _coords[] = {
+			(GINT)(m_point[0] * 1000.0f + 1.0f),
+			(GINT)(m_point[1] * 1333.0f),
+			(GINT)(m_point[2] * 2133.0f + 3.0f)};
+		GUINT _hash = 0;
 		GUINT *_uitmp = (GUINT *)(&_coords[0]);
 		_hash = *_uitmp;
 		_uitmp++;
-		_hash += (*_uitmp)<<4;
+		_hash += (*_uitmp) << 4;
 		_uitmp++;
-		_hash += (*_uitmp)<<8;
+		_hash += (*_uitmp) << 8;
 		return _hash;
-    }
+	}
 
-    SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,GUINT normal_count)
-    {
-    	btVector3 vec_sum(m_normal);
-		for(GUINT i=0;i<normal_count;i++)
+	SIMD_FORCE_INLINE void interpolate_normals(btVector3 *normals, GUINT normal_count)
+	{
+		btVector3 vec_sum(m_normal);
+		for (GUINT i = 0; i < normal_count; i++)
 		{
 			vec_sum += normals[i];
 		}
 
 		GREAL vec_sum_len = vec_sum.length2();
-		if(vec_sum_len <CONTACT_DIFF_EPSILON) return;
+		if (vec_sum_len < CONTACT_DIFF_EPSILON) return;
 
-		GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len)
-
-		m_normal = vec_sum*vec_sum_len;
-    }
+		GIM_INV_SQRT(vec_sum_len, vec_sum_len);  // 1/sqrt(vec_sum_len)
 
+		m_normal = vec_sum * vec_sum_len;
+	}
 };
 
+#endif
 
-class gim_contact_array:public gim_array<GIM_CONTACT>
+class gim_contact_array : public gim_array<GIM_CONTACT>
 {
 public:
-	gim_contact_array():gim_array<GIM_CONTACT>(64)
+	gim_contact_array() : gim_array<GIM_CONTACT>(64)
 	{
 	}
 
-	SIMD_FORCE_INLINE void push_contact(const btVector3 &point,const btVector3 & normal,
-    	 			GREAL depth, GUINT feature1, GUINT feature2)
+	SIMD_FORCE_INLINE void push_contact(const btVector3 &point, const btVector3 &normal,
+										GREAL depth, GUINT feature1, GUINT feature2)
 	{
 		push_back_mem();
-		GIM_CONTACT & newele = back();
+		GIM_CONTACT &newele = back();
 		newele.m_point = point;
 		newele.m_normal = normal;
 		newele.m_depth = depth;
@@ -142,13 +146,13 @@ public:
 	}
 
 	SIMD_FORCE_INLINE void push_triangle_contacts(
-		const GIM_TRIANGLE_CONTACT_DATA & tricontact,
-		GUINT feature1,GUINT feature2)
+		const GIM_TRIANGLE_CONTACT_DATA &tricontact,
+		GUINT feature1, GUINT feature2)
 	{
-		for(GUINT i = 0;i<tricontact.m_point_count ;i++ )
+		for (GUINT i = 0; i < tricontact.m_point_count; i++)
 		{
 			push_back_mem();
-			GIM_CONTACT & newele = back();
+			GIM_CONTACT &newele = back();
 			newele.m_point = tricontact.m_points[i];
 			newele.m_normal = tricontact.m_separating_normal;
 			newele.m_depth = tricontact.m_penetration_depth;
@@ -157,8 +161,8 @@ public:
 		}
 	}
 
-	void merge_contacts(const gim_contact_array & contacts, bool normal_contact_average = true);
-	void merge_contacts_unique(const gim_contact_array & contacts);
+	void merge_contacts(const gim_contact_array &contacts, bool normal_contact_average = true);
+	void merge_contacts_unique(const gim_contact_array &contacts);
 };
 
-#endif // GIM_CONTACT_H_INCLUDED
+#endif  // GIM_CONTACT_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h
index 6b8f9ea6c21..9dc48f354b9 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h
@@ -33,11 +33,8 @@ email: projectileman@yahoo.com
 -----------------------------------------------------------------------------
 */
 
-
 #include "gim_math.h"
 
-
-
 //! Short Integer vector 2D
 typedef GSHORT vec2s[2];
 //! Integer vector 3D
@@ -92,6 +89,4 @@ typedef GREAL quatf[4];
 
 //typedef struct _aabb3f aabb3f;
 
-
-
-#endif // GIM_GEOM_TYPES_H_INCLUDED
+#endif  // GIM_GEOM_TYPES_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_geometry.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_geometry.h
index c67a6991c02..4a7ac3c4d80 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_geometry.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_geometry.h
@@ -39,4 +39,4 @@ email: projectileman@yahoo.com
 #include "gim_box_collision.h"
 #include "gim_tri_collision.h"
 
-#endif // GIM_VECTOR_H_INCLUDED
+#endif  // GIM_VECTOR_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h
index e4237c2c570..abf88d3108d 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h
@@ -34,53 +34,52 @@ email: projectileman@yahoo.com
 
 #include "gim_radixsort.h"
 
-
-#define GIM_INVALID_HASH 0xffffffff //!< A very very high value
+#define GIM_INVALID_HASH 0xffffffff  //!< A very very high value
 #define GIM_DEFAULT_HASH_TABLE_SIZE 380
 #define GIM_DEFAULT_HASH_TABLE_NODE_SIZE 4
 #define GIM_HASH_TABLE_GROW_FACTOR 2
 
-#define GIM_MIN_RADIX_SORT_SIZE 860 //!< calibrated on a PIII
+#define GIM_MIN_RADIX_SORT_SIZE 860  //!< calibrated on a PIII
 
-template<typename T>
+template <typename T>
 struct GIM_HASH_TABLE_NODE
 {
-    GUINT m_key;
-    T m_data;
-    GIM_HASH_TABLE_NODE()
-    {
-    }
-
-    GIM_HASH_TABLE_NODE(const GIM_HASH_TABLE_NODE & value)
-    {
-        m_key = value.m_key;
-        m_data = value.m_data;
-    }
-
-    GIM_HASH_TABLE_NODE(GUINT key, const T & data)
-    {
-        m_key = key;
-        m_data = data;
-    }
-
-    bool operator <(const GIM_HASH_TABLE_NODE<T> & other) const
+	GUINT m_key;
+	T m_data;
+	GIM_HASH_TABLE_NODE()
+	{
+	}
+
+	GIM_HASH_TABLE_NODE(const GIM_HASH_TABLE_NODE& value)
+	{
+		m_key = value.m_key;
+		m_data = value.m_data;
+	}
+
+	GIM_HASH_TABLE_NODE(GUINT key, const T& data)
+	{
+		m_key = key;
+		m_data = data;
+	}
+
+	bool operator<(const GIM_HASH_TABLE_NODE<T>& other) const
 	{
 		///inverse order, further objects are first
-		if(m_key <  other.m_key) return true;
+		if (m_key < other.m_key) return true;
 		return false;
 	}
 
-	bool operator >(const GIM_HASH_TABLE_NODE<T> & other) const
+	bool operator>(const GIM_HASH_TABLE_NODE<T>& other) const
 	{
 		///inverse order, further objects are first
-		if(m_key >  other.m_key) return true;
+		if (m_key > other.m_key) return true;
 		return false;
 	}
 
-	bool operator ==(const GIM_HASH_TABLE_NODE<T> & other) const
+	bool operator==(const GIM_HASH_TABLE_NODE<T>& other) const
 	{
 		///inverse order, further objects are first
-		if(m_key ==  other.m_key) return true;
+		if (m_key == other.m_key) return true;
 		return false;
 	}
 };
@@ -89,21 +88,19 @@ struct GIM_HASH_TABLE_NODE
 class GIM_HASH_NODE_GET_KEY
 {
 public:
-	template<class T>
-	inline GUINT operator()( const T& a)
+	template <class T>
+	inline GUINT operator()(const T& a)
 	{
 		return a.m_key;
 	}
 };
 
-
-
 ///Macro for comparing the key and the element
 class GIM_HASH_NODE_CMP_KEY_MACRO
 {
 public:
-	template<class T>
-	inline int operator() ( const T& a, GUINT key)
+	template <class T>
+	inline int operator()(const T& a, GUINT key)
 	{
 		return ((int)(a.m_key - key));
 	}
@@ -113,65 +110,53 @@ public:
 class GIM_HASH_NODE_CMP_MACRO
 {
 public:
-	template<class T>
-	inline int operator() ( const T& a, const T& b )
+	template <class T>
+	inline int operator()(const T& a, const T& b)
 	{
 		return ((int)(a.m_key - b.m_key));
 	}
 };
 
-
-
-
-
 //! Sorting for hash table
 /*!
 switch automatically between quicksort and radixsort
 */
-template<typename T>
-void gim_sort_hash_node_array(T * array, GUINT array_count)
+template <typename T>
+void gim_sort_hash_node_array(T* array, GUINT array_count)
 {
-    if(array_count<GIM_MIN_RADIX_SORT_SIZE)
-    {
-    	gim_heap_sort(array,array_count,GIM_HASH_NODE_CMP_MACRO());
-    }
-    else
-    {
-    	memcopy_elements_func cmpfunc;
-    	gim_radix_sort(array,array_count,GIM_HASH_NODE_GET_KEY(),cmpfunc);
-    }
+	if (array_count < GIM_MIN_RADIX_SORT_SIZE)
+	{
+		gim_heap_sort(array, array_count, GIM_HASH_NODE_CMP_MACRO());
+	}
+	else
+	{
+		memcopy_elements_func cmpfunc;
+		gim_radix_sort(array, array_count, GIM_HASH_NODE_GET_KEY(), cmpfunc);
+	}
 }
 
-
-
-
-
-
 // Note: assumes long is at least 32 bits.
 #define GIM_NUM_PRIME 28
 
 static const GUINT gim_prime_list[GIM_NUM_PRIME] =
-{
-  53ul,         97ul,         193ul,       389ul,       769ul,
-  1543ul,       3079ul,       6151ul,      12289ul,     24593ul,
-  49157ul,      98317ul,      196613ul,    393241ul,    786433ul,
-  1572869ul,    3145739ul,    6291469ul,   12582917ul,  25165843ul,
-  50331653ul,   100663319ul,  201326611ul, 402653189ul, 805306457ul,
-  1610612741ul, 3221225473ul, 4294967291ul
-};
+	{
+		53ul, 97ul, 193ul, 389ul, 769ul,
+		1543ul, 3079ul, 6151ul, 12289ul, 24593ul,
+		49157ul, 98317ul, 196613ul, 393241ul, 786433ul,
+		1572869ul, 3145739ul, 6291469ul, 12582917ul, 25165843ul,
+		50331653ul, 100663319ul, 201326611ul, 402653189ul, 805306457ul,
+		1610612741ul, 3221225473ul, 4294967291ul};
 
 inline GUINT gim_next_prime(GUINT number)
 {
-    //Find nearest upper prime
-    GUINT result_ind = 0;
-    gim_binary_search(gim_prime_list,0,(GIM_NUM_PRIME-2),number,result_ind);
+	//Find nearest upper prime
+	GUINT result_ind = 0;
+	gim_binary_search(gim_prime_list, 0, (GIM_NUM_PRIME - 2), number, result_ind);
 
-    // inv: result_ind < 28
-    return gim_prime_list[result_ind];
+	// inv: result_ind < 28
+	return gim_prime_list[result_ind];
 }
 
-
-
 //! A compact hash table implementation
 /*!
 A memory aligned compact hash table that coud be treated as an array.
@@ -187,129 +172,124 @@ When the array size reaches the size equivalent to 'min_hash_table_size', then i
 </ul>
 
 */
-template<class T>
+template <class T>
 class gim_hash_table
 {
 protected:
-    typedef GIM_HASH_TABLE_NODE<T> _node_type;
-
-    //!The nodes
-    //array< _node_type, SuperAllocator<_node_type> > m_nodes;
-    gim_array< _node_type > m_nodes;
-    //SuperBufferedArray< _node_type > m_nodes;
-    bool m_sorted;
-
-    ///Hash table data management. The hash table has the indices to the corresponding m_nodes array
-    GUINT * m_hash_table;//!<
-    GUINT m_table_size;//!<
-    GUINT m_node_size;//!<
-    GUINT m_min_hash_table_size;
-
-
-
-    //! Returns the cell index
-    inline GUINT _find_cell(GUINT hashkey)
-    {
-        _node_type * nodesptr = m_nodes.pointer();
-        GUINT start_index = (hashkey%m_table_size)*m_node_size;
-        GUINT end_index = start_index + m_node_size;
-
-        while(start_index<end_index)
-        {
-            GUINT value = m_hash_table[start_index];
-            if(value != GIM_INVALID_HASH)
-            {
-                if(nodesptr[value].m_key == hashkey) return start_index;
-            }
-            start_index++;
-        }
-        return GIM_INVALID_HASH;
-    }
-
-    //! Find the avaliable cell for the hashkey, and return an existing cell if it has the same hash key
-    inline GUINT _find_avaliable_cell(GUINT hashkey)
-    {
-        _node_type * nodesptr = m_nodes.pointer();
-        GUINT avaliable_index = GIM_INVALID_HASH;
-        GUINT start_index = (hashkey%m_table_size)*m_node_size;
-        GUINT end_index = start_index + m_node_size;
-
-        while(start_index<end_index)
-        {
-            GUINT value = m_hash_table[start_index];
-            if(value == GIM_INVALID_HASH)
-            {
-                if(avaliable_index==GIM_INVALID_HASH)
-                {
-                    avaliable_index = start_index;
-                }
-            }
-            else if(nodesptr[value].m_key == hashkey)
-            {
-                return start_index;
-            }
-            start_index++;
-        }
-        return avaliable_index;
-    }
-
-
-
-    //! reserves the memory for the hash table.
-    /*!
+	typedef GIM_HASH_TABLE_NODE<T> _node_type;
+
+	//!The nodes
+	//array< _node_type, SuperAllocator<_node_type> > m_nodes;
+	gim_array<_node_type> m_nodes;
+	//SuperBufferedArray< _node_type > m_nodes;
+	bool m_sorted;
+
+	///Hash table data management. The hash table has the indices to the corresponding m_nodes array
+	GUINT* m_hash_table;  //!<
+	GUINT m_table_size;   //!<
+	GUINT m_node_size;    //!<
+	GUINT m_min_hash_table_size;
+
+	//! Returns the cell index
+	inline GUINT _find_cell(GUINT hashkey)
+	{
+		_node_type* nodesptr = m_nodes.pointer();
+		GUINT start_index = (hashkey % m_table_size) * m_node_size;
+		GUINT end_index = start_index + m_node_size;
+
+		while (start_index < end_index)
+		{
+			GUINT value = m_hash_table[start_index];
+			if (value != GIM_INVALID_HASH)
+			{
+				if (nodesptr[value].m_key == hashkey) return start_index;
+			}
+			start_index++;
+		}
+		return GIM_INVALID_HASH;
+	}
+
+	//! Find the avaliable cell for the hashkey, and return an existing cell if it has the same hash key
+	inline GUINT _find_avaliable_cell(GUINT hashkey)
+	{
+		_node_type* nodesptr = m_nodes.pointer();
+		GUINT avaliable_index = GIM_INVALID_HASH;
+		GUINT start_index = (hashkey % m_table_size) * m_node_size;
+		GUINT end_index = start_index + m_node_size;
+
+		while (start_index < end_index)
+		{
+			GUINT value = m_hash_table[start_index];
+			if (value == GIM_INVALID_HASH)
+			{
+				if (avaliable_index == GIM_INVALID_HASH)
+				{
+					avaliable_index = start_index;
+				}
+			}
+			else if (nodesptr[value].m_key == hashkey)
+			{
+				return start_index;
+			}
+			start_index++;
+		}
+		return avaliable_index;
+	}
+
+	//! reserves the memory for the hash table.
+	/*!
     \pre hash table must be empty
     \post reserves the memory for the hash table, an initializes all elements to GIM_INVALID_HASH.
     */
-    inline void _reserve_table_memory(GUINT newtablesize)
-    {
-        if(newtablesize==0) return;
-        if(m_node_size==0) return;
-
-        //Get a Prime size
-
-        m_table_size = gim_next_prime(newtablesize);
-
-        GUINT datasize = m_table_size*m_node_size;
-        //Alloc the data buffer
-        m_hash_table =  (GUINT *)gim_alloc(datasize*sizeof(GUINT));
-    }
-
-    inline void _invalidate_keys()
-    {
-        GUINT datasize = m_table_size*m_node_size;
-        for(GUINT i=0;i<datasize;i++)
-        {
-            m_hash_table[i] = GIM_INVALID_HASH;// invalidate keys
-        }
-    }
-
-    //! Clear all memory for the hash table
-    inline void _clear_table_memory()
-    {
-        if(m_hash_table==NULL) return;
-        gim_free(m_hash_table);
-        m_hash_table = NULL;
-        m_table_size = 0;
-    }
-
-    //! Invalidates the keys (Assigning GIM_INVALID_HASH to all) Reorders the hash keys
-    inline void _rehash()
-    {
-        _invalidate_keys();
-
-        _node_type * nodesptr = m_nodes.pointer();
-        for(GUINT i=0;i<(GUINT)m_nodes.size();i++)
-        {
-            GUINT nodekey = nodesptr[i].m_key;
-            if(nodekey != GIM_INVALID_HASH)
-            {
-                //Search for the avaliable cell in buffer
-                GUINT index = _find_avaliable_cell(nodekey);
-
-
-				if(m_hash_table[index]!=GIM_INVALID_HASH)
-				{//The new index is alreade used... discard this new incomming object, repeated key
-				    btAssert(m_hash_table[index]==nodekey);
+	inline void _reserve_table_memory(GUINT newtablesize)
+	{
+		if (newtablesize == 0) return;
+		if (m_node_size == 0) return;
+
+		//Get a Prime size
+
+		m_table_size = gim_next_prime(newtablesize);
+
+		GUINT datasize = m_table_size * m_node_size;
+		//Alloc the data buffer
+		m_hash_table = (GUINT*)gim_alloc(datasize * sizeof(GUINT));
+	}
+
+	inline void _invalidate_keys()
+	{
+		GUINT datasize = m_table_size * m_node_size;
+		for (GUINT i = 0; i < datasize; i++)
+		{
+			m_hash_table[i] = GIM_INVALID_HASH;  // invalidate keys
+		}
+	}
+
+	//! Clear all memory for the hash table
+	inline void _clear_table_memory()
+	{
+		if (m_hash_table == NULL) return;
+		gim_free(m_hash_table);
+		m_hash_table = NULL;
+		m_table_size = 0;
+	}
+
+	//! Invalidates the keys (Assigning GIM_INVALID_HASH to all) Reorders the hash keys
+	inline void _rehash()
+	{
+		_invalidate_keys();
+
+		_node_type* nodesptr = m_nodes.pointer();
+		for (GUINT i = 0; i < (GUINT)m_nodes.size(); i++)
+		{
+			GUINT nodekey = nodesptr[i].m_key;
+			if (nodekey != GIM_INVALID_HASH)
+			{
+				//Search for the avaliable cell in buffer
+				GUINT index = _find_avaliable_cell(nodekey);
+
+				if (m_hash_table[index] != GIM_INVALID_HASH)
+				{  //The new index is alreade used... discard this new incomming object, repeated key
+					btAssert(m_hash_table[index] == nodekey);
 					nodesptr[i].m_key = GIM_INVALID_HASH;
 				}
 				else
@@ -318,585 +298,560 @@ protected:
 					//Assign the value for alloc
 					m_hash_table[index] = i;
 				}
-            }
-        }
-    }
-
-    //! Resize hash table indices
-    inline void _resize_table(GUINT newsize)
-    {
-        //Clear memory
-        _clear_table_memory();
-        //Alloc the data
-        _reserve_table_memory(newsize);
-        //Invalidate keys and rehash
-        _rehash();
-    }
-
-    //! Destroy hash table memory
-    inline void _destroy()
-    {
-        if(m_hash_table==NULL) return;
-        _clear_table_memory();
-    }
-
-    //! Finds an avaliable hash table cell, and resizes the table if there isn't space
-    inline GUINT _assign_hash_table_cell(GUINT hashkey)
-    {
-        GUINT cell_index = _find_avaliable_cell(hashkey);
-
-        if(cell_index==GIM_INVALID_HASH)
-        {
-            //rehashing
-            _resize_table(m_table_size+1);
-            GUINT cell_index = _find_avaliable_cell(hashkey);
-            btAssert(cell_index!=GIM_INVALID_HASH);
-        }
-        return cell_index;
-    }
-
-    //! erase by index in hash table
-    inline bool _erase_by_index_hash_table(GUINT index)
-    {
-        if(index >= m_nodes.size()) return false;
-        if(m_nodes[index].m_key != GIM_INVALID_HASH)
-        {
-            //Search for the avaliable cell in buffer
-            GUINT cell_index = _find_cell(m_nodes[index].m_key);
-
-            btAssert(cell_index!=GIM_INVALID_HASH);
-            btAssert(m_hash_table[cell_index]==index);
-
-            m_hash_table[cell_index] = GIM_INVALID_HASH;
-        }
-
-        return this->_erase_unsorted(index);
-    }
-
-    //! erase by key in hash table
-    inline bool _erase_hash_table(GUINT hashkey)
-    {
-        if(hashkey == GIM_INVALID_HASH) return false;
-
-        //Search for the avaliable cell in buffer
-        GUINT cell_index = _find_cell(hashkey);
-        if(cell_index ==GIM_INVALID_HASH) return false;
-
-        GUINT index = m_hash_table[cell_index];
-        m_hash_table[cell_index] = GIM_INVALID_HASH;
-
-        return this->_erase_unsorted(index);
-    }
-
-
-
-    //! insert an element in hash table
-    /*!
+			}
+		}
+	}
+
+	//! Resize hash table indices
+	inline void _resize_table(GUINT newsize)
+	{
+		//Clear memory
+		_clear_table_memory();
+		//Alloc the data
+		_reserve_table_memory(newsize);
+		//Invalidate keys and rehash
+		_rehash();
+	}
+
+	//! Destroy hash table memory
+	inline void _destroy()
+	{
+		if (m_hash_table == NULL) return;
+		_clear_table_memory();
+	}
+
+	//! Finds an avaliable hash table cell, and resizes the table if there isn't space
+	inline GUINT _assign_hash_table_cell(GUINT hashkey)
+	{
+		GUINT cell_index = _find_avaliable_cell(hashkey);
+
+		if (cell_index == GIM_INVALID_HASH)
+		{
+			//rehashing
+			_resize_table(m_table_size + 1);
+			GUINT cell_index = _find_avaliable_cell(hashkey);
+			btAssert(cell_index != GIM_INVALID_HASH);
+		}
+		return cell_index;
+	}
+
+	//! erase by index in hash table
+	inline bool _erase_by_index_hash_table(GUINT index)
+	{
+		if (index >= m_nodes.size()) return false;
+		if (m_nodes[index].m_key != GIM_INVALID_HASH)
+		{
+			//Search for the avaliable cell in buffer
+			GUINT cell_index = _find_cell(m_nodes[index].m_key);
+
+			btAssert(cell_index != GIM_INVALID_HASH);
+			btAssert(m_hash_table[cell_index] == index);
+
+			m_hash_table[cell_index] = GIM_INVALID_HASH;
+		}
+
+		return this->_erase_unsorted(index);
+	}
+
+	//! erase by key in hash table
+	inline bool _erase_hash_table(GUINT hashkey)
+	{
+		if (hashkey == GIM_INVALID_HASH) return false;
+
+		//Search for the avaliable cell in buffer
+		GUINT cell_index = _find_cell(hashkey);
+		if (cell_index == GIM_INVALID_HASH) return false;
+
+		GUINT index = m_hash_table[cell_index];
+		m_hash_table[cell_index] = GIM_INVALID_HASH;
+
+		return this->_erase_unsorted(index);
+	}
+
+	//! insert an element in hash table
+	/*!
     If the element exists, this won't insert the element
     \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted
     If so, the element has been inserted at the last position of the array.
     */
-    inline GUINT _insert_hash_table(GUINT hashkey, const T & value)
-    {
-        if(hashkey==GIM_INVALID_HASH)
-        {
-            //Insert anyway
-            _insert_unsorted(hashkey,value);
-            return GIM_INVALID_HASH;
-        }
+	inline GUINT _insert_hash_table(GUINT hashkey, const T& value)
+	{
+		if (hashkey == GIM_INVALID_HASH)
+		{
+			//Insert anyway
+			_insert_unsorted(hashkey, value);
+			return GIM_INVALID_HASH;
+		}
 
-        GUINT cell_index = _assign_hash_table_cell(hashkey);
+		GUINT cell_index = _assign_hash_table_cell(hashkey);
 
-        GUINT value_key = m_hash_table[cell_index];
+		GUINT value_key = m_hash_table[cell_index];
 
-        if(value_key!= GIM_INVALID_HASH) return value_key;// Not overrited
+		if (value_key != GIM_INVALID_HASH) return value_key;  // Not overrited
 
-        m_hash_table[cell_index] = m_nodes.size();
+		m_hash_table[cell_index] = m_nodes.size();
 
-        _insert_unsorted(hashkey,value);
-        return GIM_INVALID_HASH;
-    }
+		_insert_unsorted(hashkey, value);
+		return GIM_INVALID_HASH;
+	}
 
-    //! insert an element in hash table.
-    /*!
+	//! insert an element in hash table.
+	/*!
     If the element exists, this replaces the element.
     \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted
     If so, the element has been inserted at the last position of the array.
     */
-    inline GUINT _insert_hash_table_replace(GUINT hashkey, const T & value)
-    {
-        if(hashkey==GIM_INVALID_HASH)
-        {
-            //Insert anyway
-            _insert_unsorted(hashkey,value);
-            return GIM_INVALID_HASH;
-        }
-
-        GUINT cell_index = _assign_hash_table_cell(hashkey);
-
-        GUINT value_key = m_hash_table[cell_index];
-
-        if(value_key!= GIM_INVALID_HASH)
-        {//replaces the existing
-            m_nodes[value_key] = _node_type(hashkey,value);
-            return value_key;// index of the replaced element
-        }
-
-        m_hash_table[cell_index] = m_nodes.size();
-
-        _insert_unsorted(hashkey,value);
-        return GIM_INVALID_HASH;
-
-    }
-
-    
-    ///Sorted array data management. The hash table has the indices to the corresponding m_nodes array
-    inline bool _erase_sorted(GUINT index)
-    {
-        if(index>=(GUINT)m_nodes.size()) return false;
-        m_nodes.erase_sorted(index);
-		if(m_nodes.size()<2) m_sorted = false;
-        return true;
-    }
-
-    //! faster, but unsorted
-    inline bool _erase_unsorted(GUINT index)
-    {
-        if(index>=m_nodes.size()) return false;
-
-        GUINT lastindex = m_nodes.size()-1;
-        if(index<lastindex && m_hash_table!=0)
-        {
-			GUINT hashkey =  m_nodes[lastindex].m_key;
-			if(hashkey!=GIM_INVALID_HASH)
+	inline GUINT _insert_hash_table_replace(GUINT hashkey, const T& value)
+	{
+		if (hashkey == GIM_INVALID_HASH)
+		{
+			//Insert anyway
+			_insert_unsorted(hashkey, value);
+			return GIM_INVALID_HASH;
+		}
+
+		GUINT cell_index = _assign_hash_table_cell(hashkey);
+
+		GUINT value_key = m_hash_table[cell_index];
+
+		if (value_key != GIM_INVALID_HASH)
+		{  //replaces the existing
+			m_nodes[value_key] = _node_type(hashkey, value);
+			return value_key;  // index of the replaced element
+		}
+
+		m_hash_table[cell_index] = m_nodes.size();
+
+		_insert_unsorted(hashkey, value);
+		return GIM_INVALID_HASH;
+	}
+
+	///Sorted array data management. The hash table has the indices to the corresponding m_nodes array
+	inline bool _erase_sorted(GUINT index)
+	{
+		if (index >= (GUINT)m_nodes.size()) return false;
+		m_nodes.erase_sorted(index);
+		if (m_nodes.size() < 2) m_sorted = false;
+		return true;
+	}
+
+	//! faster, but unsorted
+	inline bool _erase_unsorted(GUINT index)
+	{
+		if (index >= m_nodes.size()) return false;
+
+		GUINT lastindex = m_nodes.size() - 1;
+		if (index < lastindex && m_hash_table != 0)
+		{
+			GUINT hashkey = m_nodes[lastindex].m_key;
+			if (hashkey != GIM_INVALID_HASH)
 			{
 				//update the new position of the last element
 				GUINT cell_index = _find_cell(hashkey);
-				btAssert(cell_index!=GIM_INVALID_HASH);
+				btAssert(cell_index != GIM_INVALID_HASH);
 				//new position of the last element which will be swaped
 				m_hash_table[cell_index] = index;
 			}
-        }
-        m_nodes.erase(index);
-        m_sorted = false;
-        return true;
-    }
-
-    //! Insert in position ordered
-    /*!
+		}
+		m_nodes.erase(index);
+		m_sorted = false;
+		return true;
+	}
+
+	//! Insert in position ordered
+	/*!
     Also checks if it is needed to transform this container to a hash table, by calling check_for_switching_to_hashtable
     */
-    inline void _insert_in_pos(GUINT hashkey, const T & value, GUINT pos)
-    {
-        m_nodes.insert(_node_type(hashkey,value),pos);
-        this->check_for_switching_to_hashtable();
-    }
-
-    //! Insert an element in an ordered array
-    inline GUINT _insert_sorted(GUINT hashkey, const T & value)
-    {
-        if(hashkey==GIM_INVALID_HASH || size()==0)
-        {
-            m_nodes.push_back(_node_type(hashkey,value));
-            return GIM_INVALID_HASH;
-        }
-        //Insert at last position
-        //Sort element
-
-
-        GUINT result_ind=0;
-        GUINT last_index = m_nodes.size()-1;
-        _node_type * ptr = m_nodes.pointer();
-
-        bool found = gim_binary_search_ex(
-        	ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO());
-
-
-        //Insert before found index
-        if(found)
-        {
-            return result_ind;
-        }
-        else
-        {
-            _insert_in_pos(hashkey, value, result_ind);
-        }
-        return GIM_INVALID_HASH;
-    }
-
-    inline GUINT _insert_sorted_replace(GUINT hashkey, const T & value)
-    {
-        if(hashkey==GIM_INVALID_HASH || size()==0)
-        {
-            m_nodes.push_back(_node_type(hashkey,value));
-            return GIM_INVALID_HASH;
-        }
-        //Insert at last position
-        //Sort element
-        GUINT result_ind;
-        GUINT last_index = m_nodes.size()-1;
-        _node_type * ptr = m_nodes.pointer();
-
-        bool found = gim_binary_search_ex(
-        	ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO());
-
-        //Insert before found index
-        if(found)
-        {
-            m_nodes[result_ind] = _node_type(hashkey,value);
-        }
-        else
-        {
-            _insert_in_pos(hashkey, value, result_ind);
-        }
-        return result_ind;
-    }
-
-    //! Fast insertion in m_nodes array
-    inline GUINT  _insert_unsorted(GUINT hashkey, const T & value)
-    {
-        m_nodes.push_back(_node_type(hashkey,value));
-        m_sorted = false;
-        return GIM_INVALID_HASH;
-    }
-
-    
+	inline void _insert_in_pos(GUINT hashkey, const T& value, GUINT pos)
+	{
+		m_nodes.insert(_node_type(hashkey, value), pos);
+		this->check_for_switching_to_hashtable();
+	}
 
-public:
+	//! Insert an element in an ordered array
+	inline GUINT _insert_sorted(GUINT hashkey, const T& value)
+	{
+		if (hashkey == GIM_INVALID_HASH || size() == 0)
+		{
+			m_nodes.push_back(_node_type(hashkey, value));
+			return GIM_INVALID_HASH;
+		}
+		//Insert at last position
+		//Sort element
+
+		GUINT result_ind = 0;
+		GUINT last_index = m_nodes.size() - 1;
+		_node_type* ptr = m_nodes.pointer();
+
+		bool found = gim_binary_search_ex(
+			ptr, 0, last_index, result_ind, hashkey, GIM_HASH_NODE_CMP_KEY_MACRO());
+
+		//Insert before found index
+		if (found)
+		{
+			return result_ind;
+		}
+		else
+		{
+			_insert_in_pos(hashkey, value, result_ind);
+		}
+		return GIM_INVALID_HASH;
+	}
 
-    /*!
+	inline GUINT _insert_sorted_replace(GUINT hashkey, const T& value)
+	{
+		if (hashkey == GIM_INVALID_HASH || size() == 0)
+		{
+			m_nodes.push_back(_node_type(hashkey, value));
+			return GIM_INVALID_HASH;
+		}
+		//Insert at last position
+		//Sort element
+		GUINT result_ind;
+		GUINT last_index = m_nodes.size() - 1;
+		_node_type* ptr = m_nodes.pointer();
+
+		bool found = gim_binary_search_ex(
+			ptr, 0, last_index, result_ind, hashkey, GIM_HASH_NODE_CMP_KEY_MACRO());
+
+		//Insert before found index
+		if (found)
+		{
+			m_nodes[result_ind] = _node_type(hashkey, value);
+		}
+		else
+		{
+			_insert_in_pos(hashkey, value, result_ind);
+		}
+		return result_ind;
+	}
+
+	//! Fast insertion in m_nodes array
+	inline GUINT _insert_unsorted(GUINT hashkey, const T& value)
+	{
+		m_nodes.push_back(_node_type(hashkey, value));
+		m_sorted = false;
+		return GIM_INVALID_HASH;
+	}
+
+public:
+	/*!
         <li> if node_size = 0, then this container becomes a simple sorted array allocator. reserve_size is used for reserve memory in m_nodes.
         When the array size reaches the size equivalent to 'min_hash_table_size', then it becomes a hash table by calling check_for_switching_to_hashtable.
         <li> If node_size != 0, then this container becomes a hash table for ever
         </ul>
     */
-    gim_hash_table(GUINT reserve_size = GIM_DEFAULT_HASH_TABLE_SIZE,
-                     GUINT node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE,
-                     GUINT min_hash_table_size = GIM_INVALID_HASH)
-    {
-        m_hash_table = NULL;
-        m_table_size = 0;
-        m_sorted = false;
-        m_node_size = node_size;
-        m_min_hash_table_size = min_hash_table_size;
-
-        if(m_node_size!=0)
-        {
-            if(reserve_size!=0)
-            {
-                m_nodes.reserve(reserve_size);
-                _reserve_table_memory(reserve_size);
-                _invalidate_keys();
-            }
-            else
-            {
-                m_nodes.reserve(GIM_DEFAULT_HASH_TABLE_SIZE);
-                _reserve_table_memory(GIM_DEFAULT_HASH_TABLE_SIZE);
-                _invalidate_keys();
-            }
-        }
-        else if(reserve_size!=0)
-        {
-            m_nodes.reserve(reserve_size);
-        }
-
-    }
-
-    ~gim_hash_table()
-    {
-        _destroy();
-    }
-
-    inline bool is_hash_table()
-    {
-        if(m_hash_table) return true;
-        return false;
-    }
-
-    inline bool is_sorted()
-    {
-        if(size()<2) return true;
-        return m_sorted;
-    }
-
-    bool sort()
-    {
-        if(is_sorted()) return true;
-        if(m_nodes.size()<2) return false;
-
-
-        _node_type * ptr = m_nodes.pointer();
-        GUINT siz = m_nodes.size();
-        gim_sort_hash_node_array(ptr,siz);
-        m_sorted=true;
-
-
-
-        if(m_hash_table)
-        {
-            _rehash();
-        }
-        return true;
-    }
-
-    bool switch_to_hashtable()
-    {
-        if(m_hash_table) return false;
-        if(m_node_size==0) m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE;
-        if(m_nodes.size()<GIM_DEFAULT_HASH_TABLE_SIZE)
-        {
-            _resize_table(GIM_DEFAULT_HASH_TABLE_SIZE);
-        }
-        else
-        {
-            _resize_table(m_nodes.size()+1);
-        }
-
-        return true;
-    }
-
-    bool switch_to_sorted_array()
-    {
-        if(m_hash_table==NULL) return true;
-        _clear_table_memory();
-        return sort();
-    }
-
-    //!If the container reaches the
-    bool check_for_switching_to_hashtable()
-    {
-        if(this->m_hash_table) return true;
-
-        if(!(m_nodes.size()< m_min_hash_table_size))
-        {
-            if(m_node_size == 0)
-            {
-                m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE;
-            }
-
-            _resize_table(m_nodes.size()+1);
-            return true;
-        }
-        return false;
-    }
-
-    inline void set_sorted(bool value)
-    {
-    	m_sorted = value;
-    }
-
-    //! Retrieves the amount of keys.
-    inline GUINT size() const
-    {
-        return m_nodes.size();
-    }
-
-    //! Retrieves the hash key.
-    inline GUINT get_key(GUINT index) const
-    {
-        return m_nodes[index].m_key;
-    }
-
-    //! Retrieves the value by index
-    /*!
+	gim_hash_table(GUINT reserve_size = GIM_DEFAULT_HASH_TABLE_SIZE,
+				   GUINT node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE,
+				   GUINT min_hash_table_size = GIM_INVALID_HASH)
+	{
+		m_hash_table = NULL;
+		m_table_size = 0;
+		m_sorted = false;
+		m_node_size = node_size;
+		m_min_hash_table_size = min_hash_table_size;
+
+		if (m_node_size != 0)
+		{
+			if (reserve_size != 0)
+			{
+				m_nodes.reserve(reserve_size);
+				_reserve_table_memory(reserve_size);
+				_invalidate_keys();
+			}
+			else
+			{
+				m_nodes.reserve(GIM_DEFAULT_HASH_TABLE_SIZE);
+				_reserve_table_memory(GIM_DEFAULT_HASH_TABLE_SIZE);
+				_invalidate_keys();
+			}
+		}
+		else if (reserve_size != 0)
+		{
+			m_nodes.reserve(reserve_size);
+		}
+	}
+
+	~gim_hash_table()
+	{
+		_destroy();
+	}
+
+	inline bool is_hash_table()
+	{
+		if (m_hash_table) return true;
+		return false;
+	}
+
+	inline bool is_sorted()
+	{
+		if (size() < 2) return true;
+		return m_sorted;
+	}
+
+	bool sort()
+	{
+		if (is_sorted()) return true;
+		if (m_nodes.size() < 2) return false;
+
+		_node_type* ptr = m_nodes.pointer();
+		GUINT siz = m_nodes.size();
+		gim_sort_hash_node_array(ptr, siz);
+		m_sorted = true;
+
+		if (m_hash_table)
+		{
+			_rehash();
+		}
+		return true;
+	}
+
+	bool switch_to_hashtable()
+	{
+		if (m_hash_table) return false;
+		if (m_node_size == 0) m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE;
+		if (m_nodes.size() < GIM_DEFAULT_HASH_TABLE_SIZE)
+		{
+			_resize_table(GIM_DEFAULT_HASH_TABLE_SIZE);
+		}
+		else
+		{
+			_resize_table(m_nodes.size() + 1);
+		}
+
+		return true;
+	}
+
+	bool switch_to_sorted_array()
+	{
+		if (m_hash_table == NULL) return true;
+		_clear_table_memory();
+		return sort();
+	}
+
+	//!If the container reaches the
+	bool check_for_switching_to_hashtable()
+	{
+		if (this->m_hash_table) return true;
+
+		if (!(m_nodes.size() < m_min_hash_table_size))
+		{
+			if (m_node_size == 0)
+			{
+				m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE;
+			}
+
+			_resize_table(m_nodes.size() + 1);
+			return true;
+		}
+		return false;
+	}
+
+	inline void set_sorted(bool value)
+	{
+		m_sorted = value;
+	}
+
+	//! Retrieves the amount of keys.
+	inline GUINT size() const
+	{
+		return m_nodes.size();
+	}
+
+	//! Retrieves the hash key.
+	inline GUINT get_key(GUINT index) const
+	{
+		return m_nodes[index].m_key;
+	}
+
+	//! Retrieves the value by index
+	/*!
     */
-    inline T * get_value_by_index(GUINT index)
-    {
-        return &m_nodes[index].m_data;
-    }
-
-    inline const T& operator[](GUINT index) const
-    {
-        return m_nodes[index].m_data;
-    }
-
-    inline T& operator[](GUINT index)
-    {
-        return m_nodes[index].m_data;
-    }
-
-    //! Finds the index of the element with the key
-    /*!
+	inline T* get_value_by_index(GUINT index)
+	{
+		return &m_nodes[index].m_data;
+	}
+
+	inline const T& operator[](GUINT index) const
+	{
+		return m_nodes[index].m_data;
+	}
+
+	inline T& operator[](GUINT index)
+	{
+		return m_nodes[index].m_data;
+	}
+
+	//! Finds the index of the element with the key
+	/*!
     \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted
     If so, the element has been inserted at the last position of the array.
     */
-    inline GUINT find(GUINT hashkey)
-    {
-        if(m_hash_table)
-        {
-            GUINT cell_index = _find_cell(hashkey);
-            if(cell_index==GIM_INVALID_HASH) return GIM_INVALID_HASH;
-            return m_hash_table[cell_index];
-        }
+	inline GUINT find(GUINT hashkey)
+	{
+		if (m_hash_table)
+		{
+			GUINT cell_index = _find_cell(hashkey);
+			if (cell_index == GIM_INVALID_HASH) return GIM_INVALID_HASH;
+			return m_hash_table[cell_index];
+		}
 		GUINT last_index = m_nodes.size();
-        if(last_index<2)
-        {
-			if(last_index==0) return GIM_INVALID_HASH;
-            if(m_nodes[0].m_key == hashkey) return 0;
-            return GIM_INVALID_HASH;
-        }
-        else if(m_sorted)
-        {
-            //Binary search
-            GUINT result_ind = 0;
+		if (last_index < 2)
+		{
+			if (last_index == 0) return GIM_INVALID_HASH;
+			if (m_nodes[0].m_key == hashkey) return 0;
+			return GIM_INVALID_HASH;
+		}
+		else if (m_sorted)
+		{
+			//Binary search
+			GUINT result_ind = 0;
 			last_index--;
-            _node_type *  ptr =  m_nodes.pointer();
+			_node_type* ptr = m_nodes.pointer();
 
-            bool found = gim_binary_search_ex(ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO());
+			bool found = gim_binary_search_ex(ptr, 0, last_index, result_ind, hashkey, GIM_HASH_NODE_CMP_KEY_MACRO());
 
+			if (found) return result_ind;
+		}
+		return GIM_INVALID_HASH;
+	}
 
-            if(found) return result_ind;
-        }
-        return GIM_INVALID_HASH;
-    }
-
-    //! Retrieves the value associated with the index
-    /*!
+	//! Retrieves the value associated with the index
+	/*!
     \return the found element, or null
     */
-    inline T * get_value(GUINT hashkey)
-    {
-        GUINT index = find(hashkey);
-        if(index == GIM_INVALID_HASH) return NULL;
-        return &m_nodes[index].m_data;
-    }
-
+	inline T* get_value(GUINT hashkey)
+	{
+		GUINT index = find(hashkey);
+		if (index == GIM_INVALID_HASH) return NULL;
+		return &m_nodes[index].m_data;
+	}
 
-    /*!
+	/*!
     */
-    inline bool erase_by_index(GUINT index)
-    {
-        if(index > m_nodes.size()) return false;
-
-        if(m_hash_table == NULL)
-        {
-            if(is_sorted())
-            {
-                return this->_erase_sorted(index);
-            }
-            else
-            {
-                return this->_erase_unsorted(index);
-            }
-        }
-        else
-        {
-            return this->_erase_by_index_hash_table(index);
-        }
-        return false;
-    }
-
-
-
-    inline bool erase_by_index_unsorted(GUINT index)
-    {
-        if(index > m_nodes.size()) return false;
-
-        if(m_hash_table == NULL)
-        {
-            return this->_erase_unsorted(index);
-        }
-        else
-        {
-            return this->_erase_by_index_hash_table(index);
-        }
-        return false;
-    }
-
-
-
-    /*!
+	inline bool erase_by_index(GUINT index)
+	{
+		if (index > m_nodes.size()) return false;
+
+		if (m_hash_table == NULL)
+		{
+			if (is_sorted())
+			{
+				return this->_erase_sorted(index);
+			}
+			else
+			{
+				return this->_erase_unsorted(index);
+			}
+		}
+		else
+		{
+			return this->_erase_by_index_hash_table(index);
+		}
+		return false;
+	}
+
+	inline bool erase_by_index_unsorted(GUINT index)
+	{
+		if (index > m_nodes.size()) return false;
+
+		if (m_hash_table == NULL)
+		{
+			return this->_erase_unsorted(index);
+		}
+		else
+		{
+			return this->_erase_by_index_hash_table(index);
+		}
+		return false;
+	}
+
+	/*!
 
     */
-    inline bool erase_by_key(GUINT hashkey)
-    {
-        if(size()==0) return false;
-
-        if(m_hash_table)
-        {
-            return this->_erase_hash_table(hashkey);
-        }
-        //Binary search
-
-        if(is_sorted()==false) return false;
-
-        GUINT result_ind = find(hashkey);
-        if(result_ind!= GIM_INVALID_HASH)
-        {
-            return this->_erase_sorted(result_ind);
-        }
-        return false;
-    }
-
-    void clear()
-    {
-        m_nodes.clear();
-
-        if(m_hash_table==NULL) return;
-        GUINT datasize = m_table_size*m_node_size;
-        //Initialize the hashkeys.
-        GUINT i;
-        for(i=0;i<datasize;i++)
-        {
-            m_hash_table[i] = GIM_INVALID_HASH;// invalidate keys
-        }
+	inline bool erase_by_key(GUINT hashkey)
+	{
+		if (size() == 0) return false;
+
+		if (m_hash_table)
+		{
+			return this->_erase_hash_table(hashkey);
+		}
+		//Binary search
+
+		if (is_sorted() == false) return false;
+
+		GUINT result_ind = find(hashkey);
+		if (result_ind != GIM_INVALID_HASH)
+		{
+			return this->_erase_sorted(result_ind);
+		}
+		return false;
+	}
+
+	void clear()
+	{
+		m_nodes.clear();
+
+		if (m_hash_table == NULL) return;
+		GUINT datasize = m_table_size * m_node_size;
+		//Initialize the hashkeys.
+		GUINT i;
+		for (i = 0; i < datasize; i++)
+		{
+			m_hash_table[i] = GIM_INVALID_HASH;  // invalidate keys
+		}
 		m_sorted = false;
-    }
+	}
 
-    //! Insert an element into the hash
-    /*!
+	//! Insert an element into the hash
+	/*!
     \return If GIM_INVALID_HASH, the object has been inserted succesfully. Else it returns the position
     of the existing element.
     */
-    inline GUINT insert(GUINT hashkey, const T & element)
-    {
-        if(m_hash_table)
-        {
-            return this->_insert_hash_table(hashkey,element);
-        }
-        if(this->is_sorted())
-        {
-            return this->_insert_sorted(hashkey,element);
-        }
-        return this->_insert_unsorted(hashkey,element);
-    }
-
-    //! Insert an element into the hash, and could overrite an existing object with the same hash.
-    /*!
+	inline GUINT insert(GUINT hashkey, const T& element)
+	{
+		if (m_hash_table)
+		{
+			return this->_insert_hash_table(hashkey, element);
+		}
+		if (this->is_sorted())
+		{
+			return this->_insert_sorted(hashkey, element);
+		}
+		return this->_insert_unsorted(hashkey, element);
+	}
+
+	//! Insert an element into the hash, and could overrite an existing object with the same hash.
+	/*!
     \return If GIM_INVALID_HASH, the object has been inserted succesfully. Else it returns the position
     of the replaced element.
     */
-    inline GUINT insert_override(GUINT hashkey, const T & element)
-    {
-        if(m_hash_table)
-        {
-            return this->_insert_hash_table_replace(hashkey,element);
-        }
-        if(this->is_sorted())
-        {
-            return this->_insert_sorted_replace(hashkey,element);
-        }
-        this->_insert_unsorted(hashkey,element);
-        return m_nodes.size();
-    }
-
-
-
-    //! Insert an element into the hash,But if this container is a sorted array, this inserts it unsorted
-    /*!
-    */
-    inline GUINT insert_unsorted(GUINT hashkey,const T & element)
-    {
-        if(m_hash_table)
-        {
-            return this->_insert_hash_table(hashkey,element);
-        }
-        return this->_insert_unsorted(hashkey,element);
-    }
-
+	inline GUINT insert_override(GUINT hashkey, const T& element)
+	{
+		if (m_hash_table)
+		{
+			return this->_insert_hash_table_replace(hashkey, element);
+		}
+		if (this->is_sorted())
+		{
+			return this->_insert_sorted_replace(hashkey, element);
+		}
+		this->_insert_unsorted(hashkey, element);
+		return m_nodes.size();
+	}
 
+	//! Insert an element into the hash,But if this container is a sorted array, this inserts it unsorted
+	/*!
+    */
+	inline GUINT insert_unsorted(GUINT hashkey, const T& element)
+	{
+		if (m_hash_table)
+		{
+			return this->_insert_hash_table(hashkey, element);
+		}
+		return this->_insert_unsorted(hashkey, element);
+	}
 };
 
-
-
-#endif // GIM_CONTAINERS_H_INCLUDED
+#endif  // GIM_CONTAINERS_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h
index 64f11b49543..98401a404ab 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h
@@ -34,962 +34,900 @@ email: projectileman@yahoo.com
 -----------------------------------------------------------------------------
 */
 
-
 #include "gim_math.h"
 #include "gim_geom_types.h"
 
-
-
-
 //! Zero out a 2D vector
-#define VEC_ZERO_2(a)				\
-{						\
-   (a)[0] = (a)[1] = 0.0f;			\
-}\
-
+#define VEC_ZERO_2(a)           \
+	{                           \
+		(a)[0] = (a)[1] = 0.0f; \
+	}
 
 //! Zero out a 3D vector
-#define VEC_ZERO(a)				\
-{						\
-   (a)[0] = (a)[1] = (a)[2] = 0.0f;		\
-}\
-
+#define VEC_ZERO(a)                      \
+	{                                    \
+		(a)[0] = (a)[1] = (a)[2] = 0.0f; \
+	}
 
 /// Zero out a 4D vector
-#define VEC_ZERO_4(a)				\
-{						\
-   (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f;	\
-}\
-
+#define VEC_ZERO_4(a)                             \
+	{                                             \
+		(a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f; \
+	}
 
 /// Vector copy
-#define VEC_COPY_2(b,a)				\
-{						\
-   (b)[0] = (a)[0];				\
-   (b)[1] = (a)[1];				\
-}\
-
+#define VEC_COPY_2(b, a) \
+	{                    \
+		(b)[0] = (a)[0]; \
+		(b)[1] = (a)[1]; \
+	}
 
 /// Copy 3D vector
-#define VEC_COPY(b,a)				\
-{						\
-   (b)[0] = (a)[0];				\
-   (b)[1] = (a)[1];				\
-   (b)[2] = (a)[2];				\
-}\
-
+#define VEC_COPY(b, a)   \
+	{                    \
+		(b)[0] = (a)[0]; \
+		(b)[1] = (a)[1]; \
+		(b)[2] = (a)[2]; \
+	}
 
 /// Copy 4D vector
-#define VEC_COPY_4(b,a)				\
-{						\
-   (b)[0] = (a)[0];				\
-   (b)[1] = (a)[1];				\
-   (b)[2] = (a)[2];				\
-   (b)[3] = (a)[3];				\
-}\
+#define VEC_COPY_4(b, a) \
+	{                    \
+		(b)[0] = (a)[0]; \
+		(b)[1] = (a)[1]; \
+		(b)[2] = (a)[2]; \
+		(b)[3] = (a)[3]; \
+	}
 
 /// VECTOR SWAP
-#define VEC_SWAP(b,a)				\
-{  \
-    GIM_SWAP_NUMBERS((b)[0],(a)[0]);\
-    GIM_SWAP_NUMBERS((b)[1],(a)[1]);\
-    GIM_SWAP_NUMBERS((b)[2],(a)[2]);\
-}\
+#define VEC_SWAP(b, a)                    \
+	{                                     \
+		GIM_SWAP_NUMBERS((b)[0], (a)[0]); \
+		GIM_SWAP_NUMBERS((b)[1], (a)[1]); \
+		GIM_SWAP_NUMBERS((b)[2], (a)[2]); \
+	}
 
 /// Vector difference
-#define VEC_DIFF_2(v21,v2,v1)			\
-{						\
-   (v21)[0] = (v2)[0] - (v1)[0];		\
-   (v21)[1] = (v2)[1] - (v1)[1];		\
-}\
-
+#define VEC_DIFF_2(v21, v2, v1)       \
+	{                                 \
+		(v21)[0] = (v2)[0] - (v1)[0]; \
+		(v21)[1] = (v2)[1] - (v1)[1]; \
+	}
 
 /// Vector difference
-#define VEC_DIFF(v21,v2,v1)			\
-{						\
-   (v21)[0] = (v2)[0] - (v1)[0];		\
-   (v21)[1] = (v2)[1] - (v1)[1];		\
-   (v21)[2] = (v2)[2] - (v1)[2];		\
-}\
-
+#define VEC_DIFF(v21, v2, v1)         \
+	{                                 \
+		(v21)[0] = (v2)[0] - (v1)[0]; \
+		(v21)[1] = (v2)[1] - (v1)[1]; \
+		(v21)[2] = (v2)[2] - (v1)[2]; \
+	}
 
 /// Vector difference
-#define VEC_DIFF_4(v21,v2,v1)			\
-{						\
-   (v21)[0] = (v2)[0] - (v1)[0];		\
-   (v21)[1] = (v2)[1] - (v1)[1];		\
-   (v21)[2] = (v2)[2] - (v1)[2];		\
-   (v21)[3] = (v2)[3] - (v1)[3];		\
-}\
-
+#define VEC_DIFF_4(v21, v2, v1)       \
+	{                                 \
+		(v21)[0] = (v2)[0] - (v1)[0]; \
+		(v21)[1] = (v2)[1] - (v1)[1]; \
+		(v21)[2] = (v2)[2] - (v1)[2]; \
+		(v21)[3] = (v2)[3] - (v1)[3]; \
+	}
 
 /// Vector sum
-#define VEC_SUM_2(v21,v2,v1)			\
-{						\
-   (v21)[0] = (v2)[0] + (v1)[0];		\
-   (v21)[1] = (v2)[1] + (v1)[1];		\
-}\
-
+#define VEC_SUM_2(v21, v2, v1)        \
+	{                                 \
+		(v21)[0] = (v2)[0] + (v1)[0]; \
+		(v21)[1] = (v2)[1] + (v1)[1]; \
+	}
 
 /// Vector sum
-#define VEC_SUM(v21,v2,v1)			\
-{						\
-   (v21)[0] = (v2)[0] + (v1)[0];		\
-   (v21)[1] = (v2)[1] + (v1)[1];		\
-   (v21)[2] = (v2)[2] + (v1)[2];		\
-}\
-
+#define VEC_SUM(v21, v2, v1)          \
+	{                                 \
+		(v21)[0] = (v2)[0] + (v1)[0]; \
+		(v21)[1] = (v2)[1] + (v1)[1]; \
+		(v21)[2] = (v2)[2] + (v1)[2]; \
+	}
 
 /// Vector sum
-#define VEC_SUM_4(v21,v2,v1)			\
-{						\
-   (v21)[0] = (v2)[0] + (v1)[0];		\
-   (v21)[1] = (v2)[1] + (v1)[1];		\
-   (v21)[2] = (v2)[2] + (v1)[2];		\
-   (v21)[3] = (v2)[3] + (v1)[3];		\
-}\
-
+#define VEC_SUM_4(v21, v2, v1)        \
+	{                                 \
+		(v21)[0] = (v2)[0] + (v1)[0]; \
+		(v21)[1] = (v2)[1] + (v1)[1]; \
+		(v21)[2] = (v2)[2] + (v1)[2]; \
+		(v21)[3] = (v2)[3] + (v1)[3]; \
+	}
 
 /// scalar times vector
-#define VEC_SCALE_2(c,a,b)			\
-{						\
-   (c)[0] = (a)*(b)[0];				\
-   (c)[1] = (a)*(b)[1];				\
-}\
-
+#define VEC_SCALE_2(c, a, b)   \
+	{                          \
+		(c)[0] = (a) * (b)[0]; \
+		(c)[1] = (a) * (b)[1]; \
+	}
 
 /// scalar times vector
-#define VEC_SCALE(c,a,b)			\
-{						\
-   (c)[0] = (a)*(b)[0];				\
-   (c)[1] = (a)*(b)[1];				\
-   (c)[2] = (a)*(b)[2];				\
-}\
-
+#define VEC_SCALE(c, a, b)     \
+	{                          \
+		(c)[0] = (a) * (b)[0]; \
+		(c)[1] = (a) * (b)[1]; \
+		(c)[2] = (a) * (b)[2]; \
+	}
 
 /// scalar times vector
-#define VEC_SCALE_4(c,a,b)			\
-{						\
-   (c)[0] = (a)*(b)[0];				\
-   (c)[1] = (a)*(b)[1];				\
-   (c)[2] = (a)*(b)[2];				\
-   (c)[3] = (a)*(b)[3];				\
-}\
-
+#define VEC_SCALE_4(c, a, b)   \
+	{                          \
+		(c)[0] = (a) * (b)[0]; \
+		(c)[1] = (a) * (b)[1]; \
+		(c)[2] = (a) * (b)[2]; \
+		(c)[3] = (a) * (b)[3]; \
+	}
 
 /// accumulate scaled vector
-#define VEC_ACCUM_2(c,a,b)			\
-{						\
-   (c)[0] += (a)*(b)[0];			\
-   (c)[1] += (a)*(b)[1];			\
-}\
-
+#define VEC_ACCUM_2(c, a, b)    \
+	{                           \
+		(c)[0] += (a) * (b)[0]; \
+		(c)[1] += (a) * (b)[1]; \
+	}
 
 /// accumulate scaled vector
-#define VEC_ACCUM(c,a,b)			\
-{						\
-   (c)[0] += (a)*(b)[0];			\
-   (c)[1] += (a)*(b)[1];			\
-   (c)[2] += (a)*(b)[2];			\
-}\
-
+#define VEC_ACCUM(c, a, b)      \
+	{                           \
+		(c)[0] += (a) * (b)[0]; \
+		(c)[1] += (a) * (b)[1]; \
+		(c)[2] += (a) * (b)[2]; \
+	}
 
 /// accumulate scaled vector
-#define VEC_ACCUM_4(c,a,b)			\
-{						\
-   (c)[0] += (a)*(b)[0];			\
-   (c)[1] += (a)*(b)[1];			\
-   (c)[2] += (a)*(b)[2];			\
-   (c)[3] += (a)*(b)[3];			\
-}\
-
+#define VEC_ACCUM_4(c, a, b)    \
+	{                           \
+		(c)[0] += (a) * (b)[0]; \
+		(c)[1] += (a) * (b)[1]; \
+		(c)[2] += (a) * (b)[2]; \
+		(c)[3] += (a) * (b)[3]; \
+	}
 
 /// Vector dot product
-#define VEC_DOT_2(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1])
-
+#define VEC_DOT_2(a, b) ((a)[0] * (b)[0] + (a)[1] * (b)[1])
 
 /// Vector dot product
-#define VEC_DOT(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])
+#define VEC_DOT(a, b) ((a)[0] * (b)[0] + (a)[1] * (b)[1] + (a)[2] * (b)[2])
 
 /// Vector dot product
-#define VEC_DOT_4(a,b)	((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3])
+#define VEC_DOT_4(a, b) ((a)[0] * (b)[0] + (a)[1] * (b)[1] + (a)[2] * (b)[2] + (a)[3] * (b)[3])
 
 /// vector impact parameter (squared)
-#define VEC_IMPACT_SQ(bsq,direction,position) {\
-   GREAL _llel_ = VEC_DOT(direction, position);\
-   bsq = VEC_DOT(position, position) - _llel_*_llel_;\
-}\
-
+#define VEC_IMPACT_SQ(bsq, direction, position)              \
+	{                                                        \
+		GREAL _llel_ = VEC_DOT(direction, position);         \
+		bsq = VEC_DOT(position, position) - _llel_ * _llel_; \
+	}
 
 /// vector impact parameter
-#define VEC_IMPACT(bsq,direction,position)	{\
-   VEC_IMPACT_SQ(bsq,direction,position);		\
-   GIM_SQRT(bsq,bsq);					\
-}\
+#define VEC_IMPACT(bsq, direction, position)     \
+	{                                            \
+		VEC_IMPACT_SQ(bsq, direction, position); \
+		GIM_SQRT(bsq, bsq);                      \
+	}
 
 /// Vector length
-#define VEC_LENGTH_2(a,l)\
-{\
-    GREAL _pp = VEC_DOT_2(a,a);\
-    GIM_SQRT(_pp,l);\
-}\
-
+#define VEC_LENGTH_2(a, l)           \
+	{                                \
+		GREAL _pp = VEC_DOT_2(a, a); \
+		GIM_SQRT(_pp, l);            \
+	}
 
 /// Vector length
-#define VEC_LENGTH(a,l)\
-{\
-    GREAL _pp = VEC_DOT(a,a);\
-    GIM_SQRT(_pp,l);\
-}\
-
+#define VEC_LENGTH(a, l)           \
+	{                              \
+		GREAL _pp = VEC_DOT(a, a); \
+		GIM_SQRT(_pp, l);          \
+	}
 
 /// Vector length
-#define VEC_LENGTH_4(a,l)\
-{\
-    GREAL _pp = VEC_DOT_4(a,a);\
-    GIM_SQRT(_pp,l);\
-}\
+#define VEC_LENGTH_4(a, l)           \
+	{                                \
+		GREAL _pp = VEC_DOT_4(a, a); \
+		GIM_SQRT(_pp, l);            \
+	}
 
 /// Vector inv length
-#define VEC_INV_LENGTH_2(a,l)\
-{\
-    GREAL _pp = VEC_DOT_2(a,a);\
-    GIM_INV_SQRT(_pp,l);\
-}\
-
+#define VEC_INV_LENGTH_2(a, l)       \
+	{                                \
+		GREAL _pp = VEC_DOT_2(a, a); \
+		GIM_INV_SQRT(_pp, l);        \
+	}
 
 /// Vector inv length
-#define VEC_INV_LENGTH(a,l)\
-{\
-    GREAL _pp = VEC_DOT(a,a);\
-    GIM_INV_SQRT(_pp,l);\
-}\
-
+#define VEC_INV_LENGTH(a, l)       \
+	{                              \
+		GREAL _pp = VEC_DOT(a, a); \
+		GIM_INV_SQRT(_pp, l);      \
+	}
 
 /// Vector inv length
-#define VEC_INV_LENGTH_4(a,l)\
-{\
-    GREAL _pp = VEC_DOT_4(a,a);\
-    GIM_INV_SQRT(_pp,l);\
-}\
-
-
+#define VEC_INV_LENGTH_4(a, l)       \
+	{                                \
+		GREAL _pp = VEC_DOT_4(a, a); \
+		GIM_INV_SQRT(_pp, l);        \
+	}
 
 /// distance between two points
-#define VEC_DISTANCE(_len,_va,_vb) {\
-    vec3f _tmp_;				\
-    VEC_DIFF(_tmp_, _vb, _va);			\
-    VEC_LENGTH(_tmp_,_len);			\
-}\
-
+#define VEC_DISTANCE(_len, _va, _vb) \
+	{                                \
+		vec3f _tmp_;                 \
+		VEC_DIFF(_tmp_, _vb, _va);   \
+		VEC_LENGTH(_tmp_, _len);     \
+	}
 
 /// Vector length
-#define VEC_CONJUGATE_LENGTH(a,l)\
-{\
-    GREAL _pp = 1.0 - a[0]*a[0] - a[1]*a[1] - a[2]*a[2];\
-    GIM_SQRT(_pp,l);\
-}\
-
+#define VEC_CONJUGATE_LENGTH(a, l)                                 \
+	{                                                              \
+		GREAL _pp = 1.0 - a[0] * a[0] - a[1] * a[1] - a[2] * a[2]; \
+		GIM_SQRT(_pp, l);                                          \
+	}
 
 /// Vector length
-#define VEC_NORMALIZE(a) {	\
-    GREAL len;\
-    VEC_INV_LENGTH(a,len); \
-    if(len<G_REAL_INFINITY)\
-    {\
-        a[0] *= len;				\
-        a[1] *= len;				\
-        a[2] *= len;				\
-    }						\
-}\
+#define VEC_NORMALIZE(a)           \
+	{                              \
+		GREAL len;                 \
+		VEC_INV_LENGTH(a, len);    \
+		if (len < G_REAL_INFINITY) \
+		{                          \
+			a[0] *= len;           \
+			a[1] *= len;           \
+			a[2] *= len;           \
+		}                          \
+	}
 
 /// Set Vector size
-#define VEC_RENORMALIZE(a,newlen) {	\
-    GREAL len;\
-    VEC_INV_LENGTH(a,len); \
-    if(len<G_REAL_INFINITY)\
-    {\
-        len *= newlen;\
-        a[0] *= len;				\
-        a[1] *= len;				\
-        a[2] *= len;				\
-    }						\
-}\
+#define VEC_RENORMALIZE(a, newlen) \
+	{                              \
+		GREAL len;                 \
+		VEC_INV_LENGTH(a, len);    \
+		if (len < G_REAL_INFINITY) \
+		{                          \
+			len *= newlen;         \
+			a[0] *= len;           \
+			a[1] *= len;           \
+			a[2] *= len;           \
+		}                          \
+	}
 
 /// Vector cross
-#define VEC_CROSS(c,a,b)		\
-{						\
-   c[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1];	\
-   c[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2];	\
-   c[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0];	\
-}\
-
+#define VEC_CROSS(c, a, b)                        \
+	{                                             \
+		c[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1]; \
+		c[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2]; \
+		c[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0]; \
+	}
 
 /*! Vector perp -- assumes that n is of unit length
  * accepts vector v, subtracts out any component parallel to n */
-#define VEC_PERPENDICULAR(vp,v,n)			\
-{						\
-   GREAL dot = VEC_DOT(v, n);			\
-   vp[0] = (v)[0] - dot*(n)[0];		\
-   vp[1] = (v)[1] - dot*(n)[1];		\
-   vp[2] = (v)[2] - dot*(n)[2];		\
-}\
-
+#define VEC_PERPENDICULAR(vp, v, n)    \
+	{                                  \
+		GREAL dot = VEC_DOT(v, n);     \
+		vp[0] = (v)[0] - dot * (n)[0]; \
+		vp[1] = (v)[1] - dot * (n)[1]; \
+		vp[2] = (v)[2] - dot * (n)[2]; \
+	}
 
 /*! Vector parallel -- assumes that n is of unit length */
-#define VEC_PARALLEL(vp,v,n)			\
-{						\
-   GREAL dot = VEC_DOT(v, n);			\
-   vp[0] = (dot) * (n)[0];			\
-   vp[1] = (dot) * (n)[1];			\
-   vp[2] = (dot) * (n)[2];			\
-}\
+#define VEC_PARALLEL(vp, v, n)     \
+	{                              \
+		GREAL dot = VEC_DOT(v, n); \
+		vp[0] = (dot) * (n)[0];    \
+		vp[1] = (dot) * (n)[1];    \
+		vp[2] = (dot) * (n)[2];    \
+	}
 
 /*! Same as Vector parallel --  n can have any length
  * accepts vector v, subtracts out any component perpendicular to n */
-#define VEC_PROJECT(vp,v,n)			\
-{ \
-	GREAL scalar = VEC_DOT(v, n);			\
-	scalar/= VEC_DOT(n, n); \
-	vp[0] = (scalar) * (n)[0];			\
-    vp[1] = (scalar) * (n)[1];			\
-    vp[2] = (scalar) * (n)[2];			\
-}\
-
+#define VEC_PROJECT(vp, v, n)         \
+	{                                 \
+		GREAL scalar = VEC_DOT(v, n); \
+		scalar /= VEC_DOT(n, n);      \
+		vp[0] = (scalar) * (n)[0];    \
+		vp[1] = (scalar) * (n)[1];    \
+		vp[2] = (scalar) * (n)[2];    \
+	}
 
 /*! accepts vector v*/
-#define VEC_UNPROJECT(vp,v,n)			\
-{ \
-	GREAL scalar = VEC_DOT(v, n);			\
-	scalar = VEC_DOT(n, n)/scalar; \
-	vp[0] = (scalar) * (n)[0];			\
-    vp[1] = (scalar) * (n)[1];			\
-    vp[2] = (scalar) * (n)[2];			\
-}\
-
+#define VEC_UNPROJECT(vp, v, n)          \
+	{                                    \
+		GREAL scalar = VEC_DOT(v, n);    \
+		scalar = VEC_DOT(n, n) / scalar; \
+		vp[0] = (scalar) * (n)[0];       \
+		vp[1] = (scalar) * (n)[1];       \
+		vp[2] = (scalar) * (n)[2];       \
+	}
 
 /*! Vector reflection -- assumes n is of unit length
  Takes vector v, reflects it against reflector n, and returns vr */
-#define VEC_REFLECT(vr,v,n)			\
-{						\
-   GREAL dot = VEC_DOT(v, n);			\
-   vr[0] = (v)[0] - 2.0 * (dot) * (n)[0];	\
-   vr[1] = (v)[1] - 2.0 * (dot) * (n)[1];	\
-   vr[2] = (v)[2] - 2.0 * (dot) * (n)[2];	\
-}\
-
+#define VEC_REFLECT(vr, v, n)                  \
+	{                                          \
+		GREAL dot = VEC_DOT(v, n);             \
+		vr[0] = (v)[0] - 2.0 * (dot) * (n)[0]; \
+		vr[1] = (v)[1] - 2.0 * (dot) * (n)[1]; \
+		vr[2] = (v)[2] - 2.0 * (dot) * (n)[2]; \
+	}
 
 /*! Vector blending
 Takes two vectors a, b, blends them together with two scalars */
-#define VEC_BLEND_AB(vr,sa,a,sb,b)			\
-{						\
-   vr[0] = (sa) * (a)[0] + (sb) * (b)[0];	\
-   vr[1] = (sa) * (a)[1] + (sb) * (b)[1];	\
-   vr[2] = (sa) * (a)[2] + (sb) * (b)[2];	\
-}\
+#define VEC_BLEND_AB(vr, sa, a, sb, b)         \
+	{                                          \
+		vr[0] = (sa) * (a)[0] + (sb) * (b)[0]; \
+		vr[1] = (sa) * (a)[1] + (sb) * (b)[1]; \
+		vr[2] = (sa) * (a)[2] + (sb) * (b)[2]; \
+	}
 
 /*! Vector blending
 Takes two vectors a, b, blends them together with s <=1 */
-#define VEC_BLEND(vr,a,b,s) VEC_BLEND_AB(vr,(1-s),a,s,b)
+#define VEC_BLEND(vr, a, b, s) VEC_BLEND_AB(vr, (1 - s), a, s, b)
 
-#define VEC_SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2];
+#define VEC_SET3(a, b, op, c) \
+	a[0] = b[0] op c[0];      \
+	a[1] = b[1] op c[1];      \
+	a[2] = b[2] op c[2];
 
 //! Finds the bigger cartesian coordinate from a vector
-#define VEC_MAYOR_COORD(vec, maxc)\
-{\
-	GREAL A[] = {fabs(vec[0]),fabs(vec[1]),fabs(vec[2])};\
-    maxc =  A[0]>A[1]?(A[0]>A[2]?0:2):(A[1]>A[2]?1:2);\
-}\
+#define VEC_MAYOR_COORD(vec, maxc)                                          \
+	{                                                                       \
+		GREAL A[] = {fabs(vec[0]), fabs(vec[1]), fabs(vec[2])};             \
+		maxc = A[0] > A[1] ? (A[0] > A[2] ? 0 : 2) : (A[1] > A[2] ? 1 : 2); \
+	}
 
 //! Finds the 2 smallest cartesian coordinates from a vector
-#define VEC_MINOR_AXES(vec, i0, i1)\
-{\
-	VEC_MAYOR_COORD(vec,i0);\
-	i0 = (i0+1)%3;\
-	i1 = (i0+1)%3;\
-}\
-
-
-
+#define VEC_MINOR_AXES(vec, i0, i1) \
+	{                               \
+		VEC_MAYOR_COORD(vec, i0);   \
+		i0 = (i0 + 1) % 3;          \
+		i1 = (i0 + 1) % 3;          \
+	}
 
-#define VEC_EQUAL(v1,v2) (v1[0]==v2[0]&&v1[1]==v2[1]&&v1[2]==v2[2])
-
-#define VEC_NEAR_EQUAL(v1,v2) (GIM_NEAR_EQUAL(v1[0],v2[0])&&GIM_NEAR_EQUAL(v1[1],v2[1])&&GIM_NEAR_EQUAL(v1[2],v2[2]))
+#define VEC_EQUAL(v1, v2) (v1[0] == v2[0] && v1[1] == v2[1] && v1[2] == v2[2])
 
+#define VEC_NEAR_EQUAL(v1, v2) (GIM_NEAR_EQUAL(v1[0], v2[0]) && GIM_NEAR_EQUAL(v1[1], v2[1]) && GIM_NEAR_EQUAL(v1[2], v2[2]))
 
 /// Vector cross
-#define X_AXIS_CROSS_VEC(dst,src)\
-{					   \
-	dst[0] = 0.0f;     \
-	dst[1] = -src[2];  \
-	dst[2] = src[1];  \
-}\
-
-#define Y_AXIS_CROSS_VEC(dst,src)\
-{					   \
-	dst[0] = src[2];     \
-	dst[1] = 0.0f;  \
-	dst[2] = -src[0];  \
-}\
-
-#define Z_AXIS_CROSS_VEC(dst,src)\
-{					   \
-	dst[0] = -src[1];     \
-	dst[1] = src[0];  \
-	dst[2] = 0.0f;  \
-}\
-
-
-
-
-
+#define X_AXIS_CROSS_VEC(dst, src) \
+	{                              \
+		dst[0] = 0.0f;             \
+		dst[1] = -src[2];          \
+		dst[2] = src[1];           \
+	}
+
+#define Y_AXIS_CROSS_VEC(dst, src) \
+	{                              \
+		dst[0] = src[2];           \
+		dst[1] = 0.0f;             \
+		dst[2] = -src[0];          \
+	}
+
+#define Z_AXIS_CROSS_VEC(dst, src) \
+	{                              \
+		dst[0] = -src[1];          \
+		dst[1] = src[0];           \
+		dst[2] = 0.0f;             \
+	}
 
 /// initialize matrix
-#define IDENTIFY_MATRIX_3X3(m)			\
-{						\
-   m[0][0] = 1.0;				\
-   m[0][1] = 0.0;				\
-   m[0][2] = 0.0;				\
-						\
-   m[1][0] = 0.0;				\
-   m[1][1] = 1.0;				\
-   m[1][2] = 0.0;				\
-						\
-   m[2][0] = 0.0;				\
-   m[2][1] = 0.0;				\
-   m[2][2] = 1.0;				\
-}\
+#define IDENTIFY_MATRIX_3X3(m) \
+	{                          \
+		m[0][0] = 1.0;         \
+		m[0][1] = 0.0;         \
+		m[0][2] = 0.0;         \
+                               \
+		m[1][0] = 0.0;         \
+		m[1][1] = 1.0;         \
+		m[1][2] = 0.0;         \
+                               \
+		m[2][0] = 0.0;         \
+		m[2][1] = 0.0;         \
+		m[2][2] = 1.0;         \
+	}
 
 /*! initialize matrix */
-#define IDENTIFY_MATRIX_4X4(m)			\
-{						\
-   m[0][0] = 1.0;				\
-   m[0][1] = 0.0;				\
-   m[0][2] = 0.0;				\
-   m[0][3] = 0.0;				\
-						\
-   m[1][0] = 0.0;				\
-   m[1][1] = 1.0;				\
-   m[1][2] = 0.0;				\
-   m[1][3] = 0.0;				\
-						\
-   m[2][0] = 0.0;				\
-   m[2][1] = 0.0;				\
-   m[2][2] = 1.0;				\
-   m[2][3] = 0.0;				\
-						\
-   m[3][0] = 0.0;				\
-   m[3][1] = 0.0;				\
-   m[3][2] = 0.0;				\
-   m[3][3] = 1.0;				\
-}\
+#define IDENTIFY_MATRIX_4X4(m) \
+	{                          \
+		m[0][0] = 1.0;         \
+		m[0][1] = 0.0;         \
+		m[0][2] = 0.0;         \
+		m[0][3] = 0.0;         \
+                               \
+		m[1][0] = 0.0;         \
+		m[1][1] = 1.0;         \
+		m[1][2] = 0.0;         \
+		m[1][3] = 0.0;         \
+                               \
+		m[2][0] = 0.0;         \
+		m[2][1] = 0.0;         \
+		m[2][2] = 1.0;         \
+		m[2][3] = 0.0;         \
+                               \
+		m[3][0] = 0.0;         \
+		m[3][1] = 0.0;         \
+		m[3][2] = 0.0;         \
+		m[3][3] = 1.0;         \
+	}
 
 /*! initialize matrix */
-#define ZERO_MATRIX_4X4(m)			\
-{						\
-   m[0][0] = 0.0;				\
-   m[0][1] = 0.0;				\
-   m[0][2] = 0.0;				\
-   m[0][3] = 0.0;				\
-						\
-   m[1][0] = 0.0;				\
-   m[1][1] = 0.0;				\
-   m[1][2] = 0.0;				\
-   m[1][3] = 0.0;				\
-						\
-   m[2][0] = 0.0;				\
-   m[2][1] = 0.0;				\
-   m[2][2] = 0.0;				\
-   m[2][3] = 0.0;				\
-						\
-   m[3][0] = 0.0;				\
-   m[3][1] = 0.0;				\
-   m[3][2] = 0.0;				\
-   m[3][3] = 0.0;				\
-}\
+#define ZERO_MATRIX_4X4(m) \
+	{                      \
+		m[0][0] = 0.0;     \
+		m[0][1] = 0.0;     \
+		m[0][2] = 0.0;     \
+		m[0][3] = 0.0;     \
+                           \
+		m[1][0] = 0.0;     \
+		m[1][1] = 0.0;     \
+		m[1][2] = 0.0;     \
+		m[1][3] = 0.0;     \
+                           \
+		m[2][0] = 0.0;     \
+		m[2][1] = 0.0;     \
+		m[2][2] = 0.0;     \
+		m[2][3] = 0.0;     \
+                           \
+		m[3][0] = 0.0;     \
+		m[3][1] = 0.0;     \
+		m[3][2] = 0.0;     \
+		m[3][3] = 0.0;     \
+	}
 
 /*! matrix rotation  X */
-#define ROTX_CS(m,cosine,sine)		\
-{					\
-   /* rotation about the x-axis */	\
-					\
-   m[0][0] = 1.0;			\
-   m[0][1] = 0.0;			\
-   m[0][2] = 0.0;			\
-   m[0][3] = 0.0;			\
-					\
-   m[1][0] = 0.0;			\
-   m[1][1] = (cosine);			\
-   m[1][2] = (sine);			\
-   m[1][3] = 0.0;			\
-					\
-   m[2][0] = 0.0;			\
-   m[2][1] = -(sine);			\
-   m[2][2] = (cosine);			\
-   m[2][3] = 0.0;			\
-					\
-   m[3][0] = 0.0;			\
-   m[3][1] = 0.0;			\
-   m[3][2] = 0.0;			\
-   m[3][3] = 1.0;			\
-}\
+#define ROTX_CS(m, cosine, sine)        \
+	{                                   \
+		/* rotation about the x-axis */ \
+                                        \
+		m[0][0] = 1.0;                  \
+		m[0][1] = 0.0;                  \
+		m[0][2] = 0.0;                  \
+		m[0][3] = 0.0;                  \
+                                        \
+		m[1][0] = 0.0;                  \
+		m[1][1] = (cosine);             \
+		m[1][2] = (sine);               \
+		m[1][3] = 0.0;                  \
+                                        \
+		m[2][0] = 0.0;                  \
+		m[2][1] = -(sine);              \
+		m[2][2] = (cosine);             \
+		m[2][3] = 0.0;                  \
+                                        \
+		m[3][0] = 0.0;                  \
+		m[3][1] = 0.0;                  \
+		m[3][2] = 0.0;                  \
+		m[3][3] = 1.0;                  \
+	}
 
 /*! matrix rotation  Y */
-#define ROTY_CS(m,cosine,sine)		\
-{					\
-   /* rotation about the y-axis */	\
-					\
-   m[0][0] = (cosine);			\
-   m[0][1] = 0.0;			\
-   m[0][2] = -(sine);			\
-   m[0][3] = 0.0;			\
-					\
-   m[1][0] = 0.0;			\
-   m[1][1] = 1.0;			\
-   m[1][2] = 0.0;			\
-   m[1][3] = 0.0;			\
-					\
-   m[2][0] = (sine);			\
-   m[2][1] = 0.0;			\
-   m[2][2] = (cosine);			\
-   m[2][3] = 0.0;			\
-					\
-   m[3][0] = 0.0;			\
-   m[3][1] = 0.0;			\
-   m[3][2] = 0.0;			\
-   m[3][3] = 1.0;			\
-}\
+#define ROTY_CS(m, cosine, sine)        \
+	{                                   \
+		/* rotation about the y-axis */ \
+                                        \
+		m[0][0] = (cosine);             \
+		m[0][1] = 0.0;                  \
+		m[0][2] = -(sine);              \
+		m[0][3] = 0.0;                  \
+                                        \
+		m[1][0] = 0.0;                  \
+		m[1][1] = 1.0;                  \
+		m[1][2] = 0.0;                  \
+		m[1][3] = 0.0;                  \
+                                        \
+		m[2][0] = (sine);               \
+		m[2][1] = 0.0;                  \
+		m[2][2] = (cosine);             \
+		m[2][3] = 0.0;                  \
+                                        \
+		m[3][0] = 0.0;                  \
+		m[3][1] = 0.0;                  \
+		m[3][2] = 0.0;                  \
+		m[3][3] = 1.0;                  \
+	}
 
 /*! matrix rotation  Z */
-#define ROTZ_CS(m,cosine,sine)		\
-{					\
-   /* rotation about the z-axis */	\
-					\
-   m[0][0] = (cosine);			\
-   m[0][1] = (sine);			\
-   m[0][2] = 0.0;			\
-   m[0][3] = 0.0;			\
-					\
-   m[1][0] = -(sine);			\
-   m[1][1] = (cosine);			\
-   m[1][2] = 0.0;			\
-   m[1][3] = 0.0;			\
-					\
-   m[2][0] = 0.0;			\
-   m[2][1] = 0.0;			\
-   m[2][2] = 1.0;			\
-   m[2][3] = 0.0;			\
-					\
-   m[3][0] = 0.0;			\
-   m[3][1] = 0.0;			\
-   m[3][2] = 0.0;			\
-   m[3][3] = 1.0;			\
-}\
+#define ROTZ_CS(m, cosine, sine)        \
+	{                                   \
+		/* rotation about the z-axis */ \
+                                        \
+		m[0][0] = (cosine);             \
+		m[0][1] = (sine);               \
+		m[0][2] = 0.0;                  \
+		m[0][3] = 0.0;                  \
+                                        \
+		m[1][0] = -(sine);              \
+		m[1][1] = (cosine);             \
+		m[1][2] = 0.0;                  \
+		m[1][3] = 0.0;                  \
+                                        \
+		m[2][0] = 0.0;                  \
+		m[2][1] = 0.0;                  \
+		m[2][2] = 1.0;                  \
+		m[2][3] = 0.0;                  \
+                                        \
+		m[3][0] = 0.0;                  \
+		m[3][1] = 0.0;                  \
+		m[3][2] = 0.0;                  \
+		m[3][3] = 1.0;                  \
+	}
 
 /*! matrix copy */
-#define COPY_MATRIX_2X2(b,a)	\
-{				\
-   b[0][0] = a[0][0];		\
-   b[0][1] = a[0][1];		\
-				\
-   b[1][0] = a[1][0];		\
-   b[1][1] = a[1][1];		\
-				\
-}\
-
+#define COPY_MATRIX_2X2(b, a) \
+	{                         \
+		b[0][0] = a[0][0];    \
+		b[0][1] = a[0][1];    \
+                              \
+		b[1][0] = a[1][0];    \
+		b[1][1] = a[1][1];    \
+	}
 
 /*! matrix copy */
-#define COPY_MATRIX_2X3(b,a)	\
-{				\
-   b[0][0] = a[0][0];		\
-   b[0][1] = a[0][1];		\
-   b[0][2] = a[0][2];		\
-				\
-   b[1][0] = a[1][0];		\
-   b[1][1] = a[1][1];		\
-   b[1][2] = a[1][2];		\
-}\
-
+#define COPY_MATRIX_2X3(b, a) \
+	{                         \
+		b[0][0] = a[0][0];    \
+		b[0][1] = a[0][1];    \
+		b[0][2] = a[0][2];    \
+                              \
+		b[1][0] = a[1][0];    \
+		b[1][1] = a[1][1];    \
+		b[1][2] = a[1][2];    \
+	}
 
 /*! matrix copy */
-#define COPY_MATRIX_3X3(b,a)	\
-{				\
-   b[0][0] = a[0][0];		\
-   b[0][1] = a[0][1];		\
-   b[0][2] = a[0][2];		\
-				\
-   b[1][0] = a[1][0];		\
-   b[1][1] = a[1][1];		\
-   b[1][2] = a[1][2];		\
-				\
-   b[2][0] = a[2][0];		\
-   b[2][1] = a[2][1];		\
-   b[2][2] = a[2][2];		\
-}\
-
+#define COPY_MATRIX_3X3(b, a) \
+	{                         \
+		b[0][0] = a[0][0];    \
+		b[0][1] = a[0][1];    \
+		b[0][2] = a[0][2];    \
+                              \
+		b[1][0] = a[1][0];    \
+		b[1][1] = a[1][1];    \
+		b[1][2] = a[1][2];    \
+                              \
+		b[2][0] = a[2][0];    \
+		b[2][1] = a[2][1];    \
+		b[2][2] = a[2][2];    \
+	}
 
 /*! matrix copy */
-#define COPY_MATRIX_4X4(b,a)	\
-{				\
-   b[0][0] = a[0][0];		\
-   b[0][1] = a[0][1];		\
-   b[0][2] = a[0][2];		\
-   b[0][3] = a[0][3];		\
-				\
-   b[1][0] = a[1][0];		\
-   b[1][1] = a[1][1];		\
-   b[1][2] = a[1][2];		\
-   b[1][3] = a[1][3];		\
-				\
-   b[2][0] = a[2][0];		\
-   b[2][1] = a[2][1];		\
-   b[2][2] = a[2][2];		\
-   b[2][3] = a[2][3];		\
-				\
-   b[3][0] = a[3][0];		\
-   b[3][1] = a[3][1];		\
-   b[3][2] = a[3][2];		\
-   b[3][3] = a[3][3];		\
-}\
-
+#define COPY_MATRIX_4X4(b, a) \
+	{                         \
+		b[0][0] = a[0][0];    \
+		b[0][1] = a[0][1];    \
+		b[0][2] = a[0][2];    \
+		b[0][3] = a[0][3];    \
+                              \
+		b[1][0] = a[1][0];    \
+		b[1][1] = a[1][1];    \
+		b[1][2] = a[1][2];    \
+		b[1][3] = a[1][3];    \
+                              \
+		b[2][0] = a[2][0];    \
+		b[2][1] = a[2][1];    \
+		b[2][2] = a[2][2];    \
+		b[2][3] = a[2][3];    \
+                              \
+		b[3][0] = a[3][0];    \
+		b[3][1] = a[3][1];    \
+		b[3][2] = a[3][2];    \
+		b[3][3] = a[3][3];    \
+	}
 
 /*! matrix transpose */
-#define TRANSPOSE_MATRIX_2X2(b,a)	\
-{				\
-   b[0][0] = a[0][0];		\
-   b[0][1] = a[1][0];		\
-				\
-   b[1][0] = a[0][1];		\
-   b[1][1] = a[1][1];		\
-}\
-
+#define TRANSPOSE_MATRIX_2X2(b, a) \
+	{                              \
+		b[0][0] = a[0][0];         \
+		b[0][1] = a[1][0];         \
+                                   \
+		b[1][0] = a[0][1];         \
+		b[1][1] = a[1][1];         \
+	}
 
 /*! matrix transpose */
-#define TRANSPOSE_MATRIX_3X3(b,a)	\
-{				\
-   b[0][0] = a[0][0];		\
-   b[0][1] = a[1][0];		\
-   b[0][2] = a[2][0];		\
-				\
-   b[1][0] = a[0][1];		\
-   b[1][1] = a[1][1];		\
-   b[1][2] = a[2][1];		\
-				\
-   b[2][0] = a[0][2];		\
-   b[2][1] = a[1][2];		\
-   b[2][2] = a[2][2];		\
-}\
-
+#define TRANSPOSE_MATRIX_3X3(b, a) \
+	{                              \
+		b[0][0] = a[0][0];         \
+		b[0][1] = a[1][0];         \
+		b[0][2] = a[2][0];         \
+                                   \
+		b[1][0] = a[0][1];         \
+		b[1][1] = a[1][1];         \
+		b[1][2] = a[2][1];         \
+                                   \
+		b[2][0] = a[0][2];         \
+		b[2][1] = a[1][2];         \
+		b[2][2] = a[2][2];         \
+	}
 
 /*! matrix transpose */
-#define TRANSPOSE_MATRIX_4X4(b,a)	\
-{				\
-   b[0][0] = a[0][0];		\
-   b[0][1] = a[1][0];		\
-   b[0][2] = a[2][0];		\
-   b[0][3] = a[3][0];		\
-				\
-   b[1][0] = a[0][1];		\
-   b[1][1] = a[1][1];		\
-   b[1][2] = a[2][1];		\
-   b[1][3] = a[3][1];		\
-				\
-   b[2][0] = a[0][2];		\
-   b[2][1] = a[1][2];		\
-   b[2][2] = a[2][2];		\
-   b[2][3] = a[3][2];		\
-				\
-   b[3][0] = a[0][3];		\
-   b[3][1] = a[1][3];		\
-   b[3][2] = a[2][3];		\
-   b[3][3] = a[3][3];		\
-}\
-
+#define TRANSPOSE_MATRIX_4X4(b, a) \
+	{                              \
+		b[0][0] = a[0][0];         \
+		b[0][1] = a[1][0];         \
+		b[0][2] = a[2][0];         \
+		b[0][3] = a[3][0];         \
+                                   \
+		b[1][0] = a[0][1];         \
+		b[1][1] = a[1][1];         \
+		b[1][2] = a[2][1];         \
+		b[1][3] = a[3][1];         \
+                                   \
+		b[2][0] = a[0][2];         \
+		b[2][1] = a[1][2];         \
+		b[2][2] = a[2][2];         \
+		b[2][3] = a[3][2];         \
+                                   \
+		b[3][0] = a[0][3];         \
+		b[3][1] = a[1][3];         \
+		b[3][2] = a[2][3];         \
+		b[3][3] = a[3][3];         \
+	}
 
 /*! multiply matrix by scalar */
-#define SCALE_MATRIX_2X2(b,s,a)		\
-{					\
-   b[0][0] = (s) * a[0][0];		\
-   b[0][1] = (s) * a[0][1];		\
-					\
-   b[1][0] = (s) * a[1][0];		\
-   b[1][1] = (s) * a[1][1];		\
-}\
-
+#define SCALE_MATRIX_2X2(b, s, a) \
+	{                             \
+		b[0][0] = (s)*a[0][0];    \
+		b[0][1] = (s)*a[0][1];    \
+                                  \
+		b[1][0] = (s)*a[1][0];    \
+		b[1][1] = (s)*a[1][1];    \
+	}
 
 /*! multiply matrix by scalar */
-#define SCALE_MATRIX_3X3(b,s,a)		\
-{					\
-   b[0][0] = (s) * a[0][0];		\
-   b[0][1] = (s) * a[0][1];		\
-   b[0][2] = (s) * a[0][2];		\
-					\
-   b[1][0] = (s) * a[1][0];		\
-   b[1][1] = (s) * a[1][1];		\
-   b[1][2] = (s) * a[1][2];		\
-					\
-   b[2][0] = (s) * a[2][0];		\
-   b[2][1] = (s) * a[2][1];		\
-   b[2][2] = (s) * a[2][2];		\
-}\
-
+#define SCALE_MATRIX_3X3(b, s, a) \
+	{                             \
+		b[0][0] = (s)*a[0][0];    \
+		b[0][1] = (s)*a[0][1];    \
+		b[0][2] = (s)*a[0][2];    \
+                                  \
+		b[1][0] = (s)*a[1][0];    \
+		b[1][1] = (s)*a[1][1];    \
+		b[1][2] = (s)*a[1][2];    \
+                                  \
+		b[2][0] = (s)*a[2][0];    \
+		b[2][1] = (s)*a[2][1];    \
+		b[2][2] = (s)*a[2][2];    \
+	}
 
 /*! multiply matrix by scalar */
-#define SCALE_MATRIX_4X4(b,s,a)		\
-{					\
-   b[0][0] = (s) * a[0][0];		\
-   b[0][1] = (s) * a[0][1];		\
-   b[0][2] = (s) * a[0][2];		\
-   b[0][3] = (s) * a[0][3];		\
-					\
-   b[1][0] = (s) * a[1][0];		\
-   b[1][1] = (s) * a[1][1];		\
-   b[1][2] = (s) * a[1][2];		\
-   b[1][3] = (s) * a[1][3];		\
-					\
-   b[2][0] = (s) * a[2][0];		\
-   b[2][1] = (s) * a[2][1];		\
-   b[2][2] = (s) * a[2][2];		\
-   b[2][3] = (s) * a[2][3];		\
-					\
-   b[3][0] = s * a[3][0];		\
-   b[3][1] = s * a[3][1];		\
-   b[3][2] = s * a[3][2];		\
-   b[3][3] = s * a[3][3];		\
-}\
-
+#define SCALE_MATRIX_4X4(b, s, a) \
+	{                             \
+		b[0][0] = (s)*a[0][0];    \
+		b[0][1] = (s)*a[0][1];    \
+		b[0][2] = (s)*a[0][2];    \
+		b[0][3] = (s)*a[0][3];    \
+                                  \
+		b[1][0] = (s)*a[1][0];    \
+		b[1][1] = (s)*a[1][1];    \
+		b[1][2] = (s)*a[1][2];    \
+		b[1][3] = (s)*a[1][3];    \
+                                  \
+		b[2][0] = (s)*a[2][0];    \
+		b[2][1] = (s)*a[2][1];    \
+		b[2][2] = (s)*a[2][2];    \
+		b[2][3] = (s)*a[2][3];    \
+                                  \
+		b[3][0] = s * a[3][0];    \
+		b[3][1] = s * a[3][1];    \
+		b[3][2] = s * a[3][2];    \
+		b[3][3] = s * a[3][3];    \
+	}
 
 /*! multiply matrix by scalar */
-#define SCALE_VEC_MATRIX_2X2(b,svec,a)		\
-{					\
-   b[0][0] = svec[0] * a[0][0];		\
-   b[1][0] = svec[0] * a[1][0];		\
-					\
-   b[0][1] = svec[1] * a[0][1];		\
-   b[1][1] = svec[1] * a[1][1];		\
-}\
-
+#define SCALE_VEC_MATRIX_2X2(b, svec, a) \
+	{                                    \
+		b[0][0] = svec[0] * a[0][0];     \
+		b[1][0] = svec[0] * a[1][0];     \
+                                         \
+		b[0][1] = svec[1] * a[0][1];     \
+		b[1][1] = svec[1] * a[1][1];     \
+	}
 
 /*! multiply matrix by scalar. Each columns is scaled by each scalar vector component */
-#define SCALE_VEC_MATRIX_3X3(b,svec,a)		\
-{					\
-   b[0][0] = svec[0] * a[0][0];		\
-   b[1][0] = svec[0] * a[1][0];		\
-   b[2][0] = svec[0] * a[2][0];		\
-					\
-   b[0][1] = svec[1] * a[0][1];		\
-   b[1][1] = svec[1] * a[1][1];		\
-   b[2][1] = svec[1] * a[2][1];		\
-					\
-   b[0][2] = svec[2] * a[0][2];		\
-   b[1][2] = svec[2] * a[1][2];		\
-   b[2][2] = svec[2] * a[2][2];		\
-}\
-
+#define SCALE_VEC_MATRIX_3X3(b, svec, a) \
+	{                                    \
+		b[0][0] = svec[0] * a[0][0];     \
+		b[1][0] = svec[0] * a[1][0];     \
+		b[2][0] = svec[0] * a[2][0];     \
+                                         \
+		b[0][1] = svec[1] * a[0][1];     \
+		b[1][1] = svec[1] * a[1][1];     \
+		b[2][1] = svec[1] * a[2][1];     \
+                                         \
+		b[0][2] = svec[2] * a[0][2];     \
+		b[1][2] = svec[2] * a[1][2];     \
+		b[2][2] = svec[2] * a[2][2];     \
+	}
 
 /*! multiply matrix by scalar */
-#define SCALE_VEC_MATRIX_4X4(b,svec,a)		\
-{					\
-   b[0][0] = svec[0] * a[0][0];		\
-   b[1][0] = svec[0] * a[1][0];		\
-   b[2][0] = svec[0] * a[2][0];		\
-   b[3][0] = svec[0] * a[3][0];		\
-					\
-   b[0][1] = svec[1] * a[0][1];		\
-   b[1][1] = svec[1] * a[1][1];		\
-   b[2][1] = svec[1] * a[2][1];		\
-   b[3][1] = svec[1] * a[3][1];		\
-					\
-   b[0][2] = svec[2] * a[0][2];		\
-   b[1][2] = svec[2] * a[1][2];		\
-   b[2][2] = svec[2] * a[2][2];		\
-   b[3][2] = svec[2] * a[3][2];		\
-   \
-   b[0][3] = svec[3] * a[0][3];		\
-   b[1][3] = svec[3] * a[1][3];		\
-   b[2][3] = svec[3] * a[2][3];		\
-   b[3][3] = svec[3] * a[3][3];		\
-}\
-
+#define SCALE_VEC_MATRIX_4X4(b, svec, a) \
+	{                                    \
+		b[0][0] = svec[0] * a[0][0];     \
+		b[1][0] = svec[0] * a[1][0];     \
+		b[2][0] = svec[0] * a[2][0];     \
+		b[3][0] = svec[0] * a[3][0];     \
+                                         \
+		b[0][1] = svec[1] * a[0][1];     \
+		b[1][1] = svec[1] * a[1][1];     \
+		b[2][1] = svec[1] * a[2][1];     \
+		b[3][1] = svec[1] * a[3][1];     \
+                                         \
+		b[0][2] = svec[2] * a[0][2];     \
+		b[1][2] = svec[2] * a[1][2];     \
+		b[2][2] = svec[2] * a[2][2];     \
+		b[3][2] = svec[2] * a[3][2];     \
+                                         \
+		b[0][3] = svec[3] * a[0][3];     \
+		b[1][3] = svec[3] * a[1][3];     \
+		b[2][3] = svec[3] * a[2][3];     \
+		b[3][3] = svec[3] * a[3][3];     \
+	}
 
 /*! multiply matrix by scalar */
-#define ACCUM_SCALE_MATRIX_2X2(b,s,a)		\
-{					\
-   b[0][0] += (s) * a[0][0];		\
-   b[0][1] += (s) * a[0][1];		\
-					\
-   b[1][0] += (s) * a[1][0];		\
-   b[1][1] += (s) * a[1][1];		\
-}\
-
+#define ACCUM_SCALE_MATRIX_2X2(b, s, a) \
+	{                                   \
+		b[0][0] += (s)*a[0][0];         \
+		b[0][1] += (s)*a[0][1];         \
+                                        \
+		b[1][0] += (s)*a[1][0];         \
+		b[1][1] += (s)*a[1][1];         \
+	}
 
 /*! multiply matrix by scalar */
-#define ACCUM_SCALE_MATRIX_3X3(b,s,a)		\
-{					\
-   b[0][0] += (s) * a[0][0];		\
-   b[0][1] += (s) * a[0][1];		\
-   b[0][2] += (s) * a[0][2];		\
-					\
-   b[1][0] += (s) * a[1][0];		\
-   b[1][1] += (s) * a[1][1];		\
-   b[1][2] += (s) * a[1][2];		\
-					\
-   b[2][0] += (s) * a[2][0];		\
-   b[2][1] += (s) * a[2][1];		\
-   b[2][2] += (s) * a[2][2];		\
-}\
-
+#define ACCUM_SCALE_MATRIX_3X3(b, s, a) \
+	{                                   \
+		b[0][0] += (s)*a[0][0];         \
+		b[0][1] += (s)*a[0][1];         \
+		b[0][2] += (s)*a[0][2];         \
+                                        \
+		b[1][0] += (s)*a[1][0];         \
+		b[1][1] += (s)*a[1][1];         \
+		b[1][2] += (s)*a[1][2];         \
+                                        \
+		b[2][0] += (s)*a[2][0];         \
+		b[2][1] += (s)*a[2][1];         \
+		b[2][2] += (s)*a[2][2];         \
+	}
 
 /*! multiply matrix by scalar */
-#define ACCUM_SCALE_MATRIX_4X4(b,s,a)		\
-{					\
-   b[0][0] += (s) * a[0][0];		\
-   b[0][1] += (s) * a[0][1];		\
-   b[0][2] += (s) * a[0][2];		\
-   b[0][3] += (s) * a[0][3];		\
-					\
-   b[1][0] += (s) * a[1][0];		\
-   b[1][1] += (s) * a[1][1];		\
-   b[1][2] += (s) * a[1][2];		\
-   b[1][3] += (s) * a[1][3];		\
-					\
-   b[2][0] += (s) * a[2][0];		\
-   b[2][1] += (s) * a[2][1];		\
-   b[2][2] += (s) * a[2][2];		\
-   b[2][3] += (s) * a[2][3];		\
-					\
-   b[3][0] += (s) * a[3][0];		\
-   b[3][1] += (s) * a[3][1];		\
-   b[3][2] += (s) * a[3][2];		\
-   b[3][3] += (s) * a[3][3];		\
-}\
+#define ACCUM_SCALE_MATRIX_4X4(b, s, a) \
+	{                                   \
+		b[0][0] += (s)*a[0][0];         \
+		b[0][1] += (s)*a[0][1];         \
+		b[0][2] += (s)*a[0][2];         \
+		b[0][3] += (s)*a[0][3];         \
+                                        \
+		b[1][0] += (s)*a[1][0];         \
+		b[1][1] += (s)*a[1][1];         \
+		b[1][2] += (s)*a[1][2];         \
+		b[1][3] += (s)*a[1][3];         \
+                                        \
+		b[2][0] += (s)*a[2][0];         \
+		b[2][1] += (s)*a[2][1];         \
+		b[2][2] += (s)*a[2][2];         \
+		b[2][3] += (s)*a[2][3];         \
+                                        \
+		b[3][0] += (s)*a[3][0];         \
+		b[3][1] += (s)*a[3][1];         \
+		b[3][2] += (s)*a[3][2];         \
+		b[3][3] += (s)*a[3][3];         \
+	}
 
 /*! matrix product */
 /*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
-#define MATRIX_PRODUCT_2X2(c,a,b)		\
-{						\
-   c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0];	\
-   c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1];	\
-						\
-   c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0];	\
-   c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1];	\
-						\
-}\
+#define MATRIX_PRODUCT_2X2(c, a, b)                      \
+	{                                                    \
+		c[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0]; \
+		c[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1]; \
+                                                         \
+		c[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0]; \
+		c[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1]; \
+	}
 
 /*! matrix product */
 /*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
-#define MATRIX_PRODUCT_3X3(c,a,b)				\
-{								\
-   c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0];	\
-   c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1];	\
-   c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2];	\
-								\
-   c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0];	\
-   c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1];	\
-   c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2];	\
-								\
-   c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0];	\
-   c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1];	\
-   c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2];	\
-}\
-
+#define MATRIX_PRODUCT_3X3(c, a, b)                                          \
+	{                                                                        \
+		c[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2] * b[2][0]; \
+		c[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2] * b[2][1]; \
+		c[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2] * b[2][2]; \
+                                                                             \
+		c[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2] * b[2][0]; \
+		c[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2] * b[2][1]; \
+		c[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2] * b[2][2]; \
+                                                                             \
+		c[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2] * b[2][0]; \
+		c[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2] * b[2][1]; \
+		c[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2] * b[2][2]; \
+	}
 
 /*! matrix product */
 /*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
-#define MATRIX_PRODUCT_4X4(c,a,b)		\
-{						\
-   c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];\
-   c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]+a[0][3]*b[3][1];\
-   c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]+a[0][3]*b[3][2];\
-   c[0][3] = a[0][0]*b[0][3]+a[0][1]*b[1][3]+a[0][2]*b[2][3]+a[0][3]*b[3][3];\
-						\
-   c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]+a[1][3]*b[3][0];\
-   c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]+a[1][3]*b[3][1];\
-   c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]+a[1][3]*b[3][2];\
-   c[1][3] = a[1][0]*b[0][3]+a[1][1]*b[1][3]+a[1][2]*b[2][3]+a[1][3]*b[3][3];\
-						\
-   c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]+a[2][3]*b[3][0];\
-   c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]+a[2][3]*b[3][1];\
-   c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]+a[2][3]*b[3][2];\
-   c[2][3] = a[2][0]*b[0][3]+a[2][1]*b[1][3]+a[2][2]*b[2][3]+a[2][3]*b[3][3];\
-						\
-   c[3][0] = a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+a[3][3]*b[3][0];\
-   c[3][1] = a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+a[3][3]*b[3][1];\
-   c[3][2] = a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+a[3][3]*b[3][2];\
-   c[3][3] = a[3][0]*b[0][3]+a[3][1]*b[1][3]+a[3][2]*b[2][3]+a[3][3]*b[3][3];\
-}\
-
+#define MATRIX_PRODUCT_4X4(c, a, b)                                                              \
+	{                                                                                            \
+		c[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2] * b[2][0] + a[0][3] * b[3][0]; \
+		c[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2] * b[2][1] + a[0][3] * b[3][1]; \
+		c[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2] * b[2][2] + a[0][3] * b[3][2]; \
+		c[0][3] = a[0][0] * b[0][3] + a[0][1] * b[1][3] + a[0][2] * b[2][3] + a[0][3] * b[3][3]; \
+                                                                                                 \
+		c[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2] * b[2][0] + a[1][3] * b[3][0]; \
+		c[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2] * b[2][1] + a[1][3] * b[3][1]; \
+		c[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2] * b[2][2] + a[1][3] * b[3][2]; \
+		c[1][3] = a[1][0] * b[0][3] + a[1][1] * b[1][3] + a[1][2] * b[2][3] + a[1][3] * b[3][3]; \
+                                                                                                 \
+		c[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2] * b[2][0] + a[2][3] * b[3][0]; \
+		c[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2] * b[2][1] + a[2][3] * b[3][1]; \
+		c[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2] * b[2][2] + a[2][3] * b[3][2]; \
+		c[2][3] = a[2][0] * b[0][3] + a[2][1] * b[1][3] + a[2][2] * b[2][3] + a[2][3] * b[3][3]; \
+                                                                                                 \
+		c[3][0] = a[3][0] * b[0][0] + a[3][1] * b[1][0] + a[3][2] * b[2][0] + a[3][3] * b[3][0]; \
+		c[3][1] = a[3][0] * b[0][1] + a[3][1] * b[1][1] + a[3][2] * b[2][1] + a[3][3] * b[3][1]; \
+		c[3][2] = a[3][0] * b[0][2] + a[3][1] * b[1][2] + a[3][2] * b[2][2] + a[3][3] * b[3][2]; \
+		c[3][3] = a[3][0] * b[0][3] + a[3][1] * b[1][3] + a[3][2] * b[2][3] + a[3][3] * b[3][3]; \
+	}
 
 /*! matrix times vector */
-#define MAT_DOT_VEC_2X2(p,m,v)					\
-{								\
-   p[0] = m[0][0]*v[0] + m[0][1]*v[1];				\
-   p[1] = m[1][0]*v[0] + m[1][1]*v[1];				\
-}\
-
+#define MAT_DOT_VEC_2X2(p, m, v)                \
+	{                                           \
+		p[0] = m[0][0] * v[0] + m[0][1] * v[1]; \
+		p[1] = m[1][0] * v[0] + m[1][1] * v[1]; \
+	}
 
 /*! matrix times vector */
-#define MAT_DOT_VEC_3X3(p,m,v)					\
-{								\
-   p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];		\
-   p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];		\
-   p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];		\
-}\
-
+#define MAT_DOT_VEC_3X3(p, m, v)                                 \
+	{                                                            \
+		p[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2]; \
+		p[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2]; \
+		p[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2]; \
+	}
 
 /*! matrix times vector
 v is a vec4f
 */
-#define MAT_DOT_VEC_4X4(p,m,v)					\
-{								\
-   p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3];	\
-   p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3];	\
-   p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3];	\
-   p[3] = m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3];	\
-}\
+#define MAT_DOT_VEC_4X4(p, m, v)                                                  \
+	{                                                                             \
+		p[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2] + m[0][3] * v[3]; \
+		p[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2] + m[1][3] * v[3]; \
+		p[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2] + m[2][3] * v[3]; \
+		p[3] = m[3][0] * v[0] + m[3][1] * v[1] + m[3][2] * v[2] + m[3][3] * v[3]; \
+	}
 
 /*! matrix times vector
 v is a vec3f
 and m is a mat4f<br>
 Last column is added as the position
 */
-#define MAT_DOT_VEC_3X4(p,m,v)					\
-{								\
-   p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3];	\
-   p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3];	\
-   p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3];	\
-}\
-
+#define MAT_DOT_VEC_3X4(p, m, v)                                           \
+	{                                                                      \
+		p[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2] + m[0][3]; \
+		p[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2] + m[1][3]; \
+		p[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2] + m[2][3]; \
+	}
 
 /*! vector transpose times matrix */
 /*! p[j] = v[0]*m[0][j] + v[1]*m[1][j] + v[2]*m[2][j]; */
-#define VEC_DOT_MAT_3X3(p,v,m)					\
-{								\
-   p[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0];		\
-   p[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1];		\
-   p[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2];		\
-}\
-
+#define VEC_DOT_MAT_3X3(p, v, m)                                 \
+	{                                                            \
+		p[0] = v[0] * m[0][0] + v[1] * m[1][0] + v[2] * m[2][0]; \
+		p[1] = v[0] * m[0][1] + v[1] * m[1][1] + v[2] * m[2][1]; \
+		p[2] = v[0] * m[0][2] + v[1] * m[1][2] + v[2] * m[2][2]; \
+	}
 
 /*! affine matrix times vector */
 /** The matrix is assumed to be an affine matrix, with last two
  * entries representing a translation */
-#define MAT_DOT_VEC_2X3(p,m,v)					\
-{								\
-   p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2];		\
-   p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2];		\
-}\
+#define MAT_DOT_VEC_2X3(p, m, v)                          \
+	{                                                     \
+		p[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2]; \
+		p[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2]; \
+	}
 
 //! Transform a plane
-#define MAT_TRANSFORM_PLANE_4X4(pout,m,plane)\
-{								\
-   pout[0] = m[0][0]*plane[0] + m[0][1]*plane[1]  + m[0][2]*plane[2];\
-   pout[1] = m[1][0]*plane[0] + m[1][1]*plane[1]  + m[1][2]*plane[2];\
-   pout[2] = m[2][0]*plane[0] + m[2][1]*plane[1]  + m[2][2]*plane[2];\
-   pout[3] = m[0][3]*pout[0] + m[1][3]*pout[1]  + m[2][3]*pout[2] + plane[3];\
-}\
-
-
+#define MAT_TRANSFORM_PLANE_4X4(pout, m, plane)                                         \
+	{                                                                                   \
+		pout[0] = m[0][0] * plane[0] + m[0][1] * plane[1] + m[0][2] * plane[2];         \
+		pout[1] = m[1][0] * plane[0] + m[1][1] * plane[1] + m[1][2] * plane[2];         \
+		pout[2] = m[2][0] * plane[0] + m[2][1] * plane[1] + m[2][2] * plane[2];         \
+		pout[3] = m[0][3] * pout[0] + m[1][3] * pout[1] + m[2][3] * pout[2] + plane[3]; \
+	}
 
 /** inverse transpose of matrix times vector
  *
@@ -1000,22 +938,22 @@ Last column is added as the position
  * It will leave normals the wrong length !!!
  * See macro below for use on normals.
  */
-#define INV_TRANSP_MAT_DOT_VEC_2X2(p,m,v)			\
-{								\
-   GREAL det;						\
-								\
-   det = m[0][0]*m[1][1] - m[0][1]*m[1][0];			\
-   p[0] = m[1][1]*v[0] - m[1][0]*v[1];				\
-   p[1] = - m[0][1]*v[0] + m[0][0]*v[1];			\
-								\
-   /* if matrix not singular, and not orthonormal, then renormalize */ \
-   if ((det!=1.0f) && (det != 0.0f)) {				\
-      det = 1.0f / det;						\
-      p[0] *= det;						\
-      p[1] *= det;						\
-   }								\
-}\
-
+#define INV_TRANSP_MAT_DOT_VEC_2X2(p, m, v)                                 \
+	{                                                                       \
+		GREAL det;                                                          \
+                                                                            \
+		det = m[0][0] * m[1][1] - m[0][1] * m[1][0];                        \
+		p[0] = m[1][1] * v[0] - m[1][0] * v[1];                             \
+		p[1] = -m[0][1] * v[0] + m[0][0] * v[1];                            \
+                                                                            \
+		/* if matrix not singular, and not orthonormal, then renormalize */ \
+		if ((det != 1.0f) && (det != 0.0f))                                 \
+		{                                                                   \
+			det = 1.0f / det;                                               \
+			p[0] *= det;                                                    \
+			p[1] *= det;                                                    \
+		}                                                                   \
+	}
 
 /** transform normal vector by inverse transpose of matrix
  * and then renormalize the vector
@@ -1024,550 +962,527 @@ Last column is added as the position
  * and multiplies vector v into it, to yeild vector p
  * Vector p is then normalized.
  */
-#define NORM_XFORM_2X2(p,m,v)					\
-{								\
-   GREAL len;							\
-								\
-   /* do nothing if off-diagonals are zero and diagonals are 	\
-    * equal */							\
-   if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) { \
-      p[0] = m[1][1]*v[0] - m[1][0]*v[1];			\
-      p[1] = - m[0][1]*v[0] + m[0][0]*v[1];			\
-								\
-      len = p[0]*p[0] + p[1]*p[1];				\
-      GIM_INV_SQRT(len,len);					\
-      p[0] *= len;						\
-      p[1] *= len;						\
-   } else {							\
-      VEC_COPY_2 (p, v);					\
-   }								\
-}\
-
+#define NORM_XFORM_2X2(p, m, v)                                           \
+	{                                                                     \
+		GREAL len;                                                        \
+                                                                          \
+		/* do nothing if off-diagonals are zero and diagonals are 	\
+    * equal */      \
+		if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) \
+		{                                                                 \
+			p[0] = m[1][1] * v[0] - m[1][0] * v[1];                       \
+			p[1] = -m[0][1] * v[0] + m[0][0] * v[1];                      \
+                                                                          \
+			len = p[0] * p[0] + p[1] * p[1];                              \
+			GIM_INV_SQRT(len, len);                                       \
+			p[0] *= len;                                                  \
+			p[1] *= len;                                                  \
+		}                                                                 \
+		else                                                              \
+		{                                                                 \
+			VEC_COPY_2(p, v);                                             \
+		}                                                                 \
+	}
 
 /** outer product of vector times vector transpose
  *
  * The outer product of vector v and vector transpose t yeilds
  * dyadic matrix m.
  */
-#define OUTER_PRODUCT_2X2(m,v,t)				\
-{								\
-   m[0][0] = v[0] * t[0];					\
-   m[0][1] = v[0] * t[1];					\
-								\
-   m[1][0] = v[1] * t[0];					\
-   m[1][1] = v[1] * t[1];					\
-}\
-
+#define OUTER_PRODUCT_2X2(m, v, t) \
+	{                              \
+		m[0][0] = v[0] * t[0];     \
+		m[0][1] = v[0] * t[1];     \
+                                   \
+		m[1][0] = v[1] * t[0];     \
+		m[1][1] = v[1] * t[1];     \
+	}
 
 /** outer product of vector times vector transpose
  *
  * The outer product of vector v and vector transpose t yeilds
  * dyadic matrix m.
  */
-#define OUTER_PRODUCT_3X3(m,v,t)				\
-{								\
-   m[0][0] = v[0] * t[0];					\
-   m[0][1] = v[0] * t[1];					\
-   m[0][2] = v[0] * t[2];					\
-								\
-   m[1][0] = v[1] * t[0];					\
-   m[1][1] = v[1] * t[1];					\
-   m[1][2] = v[1] * t[2];					\
-								\
-   m[2][0] = v[2] * t[0];					\
-   m[2][1] = v[2] * t[1];					\
-   m[2][2] = v[2] * t[2];					\
-}\
-
+#define OUTER_PRODUCT_3X3(m, v, t) \
+	{                              \
+		m[0][0] = v[0] * t[0];     \
+		m[0][1] = v[0] * t[1];     \
+		m[0][2] = v[0] * t[2];     \
+                                   \
+		m[1][0] = v[1] * t[0];     \
+		m[1][1] = v[1] * t[1];     \
+		m[1][2] = v[1] * t[2];     \
+                                   \
+		m[2][0] = v[2] * t[0];     \
+		m[2][1] = v[2] * t[1];     \
+		m[2][2] = v[2] * t[2];     \
+	}
 
 /** outer product of vector times vector transpose
  *
  * The outer product of vector v and vector transpose t yeilds
  * dyadic matrix m.
  */
-#define OUTER_PRODUCT_4X4(m,v,t)				\
-{								\
-   m[0][0] = v[0] * t[0];					\
-   m[0][1] = v[0] * t[1];					\
-   m[0][2] = v[0] * t[2];					\
-   m[0][3] = v[0] * t[3];					\
-								\
-   m[1][0] = v[1] * t[0];					\
-   m[1][1] = v[1] * t[1];					\
-   m[1][2] = v[1] * t[2];					\
-   m[1][3] = v[1] * t[3];					\
-								\
-   m[2][0] = v[2] * t[0];					\
-   m[2][1] = v[2] * t[1];					\
-   m[2][2] = v[2] * t[2];					\
-   m[2][3] = v[2] * t[3];					\
-								\
-   m[3][0] = v[3] * t[0];					\
-   m[3][1] = v[3] * t[1];					\
-   m[3][2] = v[3] * t[2];					\
-   m[3][3] = v[3] * t[3];					\
-}\
-
+#define OUTER_PRODUCT_4X4(m, v, t) \
+	{                              \
+		m[0][0] = v[0] * t[0];     \
+		m[0][1] = v[0] * t[1];     \
+		m[0][2] = v[0] * t[2];     \
+		m[0][3] = v[0] * t[3];     \
+                                   \
+		m[1][0] = v[1] * t[0];     \
+		m[1][1] = v[1] * t[1];     \
+		m[1][2] = v[1] * t[2];     \
+		m[1][3] = v[1] * t[3];     \
+                                   \
+		m[2][0] = v[2] * t[0];     \
+		m[2][1] = v[2] * t[1];     \
+		m[2][2] = v[2] * t[2];     \
+		m[2][3] = v[2] * t[3];     \
+                                   \
+		m[3][0] = v[3] * t[0];     \
+		m[3][1] = v[3] * t[1];     \
+		m[3][2] = v[3] * t[2];     \
+		m[3][3] = v[3] * t[3];     \
+	}
 
 /** outer product of vector times vector transpose
  *
  * The outer product of vector v and vector transpose t yeilds
  * dyadic matrix m.
  */
-#define ACCUM_OUTER_PRODUCT_2X2(m,v,t)				\
-{								\
-   m[0][0] += v[0] * t[0];					\
-   m[0][1] += v[0] * t[1];					\
-								\
-   m[1][0] += v[1] * t[0];					\
-   m[1][1] += v[1] * t[1];					\
-}\
-
+#define ACCUM_OUTER_PRODUCT_2X2(m, v, t) \
+	{                                    \
+		m[0][0] += v[0] * t[0];          \
+		m[0][1] += v[0] * t[1];          \
+                                         \
+		m[1][0] += v[1] * t[0];          \
+		m[1][1] += v[1] * t[1];          \
+	}
 
 /** outer product of vector times vector transpose
  *
  * The outer product of vector v and vector transpose t yeilds
  * dyadic matrix m.
  */
-#define ACCUM_OUTER_PRODUCT_3X3(m,v,t)				\
-{								\
-   m[0][0] += v[0] * t[0];					\
-   m[0][1] += v[0] * t[1];					\
-   m[0][2] += v[0] * t[2];					\
-								\
-   m[1][0] += v[1] * t[0];					\
-   m[1][1] += v[1] * t[1];					\
-   m[1][2] += v[1] * t[2];					\
-								\
-   m[2][0] += v[2] * t[0];					\
-   m[2][1] += v[2] * t[1];					\
-   m[2][2] += v[2] * t[2];					\
-}\
-
+#define ACCUM_OUTER_PRODUCT_3X3(m, v, t) \
+	{                                    \
+		m[0][0] += v[0] * t[0];          \
+		m[0][1] += v[0] * t[1];          \
+		m[0][2] += v[0] * t[2];          \
+                                         \
+		m[1][0] += v[1] * t[0];          \
+		m[1][1] += v[1] * t[1];          \
+		m[1][2] += v[1] * t[2];          \
+                                         \
+		m[2][0] += v[2] * t[0];          \
+		m[2][1] += v[2] * t[1];          \
+		m[2][2] += v[2] * t[2];          \
+	}
 
 /** outer product of vector times vector transpose
  *
  * The outer product of vector v and vector transpose t yeilds
  * dyadic matrix m.
  */
-#define ACCUM_OUTER_PRODUCT_4X4(m,v,t)				\
-{								\
-   m[0][0] += v[0] * t[0];					\
-   m[0][1] += v[0] * t[1];					\
-   m[0][2] += v[0] * t[2];					\
-   m[0][3] += v[0] * t[3];					\
-								\
-   m[1][0] += v[1] * t[0];					\
-   m[1][1] += v[1] * t[1];					\
-   m[1][2] += v[1] * t[2];					\
-   m[1][3] += v[1] * t[3];					\
-								\
-   m[2][0] += v[2] * t[0];					\
-   m[2][1] += v[2] * t[1];					\
-   m[2][2] += v[2] * t[2];					\
-   m[2][3] += v[2] * t[3];					\
-								\
-   m[3][0] += v[3] * t[0];					\
-   m[3][1] += v[3] * t[1];					\
-   m[3][2] += v[3] * t[2];					\
-   m[3][3] += v[3] * t[3];					\
-}\
-
+#define ACCUM_OUTER_PRODUCT_4X4(m, v, t) \
+	{                                    \
+		m[0][0] += v[0] * t[0];          \
+		m[0][1] += v[0] * t[1];          \
+		m[0][2] += v[0] * t[2];          \
+		m[0][3] += v[0] * t[3];          \
+                                         \
+		m[1][0] += v[1] * t[0];          \
+		m[1][1] += v[1] * t[1];          \
+		m[1][2] += v[1] * t[2];          \
+		m[1][3] += v[1] * t[3];          \
+                                         \
+		m[2][0] += v[2] * t[0];          \
+		m[2][1] += v[2] * t[1];          \
+		m[2][2] += v[2] * t[2];          \
+		m[2][3] += v[2] * t[3];          \
+                                         \
+		m[3][0] += v[3] * t[0];          \
+		m[3][1] += v[3] * t[1];          \
+		m[3][2] += v[3] * t[2];          \
+		m[3][3] += v[3] * t[3];          \
+	}
 
 /** determinant of matrix
  *
  * Computes determinant of matrix m, returning d
  */
-#define DETERMINANT_2X2(d,m)					\
-{								\
-   d = m[0][0] * m[1][1] - m[0][1] * m[1][0];			\
-}\
-
+#define DETERMINANT_2X2(d, m)                      \
+	{                                              \
+		d = m[0][0] * m[1][1] - m[0][1] * m[1][0]; \
+	}
 
 /** determinant of matrix
  *
  * Computes determinant of matrix m, returning d
  */
-#define DETERMINANT_3X3(d,m)					\
-{								\
-   d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]);		\
-   d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]);	\
-   d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]);	\
-}\
-
+#define DETERMINANT_3X3(d, m)                                   \
+	{                                                           \
+		d = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]);  \
+		d -= m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]); \
+		d += m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); \
+	}
 
 /** i,j,th cofactor of a 4x4 matrix
  *
  */
-#define COFACTOR_4X4_IJ(fac,m,i,j) 				\
-{								\
-   GUINT __ii[4], __jj[4], __k;						\
-								\
-   for (__k=0; __k<i; __k++) __ii[__k] = __k;				\
-   for (__k=i; __k<3; __k++) __ii[__k] = __k+1;				\
-   for (__k=0; __k<j; __k++) __jj[__k] = __k;				\
-   for (__k=j; __k<3; __k++) __jj[__k] = __k+1;				\
-								\
-   (fac) = m[__ii[0]][__jj[0]] * (m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[2]] 	\
-                            - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[1]]); \
-   (fac) -= m[__ii[0]][__jj[1]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[2]]	\
-                             - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[0]]);\
-   (fac) += m[__ii[0]][__jj[2]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[1]]	\
-                             - m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[0]]);\
-								\
-   __k = i+j;							\
-   if ( __k != (__k/2)*2) {						\
-      (fac) = -(fac);						\
-   }								\
-}\
-
+#define COFACTOR_4X4_IJ(fac, m, i, j)                                                                                           \
+	{                                                                                                                           \
+		GUINT __ii[4], __jj[4], __k;                                                                                            \
+                                                                                                                                \
+		for (__k = 0; __k < i; __k++) __ii[__k] = __k;                                                                          \
+		for (__k = i; __k < 3; __k++) __ii[__k] = __k + 1;                                                                      \
+		for (__k = 0; __k < j; __k++) __jj[__k] = __k;                                                                          \
+		for (__k = j; __k < 3; __k++) __jj[__k] = __k + 1;                                                                      \
+                                                                                                                                \
+		(fac) = m[__ii[0]][__jj[0]] * (m[__ii[1]][__jj[1]] * m[__ii[2]][__jj[2]] - m[__ii[1]][__jj[2]] * m[__ii[2]][__jj[1]]);  \
+		(fac) -= m[__ii[0]][__jj[1]] * (m[__ii[1]][__jj[0]] * m[__ii[2]][__jj[2]] - m[__ii[1]][__jj[2]] * m[__ii[2]][__jj[0]]); \
+		(fac) += m[__ii[0]][__jj[2]] * (m[__ii[1]][__jj[0]] * m[__ii[2]][__jj[1]] - m[__ii[1]][__jj[1]] * m[__ii[2]][__jj[0]]); \
+                                                                                                                                \
+		__k = i + j;                                                                                                            \
+		if (__k != (__k / 2) * 2)                                                                                               \
+		{                                                                                                                       \
+			(fac) = -(fac);                                                                                                     \
+		}                                                                                                                       \
+	}
 
 /** determinant of matrix
  *
  * Computes determinant of matrix m, returning d
  */
-#define DETERMINANT_4X4(d,m)					\
-{								\
-   GREAL cofac;						\
-   COFACTOR_4X4_IJ (cofac, m, 0, 0);				\
-   d = m[0][0] * cofac;						\
-   COFACTOR_4X4_IJ (cofac, m, 0, 1);				\
-   d += m[0][1] * cofac;					\
-   COFACTOR_4X4_IJ (cofac, m, 0, 2);				\
-   d += m[0][2] * cofac;					\
-   COFACTOR_4X4_IJ (cofac, m, 0, 3);				\
-   d += m[0][3] * cofac;					\
-}\
-
+#define DETERMINANT_4X4(d, m)            \
+	{                                    \
+		GREAL cofac;                     \
+		COFACTOR_4X4_IJ(cofac, m, 0, 0); \
+		d = m[0][0] * cofac;             \
+		COFACTOR_4X4_IJ(cofac, m, 0, 1); \
+		d += m[0][1] * cofac;            \
+		COFACTOR_4X4_IJ(cofac, m, 0, 2); \
+		d += m[0][2] * cofac;            \
+		COFACTOR_4X4_IJ(cofac, m, 0, 3); \
+		d += m[0][3] * cofac;            \
+	}
 
 /** cofactor of matrix
  *
  * Computes cofactor of matrix m, returning a
  */
-#define COFACTOR_2X2(a,m)					\
-{								\
-   a[0][0] = (m)[1][1];						\
-   a[0][1] = - (m)[1][0];						\
-   a[1][0] = - (m)[0][1];						\
-   a[1][1] = (m)[0][0];						\
-}\
-
+#define COFACTOR_2X2(a, m)    \
+	{                         \
+		a[0][0] = (m)[1][1];  \
+		a[0][1] = -(m)[1][0]; \
+		a[1][0] = -(m)[0][1]; \
+		a[1][1] = (m)[0][0];  \
+	}
 
 /** cofactor of matrix
  *
  * Computes cofactor of matrix m, returning a
  */
-#define COFACTOR_3X3(a,m)					\
-{								\
-   a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];			\
-   a[0][1] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);		\
-   a[0][2] = m[1][0]*m[2][1] - m[1][1]*m[2][0];			\
-   a[1][0] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);		\
-   a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];			\
-   a[1][2] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);		\
-   a[2][0] = m[0][1]*m[1][2] - m[0][2]*m[1][1];			\
-   a[2][1] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);		\
-   a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);		\
-}\
-
+#define COFACTOR_3X3(a, m)                                  \
+	{                                                       \
+		a[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];    \
+		a[0][1] = -(m[1][0] * m[2][2] - m[2][0] * m[1][2]); \
+		a[0][2] = m[1][0] * m[2][1] - m[1][1] * m[2][0];    \
+		a[1][0] = -(m[0][1] * m[2][2] - m[0][2] * m[2][1]); \
+		a[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0];    \
+		a[1][2] = -(m[0][0] * m[2][1] - m[0][1] * m[2][0]); \
+		a[2][0] = m[0][1] * m[1][2] - m[0][2] * m[1][1];    \
+		a[2][1] = -(m[0][0] * m[1][2] - m[0][2] * m[1][0]); \
+   a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);            \
+	}
 
 /** cofactor of matrix
  *
  * Computes cofactor of matrix m, returning a
  */
-#define COFACTOR_4X4(a,m)					\
-{								\
-   int i,j;							\
-								\
-   for (i=0; i<4; i++) {					\
-      for (j=0; j<4; j++) {					\
-         COFACTOR_4X4_IJ (a[i][j], m, i, j);			\
-      }								\
-   }								\
-}\
-
+#define COFACTOR_4X4(a, m)                         \
+	{                                              \
+		int i, j;                                  \
+                                                   \
+		for (i = 0; i < 4; i++)                    \
+		{                                          \
+			for (j = 0; j < 4; j++)                \
+			{                                      \
+				COFACTOR_4X4_IJ(a[i][j], m, i, j); \
+			}                                      \
+		}                                          \
+	}
 
 /** adjoint of matrix
  *
  * Computes adjoint of matrix m, returning a
  * (Note that adjoint is just the transpose of the cofactor matrix)
  */
-#define ADJOINT_2X2(a,m)					\
-{								\
-   a[0][0] = (m)[1][1];						\
-   a[1][0] = - (m)[1][0];						\
-   a[0][1] = - (m)[0][1];						\
-   a[1][1] = (m)[0][0];						\
-}\
-
+#define ADJOINT_2X2(a, m)     \
+	{                         \
+		a[0][0] = (m)[1][1];  \
+		a[1][0] = -(m)[1][0]; \
+		a[0][1] = -(m)[0][1]; \
+		a[1][1] = (m)[0][0];  \
+	}
 
 /** adjoint of matrix
  *
  * Computes adjoint of matrix m, returning a
  * (Note that adjoint is just the transpose of the cofactor matrix)
  */
-#define ADJOINT_3X3(a,m)					\
-{								\
-   a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];			\
-   a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);		\
-   a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];			\
-   a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);		\
-   a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];			\
-   a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);		\
-   a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];			\
-   a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);		\
-   a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);		\
-}\
-
+#define ADJOINT_3X3(a, m)                                   \
+	{                                                       \
+		a[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];    \
+		a[1][0] = -(m[1][0] * m[2][2] - m[2][0] * m[1][2]); \
+		a[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0];    \
+		a[0][1] = -(m[0][1] * m[2][2] - m[0][2] * m[2][1]); \
+		a[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0];    \
+		a[2][1] = -(m[0][0] * m[2][1] - m[0][1] * m[2][0]); \
+		a[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1];    \
+		a[1][2] = -(m[0][0] * m[1][2] - m[0][2] * m[1][0]); \
+   a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);            \
+	}
 
 /** adjoint of matrix
  *
  * Computes adjoint of matrix m, returning a
  * (Note that adjoint is just the transpose of the cofactor matrix)
  */
-#define ADJOINT_4X4(a,m)					\
-{								\
-   char _i_,_j_;							\
-								\
-   for (_i_=0; _i_<4; _i_++) {					\
-      for (_j_=0; _j_<4; _j_++) {					\
-         COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_);			\
-      }								\
-   }								\
-}\
-
+#define ADJOINT_4X4(a, m)                                  \
+	{                                                      \
+		char _i_, _j_;                                     \
+                                                           \
+		for (_i_ = 0; _i_ < 4; _i_++)                      \
+		{                                                  \
+			for (_j_ = 0; _j_ < 4; _j_++)                  \
+			{                                              \
+				COFACTOR_4X4_IJ(a[_j_][_i_], m, _i_, _j_); \
+			}                                              \
+		}                                                  \
+	}
 
 /** compute adjoint of matrix and scale
  *
  * Computes adjoint of matrix m, scales it by s, returning a
  */
-#define SCALE_ADJOINT_2X2(a,s,m)				\
-{								\
-   a[0][0] = (s) * m[1][1];					\
-   a[1][0] = - (s) * m[1][0];					\
-   a[0][1] = - (s) * m[0][1];					\
-   a[1][1] = (s) * m[0][0];					\
-}\
-
+#define SCALE_ADJOINT_2X2(a, s, m) \
+	{                              \
+		a[0][0] = (s)*m[1][1];     \
+		a[1][0] = -(s)*m[1][0];    \
+		a[0][1] = -(s)*m[0][1];    \
+		a[1][1] = (s)*m[0][0];     \
+	}
 
 /** compute adjoint of matrix and scale
  *
  * Computes adjoint of matrix m, scales it by s, returning a
  */
-#define SCALE_ADJOINT_3X3(a,s,m)				\
-{								\
-   a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]);	\
-   a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]);	\
-   a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);	\
-								\
-   a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]);	\
-   a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]);	\
-   a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]);	\
-								\
-   a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]);	\
-   a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]);	\
-   a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]);	\
-}\
-
+#define SCALE_ADJOINT_3X3(a, s, m)                               \
+	{                                                            \
+		a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]); \
+		a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]); \
+		a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); \
+                                                                 \
+		a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]); \
+		a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]); \
+		a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]); \
+                                                                 \
+		a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]); \
+		a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]); \
+		a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]); \
+	}
 
 /** compute adjoint of matrix and scale
  *
  * Computes adjoint of matrix m, scales it by s, returning a
  */
-#define SCALE_ADJOINT_4X4(a,s,m)				\
-{								\
-   char _i_,_j_; \
-   for (_i_=0; _i_<4; _i_++) {					\
-      for (_j_=0; _j_<4; _j_++) {					\
-         COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_);			\
-         a[_j_][_i_] *= s;						\
-      }								\
-   }								\
-}\
+#define SCALE_ADJOINT_4X4(a, s, m)                         \
+	{                                                      \
+		char _i_, _j_;                                     \
+		for (_i_ = 0; _i_ < 4; _i_++)                      \
+		{                                                  \
+			for (_j_ = 0; _j_ < 4; _j_++)                  \
+			{                                              \
+				COFACTOR_4X4_IJ(a[_j_][_i_], m, _i_, _j_); \
+				a[_j_][_i_] *= s;                          \
+			}                                              \
+		}                                                  \
+	}
 
 /** inverse of matrix
  *
  * Compute inverse of matrix a, returning determinant m and
  * inverse b
  */
-#define INVERT_2X2(b,det,a)			\
-{						\
-   GREAL _tmp_;					\
-   DETERMINANT_2X2 (det, a);			\
-   _tmp_ = 1.0 / (det);				\
-   SCALE_ADJOINT_2X2 (b, _tmp_, a);		\
-}\
-
+#define INVERT_2X2(b, det, a)           \
+	{                                   \
+		GREAL _tmp_;                    \
+		DETERMINANT_2X2(det, a);        \
+		_tmp_ = 1.0 / (det);            \
+		SCALE_ADJOINT_2X2(b, _tmp_, a); \
+	}
 
 /** inverse of matrix
  *
  * Compute inverse of matrix a, returning determinant m and
  * inverse b
  */
-#define INVERT_3X3(b,det,a)			\
-{						\
-   GREAL _tmp_;					\
-   DETERMINANT_3X3 (det, a);			\
-   _tmp_ = 1.0 / (det);				\
-   SCALE_ADJOINT_3X3 (b, _tmp_, a);		\
-}\
-
+#define INVERT_3X3(b, det, a)           \
+	{                                   \
+		GREAL _tmp_;                    \
+		DETERMINANT_3X3(det, a);        \
+		_tmp_ = 1.0 / (det);            \
+		SCALE_ADJOINT_3X3(b, _tmp_, a); \
+	}
 
 /** inverse of matrix
  *
  * Compute inverse of matrix a, returning determinant m and
  * inverse b
  */
-#define INVERT_4X4(b,det,a)			\
-{						\
-   GREAL _tmp_;					\
-   DETERMINANT_4X4 (det, a);			\
-   _tmp_ = 1.0 / (det);				\
-   SCALE_ADJOINT_4X4 (b, _tmp_, a);		\
-}\
+#define INVERT_4X4(b, det, a)           \
+	{                                   \
+		GREAL _tmp_;                    \
+		DETERMINANT_4X4(det, a);        \
+		_tmp_ = 1.0 / (det);            \
+		SCALE_ADJOINT_4X4(b, _tmp_, a); \
+	}
 
 //! Get the triple(3) row of a transform matrix
-#define MAT_GET_ROW(mat,vec3,rowindex)\
-{\
-    vec3[0] = mat[rowindex][0];\
-    vec3[1] = mat[rowindex][1];\
-    vec3[2] = mat[rowindex][2]; \
-}\
+#define MAT_GET_ROW(mat, vec3, rowindex) \
+	{                                    \
+		vec3[0] = mat[rowindex][0];      \
+		vec3[1] = mat[rowindex][1];      \
+		vec3[2] = mat[rowindex][2];      \
+	}
 
 //! Get the tuple(2) row of a transform matrix
-#define MAT_GET_ROW2(mat,vec2,rowindex)\
-{\
-    vec2[0] = mat[rowindex][0];\
-    vec2[1] = mat[rowindex][1];\
-}\
-
+#define MAT_GET_ROW2(mat, vec2, rowindex) \
+	{                                     \
+		vec2[0] = mat[rowindex][0];       \
+		vec2[1] = mat[rowindex][1];       \
+	}
 
 //! Get the quad (4) row of a transform matrix
-#define MAT_GET_ROW4(mat,vec4,rowindex)\
-{\
-    vec4[0] = mat[rowindex][0];\
-    vec4[1] = mat[rowindex][1];\
-    vec4[2] = mat[rowindex][2];\
-    vec4[3] = mat[rowindex][3];\
-}\
+#define MAT_GET_ROW4(mat, vec4, rowindex) \
+	{                                     \
+		vec4[0] = mat[rowindex][0];       \
+		vec4[1] = mat[rowindex][1];       \
+		vec4[2] = mat[rowindex][2];       \
+		vec4[3] = mat[rowindex][3];       \
+	}
 
 //! Get the triple(3) col of a transform matrix
-#define MAT_GET_COL(mat,vec3,colindex)\
-{\
-    vec3[0] = mat[0][colindex];\
-    vec3[1] = mat[1][colindex];\
-    vec3[2] = mat[2][colindex]; \
-}\
+#define MAT_GET_COL(mat, vec3, colindex) \
+	{                                    \
+		vec3[0] = mat[0][colindex];      \
+		vec3[1] = mat[1][colindex];      \
+		vec3[2] = mat[2][colindex];      \
+	}
 
 //! Get the tuple(2) col of a transform matrix
-#define MAT_GET_COL2(mat,vec2,colindex)\
-{\
-    vec2[0] = mat[0][colindex];\
-    vec2[1] = mat[1][colindex];\
-}\
-
+#define MAT_GET_COL2(mat, vec2, colindex) \
+	{                                     \
+		vec2[0] = mat[0][colindex];       \
+		vec2[1] = mat[1][colindex];       \
+	}
 
 //! Get the quad (4) col of a transform matrix
-#define MAT_GET_COL4(mat,vec4,colindex)\
-{\
-    vec4[0] = mat[0][colindex];\
-    vec4[1] = mat[1][colindex];\
-    vec4[2] = mat[2][colindex];\
-    vec4[3] = mat[3][colindex];\
-}\
+#define MAT_GET_COL4(mat, vec4, colindex) \
+	{                                     \
+		vec4[0] = mat[0][colindex];       \
+		vec4[1] = mat[1][colindex];       \
+		vec4[2] = mat[2][colindex];       \
+		vec4[3] = mat[3][colindex];       \
+	}
 
 //! Get the triple(3) col of a transform matrix
-#define MAT_GET_X(mat,vec3)\
-{\
-    MAT_GET_COL(mat,vec3,0);\
-}\
+#define MAT_GET_X(mat, vec3)       \
+	{                              \
+		MAT_GET_COL(mat, vec3, 0); \
+	}
 
 //! Get the triple(3) col of a transform matrix
-#define MAT_GET_Y(mat,vec3)\
-{\
-    MAT_GET_COL(mat,vec3,1);\
-}\
+#define MAT_GET_Y(mat, vec3)       \
+	{                              \
+		MAT_GET_COL(mat, vec3, 1); \
+	}
 
 //! Get the triple(3) col of a transform matrix
-#define MAT_GET_Z(mat,vec3)\
-{\
-    MAT_GET_COL(mat,vec3,2);\
-}\
-
+#define MAT_GET_Z(mat, vec3)       \
+	{                              \
+		MAT_GET_COL(mat, vec3, 2); \
+	}
 
 //! Get the triple(3) col of a transform matrix
-#define MAT_SET_X(mat,vec3)\
-{\
-    mat[0][0] = vec3[0];\
-    mat[1][0] = vec3[1];\
-    mat[2][0] = vec3[2];\
-}\
+#define MAT_SET_X(mat, vec3) \
+	{                        \
+		mat[0][0] = vec3[0]; \
+		mat[1][0] = vec3[1]; \
+		mat[2][0] = vec3[2]; \
+	}
 
 //! Get the triple(3) col of a transform matrix
-#define MAT_SET_Y(mat,vec3)\
-{\
-    mat[0][1] = vec3[0];\
-    mat[1][1] = vec3[1];\
-    mat[2][1] = vec3[2];\
-}\
+#define MAT_SET_Y(mat, vec3) \
+	{                        \
+		mat[0][1] = vec3[0]; \
+		mat[1][1] = vec3[1]; \
+		mat[2][1] = vec3[2]; \
+	}
 
 //! Get the triple(3) col of a transform matrix
-#define MAT_SET_Z(mat,vec3)\
-{\
-    mat[0][2] = vec3[0];\
-    mat[1][2] = vec3[1];\
-    mat[2][2] = vec3[2];\
-}\
-
+#define MAT_SET_Z(mat, vec3) \
+	{                        \
+		mat[0][2] = vec3[0]; \
+		mat[1][2] = vec3[1]; \
+		mat[2][2] = vec3[2]; \
+	}
 
 //! Get the triple(3) col of a transform matrix
-#define MAT_GET_TRANSLATION(mat,vec3)\
-{\
-    vec3[0] = mat[0][3];\
-    vec3[1] = mat[1][3];\
-    vec3[2] = mat[2][3]; \
-}\
+#define MAT_GET_TRANSLATION(mat, vec3) \
+	{                                  \
+		vec3[0] = mat[0][3];           \
+		vec3[1] = mat[1][3];           \
+		vec3[2] = mat[2][3];           \
+	}
 
 //! Set the triple(3) col of a transform matrix
-#define MAT_SET_TRANSLATION(mat,vec3)\
-{\
-    mat[0][3] = vec3[0];\
-    mat[1][3] = vec3[1];\
-    mat[2][3] = vec3[2]; \
-}\
-
-
+#define MAT_SET_TRANSLATION(mat, vec3) \
+	{                                  \
+		mat[0][3] = vec3[0];           \
+		mat[1][3] = vec3[1];           \
+		mat[2][3] = vec3[2];           \
+	}
 
 //! Returns the dot product between a vec3f and the row of a matrix
-#define MAT_DOT_ROW(mat,vec3,rowindex) (vec3[0]*mat[rowindex][0] + vec3[1]*mat[rowindex][1] + vec3[2]*mat[rowindex][2])
+#define MAT_DOT_ROW(mat, vec3, rowindex) (vec3[0] * mat[rowindex][0] + vec3[1] * mat[rowindex][1] + vec3[2] * mat[rowindex][2])
 
 //! Returns the dot product between a vec2f and the row of a matrix
-#define MAT_DOT_ROW2(mat,vec2,rowindex) (vec2[0]*mat[rowindex][0] + vec2[1]*mat[rowindex][1])
+#define MAT_DOT_ROW2(mat, vec2, rowindex) (vec2[0] * mat[rowindex][0] + vec2[1] * mat[rowindex][1])
 
 //! Returns the dot product between a vec4f and the row of a matrix
-#define MAT_DOT_ROW4(mat,vec4,rowindex) (vec4[0]*mat[rowindex][0] + vec4[1]*mat[rowindex][1] + vec4[2]*mat[rowindex][2] + vec4[3]*mat[rowindex][3])
-
+#define MAT_DOT_ROW4(mat, vec4, rowindex) (vec4[0] * mat[rowindex][0] + vec4[1] * mat[rowindex][1] + vec4[2] * mat[rowindex][2] + vec4[3] * mat[rowindex][3])
 
 //! Returns the dot product between a vec3f and the col of a matrix
-#define MAT_DOT_COL(mat,vec3,colindex) (vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex])
+#define MAT_DOT_COL(mat, vec3, colindex) (vec3[0] * mat[0][colindex] + vec3[1] * mat[1][colindex] + vec3[2] * mat[2][colindex])
 
 //! Returns the dot product between a vec2f and the col of a matrix
-#define MAT_DOT_COL2(mat,vec2,colindex) (vec2[0]*mat[0][colindex] + vec2[1]*mat[1][colindex])
+#define MAT_DOT_COL2(mat, vec2, colindex) (vec2[0] * mat[0][colindex] + vec2[1] * mat[1][colindex])
 
 //! Returns the dot product between a vec4f and the col of a matrix
-#define MAT_DOT_COL4(mat,vec4,colindex) (vec4[0]*mat[0][colindex] + vec4[1]*mat[1][colindex] + vec4[2]*mat[2][colindex] + vec4[3]*mat[3][colindex])
+#define MAT_DOT_COL4(mat, vec4, colindex) (vec4[0] * mat[0][colindex] + vec4[1] * mat[1][colindex] + vec4[2] * mat[2][colindex] + vec4[3] * mat[3][colindex])
 
 /*!Transpose matrix times vector
 v is a vec3f
 and m is a mat4f<br>
 */
-#define INV_MAT_DOT_VEC_3X3(p,m,v)					\
-{								\
-   p[0] = MAT_DOT_COL(m,v,0); \
-   p[1] = MAT_DOT_COL(m,v,1);	\
-   p[2] = MAT_DOT_COL(m,v,2);	\
-}\
-
-
-
-#endif // GIM_VECTOR_H_INCLUDED
+#define INV_MAT_DOT_VEC_3X3(p, m, v) \
+	{                                \
+		p[0] = MAT_DOT_COL(m, v, 0); \
+		p[1] = MAT_DOT_COL(m, v, 1); \
+		p[2] = MAT_DOT_COL(m, v, 2); \
+	}
+
+#endif  // GIM_VECTOR_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h
index 939079e1040..3c4f821a722 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h
@@ -34,8 +34,6 @@ email: projectileman@yahoo.com
 
 #include "LinearMath/btScalar.h"
 
-
-
 #define GREAL btScalar
 #define GREAL2 double
 #define GINT int
@@ -45,8 +43,6 @@ email: projectileman@yahoo.com
 #define GINT64 long long
 #define GUINT64 unsigned long long
 
-
-
 #define G_PI 3.14159265358979f
 #define G_HALF_PI 1.5707963f
 //267948966
@@ -54,16 +50,14 @@ email: projectileman@yahoo.com
 //71795864
 #define G_ROOT3 1.73205f
 #define G_ROOT2 1.41421f
-#define G_UINT_INFINITY 0xffffffff //!< A very very high value
+#define G_UINT_INFINITY 0xffffffff  //!< A very very high value
 #define G_REAL_INFINITY FLT_MAX
-#define	G_SIGN_BITMASK			0x80000000
+#define G_SIGN_BITMASK 0x80000000
 #define G_EPSILON SIMD_EPSILON
 
-
-
 enum GIM_SCALAR_TYPES
 {
-	G_STYPE_REAL =0,
+	G_STYPE_REAL = 0,
 	G_STYPE_REAL2,
 	G_STYPE_SHORT,
 	G_STYPE_USHORT,
@@ -73,85 +67,82 @@ enum GIM_SCALAR_TYPES
 	G_STYPE_UINT64
 };
 
-
-
-#define G_DEGTORAD(X) ((X)*3.1415926f/180.0f)
-#define G_RADTODEG(X) ((X)*180.0f/3.1415926f)
+#define G_DEGTORAD(X) ((X)*3.1415926f / 180.0f)
+#define G_RADTODEG(X) ((X)*180.0f / 3.1415926f)
 
 //! Integer representation of a floating-point value.
-#define GIM_IR(x)					((GUINT&)(x))
+#define GIM_IR(x) ((GUINT&)(x))
 
 //! Signed integer representation of a floating-point value.
-#define GIM_SIR(x)					((GINT&)(x))
+#define GIM_SIR(x) ((GINT&)(x))
 
 //! Absolute integer representation of a floating-point value
-#define GIM_AIR(x)					(GIM_IR(x)&0x7fffffff)
+#define GIM_AIR(x) (GIM_IR(x) & 0x7fffffff)
 
 //! Floating-point representation of an integer value.
-#define GIM_FR(x)					((GREAL&)(x))
+#define GIM_FR(x) ((GREAL&)(x))
 
-#define GIM_MAX(a,b) (a<b?b:a)
-#define GIM_MIN(a,b) (a>b?b:a)
+#define GIM_MAX(a, b) (a < b ? b : a)
+#define GIM_MIN(a, b) (a > b ? b : a)
 
-#define GIM_MAX3(a,b,c) GIM_MAX(a,GIM_MAX(b,c))
-#define GIM_MIN3(a,b,c) GIM_MIN(a,GIM_MIN(b,c))
+#define GIM_MAX3(a, b, c) GIM_MAX(a, GIM_MAX(b, c))
+#define GIM_MIN3(a, b, c) GIM_MIN(a, GIM_MIN(b, c))
 
-#define GIM_IS_ZERO(value) (value < G_EPSILON &&  value > -G_EPSILON)
+#define GIM_IS_ZERO(value) (value < G_EPSILON && value > -G_EPSILON)
 
 #define GIM_IS_NEGATIVE(value) (value <= -G_EPSILON)
 
 #define GIM_IS_POSISITVE(value) (value >= G_EPSILON)
 
-#define GIM_NEAR_EQUAL(v1,v2) GIM_IS_ZERO((v1-v2))
+#define GIM_NEAR_EQUAL(v1, v2) GIM_IS_ZERO((v1 - v2))
 
 ///returns a clamped number
-#define GIM_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
+#define GIM_CLAMP(number, minval, maxval) (number < minval ? minval : (number > maxval ? maxval : number))
 
-#define GIM_GREATER(x, y)	btFabs(x) > (y)
+#define GIM_GREATER(x, y) btFabs(x) > (y)
 
 ///Swap numbers
-#define GIM_SWAP_NUMBERS(a,b){ \
-    a = a+b; \
-    b = a-b; \
-    a = a-b; \
-}\
-
-#define GIM_INV_SQRT(va,isva)\
-{\
-    if(va<=0.0000001f)\
-    {\
-        isva = G_REAL_INFINITY;\
-    }\
-    else\
-    {\
-        GREAL _x = va * 0.5f;\
-        GUINT _y = 0x5f3759df - ( GIM_IR(va) >> 1);\
-        isva = GIM_FR(_y);\
-        isva  = isva * ( 1.5f - ( _x * isva * isva ) );\
-    }\
-}\
-
-#define GIM_SQRT(va,sva)\
-{\
-    GIM_INV_SQRT(va,sva);\
-    sva = 1.0f/sva;\
-}\
+#define GIM_SWAP_NUMBERS(a, b) \
+	{                          \
+		a = a + b;             \
+		b = a - b;             \
+		a = a - b;             \
+	}
+
+#define GIM_INV_SQRT(va, isva)                         \
+	{                                                  \
+		if (va <= 0.0000001f)                          \
+		{                                              \
+			isva = G_REAL_INFINITY;                    \
+		}                                              \
+		else                                           \
+		{                                              \
+			GREAL _x = va * 0.5f;                      \
+			GUINT _y = 0x5f3759df - (GIM_IR(va) >> 1); \
+			isva = GIM_FR(_y);                         \
+			isva = isva * (1.5f - (_x * isva * isva)); \
+		}                                              \
+	}
+
+#define GIM_SQRT(va, sva)      \
+	{                          \
+		GIM_INV_SQRT(va, sva); \
+		sva = 1.0f / sva;      \
+	}
 
 //! Computes 1.0f / sqrtf(x). Comes from Quake3. See http://www.magic-software.com/3DGEDInvSqrt.html
 inline GREAL gim_inv_sqrt(GREAL f)
 {
-    GREAL r;
-    GIM_INV_SQRT(f,r);
-    return r;
+	GREAL r;
+	GIM_INV_SQRT(f, r);
+	return r;
 }
 
 inline GREAL gim_sqrt(GREAL f)
 {
-    GREAL r;
-    GIM_SQRT(f,r);
-    return r;
+	GREAL r;
+	GIM_SQRT(f, r);
+	return r;
 }
 
-
-
-#endif // GIM_MATH_H_INCLUDED
+#endif  // GIM_MATH_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.cpp b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.cpp
index 1636eb7867c..9e29ab91d67 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.cpp
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.cpp
@@ -27,7 +27,6 @@ email: projectileman@yahoo.com
 -----------------------------------------------------------------------------
 */
 
-
 #include "gim_memory.h"
 #include "stdlib.h"
 
@@ -40,52 +39,49 @@ static gim_alloca_function *g_allocafn = 0;
 static gim_realloc_function *g_reallocfn = 0;
 static gim_free_function *g_freefn = 0;
 
-void gim_set_alloc_handler (gim_alloc_function *fn)
+void gim_set_alloc_handler(gim_alloc_function *fn)
 {
-  g_allocfn = fn;
+	g_allocfn = fn;
 }
 
-void gim_set_alloca_handler (gim_alloca_function *fn)
+void gim_set_alloca_handler(gim_alloca_function *fn)
 {
-  g_allocafn = fn;
+	g_allocafn = fn;
 }
 
-void gim_set_realloc_handler (gim_realloc_function *fn)
+void gim_set_realloc_handler(gim_realloc_function *fn)
 {
-  g_reallocfn = fn;
+	g_reallocfn = fn;
 }
 
-void gim_set_free_handler (gim_free_function *fn)
+void gim_set_free_handler(gim_free_function *fn)
 {
-  g_freefn = fn;
+	g_freefn = fn;
 }
 
 gim_alloc_function *gim_get_alloc_handler()
 {
-  return g_allocfn;
+	return g_allocfn;
 }
 
 gim_alloca_function *gim_get_alloca_handler()
 {
-  return g_allocafn;
+	return g_allocafn;
 }
 
-
-gim_realloc_function *gim_get_realloc_handler ()
+gim_realloc_function *gim_get_realloc_handler()
 {
-  return g_reallocfn;
+	return g_reallocfn;
 }
 
-
-gim_free_function  *gim_get_free_handler ()
+gim_free_function *gim_get_free_handler()
 {
-  return g_freefn;
+	return g_freefn;
 }
 
-
-void * gim_alloc(size_t size)
+void *gim_alloc(size_t size)
 {
-	void * ptr;
+	void *ptr;
 	if (g_allocfn)
 	{
 		ptr = g_allocfn(size);
@@ -93,27 +89,29 @@ void * gim_alloc(size_t size)
 	else
 	{
 #ifdef GIM_SIMD_MEMORY
-		ptr = btAlignedAlloc(size,16);
+		ptr = btAlignedAlloc(size, 16);
 #else
 		ptr = malloc(size);
 #endif
 	}
-  	return ptr;
+	return ptr;
 }
 
-void * gim_alloca(size_t size)
+void *gim_alloca(size_t size)
 {
-  if (g_allocafn) return g_allocafn(size); else return gim_alloc(size);
+	if (g_allocafn)
+		return g_allocafn(size);
+	else
+		return gim_alloc(size);
 }
 
-
-void * gim_realloc(void *ptr, size_t oldsize, size_t newsize)
+void *gim_realloc(void *ptr, size_t oldsize, size_t newsize)
 {
- 	void * newptr = gim_alloc(newsize);
-    size_t copysize = oldsize<newsize?oldsize:newsize;
-    gim_simd_memcpy(newptr,ptr,copysize);
-    gim_free(ptr);
-    return newptr;
+	void *newptr = gim_alloc(newsize);
+	size_t copysize = oldsize < newsize ? oldsize : newsize;
+	gim_simd_memcpy(newptr, ptr, copysize);
+	gim_free(ptr);
+	return newptr;
 }
 
 void gim_free(void *ptr)
@@ -121,15 +119,14 @@ void gim_free(void *ptr)
 	if (!ptr) return;
 	if (g_freefn)
 	{
-	   g_freefn(ptr);
+		g_freefn(ptr);
 	}
 	else
 	{
-	#ifdef GIM_SIMD_MEMORY
+#ifdef GIM_SIMD_MEMORY
 		btAlignedFree(ptr);
-	#else
+#else
 		free(ptr);
-	#endif
+#endif
 	}
 }
-
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h
index e203888a1e2..fffbfa23d8e 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h
@@ -32,93 +32,84 @@ email: projectileman@yahoo.com
 -----------------------------------------------------------------------------
 */
 
-
 #include "gim_math.h"
 #include <string.h>
 
 #ifdef PREFETCH
-#include <xmmintrin.h>	// for prefetch
-#define pfval	64
-#define pfval2	128
+#include <xmmintrin.h>  // for prefetch
+#define pfval 64
+#define pfval2 128
 //! Prefetch 64
-#define pf(_x,_i)	_mm_prefetch((void *)(_x + _i + pfval), 0)
+#define pf(_x, _i) _mm_prefetch((void *)(_x + _i + pfval), 0)
 //! Prefetch 128
-#define pf2(_x,_i)	_mm_prefetch((void *)(_x + _i + pfval2), 0)
+#define pf2(_x, _i) _mm_prefetch((void *)(_x + _i + pfval2), 0)
 #else
 //! Prefetch 64
-#define pf(_x,_i)
+#define pf(_x, _i)
 //! Prefetch 128
-#define pf2(_x,_i)
+#define pf2(_x, _i)
 #endif
 
-
 ///Functions for manip packed arrays of numbers
-#define GIM_COPY_ARRAYS(dest_array,source_array,element_count)\
-{\
-    for (GUINT _i_=0;_i_<element_count ;++_i_)\
-    {\
-    	dest_array[_i_] = source_array[_i_];\
-    }\
-}\
-
-#define GIM_COPY_ARRAYS_1(dest_array,source_array,element_count,copy_macro)\
-{\
-    for (GUINT _i_=0;_i_<element_count ;++_i_)\
-    {\
-    	copy_macro(dest_array[_i_],source_array[_i_]);\
-    }\
-}\
-
-
-#define GIM_ZERO_ARRAY(array,element_count)\
-{\
-    for (GUINT _i_=0;_i_<element_count ;++_i_)\
-    {\
-    	array[_i_] = 0;\
-    }\
-}\
-
-#define GIM_CONSTANT_ARRAY(array,element_count,constant)\
-{\
-    for (GUINT _i_=0;_i_<element_count ;++_i_)\
-    {\
-    	array[_i_] = constant;\
-    }\
-}\
-
+#define GIM_COPY_ARRAYS(dest_array, source_array, element_count) \
+	{                                                            \
+		for (GUINT _i_ = 0; _i_ < element_count; ++_i_)          \
+		{                                                        \
+			dest_array[_i_] = source_array[_i_];                 \
+		}                                                        \
+	}
+
+#define GIM_COPY_ARRAYS_1(dest_array, source_array, element_count, copy_macro) \
+	{                                                                          \
+		for (GUINT _i_ = 0; _i_ < element_count; ++_i_)                        \
+		{                                                                      \
+			copy_macro(dest_array[_i_], source_array[_i_]);                    \
+		}                                                                      \
+	}
+
+#define GIM_ZERO_ARRAY(array, element_count)            \
+	{                                                   \
+		for (GUINT _i_ = 0; _i_ < element_count; ++_i_) \
+		{                                               \
+			array[_i_] = 0;                             \
+		}                                               \
+	}
+
+#define GIM_CONSTANT_ARRAY(array, element_count, constant) \
+	{                                                      \
+		for (GUINT _i_ = 0; _i_ < element_count; ++_i_)    \
+		{                                                  \
+			array[_i_] = constant;                         \
+		}                                                  \
+	}
 
 ///Function prototypes to allocate and free memory.
-typedef void * gim_alloc_function (size_t size);
-typedef void * gim_alloca_function (size_t size);//Allocs on the heap
-typedef void * gim_realloc_function (void *ptr, size_t oldsize, size_t newsize);
-typedef void gim_free_function (void *ptr);
-
+typedef void *gim_alloc_function(size_t size);
+typedef void *gim_alloca_function(size_t size);  //Allocs on the heap
+typedef void *gim_realloc_function(void *ptr, size_t oldsize, size_t newsize);
+typedef void gim_free_function(void *ptr);
 
 ///Memory Function Handlers
 ///set new memory management functions. if fn is 0, the default handlers are used.
-void gim_set_alloc_handler (gim_alloc_function *fn);
-void gim_set_alloca_handler (gim_alloca_function *fn);
-void gim_set_realloc_handler (gim_realloc_function *fn);
-void gim_set_free_handler (gim_free_function *fn);
-
+void gim_set_alloc_handler(gim_alloc_function *fn);
+void gim_set_alloca_handler(gim_alloca_function *fn);
+void gim_set_realloc_handler(gim_realloc_function *fn);
+void gim_set_free_handler(gim_free_function *fn);
 
 ///get current memory management functions.
-gim_alloc_function *gim_get_alloc_handler (void);
+gim_alloc_function *gim_get_alloc_handler(void);
 gim_alloca_function *gim_get_alloca_handler(void);
-gim_realloc_function *gim_get_realloc_handler (void);
-gim_free_function  *gim_get_free_handler (void);
-
+gim_realloc_function *gim_get_realloc_handler(void);
+gim_free_function *gim_get_free_handler(void);
 
 ///Standar Memory functions
-void * gim_alloc(size_t size);
-void * gim_alloca(size_t size);
-void * gim_realloc(void *ptr, size_t oldsize, size_t newsize);
+void *gim_alloc(size_t size);
+void *gim_alloca(size_t size);
+void *gim_realloc(void *ptr, size_t oldsize, size_t newsize);
 void gim_free(void *ptr);
 
-
-
-#if defined (_WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__)
-    #define GIM_SIMD_MEMORY 1
+#if defined(_WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__)
+#define GIM_SIMD_MEMORY 1
 #endif
 
 //! SIMD POINTER INTEGER
@@ -126,11 +117,10 @@ void gim_free(void *ptr);
 //! SIMD INTEGER SIZE
 #define SIMD_T_SIZE sizeof(SIMD_T)
 
-
-inline void gim_simd_memcpy(void * dst, const void * src, size_t copysize)
+inline void gim_simd_memcpy(void *dst, const void *src, size_t copysize)
 {
 #ifdef GIM_SIMD_MEMORY
-/*
+	/*
 //'long long int' is incompatible with visual studio 6...
     //copy words
     SIMD_T * ui_src_ptr = (SIMD_T *)src;
@@ -143,48 +133,45 @@ inline void gim_simd_memcpy(void * dst, const void * src, size_t copysize)
     if(copysize==0) return;
 */
 
-    char * c_src_ptr = (char *)src;
-    char * c_dst_ptr = (char *)dst;
-    while(copysize>0)
-    {
-        *(c_dst_ptr++) = *(c_src_ptr++);
-        copysize--;
-    }
-    return;
+	char *c_src_ptr = (char *)src;
+	char *c_dst_ptr = (char *)dst;
+	while (copysize > 0)
+	{
+		*(c_dst_ptr++) = *(c_src_ptr++);
+		copysize--;
+	}
+	return;
 #else
-    memcpy(dst,src,copysize);
+	memcpy(dst, src, copysize);
 #endif
 }
 
-
-
-template<class T>
-inline void gim_swap_elements(T* _array,size_t _i,size_t _j)
+template <class T>
+inline void gim_swap_elements(T *_array, size_t _i, size_t _j)
 {
 	T _e_tmp_ = _array[_i];
 	_array[_i] = _array[_j];
 	_array[_j] = _e_tmp_;
 }
 
-
-template<class T>
-inline void gim_swap_elements_memcpy(T* _array,size_t _i,size_t _j)
+template <class T>
+inline void gim_swap_elements_memcpy(T *_array, size_t _i, size_t _j)
 {
 	char _e_tmp_[sizeof(T)];
-	gim_simd_memcpy(_e_tmp_,&_array[_i],sizeof(T));
-	gim_simd_memcpy(&_array[_i],&_array[_j],sizeof(T));
-	gim_simd_memcpy(&_array[_j],_e_tmp_,sizeof(T));
+	gim_simd_memcpy(_e_tmp_, &_array[_i], sizeof(T));
+	gim_simd_memcpy(&_array[_i], &_array[_j], sizeof(T));
+	gim_simd_memcpy(&_array[_j], _e_tmp_, sizeof(T));
 }
 
 template <int SIZE>
-inline void gim_swap_elements_ptr(char * _array,size_t _i,size_t _j)
+inline void gim_swap_elements_ptr(char *_array, size_t _i, size_t _j)
 {
 	char _e_tmp_[SIZE];
-	_i*=SIZE;
-	_j*=SIZE;
-	gim_simd_memcpy(_e_tmp_,_array+_i,SIZE);
-	gim_simd_memcpy(_array+_i,_array+_j,SIZE);
-	gim_simd_memcpy(_array+_j,_e_tmp_,SIZE);
+	_i *= SIZE;
+	_j *= SIZE;
+	gim_simd_memcpy(_e_tmp_, _array + _i, SIZE);
+	gim_simd_memcpy(_array + _i, _array + _j, SIZE);
+	gim_simd_memcpy(_array + _j, _e_tmp_, SIZE);
 }
 
-#endif // GIM_MEMORY_H_INCLUDED
+#endif  // GIM_MEMORY_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_pair.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_pair.h
new file mode 100644
index 00000000000..56c185a5dc9
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_pair.h
@@ -0,0 +1,28 @@
+#ifndef GIM_PAIR_H
+#define GIM_PAIR_H
+
+
+//! Overlapping pair
+struct GIM_PAIR
+{
+        int m_index1;
+        int m_index2;
+        GIM_PAIR()
+        {
+        }
+
+        GIM_PAIR(const GIM_PAIR& p)
+        {
+                m_index1 = p.m_index1;
+                m_index2 = p.m_index2;
+        }
+
+        GIM_PAIR(int index1, int index2)
+        {
+                m_index1 = index1;
+                m_index2 = index2;
+        }
+};
+
+#endif //GIM_PAIR_H
+
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h
index c246ef12543..ff7907adca0 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h
@@ -40,24 +40,22 @@ email: projectileman@yahoo.com
 //! Prototype for comparators
 class less_comparator
 {
-	public:
-
-	template<class T,class Z>
-	inline int operator() ( const T& a, const Z& b )
+public:
+	template <class T, class Z>
+	inline int operator()(const T& a, const Z& b)
 	{
-		return ( a<b?-1:(a>b?1:0));
+		return (a < b ? -1 : (a > b ? 1 : 0));
 	}
 };
 
 //! Prototype for comparators
 class integer_comparator
 {
-	public:
-
-	template<class T>
-	inline int operator() ( const T& a, const T& b )
+public:
+	template <class T>
+	inline int operator()(const T& a, const T& b)
 	{
-		return (int)(a-b);
+		return (int)(a - b);
 	}
 };
 
@@ -65,20 +63,19 @@ class integer_comparator
 class uint_key_func
 {
 public:
-	template<class T>
-	inline GUINT operator()( const T& a)
+	template <class T>
+	inline GUINT operator()(const T& a)
 	{
 		return (GUINT)a;
 	}
 };
 
-
 //!Prototype for copying elements
 class copy_elements_func
 {
 public:
-	template<class T>
-	inline void operator()(T& a,T& b)
+	template <class T>
+	inline void operator()(T& a, T& b)
 	{
 		a = b;
 	}
@@ -88,34 +85,33 @@ public:
 class memcopy_elements_func
 {
 public:
-	template<class T>
-	inline void operator()(T& a,T& b)
+	template <class T>
+	inline void operator()(T& a, T& b)
 	{
-		gim_simd_memcpy(&a,&b,sizeof(T));
+		gim_simd_memcpy(&a, &b, sizeof(T));
 	}
 };
 
-
 //! @{
 struct GIM_RSORT_TOKEN
 {
-    GUINT m_key;
-    GUINT m_value;
-    GIM_RSORT_TOKEN()
-    {
-    }
-    GIM_RSORT_TOKEN(const GIM_RSORT_TOKEN& rtoken)
-    {
-    	m_key = rtoken.m_key;
-    	m_value = rtoken.m_value;
-    }
+	GUINT m_key;
+	GUINT m_value;
+	GIM_RSORT_TOKEN()
+	{
+	}
+	GIM_RSORT_TOKEN(const GIM_RSORT_TOKEN& rtoken)
+	{
+		m_key = rtoken.m_key;
+		m_value = rtoken.m_value;
+	}
 
-    inline bool operator <(const GIM_RSORT_TOKEN& other) const
+	inline bool operator<(const GIM_RSORT_TOKEN& other) const
 	{
 		return (m_key < other.m_key);
 	}
 
-	inline bool operator >(const GIM_RSORT_TOKEN& other) const
+	inline bool operator>(const GIM_RSORT_TOKEN& other) const
 	{
 		return (m_key > other.m_key);
 	}
@@ -124,33 +120,28 @@ struct GIM_RSORT_TOKEN
 //! Prototype for comparators
 class GIM_RSORT_TOKEN_COMPARATOR
 {
-	public:
-
-	inline int operator()( const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b )
+public:
+	inline int operator()(const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b)
 	{
 		return (int)((a.m_key) - (b.m_key));
 	}
 };
 
-
-
 #define kHist 2048
 // ---- utils for accessing 11-bit quantities
-#define D11_0(x)	(x & 0x7FF)
-#define D11_1(x)	(x >> 11 & 0x7FF)
-#define D11_2(x)	(x >> 22 )
-
-
+#define D11_0(x) (x & 0x7FF)
+#define D11_1(x) (x >> 11 & 0x7FF)
+#define D11_2(x) (x >> 22)
 
 ///Radix sort for unsigned integer keys
 inline void gim_radix_sort_rtokens(
-				GIM_RSORT_TOKEN * array,
-				GIM_RSORT_TOKEN * sorted, GUINT element_count)
+	GIM_RSORT_TOKEN* array,
+	GIM_RSORT_TOKEN* sorted, GUINT element_count)
 {
 	GUINT i;
 	GUINT b0[kHist * 3];
-	GUINT *b1 = b0 + kHist;
-	GUINT *b2 = b1 + kHist;
+	GUINT* b1 = b0 + kHist;
+	GUINT* b2 = b1 + kHist;
 	for (i = 0; i < kHist * 3; ++i)
 	{
 		b0[i] = 0;
@@ -159,10 +150,10 @@ inline void gim_radix_sort_rtokens(
 	GUINT pos;
 	for (i = 0; i < element_count; ++i)
 	{
-	    fi = array[i].m_key;
-		b0[D11_0(fi)] ++;
-		b1[D11_1(fi)] ++;
-		b2[D11_2(fi)] ++;
+		fi = array[i].m_key;
+		b0[D11_0(fi)]++;
+		b1[D11_1(fi)]++;
+		b2[D11_2(fi)]++;
 	}
 	{
 		GUINT sum0 = 0, sum1 = 0, sum2 = 0;
@@ -182,7 +173,7 @@ inline void gim_radix_sort_rtokens(
 	}
 	for (i = 0; i < element_count; ++i)
 	{
-        fi = array[i].m_key;
+		fi = array[i].m_key;
 		pos = D11_0(fi);
 		pos = ++b0[pos];
 		sorted[pos].m_key = array[i].m_key;
@@ -190,7 +181,7 @@ inline void gim_radix_sort_rtokens(
 	}
 	for (i = 0; i < element_count; ++i)
 	{
-        fi = sorted[i].m_key;
+		fi = sorted[i].m_key;
 		pos = D11_1(fi);
 		pos = ++b1[pos];
 		array[pos].m_key = sorted[i].m_key;
@@ -198,7 +189,7 @@ inline void gim_radix_sort_rtokens(
 	}
 	for (i = 0; i < element_count; ++i)
 	{
-        fi = array[i].m_key;
+		fi = array[i].m_key;
 		pos = D11_2(fi);
 		pos = ++b2[pos];
 		sorted[pos].m_key = array[i].m_key;
@@ -206,9 +197,6 @@ inline void gim_radix_sort_rtokens(
 	}
 }
 
-
-
-
 /// Get the sorted tokens from an array. For generic use. Tokens are IRR_RSORT_TOKEN
 /*!
 *\param array Array of elements to sort
@@ -216,21 +204,21 @@ inline void gim_radix_sort_rtokens(
 *\param element_count element count
 *\param uintkey_macro Functor which retrieves the integer representation of an array element
 */
-template<typename T, class GETKEY_CLASS>
+template <typename T, class GETKEY_CLASS>
 void gim_radix_sort_array_tokens(
-			T* array ,
-			GIM_RSORT_TOKEN * sorted_tokens,
-			GUINT element_count,GETKEY_CLASS uintkey_macro)
+	T* array,
+	GIM_RSORT_TOKEN* sorted_tokens,
+	GUINT element_count, GETKEY_CLASS uintkey_macro)
 {
-	GIM_RSORT_TOKEN * _unsorted = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count);
-    for (GUINT _i=0;_i<element_count;++_i)
-    {
-        _unsorted[_i].m_key = uintkey_macro(array[_i]);
-        _unsorted[_i].m_value = _i;
-    }
-    gim_radix_sort_rtokens(_unsorted,sorted_tokens,element_count);
-    gim_free(_unsorted);
-    gim_free(_unsorted);
+	GIM_RSORT_TOKEN* _unsorted = (GIM_RSORT_TOKEN*)gim_alloc(sizeof(GIM_RSORT_TOKEN) * element_count);
+	for (GUINT _i = 0; _i < element_count; ++_i)
+	{
+		_unsorted[_i].m_key = uintkey_macro(array[_i]);
+		_unsorted[_i].m_value = _i;
+	}
+	gim_radix_sort_rtokens(_unsorted, sorted_tokens, element_count);
+	gim_free(_unsorted);
+	gim_free(_unsorted);
 }
 
 /// Sorts array in place. For generic use
@@ -241,21 +229,21 @@ void gim_radix_sort_array_tokens(
 \param get_uintkey_macro Macro for extract the Integer value of the element. Similar to SIMPLE_GET_UINTKEY
 \param copy_elements_macro Macro for copy elements, similar to SIMPLE_COPY_ELEMENTS
 */
-template<typename T, class GETKEY_CLASS, class COPY_CLASS>
+template <typename T, class GETKEY_CLASS, class COPY_CLASS>
 void gim_radix_sort(
-	T * array, GUINT element_count,
+	T* array, GUINT element_count,
 	GETKEY_CLASS get_uintkey_macro, COPY_CLASS copy_elements_macro)
 {
-	GIM_RSORT_TOKEN * _sorted = (GIM_RSORT_TOKEN  *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count);
-    gim_radix_sort_array_tokens(array,_sorted,element_count,get_uintkey_macro);
-    T * _original_array = (T *) gim_alloc(sizeof(T)*element_count);
-    gim_simd_memcpy(_original_array,array,sizeof(T)*element_count);
-    for (GUINT _i=0;_i<element_count;++_i)
-    {
-        copy_elements_macro(array[_i],_original_array[_sorted[_i].m_value]);
-    }
-    gim_free(_original_array);
-    gim_free(_sorted);
+	GIM_RSORT_TOKEN* _sorted = (GIM_RSORT_TOKEN*)gim_alloc(sizeof(GIM_RSORT_TOKEN) * element_count);
+	gim_radix_sort_array_tokens(array, _sorted, element_count, get_uintkey_macro);
+	T* _original_array = (T*)gim_alloc(sizeof(T) * element_count);
+	gim_simd_memcpy(_original_array, array, sizeof(T) * element_count);
+	for (GUINT _i = 0; _i < element_count; ++_i)
+	{
+		copy_elements_macro(array[_i], _original_array[_sorted[_i].m_value]);
+	}
+	gim_free(_original_array);
+	gim_free(_sorted);
 }
 
 //! Failsafe Iterative binary search,
@@ -269,20 +257,20 @@ If the element is not found, it returns the nearest upper element position, may
 \param _found If true the value has found. Boolean
 \param _result_index the index of the found element, or if not found then it will get the index of the  closest bigger value
 */
-template<class T, typename KEYCLASS, typename COMP_CLASS>
-bool  gim_binary_search_ex(
-		const T* _array, GUINT _start_i,
-		GUINT _end_i,GUINT & _result_index,
-		const KEYCLASS & _search_key,
-		COMP_CLASS _comp_macro)
+template <class T, typename KEYCLASS, typename COMP_CLASS>
+bool gim_binary_search_ex(
+	const T* _array, GUINT _start_i,
+	GUINT _end_i, GUINT& _result_index,
+	const KEYCLASS& _search_key,
+	COMP_CLASS _comp_macro)
 {
 	GUINT _k;
 	int _comp_result;
 	GUINT _i = _start_i;
-	GUINT _j = _end_i+1;
+	GUINT _j = _end_i + 1;
 	while (_i < _j)
 	{
-		_k = (_j+_i-1)/2;
+		_k = (_j + _i - 1) / 2;
 		_comp_result = _comp_macro(_array[_k], _search_key);
 		if (_comp_result == 0)
 		{
@@ -291,7 +279,7 @@ bool  gim_binary_search_ex(
 		}
 		else if (_comp_result < 0)
 		{
-			_i = _k+1;
+			_i = _k + 1;
 		}
 		else
 		{
@@ -302,8 +290,6 @@ bool  gim_binary_search_ex(
 	return false;
 }
 
-
-
 //! Failsafe Iterative binary search,Template version
 /*!
 If the element is not found, it returns the nearest upper element position, may be the further position after the last element.
@@ -314,26 +300,26 @@ If the element is not found, it returns the nearest upper element position, may
 \param _result_index the index of the found element, or if not found then it will get the index of the  closest bigger value
 \return true if found, else false
 */
-template<class T>
+template <class T>
 bool gim_binary_search(
-	const T*_array,GUINT _start_i,
-	GUINT _end_i,const T & _search_key,
-	GUINT & _result_index)
+	const T* _array, GUINT _start_i,
+	GUINT _end_i, const T& _search_key,
+	GUINT& _result_index)
 {
 	GUINT _i = _start_i;
-	GUINT _j = _end_i+1;
+	GUINT _j = _end_i + 1;
 	GUINT _k;
-	while(_i < _j)
+	while (_i < _j)
 	{
-		_k = (_j+_i-1)/2;
-		if(_array[_k]==_search_key)
+		_k = (_j + _i - 1) / 2;
+		if (_array[_k] == _search_key)
 		{
 			_result_index = _k;
 			return true;
 		}
-		else if (_array[_k]<_search_key)
+		else if (_array[_k] < _search_key)
 		{
-			_i = _k+1;
+			_i = _k + 1;
 		}
 		else
 		{
@@ -344,27 +330,25 @@ bool gim_binary_search(
 	return false;
 }
 
-
-
 ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
 template <typename T, typename COMP_CLASS>
-void gim_down_heap(T *pArr, GUINT k, GUINT n,COMP_CLASS CompareFunc)
+void gim_down_heap(T* pArr, GUINT k, GUINT n, COMP_CLASS CompareFunc)
 {
 	/*  PRE: a[k+1..N] is a heap */
 	/* POST:  a[k..N]  is a heap */
 
 	T temp = pArr[k - 1];
 	/* k has child(s) */
-	while (k <= n/2)
+	while (k <= n / 2)
 	{
-		int child = 2*k;
+		int child = 2 * k;
 
-		if ((child < (int)n) && CompareFunc(pArr[child - 1] , pArr[child])<0)
+		if ((child < (int)n) && CompareFunc(pArr[child - 1], pArr[child]) < 0)
 		{
 			child++;
 		}
 		/* pick larger child */
-		if (CompareFunc(temp , pArr[child - 1])<0)
+		if (CompareFunc(temp, pArr[child - 1]) < 0)
 		{
 			/* move child up */
 			pArr[k - 1] = pArr[child - 1];
@@ -378,29 +362,25 @@ void gim_down_heap(T *pArr, GUINT k, GUINT n,COMP_CLASS CompareFunc)
 	pArr[k - 1] = temp;
 } /*downHeap*/
 
-
 template <typename T, typename COMP_CLASS>
-void gim_heap_sort(T *pArr, GUINT element_count, COMP_CLASS CompareFunc)
+void gim_heap_sort(T* pArr, GUINT element_count, COMP_CLASS CompareFunc)
 {
 	/* sort a[0..N-1],  N.B. 0 to N-1 */
 	GUINT k;
 	GUINT n = element_count;
-	for (k = n/2; k > 0; k--)
+	for (k = n / 2; k > 0; k--)
 	{
 		gim_down_heap(pArr, k, n, CompareFunc);
 	}
 
 	/* a[1..N] is now a heap */
-	while ( n>=2 )
+	while (n >= 2)
 	{
-		gim_swap_elements(pArr,0,n-1); /* largest of a[0..n-1] */
+		gim_swap_elements(pArr, 0, n - 1); /* largest of a[0..n-1] */
 		--n;
 		/* restore a[1..i-1] heap */
 		gim_down_heap(pArr, 1, n, CompareFunc);
 	}
 }
 
-
-
-
-#endif // GIM_RADIXSORT_H_INCLUDED
+#endif  // GIM_RADIXSORT_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp
index f9727e1d53b..8d83e95da40 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp
@@ -33,15 +33,13 @@ email: projectileman@yahoo.com
 
 #include "gim_tri_collision.h"
 
-
 #define TRI_LOCAL_EPSILON 0.000001f
 #define MIN_EDGE_EDGE_DIS 0.00001f
 
-
 class GIM_TRIANGLE_CALCULATION_CACHE
 {
 public:
-	GREAL margin;	
+	GREAL margin;
 	btVector3 tu_vertices[3];
 	btVector3 tv_vertices[3];
 	btVector4 tu_plane;
@@ -55,46 +53,47 @@ public:
 	GREAL du0du2;
 	GREAL dv[4];
 	GREAL dv0dv1;
-	GREAL dv0dv2;	
+	GREAL dv0dv2;
 	btVector3 temp_points[MAX_TRI_CLIPPING];
 	btVector3 temp_points1[MAX_TRI_CLIPPING];
 	btVector3 contact_points[MAX_TRI_CLIPPING];
-	
-
 
 	//! if returns false, the faces are paralele
 	SIMD_FORCE_INLINE bool compute_intervals(
-					const GREAL &D0,
-					const GREAL &D1,
-					const GREAL &D2,
-					const GREAL &D0D1,
-					const GREAL &D0D2,
-					GREAL & scale_edge0,
-					GREAL & scale_edge1,
-					GUINT &edge_index0,
-					GUINT &edge_index1)
+		const GREAL &D0,
+		const GREAL &D1,
+		const GREAL &D2,
+		const GREAL &D0D1,
+		const GREAL &D0D2,
+		GREAL &scale_edge0,
+		GREAL &scale_edge1,
+		GUINT &edge_index0,
+		GUINT &edge_index1)
 	{
-		if(D0D1>0.0f)
+		if (D0D1 > 0.0f)
 		{
 			/* here we know that D0D2<=0.0 */
 			/* that is D0, D1 are on the same side, D2 on the other or on the plane */
-			scale_edge0 = -D2/(D0-D2);
-			scale_edge1 = -D1/(D2-D1);
-			edge_index0 = 2;edge_index1 = 1;
+			scale_edge0 = -D2 / (D0 - D2);
+			scale_edge1 = -D1 / (D2 - D1);
+			edge_index0 = 2;
+			edge_index1 = 1;
 		}
-		else if(D0D2>0.0f)
+		else if (D0D2 > 0.0f)
 		{
 			/* here we know that d0d1<=0.0 */
-			scale_edge0 = -D0/(D1-D0);
-			scale_edge1 = -D1/(D2-D1);
-			edge_index0 = 0;edge_index1 = 1;
+			scale_edge0 = -D0 / (D1 - D0);
+			scale_edge1 = -D1 / (D2 - D1);
+			edge_index0 = 0;
+			edge_index1 = 1;
 		}
-		else if(D1*D2>0.0f || D0!=0.0f)
+		else if (D1 * D2 > 0.0f || D0 != 0.0f)
 		{
 			/* here we know that d0d1<=0.0 or that D0!=0.0 */
-			scale_edge0 = -D0/(D1-D0);
-			scale_edge1 = -D2/(D0-D2);
-			edge_index0 = 0 ;edge_index1 = 2;
+			scale_edge0 = -D0 / (D1 - D0);
+			scale_edge1 = -D2 / (D0 - D2);
+			edge_index0 = 0;
+			edge_index1 = 2;
 		}
 		else
 		{
@@ -103,46 +102,44 @@ public:
 		return true;
 	}
 
-
 	//! clip triangle
 	/*!
 	*/
 	SIMD_FORCE_INLINE GUINT clip_triangle(
-		const btVector4 & tri_plane,
-		const btVector3 * tripoints,
-		const btVector3 * srcpoints,
-		btVector3 * clip_points)
+		const btVector4 &tri_plane,
+		const btVector3 *tripoints,
+		const btVector3 *srcpoints,
+		btVector3 *clip_points)
 	{
 		// edge 0
 
 		btVector4 edgeplane;
 
-		EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
+		EDGE_PLANE(tripoints[0], tripoints[1], tri_plane, edgeplane);
 
 		GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
-			edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
+			edgeplane, srcpoints[0], srcpoints[1], srcpoints[2], temp_points);
 
-		if(clipped_count == 0) return 0;
+		if (clipped_count == 0) return 0;
 
 		// edge 1
 
-		EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
+		EDGE_PLANE(tripoints[1], tripoints[2], tri_plane, edgeplane);
 
 		clipped_count = PLANE_CLIP_POLYGON3D(
-			edgeplane,temp_points,clipped_count,temp_points1);
+			edgeplane, temp_points, clipped_count, temp_points1);
 
-		if(clipped_count == 0) return 0;
+		if (clipped_count == 0) return 0;
 
 		// edge 2
 
-		EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
+		EDGE_PLANE(tripoints[2], tripoints[0], tri_plane, edgeplane);
 
 		clipped_count = PLANE_CLIP_POLYGON3D(
-			edgeplane,temp_points1,clipped_count,clip_points);
+			edgeplane, temp_points1, clipped_count, clip_points);
 
 		return clipped_count;
 
-
 		/*GUINT i0 = (tri_plane.closestAxis()+1)%3;
 		GUINT i1 = (i0+1)%3;
 		// edge 0
@@ -172,13 +169,13 @@ public:
 	}
 
 	SIMD_FORCE_INLINE void sort_isect(
-		GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1)
+		GREAL &isect0, GREAL &isect1, GUINT &e0, GUINT &e1, btVector3 &vec0, btVector3 &vec1)
 	{
-		if(isect1<isect0)
+		if (isect1 < isect0)
 		{
 			//swap
-			GIM_SWAP_NUMBERS(isect0,isect1);
-			GIM_SWAP_NUMBERS(e0,e1);
+			GIM_SWAP_NUMBERS(isect0, isect1);
+			GIM_SWAP_NUMBERS(e0, e1);
 			btVector3 tmp = vec0;
 			vec0 = vec1;
 			vec1 = tmp;
@@ -202,53 +199,52 @@ public:
 		// Compute direction of intersection line
 		edge_edge_dir = tu_plane.cross(tv_plane);
 		GREAL Dlen;
-		VEC_LENGTH(edge_edge_dir,Dlen);
+		VEC_LENGTH(edge_edge_dir, Dlen);
 
-		if(Dlen<0.0001)
+		if (Dlen < 0.0001)
 		{
-			return 0; //faces near paralele
+			return 0;  //faces near paralele
 		}
 
-		edge_edge_dir*= 1/Dlen;//normalize
-
+		edge_edge_dir *= 1 / Dlen;  //normalize
 
 		// Compute interval for triangle 1
-		GUINT tu_e0,tu_e1;//edge indices
-		GREAL tu_scale_e0,tu_scale_e1;//edge scale
-		if(!compute_intervals(du[0],du[1],du[2],
-			du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0;
+		GUINT tu_e0, tu_e1;              //edge indices
+		GREAL tu_scale_e0, tu_scale_e1;  //edge scale
+		if (!compute_intervals(du[0], du[1], du[2],
+							   du0du1, du0du2, tu_scale_e0, tu_scale_e1, tu_e0, tu_e1)) return 0;
 
 		// Compute interval for triangle 2
-		GUINT tv_e0,tv_e1;//edge indices
-		GREAL tv_scale_e0,tv_scale_e1;//edge scale
+		GUINT tv_e0, tv_e1;              //edge indices
+		GREAL tv_scale_e0, tv_scale_e1;  //edge scale
 
-		if(!compute_intervals(dv[0],dv[1],dv[2],
-			dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0;
+		if (!compute_intervals(dv[0], dv[1], dv[2],
+							   dv0dv1, dv0dv2, tv_scale_e0, tv_scale_e1, tv_e0, tv_e1)) return 0;
 
 		//proyected vertices
-		btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0);
-		btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1);
+		btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0 + 1) % 3], tu_scale_e0);
+		btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1 + 1) % 3], tu_scale_e1);
 
-		btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0);
-		btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1);
+		btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0 + 1) % 3], tv_scale_e0);
+		btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1 + 1) % 3], tv_scale_e1);
 
 		//proyected intervals
-		GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)};
-		GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)};
+		GREAL isect_u[] = {up_e0.dot(edge_edge_dir), up_e1.dot(edge_edge_dir)};
+		GREAL isect_v[] = {vp_e0.dot(edge_edge_dir), vp_e1.dot(edge_edge_dir)};
 
-		sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1);
-		sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1);
+		sort_isect(isect_u[0], isect_u[1], tu_e0, tu_e1, up_e0, up_e1);
+		sort_isect(isect_v[0], isect_v[1], tv_e0, tv_e1, vp_e0, vp_e1);
 
-		const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint
-		const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint
+		const GREAL midpoint_u = 0.5f * (isect_u[0] + isect_u[1]);  // midpoint
+		const GREAL midpoint_v = 0.5f * (isect_v[0] + isect_v[1]);  // midpoint
 
-		if(midpoint_u<midpoint_v)
+		if (midpoint_u < midpoint_v)
 		{
-			if(isect_u[1]>=isect_v[1]) // face U casts face V
+			if (isect_u[1] >= isect_v[1])  // face U casts face V
 			{
 				return 1;
 			}
-			else if(isect_v[0]<=isect_u[0]) // face V casts face U
+			else if (isect_v[0] <= isect_u[0])  // face V casts face U
 			{
 				return 2;
 			}
@@ -257,32 +253,31 @@ public:
 			closest_point_v = vp_e0;
 			// calc edges and separation
 
-			if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead
+			if (isect_u[1] + MIN_EDGE_EDGE_DIS < isect_v[0])  //calc distance between two lines instead
 			{
 				SEGMENT_COLLISION(
-					tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3],
-					tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3],
+					tu_vertices[tu_e1], tu_vertices[(tu_e1 + 1) % 3],
+					tv_vertices[tv_e0], tv_vertices[(tv_e0 + 1) % 3],
 					closest_point_u,
 					closest_point_v);
 
-				edge_edge_dir = closest_point_u-closest_point_v;
-				VEC_LENGTH(edge_edge_dir,distances[2]);
-				edge_edge_dir *= 1.0f/distances[2];// normalize
+				edge_edge_dir = closest_point_u - closest_point_v;
+				VEC_LENGTH(edge_edge_dir, distances[2]);
+				edge_edge_dir *= 1.0f / distances[2];  // normalize
 			}
 			else
 			{
-				distances[2] = isect_v[0]-isect_u[1];//distance negative
-				//edge_edge_dir *= -1.0f; //normal pointing from V to U
+				distances[2] = isect_v[0] - isect_u[1];  //distance negative
+														 //edge_edge_dir *= -1.0f; //normal pointing from V to U
 			}
-
 		}
 		else
 		{
-			if(isect_v[1]>=isect_u[1]) // face V casts face U
+			if (isect_v[1] >= isect_u[1])  // face V casts face U
 			{
 				return 2;
 			}
-			else if(isect_u[0]<=isect_v[0]) // face U casts face V
+			else if (isect_u[0] <= isect_v[0])  // face U casts face V
 			{
 				return 1;
 			}
@@ -291,41 +286,39 @@ public:
 			closest_point_v = vp_e1;
 			// calc edges and separation
 
-			if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead
+			if (isect_v[1] + MIN_EDGE_EDGE_DIS < isect_u[0])  //calc distance between two lines instead
 			{
 				SEGMENT_COLLISION(
-					tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3],
-					tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3],
+					tu_vertices[tu_e0], tu_vertices[(tu_e0 + 1) % 3],
+					tv_vertices[tv_e1], tv_vertices[(tv_e1 + 1) % 3],
 					closest_point_u,
 					closest_point_v);
 
-				edge_edge_dir = closest_point_u-closest_point_v;
-				VEC_LENGTH(edge_edge_dir,distances[2]);
-				edge_edge_dir *= 1.0f/distances[2];// normalize
+				edge_edge_dir = closest_point_u - closest_point_v;
+				VEC_LENGTH(edge_edge_dir, distances[2]);
+				edge_edge_dir *= 1.0f / distances[2];  // normalize
 			}
 			else
 			{
-				distances[2] = isect_u[0]-isect_v[1];//distance negative
-				//edge_edge_dir *= -1.0f; //normal pointing from V to U
+				distances[2] = isect_u[0] - isect_v[1];  //distance negative
+														 //edge_edge_dir *= -1.0f; //normal pointing from V to U
 			}
 		}
 		return 3;
 	}
 
-
 	//! collides by two sides
 	SIMD_FORCE_INLINE bool triangle_collision(
-					const btVector3 & u0,
-					const btVector3 & u1,
-					const btVector3 & u2,
-					GREAL margin_u,
-					const btVector3 & v0,
-					const btVector3 & v1,
-					const btVector3 & v2,
-					GREAL margin_v,
-					GIM_TRIANGLE_CONTACT_DATA & contacts)
+		const btVector3 &u0,
+		const btVector3 &u1,
+		const btVector3 &u2,
+		GREAL margin_u,
+		const btVector3 &v0,
+		const btVector3 &v1,
+		const btVector3 &v2,
+		GREAL margin_v,
+		GIM_TRIANGLE_CONTACT_DATA &contacts)
 	{
-
 		margin = margin_u + margin_v;
 
 		tu_vertices[0] = u0;
@@ -339,103 +332,99 @@ public:
 		//create planes
 		// plane v vs U points
 
-		TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane);
-
-		du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]);
-		du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]);
-		du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]);
+		TRIANGLE_PLANE(tv_vertices[0], tv_vertices[1], tv_vertices[2], tv_plane);
 
+		du[0] = DISTANCE_PLANE_POINT(tv_plane, tu_vertices[0]);
+		du[1] = DISTANCE_PLANE_POINT(tv_plane, tu_vertices[1]);
+		du[2] = DISTANCE_PLANE_POINT(tv_plane, tu_vertices[2]);
 
 		du0du1 = du[0] * du[1];
 		du0du2 = du[0] * du[2];
 
-
-		if(du0du1>0.0f && du0du2>0.0f)	// same sign on all of them + not equal 0 ?
+		if (du0du1 > 0.0f && du0du2 > 0.0f)  // same sign on all of them + not equal 0 ?
 		{
-			if(du[0]<0) //we need test behind the triangle plane
+			if (du[0] < 0)  //we need test behind the triangle plane
 			{
-				distances[0] = GIM_MAX3(du[0],du[1],du[2]);
+				distances[0] = GIM_MAX3(du[0], du[1], du[2]);
 				distances[0] = -distances[0];
-				if(distances[0]>margin) return false; //never intersect
+				if (distances[0] > margin) return false;  //never intersect
 
 				//reorder triangle v
-				VEC_SWAP(tv_vertices[0],tv_vertices[1]);
-				VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
+				VEC_SWAP(tv_vertices[0], tv_vertices[1]);
+				VEC_SCALE_4(tv_plane, -1.0f, tv_plane);
 			}
 			else
 			{
-				distances[0] = GIM_MIN3(du[0],du[1],du[2]);
-				if(distances[0]>margin) return false; //never intersect
+				distances[0] = GIM_MIN3(du[0], du[1], du[2]);
+				if (distances[0] > margin) return false;  //never intersect
 			}
 		}
 		else
 		{
 			//Look if we need to invert the triangle
-			distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid
+			distances[0] = (du[0] + du[1] + du[2]) / 3.0f;  //centroid
 
-			if(distances[0]<0.0f)
+			if (distances[0] < 0.0f)
 			{
 				//reorder triangle v
-				VEC_SWAP(tv_vertices[0],tv_vertices[1]);
-				VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
+				VEC_SWAP(tv_vertices[0], tv_vertices[1]);
+				VEC_SCALE_4(tv_plane, -1.0f, tv_plane);
 
-				distances[0] = GIM_MAX3(du[0],du[1],du[2]);
+				distances[0] = GIM_MAX3(du[0], du[1], du[2]);
 				distances[0] = -distances[0];
 			}
 			else
 			{
-				distances[0] = GIM_MIN3(du[0],du[1],du[2]);
+				distances[0] = GIM_MIN3(du[0], du[1], du[2]);
 			}
 		}
 
-
 		// plane U vs V points
 
-		TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane);
+		TRIANGLE_PLANE(tu_vertices[0], tu_vertices[1], tu_vertices[2], tu_plane);
 
-		dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]);
-		dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]);
-		dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]);
+		dv[0] = DISTANCE_PLANE_POINT(tu_plane, tv_vertices[0]);
+		dv[1] = DISTANCE_PLANE_POINT(tu_plane, tv_vertices[1]);
+		dv[2] = DISTANCE_PLANE_POINT(tu_plane, tv_vertices[2]);
 
 		dv0dv1 = dv[0] * dv[1];
 		dv0dv2 = dv[0] * dv[2];
 
-
-		if(dv0dv1>0.0f && dv0dv2>0.0f)	// same sign on all of them + not equal 0 ?
+		if (dv0dv1 > 0.0f && dv0dv2 > 0.0f)  // same sign on all of them + not equal 0 ?
 		{
-			if(dv[0]<0) //we need test behind the triangle plane
+			if (dv[0] < 0)  //we need test behind the triangle plane
 			{
-				distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
+				distances[1] = GIM_MAX3(dv[0], dv[1], dv[2]);
 				distances[1] = -distances[1];
-				if(distances[1]>margin) return false; //never intersect
+				if (distances[1] > margin) return false;  //never intersect
 
 				//reorder triangle u
-				VEC_SWAP(tu_vertices[0],tu_vertices[1]);
-				VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
+				VEC_SWAP(tu_vertices[0], tu_vertices[1]);
+				VEC_SCALE_4(tu_plane, -1.0f, tu_plane);
 			}
 			else
 			{
-				distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
-				if(distances[1]>margin) return false; //never intersect
+				distances[1] = GIM_MIN3(dv[0], dv[1], dv[2]);
+				if (distances[1] > margin) return false;  //never intersect
 			}
 		}
 		else
 		{
 			//Look if we need to invert the triangle
-			distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid
+			distances[1] = (dv[0] + dv[1] + dv[2]) / 3.0f;  //centroid
 
-			if(distances[1]<0.0f)
+			if (distances[1] < 0.0f)
 			{
 				//reorder triangle v
-				VEC_SWAP(tu_vertices[0],tu_vertices[1]);
-				VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
+				VEC_SWAP(tu_vertices[0], tu_vertices[1]);
+				VEC_SCALE_4(tu_plane, -1.0f, tu_plane);
 
-				distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
+				distances[1] = GIM_MAX3(dv[0], dv[1], dv[2]);
 				distances[1] = -distances[1];
 			}
 			else
 			{
-				distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
+				distances[1] = GIM_MIN3(dv[0], dv[1], dv[2]);
 			}
 		}
 
@@ -448,47 +437,44 @@ public:
 		}
 		else
 		{*/
-			bl = 0;
-			if(distances[0]<distances[1]) bl = 1;
+		bl = 0;
+		if (distances[0] < distances[1]) bl = 1;
 		//}
 
-		if(bl==2) //edge edge separation
+		if (bl == 2)  //edge edge separation
 		{
-			if(distances[2]>margin) return false;
+			if (distances[2] > margin) return false;
 
 			contacts.m_penetration_depth = -distances[2] + margin;
 			contacts.m_points[0] = closest_point_v;
 			contacts.m_point_count = 1;
-			VEC_COPY(contacts.m_separating_normal,edge_edge_dir);
+			VEC_COPY(contacts.m_separating_normal, edge_edge_dir);
 
 			return true;
 		}
 
 		//clip face against other
 
-		
 		GUINT point_count;
 		//TODO
-		if(bl == 0) //clip U points against V
+		if (bl == 0)  //clip U points against V
 		{
-			point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points);
-			if(point_count == 0) return false;						
-			contacts.merge_points(tv_plane,margin,contact_points,point_count);			
+			point_count = clip_triangle(tv_plane, tv_vertices, tu_vertices, contact_points);
+			if (point_count == 0) return false;
+			contacts.merge_points(tv_plane, margin, contact_points, point_count);
 		}
-		else //clip V points against U
+		else  //clip V points against U
 		{
-			point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points);
-			if(point_count == 0) return false;			
-			contacts.merge_points(tu_plane,margin,contact_points,point_count);
+			point_count = clip_triangle(tu_plane, tu_vertices, tv_vertices, contact_points);
+			if (point_count == 0) return false;
+			contacts.merge_points(tu_plane, margin, contact_points, point_count);
 			contacts.m_separating_normal *= -1.f;
 		}
-		if(contacts.m_point_count == 0) return false;
+		if (contacts.m_point_count == 0) return false;
 		return true;
 	}
-
 };
 
-
 /*class GIM_TRIANGLE_CALCULATION_CACHE
 {
 public:
@@ -621,20 +607,13 @@ public:
 
 };*/
 
-
-
 bool GIM_TRIANGLE::collide_triangle_hard_test(
-		const GIM_TRIANGLE & other,
-		GIM_TRIANGLE_CONTACT_DATA & contact_data) const
+	const GIM_TRIANGLE &other,
+	GIM_TRIANGLE_CONTACT_DATA &contact_data) const
 {
-	GIM_TRIANGLE_CALCULATION_CACHE calc_cache;	
+	GIM_TRIANGLE_CALCULATION_CACHE calc_cache;
 	return calc_cache.triangle_collision(
-					m_vertices[0],m_vertices[1],m_vertices[2],m_margin,
-					other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin,
-					contact_data);
-
+		m_vertices[0], m_vertices[1], m_vertices[2], m_margin,
+		other.m_vertices[0], other.m_vertices[1], other.m_vertices[2], other.m_margin,
+		contact_data);
 }
-
-
-
-
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h
index 5b552a1ed51..e6d4bf54708 100644
--- a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h
+++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h
@@ -36,26 +36,25 @@ email: projectileman@yahoo.com
 #include "gim_box_collision.h"
 #include "gim_clip_polygon.h"
 
-
-
-
+#ifndef MAX_TRI_CLIPPING
 #define MAX_TRI_CLIPPING 16
+#endif
 
 //! Structure for collision
 struct GIM_TRIANGLE_CONTACT_DATA
 {
-    GREAL m_penetration_depth;
-    GUINT m_point_count;
-    btVector4 m_separating_normal;
-    btVector3 m_points[MAX_TRI_CLIPPING];
+	GREAL m_penetration_depth;
+	GUINT m_point_count;
+	btVector4 m_separating_normal;
+	btVector3 m_points[MAX_TRI_CLIPPING];
 
-	SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA& other)
+	SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA &other)
 	{
 		m_penetration_depth = other.m_penetration_depth;
 		m_separating_normal = other.m_separating_normal;
 		m_point_count = other.m_point_count;
 		GUINT i = m_point_count;
-		while(i--)
+		while (i--)
 		{
 			m_points[i] = other.m_points[i];
 		}
@@ -65,39 +64,36 @@ struct GIM_TRIANGLE_CONTACT_DATA
 	{
 	}
 
-	GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA& other)
+	GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA &other)
 	{
 		copy_from(other);
 	}
 
-	
-	
-
-    //! classify points that are closer
-    template<typename DISTANCE_FUNC,typename CLASS_PLANE>
-    SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE & plane,
-    				GREAL margin, const btVector3 * points, GUINT point_count, DISTANCE_FUNC distance_func)
-    {	
-    	m_point_count = 0;
-    	m_penetration_depth= -1000.0f;
+	//! classify points that are closer
+	template <typename DISTANCE_FUNC, typename CLASS_PLANE>
+	SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE &plane,
+											   GREAL margin, const btVector3 *points, GUINT point_count, DISTANCE_FUNC distance_func)
+	{
+		m_point_count = 0;
+		m_penetration_depth = -1000.0f;
 
 		GUINT point_indices[MAX_TRI_CLIPPING];
 
 		GUINT _k;
 
-		for(_k=0;_k<point_count;_k++)
+		for (_k = 0; _k < point_count; _k++)
 		{
-			GREAL _dist = -distance_func(plane,points[_k]) + margin;
+			GREAL _dist = -distance_func(plane, points[_k]) + margin;
 
-			if(_dist>=0.0f)
+			if (_dist >= 0.0f)
 			{
-				if(_dist>m_penetration_depth)
+				if (_dist > m_penetration_depth)
 				{
 					m_penetration_depth = _dist;
 					point_indices[0] = _k;
-					m_point_count=1;
+					m_point_count = 1;
 				}
-				else if((_dist+G_EPSILON)>=m_penetration_depth)
+				else if ((_dist + G_EPSILON) >= m_penetration_depth)
 				{
 					point_indices[m_point_count] = _k;
 					m_point_count++;
@@ -105,88 +101,87 @@ struct GIM_TRIANGLE_CONTACT_DATA
 			}
 		}
 
-		for( _k=0;_k<m_point_count;_k++)
+		for (_k = 0; _k < m_point_count; _k++)
 		{
 			m_points[_k] = points[point_indices[_k]];
 		}
 	}
 
 	//! classify points that are closer
-	SIMD_FORCE_INLINE void merge_points(const btVector4 & plane, GREAL margin,
-										 const btVector3 * points, GUINT point_count)
+	SIMD_FORCE_INLINE void merge_points(const btVector4 &plane, GREAL margin,
+										const btVector3 *points, GUINT point_count)
 	{
 		m_separating_normal = plane;
 		mergepoints_generic(plane, margin, points, point_count, DISTANCE_PLANE_3D_FUNC());
 	}
 };
 
-
 //! Class for colliding triangles
 class GIM_TRIANGLE
 {
 public:
 	btScalar m_margin;
-    btVector3 m_vertices[3];
-
-    GIM_TRIANGLE():m_margin(0.1f)
-    {
-    }
-
-    SIMD_FORCE_INLINE GIM_AABB get_box()  const
-    {
-    	return GIM_AABB(m_vertices[0],m_vertices[1],m_vertices[2],m_margin);
-    }
-
-    SIMD_FORCE_INLINE void get_normal(btVector3 &normal)  const
-    {
-    	TRIANGLE_NORMAL(m_vertices[0],m_vertices[1],m_vertices[2],normal);
-    }
-
-    SIMD_FORCE_INLINE void get_plane(btVector4 &plane)  const
-    {
-    	TRIANGLE_PLANE(m_vertices[0],m_vertices[1],m_vertices[2],plane);;
-    }
-
-    SIMD_FORCE_INLINE void apply_transform(const btTransform & trans)
-    {
-    	m_vertices[0] = trans(m_vertices[0]);
-    	m_vertices[1] = trans(m_vertices[1]);
-    	m_vertices[2] = trans(m_vertices[2]);
-    }
-
-    SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index,const btVector3 &triangle_normal,btVector4 &plane)  const
-    {
-		const btVector3 & e0 = m_vertices[edge_index];
-		const btVector3 & e1 = m_vertices[(edge_index+1)%3];
-		EDGE_PLANE(e0,e1,triangle_normal,plane);
-    }
-
-    //! Gets the relative transformation of this triangle
-    /*!
+	btVector3 m_vertices[3];
+
+	GIM_TRIANGLE() : m_margin(0.1f)
+	{
+	}
+
+	SIMD_FORCE_INLINE GIM_AABB get_box() const
+	{
+		return GIM_AABB(m_vertices[0], m_vertices[1], m_vertices[2], m_margin);
+	}
+
+	SIMD_FORCE_INLINE void get_normal(btVector3 &normal) const
+	{
+		TRIANGLE_NORMAL(m_vertices[0], m_vertices[1], m_vertices[2], normal);
+	}
+
+	SIMD_FORCE_INLINE void get_plane(btVector4 &plane) const
+	{
+		TRIANGLE_PLANE(m_vertices[0], m_vertices[1], m_vertices[2], plane);
+		;
+	}
+
+	SIMD_FORCE_INLINE void apply_transform(const btTransform &trans)
+	{
+		m_vertices[0] = trans(m_vertices[0]);
+		m_vertices[1] = trans(m_vertices[1]);
+		m_vertices[2] = trans(m_vertices[2]);
+	}
+
+	SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index, const btVector3 &triangle_normal, btVector4 &plane) const
+	{
+		const btVector3 &e0 = m_vertices[edge_index];
+		const btVector3 &e1 = m_vertices[(edge_index + 1) % 3];
+		EDGE_PLANE(e0, e1, triangle_normal, plane);
+	}
+
+	//! Gets the relative transformation of this triangle
+	/*!
     The transformation is oriented to the triangle normal , and aligned to the 1st edge of this triangle. The position corresponds to vertice 0:
     - triangle normal corresponds to Z axis.
     - 1st normalized edge corresponds to X axis,
 
     */
-    SIMD_FORCE_INLINE void get_triangle_transform(btTransform & triangle_transform)  const
-    {
-    	btMatrix3x3 & matrix = triangle_transform.getBasis();
-
-    	btVector3 zaxis;
-    	get_normal(zaxis);
-    	MAT_SET_Z(matrix,zaxis);
+	SIMD_FORCE_INLINE void get_triangle_transform(btTransform &triangle_transform) const
+	{
+		btMatrix3x3 &matrix = triangle_transform.getBasis();
 
-    	btVector3 xaxis = m_vertices[1] - m_vertices[0];
-    	VEC_NORMALIZE(xaxis);
-    	MAT_SET_X(matrix,xaxis);
+		btVector3 zaxis;
+		get_normal(zaxis);
+		MAT_SET_Z(matrix, zaxis);
 
-    	//y axis
-    	xaxis = zaxis.cross(xaxis);
-    	MAT_SET_Y(matrix,xaxis);
+		btVector3 xaxis = m_vertices[1] - m_vertices[0];
+		VEC_NORMALIZE(xaxis);
+		MAT_SET_X(matrix, xaxis);
 
-    	triangle_transform.setOrigin(m_vertices[0]);
-    }
+		//y axis
+		xaxis = zaxis.cross(xaxis);
+		MAT_SET_Y(matrix, xaxis);
 
+		triangle_transform.setOrigin(m_vertices[0]);
+	}
 
 	//! Test triangles by finding separating axis
 	/*!
@@ -194,8 +189,8 @@ public:
 	\param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle
 	*/
 	bool collide_triangle_hard_test(
-		const GIM_TRIANGLE & other,
-		GIM_TRIANGLE_CONTACT_DATA & contact_data) const;
+		const GIM_TRIANGLE &other,
+		GIM_TRIANGLE_CONTACT_DATA &contact_data) const;
 
 	//! Test boxes before doing hard test
 	/*!
@@ -204,16 +199,16 @@ public:
 	\
 	*/
 	SIMD_FORCE_INLINE bool collide_triangle(
-		const GIM_TRIANGLE & other,
-		GIM_TRIANGLE_CONTACT_DATA & contact_data) const
+		const GIM_TRIANGLE &other,
+		GIM_TRIANGLE_CONTACT_DATA &contact_data) const
 	{
 		//test box collisioin
-		GIM_AABB boxu(m_vertices[0],m_vertices[1],m_vertices[2],m_margin);
-		GIM_AABB boxv(other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin);
-		if(!boxu.has_collision(boxv)) return false;
+		GIM_AABB boxu(m_vertices[0], m_vertices[1], m_vertices[2], m_margin);
+		GIM_AABB boxv(other.m_vertices[0], other.m_vertices[1], other.m_vertices[2], other.m_margin);
+		if (!boxu.has_collision(boxv)) return false;
 
 		//do hard test
-		return collide_triangle_hard_test(other,contact_data);
+		return collide_triangle_hard_test(other, contact_data);
 	}
 
 	/*!
@@ -245,43 +240,43 @@ if 0.0<= u+v <=1.0 then they are inside of triangle
 	\return false if the point is outside of triangle.This function  doesn't take the margin
 	*/
 	SIMD_FORCE_INLINE bool get_uv_parameters(
-			const btVector3 & point,
-			const btVector3 & tri_plane,
-			GREAL & u, GREAL & v) const
+		const btVector3 &point,
+		const btVector3 &tri_plane,
+		GREAL &u, GREAL &v) const
 	{
-		btVector3 _axe1 = m_vertices[1]-m_vertices[0];
-		btVector3 _axe2 = m_vertices[2]-m_vertices[0];
+		btVector3 _axe1 = m_vertices[1] - m_vertices[0];
+		btVector3 _axe2 = m_vertices[2] - m_vertices[0];
 		btVector3 _vecproj = point - m_vertices[0];
-		GUINT _i1 = (tri_plane.closestAxis()+1)%3;
-		GUINT _i2 = (_i1+1)%3;
-		if(btFabs(_axe2[_i2])<G_EPSILON)
+		GUINT _i1 = (tri_plane.closestAxis() + 1) % 3;
+		GUINT _i2 = (_i1 + 1) % 3;
+		if (btFabs(_axe2[_i2]) < G_EPSILON)
 		{
-			u = (_vecproj[_i2]*_axe2[_i1] - _vecproj[_i1]*_axe2[_i2]) /(_axe1[_i2]*_axe2[_i1]  - _axe1[_i1]*_axe2[_i2]);
-			v = (_vecproj[_i1] - u*_axe1[_i1])/_axe2[_i1];
+			u = (_vecproj[_i2] * _axe2[_i1] - _vecproj[_i1] * _axe2[_i2]) / (_axe1[_i2] * _axe2[_i1] - _axe1[_i1] * _axe2[_i2]);
+			v = (_vecproj[_i1] - u * _axe1[_i1]) / _axe2[_i1];
 		}
 		else
 		{
-			u = (_vecproj[_i1]*_axe2[_i2] - _vecproj[_i2]*_axe2[_i1]) /(_axe1[_i1]*_axe2[_i2]  - _axe1[_i2]*_axe2[_i1]);
-			v = (_vecproj[_i2] - u*_axe1[_i2])/_axe2[_i2];
+			u = (_vecproj[_i1] * _axe2[_i2] - _vecproj[_i2] * _axe2[_i1]) / (_axe1[_i1] * _axe2[_i2] - _axe1[_i2] * _axe2[_i1]);
+			v = (_vecproj[_i2] - u * _axe1[_i2]) / _axe2[_i2];
 		}
 
-		if(u<-G_EPSILON)
+		if (u < -G_EPSILON)
 		{
 			return false;
 		}
-		else if(v<-G_EPSILON)
+		else if (v < -G_EPSILON)
 		{
 			return false;
 		}
 		else
 		{
 			btScalar sumuv;
-			sumuv = u+v;
-			if(sumuv<-G_EPSILON)
+			sumuv = u + v;
+			if (sumuv < -G_EPSILON)
 			{
 				return false;
 			}
-			else if(sumuv-1.0f>G_EPSILON)
+			else if (sumuv - 1.0f > G_EPSILON)
 			{
 				return false;
 			}
@@ -293,50 +288,49 @@ if 0.0<= u+v <=1.0 then they are inside of triangle
 	/*!
 	Test if point is in triangle, with m_margin tolerance
 	*/
-	SIMD_FORCE_INLINE bool is_point_inside(const btVector3 & point, const btVector3 & tri_normal) const
+	SIMD_FORCE_INLINE bool is_point_inside(const btVector3 &point, const btVector3 &tri_normal) const
 	{
 		//Test with edge 0
 		btVector4 edge_plane;
-		this->get_edge_plane(0,tri_normal,edge_plane);
-		GREAL dist = DISTANCE_PLANE_POINT(edge_plane,point);
-		if(dist-m_margin>0.0f) return false; // outside plane
+		this->get_edge_plane(0, tri_normal, edge_plane);
+		GREAL dist = DISTANCE_PLANE_POINT(edge_plane, point);
+		if (dist - m_margin > 0.0f) return false;  // outside plane
 
-		this->get_edge_plane(1,tri_normal,edge_plane);
-		dist = DISTANCE_PLANE_POINT(edge_plane,point);
-		if(dist-m_margin>0.0f) return false; // outside plane
+		this->get_edge_plane(1, tri_normal, edge_plane);
+		dist = DISTANCE_PLANE_POINT(edge_plane, point);
+		if (dist - m_margin > 0.0f) return false;  // outside plane
 
-		this->get_edge_plane(2,tri_normal,edge_plane);
-		dist = DISTANCE_PLANE_POINT(edge_plane,point);
-		if(dist-m_margin>0.0f) return false; // outside plane
+		this->get_edge_plane(2, tri_normal, edge_plane);
+		dist = DISTANCE_PLANE_POINT(edge_plane, point);
+		if (dist - m_margin > 0.0f) return false;  // outside plane
 		return true;
 	}
 
-
 	//! Bidireccional ray collision
 	SIMD_FORCE_INLINE bool ray_collision(
-		const btVector3 & vPoint,
-		const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal,
-		GREAL & tparam, GREAL tmax = G_REAL_INFINITY)
+		const btVector3 &vPoint,
+		const btVector3 &vDir, btVector3 &pout, btVector3 &triangle_normal,
+		GREAL &tparam, GREAL tmax = G_REAL_INFINITY)
 	{
 		btVector4 faceplane;
 		{
 			btVector3 dif1 = m_vertices[1] - m_vertices[0];
 			btVector3 dif2 = m_vertices[2] - m_vertices[0];
-    		VEC_CROSS(faceplane,dif1,dif2);
-    		faceplane[3] = m_vertices[0].dot(faceplane);
+			VEC_CROSS(faceplane, dif1, dif2);
+			faceplane[3] = m_vertices[0].dot(faceplane);
 		}
 
-		GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax);
-		if(res == 0) return false;
-		if(! is_point_inside(pout,faceplane)) return false;
+		GUINT res = LINE_PLANE_COLLISION(faceplane, vDir, vPoint, pout, tparam, btScalar(0), tmax);
+		if (res == 0) return false;
+		if (!is_point_inside(pout, faceplane)) return false;
 
-		if(res==2) //invert normal
+		if (res == 2)  //invert normal
 		{
-			triangle_normal.setValue(-faceplane[0],-faceplane[1],-faceplane[2]);
+			triangle_normal.setValue(-faceplane[0], -faceplane[1], -faceplane[2]);
 		}
 		else
 		{
-			triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]);
+			triangle_normal.setValue(faceplane[0], faceplane[1], faceplane[2]);
 		}
 
 		VEC_NORMALIZE(triangle_normal);
@@ -344,36 +338,31 @@ if 0.0<= u+v <=1.0 then they are inside of triangle
 		return true;
 	}
 
-
 	//! one direccion ray collision
 	SIMD_FORCE_INLINE bool ray_collision_front_side(
-		const btVector3 & vPoint,
-		const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal,
-		GREAL & tparam, GREAL tmax = G_REAL_INFINITY)
+		const btVector3 &vPoint,
+		const btVector3 &vDir, btVector3 &pout, btVector3 &triangle_normal,
+		GREAL &tparam, GREAL tmax = G_REAL_INFINITY)
 	{
 		btVector4 faceplane;
 		{
 			btVector3 dif1 = m_vertices[1] - m_vertices[0];
 			btVector3 dif2 = m_vertices[2] - m_vertices[0];
-    		VEC_CROSS(faceplane,dif1,dif2);
-    		faceplane[3] = m_vertices[0].dot(faceplane);
+			VEC_CROSS(faceplane, dif1, dif2);
+			faceplane[3] = m_vertices[0].dot(faceplane);
 		}
 
-		GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax);
-		if(res != 1) return false;
+		GUINT res = LINE_PLANE_COLLISION(faceplane, vDir, vPoint, pout, tparam, btScalar(0), tmax);
+		if (res != 1) return false;
 
-		if(!is_point_inside(pout,faceplane)) return false;
+		if (!is_point_inside(pout, faceplane)) return false;
 
-		triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]);
+		triangle_normal.setValue(faceplane[0], faceplane[1], faceplane[2]);
 
 		VEC_NORMALIZE(triangle_normal);
 
 		return true;
 	}
-
 };
 
-
-
-
-#endif // GIM_TRI_COLLISION_H_INCLUDED
+#endif  // GIM_TRI_COLLISION_H_INCLUDED
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h
index 9eb880b8dfc..3c82133037e 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h
@@ -16,163 +16,153 @@ subject to the following restrictions:
 #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 "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))
 
-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;
+	{
+		//	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)
+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();
+	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 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
+			btVector3 separatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
+
+			btVector3 pInA = a.getLocalSupportWithoutMargin(separatingAxisInA);
+			btVector3 qInB = b.getLocalSupportWithoutMargin(separatingAxisInB);
+
+			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)
@@ -182,188 +172,183 @@ int	btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate
                 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  //
+
+			//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;
-}
 
+				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
+#endif  //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
index 940282f5762..38df8d4808e 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btContinuousConvexCollision.h"
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
@@ -24,59 +23,60 @@ subject to the following restrictions:
 #include "btPointCollector.h"
 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
 
-
-
-btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape*	convexA,const btConvexShape*	convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
-:m_simplexSolver(simplexSolver),
-m_penetrationDepthSolver(penetrationDepthSolver),
-m_convexA(convexA),m_convexB1(convexB),m_planeShape(0)
+btContinuousConvexCollision::btContinuousConvexCollision(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
+	: m_simplexSolver(simplexSolver),
+	  m_penetrationDepthSolver(penetrationDepthSolver),
+	  m_convexA(convexA),
+	  m_convexB1(convexB),
+	  m_planeShape(0)
 {
 }
 
-
-btContinuousConvexCollision::btContinuousConvexCollision( const btConvexShape*	convexA,const btStaticPlaneShape*	plane)
-:m_simplexSolver(0),
-m_penetrationDepthSolver(0),
-m_convexA(convexA),m_convexB1(0),m_planeShape(plane)
+btContinuousConvexCollision::btContinuousConvexCollision(const btConvexShape* convexA, const btStaticPlaneShape* plane)
+	: m_simplexSolver(0),
+	  m_penetrationDepthSolver(0),
+	  m_convexA(convexA),
+	  m_convexB1(0),
+	  m_planeShape(plane)
 {
 }
 
-
 /// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
 /// You don't want your game ever to lock-up.
 #define MAX_ITERATIONS 64
 
-void btContinuousConvexCollision::computeClosestPoints( const btTransform& transA, const btTransform& transB,btPointCollector& pointCollector)
+void btContinuousConvexCollision::computeClosestPoints(const btTransform& transA, const btTransform& transB, btPointCollector& pointCollector)
 {
 	if (m_convexB1)
 	{
 		m_simplexSolver->reset();
-		btGjkPairDetector gjk(m_convexA,m_convexB1,m_convexA->getShapeType(),m_convexB1->getShapeType(),m_convexA->getMargin(),m_convexB1->getMargin(),m_simplexSolver,m_penetrationDepthSolver);		
+		btGjkPairDetector gjk(m_convexA, m_convexB1, m_convexA->getShapeType(), m_convexB1->getShapeType(), m_convexA->getMargin(), m_convexB1->getMargin(), m_simplexSolver, m_penetrationDepthSolver);
 		btGjkPairDetector::ClosestPointInput input;
 		input.m_transformA = transA;
 		input.m_transformB = transB;
-		gjk.getClosestPoints(input,pointCollector,0);
-	} else
+		gjk.getClosestPoints(input, pointCollector, 0);
+	}
+	else
 	{
 		//convex versus plane
 		const btConvexShape* convexShape = m_convexA;
 		const btStaticPlaneShape* planeShape = m_planeShape;
-		
+
 		const btVector3& planeNormal = planeShape->getPlaneNormal();
 		const btScalar& planeConstant = planeShape->getPlaneConstant();
-		
+
 		btTransform convexWorldTransform = transA;
 		btTransform convexInPlaneTrans;
-		convexInPlaneTrans= transB.inverse() * convexWorldTransform;
+		convexInPlaneTrans = transB.inverse() * convexWorldTransform;
 		btTransform planeInConvex;
-		planeInConvex= convexWorldTransform.inverse() * transB;
-		
-		btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
+		planeInConvex = convexWorldTransform.inverse() * transB;
+
+		btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal);
 
 		btVector3 vtxInPlane = convexInPlaneTrans(vtx);
 		btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
 
-		btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
+		btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
 		btVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
 		btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal;
 
@@ -87,41 +87,33 @@ void btContinuousConvexCollision::computeClosestPoints( const btTransform& trans
 	}
 }
 
-bool	btContinuousConvexCollision::calcTimeOfImpact(
-				const btTransform& fromA,
-				const btTransform& toA,
-				const btTransform& fromB,
-				const btTransform& toB,
-				CastResult& result)
+bool btContinuousConvexCollision::calcTimeOfImpact(
+	const btTransform& fromA,
+	const btTransform& toA,
+	const btTransform& fromB,
+	const btTransform& toB,
+	CastResult& result)
 {
-
-
 	/// compute linear and angular velocity for this interval, to interpolate
-	btVector3 linVelA,angVelA,linVelB,angVelB;
-	btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA);
-	btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB);
-
+	btVector3 linVelA, angVelA, linVelB, angVelB;
+	btTransformUtil::calculateVelocity(fromA, toA, btScalar(1.), linVelA, angVelA);
+	btTransformUtil::calculateVelocity(fromB, toB, btScalar(1.), linVelB, angVelB);
 
 	btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
-	btScalar boundingRadiusB = m_convexB1?m_convexB1->getAngularMotionDisc():0.f;
+	btScalar boundingRadiusB = m_convexB1 ? m_convexB1->getAngularMotionDisc() : 0.f;
 
 	btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
-	btVector3 relLinVel = (linVelB-linVelA);
-
-	btScalar relLinVelocLength = (linVelB-linVelA).length();
-	
-	if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f)
-		return false;
+	btVector3 relLinVel = (linVelB - linVelA);
 
+	btScalar relLinVelocLength = (linVelB - linVelA).length();
 
+	if ((relLinVelocLength + maxAngularProjectedVelocity) == 0.f)
+		return false;
 
 	btScalar lambda = btScalar(0.);
-	btVector3 v(1,0,0);
-
-	int maxIter = MAX_ITERATIONS;
 
 	btVector3 n;
-	n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+	n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 	bool hasResult = false;
 	btVector3 c;
 
@@ -131,15 +123,13 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 	int numIter = 0;
 	//first solution, using GJK
 
-
 	btScalar radius = 0.001f;
-//	result.drawCoordSystem(sphereTr);
+	//	result.drawCoordSystem(sphereTr);
 
-	btPointCollector	pointCollector1;
+	btPointCollector pointCollector1;
 
 	{
-	
-		computeClosestPoints(fromA,fromB,pointCollector1);
+		computeClosestPoints(fromA, fromB, pointCollector1);
 
 		hasResult = pointCollector1.m_hasResult;
 		c = pointCollector1.m_pointInWorld;
@@ -151,7 +141,7 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 		dist = pointCollector1.m_distance + result.m_allowedPenetration;
 		n = pointCollector1.m_normalOnBInWorld;
 		btScalar projectedLinearVelocity = relLinVel.dot(n);
-		if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
+		if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON)
 			return false;
 
 		//not close enough
@@ -159,77 +149,69 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 		{
 			if (result.m_debugDrawer)
 			{
-				result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1));
+				result.m_debugDrawer->drawSphere(c, 0.2f, btVector3(1, 1, 1));
 			}
 			btScalar dLambda = btScalar(0.);
 
 			projectedLinearVelocity = relLinVel.dot(n);
 
-			
 			//don't report time of impact for motion away from the contact normal (or causes minor penetration)
-			if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
+			if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON)
 				return false;
-			
-			dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
 
-			
-			
-			lambda = lambda + dLambda;
+			dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
 
-			if (lambda > btScalar(1.))
-				return false;
+			lambda += dLambda;
 
-			if (lambda < btScalar(0.))
+			if (lambda > btScalar(1.) || lambda < btScalar(0.))
 				return false;
 
-
 			//todo: next check with relative epsilon
 			if (lambda <= lastLambda)
 			{
 				return false;
 				//n.setValue(0,0,0);
-				break;
+				//break;
 			}
 			lastLambda = lambda;
 
-			
-
 			//interpolate to next lambda
-			btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
+			btTransform interpolatedTransA, interpolatedTransB, relativeTrans;
 
-			btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
-			btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
+			btTransformUtil::integrateTransform(fromA, linVelA, angVelA, lambda, interpolatedTransA);
+			btTransformUtil::integrateTransform(fromB, linVelB, angVelB, lambda, interpolatedTransB);
 			relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
 
 			if (result.m_debugDrawer)
 			{
-				result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0));
+				result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(), 0.2f, btVector3(1, 0, 0));
 			}
 
-			result.DebugDraw( lambda );
+			result.DebugDraw(lambda);
 
-			btPointCollector	pointCollector;
-			computeClosestPoints(interpolatedTransA,interpolatedTransB,pointCollector);
+			btPointCollector pointCollector;
+			computeClosestPoints(interpolatedTransA, interpolatedTransB, pointCollector);
 
 			if (pointCollector.m_hasResult)
 			{
-				dist = pointCollector.m_distance+result.m_allowedPenetration;
-				c = pointCollector.m_pointInWorld;		
+				dist = pointCollector.m_distance + result.m_allowedPenetration;
+				c = pointCollector.m_pointInWorld;
 				n = pointCollector.m_normalOnBInWorld;
-			} else
+			}
+			else
 			{
 				result.reportFailure(-1, numIter);
 				return false;
 			}
 
 			numIter++;
-			if (numIter > maxIter)
+			if (numIter > MAX_ITERATIONS)
 			{
 				result.reportFailure(-2, numIter);
 				return false;
 			}
 		}
-	
+
 		result.m_fraction = lambda;
 		result.m_normal = n;
 		result.m_hitPoint = c;
@@ -237,6 +219,4 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 	}
 
 	return false;
-
 }
-
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
index bdc0572f75a..67b2205c366 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_CONTINUOUS_COLLISION_CONVEX_CAST_H
 #define BT_CONTINUOUS_COLLISION_CONVEX_CAST_H
 
@@ -25,35 +24,30 @@ class btStaticPlaneShape;
 
 /// btContinuousConvexCollision implements angular and linear time of impact for convex objects.
 /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
-/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
+/// Algorithm operates in worldspace, in order to keep in between motion globally consistent.
 /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
 class btContinuousConvexCollision : public btConvexCast
 {
 	btSimplexSolverInterface* m_simplexSolver;
-	btConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
-	const btConvexShape*	m_convexA;
+	btConvexPenetrationDepthSolver* m_penetrationDepthSolver;
+	const btConvexShape* m_convexA;
 	//second object is either a convex or a plane (code sharing)
-	const btConvexShape*	m_convexB1;
-	const btStaticPlaneShape*	m_planeShape;
+	const btConvexShape* m_convexB1;
+	const btStaticPlaneShape* m_planeShape;
 
-	void computeClosestPoints( const btTransform& transA, const btTransform& transB,struct btPointCollector& pointCollector);
+	void computeClosestPoints(const btTransform& transA, const btTransform& transB, struct btPointCollector& pointCollector);
 
 public:
+	btContinuousConvexCollision(const btConvexShape* shapeA, const btConvexShape* shapeB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver);
 
-	btContinuousConvexCollision (const btConvexShape*	shapeA,const btConvexShape*	shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
-
-	btContinuousConvexCollision(const btConvexShape*	shapeA,const btStaticPlaneShape*	plane );
-
-	virtual bool	calcTimeOfImpact(
-				const btTransform& fromA,
-				const btTransform& toA,
-				const btTransform& fromB,
-				const btTransform& toB,
-				CastResult& result);
-
+	btContinuousConvexCollision(const btConvexShape* shapeA, const btStaticPlaneShape* plane);
 
+	virtual bool calcTimeOfImpact(
+		const btTransform& fromA,
+		const btTransform& toA,
+		const btTransform& fromB,
+		const btTransform& toB,
+		CastResult& result);
 };
 
-
-#endif //BT_CONTINUOUS_COLLISION_CONVEX_CAST_H
-
+#endif  //BT_CONTINUOUS_COLLISION_CONVEX_CAST_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
index bfd79d03beb..77b19be599f 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_CONVEX_CAST_H
 #define BT_CONVEX_CAST_H
 
@@ -23,51 +22,69 @@ subject to the following restrictions:
 class btMinkowskiSumShape;
 #include "LinearMath/btIDebugDraw.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define MAX_CONVEX_CAST_ITERATIONS 64
+#define MAX_CONVEX_CAST_EPSILON (SIMD_EPSILON * 10)
+#else
+#define MAX_CONVEX_CAST_ITERATIONS 32
+#define MAX_CONVEX_CAST_EPSILON btScalar(0.0001)
+#endif
+///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
+///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
+//will need to digg deeper to make the algorithm more robust
+//since, a large epsilon can cause an early termination with false
+//positive results (ray intersections that shouldn't be there)
+
 /// btConvexCast is an interface for Casting
 class btConvexCast
 {
 public:
-
-
 	virtual ~btConvexCast();
 
 	///RayResult stores the closest result
 	/// alternatively, add a callback method to decide about closest/all results
-	struct	CastResult
+	struct CastResult
 	{
 		//virtual bool	addRayResult(const btVector3& normal,btScalar	fraction) = 0;
-				
-		virtual void	DebugDraw(btScalar	fraction) {(void)fraction;}
-		virtual void	drawCoordSystem(const btTransform& trans) {(void)trans;}
-		virtual void	reportFailure(int errNo, int numIterations) {(void)errNo;(void)numIterations;}
+
+		virtual void DebugDraw(btScalar fraction) { (void)fraction; }
+		virtual void drawCoordSystem(const btTransform& trans) { (void)trans; }
+		virtual void reportFailure(int errNo, int numIterations)
+		{
+			(void)errNo;
+			(void)numIterations;
+		}
 		CastResult()
-			:m_fraction(btScalar(BT_LARGE_FLOAT)),
-			m_debugDrawer(0),
-			m_allowedPenetration(btScalar(0))
+			: m_fraction(btScalar(BT_LARGE_FLOAT)),
+			  m_debugDrawer(0),
+			  m_allowedPenetration(btScalar(0)),
+			  m_subSimplexCastMaxIterations(MAX_CONVEX_CAST_ITERATIONS),
+			  m_subSimplexCastEpsilon(MAX_CONVEX_CAST_EPSILON)
 		{
 		}
 
+		virtual ~CastResult(){};
 
-		virtual ~CastResult() {};
-
-		btTransform	m_hitTransformA;
-		btTransform	m_hitTransformB;
-		btVector3	m_normal;
-		btVector3   m_hitPoint;
-		btScalar	m_fraction; //input and output
+		btTransform m_hitTransformA;
+		btTransform m_hitTransformB;
+		btVector3 m_normal;
+		btVector3 m_hitPoint;
+		btScalar m_fraction;  //input and output
 		btIDebugDraw* m_debugDrawer;
-		btScalar	m_allowedPenetration;
+		btScalar m_allowedPenetration;
+		
+		int m_subSimplexCastMaxIterations;
+		btScalar m_subSimplexCastEpsilon;
 
 	};
 
-
 	/// cast a convex against another convex object
-	virtual bool	calcTimeOfImpact(
-					const btTransform& fromA,
-					const btTransform& toA,
-					const btTransform& fromB,
-					const btTransform& toB,
-					CastResult& result) = 0;
+	virtual bool calcTimeOfImpact(
+		const btTransform& fromA,
+		const btTransform& toA,
+		const btTransform& fromB,
+		const btTransform& toB,
+		CastResult& result) = 0;
 };
 
-#endif //BT_CONVEX_CAST_H
+#endif  //BT_CONVEX_CAST_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
index 29620abffb9..65c9df9340b 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_CONVEX_PENETRATION_DEPTH_H
 #define BT_CONVEX_PENETRATION_DEPTH_H
 
@@ -25,16 +24,12 @@ class btTransform;
 ///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
 class btConvexPenetrationDepthSolver
 {
-public:	
-	
-	virtual ~btConvexPenetrationDepthSolver() {};
-	virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
-		const btConvexShape* convexA,const btConvexShape* convexB,
-					const btTransform& transA,const btTransform& transB,
-				btVector3& v, btVector3& pa, btVector3& pb,
-				class btIDebugDraw* debugDraw) = 0;
-
-
+public:
+	virtual ~btConvexPenetrationDepthSolver(){};
+	virtual bool calcPenDepth(btSimplexSolverInterface& simplexSolver,
+							  const btConvexShape* convexA, const btConvexShape* convexB,
+							  const btTransform& transA, const btTransform& transB,
+							  btVector3& v, btVector3& pa, btVector3& pb,
+							  class btIDebugDraw* debugDraw) = 0;
 };
-#endif //BT_CONVEX_PENETRATION_DEPTH_H
-
+#endif  //BT_CONVEX_PENETRATION_DEPTH_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
index 46ce1ab75e7..d1bbb1a46ee 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
 #define BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
 
@@ -27,62 +26,60 @@ subject to the following restrictions:
 /// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB
 struct btDiscreteCollisionDetectorInterface
 {
-	
 	struct Result
 	{
-	
-		virtual ~Result(){}	
+		virtual ~Result() {}
 
 		///setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material combiner
-		virtual void setShapeIdentifiersA(int partId0,int index0)=0;
-		virtual void setShapeIdentifiersB(int partId1,int index1)=0;
-		virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)=0;
+		virtual void setShapeIdentifiersA(int partId0, int index0) = 0;
+		virtual void setShapeIdentifiersB(int partId1, int index1) = 0;
+		virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) = 0;
 	};
 
 	struct ClosestPointInput
 	{
 		ClosestPointInput()
-			:m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT))
+			: m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT))
 		{
 		}
 
 		btTransform m_transformA;
 		btTransform m_transformB;
-		btScalar	m_maximumDistanceSquared;
+		btScalar m_maximumDistanceSquared;
 	};
 
-	virtual ~btDiscreteCollisionDetectorInterface() {};
+	virtual ~btDiscreteCollisionDetectorInterface(){};
 
 	//
 	// give either closest points (distance > 0) or penetration (distance)
 	// the normal always points from B towards A
 	//
-	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false) = 0;
-
+	virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false) = 0;
 };
 
 struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result
 {
-		btVector3	m_normalOnSurfaceB;
-		btVector3	m_closestPointInB;
-		btScalar	m_distance; //negative means penetration !
+	btVector3 m_normalOnSurfaceB;
+	btVector3 m_closestPointInB;
+	btScalar m_distance;  //negative means penetration !
 
-		btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT))
-		{
+protected:
+	btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT))
+	{
+	}
 
-		}
-		virtual ~btStorageResult() {};
+public:
+	virtual ~btStorageResult(){};
 
-		virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+	virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
+	{
+		if (depth < m_distance)
 		{
-			if (depth < m_distance)
-			{
-				m_normalOnSurfaceB = normalOnBInWorld;
-				m_closestPointInB = pointInWorld;
-				m_distance = depth;
-			}
+			m_normalOnSurfaceB = normalOnBInWorld;
+			m_closestPointInB = pointInWorld;
+			m_distance = depth;
 		}
+	}
 };
 
-#endif //BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
-
+#endif  //BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h
index 0b49b0ecc65..c9fd84bebfa 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef GJK_COLLISION_DESCRIPTION_H
 #define GJK_COLLISION_DESCRIPTION_H
 
@@ -21,21 +20,20 @@ subject to the following restrictions:
 
 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()
-    {
-    }
+	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
-
+#endif  //GJK_COLLISION_DESCRIPTION_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp
index bef697a0a11..9d61e75dac8 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp
@@ -13,8 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #include "btGjkConvexCast.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
 #include "btGjkPairDetector.h"
@@ -27,41 +25,39 @@ subject to the following restrictions:
 #define MAX_ITERATIONS 32
 #endif
 
-btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
-:m_simplexSolver(simplexSolver),
-m_convexA(convexA),
-m_convexB(convexB)
+btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver)
+	: m_simplexSolver(simplexSolver),
+	  m_convexA(convexA),
+	  m_convexB(convexB)
 {
 }
 
-bool	btGjkConvexCast::calcTimeOfImpact(
-					const btTransform& fromA,
-					const btTransform& toA,
-					const btTransform& fromB,
-					const btTransform& toB,
-					CastResult& result)
+bool btGjkConvexCast::calcTimeOfImpact(
+	const btTransform& fromA,
+	const btTransform& toA,
+	const btTransform& fromB,
+	const btTransform& toB,
+	CastResult& result)
 {
-
-
 	m_simplexSolver->reset();
 
 	/// compute linear velocity for this interval, to interpolate
 	//assume no rotation/angular velocity, assert here?
-	btVector3 linVelA,linVelB;
-	linVelA = toA.getOrigin()-fromA.getOrigin();
-	linVelB = toB.getOrigin()-fromB.getOrigin();
+	btVector3 linVelA, linVelB;
+	linVelA = toA.getOrigin() - fromA.getOrigin();
+	linVelB = toB.getOrigin() - fromB.getOrigin();
 
 	btScalar radius = btScalar(0.001);
 	btScalar lambda = btScalar(0.);
-	btVector3 v(1,0,0);
+	btVector3 v(1, 0, 0);
 
 	int maxIter = MAX_ITERATIONS;
 
 	btVector3 n;
-	n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+	n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 	bool hasResult = false;
 	btVector3 c;
-	btVector3 r = (linVelA-linVelB);
+	btVector3 r = (linVelA - linVelB);
 
 	btScalar lastLambda = lambda;
 	//btScalar epsilon = btScalar(0.001);
@@ -69,17 +65,14 @@ bool	btGjkConvexCast::calcTimeOfImpact(
 	int numIter = 0;
 	//first solution, using GJK
 
-
 	btTransform identityTrans;
 	identityTrans.setIdentity();
 
+	//	result.drawCoordSystem(sphereTr);
 
-//	result.drawCoordSystem(sphereTr);
-
-	btPointCollector	pointCollector;
+	btPointCollector pointCollector;
 
-		
-	btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver);		
+	btGjkPairDetector gjk(m_convexA, m_convexB, m_simplexSolver, 0);  //m_penetrationDepthSolver);
 	btGjkPairDetector::ClosestPointInput input;
 
 	//we don't use margins during CCD
@@ -87,7 +80,7 @@ bool	btGjkConvexCast::calcTimeOfImpact(
 
 	input.m_transformA = fromA;
 	input.m_transformB = fromB;
-	gjk.getClosestPoints(input,pointCollector,0);
+	gjk.getClosestPoints(input, pointCollector, 0);
 
 	hasResult = pointCollector.m_hasResult;
 	c = pointCollector.m_pointInWorld;
@@ -98,20 +91,18 @@ bool	btGjkConvexCast::calcTimeOfImpact(
 		dist = pointCollector.m_distance;
 		n = pointCollector.m_normalOnBInWorld;
 
-	
-
 		//not close enough
 		while (dist > radius)
 		{
 			numIter++;
 			if (numIter > maxIter)
 			{
-				return false; //todo: report a failure
+				return false;  //todo: report a failure
 			}
 			btScalar dLambda = btScalar(0.);
 
 			btScalar projectedLinearVelocity = r.dot(n);
-			
+
 			dLambda = dist / (projectedLinearVelocity);
 
 			lambda = lambda - dLambda;
@@ -132,35 +123,35 @@ bool	btGjkConvexCast::calcTimeOfImpact(
 			lastLambda = lambda;
 
 			//interpolate to next lambda
-			result.DebugDraw( lambda );
-			input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
-			input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
-			
-			gjk.getClosestPoints(input,pointCollector,0);
+			result.DebugDraw(lambda);
+			input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(), toA.getOrigin(), lambda);
+			input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(), toB.getOrigin(), lambda);
+
+			gjk.getClosestPoints(input, pointCollector, 0);
 			if (pointCollector.m_hasResult)
 			{
 				if (pointCollector.m_distance < btScalar(0.))
 				{
 					result.m_fraction = lastLambda;
 					n = pointCollector.m_normalOnBInWorld;
-					result.m_normal=n;
+					result.m_normal = n;
 					result.m_hitPoint = pointCollector.m_pointInWorld;
 					return true;
 				}
-				c = pointCollector.m_pointInWorld;		
+				c = pointCollector.m_pointInWorld;
 				n = pointCollector.m_normalOnBInWorld;
 				dist = pointCollector.m_distance;
-			} else
+			}
+			else
 			{
 				//??
 				return false;
 			}
-
 		}
 
 		//is n normalized?
 		//don't report time of impact for motion away from the contact normal (or causes minor penetration)
-		if (n.dot(r)>=-result.m_allowedPenetration)
+		if (n.dot(r) >= -result.m_allowedPenetration)
 			return false;
 
 		result.m_fraction = lambda;
@@ -170,7 +161,4 @@ bool	btGjkConvexCast::calcTimeOfImpact(
 	}
 
 	return false;
-
-
 }
-
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
index 6a42ee63b03..ef5979173e9 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
@@ -13,8 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef BT_GJK_CONVEX_CAST_H
 #define BT_GJK_CONVEX_CAST_H
 
@@ -29,22 +27,20 @@ class btMinkowskiSumShape;
 ///GjkConvexCast performs a raycast on a convex object using support mapping.
 class btGjkConvexCast : public btConvexCast
 {
-	btSimplexSolverInterface*	m_simplexSolver;
-	const btConvexShape*	m_convexA;
-	const btConvexShape*	m_convexB;
+	btSimplexSolverInterface* m_simplexSolver;
+	const btConvexShape* m_convexA;
+	const btConvexShape* m_convexB;
 
 public:
-
-	btGjkConvexCast(const btConvexShape*	convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver);
+	btGjkConvexCast(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver);
 
 	/// cast a convex against another convex object
-	virtual bool	calcTimeOfImpact(
-					const btTransform& fromA,
-					const btTransform& toA,
-					const btTransform& fromB,
-					const btTransform& toB,
-					CastResult& result);
-
+	virtual bool calcTimeOfImpact(
+		const btTransform& fromA,
+		const btTransform& toA,
+		const btTransform& fromB,
+		const btTransform& toB,
+		CastResult& result);
 };
 
-#endif //BT_GJK_CONVEX_CAST_H
+#endif  //BT_GJK_CONVEX_CAST_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
index 3268f06c2f9..7d53f8624a8 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
@@ -26,996 +26,1069 @@ GJK-EPA collision solver by Nathanael Presson, 2008
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
 #include "btGjkEpa2.h"
 
-#if defined(DEBUG) || defined (_DEBUG)
-#include <stdio.h> //for debug printf
+#if defined(DEBUG) || defined(_DEBUG)
+#include <stdio.h>  //for debug printf
 #ifdef __SPU__
 #include <spu_printf.h>
 #define printf spu_printf
-#endif //__SPU__
+#endif  //__SPU__
 #endif
 
 namespace gjkepa2_impl
 {
+// Config
 
-	// Config
+/* GJK	*/
+#define GJK_MAX_ITERATIONS 128
 
-	/* 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)
+#ifdef BT_USE_DOUBLE_PRECISION
+#define GJK_ACCURACY ((btScalar)1e-12)
+#define GJK_MIN_DISTANCE ((btScalar)1e-12)
+#define GJK_DUPLICATED_EPS ((btScalar)1e-12)
+#else
+#define GJK_ACCURACY ((btScalar)0.0001)
+#define GJK_MIN_DISTANCE ((btScalar)0.0001)
+#define GJK_DUPLICATED_EPS ((btScalar)0.0001)
+#endif  //BT_USE_DOUBLE_PRECISION
 
+#define GJK_SIMPLEX2_EPS ((btScalar)0.0)
+#define GJK_SIMPLEX3_EPS ((btScalar)0.0)
+#define GJK_SIMPLEX4_EPS ((btScalar)0.0)
 
-	// Shorthands
-	typedef unsigned int	U;
-	typedef unsigned char	U1;
+/* EPA	*/
+#define EPA_MAX_VERTICES 128
+#define EPA_MAX_ITERATIONS 255
 
-	// MinkowskiDiff
-	struct	MinkowskiDiff
-	{
-		const btConvexShape*	m_shapes[2];
-		btMatrix3x3				m_toshape1;
-		btTransform				m_toshape0;
-#ifdef __SPU__
-		bool					m_enableMargin;
+#ifdef BT_USE_DOUBLE_PRECISION
+#define EPA_ACCURACY ((btScalar)1e-12)
+#define EPA_PLANE_EPS ((btScalar)1e-14)
+#define EPA_INSIDE_EPS ((btScalar)1e-9)
 #else
-		btVector3				(btConvexShape::*Ls)(const btVector3&) const;
-#endif//__SPU__
-		
+#define EPA_ACCURACY ((btScalar)0.0001)
+#define EPA_PLANE_EPS ((btScalar)0.00001)
+#define EPA_INSIDE_EPS ((btScalar)0.01)
+#endif
 
-		MinkowskiDiff()
-		{
+#define EPA_FALLBACK (10 * EPA_ACCURACY)
+#define EPA_MAX_FACES (EPA_MAX_VERTICES * 2)
 
-		}
+// Shorthands
+typedef unsigned int U;
+typedef unsigned char U1;
+
+// MinkowskiDiff
+struct MinkowskiDiff
+{
+	const btConvexShape* m_shapes[2];
+	btMatrix3x3 m_toshape1;
+	btTransform m_toshape0;
 #ifdef __SPU__
-			void					EnableMargin(bool enable)
-		{
-			m_enableMargin = enable;
-		}	
-		inline btVector3		Support0(const btVector3& d) const
-		{
-			if (m_enableMargin)
-			{
-				return m_shapes[0]->localGetSupportVertexNonVirtual(d);
-			} else
-			{
-				return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d);
-			}
-		}
-		inline btVector3		Support1(const btVector3& d) const
-		{
-			if (m_enableMargin)
-			{
-				return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d));
-			} else
-			{
-				return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d));
-			}
-		}
+	bool m_enableMargin;
 #else
-		void					EnableMargin(bool enable)
-		{
-			if(enable)
-				Ls=&btConvexShape::localGetSupportVertexNonVirtual;
-			else
-				Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
-		}	
-		inline btVector3		Support0(const btVector3& d) const
+	btVector3 (btConvexShape::*Ls)(const btVector3&) const;
+#endif  //__SPU__
+
+	MinkowskiDiff()
+	{
+	}
+#ifdef __SPU__
+	void EnableMargin(bool enable)
+	{
+		m_enableMargin = enable;
+	}
+	inline btVector3 Support0(const btVector3& d) const
+	{
+		if (m_enableMargin)
 		{
-			return(((m_shapes[0])->*(Ls))(d));
+			return m_shapes[0]->localGetSupportVertexNonVirtual(d);
 		}
-		inline btVector3		Support1(const btVector3& d) const
+		else
 		{
-			return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
+			return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d);
 		}
-#endif //__SPU__
-
-		inline btVector3		Support(const btVector3& d) const
+	}
+	inline btVector3 Support1(const btVector3& d) const
+	{
+		if (m_enableMargin)
 		{
-			return(Support0(d)-Support1(-d));
+			return m_toshape0 * (m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1 * d));
 		}
-		btVector3				Support(const btVector3& d,U index) const
+		else
 		{
-			if(index)
-				return(Support1(d));
-			else
-				return(Support0(d));
+			return m_toshape0 * (m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1 * d));
 		}
-	};
+	}
+#else
+	void EnableMargin(bool enable)
+	{
+		if (enable)
+			Ls = &btConvexShape::localGetSupportVertexNonVirtual;
+		else
+			Ls = &btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
+	}
+	inline btVector3 Support0(const btVector3& d) const
+	{
+		return (((m_shapes[0])->*(Ls))(d));
+	}
+	inline btVector3 Support1(const btVector3& d) const
+	{
+		return (m_toshape0 * ((m_shapes[1])->*(Ls))(m_toshape1 * d));
+	}
+#endif  //__SPU__
 
-	typedef	MinkowskiDiff	tShape;
+	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));
+	}
+};
 
+typedef MinkowskiDiff tShape;
 
-	// GJK
-	struct	GJK
+// GJK
+struct GJK
+{
+	/* Types		*/
+	struct sSV
 	{
-		/* Types		*/ 
-		struct	sSV
-		{
-			btVector3	d,w;
-		};
-		struct	sSimplex
+		btVector3 d, w;
+	};
+	struct sSimplex
+	{
+		sSV* c[4];
+		btScalar p[4];
+		U rank;
+	};
+	struct eStatus
+	{
+		enum _
 		{
-			sSV*		c[4];
-			btScalar	p[4];
-			U			rank;
-		};
-		struct	eStatus	{ enum _ {
 			Valid,
 			Inside,
-			Failed		};};
-			/* Fields		*/ 
-			tShape			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;
-			eStatus::_		m_status;
-			/* Methods		*/ 
-			GJK()
-			{
-				Initialize();
+			Failed
+		};
+	};
+	/* Fields		*/
+	tShape 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;
+	eStatus::_ m_status;
+	/* Methods		*/
+	GJK()
+	{
+		Initialize();
+	}
+	void Initialize()
+	{
+		m_ray = btVector3(0, 0, 0);
+		m_nfree = 0;
+		m_status = eStatus::Failed;
+		m_current = 0;
+		m_distance = 0;
+	}
+	eStatus::_ Evaluate(const tShape& 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 = eStatus::Valid;
+		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 = eStatus::Inside;
+				break;
 			}
-			void				Initialize()
+			/* 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)
 			{
-				m_ray		=	btVector3(0,0,0);
-				m_nfree		=	0;
-				m_status	=	eStatus::Failed;
-				m_current	=	0;
-				m_distance	=	0;
+				if ((w - lastw[i]).length2() < GJK_DUPLICATED_EPS)
+				{
+					found = true;
+					break;
+				}
 			}
-			eStatus::_			Evaluate(const tShape& shapearg,const btVector3& guess)
+			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_ACCURACY * rl)) <= 0)
+			{ /* Return old simplex				*/
+				removevertice(m_simplices[m_current]);
+				break;
+			}
+			/* Reduce simplex						*/
+			btScalar weights[4];
+			U mask = 0;
+			switch (cs.rank)
 			{
-				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			=	eStatus::Valid;
-				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=eStatus::Inside;
-						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)
+				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))
 					{
-						if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
-						{ found=true;break; }
-					}
-					if(found)
-					{/* Return old simplex				*/ 
-						removevertice(m_simplices[m_current]);
-						break;
+						ns.c[ns.rank] = cs.c[i];
+						ns.p[ns.rank++] = weights[i];
+						m_ray += cs.c[i]->w * weights[i];
 					}
 					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=eStatus::Inside;
-					}
-					else
-					{/* Return old simplex				*/ 
-						removevertice(m_simplices[m_current]);
-						break;
+						m_free[m_nfree++] = cs.c[i];
 					}
-					m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
-				} while(m_status==eStatus::Valid);
-				m_simplex=&m_simplices[m_current];
-				switch(m_status)
+				}
+				if (mask == 15) m_status = eStatus::Inside;
+			}
+			else
+			{ /* Return old simplex				*/
+				removevertice(m_simplices[m_current]);
+				break;
+			}
+			m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eStatus::Failed;
+		} while (m_status == eStatus::Valid);
+		m_simplex = &m_simplices[m_current];
+		switch (m_status)
+		{
+			case eStatus::Valid:
+				m_distance = m_ray.length();
+				break;
+			case eStatus::Inside:
+				m_distance = 0;
+				break;
+			default:
+			{
+			}
+		}
+		return (m_status);
+	}
+	bool EncloseOrigin()
+	{
+		switch (m_simplex->rank)
+		{
+			case 1:
+			{
+				for (U i = 0; i < 3; ++i)
 				{
-				case	eStatus::Valid:		m_distance=m_ray.length();break;
-				case	eStatus::Inside:	m_distance=0;break;
-				default:
-					{
-					}
-				}	
-				return(m_status);
+					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);
+				}
 			}
-			bool					EncloseOrigin()
+			break;
+			case 2:
 			{
-				switch(m_simplex->rank)
+				const btVector3 d = m_simplex->c[1]->w - m_simplex->c[0]->w;
+				for (U i = 0; i < 3; ++i)
 				{
-				case	1:
+					btVector3 axis = btVector3(0, 0, 0);
+					axis[i] = 1;
+					const btVector3 p = btCross(d, axis);
+					if (p.length2() > 0)
 					{
-						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);
-							}
-						}
+						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
+			break;
+			case 3:
 			{
-				sv.d	=	d/d.length();
-				sv.w	=	m_shape.Support(sv.d);
+				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);
+				}
 			}
-			void				removevertice(sSimplex& simplex)
+			break;
+			case 4:
 			{
-				m_free[m_nfree++]=simplex.c[--simplex.rank];
+				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);
 			}
-			void				appendvertice(sSimplex& simplex,const btVector3& v)
+			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)
 			{
-				simplex.p[simplex.rank]=0;
-				simplex.c[simplex.rank]=m_free[--m_nfree];
-				getsupport(v,*simplex.c[simplex.rank++]);
+				w[0] = 0;
+				w[1] = 1;
+				m = 2;
+				return (b.length2());
 			}
-			static btScalar		det(const btVector3& a,const btVector3& b,const btVector3& c)
+			else if (t <= 0)
 			{
-				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());
+				w[0] = 1;
+				w[1] = 0;
+				m = 1;
+				return (a.length2());
 			}
-			static btScalar		projectorigin(	const btVector3& a,
-				const btVector3& b,
-				btScalar* w,U& m)
+			else
 			{
-				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);
+				w[0] = 1 - (w[1] = t);
+				m = 3;
+				return ((a + d * t).length2());
 			}
-			static btScalar		projectorigin(	const btVector3& a,
-				const btVector3& b,
-				const btVector3& c,
-				btScalar* w,U& m)
+		}
+		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)
 			{
-				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)
+				if (btDot(*vt[i], btCross(dl[i], n)) > 0)
 				{
-					btScalar	mindist=-1;
-					btScalar	subw[2]={0.f,0.f};
-					U			subm(0);
-					for(U i=0;i<3;++i)
+					const U j = imd3[i];
+					const btScalar subd(projectorigin(*vt[i], *vt[j], subw, subm));
+					if ((mindist < 0) || (subd < mindist))
 					{
-						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;				
-							}
-						}
+						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)
+			if (mindist < 0)
 			{
-				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))
+				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)
 				{
-					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)
+					const btScalar subd = projectorigin(*vt[i], *vt[j], d, subw, subm);
+					if ((mindist < 0) || (subd < mindist))
 					{
-						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]);
+						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];
 					}
-					return(mindist);
 				}
-				return(-1);
 			}
-	};
+			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);
+	}
+};
 
-	// EPA
-	struct	EPA
+// EPA
+struct EPA
+{
+	/* Types		*/
+	typedef GJK::sSV sSV;
+	struct sFace
 	{
-		/* Types		*/ 
-		typedef	GJK::sSV	sSV;
-		struct	sFace
-		{
-			btVector3	n;
-			btScalar	d;
-			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
+		btVector3 n;
+		btScalar d;
+		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) {}
+	};
+	struct eStatus
+	{
+		enum _
 		{
-			sFace*		cf;
-			sFace*		ff;
-			U			nf;
-			sHorizon() : cf(0),ff(0),nf(0)	{}
-		};
-		struct	eStatus { enum _ {
 			Valid,
 			Touching,
 			Degenerated,
 			NonConvex,
-			InvalidHull,		
+			InvalidHull,
 			OutOfFaces,
 			OutOfVertices,
 			AccuraryReached,
 			FallBack,
-			Failed		};};
-			/* Fields		*/ 
-			eStatus::_		m_status;
-			GJK::sSimplex	m_result;
-			btVector3		m_normal;
-			btScalar		m_depth;
-			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();	
-			}
+			Failed
+		};
+	};
+	/* Fields		*/
+	eStatus::_ m_status;
+	GJK::sSimplex m_result;
+	btVector3 m_normal;
+	btScalar m_depth;
+	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;
+	}
 
-			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)
+	void Initialize()
+	{
+		m_status = eStatus::Failed;
+		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]);
+		}
+	}
+	eStatus::_ Evaluate(GJK& gjk, const btVector3& guess)
+	{
+		GJK::sSimplex& simplex = *gjk.m_simplex;
+		if ((simplex.rank > 1) && gjk.EncloseOrigin())
+		{
+			/* Clean up				*/
+			while (m_hull.root)
 			{
-				face->l[0]	=	0;
-				face->l[1]	=	list.root;
-				if(list.root) list.root->l[0]=face;
-				list.root	=	face;
-				++list.count;
+				sFace* f = m_hull.root;
+				remove(m_hull, f);
+				append(m_stock, f);
 			}
-			static inline void		remove(sList& list,sFace* face)
+			m_status = eStatus::Valid;
+			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)
 			{
-				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;
+				btSwap(simplex.c[0], simplex.c[1]);
+				btSwap(simplex.p[0], simplex.p[1]);
 			}
-
-
-			void				Initialize()
+			/* 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)
 			{
-				m_status	=	eStatus::Failed;
-				m_normal	=	btVector3(0,0,0);
-				m_depth		=	0;
-				m_nextsv	=	0;
-				for(U i=0;i<EPA_MAX_FACES;++i)
+				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 = eStatus::Valid;
+				for (; iterations < EPA_MAX_ITERATIONS; ++iterations)
 				{
-					append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
-				}
-			}
-			eStatus::_			Evaluate(GJK& gjk,const btVector3& guess)
-			{
-				GJK::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	=	eStatus::Valid;
-					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)
+					if (m_nextsv < EPA_MAX_VERTICES)
 					{
-						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=eStatus::Valid;
-						for(;iterations<EPA_MAX_ITERATIONS;++iterations)
+						sHorizon horizon;
+						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)
 						{
-							if(m_nextsv<EPA_MAX_VERTICES)
-							{	
-								sHorizon		horizon;
-								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=eStatus::InvalidHull;break; }
-								} else { m_status=eStatus::AccuraryReached;break; }
-							} else { m_status=eStatus::OutOfVertices;break; }
+							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 = eStatus::InvalidHull;
+								break;
+							}
+						}
+						else
+						{
+							m_status = eStatus::AccuraryReached;
+							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	=	eStatus::FallBack;
-				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, sSV* a, 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));
+						m_status = eStatus::OutOfVertices;
+						break;
 					}
-
-					return true;
 				}
+				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 = eStatus::FallBack;
+		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, sSV* a, 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
 
-				return false;
+		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();
 			}
-			sFace*				newface(sSV* a,sSV* b,sSV* c,bool forced)
+			else if (b_dot_ba < 0)
 			{
-				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;
+				// 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));
+			}
 
-					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;
-						}
+			return true;
+		}
 
-						face->n /= l;
-						if(forced || (face->d >= -EPA_PLANE_EPS))
-						{
-							return face;
-						}
-						else
-							m_status=eStatus::NonConvex;
-					}
-					else
-						m_status=eStatus::Degenerated;
+		return false;
+	}
+	sFace* newface(sSV* a, sSV* b, 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;
 
-					remove(m_hull, face);
-					append(m_stock, face);
-					return 0;
+			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;
 				}
-				m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
-				return 0;
+				else
+					m_status = eStatus::NonConvex;
+			}
+			else
+				m_status = eStatus::Degenerated;
+
+			remove(m_hull, face);
+			append(m_stock, face);
+			return 0;
+		}
+		m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
+		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;
 			}
-			sFace*				findbest()
+		}
+		return (minf);
+	}
+	bool expand(U pass, 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*		minf=m_hull.root;
-				btScalar	mind=minf->d*minf->d;
-				for(sFace* f=minf->l[1];f;f=f->l[1])
+				sFace* nf = newface(f->c[e1], f->c[e], w, false);
+				if (nf)
 				{
-					const btScalar	sqd=f->d*f->d;
-					if(sqd<mind)
-					{
-						minf=f;
-						mind=sqd;
-					}
+					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);
 				}
-				return(minf);
 			}
-			bool				expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
+			else
 			{
-				static const U	i1m3[]={1,2,0};
-				static const U	i2m3[]={2,0,1};
-				if(f->pass!=pass)
+				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))
 				{
-					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);
-						}
-					}
+					remove(m_hull, f);
+					append(m_stock, f);
+					return (true);
 				}
-				return(false);
 			}
-
-	};
-
-	//
-	static void	Initialize(	const btConvexShape* shape0,const btTransform& wtrs0,
-		const btConvexShape* shape1,const btTransform& wtrs1,
-		btGjkEpaSolver2::sResults& results,
-		tShape& shape,
-		bool withmargins)
-	{
-		/* Results		*/ 
-		results.witnesses[0]	=
-			results.witnesses[1]	=	btVector3(0,0,0);
-		results.status			=	btGjkEpaSolver2::sResults::Separated;
-		/* Shape		*/ 
-		shape.m_shapes[0]		=	shape0;
-		shape.m_shapes[1]		=	shape1;
-		shape.m_toshape1		=	wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
-		shape.m_toshape0		=	wtrs0.inverseTimes(wtrs1);
-		shape.EnableMargin(withmargins);
+		}
+		return (false);
 	}
+};
 
+//
+static void Initialize(const btConvexShape* shape0, const btTransform& wtrs0,
+					   const btConvexShape* shape1, const btTransform& wtrs1,
+					   btGjkEpaSolver2::sResults& results,
+					   tShape& shape,
+					   bool withmargins)
+{
+	/* Results		*/
+	results.witnesses[0] =
+		results.witnesses[1] = btVector3(0, 0, 0);
+	results.status = btGjkEpaSolver2::sResults::Separated;
+	/* Shape		*/
+	shape.m_shapes[0] = shape0;
+	shape.m_shapes[1] = shape1;
+	shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
+	shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);
+	shape.EnableMargin(withmargins);
 }
 
+}  // namespace gjkepa2_impl
+
 //
 // Api
 //
 
-using namespace	gjkepa2_impl;
+using namespace gjkepa2_impl;
 
 //
-int			btGjkEpaSolver2::StackSizeRequirement()
+int btGjkEpaSolver2::StackSizeRequirement()
 {
-	return(sizeof(GJK)+sizeof(EPA));
+	return (sizeof(GJK) + sizeof(EPA));
 }
 
 //
-bool		btGjkEpaSolver2::Distance(	const btConvexShape*	shape0,
-									  const btTransform&		wtrs0,
-									  const btConvexShape*	shape1,
-									  const btTransform&		wtrs1,
-									  const btVector3&		guess,
-									  sResults&				results)
+bool btGjkEpaSolver2::Distance(const btConvexShape* shape0,
+							   const btTransform& wtrs0,
+							   const btConvexShape* shape1,
+							   const btTransform& wtrs1,
+							   const btVector3& guess,
+							   sResults& results)
 {
-	tShape			shape;
-	Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
-	GJK				gjk;
-	GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,guess);
-	if(gjk_status==GJK::eStatus::Valid)
+	tShape shape;
+	Initialize(shape0, wtrs0, shape1, wtrs1, results, shape, false);
+	GJK gjk;
+	GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, guess);
+	if (gjk_status == GJK::eStatus::Valid)
 	{
-		btVector3	w0=btVector3(0,0,0);
-		btVector3	w1=btVector3(0,0,0);
-		for(U i=0;i<gjk.m_simplex->rank;++i)
+		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;
+			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]	=	wtrs0*w0;
-		results.witnesses[1]	=	wtrs0*w1;
-		results.normal			=	w0-w1;
-		results.distance		=	results.normal.length();
-		results.normal			/=	results.distance>GJK_MIN_DISTANCE?results.distance:1;
-		return(true);
+		results.witnesses[0] = wtrs0 * w0;
+		results.witnesses[1] = wtrs0 * 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==GJK::eStatus::Inside?
-			sResults::Penetrating	:
-		sResults::GJK_Failed	;
-		return(false);
+		results.status = gjk_status == GJK::eStatus::Inside ? sResults::Penetrating : sResults::GJK_Failed;
+		return (false);
 	}
 }
 
 //
-bool	btGjkEpaSolver2::Penetration(	const btConvexShape*	shape0,
-									 const btTransform&		wtrs0,
-									 const btConvexShape*	shape1,
-									 const btTransform&		wtrs1,
-									 const btVector3&		guess,
-									 sResults&				results,
-									 bool					usemargins)
+bool btGjkEpaSolver2::Penetration(const btConvexShape* shape0,
+								  const btTransform& wtrs0,
+								  const btConvexShape* shape1,
+								  const btTransform& wtrs1,
+								  const btVector3& guess,
+								  sResults& results,
+								  bool usemargins)
 {
-	tShape			shape;
-	Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
-	GJK				gjk;	
-	GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,-guess);
-	switch(gjk_status)
+	tShape shape;
+	Initialize(shape0, wtrs0, shape1, wtrs1, results, shape, usemargins);
+	GJK gjk;
+	GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, -guess);
+	switch (gjk_status)
 	{
-	case	GJK::eStatus::Inside:
+		case GJK::eStatus::Inside:
 		{
-			EPA				epa;
-			EPA::eStatus::_	epa_status=epa.Evaluate(gjk,-guess);
-			if(epa_status!=EPA::eStatus::Failed)
+			EPA epa;
+			EPA::eStatus::_ epa_status = epa.Evaluate(gjk, -guess);
+			if (epa_status != EPA::eStatus::Failed)
 			{
-				btVector3	w0=btVector3(0,0,0);
-				for(U i=0;i<epa.m_result.rank;++i)
+				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];
+					w0 += shape.Support(epa.m_result.c[i]->d, 0) * epa.m_result.p[i];
 				}
-				results.status			=	sResults::Penetrating;
-				results.witnesses[0]	=	wtrs0*w0;
-				results.witnesses[1]	=	wtrs0*(w0-epa.m_normal*epa.m_depth);
-				results.normal			=	-epa.m_normal;
-				results.distance		=	-epa.m_depth;
-				return(true);
-			} else results.status=sResults::EPA_Failed;
+				results.status = sResults::Penetrating;
+				results.witnesses[0] = wtrs0 * w0;
+				results.witnesses[1] = wtrs0 * (w0 - epa.m_normal * epa.m_depth);
+				results.normal = -epa.m_normal;
+				results.distance = -epa.m_depth;
+				return (true);
+			}
+			else
+				results.status = sResults::EPA_Failed;
 		}
 		break;
-	case	GJK::eStatus::Failed:
-		results.status=sResults::GJK_Failed;
-		break;
+		case GJK::eStatus::Failed:
+			results.status = sResults::GJK_Failed;
+			break;
 		default:
-					{
-					}
+		{
+		}
 	}
-	return(false);
+	return (false);
 }
 
 #ifndef __SPU__
 //
-btScalar	btGjkEpaSolver2::SignedDistance(const btVector3& position,
-											btScalar margin,
-											const btConvexShape* shape0,
-											const btTransform& wtrs0,
-											sResults& results)
+btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position,
+										 btScalar margin,
+										 const btConvexShape* shape0,
+										 const btTransform& wtrs0,
+										 sResults& results)
 {
-	tShape			shape;
-	btSphereShape	shape1(margin);
-	btTransform		wtrs1(btQuaternion(0,0,0,1),position);
-	Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
-	GJK				gjk;	
-	GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
-	if(gjk_status==GJK::eStatus::Valid)
+	tShape shape;
+	btSphereShape shape1(margin);
+	btTransform wtrs1(btQuaternion(0, 0, 0, 1), position);
+	Initialize(shape0, wtrs0, &shape1, wtrs1, results, shape, false);
+	GJK gjk;
+	GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, btVector3(1, 1, 1));
+	if (gjk_status == GJK::eStatus::Valid)
 	{
-		btVector3	w0=btVector3(0,0,0);
-		btVector3	w1=btVector3(0,0,0);
-		for(U i=0;i<gjk.m_simplex->rank;++i)
+		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;
+			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]	=	wtrs0*w0;
-		results.witnesses[1]	=	wtrs0*w1;
-		const btVector3	delta=	results.witnesses[1]-
-			results.witnesses[0];
-		const btScalar	margin=	shape0->getMarginNonVirtual()+
-			shape1.getMarginNonVirtual();
-		const btScalar	length=	delta.length();	
-		results.normal			=	delta/length;
-		results.witnesses[0]	+=	results.normal*margin;
-		return(length-margin);
+		results.witnesses[0] = wtrs0 * w0;
+		results.witnesses[1] = wtrs0 * w1;
+		const btVector3 delta = results.witnesses[1] -
+								results.witnesses[0];
+		const btScalar margin = shape0->getMarginNonVirtual() +
+								shape1.getMarginNonVirtual();
+		const btScalar length = delta.length();
+		results.normal = delta / length;
+		results.witnesses[0] += results.normal * margin;
+		results.distance = length - margin;
+		return results.distance;
 	}
 	else
 	{
-		if(gjk_status==GJK::eStatus::Inside)
+		if (gjk_status == GJK::eStatus::Inside)
 		{
-			if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
+			if (Penetration(shape0, wtrs0, &shape1, wtrs1, gjk.m_ray, results))
 			{
-				const btVector3	delta=	results.witnesses[0]-
-					results.witnesses[1];
-				const btScalar	length=	delta.length();
+				const btVector3 delta = results.witnesses[0] -
+										results.witnesses[1];
+				const btScalar length = delta.length();
 				if (length >= SIMD_EPSILON)
-					results.normal	=	delta/length;			
-				return(-length);
+					results.normal = delta / length;
+				return (-length);
 			}
-		}	
+		}
 	}
-	return(SIMD_INFINITY);
+	return (SIMD_INFINITY);
 }
 
 //
-bool	btGjkEpaSolver2::SignedDistance(const btConvexShape*	shape0,
-										const btTransform&		wtrs0,
-										const btConvexShape*	shape1,
-										const btTransform&		wtrs1,
-										const btVector3&		guess,
-										sResults&				results)
+bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
+									 const btTransform& wtrs0,
+									 const btConvexShape* shape1,
+									 const btTransform& wtrs1,
+									 const btVector3& guess,
+									 sResults& results)
 {
-	if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
-		return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
+	if (!Distance(shape0, wtrs0, shape1, wtrs1, guess, results))
+		return (Penetration(shape0, wtrs0, shape1, wtrs1, guess, results, false));
 	else
-		return(true);
+		return (true);
 }
-#endif //__SPU__
+#endif  //__SPU__
 
-/* Symbols cleanup		*/ 
+/* Symbols cleanup		*/
 
 #undef GJK_MAX_ITERATIONS
-#undef GJK_ACCURARY
+#undef GJK_ACCURACY
 #undef GJK_MIN_DISTANCE
 #undef GJK_DUPLICATED_EPS
 #undef GJK_SIMPLEX2_EPS
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h
index ac501d5ecfe..893daea3f54 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h
@@ -28,48 +28,46 @@ GJK-EPA collision solver by Nathanael Presson, 2008
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 
 ///btGjkEpaSolver contributed under zlib by Nathanael Presson
-struct	btGjkEpaSolver2
+struct btGjkEpaSolver2
 {
-struct	sResults
+	struct sResults
 	{
-	enum eStatus
+		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;
+			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;
 	};
 
-static int		StackSizeRequirement();
+	static int StackSizeRequirement();
 
-static bool		Distance(	const btConvexShape* shape0,const btTransform& wtrs0,
-							const btConvexShape* shape1,const btTransform& wtrs1,
-							const btVector3& guess,
-							sResults& results);
+	static bool Distance(const btConvexShape* shape0, const btTransform& wtrs0,
+						 const btConvexShape* shape1, const btTransform& wtrs1,
+						 const btVector3& guess,
+						 sResults& results);
 
-static bool		Penetration(const btConvexShape* shape0,const btTransform& wtrs0,
-							const btConvexShape* shape1,const btTransform& wtrs1,
+	static bool Penetration(const btConvexShape* shape0, const btTransform& wtrs0,
+							const btConvexShape* shape1, const btTransform& wtrs1,
 							const btVector3& guess,
 							sResults& results,
-							bool usemargins=true);
+							bool usemargins = true);
 #ifndef __SPU__
-static btScalar	SignedDistance(	const btVector3& position,
-								btScalar margin,
-								const btConvexShape* shape,
-								const btTransform& wtrs,
-								sResults& results);
-							
-static bool		SignedDistance(	const btConvexShape* shape0,const btTransform& wtrs0,
-								const btConvexShape* shape1,const btTransform& wtrs1,
-								const btVector3& guess,
-								sResults& results);
-#endif //__SPU__
+	static btScalar SignedDistance(const btVector3& position,
+								   btScalar margin,
+								   const btConvexShape* shape,
+								   const btTransform& wtrs,
+								   sResults& results);
 
+	static bool SignedDistance(const btConvexShape* shape0, const btTransform& wtrs0,
+							   const btConvexShape* shape1, const btTransform& wtrs1,
+							   const btVector3& guess,
+							   sResults& results);
+#endif  //__SPU__
 };
 
-#endif //BT_GJK_EPA2_H
-
+#endif  //BT_GJK_EPA2_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h
index ce1f24bc50d..6fedbbb3e5f 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h
@@ -29,915 +29,946 @@ Improvements and refactoring by Erwin Coumans, 2008-2014
 #include "LinearMath/btTransform.h"
 #include "btGjkCollisionDescription.h"
 
-
-
-struct	btGjkEpaSolver3
+struct btGjkEpaSolver3
 {
-struct	sResults
+	struct sResults
 	{
-	enum eStatus
+		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;
+			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
+#if defined(DEBUG) || defined(_DEBUG)
+#include <stdio.h>  //for debug printf
 #ifdef __SPU__
 #include <spu_printf.h>
 #define printf spu_printf
-#endif //__SPU__
+#endif  //__SPU__
 #endif
 
+// Config
 
-    
-    // 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
+/* 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
 {
-    eGjkValid,
-    eGjkInside,
-    eGjkFailed
+	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));
+	}
 };
 
-    // 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 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
+enum eEpaStatus
 {
-    eEpaValid,
-    eEpaTouching,
-    eEpaDegenerated,
-    eEpaNonConvex,
-    eEpaInvalidHull,
-    eEpaOutOfFaces,
-    eEpaOutOfVertices,
-    eEpaAccuraryReached,
-    eEpaFallBack,
-    eEpaFailed
+	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);
+	}
+};
 
-    // 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());
-        
-    }
-    
+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)
+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);
-    }
+	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)
+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);
+	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
@@ -990,28 +1021,28 @@ int	btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanc
 #endif
 
 template <typename btConvexTemplate, typename btDistanceInfoTemplate>
-int	btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b,
-                         const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo)
+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;
+	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		*/ 
+/* Symbols cleanup		*/
 
 #undef GJK_MAX_ITERATIONS
 #undef GJK_ACCURARY
@@ -1029,7 +1060,4 @@ int	btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b,
 #undef EPA_PLANE_EPS
 #undef EPA_INSIDE_EPS
 
-
-
-#endif //BT_GJK_EPA3_H
-
+#endif  //BT_GJK_EPA3_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
index 572ec36f563..07629229abc 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
@@ -18,49 +18,64 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 #include "btGjkEpaPenetrationDepthSolver.h"
 
-
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
 
-bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
-											  const btConvexShape* pConvexA, const btConvexShape* pConvexB,
-											  const btTransform& transformA, const btTransform& transformB,
-											  btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
-											  class btIDebugDraw* debugDraw)
+bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
+												  const btConvexShape* pConvexA, const btConvexShape* pConvexB,
+												  const btTransform& transformA, const btTransform& transformB,
+												  btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
+												  class btIDebugDraw* debugDraw)
 {
-
 	(void)debugDraw;
 	(void)v;
 	(void)simplexSolver;
 
-//	const btScalar				radialmargin(btScalar(0.));
-	
-	btVector3	guessVector(transformB.getOrigin()-transformA.getOrigin());
-	btGjkEpaSolver2::sResults	results;
-	
+	btVector3 guessVectors[] = {
+		btVector3(transformB.getOrigin() - transformA.getOrigin()).safeNormalize(),
+		btVector3(transformA.getOrigin() - transformB.getOrigin()).safeNormalize(),
+		btVector3(0, 0, 1),
+		btVector3(0, 1, 0),
+		btVector3(1, 0, 0),
+		btVector3(1, 1, 0),
+		btVector3(1, 1, 1),
+		btVector3(0, 1, 1),
+		btVector3(1, 0, 1),
+	};
 
-	if(btGjkEpaSolver2::Penetration(pConvexA,transformA,
-								pConvexB,transformB,
-								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
+	int numVectors = sizeof(guessVectors) / sizeof(btVector3);
+
+	for (int i = 0; i < numVectors; i++)
 	{
-		if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results))
+		simplexSolver.reset();
+		btVector3 guessVector = guessVectors[i];
+
+		btGjkEpaSolver2::sResults results;
+
+		if (btGjkEpaSolver2::Penetration(pConvexA, transformA,
+										 pConvexB, transformB,
+										 guessVector, results))
+
 		{
 			wWitnessOnA = results.witnesses[0];
 			wWitnessOnB = results.witnesses[1];
 			v = results.normal;
-			return false;
+			return true;
+		}
+		else
+		{
+			if (btGjkEpaSolver2::Distance(pConvexA, transformA, pConvexB, transformB, guessVector, results))
+			{
+				wWitnessOnA = results.witnesses[0];
+				wWitnessOnB = results.witnesses[1];
+				v = results.normal;
+				return false;
+			}
 		}
 	}
 
+	//failed to find a distance/penetration
+	wWitnessOnA.setValue(0, 0, 0);
+	wWitnessOnB.setValue(0, 0, 0);
+	v.setValue(0, 0, 0);
 	return false;
 }
-
-
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
index 1ed6340af3b..92d6df1729f 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
@@ -23,21 +23,18 @@ subject to the following restrictions:
 ///calculate the penetration depth between two convex shapes.
 class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
 {
-	public :
-
-		btGjkEpaPenetrationDepthSolver()
-		{
-		}
-
-		bool			calcPenDepth( btSimplexSolverInterface& simplexSolver,
-									  const btConvexShape* pConvexA, const btConvexShape* pConvexB,
-									  const btTransform& transformA, const btTransform& transformB,
-									  btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
-									  class btIDebugDraw* debugDraw);
-
-	private :
-
+public:
+	btGjkEpaPenetrationDepthSolver()
+	{
+	}
+
+	bool calcPenDepth(btSimplexSolverInterface& simplexSolver,
+					  const btConvexShape* pConvexA, const btConvexShape* pConvexB,
+					  const btTransform& transformA, const btTransform& transformB,
+					  btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
+					  class btIDebugDraw* debugDraw);
+
+private:
 };
 
-#endif	// BT_GJP_EPA_PENETRATION_DEPTH_H
-
+#endif  // BT_GJP_EPA_PENETRATION_DEPTH_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
index 759443a9613..5af93cb2fb3 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
@@ -18,80 +18,685 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
 
-
-
-#if defined(DEBUG) || defined (_DEBUG)
+#if defined(DEBUG) || defined(_DEBUG)
 //#define TEST_NON_VIRTUAL 1
-#include <stdio.h> //for debug printf
+#include <stdio.h>  //for debug printf
 #ifdef __SPU__
 #include <spu_printf.h>
 #define printf spu_printf
-#endif //__SPU__
+#endif  //__SPU__
 #endif
 
 //must be above the machine epsilon
+#ifdef BT_USE_DOUBLE_PRECISION
+#define REL_ERROR2 btScalar(1.0e-12)
+btScalar gGjkEpaPenetrationTolerance = 1.0e-12;
+#else
 #define REL_ERROR2 btScalar(1.0e-6)
+btScalar gGjkEpaPenetrationTolerance = 0.001;
+#endif
+
 
-//temp globals, to improve GJK/EPA/penetration calculations
-int gNumDeepPenetrationChecks = 0;
-int gNumGjkChecks = 0;
-
-
-btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver)
-:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
-m_penetrationDepthSolver(penetrationDepthSolver),
-m_simplexSolver(simplexSolver),
-m_minkowskiA(objectA),
-m_minkowskiB(objectB),
-m_shapeTypeA(objectA->getShapeType()),
-m_shapeTypeB(objectB->getShapeType()),
-m_marginA(objectA->getMargin()),
-m_marginB(objectB->getMargin()),
-m_ignoreMargin(false),
-m_lastUsedMethod(-1),
-m_catchDegeneracies(1),
-m_fixContactNormalDirection(1)
-{
-}
-btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver)
-:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
-m_penetrationDepthSolver(penetrationDepthSolver),
-m_simplexSolver(simplexSolver),
-m_minkowskiA(objectA),
-m_minkowskiB(objectB),
-m_shapeTypeA(shapeTypeA),
-m_shapeTypeB(shapeTypeB),
-m_marginA(marginA),
-m_marginB(marginB),
-m_ignoreMargin(false),
-m_lastUsedMethod(-1),
-m_catchDegeneracies(1),
-m_fixContactNormalDirection(1)
-{
-}
-
-void	btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
+btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
+	: m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)),
+	  m_penetrationDepthSolver(penetrationDepthSolver),
+	  m_simplexSolver(simplexSolver),
+	  m_minkowskiA(objectA),
+	  m_minkowskiB(objectB),
+	  m_shapeTypeA(objectA->getShapeType()),
+	  m_shapeTypeB(objectB->getShapeType()),
+	  m_marginA(objectA->getMargin()),
+	  m_marginB(objectB->getMargin()),
+	  m_ignoreMargin(false),
+	  m_lastUsedMethod(-1),
+	  m_catchDegeneracies(1),
+	  m_fixContactNormalDirection(1)
+{
+}
+btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, int shapeTypeA, int shapeTypeB, btScalar marginA, btScalar marginB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
+	: m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)),
+	  m_penetrationDepthSolver(penetrationDepthSolver),
+	  m_simplexSolver(simplexSolver),
+	  m_minkowskiA(objectA),
+	  m_minkowskiB(objectB),
+	  m_shapeTypeA(shapeTypeA),
+	  m_shapeTypeB(shapeTypeB),
+	  m_marginA(marginA),
+	  m_marginB(marginB),
+	  m_ignoreMargin(false),
+	  m_lastUsedMethod(-1),
+	  m_catchDegeneracies(1),
+	  m_fixContactNormalDirection(1)
+{
+}
+
+void btGjkPairDetector::getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults)
 {
 	(void)swapResults;
 
-	getClosestPointsNonVirtual(input,output,debugDraw);
+	getClosestPointsNonVirtual(input, output, debugDraw);
+}
+
+static void btComputeSupport(const btConvexShape *convexA, const btTransform &localTransA, const btConvexShape *convexB, const btTransform &localTransB, const btVector3 &dir, bool check2d, btVector3 &supAworld, btVector3 &supBworld, btVector3 &aMinb)
+{
+	btVector3 separatingAxisInA = (dir)*localTransA.getBasis();
+	btVector3 separatingAxisInB = (-dir) * localTransB.getBasis();
+
+	btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
+	btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
+
+	btVector3 pInA = pInANoMargin;
+	btVector3 qInB = qInBNoMargin;
+
+	supAworld = localTransA(pInA);
+	supBworld = localTransB(qInB);
+
+	if (check2d)
+	{
+		supAworld[2] = 0.f;
+		supBworld[2] = 0.f;
+	}
+
+	aMinb = supAworld - supBworld;
+}
+
+struct btSupportVector
+{
+	btVector3 v;   //!< Support point in minkowski sum
+	btVector3 v1;  //!< Support point in obj1
+	btVector3 v2;  //!< Support point in obj2
+};
+
+struct btSimplex
+{
+	btSupportVector ps[4];
+	int last;  //!< index of last added point
+};
+
+static btVector3 ccd_vec3_origin(0, 0, 0);
+
+inline void btSimplexInit(btSimplex *s)
+{
+	s->last = -1;
+}
+
+inline int btSimplexSize(const btSimplex *s)
+{
+	return s->last + 1;
+}
+
+inline const btSupportVector *btSimplexPoint(const btSimplex *s, int idx)
+{
+	// here is no check on boundaries
+	return &s->ps[idx];
+}
+inline void btSupportCopy(btSupportVector *d, const btSupportVector *s)
+{
+	*d = *s;
+}
+
+inline void btVec3Copy(btVector3 *v, const btVector3 *w)
+{
+	*v = *w;
+}
+
+inline void ccdVec3Add(btVector3 *v, const btVector3 *w)
+{
+	v->m_floats[0] += w->m_floats[0];
+	v->m_floats[1] += w->m_floats[1];
+	v->m_floats[2] += w->m_floats[2];
+}
+
+inline void ccdVec3Sub(btVector3 *v, const btVector3 *w)
+{
+	*v -= *w;
+}
+inline void btVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
+{
+	*d = (*v) - (*w);
+}
+inline btScalar btVec3Dot(const btVector3 *a, const btVector3 *b)
+{
+	btScalar dot;
+	dot = a->dot(*b);
+
+	return dot;
+}
+
+inline btScalar ccdVec3Dist2(const btVector3 *a, const btVector3 *b)
+{
+	btVector3 ab;
+	btVec3Sub2(&ab, a, b);
+	return btVec3Dot(&ab, &ab);
+}
+
+inline void btVec3Scale(btVector3 *d, btScalar k)
+{
+	d->m_floats[0] *= k;
+	d->m_floats[1] *= k;
+	d->m_floats[2] *= k;
+}
+
+inline void btVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
+{
+	d->m_floats[0] = (a->m_floats[1] * b->m_floats[2]) - (a->m_floats[2] * b->m_floats[1]);
+	d->m_floats[1] = (a->m_floats[2] * b->m_floats[0]) - (a->m_floats[0] * b->m_floats[2]);
+	d->m_floats[2] = (a->m_floats[0] * b->m_floats[1]) - (a->m_floats[1] * b->m_floats[0]);
+}
+
+inline void btTripleCross(const btVector3 *a, const btVector3 *b,
+						  const btVector3 *c, btVector3 *d)
+{
+	btVector3 e;
+	btVec3Cross(&e, a, b);
+	btVec3Cross(d, &e, c);
+}
+
+inline int ccdEq(btScalar _a, btScalar _b)
+{
+	btScalar ab;
+	btScalar a, b;
+
+	ab = btFabs(_a - _b);
+	if (btFabs(ab) < SIMD_EPSILON)
+		return 1;
+
+	a = btFabs(_a);
+	b = btFabs(_b);
+	if (b > a)
+	{
+		return ab < SIMD_EPSILON * b;
+	}
+	else
+	{
+		return ab < SIMD_EPSILON * a;
+	}
+}
+
+btScalar ccdVec3X(const btVector3 *v)
+{
+	return v->x();
+}
+
+btScalar ccdVec3Y(const btVector3 *v)
+{
+	return v->y();
+}
+
+btScalar ccdVec3Z(const btVector3 *v)
+{
+	return v->z();
+}
+inline int btVec3Eq(const btVector3 *a, const btVector3 *b)
+{
+	return ccdEq(ccdVec3X(a), ccdVec3X(b)) && ccdEq(ccdVec3Y(a), ccdVec3Y(b)) && ccdEq(ccdVec3Z(a), ccdVec3Z(b));
+}
+
+inline void btSimplexAdd(btSimplex *s, const btSupportVector *v)
+{
+	// here is no check on boundaries in sake of speed
+	++s->last;
+	btSupportCopy(s->ps + s->last, v);
+}
+
+inline void btSimplexSet(btSimplex *s, size_t pos, const btSupportVector *a)
+{
+	btSupportCopy(s->ps + pos, a);
+}
+
+inline void btSimplexSetSize(btSimplex *s, int size)
+{
+	s->last = size - 1;
+}
+
+inline const btSupportVector *ccdSimplexLast(const btSimplex *s)
+{
+	return btSimplexPoint(s, s->last);
+}
+
+inline int ccdSign(btScalar val)
+{
+	if (btFuzzyZero(val))
+	{
+		return 0;
+	}
+	else if (val < btScalar(0))
+	{
+		return -1;
+	}
+	return 1;
+}
+
+inline btScalar btVec3PointSegmentDist2(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.
+
+	btScalar dist, t;
+	btVector3 d, a;
+
+	// direction of segment
+	btVec3Sub2(&d, b, x0);
+
+	// precompute vector from P to x0
+	btVec3Sub2(&a, x0, P);
+
+	t = -btScalar(1.) * btVec3Dot(&a, &d);
+	t /= btVec3Dot(&d, &d);
+
+	if (t < btScalar(0) || btFuzzyZero(t))
+	{
+		dist = ccdVec3Dist2(x0, P);
+		if (witness)
+			btVec3Copy(witness, x0);
+	}
+	else if (t > btScalar(1) || ccdEq(t, btScalar(1)))
+	{
+		dist = ccdVec3Dist2(b, P);
+		if (witness)
+			btVec3Copy(witness, b);
+	}
+	else
+	{
+		if (witness)
+		{
+			btVec3Copy(witness, &d);
+			btVec3Scale(witness, t);
+			ccdVec3Add(witness, x0);
+			dist = ccdVec3Dist2(witness, P);
+		}
+		else
+		{
+			// recycling variables
+			btVec3Scale(&d, t);
+			ccdVec3Add(&d, &a);
+			dist = btVec3Dot(&d, &d);
+		}
+	}
+
+	return dist;
+}
+
+btScalar btVec3PointTriDist2(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;
+	double u, v, w, p, q, r;
+	double s, t, dist, dist2;
+	btVector3 witness2;
+
+	btVec3Sub2(&d1, B, x0);
+	btVec3Sub2(&d2, C, x0);
+	btVec3Sub2(&a, x0, P);
+
+	u = btVec3Dot(&a, &a);
+	v = btVec3Dot(&d1, &d1);
+	w = btVec3Dot(&d2, &d2);
+	p = btVec3Dot(&a, &d1);
+	q = btVec3Dot(&a, &d2);
+	r = btVec3Dot(&d1, &d2);
+
+	s = (q * r - w * p) / (w * v - r * r);
+	t = (-s * r - q) / w;
+
+	if ((btFuzzyZero(s) || s > btScalar(0)) && (ccdEq(s, btScalar(1)) || s < btScalar(1)) && (btFuzzyZero(t) || t > btScalar(0)) && (ccdEq(t, btScalar(1)) || t < btScalar(1)) && (ccdEq(t + s, btScalar(1)) || t + s < btScalar(1)))
+	{
+		if (witness)
+		{
+			btVec3Scale(&d1, s);
+			btVec3Scale(&d2, t);
+			btVec3Copy(witness, x0);
+			ccdVec3Add(witness, &d1);
+			ccdVec3Add(witness, &d2);
+
+			dist = ccdVec3Dist2(witness, P);
+		}
+		else
+		{
+			dist = s * s * v;
+			dist += t * t * w;
+			dist += btScalar(2.) * s * t * r;
+			dist += btScalar(2.) * s * p;
+			dist += btScalar(2.) * t * q;
+			dist += u;
+		}
+	}
+	else
+	{
+		dist = btVec3PointSegmentDist2(P, x0, B, witness);
+
+		dist2 = btVec3PointSegmentDist2(P, x0, C, &witness2);
+		if (dist2 < dist)
+		{
+			dist = dist2;
+			if (witness)
+				btVec3Copy(witness, &witness2);
+		}
+
+		dist2 = btVec3PointSegmentDist2(P, B, C, &witness2);
+		if (dist2 < dist)
+		{
+			dist = dist2;
+			if (witness)
+				btVec3Copy(witness, &witness2);
+		}
+	}
+
+	return dist;
+}
+
+static int btDoSimplex2(btSimplex *simplex, btVector3 *dir)
+{
+	const btSupportVector *A, *B;
+	btVector3 AB, AO, tmp;
+	btScalar dot;
+
+	// get last added as A
+	A = ccdSimplexLast(simplex);
+	// get the other point
+	B = btSimplexPoint(simplex, 0);
+	// compute AB oriented segment
+	btVec3Sub2(&AB, &B->v, &A->v);
+	// compute AO vector
+	btVec3Copy(&AO, &A->v);
+	btVec3Scale(&AO, -btScalar(1));
+
+	// dot product AB . AO
+	dot = btVec3Dot(&AB, &AO);
+
+	// check if origin doesn't lie on AB segment
+	btVec3Cross(&tmp, &AB, &AO);
+	if (btFuzzyZero(btVec3Dot(&tmp, &tmp)) && dot > btScalar(0))
+	{
+		return 1;
+	}
+
+	// check if origin is in area where AB segment is
+	if (btFuzzyZero(dot) || dot < btScalar(0))
+	{
+		// origin is in outside are of A
+		btSimplexSet(simplex, 0, A);
+		btSimplexSetSize(simplex, 1);
+		btVec3Copy(dir, &AO);
+	}
+	else
+	{
+		// origin is in area where AB segment is
+
+		// keep simplex untouched and set direction to
+		// AB x AO x AB
+		btTripleCross(&AB, &AO, &AB, dir);
+	}
+
+	return 0;
+}
+
+static int btDoSimplex3(btSimplex *simplex, btVector3 *dir)
+{
+	const btSupportVector *A, *B, *C;
+	btVector3 AO, AB, AC, ABC, tmp;
+	btScalar dot, dist;
+
+	// get last added as A
+	A = ccdSimplexLast(simplex);
+	// get the other points
+	B = btSimplexPoint(simplex, 1);
+	C = btSimplexPoint(simplex, 0);
+
+	// check touching contact
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0);
+	if (btFuzzyZero(dist))
+	{
+		return 1;
+	}
+
+	// check if triangle is really triangle (has area > 0)
+	// if not simplex can't be expanded and thus no itersection is found
+	if (btVec3Eq(&A->v, &B->v) || btVec3Eq(&A->v, &C->v))
+	{
+		return -1;
+	}
+
+	// compute AO vector
+	btVec3Copy(&AO, &A->v);
+	btVec3Scale(&AO, -btScalar(1));
+
+	// compute AB and AC segments and ABC vector (perpendircular to triangle)
+	btVec3Sub2(&AB, &B->v, &A->v);
+	btVec3Sub2(&AC, &C->v, &A->v);
+	btVec3Cross(&ABC, &AB, &AC);
+
+	btVec3Cross(&tmp, &ABC, &AC);
+	dot = btVec3Dot(&tmp, &AO);
+	if (btFuzzyZero(dot) || dot > btScalar(0))
+	{
+		dot = btVec3Dot(&AC, &AO);
+		if (btFuzzyZero(dot) || dot > btScalar(0))
+		{
+			// C is already in place
+			btSimplexSet(simplex, 1, A);
+			btSimplexSetSize(simplex, 2);
+			btTripleCross(&AC, &AO, &AC, dir);
+		}
+		else
+		{
+			dot = btVec3Dot(&AB, &AO);
+			if (btFuzzyZero(dot) || dot > btScalar(0))
+			{
+				btSimplexSet(simplex, 0, B);
+				btSimplexSet(simplex, 1, A);
+				btSimplexSetSize(simplex, 2);
+				btTripleCross(&AB, &AO, &AB, dir);
+			}
+			else
+			{
+				btSimplexSet(simplex, 0, A);
+				btSimplexSetSize(simplex, 1);
+				btVec3Copy(dir, &AO);
+			}
+		}
+	}
+	else
+	{
+		btVec3Cross(&tmp, &AB, &ABC);
+		dot = btVec3Dot(&tmp, &AO);
+		if (btFuzzyZero(dot) || dot > btScalar(0))
+		{
+			dot = btVec3Dot(&AB, &AO);
+			if (btFuzzyZero(dot) || dot > btScalar(0))
+			{
+				btSimplexSet(simplex, 0, B);
+				btSimplexSet(simplex, 1, A);
+				btSimplexSetSize(simplex, 2);
+				btTripleCross(&AB, &AO, &AB, dir);
+			}
+			else
+			{
+				btSimplexSet(simplex, 0, A);
+				btSimplexSetSize(simplex, 1);
+				btVec3Copy(dir, &AO);
+			}
+		}
+		else
+		{
+			dot = btVec3Dot(&ABC, &AO);
+			if (btFuzzyZero(dot) || dot > btScalar(0))
+			{
+				btVec3Copy(dir, &ABC);
+			}
+			else
+			{
+				btSupportVector tmp;
+				btSupportCopy(&tmp, C);
+				btSimplexSet(simplex, 0, B);
+				btSimplexSet(simplex, 1, &tmp);
+
+				btVec3Copy(dir, &ABC);
+				btVec3Scale(dir, -btScalar(1));
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
+{
+	const btSupportVector *A, *B, *C, *D;
+	btVector3 AO, AB, AC, AD, ABC, ACD, ADB;
+	int B_on_ACD, C_on_ADB, D_on_ABC;
+	int AB_O, AC_O, AD_O;
+	btScalar dist;
+
+	// get last added as A
+	A = ccdSimplexLast(simplex);
+	// get the other points
+	B = btSimplexPoint(simplex, 2);
+	C = btSimplexPoint(simplex, 1);
+	D = btSimplexPoint(simplex, 0);
+
+	// check if tetrahedron is really tetrahedron (has volume > 0)
+	// if it is not simplex can't be expanded and thus no intersection is
+	// found
+	dist = btVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, 0);
+	if (btFuzzyZero(dist))
+	{
+		return -1;
+	}
+
+	// check if origin lies on some of tetrahedron's face - if so objects
+	// intersect
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0);
+	if (btFuzzyZero(dist))
+		return 1;
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &C->v, &D->v, 0);
+	if (btFuzzyZero(dist))
+		return 1;
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &D->v, 0);
+	if (btFuzzyZero(dist))
+		return 1;
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &B->v, &C->v, &D->v, 0);
+	if (btFuzzyZero(dist))
+		return 1;
+
+	// compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
+	btVec3Copy(&AO, &A->v);
+	btVec3Scale(&AO, -btScalar(1));
+	btVec3Sub2(&AB, &B->v, &A->v);
+	btVec3Sub2(&AC, &C->v, &A->v);
+	btVec3Sub2(&AD, &D->v, &A->v);
+	btVec3Cross(&ABC, &AB, &AC);
+	btVec3Cross(&ACD, &AC, &AD);
+	btVec3Cross(&ADB, &AD, &AB);
+
+	// side (positive or negative) of B, C, D relative to planes ACD, ADB
+	// and ABC respectively
+	B_on_ACD = ccdSign(btVec3Dot(&ACD, &AB));
+	C_on_ADB = ccdSign(btVec3Dot(&ADB, &AC));
+	D_on_ABC = ccdSign(btVec3Dot(&ABC, &AD));
+
+	// whether origin is on same side of ACD, ADB, ABC as B, C, D
+	// respectively
+	AB_O = ccdSign(btVec3Dot(&ACD, &AO)) == B_on_ACD;
+	AC_O = ccdSign(btVec3Dot(&ADB, &AO)) == C_on_ADB;
+	AD_O = ccdSign(btVec3Dot(&ABC, &AO)) == D_on_ABC;
+
+	if (AB_O && AC_O && AD_O)
+	{
+		// origin is in tetrahedron
+		return 1;
+		// rearrange simplex to triangle and call btDoSimplex3()
+	}
+	else if (!AB_O)
+	{
+		// B is farthest from the origin among all of the tetrahedron's
+		// points, so remove it from the list and go on with the triangle
+		// case
+
+		// D and C are in place
+		btSimplexSet(simplex, 2, A);
+		btSimplexSetSize(simplex, 3);
+	}
+	else if (!AC_O)
+	{
+		// C is farthest
+		btSimplexSet(simplex, 1, D);
+		btSimplexSet(simplex, 0, B);
+		btSimplexSet(simplex, 2, A);
+		btSimplexSetSize(simplex, 3);
+	}
+	else
+	{  // (!AD_O)
+		btSimplexSet(simplex, 0, C);
+		btSimplexSet(simplex, 1, B);
+		btSimplexSet(simplex, 2, A);
+		btSimplexSetSize(simplex, 3);
+	}
+
+	return btDoSimplex3(simplex, dir);
+}
+
+static int btDoSimplex(btSimplex *simplex, btVector3 *dir)
+{
+	if (btSimplexSize(simplex) == 2)
+	{
+		// simplex contains segment only one segment
+		return btDoSimplex2(simplex, dir);
+	}
+	else if (btSimplexSize(simplex) == 3)
+	{
+		// simplex contains triangle
+		return btDoSimplex3(simplex, dir);
+	}
+	else
+	{  // btSimplexSize(simplex) == 4
+		// tetrahedron - this is the only shape which can encapsule origin
+		// so btDoSimplex4() also contains test on it
+		return btDoSimplex4(simplex, dir);
+	}
 }
 
 #ifdef __SPU__
-void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+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.));
+	btScalar distance = btScalar(0.);
+	btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.));
 
-	btVector3 pointOnA,pointOnB;
-	btTransform	localTransA = input.m_transformA;
+	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;
 
@@ -100,7 +705,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 	btScalar marginA = m_marginA;
 	btScalar marginB = m_marginB;
 
-	gNumGjkChecks++;
 
 	//for CCD we don't use margins
 	if (m_ignoreMargin)
@@ -110,8 +714,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 	}
 
 	m_curIter = 0;
-	int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
-	m_cachedSeparatingAxis.setValue(0,1,0);
+	int gGjkMaxIter = 1000;  //this is to catch invalid input, perhaps check for #NaN?
+	m_cachedSeparatingAxis.setValue(0, 1, 0);
 
 	bool isValid = false;
 	bool checkSimplex = false;
@@ -119,191 +723,291 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 	m_degenerateSimplex = 0;
 
 	m_lastUsedMethod = -1;
-
+	int status = -2;
+	btVector3 orgNormalInB(0, 0, 0);
+	btScalar margin = marginA + marginB;
+
+	//we add a separate implementation to check if the convex shapes intersect
+	//See also "Real-time Collision Detection with Implicit Objects" by Leif Olvang
+	//Todo: integrate the simplex penetration check directly inside the Bullet btVoronoiSimplexSolver
+	//and remove this temporary code from libCCD
+	//this fixes issue https://github.com/bulletphysics/bullet3/issues/1703
+	//note, for large differences in shapes, use double precision build!
 	{
 		btScalar squaredDistance = BT_LARGE_FLOAT;
 		btScalar delta = btScalar(0.);
-		
-		btScalar margin = marginA + marginB;
-		
-		
 
-		m_simplexSolver->reset();
-		
-		for ( ; ; )
-		//while (true)
-		{
+		btSimplex simplex1;
+		btSimplex *simplex = &simplex1;
+		btSimplexInit(simplex);
 
-			btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
-			btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
+		btVector3 dir(1, 0, 0);
 
+		{
+			btVector3 lastSupV;
+			btVector3 supAworld;
+			btVector3 supBworld;
+			btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
 
-			btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
-			btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+			btSupportVector last;
+			last.v = lastSupV;
+			last.v1 = supAworld;
+			last.v2 = supBworld;
 
-			btVector3  pWorld = localTransA(pInA);	
-			btVector3  qWorld = localTransB(qInB);
+			btSimplexAdd(simplex, &last);
 
+			dir = -lastSupV;
 
-			if (check2d)
+			// start iterations
+			for (int iterations = 0; iterations < gGjkMaxIter; iterations++)
 			{
-				pWorld[2] = 0.f;
-				qWorld[2] = 0.f;
-			}
+				// obtain support point
+				btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
+
+				// check if farthest point in Minkowski difference in direction dir
+				// isn't somewhere before origin (the test on negative dot product)
+				// - because if it is, objects are not intersecting at all.
+				btScalar delta = lastSupV.dot(dir);
+				if (delta < 0)
+				{
+					//no intersection, besides margin
+					status = -1;
+					break;
+				}
 
-			btVector3 w	= pWorld - qWorld;
-			delta = m_cachedSeparatingAxis.dot(w);
+				// add last support vector to simplex
+				last.v = lastSupV;
+				last.v1 = supAworld;
+				last.v2 = supBworld;
 
-			// potential exit, they don't overlap
-			if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) 
-			{
-				m_degenerateSimplex = 10;
-				checkSimplex=true;
-				//checkPenetration = false;
-				break;
-			}
+				btSimplexAdd(simplex, &last);
 
-			//exit 0: the new point is already in the simplex, or we didn't come any closer
-			if (m_simplexSolver->inSimplex(w))
-			{
-				m_degenerateSimplex = 1;
-				checkSimplex = true;
-				break;
-			}
-			// are we getting any closer ?
-			btScalar f0 = squaredDistance - delta;
-			btScalar f1 = squaredDistance * REL_ERROR2;
+				// if btDoSimplex returns 1 if objects intersect, -1 if objects don't
+				// intersect and 0 if algorithm should continue
 
-			if (f0 <= f1)
-			{
-				if (f0 <= btScalar(0.))
+				btVector3 newDir;
+				int do_simplex_res = btDoSimplex(simplex, &dir);
+
+				if (do_simplex_res == 1)
+				{
+					status = 0;  // intersection found
+					break;
+				}
+				else if (do_simplex_res == -1)
+				{
+					// intersection not found
+					status = -1;
+					break;
+				}
+
+				if (btFuzzyZero(btVec3Dot(&dir, &dir)))
+				{
+					// intersection not found
+					status = -1;
+				}
+
+				if (dir.length2() < SIMD_EPSILON)
 				{
-					m_degenerateSimplex = 2;
-				} else
+					//no intersection, besides margin
+					status = -1;
+					break;
+				}
+
+				if (dir.fuzzyZero())
 				{
-					m_degenerateSimplex = 11;
+					// intersection not found
+					status = -1;
+					break;
 				}
-				checkSimplex = true;
-				break;
 			}
+		}
 
-			//add current vertex to simplex
-			m_simplexSolver->addVertex(w, pWorld, qWorld);
-			btVector3 newCachedSeparatingAxis;
+		m_simplexSolver->reset();
+		if (status == 0)
+		{
+			//status = 0;
+			//printf("Intersect!\n");
+		}
 
-			//calculate the closest point to the origin (update vector v)
-			if (!m_simplexSolver->closest(newCachedSeparatingAxis))
+		if (status == -1)
+		{
+			//printf("not intersect\n");
+		}
+		//printf("dir=%f,%f,%f\n",dir[0],dir[1],dir[2]);
+		if (1)
+		{
+			for (;;)
+			//while (true)
 			{
-				m_degenerateSimplex = 3;
-				checkSimplex = true;
-				break;
-			}
+				btVector3 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
+				btVector3 separatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
 
-			if(newCachedSeparatingAxis.length2()<REL_ERROR2)
-            {
-				m_cachedSeparatingAxis = newCachedSeparatingAxis;
-                m_degenerateSimplex = 6;
-                checkSimplex = true;
-                break;
-            }
+				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
+				btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
+
+				btVector3 pWorld = localTransA(pInA);
+				btVector3 qWorld = localTransB(qInB);
+
+				if (check2d)
+				{
+					pWorld[2] = 0.f;
+					qWorld[2] = 0.f;
+				}
+
+				btVector3 w = pWorld - qWorld;
+				delta = m_cachedSeparatingAxis.dot(w);
+
+				// potential exit, they don't overlap
+				if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.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 (m_simplexSolver->inSimplex(w))
+				{
+					m_degenerateSimplex = 1;
+					checkSimplex = true;
+					break;
+				}
+				// are we getting any closer ?
+				btScalar f0 = squaredDistance - delta;
+				btScalar f1 = squaredDistance * REL_ERROR2;
+
+				if (f0 <= f1)
+				{
+					if (f0 <= btScalar(0.))
+					{
+						m_degenerateSimplex = 2;
+					}
+					else
+					{
+						m_degenerateSimplex = 11;
+					}
+					checkSimplex = true;
+					break;
+				}
+
+				//add current vertex to simplex
+				m_simplexSolver->addVertex(w, pWorld, qWorld);
+				btVector3 newCachedSeparatingAxis;
+
+				//calculate the closest point to the origin (update vector v)
+				if (!m_simplexSolver->closest(newCachedSeparatingAxis))
+				{
+					m_degenerateSimplex = 3;
+					checkSimplex = true;
+					break;
+				}
+
+				if (newCachedSeparatingAxis.length2() < REL_ERROR2)
+				{
+					m_cachedSeparatingAxis = newCachedSeparatingAxis;
+					m_degenerateSimplex = 6;
+					checkSimplex = true;
+					break;
+				}
 
-			btScalar previousSquaredDistance = squaredDistance;
-			squaredDistance = newCachedSeparatingAxis.length2();
+				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;
-			}
+				///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  //
 
-			m_cachedSeparatingAxis = newCachedSeparatingAxis;
+				//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;
 
-			  //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
-              if (m_curIter++ > gGjkMaxIter)   
-              {   
-                      #if defined(DEBUG) || defined (_DEBUG)
+					break;
+				}
 
-                              printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);   
-                              printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",   
-                              m_cachedSeparatingAxis.getX(),   
-                              m_cachedSeparatingAxis.getY(),   
-                              m_cachedSeparatingAxis.getZ(),   
-                              squaredDistance,   
-                              m_minkowskiA->getShapeType(),   
-                              m_minkowskiB->getShapeType());   
+				m_cachedSeparatingAxis = newCachedSeparatingAxis;
 
-                      #endif   
-                      break;   
+				//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, shapeTypeA=%i,shapeTypeB=%i\n",
+						   m_cachedSeparatingAxis.getX(),
+						   m_cachedSeparatingAxis.getY(),
+						   m_cachedSeparatingAxis.getZ(),
+						   squaredDistance,
+						   m_minkowskiA->getShapeType(),
+						   m_minkowskiB->getShapeType());
 
+#endif
+					break;
+				}
 
-			bool check = (!m_simplexSolver->fullSimplex());
-			//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
+				bool check = (!m_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 (!check)
+				{
+					//do we need this backup_closest here ?
+					//				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+					m_degenerateSimplex = 13;
+					break;
+				}
 			}
-		}
-
-		if (checkSimplex)
-		{
-			m_simplexSolver->compute_points(pointOnA, pointOnB);
-			normalInB = m_cachedSeparatingAxis;
 
-			btScalar lenSqr =m_cachedSeparatingAxis.length2();
-			
-			//valid normal
-			if (lenSqr < 0.0001)
+			if (checkSimplex)
 			{
-				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;
+				m_simplexSolver->compute_points(pointOnA, pointOnB);
+				normalInB = m_cachedSeparatingAxis;
+
+				btScalar lenSqr = m_cachedSeparatingAxis.length2();
+
+				//valid normal
+				if (lenSqr < REL_ERROR2)
+				{
+					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;
+					orgNormalInB = normalInB;
+
+					m_lastUsedMethod = 1;
+				}
+				else
+				{
+					m_lastUsedMethod = 2;
+				}
 			}
 		}
 
-		bool catchDegeneratePenetrationCase = 
-			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
+		bool catchDegeneratePenetrationCase =
+			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance + margin) < gGjkEpaPenetrationTolerance));
 
 		//if (checkPenetration && !isValid)
-		if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
+		if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase)) || (status == 0))
 		{
 			//penetration case
 
@@ -311,149 +1015,169 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 			if (m_penetrationDepthSolver)
 			{
 				// Penetration depth case.
-				btVector3 tmpPointOnA,tmpPointOnB;
-				
-				gNumDeepPenetrationChecks++;
+				btVector3 tmpPointOnA, tmpPointOnB;
+
 				m_cachedSeparatingAxis.setZero();
 
-				bool isValid2 = m_penetrationDepthSolver->calcPenDepth( 
-					*m_simplexSolver, 
-					m_minkowskiA,m_minkowskiB,
-					localTransA,localTransB,
+				bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
+					*m_simplexSolver,
+					m_minkowskiA, m_minkowskiB,
+					localTransA, localTransB,
 					m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
-					debugDraw
-					);
+					debugDraw);
 
-
-				if (isValid2)
+				if (m_cachedSeparatingAxis.length2())
 				{
-					btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
-					btScalar lenSqr = tmpNormalInB.length2();
-					if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
+					if (isValid2)
 					{
-						tmpNormalInB = m_cachedSeparatingAxis;
-						lenSqr = m_cachedSeparatingAxis.length2();
-					}
+						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();
-						m_lastUsedMethod = 3;
-						//only replace valid penetrations when the result is deeper (check)
-						if (!isValid || (distance2 < distance))
+						if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON))
 						{
-							distance = distance2;
-							pointOnA = tmpPointOnA;
-							pointOnB = tmpPointOnB;
-							normalInB = tmpNormalInB;
-							///todo: need to track down this EPA penetration solver degeneracy
-							///the penetration solver reports penetration but the contact normal
-							///connecting the contact points is pointing in the opposite direction
-							///until then, detect the issue and revert the normal
+							tmpNormalInB /= btSqrt(lenSqr);
+							btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length();
+							m_lastUsedMethod = 3;
+							//only replace valid penetrations when the result is deeper (check)
+							if (!isValid || (distance2 < distance))
 							{
-								btScalar d1=0;
-								{
-									btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis();
-									btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis();
-								
-
-									btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
-									btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
-
-									btVector3  pWorld = localTransA(pInA);	
-									btVector3  qWorld = localTransB(qInB);
-									btVector3 w	= pWorld - qWorld;
-									d1 = (-normalInB).dot(w);
-								}
-								btScalar d0 = 0.f;
-								{
-									btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis();
-									btVector3 seperatingAxisInB = normalInB* input.m_transformB.getBasis();
-								
-
-									btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
-									btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
-
-									btVector3  pWorld = localTransA(pInA);	
-									btVector3  qWorld = localTransB(qInB);
-									btVector3 w	= pWorld - qWorld;
-									d0 = normalInB.dot(w);
-								}
-								if (d1>d0)
-								{
-									m_lastUsedMethod = 10;
-									normalInB*=-1;
-								} 
-
+								distance = distance2;
+								pointOnA = tmpPointOnA;
+								pointOnB = tmpPointOnB;
+								normalInB = tmpNormalInB;
+								isValid = true;
+							}
+							else
+							{
+								m_lastUsedMethod = 8;
 							}
-							isValid = true;
-							
-						} else
+						}
+						else
 						{
-							m_lastUsedMethod = 8;
+							m_lastUsedMethod = 9;
 						}
-					} else
-					{
-						m_lastUsedMethod = 9;
 					}
-				} else
+					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
+						///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.))
 						{
-							m_lastUsedMethod = 5;
+							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;
+							}
 						}
 					}
 				}
-				
+				else
+				{
+					//printf("EPA didn't return a valid value\n");
+				}
 			}
-
 		}
 	}
 
-	
-
-	if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
+	if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
 	{
-
 		m_cachedSeparatingAxis = normalInB;
 		m_cachedSeparatingDistance = distance;
+		if (1)
+		{
+			///todo: need to track down this EPA penetration solver degeneracy
+			///the penetration solver reports penetration but the contact normal
+			///connecting the contact points is pointing in the opposite direction
+			///until then, detect the issue and revert the normal
 
-		output.addContactPoint(
-			normalInB,
-			pointOnB+positionOffset,
-			distance);
+			btScalar d2 = 0.f;
+			{
+				btVector3 separatingAxisInA = (-orgNormalInB) * localTransA.getBasis();
+				btVector3 separatingAxisInB = orgNormalInB * localTransB.getBasis();
 
-	}
+				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
+				btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
 
+				btVector3 pWorld = localTransA(pInA);
+				btVector3 qWorld = localTransB(qInB);
+				btVector3 w = pWorld - qWorld;
+				d2 = orgNormalInB.dot(w) - margin;
+			}
 
-}
+			btScalar d1 = 0;
+			{
+				btVector3 separatingAxisInA = (normalInB)*localTransA.getBasis();
+				btVector3 separatingAxisInB = -normalInB * localTransB.getBasis();
 
+				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
+				btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
 
+				btVector3 pWorld = localTransA(pInA);
+				btVector3 qWorld = localTransB(qInB);
+				btVector3 w = pWorld - qWorld;
+				d1 = (-normalInB).dot(w) - margin;
+			}
+			btScalar d0 = 0.f;
+			{
+				btVector3 separatingAxisInA = (-normalInB) * input.m_transformA.getBasis();
+				btVector3 separatingAxisInB = normalInB * input.m_transformB.getBasis();
 
+				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
+				btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
 
+				btVector3 pWorld = localTransA(pInA);
+				btVector3 qWorld = localTransB(qInB);
+				btVector3 w = pWorld - qWorld;
+				d0 = normalInB.dot(w) - margin;
+			}
 
+			if (d1 > d0)
+			{
+				m_lastUsedMethod = 10;
+				normalInB *= -1;
+			}
+
+			if (orgNormalInB.length2())
+			{
+				if (d2 > d0 && d2 > d1 && d2 > distance)
+				{
+					normalInB = orgNormalInB;
+					distance = d2;
+				}
+			}
+		}
+
+		output.addContactPoint(
+			normalInB,
+			pointOnB + positionOffset,
+			distance);
+	}
+	else
+	{
+		//printf("invalid gjk query\n");
+	}
+}
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
index feeae686213..faa02287cae 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
@@ -13,9 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
-
 #ifndef BT_GJK_PAIR_DETECTOR_H
 #define BT_GJK_PAIR_DETECTOR_H
 
@@ -29,39 +26,34 @@ class btConvexPenetrationDepthSolver;
 /// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
 class btGjkPairDetector : public btDiscreteCollisionDetectorInterface
 {
-	
-
-	btVector3	m_cachedSeparatingAxis;
-	btConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
+	btVector3 m_cachedSeparatingAxis;
+	btConvexPenetrationDepthSolver* m_penetrationDepthSolver;
 	btSimplexSolverInterface* m_simplexSolver;
 	const btConvexShape* m_minkowskiA;
 	const btConvexShape* m_minkowskiB;
-	int	m_shapeTypeA;
+	int m_shapeTypeA;
 	int m_shapeTypeB;
-	btScalar	m_marginA;
-	btScalar	m_marginB;
+	btScalar m_marginA;
+	btScalar m_marginB;
 
-	bool		m_ignoreMargin;
-	btScalar	m_cachedSeparatingDistance;
-	
+	bool m_ignoreMargin;
+	btScalar m_cachedSeparatingDistance;
 
 public:
-
 	//some debugging to fix degeneracy problems
-	int			m_lastUsedMethod;
-	int			m_curIter;
-	int			m_degenerateSimplex;
-	int			m_catchDegeneracies;
-	int			m_fixContactNormalDirection;
+	int m_lastUsedMethod;
+	int m_curIter;
+	int m_degenerateSimplex;
+	int m_catchDegeneracies;
+	int m_fixContactNormalDirection;
 
-	btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver);
-	btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver);
-	virtual ~btGjkPairDetector() {};
+	btGjkPairDetector(const btConvexShape* objectA, const btConvexShape* objectB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver);
+	btGjkPairDetector(const btConvexShape* objectA, const btConvexShape* objectB, int shapeTypeA, int shapeTypeB, btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver);
+	virtual ~btGjkPairDetector(){};
 
-	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
+	virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false);
 
-	void	getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw);
-	
+	void getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw);
 
 	void setMinkowskiA(const btConvexShape* minkA)
 	{
@@ -72,32 +64,30 @@ public:
 	{
 		m_minkowskiB = minkB;
 	}
-	void setCachedSeperatingAxis(const btVector3& seperatingAxis)
+	void setCachedSeparatingAxis(const btVector3& separatingAxis)
 	{
-		m_cachedSeparatingAxis = seperatingAxis;
+		m_cachedSeparatingAxis = separatingAxis;
 	}
 
 	const btVector3& getCachedSeparatingAxis() const
 	{
 		return m_cachedSeparatingAxis;
 	}
-	btScalar	getCachedSeparatingDistance() const
+	btScalar getCachedSeparatingDistance() const
 	{
 		return m_cachedSeparatingDistance;
 	}
 
-	void	setPenetrationDepthSolver(btConvexPenetrationDepthSolver*	penetrationDepthSolver)
+	void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver)
 	{
 		m_penetrationDepthSolver = penetrationDepthSolver;
 	}
 
 	///don't use setIgnoreMargin, it's for Bullet's internal use
-	void	setIgnoreMargin(bool ignoreMargin)
+	void setIgnoreMargin(bool ignoreMargin)
 	{
 		m_ignoreMargin = ignoreMargin;
 	}
-
-
 };
 
-#endif //BT_GJK_PAIR_DETECTOR_H
+#endif  //BT_GJK_PAIR_DETECTOR_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
index e40fb1d3db6..573fc86bf97 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
@@ -20,137 +20,155 @@ subject to the following restrictions:
 #include "LinearMath/btTransformUtil.h"
 
 #ifdef PFX_USE_FREE_VECTORMATH
-	#include "physics_effects/base_level/solver/pfx_constraint_row.h"
+#include "physics_effects/base_level/solver/pfx_constraint_row.h"
 typedef sce::PhysicsEffects::PfxConstraintRow btConstraintRow;
 #else
-	// Don't change following order of parameters
-	ATTRIBUTE_ALIGNED16(struct) btConstraintRow {
-		btScalar m_normal[3];
-		btScalar m_rhs;
-		btScalar m_jacDiagInv;
-		btScalar m_lowerLimit;
-		btScalar m_upperLimit;
-		btScalar m_accumImpulse;
-	};
-	typedef btConstraintRow PfxConstraintRow;
-#endif //PFX_USE_FREE_VECTORMATH
-
-
+// Don't change following order of parameters
+ATTRIBUTE_ALIGNED16(struct)
+btConstraintRow
+{
+	btScalar m_normal[3];
+	btScalar m_rhs;
+	btScalar m_jacDiagInv;
+	btScalar m_lowerLimit;
+	btScalar m_upperLimit;
+	btScalar m_accumImpulse;
+};
+typedef btConstraintRow PfxConstraintRow;
+#endif  //PFX_USE_FREE_VECTORMATH
+
+enum btContactPointFlags
+{
+	BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED = 1,
+	BT_CONTACT_FLAG_HAS_CONTACT_CFM = 2,
+	BT_CONTACT_FLAG_HAS_CONTACT_ERP = 4,
+	BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8,
+	BT_CONTACT_FLAG_FRICTION_ANCHOR = 16,
+};
 
 /// ManifoldContactPoint collects and maintains persistent contactpoints.
 /// used to improve stability and performance of rigidbody dynamics response.
 class btManifoldPoint
+{
+public:
+	btManifoldPoint()
+		: m_userPersistentData(0),
+		  m_contactPointFlags(0),
+		  m_appliedImpulse(0.f),
+		  m_prevRHS(0.f),
+		  m_appliedImpulseLateral1(0.f),
+		  m_appliedImpulseLateral2(0.f),
+		  m_contactMotion1(0.f),
+		  m_contactMotion2(0.f),
+		  m_contactCFM(0.f),
+		  m_contactERP(0.f),
+		  m_frictionCFM(0.f),
+		  m_lifeTime(0)
 	{
-		public:
-			btManifoldPoint()
-				:m_userPersistentData(0),
-				m_lateralFrictionInitialized(false),
-                m_appliedImpulse(0.f),
-                m_appliedImpulseLateral1(0.f),
-				m_appliedImpulseLateral2(0.f),
-				m_contactMotion1(0.f),
-				m_contactMotion2(0.f),
-				m_contactCFM1(0.f),
-				m_contactCFM2(0.f),
-				m_lifeTime(0)
-			{
-			}
-
-			btManifoldPoint( const btVector3 &pointA, const btVector3 &pointB, 
-					const btVector3 &normal, 
-					btScalar distance ) :
-					m_localPointA( pointA ), 
-					m_localPointB( pointB ), 
-					m_normalWorldOnB( normal ), 
-					m_distance1( distance ),
-					m_combinedFriction(btScalar(0.)),
-					m_combinedRollingFriction(btScalar(0.)),
-					m_combinedRestitution(btScalar(0.)),
-					m_userPersistentData(0),
-					m_lateralFrictionInitialized(false),
-                    m_appliedImpulse(0.f),
-                    m_appliedImpulseLateral1(0.f),
-					m_appliedImpulseLateral2(0.f),
-					m_contactMotion1(0.f),
-					m_contactMotion2(0.f),
-					m_contactCFM1(0.f),
-					m_contactCFM2(0.f),
-					m_lifeTime(0)
-			{
-				
-			}
-
-			
-
-			btVector3 m_localPointA;			
-			btVector3 m_localPointB;			
-			btVector3	m_positionWorldOnB;
-			///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity
-			btVector3	m_positionWorldOnA;
-			btVector3 m_normalWorldOnB;
-		
-			btScalar	m_distance1;
-			btScalar	m_combinedFriction;
-			btScalar	m_combinedRollingFriction;
-			btScalar	m_combinedRestitution;
-
-			//BP mod, store contact triangles.
-			int			m_partId0;
-			int			m_partId1;
-			int			m_index0;
-			int			m_index1;
-				
-			mutable void*	m_userPersistentData;
-			bool			m_lateralFrictionInitialized;
-
-			btScalar		m_appliedImpulse;
-			btScalar		m_appliedImpulseLateral1;
-			btScalar		m_appliedImpulseLateral2;
-			btScalar		m_contactMotion1;
-			btScalar		m_contactMotion2;
-			btScalar		m_contactCFM1;
-			btScalar		m_contactCFM2;
-
-			int				m_lifeTime;//lifetime of the contactpoint in frames
-			
-			btVector3		m_lateralFrictionDir1;
-			btVector3		m_lateralFrictionDir2;
-
-
-
-
-			btScalar getDistance() const
-			{
-				return m_distance1;
-			}
-			int	getLifeTime() const
-			{
-				return m_lifeTime;
-			}
-
-			const btVector3& getPositionWorldOnA() const {
-				return m_positionWorldOnA;
-//				return m_positionWorldOnB + m_normalWorldOnB * m_distance1;
-			}
-
-			const btVector3& getPositionWorldOnB() const
-			{
-				return m_positionWorldOnB;
-			}
-
-			void	setDistance(btScalar dist)
-			{
-				m_distance1 = dist;
-			}
-			
-			///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver
-			btScalar	getAppliedImpulse() const
-			{
-				return m_appliedImpulse;
-			}
-
-			
+	}
+
+	btManifoldPoint(const btVector3& pointA, const btVector3& pointB,
+					const btVector3& normal,
+					btScalar distance) : m_localPointA(pointA),
+										 m_localPointB(pointB),
+										 m_normalWorldOnB(normal),
+										 m_distance1(distance),
+										 m_combinedFriction(btScalar(0.)),
+										 m_combinedRollingFriction(btScalar(0.)),
+										 m_combinedSpinningFriction(btScalar(0.)),
+										 m_combinedRestitution(btScalar(0.)),
+										 m_userPersistentData(0),
+										 m_contactPointFlags(0),
+										 m_appliedImpulse(0.f),
+										 m_prevRHS(0.f),
+										 m_appliedImpulseLateral1(0.f),
+										 m_appliedImpulseLateral2(0.f),
+										 m_contactMotion1(0.f),
+										 m_contactMotion2(0.f),
+										 m_contactCFM(0.f),
+										 m_contactERP(0.f),
+										 m_frictionCFM(0.f),
+										 m_lifeTime(0)
+	{
+	}
+
+	btVector3 m_localPointA;
+	btVector3 m_localPointB;
+	btVector3 m_positionWorldOnB;
+	///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity
+	btVector3 m_positionWorldOnA;
+	btVector3 m_normalWorldOnB;
+
+	btScalar m_distance1;
+	btScalar m_combinedFriction;
+	btScalar m_combinedRollingFriction;   //torsional friction orthogonal to contact normal, useful to make spheres stop rolling forever
+	btScalar m_combinedSpinningFriction;  //torsional friction around contact normal, useful for grasping objects
+	btScalar m_combinedRestitution;
+
+	//BP mod, store contact triangles.
+	int m_partId0;
+	int m_partId1;
+	int m_index0;
+	int m_index1;
+
+	mutable void* m_userPersistentData;
+	//bool			m_lateralFrictionInitialized;
+	int m_contactPointFlags;
+
+	btScalar m_appliedImpulse;
+	btScalar m_prevRHS;
+	btScalar m_appliedImpulseLateral1;
+	btScalar m_appliedImpulseLateral2;
+	btScalar m_contactMotion1;
+	btScalar m_contactMotion2;
+
+	union {
+		btScalar m_contactCFM;
+		btScalar m_combinedContactStiffness1;
+	};
 
+	union {
+		btScalar m_contactERP;
+		btScalar m_combinedContactDamping1;
 	};
 
-#endif //BT_MANIFOLD_CONTACT_POINT_H
+	btScalar m_frictionCFM;
+
+	int m_lifeTime;  //lifetime of the contactpoint in frames
+
+	btVector3 m_lateralFrictionDir1;
+	btVector3 m_lateralFrictionDir2;
+
+	btScalar getDistance() const
+	{
+		return m_distance1;
+	}
+	int getLifeTime() const
+	{
+		return m_lifeTime;
+	}
+
+	const btVector3& getPositionWorldOnA() const
+	{
+		return m_positionWorldOnA;
+		//				return m_positionWorldOnB + m_normalWorldOnB * m_distance1;
+	}
+
+	const btVector3& getPositionWorldOnB() const
+	{
+		return m_positionWorldOnB;
+	}
+
+	void setDistance(btScalar dist)
+	{
+		m_distance1 = dist;
+	}
+
+	///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver
+	btScalar getAppliedImpulse() const
+	{
+		return m_appliedImpulse;
+	}
+};
+
+#endif  //BT_MANIFOLD_CONTACT_POINT_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
index fa45f49037e..c042c242080 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
@@ -21,42 +21,38 @@ subject to the following restrictions:
 
 #define NUM_UNITSPHERE_POINTS 42
 
-
 bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
-												   const btConvexShape* convexA,const btConvexShape* convexB,
-												   const btTransform& transA,const btTransform& transB,
-												   btVector3& v, btVector3& pa, btVector3& pb,
-												   class btIDebugDraw* debugDraw
-												   )
+													 const btConvexShape* convexA, const btConvexShape* convexB,
+													 const btTransform& transA, const btTransform& transB,
+													 btVector3& v, btVector3& pa, btVector3& pb,
+													 class btIDebugDraw* debugDraw)
 {
-
 	(void)v;
-	
-	bool check2d= convexA->isConvex2d() && convexB->isConvex2d();
+
+	bool check2d = convexA->isConvex2d() && convexB->isConvex2d();
 
 	struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
 	{
-
-		btIntermediateResult():m_hasResult(false)
+		btIntermediateResult() : m_hasResult(false)
 		{
 		}
-		
+
 		btVector3 m_normalOnBInWorld;
 		btVector3 m_pointInWorld;
 		btScalar m_depth;
-		bool	m_hasResult;
+		bool m_hasResult;
 
-		virtual void setShapeIdentifiersA(int partId0,int index0)
+		virtual void setShapeIdentifiersA(int partId0, int index0)
 		{
 			(void)partId0;
 			(void)index0;
 		}
-		virtual void setShapeIdentifiersB(int partId1,int index1)
+		virtual void setShapeIdentifiersB(int partId1, int index1)
 		{
 			(void)partId1;
 			(void)index1;
 		}
-		void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+		void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
 		{
 			m_normalOnBInWorld = normalOnBInWorld;
 			m_pointInWorld = pointInWorld;
@@ -68,42 +64,42 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 	//just take fixed number of orientation, and sample the penetration depth in that direction
 	btScalar minProj = btScalar(BT_LARGE_FLOAT);
 	btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
-	btVector3 minA,minB;
-	btVector3 seperatingAxisInA,seperatingAxisInB;
-	btVector3 pInA,qInB,pWorld,qWorld,w;
+	btVector3 minA, minB;
+	btVector3 separatingAxisInA, separatingAxisInB;
+	btVector3 pInA, qInB, pWorld, qWorld, w;
 
 #ifndef __SPU__
 #define USE_BATCHED_SUPPORT 1
 #endif
 #ifdef USE_BATCHED_SUPPORT
 
-	btVector3	supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
-	btVector3	supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
-	btVector3	seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
-	btVector3	seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
+	btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
+	btVector3 separatingAxisInABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
+	btVector3 separatingAxisInBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
 	int i;
 
 	int numSampleDirections = NUM_UNITSPHERE_POINTS;
 
-	for (i=0;i<numSampleDirections;i++)
+	for (i = 0; i < numSampleDirections; i++)
 	{
 		btVector3 norm = getPenetrationDirections()[i];
-		seperatingAxisInABatch[i] =  (-norm) * transA.getBasis() ;
-		seperatingAxisInBBatch[i] =  norm   * transB.getBasis() ;
+		separatingAxisInABatch[i] = (-norm) * transA.getBasis();
+		separatingAxisInBBatch[i] = norm * transB.getBasis();
 	}
 
 	{
 		int numPDA = convexA->getNumPreferredPenetrationDirections();
 		if (numPDA)
 		{
-			for (int i=0;i<numPDA;i++)
+			for (int i = 0; i < numPDA; i++)
 			{
 				btVector3 norm;
-				convexA->getPreferredPenetrationDirection(i,norm);
-				norm  = transA.getBasis() * norm;
+				convexA->getPreferredPenetrationDirection(i, norm);
+				norm = transA.getBasis() * norm;
 				getPenetrationDirections()[numSampleDirections] = norm;
-				seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
-				seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
+				separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
+				separatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
 				numSampleDirections++;
 			}
 		}
@@ -113,42 +109,38 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 		int numPDB = convexB->getNumPreferredPenetrationDirections();
 		if (numPDB)
 		{
-			for (int i=0;i<numPDB;i++)
+			for (int i = 0; i < numPDB; i++)
 			{
 				btVector3 norm;
-				convexB->getPreferredPenetrationDirection(i,norm);
-				norm  = transB.getBasis() * norm;
+				convexB->getPreferredPenetrationDirection(i, norm);
+				norm = transB.getBasis() * norm;
 				getPenetrationDirections()[numSampleDirections] = norm;
-				seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
-				seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
+				separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
+				separatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
 				numSampleDirections++;
 			}
 		}
 	}
 
+	convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(separatingAxisInABatch, supportVerticesABatch, numSampleDirections);
+	convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(separatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
 
-
-
-	convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
-	convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
-
-	for (i=0;i<numSampleDirections;i++)
+	for (i = 0; i < numSampleDirections; i++)
 	{
 		btVector3 norm = getPenetrationDirections()[i];
 		if (check2d)
 		{
 			norm[2] = 0.f;
 		}
-		if (norm.length2()>0.01)
+		if (norm.length2() > 0.01)
 		{
-
-			seperatingAxisInA = seperatingAxisInABatch[i];
-			seperatingAxisInB = seperatingAxisInBBatch[i];
+			separatingAxisInA = separatingAxisInABatch[i];
+			separatingAxisInB = separatingAxisInBBatch[i];
 
 			pInA = supportVerticesABatch[i];
 			qInB = supportVerticesBBatch[i];
 
-			pWorld = transA(pInA);	
+			pWorld = transA(pInA);
 			qWorld = transB(qInB);
 			if (check2d)
 			{
@@ -156,7 +148,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 				qWorld[2] = 0.f;
 			}
 
-			w	= qWorld - pWorld;
+			w = qWorld - pWorld;
 			btScalar delta = norm.dot(w);
 			//find smallest delta
 			if (delta < minProj)
@@ -167,7 +159,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 				minB = qWorld;
 			}
 		}
-	}	
+	}
 #else
 
 	int numSampleDirections = NUM_UNITSPHERE_POINTS;
@@ -177,11 +169,11 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 		int numPDA = convexA->getNumPreferredPenetrationDirections();
 		if (numPDA)
 		{
-			for (int i=0;i<numPDA;i++)
+			for (int i = 0; i < numPDA; i++)
 			{
 				btVector3 norm;
-				convexA->getPreferredPenetrationDirection(i,norm);
-				norm  = transA.getBasis() * norm;
+				convexA->getPreferredPenetrationDirection(i, norm);
+				norm = transA.getBasis() * norm;
 				getPenetrationDirections()[numSampleDirections] = norm;
 				numSampleDirections++;
 			}
@@ -192,28 +184,28 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 		int numPDB = convexB->getNumPreferredPenetrationDirections();
 		if (numPDB)
 		{
-			for (int i=0;i<numPDB;i++)
+			for (int i = 0; i < numPDB; i++)
 			{
 				btVector3 norm;
-				convexB->getPreferredPenetrationDirection(i,norm);
-				norm  = transB.getBasis() * norm;
+				convexB->getPreferredPenetrationDirection(i, norm);
+				norm = transB.getBasis() * norm;
 				getPenetrationDirections()[numSampleDirections] = norm;
 				numSampleDirections++;
 			}
 		}
 	}
-#endif // __SPU__
+#endif  // __SPU__
 
-	for (int i=0;i<numSampleDirections;i++)
+	for (int i = 0; i < numSampleDirections; i++)
 	{
 		const btVector3& norm = getPenetrationDirections()[i];
-		seperatingAxisInA = (-norm)* transA.getBasis();
-		seperatingAxisInB = norm* transB.getBasis();
-		pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
-		qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
-		pWorld = transA(pInA);	
+		separatingAxisInA = (-norm) * transA.getBasis();
+		separatingAxisInB = norm * transB.getBasis();
+		pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
+		qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
+		pWorld = transA(pInA);
 		qWorld = transB(qInB);
-		w	= qWorld - pWorld;
+		w = qWorld - pWorld;
 		btScalar delta = norm.dot(w);
 		//find smallest delta
 		if (delta < minProj)
@@ -224,48 +216,39 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 			minB = qWorld;
 		}
 	}
-#endif //USE_BATCHED_SUPPORT
+#endif  //USE_BATCHED_SUPPORT
 
 	//add the margins
 
-	minA += minNorm*convexA->getMarginNonVirtual();
-	minB -= minNorm*convexB->getMarginNonVirtual();
+	minA += minNorm * convexA->getMarginNonVirtual();
+	minB -= minNorm * convexB->getMarginNonVirtual();
 	//no penetration
 	if (minProj < btScalar(0.))
 		return false;
 
-	btScalar extraSeparation = 0.5f;///scale dependent
-	minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
-
-
-
-
+	btScalar extraSeparation = 0.5f;  ///scale dependent
+	minProj += extraSeparation + (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
 
 //#define DEBUG_DRAW 1
 #ifdef DEBUG_DRAW
 	if (debugDraw)
 	{
-		btVector3 color(0,1,0);
-		debugDraw->drawLine(minA,minB,color);
-		color = btVector3 (1,1,1);
-		btVector3 vec = minB-minA;
+		btVector3 color(0, 1, 0);
+		debugDraw->drawLine(minA, minB, color);
+		color = btVector3(1, 1, 1);
+		btVector3 vec = minB - minA;
 		btScalar prj2 = minNorm.dot(vec);
-		debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
-
+		debugDraw->drawLine(minA, minA + (minNorm * minProj), color);
 	}
-#endif //DEBUG_DRAW
+#endif  //DEBUG_DRAW
 
-	
-
-	btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
+	btGjkPairDetector gjkdet(convexA, convexB, &simplexSolver, 0);
 
 	btScalar offsetDist = minProj;
 	btVector3 offset = minNorm * offsetDist;
-	
-
 
 	btGjkPairDetector::ClosestPointInput input;
-		
+
 	btVector3 newOrg = transA.getOrigin() + offset;
 
 	btTransform displacedTrans = transA;
@@ -273,89 +256,81 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 
 	input.m_transformA = displacedTrans;
 	input.m_transformB = transB;
-	input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
-	
+	input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);  //minProj;
+
 	btIntermediateResult res;
-	gjkdet.setCachedSeperatingAxis(-minNorm);
-	gjkdet.getClosestPoints(input,res,debugDraw);
+	gjkdet.setCachedSeparatingAxis(-minNorm);
+	gjkdet.getClosestPoints(input, res, debugDraw);
 
 	btScalar correctedMinNorm = minProj - res.m_depth;
 
-
 	//the penetration depth is over-estimated, relax it
-	btScalar penetration_relaxation= btScalar(1.);
-	minNorm*=penetration_relaxation;
-	
+	btScalar penetration_relaxation = btScalar(1.);
+	minNorm *= penetration_relaxation;
 
 	if (res.m_hasResult)
 	{
-
 		pa = res.m_pointInWorld - minNorm * correctedMinNorm;
 		pb = res.m_pointInWorld;
 		v = minNorm;
-		
+
 #ifdef DEBUG_DRAW
 		if (debugDraw)
 		{
-			btVector3 color(1,0,0);
-			debugDraw->drawLine(pa,pb,color);
+			btVector3 color(1, 0, 0);
+			debugDraw->drawLine(pa, pb, color);
 		}
-#endif//DEBUG_DRAW
-
-
+#endif  //DEBUG_DRAW
 	}
 	return res.m_hasResult;
 }
 
-btVector3*	btMinkowskiPenetrationDepthSolver::getPenetrationDirections()
+btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections()
 {
-	static btVector3	sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
-	{
-	btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
-	btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
-	btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
-	btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
-	btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
-	btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
-	btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
-	btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
-	btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
-	btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
-	btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
-	btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
-	btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
-	btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
-	btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
-	btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
-	btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
-	btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
-	btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
-	btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
-	btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
-	btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
-	btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
-	btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
-	btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
-	btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
-	btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
-	btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
-	btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
-	btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
-	btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
-	btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
-	btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
-	btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
-	btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
-	btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
-	btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
-	btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
-	btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
-	btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
-	btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
-	btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
-	};
+	static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] =
+		{
+			btVector3(btScalar(0.000000), btScalar(-0.000000), btScalar(-1.000000)),
+			btVector3(btScalar(0.723608), btScalar(-0.525725), btScalar(-0.447219)),
+			btVector3(btScalar(-0.276388), btScalar(-0.850649), btScalar(-0.447219)),
+			btVector3(btScalar(-0.894426), btScalar(-0.000000), btScalar(-0.447216)),
+			btVector3(btScalar(-0.276388), btScalar(0.850649), btScalar(-0.447220)),
+			btVector3(btScalar(0.723608), btScalar(0.525725), btScalar(-0.447219)),
+			btVector3(btScalar(0.276388), btScalar(-0.850649), btScalar(0.447220)),
+			btVector3(btScalar(-0.723608), btScalar(-0.525725), btScalar(0.447219)),
+			btVector3(btScalar(-0.723608), btScalar(0.525725), btScalar(0.447219)),
+			btVector3(btScalar(0.276388), btScalar(0.850649), btScalar(0.447219)),
+			btVector3(btScalar(0.894426), btScalar(0.000000), btScalar(0.447216)),
+			btVector3(btScalar(-0.000000), btScalar(0.000000), btScalar(1.000000)),
+			btVector3(btScalar(0.425323), btScalar(-0.309011), btScalar(-0.850654)),
+			btVector3(btScalar(-0.162456), btScalar(-0.499995), btScalar(-0.850654)),
+			btVector3(btScalar(0.262869), btScalar(-0.809012), btScalar(-0.525738)),
+			btVector3(btScalar(0.425323), btScalar(0.309011), btScalar(-0.850654)),
+			btVector3(btScalar(0.850648), btScalar(-0.000000), btScalar(-0.525736)),
+			btVector3(btScalar(-0.525730), btScalar(-0.000000), btScalar(-0.850652)),
+			btVector3(btScalar(-0.688190), btScalar(-0.499997), btScalar(-0.525736)),
+			btVector3(btScalar(-0.162456), btScalar(0.499995), btScalar(-0.850654)),
+			btVector3(btScalar(-0.688190), btScalar(0.499997), btScalar(-0.525736)),
+			btVector3(btScalar(0.262869), btScalar(0.809012), btScalar(-0.525738)),
+			btVector3(btScalar(0.951058), btScalar(0.309013), btScalar(0.000000)),
+			btVector3(btScalar(0.951058), btScalar(-0.309013), btScalar(0.000000)),
+			btVector3(btScalar(0.587786), btScalar(-0.809017), btScalar(0.000000)),
+			btVector3(btScalar(0.000000), btScalar(-1.000000), btScalar(0.000000)),
+			btVector3(btScalar(-0.587786), btScalar(-0.809017), btScalar(0.000000)),
+			btVector3(btScalar(-0.951058), btScalar(-0.309013), btScalar(-0.000000)),
+			btVector3(btScalar(-0.951058), btScalar(0.309013), btScalar(-0.000000)),
+			btVector3(btScalar(-0.587786), btScalar(0.809017), btScalar(-0.000000)),
+			btVector3(btScalar(-0.000000), btScalar(1.000000), btScalar(-0.000000)),
+			btVector3(btScalar(0.587786), btScalar(0.809017), btScalar(-0.000000)),
+			btVector3(btScalar(0.688190), btScalar(-0.499997), btScalar(0.525736)),
+			btVector3(btScalar(-0.262869), btScalar(-0.809012), btScalar(0.525738)),
+			btVector3(btScalar(-0.850648), btScalar(0.000000), btScalar(0.525736)),
+			btVector3(btScalar(-0.262869), btScalar(0.809012), btScalar(0.525738)),
+			btVector3(btScalar(0.688190), btScalar(0.499997), btScalar(0.525736)),
+			btVector3(btScalar(0.525730), btScalar(0.000000), btScalar(0.850652)),
+			btVector3(btScalar(0.162456), btScalar(-0.499995), btScalar(0.850654)),
+			btVector3(btScalar(-0.425323), btScalar(-0.309011), btScalar(0.850654)),
+			btVector3(btScalar(-0.425323), btScalar(0.309011), btScalar(0.850654)),
+			btVector3(btScalar(0.162456), btScalar(0.499995), btScalar(0.850654))};
 
 	return sPenetrationDirections;
 }
-
-
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
index fd533b4fc33..8e3e3932591 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
@@ -23,18 +23,14 @@ subject to the following restrictions:
 class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver
 {
 protected:
-
-	static btVector3*	getPenetrationDirections();
+	static btVector3* getPenetrationDirections();
 
 public:
-
-	virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
-	const btConvexShape* convexA,const btConvexShape* convexB,
-				const btTransform& transA,const btTransform& transB,
-			btVector3& v, btVector3& pa, btVector3& pb,
-			class btIDebugDraw* debugDraw
-			);
+	virtual bool calcPenDepth(btSimplexSolverInterface& simplexSolver,
+							  const btConvexShape* convexA, const btConvexShape* convexB,
+							  const btTransform& transA, const btTransform& transB,
+							  btVector3& v, btVector3& pa, btVector3& pb,
+							  class btIDebugDraw* debugDraw);
 };
 
-#endif //BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
-
+#endif  //BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h
index a22a0bae66b..534a66d3fa3 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h
@@ -28,32 +28,31 @@
 
 //#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()
-    {
-    }
+	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;
+	btVector3 m_pointOnA;
+	btVector3 m_pointOnB;
+	btVector3 m_normalBtoA;
+	btScalar m_distance;
 };
 
 #ifdef __cplusplus
@@ -67,131 +66,112 @@ struct btMprDistanceInfo
 #define BT_MPR_TOLERANCE 1E-6f
 #define BT_MPR_MAX_ITERATIONS 1000
 
-struct _btMprSupport_t 
+struct _btMprSupport_t
 {
-    btVector3 v;  //!< Support point in minkowski sum
-    btVector3 v1; //!< Support point in obj1
-    btVector3 v2; //!< Support point in obj2
+	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 
+struct _btMprSimplex_t
 {
-    btMprSupport_t ps[4];
-    int last; //!< index of last added point
+	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)
+inline btMprSupport_t *btMprSimplexPointW(btMprSimplex_t *s, int idx)
 {
-    return &s->ps[idx];
+	return &s->ps[idx];
 }
 
 inline void btMprSimplexSetSize(btMprSimplex_t *s, int size)
 {
-    s->last = size - 1;
+	s->last = size - 1;
 }
 
 #ifdef DEBUG_MPR
-inline void btPrintPortalVertex(_btMprSimplex_t* portal, int index)
+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());
+	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
-
-
-
+#endif  //DEBUG_MPR
 
 inline int btMprSimplexSize(const btMprSimplex_t *s)
 {
-    return s->last + 1;
+	return s->last + 1;
 }
 
-
-inline const btMprSupport_t* btMprSimplexPoint(const btMprSimplex_t* s, int idx)
+inline const btMprSupport_t *btMprSimplexPoint(const btMprSimplex_t *s, int idx)
 {
-    // here is no check on boundaries
-    return &s->ps[idx];
+	// here is no check on boundaries
+	return &s->ps[idx];
 }
 
 inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s)
 {
-    *d = *s;
+	*d = *s;
 }
 
 inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a)
 {
-    btMprSupportCopy(s->ps + pos, a);
+	btMprSupportCopy(s->ps + pos, a);
 }
 
-
 inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2)
 {
-    btMprSupport_t supp;
+	btMprSupport_t supp;
 
-    btMprSupportCopy(&supp, &s->ps[pos1]);
-    btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
-    btMprSupportCopy(&s->ps[pos2], &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;
+	return BT_MPR_FABS(val) < FLT_EPSILON;
 }
 
-
-
 inline int btMprEq(float _a, float _b)
 {
-    float ab;
-    float a, b;
+	float ab;
+	float a, b;
 
-    ab = BT_MPR_FABS(_a - _b);
-    if (BT_MPR_FABS(ab) < FLT_EPSILON)
-        return 1;
+	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;
-    }
+	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)
+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());
+	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)
+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;
+	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);
+	v->setValue(x, y, z);
 }
 
 inline void btMprVec3Add(btVector3 *v, const btVector3 *w)
@@ -201,41 +181,38 @@ inline void btMprVec3Add(btVector3 *v, const btVector3 *w)
 
 inline void btMprVec3Copy(btVector3 *v, const btVector3 *w)
 {
-    *v = *w;
+	*v = *w;
 }
 
 inline void btMprVec3Scale(btVector3 *d, float k)
 {
-    *d *= k;
+	*d *= k;
 }
 
 inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b)
 {
-    float dot;
+	float dot;
 
-	dot = btDot(*a,*b);
-    return dot;
+	dot = btDot(*a, *b);
+	return dot;
 }
 
-
 inline float btMprVec3Len2(const btVector3 *v)
 {
-    return btMprVec3Dot(v, v);
+	return btMprVec3Dot(v, v);
 }
 
 inline void btMprVec3Normalize(btVector3 *d)
 {
-    float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d));
-    btMprVec3Scale(d, k);
+	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);
-	
+	*d = btCross(*a, *b);
 }
 
-
 inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
 {
 	*d = *v - *w;
@@ -243,666 +220,665 @@ inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
 
 inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir)
 {
-    btVector3 v2v1, v3v1;
+	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);
+	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)
+								  const btVector3 *dir)
 {
-    float dot;
-    dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v);
-    return btMprIsZero(dot) || dot > 0.f;
+	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)
+								const btMprSupport_t *v4,
+								const btVector3 *dir)
 {
-    float dv1, dv2, dv3, dv4;
-    float dot1, dot2, dot3;
+	float dv1, dv2, dv3, dv4;
+	float dot1, dot2, dot3;
 
-    // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
+	// 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);
+	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 = dv4 - dv1;
+	dot2 = dv4 - dv2;
+	dot3 = dv4 - dv3;
 
-    dot1 = BT_MPR_FMIN(dot1, dot2);
-    dot1 = BT_MPR_FMIN(dot1, dot3);
+	dot1 = BT_MPR_FMIN(dot1, dot2);
+	dot1 = BT_MPR_FMIN(dot1, dot3);
 
-    return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE;
+	return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE;
 }
 
 inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal,
-                                         const btMprSupport_t *v4,
-                                         const btVector3 *dir)
+									const btMprSupport_t *v4,
+									const btVector3 *dir)
 {
-    float dot;
-    dot = btMprVec3Dot(&v4->v, dir);
-    return btMprIsZero(dot) || dot > 0.f;
+	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);
-        }
-    }
+						   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)
+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 separatingAxisInA = dir * a.getWorldTransform().getBasis();
+	btVector3 separatingAxisInB = -dir * b.getWorldTransform().getBasis();
 
-	btVector3 pInA = a.getLocalSupportWithMargin(seperatingAxisInA);
-	btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB);
+	btVector3 pInA = a.getLocalSupportWithMargin(separatingAxisInA);
+	btVector3 qInB = b.getLocalSupportWithMargin(separatingAxisInB);
 
 	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;
+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)
+static int btRefinePortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc,
+						  btMprSimplex_t *portal)
 {
-    btVector3 dir;
-    btMprSupport_t v4;
+	btVector3 dir;
+	btMprSupport_t v4;
 
-	for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++)
-    //while (1)
+	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);
+		// compute direction outside the portal (from v0 through v1,v2,v3
+		// face)
+		btPortalDir(portal, &dir);
 
-        // test if origin is inside the portal
-        if (portalEncapsulesOrigin(portal, &dir))
-            return 0;
+		// test if origin is inside the portal
+		if (portalEncapsulesOrigin(portal, &dir))
+			return 0;
 
-        // get next support point
-        
-		 btMprSupport(a,b,colDesc, dir, &v4);
+		// 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))
+		// 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;
-        }
+			return -1;
+		}
 
-        // v1-v2-v3 triangle must be rearranged to face outside Minkowski
-        // difference (direction from v0).
-        btExpandPortal(portal, &v4);
-    }
+		// v1-v2-v3 triangle must be rearranged to face outside Minkowski
+		// difference (direction from v0).
+		btExpandPortal(portal, &v4);
+	}
 
-    return -1;
+	return -1;
 }
 
 static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos)
 {
+	btVector3 zero = btVector3(0, 0, 0);
+	btVector3 *origin = &zero;
 
-	btVector3 zero = btVector3(0,0,0);
-	btVector3* origin = &zero;
-
-    btVector3 dir;
-    size_t i;
-    float b[4], sum, inv;
-    btVector3 vec, p1, p2;
+	btVector3 dir;
+	size_t i;
+	float b[4], sum, inv;
+	btVector3 vec, p1, p2;
 
-    btPortalDir(portal, &dir);
+	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);
+	// 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, 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, 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);
+	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){
+	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);
+		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);
+	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);
+	btMprVec3Copy(pos, &p1);
+	btMprVec3Add(pos, &p2);
+	btMprVec3Scale(pos, 0.5);
 #else
-    btMprVec3Copy(pos, &p2);
-#endif//MPR_AVERAGE_CONTACT_POSITIONS
+	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);
+	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;
-}
-
+										 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);
+									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);
 
 	btScalar div = (w * v - r * r);
 	if (btMprIsZero(div))
 	{
-		s=-1;
-	} else
+		s = -1;
+	}
+	else
 	{
 		s = (q * r - w * p) / div;
 		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;
+	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)
+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 dir;
+	btMprSupport_t v4;
+	unsigned long iterations;
 
-	btVector3 zero = btVector3(0,0,0);
-	btVector3* origin = &zero;
+	btVector3 zero = btVector3(0, 0, 0);
+	btVector3 *origin = &zero;
 
-
-    iterations = 1UL;
-	for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++)
-    //while (1)
+	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);
+		// 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)
+		// 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);
-			
+			*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);
 
+			// barycentric coordinates:
+			btFindPos(portal, pos);
 
-            return;
-        }
+			return;
+		}
 
-        btExpandPortal(portal, &v4);
+		btExpandPortal(portal, &v4);
 
-        iterations++;
-    }
+		iterations++;
+	}
 }
 
-static void btFindPenetrTouch(btMprSimplex_t *portal,float *depth, btVector3 *dir, btVector3 *pos)
+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;
-
+	// 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);
+	btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
+	btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
+	btMprVec3Scale(pos, 0.5);
 #else
-     btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
+	btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
 #endif
 }
 
 static void btFindPenetrSegment(btMprSimplex_t *portal,
-                              float *depth, btVector3 *dir, btVector3 *pos)
+								float *depth, btVector3 *dir, btVector3 *pos)
 {
-    
-    // Origin lies on v0-v1 segment.
-    // Depth is distance to v1, direction also and position must be
-    // computed
+	// 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);
+	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);
+	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)
+inline int btMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b,
+							const btMprCollisionDescription &colDesc,
+							float *depthOut, btVector3 *dirOut, btVector3 *posOut)
 {
-	
-	 btMprSimplex_t portal;
+	btMprSimplex_t portal;
 
+	// Phase 1: Portal discovery
+	int result = btDiscoverPortal(a, b, colDesc, &portal);
 
-    // Phase 1: Portal discovery
-    int result = btDiscoverPortal(a,b,colDesc, &portal);
-	
-	  
 	//sepAxis[pairIndex] = *pdir;//or -dir?
 
 	switch (result)
 	{
-	case 0:
+		case 0:
 		{
 			// Phase 2: Portal refinement
-		
-			result = btRefinePortal(a,b,colDesc, &portal);
+
+			result = btRefinePortal(a, b, colDesc, &portal);
 			if (result < 0)
 				return -1;
 
 			// Phase 3. Penetration info
-			btFindPenetr(a,b,colDesc, &portal, depthOut, dirOut, posOut);
-			
-			
+			btFindPenetr(a, b, colDesc, &portal, depthOut, dirOut, posOut);
+
 			break;
 		}
-	case 1:
+		case 1:
 		{
-			 // Touching contact on portal's v1.
+			// Touching contact on portal's v1.
 			btFindPenetrTouch(&portal, depthOut, dirOut, posOut);
-			result=0;
+			result = 0;
 			break;
 		}
-	case 2:
+		case 2:
 		{
-			
-			btFindPenetrSegment( &portal, depthOut, dirOut, posOut);
-			result=0;
+			btFindPenetrSegment(&portal, depthOut, dirOut, posOut);
+			result = 0;
 			break;
 		}
-	default:
+		default:
 		{
 			//if (res < 0)
 			//{
-				// Origin isn't inside portal - no collision.
-				result = -1;
+			// 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)
+template <typename btConvexTemplate, typename btMprDistanceTemplate>
+inline int btComputeMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprDistanceTemplate *distInfo)
 {
-	btVector3 dir,pos;
+	btVector3 dir, pos;
 	float depth;
 
-	int res = btMprPenetration(a,b,colDesc,&depth, &dir, &pos);
-	if (res==0)
+	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;
+		distInfo->m_pointOnA = pos - distInfo->m_distance * dir;
 		return 0;
 	}
 
 	return -1;
 }
 
-
-
-#endif //BT_MPR_PENETRATION_H
+#endif  //BT_MPR_PENETRATION_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
index 4d92e853d3f..dca3e09267d 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
@@ -13,84 +13,86 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btPersistentManifold.h"
 #include "LinearMath/btTransform.h"
+#include "LinearMath/btSerializer.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btCollisionObjectData btCollisionObjectDoubleData
+#else
+#define btCollisionObjectData btCollisionObjectFloatData
+#endif
 
-btScalar					gContactBreakingThreshold = btScalar(0.02);
-ContactDestroyedCallback	gContactDestroyedCallback = 0;
-ContactProcessedCallback	gContactProcessedCallback = 0;
+btScalar gContactBreakingThreshold = btScalar(0.02);
+ContactDestroyedCallback gContactDestroyedCallback = 0;
+ContactProcessedCallback gContactProcessedCallback = 0;
+ContactStartedCallback gContactStartedCallback = 0;
+ContactEndedCallback gContactEndedCallback = 0;
 ///gContactCalcArea3Points will approximate the convex hull area using 3 points
 ///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower
-bool						gContactCalcArea3Points = true;
-
+bool gContactCalcArea3Points = true;
 
 btPersistentManifold::btPersistentManifold()
-:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
-m_body0(0),
-m_body1(0),
-m_cachedPoints (0),
-m_index1a(0)
+	: btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
+	  m_body0(0),
+	  m_body1(0),
+	  m_cachedPoints(0),
+	  m_companionIdA(0),
+	  m_companionIdB(0),
+	  m_index1a(0)
 {
 }
 
-
-
-
 #ifdef DEBUG_PERSISTENCY
 #include <stdio.h>
-void	btPersistentManifold::DebugPersistency()
+void btPersistentManifold::DebugPersistency()
 {
 	int i;
-	printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
-	for (i=0;i<m_cachedPoints;i++)
+	printf("DebugPersistency : numPoints %d\n", m_cachedPoints);
+	for (i = 0; i < m_cachedPoints; i++)
 	{
-		printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
+		printf("m_pointCache[%d].m_userPersistentData = %x\n", i, m_pointCache[i].m_userPersistentData);
 	}
 }
-#endif //DEBUG_PERSISTENCY
+#endif  //DEBUG_PERSISTENCY
 
 void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
 {
-
 	void* oldPtr = pt.m_userPersistentData;
 	if (oldPtr)
 	{
 #ifdef DEBUG_PERSISTENCY
 		int i;
 		int occurance = 0;
-		for (i=0;i<m_cachedPoints;i++)
+		for (i = 0; i < m_cachedPoints; i++)
 		{
 			if (m_pointCache[i].m_userPersistentData == oldPtr)
 			{
 				occurance++;
-				if (occurance>1)
+				if (occurance > 1)
 					printf("error in clearUserCache\n");
 			}
 		}
-		btAssert(occurance<=0);
-#endif //DEBUG_PERSISTENCY
+		btAssert(occurance <= 0);
+#endif  //DEBUG_PERSISTENCY
 
 		if (pt.m_userPersistentData && gContactDestroyedCallback)
 		{
 			(*gContactDestroyedCallback)(pt.m_userPersistentData);
 			pt.m_userPersistentData = 0;
 		}
-		
+
 #ifdef DEBUG_PERSISTENCY
 		DebugPersistency();
 #endif
 	}
-
-	
 }
 
-static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3)
+static inline btScalar calcArea4Points(const btVector3& p0, const btVector3& p1, const btVector3& p2, const btVector3& p3)
 {
 	// It calculates possible 3 area constructed from random 4 points and returns the biggest one.
 
-	btVector3 a[3],b[3];
+	btVector3 a[3], b[3];
 	a[0] = p0 - p1;
 	a[1] = p0 - p2;
 	a[2] = p0 - p3;
@@ -103,100 +105,102 @@ static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,c
 	btVector3 tmp1 = a[1].cross(b[1]);
 	btVector3 tmp2 = a[2].cross(b[2]);
 
-	return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2());
+	return btMax(btMax(tmp0.length2(), tmp1.length2()), tmp2.length2());
 }
 
-int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) 
+int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt)
 {
-		//calculate 4 possible cases areas, and take biggest area
-		//also need to keep 'deepest'
-		
-		int maxPenetrationIndex = -1;
+	//calculate 4 possible cases areas, and take biggest area
+	//also need to keep 'deepest'
+
+	int maxPenetrationIndex = -1;
 #define KEEP_DEEPEST_POINT 1
 #ifdef KEEP_DEEPEST_POINT
-		btScalar maxPenetration = pt.getDistance();
-		for (int i=0;i<4;i++)
+	btScalar maxPenetration = pt.getDistance();
+	for (int i = 0; i < 4; i++)
+	{
+		if (m_pointCache[i].getDistance() < maxPenetration)
 		{
-			if (m_pointCache[i].getDistance() < maxPenetration)
-			{
-				maxPenetrationIndex = i;
-				maxPenetration = m_pointCache[i].getDistance();
-			}
+			maxPenetrationIndex = i;
+			maxPenetration = m_pointCache[i].getDistance();
 		}
-#endif //KEEP_DEEPEST_POINT
-		
-		btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
+	}
+#endif  //KEEP_DEEPEST_POINT
+
+	btScalar res0(btScalar(0.)), res1(btScalar(0.)), res2(btScalar(0.)), res3(btScalar(0.));
 
 	if (gContactCalcArea3Points)
 	{
 		if (maxPenetrationIndex != 0)
 		{
-			btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
-			btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
+			btVector3 a0 = pt.m_localPointA - m_pointCache[1].m_localPointA;
+			btVector3 b0 = m_pointCache[3].m_localPointA - m_pointCache[2].m_localPointA;
 			btVector3 cross = a0.cross(b0);
 			res0 = cross.length2();
 		}
 		if (maxPenetrationIndex != 1)
 		{
-			btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
-			btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
+			btVector3 a1 = pt.m_localPointA - m_pointCache[0].m_localPointA;
+			btVector3 b1 = m_pointCache[3].m_localPointA - m_pointCache[2].m_localPointA;
 			btVector3 cross = a1.cross(b1);
 			res1 = cross.length2();
 		}
 
 		if (maxPenetrationIndex != 2)
 		{
-			btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
-			btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
+			btVector3 a2 = pt.m_localPointA - m_pointCache[0].m_localPointA;
+			btVector3 b2 = m_pointCache[3].m_localPointA - m_pointCache[1].m_localPointA;
 			btVector3 cross = a2.cross(b2);
 			res2 = cross.length2();
 		}
 
 		if (maxPenetrationIndex != 3)
 		{
-			btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
-			btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
+			btVector3 a3 = pt.m_localPointA - m_pointCache[0].m_localPointA;
+			btVector3 b3 = m_pointCache[2].m_localPointA - m_pointCache[1].m_localPointA;
 			btVector3 cross = a3.cross(b3);
 			res3 = cross.length2();
 		}
-	} 
+	}
 	else
 	{
-		if(maxPenetrationIndex != 0) {
-			res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
+		if (maxPenetrationIndex != 0)
+		{
+			res0 = calcArea4Points(pt.m_localPointA, m_pointCache[1].m_localPointA, m_pointCache[2].m_localPointA, m_pointCache[3].m_localPointA);
 		}
 
-		if(maxPenetrationIndex != 1) {
-			res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
+		if (maxPenetrationIndex != 1)
+		{
+			res1 = calcArea4Points(pt.m_localPointA, m_pointCache[0].m_localPointA, m_pointCache[2].m_localPointA, m_pointCache[3].m_localPointA);
 		}
 
-		if(maxPenetrationIndex != 2) {
-			res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA);
+		if (maxPenetrationIndex != 2)
+		{
+			res2 = calcArea4Points(pt.m_localPointA, m_pointCache[0].m_localPointA, m_pointCache[1].m_localPointA, m_pointCache[3].m_localPointA);
 		}
 
-		if(maxPenetrationIndex != 3) {
-			res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA);
+		if (maxPenetrationIndex != 3)
+		{
+			res3 = calcArea4Points(pt.m_localPointA, m_pointCache[0].m_localPointA, m_pointCache[1].m_localPointA, m_pointCache[2].m_localPointA);
 		}
 	}
-	btVector4 maxvec(res0,res1,res2,res3);
+	btVector4 maxvec(res0, res1, res2, res3);
 	int biggestarea = maxvec.closestAxis4();
 	return biggestarea;
-	
 }
 
-
 int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
 {
-	btScalar shortestDist =  getContactBreakingThreshold() * getContactBreakingThreshold();
+	btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold();
 	int size = getNumContacts();
 	int nearestPoint = -1;
-	for( int i = 0; i < size; i++ )
+	for (int i = 0; i < size; i++)
 	{
-		const btManifoldPoint &mp = m_pointCache[i];
+		const btManifoldPoint& mp = m_pointCache[i];
 
-		btVector3 diffA =  mp.m_localPointA- newPoint.m_localPointA;
+		btVector3 diffA = mp.m_localPointA - newPoint.m_localPointA;
 		const btScalar distToManiPoint = diffA.dot(diffA);
-		if( distToManiPoint < shortestDist )
+		if (distToManiPoint < shortestDist)
 		{
 			shortestDist = distToManiPoint;
 			nearestPoint = i;
@@ -211,7 +215,7 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool
 	{
 		btAssert(validContactDistance(newPoint));
 	}
-	
+
 	int insertIndex = getNumContacts();
 	if (insertIndex == MANIFOLD_CACHE_SIZE)
 	{
@@ -222,84 +226,225 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool
 		insertIndex = 0;
 #endif
 		clearUserCache(m_pointCache[insertIndex]);
-		
-	} else
+	}
+	else
 	{
 		m_cachedPoints++;
-
-		
 	}
-	if (insertIndex<0)
-		insertIndex=0;
+	if (insertIndex < 0)
+		insertIndex = 0;
 
-	btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
+	btAssert(m_pointCache[insertIndex].m_userPersistentData == 0);
 	m_pointCache[insertIndex] = newPoint;
 	return insertIndex;
 }
 
-btScalar	btPersistentManifold::getContactBreakingThreshold() const
+btScalar btPersistentManifold::getContactBreakingThreshold() const
 {
 	return m_contactBreakingThreshold;
 }
 
-
-
-void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
+void btPersistentManifold::refreshContactPoints(const btTransform& trA, const btTransform& trB)
 {
 	int i;
 #ifdef DEBUG_PERSISTENCY
 	printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
-		trA.getOrigin().getX(),
-		trA.getOrigin().getY(),
-		trA.getOrigin().getZ(),
-		trB.getOrigin().getX(),
-		trB.getOrigin().getY(),
-		trB.getOrigin().getZ());
-#endif //DEBUG_PERSISTENCY
+		   trA.getOrigin().getX(),
+		   trA.getOrigin().getY(),
+		   trA.getOrigin().getZ(),
+		   trB.getOrigin().getX(),
+		   trB.getOrigin().getY(),
+		   trB.getOrigin().getZ());
+#endif  //DEBUG_PERSISTENCY
 	/// first refresh worldspace positions and distance
-	for (i=getNumContacts()-1;i>=0;i--)
+	for (i = getNumContacts() - 1; i >= 0; i--)
 	{
-		btManifoldPoint &manifoldPoint = m_pointCache[i];
-		manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
-		manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
-		manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
+		btManifoldPoint& manifoldPoint = m_pointCache[i];
+		manifoldPoint.m_positionWorldOnA = trA(manifoldPoint.m_localPointA);
+		manifoldPoint.m_positionWorldOnB = trB(manifoldPoint.m_localPointB);
+		manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
 		manifoldPoint.m_lifeTime++;
 	}
 
-	/// then 
+	/// then
 	btScalar distance2d;
-	btVector3 projectedDifference,projectedPoint;
-	for (i=getNumContacts()-1;i>=0;i--)
+	btVector3 projectedDifference, projectedPoint;
+	for (i = getNumContacts() - 1; i >= 0; i--)
 	{
-		
-		btManifoldPoint &manifoldPoint = m_pointCache[i];
+		btManifoldPoint& manifoldPoint = m_pointCache[i];
 		//contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
 		if (!validContactDistance(manifoldPoint))
 		{
 			removeContactPoint(i);
-		} else
+		}
+		else
 		{
+			//todo: friction anchor may require the contact to be around a bit longer
 			//contact also becomes invalid when relative movement orthogonal to normal exceeds margin
 			projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
 			projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
 			distance2d = projectedDifference.dot(projectedDifference);
-			if (distance2d  > getContactBreakingThreshold()*getContactBreakingThreshold() )
+			if (distance2d > getContactBreakingThreshold() * getContactBreakingThreshold())
 			{
 				removeContactPoint(i);
-			} else
+			}
+			else
 			{
 				//contact point processed callback
 				if (gContactProcessedCallback)
-					(*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1);
+					(*gContactProcessedCallback)(manifoldPoint, (void*)m_body0, (void*)m_body1);
 			}
 		}
 	}
 #ifdef DEBUG_PERSISTENCY
 	DebugPersistency();
-#endif //
+#endif  //
 }
 
+int btPersistentManifold::calculateSerializeBufferSize() const
+{
+	return sizeof(btPersistentManifoldData);
+}
 
+const char* btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const
+{
+	btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer;
+	memset(dataOut, 0, sizeof(btPersistentManifoldData));
+
+	dataOut->m_body0 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody0());
+	dataOut->m_body1 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody1());
+	dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold();
+	dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold();
+	dataOut->m_numCachedPoints = manifold->getNumContacts();
+	dataOut->m_companionIdA = manifold->m_companionIdA;
+	dataOut->m_companionIdB = manifold->m_companionIdB;
+	dataOut->m_index1a = manifold->m_index1a;
+	dataOut->m_objectType = manifold->m_objectType;
+
+	for (int i = 0; i < this->getNumContacts(); i++)
+	{
+		const btManifoldPoint& pt = manifold->getContactPoint(i);
+		dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse;
+		dataOut->m_pointCachePrevRHS[i] = pt.m_prevRHS;
+		dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
+		dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2;
+		pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]);
+		pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]);
+		pt.m_normalWorldOnB.serialize(dataOut->m_pointCacheNormalWorldOnB[i]);
+		dataOut->m_pointCacheDistance[i] = pt.m_distance1;
+		dataOut->m_pointCacheCombinedContactDamping1[i] = pt.m_combinedContactDamping1;
+		dataOut->m_pointCacheCombinedContactStiffness1[i] = pt.m_combinedContactStiffness1;
+		dataOut->m_pointCacheLifeTime[i] = pt.m_lifeTime;
+		dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM;
+		dataOut->m_pointCacheContactERP[i] = pt.m_contactERP;
+		dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM;
+		dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags;
+		dataOut->m_pointCacheIndex0[i] = pt.m_index0;
+		dataOut->m_pointCacheIndex1[i] = pt.m_index1;
+		dataOut->m_pointCachePartId0[i] = pt.m_partId0;
+		dataOut->m_pointCachePartId1[i] = pt.m_partId1;
+		pt.m_positionWorldOnA.serialize(dataOut->m_pointCachePositionWorldOnA[i]);
+		pt.m_positionWorldOnB.serialize(dataOut->m_pointCachePositionWorldOnB[i]);
+		dataOut->m_pointCacheCombinedFriction[i] = pt.m_combinedFriction;
+		pt.m_lateralFrictionDir1.serialize(dataOut->m_pointCacheLateralFrictionDir1[i]);
+		pt.m_lateralFrictionDir2.serialize(dataOut->m_pointCacheLateralFrictionDir2[i]);
+		dataOut->m_pointCacheCombinedRollingFriction[i] = pt.m_combinedRollingFriction;
+		dataOut->m_pointCacheCombinedSpinningFriction[i] = pt.m_combinedSpinningFriction;
+		dataOut->m_pointCacheCombinedRestitution[i] = pt.m_combinedRestitution;
+		dataOut->m_pointCacheContactMotion1[i] = pt.m_contactMotion1;
+		dataOut->m_pointCacheContactMotion2[i] = pt.m_contactMotion2;
+	}
+	return btPersistentManifoldDataName;
+}
 
+void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr)
+{
+	m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold;
+	m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold;
+	m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
+	m_companionIdA = manifoldDataPtr->m_companionIdA;
+	m_companionIdB = manifoldDataPtr->m_companionIdB;
+	//m_index1a = manifoldDataPtr->m_index1a;
+	m_objectType = manifoldDataPtr->m_objectType;
+
+	for (int i = 0; i < this->getNumContacts(); i++)
+	{
+		btManifoldPoint& pt = m_pointCache[i];
+
+		pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
+		pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i];
+		pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
+		pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
+		pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]);
+		pt.m_localPointB.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointB[i]);
+		pt.m_normalWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]);
+		pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
+		pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i];
+		pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i];
+		pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
+		pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
+		pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
+		pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
+		pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
+		pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
+		pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
+		pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
+		pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
+		pt.m_positionWorldOnA.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnA[i]);
+		pt.m_positionWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnB[i]);
+		pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
+		pt.m_lateralFrictionDir1.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]);
+		pt.m_lateralFrictionDir2.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]);
+		pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i];
+		pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i];
+		pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i];
+		pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
+		pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
+	}
+}
 
-
+void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr)
+{
+	m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold;
+	m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold;
+	m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
+	m_companionIdA = manifoldDataPtr->m_companionIdA;
+	m_companionIdB = manifoldDataPtr->m_companionIdB;
+	//m_index1a = manifoldDataPtr->m_index1a;
+	m_objectType = manifoldDataPtr->m_objectType;
+
+	for (int i = 0; i < this->getNumContacts(); i++)
+	{
+		btManifoldPoint& pt = m_pointCache[i];
+
+		pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
+		pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i];
+		pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
+		pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
+		pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]);
+		pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]);
+		pt.m_normalWorldOnB.deSerialize(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]);
+		pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
+		pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i];
+		pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i];
+		pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
+		pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
+		pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
+		pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
+		pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
+		pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
+		pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
+		pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
+		pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
+		pt.m_positionWorldOnA.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnA[i]);
+		pt.m_positionWorldOnB.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnB[i]);
+		pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
+		pt.m_lateralFrictionDir1.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]);
+		pt.m_lateralFrictionDir2.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]);
+		pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i];
+		pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i];
+		pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i];
+		pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
+		pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
+	}
+}
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
index 5026397f67f..0e26da0ebe7 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
@@ -16,7 +16,6 @@ subject to the following restrictions:
 #ifndef BT_PERSISTENT_MANIFOLD_H
 #define BT_PERSISTENT_MANIFOLD_H
 
-
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btTransform.h"
 #include "btManifoldPoint.h"
@@ -24,14 +23,24 @@ class btCollisionObject;
 #include "LinearMath/btAlignedAllocator.h"
 
 struct btCollisionResult;
+struct btCollisionObjectDoubleData;
+struct btCollisionObjectFloatData;
 
 ///maximum contact breaking and merging threshold
 extern btScalar gContactBreakingThreshold;
 
+#ifndef SWIG
+class btPersistentManifold;
+
 typedef bool (*ContactDestroyedCallback)(void* userPersistentData);
-typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1);
-extern ContactDestroyedCallback	gContactDestroyedCallback;
+typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp, void* body0, void* body1);
+typedef void (*ContactStartedCallback)(btPersistentManifold* const& manifold);
+typedef void (*ContactEndedCallback)(btPersistentManifold* const& manifold);
+extern ContactDestroyedCallback gContactDestroyedCallback;
 extern ContactProcessedCallback gContactProcessedCallback;
+extern ContactStartedCallback gContactStartedCallback;
+extern ContactEndedCallback gContactEndedCallback;
+#endif  //SWIG
 
 //the enum starts at 1024 to avoid type conflicts with btTypedConstraint
 enum btContactManifoldTypes
@@ -50,70 +59,74 @@ enum btContactManifoldTypes
 ///the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points
 ///note that some pairs of objects might have more then one contact manifold.
 
-
-ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject
-//ATTRIBUTE_ALIGNED16( class) btPersistentManifold : public btTypedObject
+//ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject
+ATTRIBUTE_ALIGNED16(class)
+btPersistentManifold : public btTypedObject
 {
-
 	btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE];
 
 	/// this two body pointers can point to the physics rigidbody class.
 	const btCollisionObject* m_body0;
 	const btCollisionObject* m_body1;
 
-	int	m_cachedPoints;
+	int m_cachedPoints;
 
-	btScalar	m_contactBreakingThreshold;
-	btScalar	m_contactProcessingThreshold;
+	btScalar m_contactBreakingThreshold;
+	btScalar m_contactProcessingThreshold;
 
-	
 	/// sort cached points so most isolated points come first
-	int	sortCachedPoints(const btManifoldPoint& pt);
+	int sortCachedPoints(const btManifoldPoint& pt);
 
-	int		findContactPoint(const btManifoldPoint* unUsed, int numUnused,const btManifoldPoint& pt);
+	int findContactPoint(const btManifoldPoint* unUsed, int numUnused, const btManifoldPoint& pt);
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	int	m_companionIdA;
-	int	m_companionIdB;
+	int m_companionIdA;
+	int m_companionIdB;
 
 	int m_index1a;
 
 	btPersistentManifold();
 
-	btPersistentManifold(const btCollisionObject* body0,const btCollisionObject* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold)
+	btPersistentManifold(const btCollisionObject* body0, const btCollisionObject* body1, int, btScalar contactBreakingThreshold, btScalar contactProcessingThreshold)
 		: btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
-	m_body0(body0),m_body1(body1),m_cachedPoints(0),
-		m_contactBreakingThreshold(contactBreakingThreshold),
-		m_contactProcessingThreshold(contactProcessingThreshold)
+		  m_body0(body0),
+		  m_body1(body1),
+		  m_cachedPoints(0),
+		  m_contactBreakingThreshold(contactBreakingThreshold),
+		  m_contactProcessingThreshold(contactProcessingThreshold),
+		  m_companionIdA(0),
+		  m_companionIdB(0),
+		  m_index1a(0)
 	{
 	}
 
-	SIMD_FORCE_INLINE const btCollisionObject* getBody0() const { return m_body0;}
-	SIMD_FORCE_INLINE const btCollisionObject* getBody1() const { return m_body1;}
+	SIMD_FORCE_INLINE const btCollisionObject* getBody0() const { return m_body0; }
+	SIMD_FORCE_INLINE const btCollisionObject* getBody1() const { return m_body1; }
 
-	void	setBodies(const btCollisionObject* body0,const btCollisionObject* body1)
+	void setBodies(const btCollisionObject* body0, const btCollisionObject* body1)
 	{
 		m_body0 = body0;
 		m_body1 = body1;
 	}
 
-	void clearUserCache(btManifoldPoint& pt);
+	void clearUserCache(btManifoldPoint & pt);
 
 #ifdef DEBUG_PERSISTENCY
-	void	DebugPersistency();
-#endif //
-	
-	SIMD_FORCE_INLINE int	getNumContacts() const { return m_cachedPoints;}
+	void DebugPersistency();
+#endif  //
+
+	SIMD_FORCE_INLINE int getNumContacts() const
+	{
+		return m_cachedPoints;
+	}
 	/// the setNumContacts API is usually not used, except when you gather/fill all contacts manually
 	void setNumContacts(int cachedPoints)
 	{
 		m_cachedPoints = cachedPoints;
 	}
 
-
 	SIMD_FORCE_INLINE const btManifoldPoint& getContactPoint(int index) const
 	{
 		btAssert(index < m_cachedPoints);
@@ -127,110 +140,235 @@ public:
 	}
 
 	///@todo: get this margin from the current physics / collision environment
-	btScalar	getContactBreakingThreshold() const;
+	btScalar getContactBreakingThreshold() const;
 
-	btScalar	getContactProcessingThreshold() const
+	btScalar getContactProcessingThreshold() const
 	{
 		return m_contactProcessingThreshold;
 	}
-	
+
 	void setContactBreakingThreshold(btScalar contactBreakingThreshold)
 	{
 		m_contactBreakingThreshold = contactBreakingThreshold;
 	}
 
-	void setContactProcessingThreshold(btScalar	contactProcessingThreshold)
+	void setContactProcessingThreshold(btScalar contactProcessingThreshold)
 	{
 		m_contactProcessingThreshold = contactProcessingThreshold;
 	}
-	
-	
-
 
 	int getCacheEntry(const btManifoldPoint& newPoint) const;
 
-	int addManifoldPoint( const btManifoldPoint& newPoint, bool isPredictive=false);
+	int addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive = false);
 
-	void removeContactPoint (int index)
+	void removeContactPoint(int index)
 	{
 		clearUserCache(m_pointCache[index]);
 
 		int lastUsedIndex = getNumContacts() - 1;
-//		m_pointCache[index] = m_pointCache[lastUsedIndex];
-		if(index != lastUsedIndex) 
+		//		m_pointCache[index] = m_pointCache[lastUsedIndex];
+		if (index != lastUsedIndex)
 		{
-			m_pointCache[index] = m_pointCache[lastUsedIndex]; 
+			m_pointCache[index] = m_pointCache[lastUsedIndex];
 			//get rid of duplicated userPersistentData pointer
 			m_pointCache[lastUsedIndex].m_userPersistentData = 0;
 			m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
-			m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false;
+			m_pointCache[lastUsedIndex].m_prevRHS = 0.f;
+			m_pointCache[lastUsedIndex].m_contactPointFlags = 0;
 			m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
 			m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
 			m_pointCache[lastUsedIndex].m_lifeTime = 0;
 		}
 
-		btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0);
+		btAssert(m_pointCache[lastUsedIndex].m_userPersistentData == 0);
 		m_cachedPoints--;
+
+		if (gContactEndedCallback && m_cachedPoints == 0)
+		{
+			gContactEndedCallback(this);
+		}
 	}
-	void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex)
+	void replaceContactPoint(const btManifoldPoint& newPoint, int insertIndex)
 	{
 		btAssert(validContactDistance(newPoint));
 
 #define MAINTAIN_PERSISTENCY 1
 #ifdef MAINTAIN_PERSISTENCY
-		int	lifeTime = m_pointCache[insertIndex].getLifeTime();
-		btScalar	appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
-		btScalar	appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
-		btScalar	appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
-//		bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized;
-		
-		
-			
-		btAssert(lifeTime>=0);
-		void* cache = m_pointCache[insertIndex].m_userPersistentData;
-		
-		m_pointCache[insertIndex] = newPoint;
+		int lifeTime = m_pointCache[insertIndex].getLifeTime();
+		btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
+		btScalar prevRHS = m_pointCache[insertIndex].m_prevRHS;
+		btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
+		btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
+
+		bool replacePoint = true;
+		///we keep existing contact points for friction anchors
+		///if the friction force is within the Coulomb friction cone
+		if (newPoint.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
+		{
+			//   printf("appliedImpulse=%f\n", appliedImpulse);
+			//   printf("appliedLateralImpulse1=%f\n", appliedLateralImpulse1);
+			//   printf("appliedLateralImpulse2=%f\n", appliedLateralImpulse2);
+			//   printf("mu = %f\n", m_pointCache[insertIndex].m_combinedFriction);
+			btScalar mu = m_pointCache[insertIndex].m_combinedFriction;
+			btScalar eps = 0;  //we could allow to enlarge or shrink the tolerance to check against the friction cone a bit, say 1e-7
+			btScalar a = appliedLateralImpulse1 * appliedLateralImpulse1 + appliedLateralImpulse2 * appliedLateralImpulse2;
+			btScalar b = eps + mu * appliedImpulse;
+			b = b * b;
+			replacePoint = (a) > (b);
+		}
 
-		m_pointCache[insertIndex].m_userPersistentData = cache;
-		m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
-		m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
-		m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
-		
+		if (replacePoint)
+		{
+			btAssert(lifeTime >= 0);
+			void* cache = m_pointCache[insertIndex].m_userPersistentData;
+
+			m_pointCache[insertIndex] = newPoint;
+			m_pointCache[insertIndex].m_userPersistentData = cache;
+			m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
+			m_pointCache[insertIndex].m_prevRHS = prevRHS;
+			m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
+			m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
+		}
 
 		m_pointCache[insertIndex].m_lifeTime = lifeTime;
 #else
 		clearUserCache(m_pointCache[insertIndex]);
 		m_pointCache[insertIndex] = newPoint;
-	
+
 #endif
 	}
 
-	
 	bool validContactDistance(const btManifoldPoint& pt) const
 	{
 		return pt.m_distance1 <= getContactBreakingThreshold();
 	}
 	/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
-	void	refreshContactPoints(  const btTransform& trA,const btTransform& trB);
+	void refreshContactPoints(const btTransform& trA, const btTransform& trB);
 
-	
-	SIMD_FORCE_INLINE	void	clearManifold()
+	SIMD_FORCE_INLINE void clearManifold()
 	{
 		int i;
-		for (i=0;i<m_cachedPoints;i++)
+		for (i = 0; i < m_cachedPoints; i++)
 		{
 			clearUserCache(m_pointCache[i]);
 		}
+
+		if (gContactEndedCallback && m_cachedPoints)
+		{
+			gContactEndedCallback(this);
+		}
 		m_cachedPoints = 0;
 	}
 
+	int calculateSerializeBufferSize() const;
+	const char* serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const;
+	void deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr);
+	void deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr);
+};
 
+// clang-format off
 
-}
-;
+struct btPersistentManifoldDoubleData
+{
+	btVector3DoubleData m_pointCacheLocalPointA[4];
+	btVector3DoubleData m_pointCacheLocalPointB[4];
+	btVector3DoubleData m_pointCachePositionWorldOnA[4];
+	btVector3DoubleData m_pointCachePositionWorldOnB[4];
+	btVector3DoubleData m_pointCacheNormalWorldOnB[4];
+	btVector3DoubleData	m_pointCacheLateralFrictionDir1[4];
+	btVector3DoubleData	m_pointCacheLateralFrictionDir2[4];
+	double m_pointCacheDistance[4];
+	double m_pointCacheAppliedImpulse[4];
+	double m_pointCachePrevRHS[4];
+	 double m_pointCacheCombinedFriction[4];
+	double m_pointCacheCombinedRollingFriction[4];
+	double m_pointCacheCombinedSpinningFriction[4];
+	double m_pointCacheCombinedRestitution[4];
+	int	m_pointCachePartId0[4];
+	int	m_pointCachePartId1[4];
+	int	m_pointCacheIndex0[4];
+	int	m_pointCacheIndex1[4];
+	int m_pointCacheContactPointFlags[4];
+	double m_pointCacheAppliedImpulseLateral1[4];
+	double m_pointCacheAppliedImpulseLateral2[4];
+	double m_pointCacheContactMotion1[4];
+	double m_pointCacheContactMotion2[4];
+	double m_pointCacheContactCFM[4];
+	double m_pointCacheCombinedContactStiffness1[4];
+	double m_pointCacheContactERP[4];
+	double m_pointCacheCombinedContactDamping1[4];
+	double m_pointCacheFrictionCFM[4];
+	int m_pointCacheLifeTime[4];
+
+	int m_numCachedPoints;
+	int m_companionIdA;
+	int m_companionIdB;
+	int m_index1a;
 
+	int m_objectType;
+	double	m_contactBreakingThreshold;
+	double	m_contactProcessingThreshold;
+	int m_padding;
 
+	btCollisionObjectDoubleData *m_body0;
+	btCollisionObjectDoubleData *m_body1;
+};
 
 
+struct btPersistentManifoldFloatData
+{
+	btVector3FloatData m_pointCacheLocalPointA[4];
+	btVector3FloatData m_pointCacheLocalPointB[4];
+	btVector3FloatData m_pointCachePositionWorldOnA[4];
+	btVector3FloatData m_pointCachePositionWorldOnB[4];
+	btVector3FloatData m_pointCacheNormalWorldOnB[4];
+	btVector3FloatData	m_pointCacheLateralFrictionDir1[4];
+	btVector3FloatData	m_pointCacheLateralFrictionDir2[4];
+	float m_pointCacheDistance[4];
+	float m_pointCacheAppliedImpulse[4];
+	float m_pointCachePrevRHS[4];
+	float m_pointCacheCombinedFriction[4];
+	float m_pointCacheCombinedRollingFriction[4];
+	float m_pointCacheCombinedSpinningFriction[4];
+	float m_pointCacheCombinedRestitution[4];
+	int	m_pointCachePartId0[4];
+	int	m_pointCachePartId1[4];
+	int	m_pointCacheIndex0[4];
+	int	m_pointCacheIndex1[4];
+	int m_pointCacheContactPointFlags[4];
+	float m_pointCacheAppliedImpulseLateral1[4];
+	float m_pointCacheAppliedImpulseLateral2[4];
+	float m_pointCacheContactMotion1[4];
+	float m_pointCacheContactMotion2[4];
+	float m_pointCacheContactCFM[4];
+	float m_pointCacheCombinedContactStiffness1[4];
+	float m_pointCacheContactERP[4];
+	float m_pointCacheCombinedContactDamping1[4];
+	float m_pointCacheFrictionCFM[4];
+	int m_pointCacheLifeTime[4];
+
+	int m_numCachedPoints;
+	int m_companionIdA;
+	int m_companionIdB;
+	int m_index1a;
+
+	int m_objectType;
+	float	m_contactBreakingThreshold;
+	float	m_contactProcessingThreshold;
+	int m_padding;
+
+	btCollisionObjectFloatData *m_body0;
+	btCollisionObjectFloatData *m_body1;
+};
+
+// clang-format on
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btPersistentManifoldData btPersistentManifoldDoubleData
+#define btPersistentManifoldDataName "btPersistentManifoldDoubleData"
+#else
+#define btPersistentManifoldData btPersistentManifoldFloatData
+#define btPersistentManifoldDataName "btPersistentManifoldFloatData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
-#endif //BT_PERSISTENT_MANIFOLD_H
+#endif  //BT_PERSISTENT_MANIFOLD_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h
index 18da171011a..0900eb6e85a 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h
@@ -18,38 +18,33 @@ subject to the following restrictions:
 
 #include "btDiscreteCollisionDetectorInterface.h"
 
-
-
 struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result
 {
-	
-	
 	btVector3 m_normalOnBInWorld;
 	btVector3 m_pointInWorld;
-	btScalar	m_distance;//negative means penetration
+	btScalar m_distance;  //negative means penetration
 
-	bool	m_hasResult;
+	bool m_hasResult;
 
-	btPointCollector () 
-		: m_distance(btScalar(BT_LARGE_FLOAT)),m_hasResult(false)
+	btPointCollector()
+		: m_distance(btScalar(BT_LARGE_FLOAT)), m_hasResult(false)
 	{
 	}
 
-	virtual void setShapeIdentifiersA(int partId0,int index0)
+	virtual void setShapeIdentifiersA(int partId0, int index0)
 	{
 		(void)partId0;
 		(void)index0;
-			
 	}
-	virtual void setShapeIdentifiersB(int partId1,int index1)
+	virtual void setShapeIdentifiersB(int partId1, int index1)
 	{
 		(void)partId1;
 		(void)index1;
 	}
 
-	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+	virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
 	{
-		if (depth< m_distance)
+		if (depth < m_distance)
 		{
 			m_hasResult = true;
 			m_normalOnBInWorld = normalOnBInWorld;
@@ -60,5 +55,4 @@ struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result
 	}
 };
 
-#endif //BT_POINT_COLLECTOR_H
-
+#endif  //BT_POINT_COLLECTOR_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
index d5f4a964bf3..9d1836037d5 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
@@ -13,45 +13,42 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 ///This file was written by Erwin Coumans
 ///Separating axis rest based on work from Pierre Terdiman, see
 ///And contact clipping based on work from Simon Hobbs
 
-
 #include "btPolyhedralContactClipping.h"
 #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
 
-#include <float.h> //for FLT_MAX
+#include <float.h>  //for FLT_MAX
 
-int gExpectedNbTests=0;
+int gExpectedNbTests = 0;
 int gActualNbTests = 0;
 bool gUseInternalObject = true;
 
 // Clips a face to the back of a plane
-void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS)
+void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS, btScalar planeEqWS)
 {
-	
 	int ve;
 	btScalar ds, de;
 	int numVerts = pVtxIn.size();
 	if (numVerts < 2)
 		return;
 
-	btVector3 firstVertex=pVtxIn[pVtxIn.size()-1];
+	btVector3 firstVertex = pVtxIn[pVtxIn.size() - 1];
 	btVector3 endVertex = pVtxIn[0];
-	
-	ds = planeNormalWS.dot(firstVertex)+planeEqWS;
+
+	ds = planeNormalWS.dot(firstVertex) + planeEqWS;
 
 	for (ve = 0; ve < numVerts; ve++)
 	{
-		endVertex=pVtxIn[ve];
+		endVertex = pVtxIn[ve];
 
-		de = planeNormalWS.dot(endVertex)+planeEqWS;
+		de = planeNormalWS.dot(endVertex) + planeEqWS;
 
-		if (ds<0)
+		if (ds < 0)
 		{
-			if (de<0)
+			if (de < 0)
 			{
 				// Start < 0, end < 0, so output endVertex
 				ppVtxOut.push_back(endVertex);
@@ -59,15 +56,15 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex
 			else
 			{
 				// Start < 0, end >= 0, so output intersection
-				ppVtxOut.push_back( 	firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
+				ppVtxOut.push_back(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de))));
 			}
 		}
 		else
 		{
-			if (de<0)
+			if (de < 0)
 			{
 				// Start >= 0, end < 0 so output intersection and end
-				ppVtxOut.push_back(firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
+				ppVtxOut.push_back(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de))));
 				ppVtxOut.push_back(endVertex);
 			}
 		}
@@ -76,47 +73,44 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex
 	}
 }
 
-
-static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB)
+static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB)
 {
-	btScalar Min0,Max0;
-	btScalar Min1,Max1;
-	btVector3 witnesPtMinA,witnesPtMaxA;
-	btVector3 witnesPtMinB,witnesPtMaxB;
+	btScalar Min0, Max0;
+	btScalar Min1, Max1;
+	btVector3 witnesPtMinA, witnesPtMaxA;
+	btVector3 witnesPtMinB, witnesPtMaxB;
 
-	hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA);
-	hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB);
+	hullA.project(transA, sep_axis, Min0, Max0, witnesPtMinA, witnesPtMaxA);
+	hullB.project(transB, sep_axis, Min1, Max1, witnesPtMinB, witnesPtMaxB);
 
-	if(Max0<Min1 || Max1<Min0)
+	if (Max0 < Min1 || Max1 < Min0)
 		return false;
 
 	btScalar d0 = Max0 - Min1;
-	btAssert(d0>=0.0f);
+	btAssert(d0 >= 0.0f);
 	btScalar d1 = Max1 - Min0;
-	btAssert(d1>=0.0f);
-	if (d0<d1)
+	btAssert(d1 >= 0.0f);
+	if (d0 < d1)
 	{
 		depth = d0;
 		witnessPointA = witnesPtMaxA;
 		witnessPointB = witnesPtMinB;
-
-	} else
+	}
+	else
 	{
 		depth = d1;
 		witnessPointA = witnesPtMinA;
 		witnessPointB = witnesPtMaxB;
 	}
-	
+
 	return true;
 }
 
-
-
-static int gActualSATPairTests=0;
+static int gActualSATPairTests = 0;
 
 inline bool IsAlmostZero(const btVector3& v)
 {
-	if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6)	return false;
+	if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false;
 	return true;
 }
 
@@ -125,9 +119,9 @@ inline bool IsAlmostZero(const btVector3& v)
 inline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar p[3])
 {
 	// This version is ~11.000 cycles (4%) faster overall in one of the tests.
-//	IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK);
-//	IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK);
-//	IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK);
+	//	IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK);
+	//	IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK);
+	//	IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK);
 	p[0] = sv[0] < 0.0f ? -extents[0] : extents[0];
 	p[1] = sv[1] < 0.0f ? -extents[1] : extents[1];
 	p[2] = sv[2] < 0.0f ? -extents[2] : extents[2];
@@ -140,90 +134,94 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans
 	const btVector3& r1 = rot[1];
 	const btVector3& r2 = rot[2];
 
-	const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z();
-	const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z();
-	const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z();
+	const btScalar x = r0.x() * in.x() + r1.x() * in.y() + r2.x() * in.z();
+	const btScalar y = r0.y() * in.x() + r1.y() * in.y() + r2.y() * in.z();
+	const btScalar z = r0.z() * in.x() + r1.z() * in.y() + r2.z() * in.z();
 
 	out.setValue(x, y, z);
 }
 
- bool TestInternalObjects( const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin)
+bool TestInternalObjects(const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin)
 {
 	const btScalar dp = delta_c.dot(axis);
 
 	btVector3 localAxis0;
-	InverseTransformPoint3x3(localAxis0, axis,trans0);
+	InverseTransformPoint3x3(localAxis0, axis, trans0);
 	btVector3 localAxis1;
-	InverseTransformPoint3x3(localAxis1, axis,trans1);
+	InverseTransformPoint3x3(localAxis1, axis, trans1);
 
 	btScalar p0[3];
 	BoxSupport(convex0.m_extents, localAxis0, p0);
 	btScalar p1[3];
 	BoxSupport(convex1.m_extents, localAxis1, p1);
 
-	const btScalar Radius0 = p0[0]*localAxis0.x() + p0[1]*localAxis0.y() + p0[2]*localAxis0.z();
-	const btScalar Radius1 = p1[0]*localAxis1.x() + p1[1]*localAxis1.y() + p1[2]*localAxis1.z();
+	const btScalar Radius0 = p0[0] * localAxis0.x() + p0[1] * localAxis0.y() + p0[2] * localAxis0.z();
+	const btScalar Radius1 = p1[0] * localAxis1.x() + p1[1] * localAxis1.y() + p1[2] * localAxis1.z();
 
-	const btScalar MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius;
-	const btScalar MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius;
+	const btScalar MinRadius = Radius0 > convex0.m_radius ? Radius0 : convex0.m_radius;
+	const btScalar MaxRadius = Radius1 > convex1.m_radius ? Radius1 : convex1.m_radius;
 
 	const btScalar MinMaxRadius = MaxRadius + MinRadius;
 	const btScalar d0 = MinMaxRadius + dp;
 	const btScalar d1 = MinMaxRadius - dp;
 
-	const btScalar depth = d0<d1 ? d0:d1;
-	if(depth>dmin)
+	const btScalar depth = d0 < d1 ? d0 : d1;
+	if (depth > dmin)
 		return false;
 	return true;
 }
-#endif //TEST_INTERNAL_OBJECTS
+#endif  //TEST_INTERNAL_OBJECTS
 
- 
- 
- SIMD_FORCE_INLINE void btSegmentsClosestPoints(
+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 )
+	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 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 ) {
+	if (denom == 0.0f)
+	{
 		tA = 0.0f;
-	} else {
-		tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
-		if ( tA < -hlenA )
+	}
+	else
+	{
+		tA = (dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB) / denom;
+		if (tA < -hlenA)
 			tA = -hlenA;
-		else if ( tA > hlenA )
+		else if (tA > hlenA)
 			tA = hlenA;
 	}
 
 	tB = tA * dirA_dot_dirB - dirB_dot_trans;
 
-	if ( tB < -hlenB ) {
+	if (tB < -hlenB)
+	{
 		tB = -hlenB;
 		tA = tB * dirA_dot_dirB + dirA_dot_trans;
 
-		if ( tA < -hlenA )
+		if (tA < -hlenA)
 			tA = -hlenA;
-		else if ( tA > hlenA )
+		else if (tA > hlenA)
 			tA = hlenA;
-	} else if ( tB > hlenB ) {
+	}
+	else if (tB > hlenB)
+	{
 		tB = hlenB;
 		tA = tB * dirA_dot_dirB + dirA_dot_trans;
 
-		if ( tA < -hlenA )
+		if (tA < -hlenA)
 			tA = -hlenA;
-		else if ( tA > hlenA )
+		else if (tA > hlenA)
 			tA = hlenA;
 	}
 
@@ -235,44 +233,42 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans
 	ptsVector = translation - offsetA + offsetB;
 }
 
-
-
-bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
+bool btPolyhedralContactClipping::findSeparatingAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
 {
 	gActualSATPairTests++;
 
-//#ifdef TEST_INTERNAL_OBJECTS
+	//#ifdef TEST_INTERNAL_OBJECTS
 	const btVector3 c0 = transA * hullA.m_localCenter;
 	const btVector3 c1 = transB * hullB.m_localCenter;
 	const btVector3 DeltaC2 = c0 - c1;
-//#endif
+	//#endif
 
 	btScalar dmin = FLT_MAX;
-	int curPlaneTests=0;
+	int curPlaneTests = 0;
 
 	int numFacesA = hullA.m_faces.size();
 	// Test normals from hullA
-	for(int i=0;i<numFacesA;i++)
+	for (int i = 0; i < numFacesA; i++)
 	{
 		const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
 		btVector3 faceANormalWS = transA.getBasis() * Normal;
-		if (DeltaC2.dot(faceANormalWS)<0)
-			faceANormalWS*=-1.f;
+		if (DeltaC2.dot(faceANormalWS) < 0)
+			faceANormalWS *= -1.f;
 
 		curPlaneTests++;
 #ifdef TEST_INTERNAL_OBJECTS
 		gExpectedNbTests++;
-		if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
+		if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
 			continue;
 		gActualNbTests++;
 #endif
 
 		btScalar d;
-		btVector3 wA,wB;
-		if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d,wA,wB))
+		btVector3 wA, wB;
+		if (!TestSepAxis(hullA, hullB, transA, transB, faceANormalWS, d, wA, wB))
 			return false;
 
-		if(d<dmin)
+		if (d < dmin)
 		{
 			dmin = d;
 			sep = faceANormalWS;
@@ -281,92 +277,89 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 
 	int numFacesB = hullB.m_faces.size();
 	// Test normals from hullB
-	for(int i=0;i<numFacesB;i++)
+	for (int i = 0; i < numFacesB; i++)
 	{
 		const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
 		btVector3 WorldNormal = transB.getBasis() * Normal;
-		if (DeltaC2.dot(WorldNormal)<0)
-			WorldNormal *=-1.f;
+		if (DeltaC2.dot(WorldNormal) < 0)
+			WorldNormal *= -1.f;
 
 		curPlaneTests++;
 #ifdef TEST_INTERNAL_OBJECTS
 		gExpectedNbTests++;
-		if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
+		if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
 			continue;
 		gActualNbTests++;
 #endif
 
 		btScalar d;
-		btVector3 wA,wB;
-		if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d,wA,wB))
+		btVector3 wA, wB;
+		if (!TestSepAxis(hullA, hullB, transA, transB, WorldNormal, d, wA, wB))
 			return false;
 
-		if(d<dmin)
+		if (d < dmin)
 		{
 			dmin = d;
 			sep = WorldNormal;
 		}
 	}
 
-	btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
-	int edgeA=-1;
-	int edgeB=-1;
+	btVector3 edgeAstart, edgeAend, edgeBstart, edgeBend;
+	int edgeA = -1;
+	int edgeB = -1;
 	btVector3 worldEdgeA;
 	btVector3 worldEdgeB;
-	btVector3 witnessPointA(0,0,0),witnessPointB(0,0,0);
-	
+	btVector3 witnessPointA(0, 0, 0), witnessPointB(0, 0, 0);
 
 	int curEdgeEdge = 0;
 	// Test edges
-	for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
+	for (int e0 = 0; e0 < hullA.m_uniqueEdges.size(); e0++)
 	{
 		const btVector3 edge0 = hullA.m_uniqueEdges[e0];
 		const btVector3 WorldEdge0 = transA.getBasis() * edge0;
-		for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++)
+		for (int e1 = 0; e1 < hullB.m_uniqueEdges.size(); e1++)
 		{
 			const btVector3 edge1 = hullB.m_uniqueEdges[e1];
 			const btVector3 WorldEdge1 = transB.getBasis() * edge1;
 
 			btVector3 Cross = WorldEdge0.cross(WorldEdge1);
 			curEdgeEdge++;
-			if(!IsAlmostZero(Cross))
+			if (!IsAlmostZero(Cross))
 			{
 				Cross = Cross.normalize();
-				if (DeltaC2.dot(Cross)<0)
+				if (DeltaC2.dot(Cross) < 0)
 					Cross *= -1.f;
 
-
 #ifdef TEST_INTERNAL_OBJECTS
 				gExpectedNbTests++;
-				if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
+				if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
 					continue;
 				gActualNbTests++;
 #endif
 
 				btScalar dist;
-				btVector3 wA,wB;
-				if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist,wA,wB))
+				btVector3 wA, wB;
+				if (!TestSepAxis(hullA, hullB, transA, transB, Cross, dist, wA, wB))
 					return false;
 
-				if(dist<dmin)
+				if (dist < dmin)
 				{
 					dmin = dist;
 					sep = Cross;
-					edgeA=e0;
-					edgeB=e1;
+					edgeA = e0;
+					edgeB = e1;
 					worldEdgeA = WorldEdge0;
 					worldEdgeB = WorldEdge1;
-					witnessPointA=wA;
-					witnessPointB=wB;
+					witnessPointA = wA;
+					witnessPointB = wB;
 				}
 			}
 		}
-
 	}
 
-	if (edgeA>=0&&edgeB>=0)
+	if (edgeA >= 0 && edgeB >= 0)
 	{
-//		printf("edge-edge\n");
+		//		printf("edge-edge\n");
 		//add an edge-edge contact
 
 		btVector3 ptsVector;
@@ -375,57 +368,55 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 		btScalar tA;
 		btScalar tB;
 
-		btVector3 translation = witnessPointB-witnessPointA;
+		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);
+		btSegmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB,
+								translation,
+								dirA, hlenA,
+								dirB, hlenB);
 
 		btScalar nlSqrt = ptsVector.length2();
-		if (nlSqrt>SIMD_EPSILON)
+		if (nlSqrt > SIMD_EPSILON)
 		{
 			btScalar nl = btSqrt(nlSqrt);
-			ptsVector *= 1.f/nl;
-			if (ptsVector.dot(DeltaC2)<0.f)
+			ptsVector *= 1.f / nl;
+			if (ptsVector.dot(DeltaC2) < 0.f)
 			{
-				ptsVector*=-1.f;
+				ptsVector *= -1.f;
 			}
 			btVector3 ptOnB = witnessPointB + offsetB;
 			btScalar distance = nl;
-			resultOut.addContactPoint(ptsVector, ptOnB,-distance);
+			resultOut.addContactPoint(ptsVector, ptOnB, -distance);
 		}
-
 	}
 
-
-	if((DeltaC2.dot(sep))<0.0f)
+	if ((DeltaC2.dot(sep)) < 0.0f)
 		sep = -sep;
 
 	return true;
 }
 
-void	btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA,  const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
+void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut)
 {
-	btVertexArray worldVertsB2;
+	worldVertsB2.resize(0);
 	btVertexArray* pVtxIn = &worldVertsB1;
 	btVertexArray* pVtxOut = &worldVertsB2;
 	pVtxOut->reserve(pVtxIn->size());
 
-	int closestFaceA=-1;
+	int closestFaceA = -1;
 	{
 		btScalar dmin = FLT_MAX;
-		for(int face=0;face<hullA.m_faces.size();face++)
+		for (int face = 0; face < hullA.m_faces.size(); face++)
 		{
 			const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
 			const btVector3 faceANormalWS = transA.getBasis() * Normal;
-		
+
 			btScalar d = faceANormalWS.dot(separatingNormal);
 			if (d < dmin)
 			{
@@ -434,69 +425,66 @@ void	btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
 			}
 		}
 	}
-	if (closestFaceA<0)
+	if (closestFaceA < 0)
 		return;
 
 	const btFace& polyA = hullA.m_faces[closestFaceA];
 
-		// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
+	// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
 	int numVerticesA = polyA.m_indices.size();
-	for(int e0=0;e0<numVerticesA;e0++)
+	for (int e0 = 0; e0 < numVerticesA; e0++)
 	{
 		const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
-		const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]];
+		const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]];
 		const btVector3 edge0 = a - b;
 		const btVector3 WorldEdge0 = transA.getBasis() * edge0;
-		btVector3 worldPlaneAnormal1 = transA.getBasis()* btVector3(polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
+		btVector3 worldPlaneAnormal1 = transA.getBasis() * btVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
 
-		btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0);
-		btVector3 worldA1 = transA*a;
+		btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);  //.cross(WorldEdge0);
+		btVector3 worldA1 = transA * a;
 		btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1);
-		
+
 //int otherFace=0;
 #ifdef BLA1
 		int otherFace = polyA.m_connectedFaces[e0];
-		btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
+		btVector3 localPlaneNormal(hullA.m_faces[otherFace].m_plane[0], hullA.m_faces[otherFace].m_plane[1], hullA.m_faces[otherFace].m_plane[2]);
 		btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
 
-		btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
-		btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
-#else 
+		btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal;
+		btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin());
+#else
 		btVector3 planeNormalWS = planeNormalWS1;
-		btScalar planeEqWS=planeEqWS1;
-		
+		btScalar planeEqWS = planeEqWS1;
+
 #endif
 		//clip face
 
-		clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
-		btSwap(pVtxIn,pVtxOut);
+		clipFace(*pVtxIn, *pVtxOut, planeNormalWS, planeEqWS);
+		btSwap(pVtxIn, pVtxOut);
 		pVtxOut->resize(0);
 	}
 
-
-
-//#define ONLY_REPORT_DEEPEST_POINT
+	//#define ONLY_REPORT_DEEPEST_POINT
 
 	btVector3 point;
-	
 
 	// only keep points that are behind the witness face
 	{
-		btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
+		btVector3 localPlaneNormal(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
 		btScalar localPlaneEq = polyA.m_plane[3];
-		btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
-		btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
-		for (int i=0;i<pVtxIn->size();i++)
+		btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal;
+		btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin());
+		for (int i = 0; i < pVtxIn->size(); i++)
 		{
 			btVector3 vtx = pVtxIn->at(i);
-			btScalar depth = planeNormalWS.dot(vtx)+planeEqWS;
-			if (depth <=minDist)
+			btScalar depth = planeNormalWS.dot(vtx) + planeEqWS;
+			if (depth <= minDist)
 			{
-//				printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
+				//				printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
 				depth = minDist;
 			}
 
-			if (depth <=maxDist)
+			if (depth <= maxDist)
 			{
 				btVector3 point = pVtxIn->at(i);
 #ifdef ONLY_REPORT_DEEPEST_POINT
@@ -507,40 +495,32 @@ void	btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
 				{
 					printf("error in btPolyhedralContactClipping depth = %f\n", depth);
 					printf("likely wrong separatingNormal passed in\n");
-				} 
-#endif				
-				resultOut.addContactPoint(separatingNormal,point,depth);
+				}
+#endif
+				resultOut.addContactPoint(separatingNormal, point, depth);
 #endif
 			}
 		}
 	}
 #ifdef ONLY_REPORT_DEEPEST_POINT
-	if (curMaxDist<maxDist)
+	if (curMaxDist < maxDist)
 	{
-		resultOut.addContactPoint(separatingNormal,point,curMaxDist);
+		resultOut.addContactPoint(separatingNormal, point, curMaxDist);
 	}
-#endif //ONLY_REPORT_DEEPEST_POINT
-
+#endif  //ONLY_REPORT_DEEPEST_POINT
 }
 
-
-
-
-
-void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
+void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btScalar minDist, btScalar maxDist, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, btDiscreteCollisionDetectorInterface::Result& resultOut)
 {
-
 	btVector3 separatingNormal = separatingNormal1.normalized();
-//	const btVector3 c0 = transA * hullA.m_localCenter;
-//	const btVector3 c1 = transB * hullB.m_localCenter;
+	//	const btVector3 c0 = transA * hullA.m_localCenter;
+	//	const btVector3 c1 = transB * hullB.m_localCenter;
 	//const btVector3 DeltaC2 = c0 - c1;
 
-
-
-	int closestFaceB=-1;
+	int closestFaceB = -1;
 	btScalar dmax = -FLT_MAX;
 	{
-		for(int face=0;face<hullB.m_faces.size();face++)
+		for (int face = 0; face < hullB.m_faces.size(); face++)
 		{
 			const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
 			const btVector3 WorldNormal = transB.getBasis() * Normal;
@@ -552,19 +532,17 @@ void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin
 			}
 		}
 	}
-				btVertexArray worldVertsB1;
-				{
-					const btFace& polyB = hullB.m_faces[closestFaceB];
-					const int numVertices = polyB.m_indices.size();
-					for(int e0=0;e0<numVertices;e0++)
-					{
-						const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
-						worldVertsB1.push_back(transB*b);
-					}
-				}
-
-	
-	if (closestFaceB>=0)
-		clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);
+	worldVertsB1.resize(0);
+	{
+		const btFace& polyB = hullB.m_faces[closestFaceB];
+		const int numVertices = polyB.m_indices.size();
+		for (int e0 = 0; e0 < numVertices; e0++)
+		{
+			const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
+			worldVertsB1.push_back(transB * b);
+		}
+	}
 
+	if (closestFaceB >= 0)
+		clipFaceAgainstHull(separatingNormal, hullA, transA, worldVertsB1, worldVertsB2, minDist, maxDist, resultOut);
 }
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h
index b87bd4f3245..328f6424bc8 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h
@@ -13,14 +13,11 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 ///This file was written by Erwin Coumans
 
-
 #ifndef BT_POLYHEDRAL_CONTACT_CLIPPING_H
 #define BT_POLYHEDRAL_CONTACT_CLIPPING_H
 
-
 #include "LinearMath/btAlignedObjectArray.h"
 #include "LinearMath/btTransform.h"
 #include "btDiscreteCollisionDetectorInterface.h"
@@ -32,15 +29,14 @@ typedef btAlignedObjectArray<btVector3> btVertexArray;
 // Clips a face to the back of a plane
 struct btPolyhedralContactClipping
 {
-	static void clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut);
-	static void	clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA,  const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut);
+	static void clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btScalar minDist, btScalar maxDist, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, btDiscreteCollisionDetectorInterface::Result& resultOut);
 
-	static bool findSeparatingAxis(	const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut);
+	static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut);
 
-	///the clipFace method is used internally
-	static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS);
+	static bool findSeparatingAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut);
 
+	///the clipFace method is used internally
+	static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS, btScalar planeEqWS);
 };
 
-#endif // BT_POLYHEDRAL_CONTACT_CLIPPING_H
-
+#endif  // BT_POLYHEDRAL_CONTACT_CLIPPING_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
index 786efd18200..3d11e5bce5f 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
@@ -23,39 +23,38 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
 #include "btRaycastCallback.h"
 
-btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags)
-	:
-	m_from(from),
-	m_to(to),
-   //@BP Mod
-   m_flags(flags),
-	m_hitFraction(btScalar(1.))
+btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from, const btVector3& to, unsigned int flags)
+	: m_from(from),
+	  m_to(to),
+	  //@BP Mod
+	  m_flags(flags),
+	  m_hitFraction(btScalar(1.))
 {
-
 }
 
-
-
-void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
+void btTriangleRaycastCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex)
 {
-	const btVector3 &vert0=triangle[0];
-	const btVector3 &vert1=triangle[1];
-	const btVector3 &vert2=triangle[2];
+	const btVector3& vert0 = triangle[0];
+	const btVector3& vert1 = triangle[1];
+	const btVector3& vert2 = triangle[2];
+
+	btVector3 v10;
+	v10 = vert1 - vert0;
+	btVector3 v20;
+	v20 = vert2 - vert0;
 
-	btVector3 v10; v10 = vert1 - vert0 ;
-	btVector3 v20; v20 = vert2 - vert0 ;
+	btVector3 triangleNormal;
+	triangleNormal = v10.cross(v20);
 
-	btVector3 triangleNormal; triangleNormal = v10.cross( v20 );
-	
 	const btScalar dist = vert0.dot(triangleNormal);
-	btScalar dist_a = triangleNormal.dot(m_from) ;
-	dist_a-= dist;
+	btScalar dist_a = triangleNormal.dot(m_from);
+	dist_a -= dist;
 	btScalar dist_b = triangleNormal.dot(m_to);
 	dist_b -= dist;
 
-	if ( dist_a * dist_b >= btScalar(0.0) )
+	if (dist_a * dist_b >= btScalar(0.0))
 	{
-		return ; // same sign
+		return;  // same sign
 	}
 
 	if (((m_flags & kF_FilterBackfaces) != 0) && (dist_a <= btScalar(0.0)))
@@ -64,52 +63,52 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId,
 		return;
 	}
 
-	
-	const btScalar proj_length=dist_a-dist_b;
-	const btScalar distance = (dist_a)/(proj_length);
+	const btScalar proj_length = dist_a - dist_b;
+	const btScalar distance = (dist_a) / (proj_length);
 	// Now we have the intersection point on the plane, we'll see if it's inside the triangle
 	// Add an epsilon as a tolerance for the raycast,
 	// in case the ray hits exacly on the edge of the triangle.
 	// It must be scaled for the triangle size.
-	
-	if(distance < m_hitFraction)
-	{
-		
 
-		btScalar edge_tolerance =triangleNormal.length2();		
+	if (distance < m_hitFraction)
+	{
+		btScalar edge_tolerance = triangleNormal.length2();
 		edge_tolerance *= btScalar(-0.0001);
-		btVector3 point; point.setInterpolate3( m_from, m_to, distance);
+		btVector3 point;
+		point.setInterpolate3(m_from, m_to, distance);
 		{
-			btVector3 v0p; v0p = vert0 - point;
-			btVector3 v1p; v1p = vert1 - point;
-			btVector3 cp0; cp0 = v0p.cross( v1p );
-
-			if ( (btScalar)(cp0.dot(triangleNormal)) >=edge_tolerance) 
+			btVector3 v0p;
+			v0p = vert0 - point;
+			btVector3 v1p;
+			v1p = vert1 - point;
+			btVector3 cp0;
+			cp0 = v0p.cross(v1p);
+
+			if ((btScalar)(cp0.dot(triangleNormal)) >= edge_tolerance)
 			{
-						
-
-				btVector3 v2p; v2p = vert2 -  point;
+				btVector3 v2p;
+				v2p = vert2 - point;
 				btVector3 cp1;
-				cp1 = v1p.cross( v2p);
-				if ( (btScalar)(cp1.dot(triangleNormal)) >=edge_tolerance) 
+				cp1 = v1p.cross(v2p);
+				if ((btScalar)(cp1.dot(triangleNormal)) >= edge_tolerance)
 				{
 					btVector3 cp2;
 					cp2 = v2p.cross(v0p);
-					
-					if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance) 
+
+					if ((btScalar)(cp2.dot(triangleNormal)) >= edge_tolerance)
 					{
-					  //@BP Mod
-					  // Triangle normal isn't normalized
-				      triangleNormal.normalize();
+						//@BP Mod
+						// Triangle normal isn't normalized
+						triangleNormal.normalize();
 
-					 //@BP Mod - Allow for unflipped normal when raycasting against backfaces
+						//@BP Mod - Allow for unflipped normal when raycasting against backfaces
 						if (((m_flags & kF_KeepUnflippedNormal) == 0) && (dist_a <= btScalar(0.0)))
 						{
-							m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex);
+							m_hitFraction = reportHit(-triangleNormal, distance, partId, triangleIndex);
 						}
 						else
 						{
-							m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex);
+							m_hitFraction = reportHit(triangleNormal, distance, partId, triangleIndex);
 						}
 					}
 				}
@@ -118,8 +117,7 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId,
 	}
 }
 
-
-btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin)
+btTriangleConvexcastCallback::btTriangleConvexcastCallback(const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin)
 {
 	m_convexShape = convexShape;
 	m_convexShapeFrom = convexShapeFrom;
@@ -130,14 +128,13 @@ btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape*
 	m_allowedPenetration = 0.f;
 }
 
-void
-btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex)
+void btTriangleConvexcastCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex)
 {
-	btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]);
-    triangleShape.setMargin(m_triangleCollisionMargin);
+	btTriangleShape triangleShape(triangle[0], triangle[1], triangle[2]);
+	triangleShape.setMargin(m_triangleCollisionMargin);
 
-	btVoronoiSimplexSolver	simplexSolver;
-	btGjkEpaPenetrationDepthSolver	gjkEpaPenetrationSolver;
+	btVoronoiSimplexSolver simplexSolver;
+	btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
 
 //#define  USE_SUBSIMPLEX_CONVEX_CAST 1
 //if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented out code below
@@ -145,21 +142,21 @@ btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId,
 	btSubsimplexConvexCast convexCaster(m_convexShape, &triangleShape, &simplexSolver);
 #else
 	//btGjkConvexCast	convexCaster(m_convexShape,&triangleShape,&simplexSolver);
-	btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,&gjkEpaPenetrationSolver);
-#endif //#USE_SUBSIMPLEX_CONVEX_CAST
-	
+	btContinuousConvexCollision convexCaster(m_convexShape, &triangleShape, &simplexSolver, &gjkEpaPenetrationSolver);
+#endif  //#USE_SUBSIMPLEX_CONVEX_CAST
+
 	btConvexCast::CastResult castResult;
 	castResult.m_fraction = btScalar(1.);
 	castResult.m_allowedPenetration = m_allowedPenetration;
-	if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult))
+	if (convexCaster.calcTimeOfImpact(m_convexShapeFrom, m_convexShapeTo, m_triangleToWorld, m_triangleToWorld, castResult))
 	{
 		//add hit
 		if (castResult.m_normal.length2() > btScalar(0.0001))
-		{					
+		{
 			if (castResult.m_fraction < m_hitFraction)
 			{
-/* btContinuousConvexCast's normal is already in world space */
-/*
+				/* btContinuousConvexCast's normal is already in world space */
+				/*
 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
 				//rotate normal into worldspace
 				castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal;
@@ -167,11 +164,11 @@ btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId,
 */
 				castResult.m_normal.normalize();
 
-				reportHit (castResult.m_normal,
-							castResult.m_hitPoint,
-							castResult.m_fraction,
-							partId,
-							triangleIndex);
+				reportHit(castResult.m_normal,
+						  castResult.m_hitPoint,
+						  castResult.m_fraction,
+						  partId,
+						  triangleIndex);
 			}
 		}
 	}
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
index f2ed0cd39c4..2d0df718a23 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
@@ -21,35 +21,34 @@ subject to the following restrictions:
 struct btBroadphaseProxy;
 class btConvexShape;
 
-class  btTriangleRaycastCallback: public btTriangleCallback
+class btTriangleRaycastCallback : public btTriangleCallback
 {
 public:
-
 	//input
 	btVector3 m_from;
 	btVector3 m_to;
 
-   //@BP Mod - allow backface filtering and unflipped normals
-   enum EFlags
-   {
-	  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
-		///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;
+	//@BP Mod - allow backface filtering and unflipped normals
+	enum EFlags
+	{
+		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
+													 ///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_DisableHeightfieldAccelerator  = 1 << 4, //don't use the heightfield raycast accelerator. See https://github.com/bulletphysics/bullet3/pull/2062
+		kF_Terminator = 0xFFFFFFFF
+	};
+	unsigned int m_flags;
+
+	btScalar m_hitFraction;
 
-	btScalar	m_hitFraction;
+	btTriangleRaycastCallback(const btVector3& from, const btVector3& to, unsigned int flags = 0);
 
-	btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags=0);
-	
 	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
 
-	virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) = 0;
-	
+	virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) = 0;
 };
 
 class btTriangleConvexcastCallback : public btTriangleCallback
@@ -63,12 +62,11 @@ public:
 	btScalar m_triangleCollisionMargin;
 	btScalar m_allowedPenetration;
 
-	btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin);
+	btTriangleConvexcastCallback(const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin);
 
-	virtual void processTriangle (btVector3* triangle, int partId, int triangleIndex);
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
 
-	virtual btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0;
+	virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0;
 };
 
-#endif //BT_RAYCAST_TRI_CALLBACK_H
-
+#endif  //BT_RAYCAST_TRI_CALLBACK_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
index da8a13914c9..ccd227109d2 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
@@ -13,8 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef BT_SIMPLEX_SOLVER_INTERFACE_H
 #define BT_SIMPLEX_SOLVER_INTERFACE_H
 
@@ -31,33 +29,30 @@ subject to the following restrictions:
 /// voronoi regions or barycentric coordinates
 class btSimplexSolverInterface
 {
-	public:
-		virtual ~btSimplexSolverInterface() {};
+public:
+	virtual ~btSimplexSolverInterface(){};
 
 	virtual void reset() = 0;
 
 	virtual void addVertex(const btVector3& w, const btVector3& p, const btVector3& q) = 0;
-	
+
 	virtual bool closest(btVector3& v) = 0;
 
 	virtual btScalar maxVertex() = 0;
 
 	virtual bool fullSimplex() const = 0;
 
-	virtual int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const = 0;
+	virtual int getSimplex(btVector3* pBuf, btVector3* qBuf, btVector3* yBuf) const = 0;
 
 	virtual bool inSimplex(const btVector3& w) = 0;
-	
+
 	virtual void backup_closest(btVector3& v) = 0;
 
 	virtual bool emptySimplex() const = 0;
 
 	virtual void compute_points(btVector3& p1, btVector3& p2) = 0;
 
-	virtual int numVertices() const =0;
-
-
+	virtual int numVertices() const = 0;
 };
 #endif
-#endif //BT_SIMPLEX_SOLVER_INTERFACE_H
-
+#endif  //BT_SIMPLEX_SOLVER_INTERFACE_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
index ec638f60ba5..37458339e70 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btSubSimplexConvexCast.h"
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 
@@ -22,32 +21,26 @@ subject to the following restrictions:
 #include "btPointCollector.h"
 #include "LinearMath/btTransformUtil.h"
 
-btSubsimplexConvexCast::btSubsimplexConvexCast (const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
-:m_simplexSolver(simplexSolver),
-m_convexA(convexA),m_convexB(convexB)
+btSubsimplexConvexCast::btSubsimplexConvexCast(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver)
+	: m_simplexSolver(simplexSolver),
+	  m_convexA(convexA),
+	  m_convexB(convexB)
 {
 }
 
-///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
-///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
-#ifdef BT_USE_DOUBLE_PRECISION
-#define MAX_ITERATIONS 64
-#else
-#define MAX_ITERATIONS 32
-#endif
-bool	btSubsimplexConvexCast::calcTimeOfImpact(
-		const btTransform& fromA,
-		const btTransform& toA,
-		const btTransform& fromB,
-		const btTransform& toB,
-		CastResult& result)
-{
 
+bool btSubsimplexConvexCast::calcTimeOfImpact(
+	const btTransform& fromA,
+	const btTransform& toA,
+	const btTransform& fromB,
+	const btTransform& toB,
+	CastResult& result)
+{
 	m_simplexSolver->reset();
 
-	btVector3 linVelA,linVelB;
-	linVelA = toA.getOrigin()-fromA.getOrigin();
-	linVelB = toB.getOrigin()-fromB.getOrigin();
+	btVector3 linVelA, linVelB;
+	linVelA = toA.getOrigin() - fromA.getOrigin();
+	linVelB = toB.getOrigin() - fromB.getOrigin();
 
 	btScalar lambda = btScalar(0.);
 
@@ -55,36 +48,31 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 	btTransform interpolatedTransB = fromB;
 
 	///take relative motion
-	btVector3 r = (linVelA-linVelB);
+	btVector3 r = (linVelA - linVelB);
 	btVector3 v;
-	
-	btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis()));
-	btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis()));
-	v = supVertexA-supVertexB;
-	int maxIter = MAX_ITERATIONS;
+
+	btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r * fromA.getBasis()));
+	btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r * fromB.getBasis()));
+	v = supVertexA - supVertexB;
+	int maxIter = result.m_subSimplexCastMaxIterations;
 
 	btVector3 n;
-	n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
-	
+	n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+
 	btVector3 c;
 
-	
+	btScalar dist2 = v.length2();
 
 
-	btScalar dist2 = v.length2();
-#ifdef BT_USE_DOUBLE_PRECISION
-	btScalar epsilon = btScalar(0.0001);
-#else
-	btScalar epsilon = btScalar(0.0001);
-#endif //BT_USE_DOUBLE_PRECISION
-	btVector3	w,p;
+
+	btVector3 w, p;
 	btScalar VdotR;
-	
-	while ( (dist2 > epsilon) && maxIter--)
+
+	while ((dist2 > result.m_subSimplexCastEpsilon) && maxIter--)
 	{
-		supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis()));
-		supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis()));
-		w = supVertexA-supVertexB;
+		supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v * interpolatedTransA.getBasis()));
+		supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v * interpolatedTransB.getBasis()));
+		w = supVertexA - supVertexB;
 
 		btScalar VdotW = v.dot(w);
 
@@ -93,68 +81,63 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 			return false;
 		}
 
-		if ( VdotW > btScalar(0.))
+		if (VdotW > btScalar(0.))
 		{
 			VdotR = v.dot(r);
 
-			if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
+			if (VdotR >= -(SIMD_EPSILON * SIMD_EPSILON))
 				return false;
 			else
 			{
 				lambda = lambda - VdotW / VdotR;
 				//interpolate to next lambda
 				//	x = s + lambda * r;
-				interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
-				interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
+				interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(), toA.getOrigin(), lambda);
+				interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(), toB.getOrigin(), lambda);
 				//m_simplexSolver->reset();
 				//check next line
-				 w = supVertexA-supVertexB;
-				
+				w = supVertexA - supVertexB;
+
 				n = v;
-				
 			}
-		} 
+		}
 		///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.
 		if (!m_simplexSolver->inSimplex(w))
-			m_simplexSolver->addVertex( w, supVertexA , supVertexB);
+			m_simplexSolver->addVertex(w, supVertexA, supVertexB);
 
 		if (m_simplexSolver->closest(v))
 		{
 			dist2 = v.length2();
-			
+
 			//todo: check this normal for validity
 			//n=v;
 			//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
 			//printf("DIST2=%f\n",dist2);
 			//printf("numverts = %i\n",m_simplexSolver->numVertices());
-		} else
+		}
+		else
 		{
 			dist2 = btScalar(0.);
-		} 
+		}
 	}
 
 	//int numiter = MAX_ITERATIONS - maxIter;
-//	printf("number of iterations: %d", numiter);
-	
+	//	printf("number of iterations: %d", numiter);
+
 	//don't report a time of impact when moving 'away' from the hitnormal
-	
 
 	result.m_fraction = lambda;
-	if (n.length2() >= (SIMD_EPSILON*SIMD_EPSILON))
+	if (n.length2() >= (SIMD_EPSILON * SIMD_EPSILON))
 		result.m_normal = n.normalized();
 	else
 		result.m_normal = btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 
 	//don't report time of impact for motion away from the contact normal (or causes minor penetration)
-	if (result.m_normal.dot(r)>=-result.m_allowedPenetration)
+	if (result.m_normal.dot(r) >= -result.m_allowedPenetration)
 		return false;
 
-	btVector3 hitA,hitB;
-	m_simplexSolver->compute_points(hitA,hitB);
-	result.m_hitPoint=hitB;
+	btVector3 hitA, hitB;
+	m_simplexSolver->compute_points(hitA, hitB);
+	result.m_hitPoint = hitB;
 	return true;
 }
-
-
-
-
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h
index 6c8127983eb..0638a30eb1a 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_SUBSIMPLEX_CONVEX_CAST_H
 #define BT_SUBSIMPLEX_CONVEX_CAST_H
 
@@ -28,23 +27,21 @@ class btConvexShape;
 class btSubsimplexConvexCast : public btConvexCast
 {
 	btSimplexSolverInterface* m_simplexSolver;
-	const btConvexShape*	m_convexA;
-	const btConvexShape*	m_convexB;
+	const btConvexShape* m_convexA;
+	const btConvexShape* m_convexB;
 
 public:
-
-	btSubsimplexConvexCast (const btConvexShape*	shapeA,const btConvexShape*	shapeB,btSimplexSolverInterface* simplexSolver);
+	btSubsimplexConvexCast(const btConvexShape* shapeA, const btConvexShape* shapeB, btSimplexSolverInterface* simplexSolver);
 
 	//virtual ~btSubsimplexConvexCast();
 	///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
 	///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
-	virtual bool	calcTimeOfImpact(
-			const btTransform& fromA,
-			const btTransform& toA,
-			const btTransform& fromB,
-			const btTransform& toB,
-			CastResult& result);
-
+	virtual bool calcTimeOfImpact(
+		const btTransform& fromA,
+		const btTransform& toA,
+		const btTransform& fromB,
+		const btTransform& toB,
+		CastResult& result);
 };
 
-#endif //BT_SUBSIMPLEX_CONVEX_CAST_H
+#endif  //BT_SUBSIMPLEX_CONVEX_CAST_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
index 23b4f79cfc2..8fda94d2adf 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
@@ -23,26 +23,24 @@ subject to the following restrictions:
 		
 */
 
-
 #include "btVoronoiSimplexSolver.h"
 
-#define VERTA  0
-#define VERTB  1
-#define VERTC  2
-#define VERTD  3
+#define VERTA 0
+#define VERTB 1
+#define VERTC 2
+#define VERTD 3
 
 #define CATCH_DEGENERATE_TETRAHEDRON 1
-void	btVoronoiSimplexSolver::removeVertex(int index)
+void btVoronoiSimplexSolver::removeVertex(int index)
 {
-	
-	btAssert(m_numVertices>0);
+	btAssert(m_numVertices > 0);
 	m_numVertices--;
 	m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
 	m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
 	m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
 }
 
-void	btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts)
+void btVoronoiSimplexSolver::reduceVertices(const btUsageBitfield& usedVerts)
 {
 	if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
 		removeVertex(3);
@@ -52,29 +50,22 @@ void	btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts)
 
 	if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
 		removeVertex(1);
-	
+
 	if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
 		removeVertex(0);
-
 }
 
-
-
-
-
 //clear the simplex, remove all the vertices
 void btVoronoiSimplexSolver::reset()
 {
 	m_cachedValidClosest = false;
 	m_numVertices = 0;
 	m_needsUpdate = true;
-	m_lastW = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+	m_lastW = btVector3(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
 	m_cachedBC.reset();
 }
 
-
-
-	//add a vertex
+//add a vertex
 void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q)
 {
 	m_lastW = w;
@@ -87,9 +78,8 @@ void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, c
 	m_numVertices++;
 }
 
-bool	btVoronoiSimplexSolver::updateClosestVectorAndPoints()
+bool btVoronoiSimplexSolver::updateClosestVectorAndPoints()
 {
-	
 	if (m_needsUpdate)
 	{
 		m_cachedBC.reset();
@@ -98,127 +88,131 @@ bool	btVoronoiSimplexSolver::updateClosestVectorAndPoints()
 
 		switch (numVertices())
 		{
-		case 0:
+			case 0:
 				m_cachedValidClosest = false;
 				break;
-		case 1:
+			case 1:
 			{
 				m_cachedP1 = m_simplexPointsP[0];
 				m_cachedP2 = m_simplexPointsQ[0];
-				m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
+				m_cachedV = m_cachedP1 - m_cachedP2;  //== m_simplexVectorW[0]
 				m_cachedBC.reset();
-				m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.));
+				m_cachedBC.setBarycentricCoordinates(btScalar(1.), btScalar(0.), btScalar(0.), btScalar(0.));
 				m_cachedValidClosest = m_cachedBC.isValid();
 				break;
 			};
-		case 2:
+			case 2:
 			{
-			//closest point origin from line segment
-					const btVector3& from = m_simplexVectorW[0];
-					const btVector3& to = m_simplexVectorW[1];
-					btVector3 nearest;
-
-					btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
-					btVector3 diff = p - from;
-					btVector3 v = to - from;
-					btScalar t = v.dot(diff);
-					
-					if (t > 0) {
-						btScalar dotVV = v.dot(v);
-						if (t < dotVV) {
-							t /= dotVV;
-							diff -= t*v;
-							m_cachedBC.m_usedVertices.usedVertexA = true;
-							m_cachedBC.m_usedVertices.usedVertexB = true;
-						} else {
-							t = 1;
-							diff -= v;
-							//reduce to 1 point
-							m_cachedBC.m_usedVertices.usedVertexB = true;
-						}
-					} else
+				//closest point origin from line segment
+				const btVector3& from = m_simplexVectorW[0];
+				const btVector3& to = m_simplexVectorW[1];
+				btVector3 nearest;
+
+				btVector3 p(btScalar(0.), btScalar(0.), btScalar(0.));
+				btVector3 diff = p - from;
+				btVector3 v = to - from;
+				btScalar t = v.dot(diff);
+
+				if (t > 0)
+				{
+					btScalar dotVV = v.dot(v);
+					if (t < dotVV)
 					{
-						t = 0;
-						//reduce to 1 point
+						t /= dotVV;
+						diff -= t * v;
 						m_cachedBC.m_usedVertices.usedVertexA = true;
+						m_cachedBC.m_usedVertices.usedVertexB = true;
+					}
+					else
+					{
+						t = 1;
+						diff -= v;
+						//reduce to 1 point
+						m_cachedBC.m_usedVertices.usedVertexB = true;
 					}
-					m_cachedBC.setBarycentricCoordinates(1-t,t);
-					nearest = from + t*v;
+				}
+				else
+				{
+					t = 0;
+					//reduce to 1 point
+					m_cachedBC.m_usedVertices.usedVertexA = true;
+				}
+				m_cachedBC.setBarycentricCoordinates(1 - t, t);
+				nearest = from + t * v;
 
-					m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
-					m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
-					m_cachedV = m_cachedP1 - m_cachedP2;
-					
-					reduceVertices(m_cachedBC.m_usedVertices);
+				m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
+				m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
+				m_cachedV = m_cachedP1 - m_cachedP2;
 
-					m_cachedValidClosest = m_cachedBC.isValid();
-					break;
+				reduceVertices(m_cachedBC.m_usedVertices);
+
+				m_cachedValidClosest = m_cachedBC.isValid();
+				break;
 			}
-		case 3: 
-			{ 
-				//closest point origin from triangle 
-				btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); 
+			case 3:
+			{
+				//closest point origin from triangle
+				btVector3 p(btScalar(0.), btScalar(0.), btScalar(0.));
 
-				const btVector3& a = m_simplexVectorW[0]; 
-				const btVector3& b = m_simplexVectorW[1]; 
-				const btVector3& c = m_simplexVectorW[2]; 
+				const btVector3& a = m_simplexVectorW[0];
+				const btVector3& b = m_simplexVectorW[1];
+				const btVector3& c = m_simplexVectorW[2];
 
-				closestPtPointTriangle(p,a,b,c,m_cachedBC); 
-				m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + 
-				m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + 
-				m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; 
+				closestPtPointTriangle(p, a, b, c, m_cachedBC);
+				m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
+							 m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+							 m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2];
 
-				m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + 
-				m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + 
-				m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; 
+				m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
+							 m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+							 m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2];
 
-				m_cachedV = m_cachedP1-m_cachedP2; 
+				m_cachedV = m_cachedP1 - m_cachedP2;
 
-				reduceVertices (m_cachedBC.m_usedVertices); 
-				m_cachedValidClosest = m_cachedBC.isValid(); 
+				reduceVertices(m_cachedBC.m_usedVertices);
+				m_cachedValidClosest = m_cachedBC.isValid();
 
-				break; 
+				break;
 			}
-		case 4:
+			case 4:
 			{
+				btVector3 p(btScalar(0.), btScalar(0.), btScalar(0.));
 
-				
-				btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
-				
 				const btVector3& a = m_simplexVectorW[0];
 				const btVector3& b = m_simplexVectorW[1];
 				const btVector3& c = m_simplexVectorW[2];
 				const btVector3& d = m_simplexVectorW[3];
 
-				bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
+				bool hasSeparation = closestPtPointTetrahedron(p, a, b, c, d, m_cachedBC);
 
-				if (hasSeperation)
+				if (hasSeparation)
 				{
-
 					m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
-						m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
-						m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
-						m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
+								 m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+								 m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
+								 m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
 
 					m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
-						m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
-						m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
-						m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
+								 m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+								 m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
+								 m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
 
-					m_cachedV = m_cachedP1-m_cachedP2;
-					reduceVertices (m_cachedBC.m_usedVertices);
-				} else
+					m_cachedV = m_cachedP1 - m_cachedP2;
+					reduceVertices(m_cachedBC.m_usedVertices);
+				}
+				else
 				{
-//					printf("sub distance got penetration\n");
+					//					printf("sub distance got penetration\n");
 
 					if (m_cachedBC.m_degenerate)
 					{
 						m_cachedValidClosest = false;
-					} else
+					}
+					else
 					{
 						m_cachedValidClosest = true;
 						//degenerate case == false, penetration = true + zero
-						m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+						m_cachedV.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 					}
 					break;
 				}
@@ -228,7 +222,7 @@ bool	btVoronoiSimplexSolver::updateClosestVectorAndPoints()
 				//closest point origin from tetrahedron
 				break;
 			}
-		default:
+			default:
 			{
 				m_cachedValidClosest = false;
 			}
@@ -236,7 +230,6 @@ bool	btVoronoiSimplexSolver::updateClosestVectorAndPoints()
 	}
 
 	return m_cachedValidClosest;
-
 }
 
 //return/calculate the closest vertex
@@ -247,13 +240,11 @@ bool btVoronoiSimplexSolver::closest(btVector3& v)
 	return succes;
 }
 
-
-
 btScalar btVoronoiSimplexSolver::maxVertex()
 {
 	int i, numverts = numVertices();
 	btScalar maxV = btScalar(0.);
-	for (i=0;i<numverts;i++)
+	for (i = 0; i < numverts; i++)
 	{
 		btScalar curLen2 = m_simplexVectorW[i].length2();
 		if (maxV < curLen2)
@@ -262,13 +253,11 @@ btScalar btVoronoiSimplexSolver::maxVertex()
 	return maxV;
 }
 
-
-
-	//return the current simplex
-int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const
+//return the current simplex
+int btVoronoiSimplexSolver::getSimplex(btVector3* pBuf, btVector3* qBuf, btVector3* yBuf) const
 {
 	int i;
-	for (i=0;i<numVertices();i++)
+	for (i = 0; i < numVertices(); i++)
 	{
 		yBuf[i] = m_simplexVectorW[i];
 		pBuf[i] = m_simplexPointsP[i];
@@ -277,20 +266,17 @@ int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVecto
 	return numVertices();
 }
 
-
-
-
 bool btVoronoiSimplexSolver::inSimplex(const btVector3& w)
 {
 	bool found = false;
 	int i, numverts = numVertices();
 	//btScalar maxV = btScalar(0.);
-	
+
 	//w is in the current (reduced) simplex
-	for (i=0;i<numverts;i++)
+	for (i = 0; i < numverts; i++)
 	{
 #ifdef BT_USE_EQUAL_VERTEX_THRESHOLD
-		if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
+		if (m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
 #else
 		if (m_simplexVectorW[i] == w)
 #endif
@@ -303,199 +289,190 @@ bool btVoronoiSimplexSolver::inSimplex(const btVector3& w)
 	//check in case lastW is already removed
 	if (w == m_lastW)
 		return true;
-    	
+
 	return found;
 }
 
-void btVoronoiSimplexSolver::backup_closest(btVector3& v) 
+void btVoronoiSimplexSolver::backup_closest(btVector3& v)
 {
 	v = m_cachedV;
 }
 
-
-bool btVoronoiSimplexSolver::emptySimplex() const 
+bool btVoronoiSimplexSolver::emptySimplex() const
 {
 	return (numVertices() == 0);
-
 }
 
-void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2) 
+void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2)
 {
 	updateClosestVectorAndPoints();
 	p1 = m_cachedP1;
 	p2 = m_cachedP2;
-
 }
 
-
-
-
-bool	btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result)
+bool btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, btSubSimplexClosestResult& result)
 {
 	result.m_usedVertices.reset();
 
-    // Check if P in vertex region outside A
-    btVector3 ab = b - a;
-    btVector3 ac = c - a;
-    btVector3 ap = p - a;
-    btScalar d1 = ab.dot(ap);
-    btScalar d2 = ac.dot(ap);
-    if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) 
+	// Check if P in vertex region outside A
+	btVector3 ab = b - a;
+	btVector3 ac = c - a;
+	btVector3 ap = p - a;
+	btScalar d1 = ab.dot(ap);
+	btScalar d2 = ac.dot(ap);
+	if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0))
 	{
 		result.m_closestPointOnSimplex = a;
 		result.m_usedVertices.usedVertexA = true;
-		result.setBarycentricCoordinates(1,0,0);
-		return true;// a; // barycentric coordinates (1,0,0)
+		result.setBarycentricCoordinates(1, 0, 0);
+		return true;  // a; // barycentric coordinates (1,0,0)
 	}
 
-    // Check if P in vertex region outside B
-    btVector3 bp = p - b;
-    btScalar d3 = ab.dot(bp);
-    btScalar d4 = ac.dot(bp);
-    if (d3 >= btScalar(0.0) && d4 <= d3) 
+	// Check if P in vertex region outside B
+	btVector3 bp = p - b;
+	btScalar d3 = ab.dot(bp);
+	btScalar d4 = ac.dot(bp);
+	if (d3 >= btScalar(0.0) && d4 <= d3)
 	{
 		result.m_closestPointOnSimplex = b;
 		result.m_usedVertices.usedVertexB = true;
-		result.setBarycentricCoordinates(0,1,0);
+		result.setBarycentricCoordinates(0, 1, 0);
 
-		return true; // b; // barycentric coordinates (0,1,0)
+		return true;  // b; // barycentric coordinates (0,1,0)
 	}
-    // Check if P in edge region of AB, if so return projection of P onto AB
-    btScalar vc = d1*d4 - d3*d2;
-    if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) {
-        btScalar v = d1 / (d1 - d3);
+	// Check if P in edge region of AB, if so return projection of P onto AB
+	btScalar vc = d1 * d4 - d3 * d2;
+	if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0))
+	{
+		btScalar v = d1 / (d1 - d3);
 		result.m_closestPointOnSimplex = a + v * ab;
 		result.m_usedVertices.usedVertexA = true;
 		result.m_usedVertices.usedVertexB = true;
-		result.setBarycentricCoordinates(1-v,v,0);
+		result.setBarycentricCoordinates(1 - v, v, 0);
 		return true;
-        //return a + v * ab; // barycentric coordinates (1-v,v,0)
-    }
-
-    // Check if P in vertex region outside C
-    btVector3 cp = p - c;
-    btScalar d5 = ab.dot(cp);
-    btScalar d6 = ac.dot(cp);
-    if (d6 >= btScalar(0.0) && d5 <= d6) 
+		//return a + v * ab; // barycentric coordinates (1-v,v,0)
+	}
+
+	// Check if P in vertex region outside C
+	btVector3 cp = p - c;
+	btScalar d5 = ab.dot(cp);
+	btScalar d6 = ac.dot(cp);
+	if (d6 >= btScalar(0.0) && d5 <= d6)
 	{
 		result.m_closestPointOnSimplex = c;
 		result.m_usedVertices.usedVertexC = true;
-		result.setBarycentricCoordinates(0,0,1);
-		return true;//c; // barycentric coordinates (0,0,1)
+		result.setBarycentricCoordinates(0, 0, 1);
+		return true;  //c; // barycentric coordinates (0,0,1)
 	}
 
-    // Check if P in edge region of AC, if so return projection of P onto AC
-    btScalar vb = d5*d2 - d1*d6;
-    if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) {
-        btScalar w = d2 / (d2 - d6);
+	// Check if P in edge region of AC, if so return projection of P onto AC
+	btScalar vb = d5 * d2 - d1 * d6;
+	if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0))
+	{
+		btScalar w = d2 / (d2 - d6);
 		result.m_closestPointOnSimplex = a + w * ac;
 		result.m_usedVertices.usedVertexA = true;
 		result.m_usedVertices.usedVertexC = true;
-		result.setBarycentricCoordinates(1-w,0,w);
+		result.setBarycentricCoordinates(1 - w, 0, w);
 		return true;
-        //return a + w * ac; // barycentric coordinates (1-w,0,w)
-    }
+		//return a + w * ac; // barycentric coordinates (1-w,0,w)
+	}
+
+	// Check if P in edge region of BC, if so return projection of P onto BC
+	btScalar va = d3 * d6 - d5 * d4;
+	if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0))
+	{
+		btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
 
-    // Check if P in edge region of BC, if so return projection of P onto BC
-    btScalar va = d3*d6 - d5*d4;
-    if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) {
-        btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
-		
 		result.m_closestPointOnSimplex = b + w * (c - b);
 		result.m_usedVertices.usedVertexB = true;
 		result.m_usedVertices.usedVertexC = true;
-		result.setBarycentricCoordinates(0,1-w,w);
-		return true;		
-       // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
-    }
-
-    // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
-    btScalar denom = btScalar(1.0) / (va + vb + vc);
-    btScalar v = vb * denom;
-    btScalar w = vc * denom;
-    
+		result.setBarycentricCoordinates(0, 1 - w, w);
+		return true;
+		// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
+	}
+
+	// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
+	btScalar denom = btScalar(1.0) / (va + vb + vc);
+	btScalar v = vb * denom;
+	btScalar w = vc * denom;
+
 	result.m_closestPointOnSimplex = a + ab * v + ac * w;
 	result.m_usedVertices.usedVertexA = true;
 	result.m_usedVertices.usedVertexB = true;
 	result.m_usedVertices.usedVertexC = true;
-	result.setBarycentricCoordinates(1-v-w,v,w);
-	
-	return true;
-//	return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
+	result.setBarycentricCoordinates(1 - v - w, v, w);
 
+	return true;
+	//	return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
 }
 
-
-
-
-
 /// Test if point p and d lie on opposite sides of plane through abc
 int btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d)
 {
-	btVector3 normal = (b-a).cross(c-a);
+	btVector3 normal = (b - a).cross(c - a);
 
-    btScalar signp = (p - a).dot(normal); // [AP AB AC]
-    btScalar signd = (d - a).dot( normal); // [AD AB AC]
+	btScalar signp = (p - a).dot(normal);  // [AP AB AC]
+	btScalar signd = (d - a).dot(normal);  // [AD AB AC]
 
 #ifdef CATCH_DEGENERATE_TETRAHEDRON
 #ifdef BT_USE_DOUBLE_PRECISION
-if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
+	if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
 	{
 		return -1;
 	}
 #else
 	if (signd * signd < (btScalar(1e-4) * btScalar(1e-4)))
 	{
-//		printf("affine dependent/degenerate\n");//
+		//		printf("affine dependent/degenerate\n");//
 		return -1;
 	}
 #endif
 
 #endif
 	// Points on opposite sides if expression signs are opposite
-    return signp * signd < btScalar(0.);
+	return signp * signd < btScalar(0.);
 }
 
-
-bool	btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult)
+bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult)
 {
 	btSubSimplexClosestResult tempResult;
 
-    // Start out assuming point inside all halfspaces, so closest to itself
+	// Start out assuming point inside all halfspaces, so closest to itself
 	finalResult.m_closestPointOnSimplex = p;
 	finalResult.m_usedVertices.reset();
-    finalResult.m_usedVertices.usedVertexA = true;
+	finalResult.m_usedVertices.usedVertexA = true;
 	finalResult.m_usedVertices.usedVertexB = true;
 	finalResult.m_usedVertices.usedVertexC = true;
 	finalResult.m_usedVertices.usedVertexD = true;
 
-    int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
+	int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
 	int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
-  	int	pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
-	int	pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
-
-   if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
-   {
-	   finalResult.m_degenerate = true;
-	   return false;
-   }
+	int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
+	int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
 
-   if (!pointOutsideABC  && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
-	 {
-		 return false;
-	 }
+	if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
+	{
+		finalResult.m_degenerate = true;
+		return false;
+	}
 
+	if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
+	{
+		return false;
+	}
 
-    btScalar bestSqDist = FLT_MAX;
-    // If point outside face abc then compute closest point on abc
-	if (pointOutsideABC) 
+	btScalar bestSqDist = FLT_MAX;
+	// If point outside face abc then compute closest point on abc
+	if (pointOutsideABC)
 	{
-        closestPtPointTriangle(p, a, b, c,tempResult);
+		closestPtPointTriangle(p, a, b, c, tempResult);
 		btVector3 q = tempResult.m_closestPointOnSimplex;
-		
-        btScalar sqDist = (q - p).dot( q - p);
-        // Update best closest point if (squared) distance is less than current best
-        if (sqDist < bestSqDist) {
+
+		btScalar sqDist = (q - p).dot(q - p);
+		// Update best closest point if (squared) distance is less than current best
+		if (sqDist < bestSqDist)
+		{
 			bestSqDist = sqDist;
 			finalResult.m_closestPointOnSimplex = q;
 			//convert result bitmask!
@@ -504,25 +481,22 @@ bool	btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const
 			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
 			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
 			finalResult.setBarycentricCoordinates(
-					tempResult.m_barycentricCoords[VERTA],
-					tempResult.m_barycentricCoords[VERTB],
-					tempResult.m_barycentricCoords[VERTC],
-					0
-			);
-
+				tempResult.m_barycentricCoords[VERTA],
+				tempResult.m_barycentricCoords[VERTB],
+				tempResult.m_barycentricCoords[VERTC],
+				0);
 		}
-    }
-  
+	}
 
 	// Repeat test for face acd
-	if (pointOutsideACD) 
+	if (pointOutsideACD)
 	{
-        closestPtPointTriangle(p, a, c, d,tempResult);
+		closestPtPointTriangle(p, a, c, d, tempResult);
 		btVector3 q = tempResult.m_closestPointOnSimplex;
 		//convert result bitmask!
 
-        btScalar sqDist = (q - p).dot( q - p);
-        if (sqDist < bestSqDist) 
+		btScalar sqDist = (q - p).dot(q - p);
+		if (sqDist < bestSqDist)
 		{
 			bestSqDist = sqDist;
 			finalResult.m_closestPointOnSimplex = q;
@@ -532,52 +506,46 @@ bool	btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const
 			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
 			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
 			finalResult.setBarycentricCoordinates(
-					tempResult.m_barycentricCoords[VERTA],
-					0,
-					tempResult.m_barycentricCoords[VERTB],
-					tempResult.m_barycentricCoords[VERTC]
-			);
-
+				tempResult.m_barycentricCoords[VERTA],
+				0,
+				tempResult.m_barycentricCoords[VERTB],
+				tempResult.m_barycentricCoords[VERTC]);
 		}
-    }
-    // Repeat test for face adb
+	}
+	// Repeat test for face adb
 
-	
 	if (pointOutsideADB)
 	{
-		closestPtPointTriangle(p, a, d, b,tempResult);
+		closestPtPointTriangle(p, a, d, b, tempResult);
 		btVector3 q = tempResult.m_closestPointOnSimplex;
 		//convert result bitmask!
 
-        btScalar sqDist = (q - p).dot( q - p);
-        if (sqDist < bestSqDist) 
+		btScalar sqDist = (q - p).dot(q - p);
+		if (sqDist < bestSqDist)
 		{
 			bestSqDist = sqDist;
 			finalResult.m_closestPointOnSimplex = q;
 			finalResult.m_usedVertices.reset();
 			finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
 			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
-			
+
 			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
 			finalResult.setBarycentricCoordinates(
-					tempResult.m_barycentricCoords[VERTA],
-					tempResult.m_barycentricCoords[VERTC],
-					0,
-					tempResult.m_barycentricCoords[VERTB]
-			);
-
+				tempResult.m_barycentricCoords[VERTA],
+				tempResult.m_barycentricCoords[VERTC],
+				0,
+				tempResult.m_barycentricCoords[VERTB]);
 		}
-    }
-    // Repeat test for face bdc
-    
+	}
+	// Repeat test for face bdc
 
 	if (pointOutsideBDC)
 	{
-        closestPtPointTriangle(p, b, d, c,tempResult);
+		closestPtPointTriangle(p, b, d, c, tempResult);
 		btVector3 q = tempResult.m_closestPointOnSimplex;
 		//convert result bitmask!
-        btScalar sqDist = (q - p).dot( q - p);
-        if (sqDist < bestSqDist) 
+		btScalar sqDist = (q - p).dot(q - p);
+		if (sqDist < bestSqDist)
 		{
 			bestSqDist = sqDist;
 			finalResult.m_closestPointOnSimplex = q;
@@ -588,25 +556,22 @@ bool	btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const
 			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
 
 			finalResult.setBarycentricCoordinates(
-					0,
-					tempResult.m_barycentricCoords[VERTA],
-					tempResult.m_barycentricCoords[VERTC],
-					tempResult.m_barycentricCoords[VERTB]
-			);
-
+				0,
+				tempResult.m_barycentricCoords[VERTA],
+				tempResult.m_barycentricCoords[VERTC],
+				tempResult.m_barycentricCoords[VERTB]);
 		}
-    }
+	}
 
 	//help! we ended up full !
-	
+
 	if (finalResult.m_usedVertices.usedVertexA &&
 		finalResult.m_usedVertices.usedVertexB &&
 		finalResult.m_usedVertices.usedVertexC &&
-		finalResult.m_usedVertices.usedVertexD) 
+		finalResult.m_usedVertices.usedVertexD)
 	{
 		return true;
 	}
 
-    return true;
+	return true;
 }
-
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h
index 2f389e27e3f..24a0a8f2df8 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h
@@ -13,23 +13,24 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef BT_VORONOI_SIMPLEX_SOLVER_H
 #define BT_VORONOI_SIMPLEX_SOLVER_H
 
 #include "btSimplexSolverInterface.h"
 
-
-
 #define VORONOI_SIMPLEX_MAX_VERTS 5
 
 ///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure
 #define BT_USE_EQUAL_VERTEX_THRESHOLD
-#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 1e-12f
+#else
+#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f
+#endif  //BT_USE_DOUBLE_PRECISION
 
-struct btUsageBitfield{
+struct btUsageBitfield
+{
 	btUsageBitfield()
 	{
 		reset();
@@ -42,140 +43,131 @@ struct btUsageBitfield{
 		usedVertexC = false;
 		usedVertexD = false;
 	}
-	unsigned short usedVertexA	: 1;
-	unsigned short usedVertexB	: 1;
-	unsigned short usedVertexC	: 1;
-	unsigned short usedVertexD	: 1;
-	unsigned short unused1		: 1;
-	unsigned short unused2		: 1;
-	unsigned short unused3		: 1;
-	unsigned short unused4		: 1;
+	unsigned short usedVertexA : 1;
+	unsigned short usedVertexB : 1;
+	unsigned short usedVertexC : 1;
+	unsigned short usedVertexD : 1;
+	unsigned short unused1 : 1;
+	unsigned short unused2 : 1;
+	unsigned short unused3 : 1;
+	unsigned short unused4 : 1;
 };
 
-
-struct	btSubSimplexClosestResult
+struct btSubSimplexClosestResult
 {
-	btVector3	m_closestPointOnSimplex;
+	btVector3 m_closestPointOnSimplex;
 	//MASK for m_usedVertices
-	//stores the simplex vertex-usage, using the MASK, 
+	//stores the simplex vertex-usage, using the MASK,
 	// if m_usedVertices & MASK then the related vertex is used
-	btUsageBitfield	m_usedVertices;
-	btScalar	m_barycentricCoords[4];
+	btUsageBitfield m_usedVertices;
+	btScalar m_barycentricCoords[4];
 	bool m_degenerate;
 
-	void	reset()
+	void reset()
 	{
 		m_degenerate = false;
 		setBarycentricCoordinates();
 		m_usedVertices.reset();
 	}
-	bool	isValid()
+	bool isValid()
 	{
 		bool valid = (m_barycentricCoords[0] >= btScalar(0.)) &&
-			(m_barycentricCoords[1] >= btScalar(0.)) &&
-			(m_barycentricCoords[2] >= btScalar(0.)) &&
-			(m_barycentricCoords[3] >= btScalar(0.));
-
+					 (m_barycentricCoords[1] >= btScalar(0.)) &&
+					 (m_barycentricCoords[2] >= btScalar(0.)) &&
+					 (m_barycentricCoords[3] >= btScalar(0.));
 
 		return valid;
 	}
-	void	setBarycentricCoordinates(btScalar a=btScalar(0.),btScalar b=btScalar(0.),btScalar c=btScalar(0.),btScalar d=btScalar(0.))
+	void setBarycentricCoordinates(btScalar a = btScalar(0.), btScalar b = btScalar(0.), btScalar c = btScalar(0.), btScalar d = btScalar(0.))
 	{
 		m_barycentricCoords[0] = a;
 		m_barycentricCoords[1] = b;
 		m_barycentricCoords[2] = c;
 		m_barycentricCoords[3] = d;
 	}
-
 };
 
 /// btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin.
 /// Can be used with GJK, as an alternative to Johnson distance algorithm.
 #ifdef NO_VIRTUAL_INTERFACE
-ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver
+ATTRIBUTE_ALIGNED16(class)
+btVoronoiSimplexSolver
 #else
-ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver : public btSimplexSolverInterface
+ATTRIBUTE_ALIGNED16(class)
+btVoronoiSimplexSolver : public btSimplexSolverInterface
 #endif
 {
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	int	m_numVertices;
+	int m_numVertices;
 
-	btVector3	m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
-	btVector3	m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
-	btVector3	m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
+	btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
+	btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
+	btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
 
-	
-
-	btVector3	m_cachedP1;
-	btVector3	m_cachedP2;
-	btVector3	m_cachedV;
-	btVector3	m_lastW;
-	
-	btScalar	m_equalVertexThreshold;
-	bool		m_cachedValidClosest;
+	btVector3 m_cachedP1;
+	btVector3 m_cachedP2;
+	btVector3 m_cachedV;
+	btVector3 m_lastW;
 
+	btScalar m_equalVertexThreshold;
+	bool m_cachedValidClosest;
 
 	btSubSimplexClosestResult m_cachedBC;
 
-	bool	m_needsUpdate;
-	
-	void	removeVertex(int index);
-	void	reduceVertices (const btUsageBitfield& usedVerts);
-	bool	updateClosestVectorAndPoints();
+	bool m_needsUpdate;
 
-	bool	closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult);
-	int		pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d);
-	bool	closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result);
+	void removeVertex(int index);
+	void reduceVertices(const btUsageBitfield& usedVerts);
+	bool updateClosestVectorAndPoints();
 
-public:
+	bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult);
+	int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d);
+	bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, btSubSimplexClosestResult& result);
 
+public:
 	btVoronoiSimplexSolver()
-		:  m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD)
+		: m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD)
 	{
 	}
-	 void reset();
-
-	 void addVertex(const btVector3& w, const btVector3& p, const btVector3& q);
+	void reset();
 
-	 void	setEqualVertexThreshold(btScalar threshold)
-	 {
-		 m_equalVertexThreshold = threshold;
-	 }
+	void addVertex(const btVector3& w, const btVector3& p, const btVector3& q);
 
-	 btScalar	getEqualVertexThreshold() const
-	 {
-		 return m_equalVertexThreshold;
-	 }
+	void setEqualVertexThreshold(btScalar threshold)
+	{
+		m_equalVertexThreshold = threshold;
+	}
 
-	 bool closest(btVector3& v);
+	btScalar getEqualVertexThreshold() const
+	{
+		return m_equalVertexThreshold;
+	}
 
-	 btScalar maxVertex();
+	bool closest(btVector3 & v);
 
-	 bool fullSimplex() const
-	 {
-		 return (m_numVertices == 4);
-	 }
+	btScalar maxVertex();
 
-	 int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const;
+	bool fullSimplex() const
+	{
+		return (m_numVertices == 4);
+	}
 
-	 bool inSimplex(const btVector3& w);
-	
-	 void backup_closest(btVector3& v) ;
+	int getSimplex(btVector3 * pBuf, btVector3 * qBuf, btVector3 * yBuf) const;
 
-	 bool emptySimplex() const ;
+	bool inSimplex(const btVector3& w);
 
-	 void compute_points(btVector3& p1, btVector3& p2) ;
+	void backup_closest(btVector3 & v);
 
-	 int numVertices() const 
-	 {
-		 return m_numVertices;
-	 }
+	bool emptySimplex() const;
 
+	void compute_points(btVector3 & p1, btVector3 & p2);
 
+	int numVertices() const
+	{
+		return m_numVertices;
+	}
 };
 
-#endif //BT_VORONOI_SIMPLEX_SOLVER_H
-
+#endif  //BT_VORONOI_SIMPLEX_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h b/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h
index dffb06dfe94..2ccf317b927 100644
--- a/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h
+++ b/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h
@@ -26,22 +26,21 @@ class btCollisionWorld;
 class btCharacterControllerInterface : public btActionInterface
 {
 public:
-	btCharacterControllerInterface () {};
-	virtual ~btCharacterControllerInterface () {};
-	
-	virtual void	setWalkDirection(const btVector3& walkDirection) = 0;
-	virtual void	setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0;
-	virtual void	reset ( btCollisionWorld* collisionWorld ) = 0;
-	virtual void	warp (const btVector3& origin) = 0;
-
-	virtual void	preStep ( btCollisionWorld* collisionWorld) = 0;
-	virtual void	playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0;
-	virtual bool	canJump () const = 0;
-	virtual void	jump () = 0;
-
-	virtual bool	onGround () const = 0;
-	virtual void	setUpInterpolate (bool value) = 0;
-};
+	btCharacterControllerInterface(){};
+	virtual ~btCharacterControllerInterface(){};
+
+	virtual void setWalkDirection(const btVector3& walkDirection) = 0;
+	virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0;
+	virtual void reset(btCollisionWorld* collisionWorld) = 0;
+	virtual void warp(const btVector3& origin) = 0;
 
-#endif //BT_CHARACTER_CONTROLLER_INTERFACE_H
+	virtual void preStep(btCollisionWorld* collisionWorld) = 0;
+	virtual void playerStep(btCollisionWorld* collisionWorld, btScalar dt) = 0;
+	virtual bool canJump() const = 0;
+	virtual void jump(const btVector3& dir = btVector3(0, 0, 0)) = 0;
+
+	virtual bool onGround() const = 0;
+	virtual void setUpInterpolate(bool value) = 0;
+};
 
+#endif  //BT_CHARACTER_CONTROLLER_INTERFACE_H
diff --git a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp
index 31faf1df5e3..2bbccb291c7 100644
--- a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp
+++ b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include <stdio.h>
 #include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/CollisionDispatch/btGhostObject.h"
@@ -24,20 +23,19 @@ subject to the following restrictions:
 #include "LinearMath/btDefaultMotionState.h"
 #include "btKinematicCharacterController.h"
 
-
 // static helper method
 static btVector3
 getNormalizedVector(const btVector3& v)
 {
 	btVector3 n(0, 0, 0);
 
-	if (v.length() > SIMD_EPSILON) {
+	if (v.length() > SIMD_EPSILON)
+	{
 		n = v.normalized();
 	}
 	return n;
 }
 
-
 ///@todo Interact with dynamic objects,
 ///Ride kinematicly animated platforms properly
 ///More realistic (or maybe just a config option) falling
@@ -47,18 +45,19 @@ getNormalizedVector(const btVector3& v)
 class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
 {
 public:
-	btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
+	btKinematicClosestNotMeRayResultCallback(btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
 	{
 		m_me = me;
 	}
 
-	virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
+	virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace)
 	{
 		if (rayResult.m_collisionObject == m_me)
 			return 1.0;
 
-		return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace);
+		return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
 	}
+
 protected:
 	btCollisionObject* m_me;
 };
@@ -66,15 +65,12 @@ protected:
 class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
 {
 public:
-	btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
-	: btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
-	, m_me(me)
-	, m_up(up)
-	, m_minSlopeDot(minSlopeDot)
+	btKinematicClosestNotMeConvexResultCallback(btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
+		: btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)), m_me(me), m_up(up), m_minSlopeDot(minSlopeDot)
 	{
 	}
 
-	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
+	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace)
 	{
 		if (convexResult.m_hitCollisionObject == m_me)
 			return btScalar(1.0);
@@ -86,19 +82,22 @@ public:
 		if (normalInWorldSpace)
 		{
 			hitNormalWorld = convexResult.m_hitNormalLocal;
-		} else
+		}
+		else
 		{
 			///need to transform normal into worldspace
-			hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
+			hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis() * convexResult.m_hitNormalLocal;
 		}
 
 		btScalar dotUp = m_up.dot(hitNormalWorld);
-		if (dotUp < m_minSlopeDot) {
+		if (dotUp < m_minSlopeDot)
+		{
 			return btScalar(1.0);
 		}
 
-		return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
+		return ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
 	}
+
 protected:
 	btCollisionObject* m_me;
 	const btVector3 m_up;
@@ -110,7 +109,7 @@ protected:
  *
  * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
  */
-btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal)
+btVector3 btKinematicCharacterController::computeReflectionDirection(const btVector3& direction, const btVector3& normal)
 {
 	return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
 }
@@ -118,7 +117,7 @@ btVector3 btKinematicCharacterController::computeReflectionDirection (const btVe
 /*
  * Returns the portion of 'direction' that is parallel to 'normal'
  */
-btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal)
+btVector3 btKinematicCharacterController::parallelComponent(const btVector3& direction, const btVector3& normal)
 {
 	btScalar magnitude = direction.dot(normal);
 	return normal * magnitude;
@@ -127,38 +126,46 @@ btVector3 btKinematicCharacterController::parallelComponent (const btVector3& di
 /*
  * Returns the portion of 'direction' that is perpindicular to 'normal'
  */
-btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal)
+btVector3 btKinematicCharacterController::perpindicularComponent(const btVector3& direction, const btVector3& normal)
 {
 	return direction - parallelComponent(direction, normal);
 }
 
-btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
+btKinematicCharacterController::btKinematicCharacterController(btPairCachingGhostObject* ghostObject, btConvexShape* convexShape, btScalar stepHeight, const btVector3& up)
 {
-	m_upAxis = upAxis;
+	m_ghostObject = ghostObject;
+	m_up.setValue(0.0f, 0.0f, 1.0f);
+	m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
 	m_addedMargin = 0.02;
-	m_walkDirection.setValue(0,0,0);
+	m_walkDirection.setValue(0.0, 0.0, 0.0);
+	m_AngVel.setValue(0.0, 0.0, 0.0);
 	m_useGhostObjectSweepTest = true;
-	m_ghostObject = ghostObject;
-	m_stepHeight = stepHeight;
 	m_turnAngle = btScalar(0.0);
-	m_convexShape=convexShape;	
-	m_useWalkDirection = true;	// use walk direction by default, legacy behavior
+	m_convexShape = convexShape;
+	m_useWalkDirection = true;  // use walk direction by default, legacy behavior
 	m_velocityTimeInterval = 0.0;
 	m_verticalVelocity = 0.0;
 	m_verticalOffset = 0.0;
-	m_gravity = 9.8 * 3 ; // 3G acceleration.
-	m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
-	m_jumpSpeed = 10.0; // ?
+	m_gravity = 9.8 * 3.0;  // 3G acceleration.
+	m_fallSpeed = 55.0;     // Terminal velocity of a sky diver in m/s.
+	m_jumpSpeed = 10.0;     // ?
+	m_SetjumpSpeed = m_jumpSpeed;
 	m_wasOnGround = false;
 	m_wasJumping = false;
 	m_interpolateUp = true;
-	setMaxSlope(btRadians(45.0));
-	m_currentStepOffset = 0;
+	m_currentStepOffset = 0.0;
+	m_maxPenetrationDepth = 0.2;
 	full_drop = false;
 	bounce_fix = false;
+	m_linearDamping = btScalar(0.0);
+	m_angularDamping = btScalar(0.0);
+
+	setUp(up);
+	setStepHeight(stepHeight);
+	setMaxSlope(btRadians(45.0));
 }
 
-btKinematicCharacterController::~btKinematicCharacterController ()
+btKinematicCharacterController::~btKinematicCharacterController()
 {
 }
 
@@ -167,7 +174,7 @@ btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
 	return m_ghostObject;
 }
 
-bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
+bool btKinematicCharacterController::recoverFromPenetration(btCollisionWorld* collisionWorld)
 {
 	// Here we must refresh the overlapping paircache as the penetrating movement itself or the
 	// previous recovery iteration might have used setWorldTransform and pushed us into an object
@@ -178,19 +185,19 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 	// paircache and the ghostobject's internal paircache at the same time.    /BW
 
 	btVector3 minAabb, maxAabb;
-	m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb,maxAabb);
-	collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(), 
-						 minAabb, 
-						 maxAabb, 
-						 collisionWorld->getDispatcher());
-						 
+	m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb, maxAabb);
+	collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(),
+											 minAabb,
+											 maxAabb,
+											 collisionWorld->getDispatcher());
+
 	bool penetration = false;
 
 	collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
 
 	m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
-	
-	btScalar maxPen = btScalar(0.0);
+
+	//	btScalar maxPen = btScalar(0.0);
 	for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
 	{
 		m_manifoldArray.resize(0);
@@ -198,195 +205,240 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 		btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
 
 		btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
-                btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
+		btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
 
 		if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
 			continue;
-		
+
+		if (!needsCollision(obj0, obj1))
+			continue;
+
 		if (collisionPair->m_algorithm)
 			collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
 
-		
-		for (int j=0;j<m_manifoldArray.size();j++)
+		for (int j = 0; j < m_manifoldArray.size(); j++)
 		{
 			btPersistentManifold* manifold = m_manifoldArray[j];
 			btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
-			for (int p=0;p<manifold->getNumContacts();p++)
+			for (int p = 0; p < manifold->getNumContacts(); p++)
 			{
-				const btManifoldPoint&pt = manifold->getContactPoint(p);
+				const btManifoldPoint& pt = manifold->getContactPoint(p);
 
 				btScalar dist = pt.getDistance();
 
-				if (dist < 0.0)
+				if (dist < -m_maxPenetrationDepth)
 				{
-					if (dist < maxPen)
-					{
-						maxPen = dist;
-						m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
+					// TODO: cause problems on slopes, not sure if it is needed
+					//if (dist < maxPen)
+					//{
+					//	maxPen = dist;
+					//	m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
 
-					}
+					//}
 					m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
 					penetration = true;
-				} else {
+				}
+				else
+				{
 					//printf("touching %f\n", dist);
 				}
 			}
-			
+
 			//manifold->clearManifold();
 		}
 	}
 	btTransform newTrans = m_ghostObject->getWorldTransform();
 	newTrans.setOrigin(m_currentPosition);
 	m_ghostObject->setWorldTransform(newTrans);
-//	printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
+	//	printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
 	return penetration;
 }
 
-void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
+void btKinematicCharacterController::stepUp(btCollisionWorld* world)
 {
+	btScalar stepHeight = 0.0f;
+	if (m_verticalVelocity < 0.0)
+		stepHeight = m_stepHeight;
+
 	// phase 1: up
 	btTransform start, end;
-	m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
 
-	start.setIdentity ();
-	end.setIdentity ();
+	start.setIdentity();
+	end.setIdentity();
 
 	/* FIXME: Handle penetration properly */
-	start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
-	end.setOrigin (m_targetPosition);
+	start.setOrigin(m_currentPosition);
+
+	m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0.f ? m_verticalOffset : 0.f));
+	m_currentPosition = m_targetPosition;
 
-	btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
+	end.setOrigin(m_targetPosition);
+
+	start.setRotation(m_currentOrientation);
+	end.setRotation(m_targetOrientation);
+
+	btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine);
 	callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
 	callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
-	
+
 	if (m_useGhostObjectSweepTest)
 	{
-		m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
+		m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
 	}
 	else
 	{
-		world->convexSweepTest (m_convexShape, start, end, callback);
+		world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
 	}
-	
-	if (callback.hasHit())
+
+	if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
 	{
 		// Only modify the position if the hit was a slope and not a wall or ceiling.
-		if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
+		if (callback.m_hitNormalWorld.dot(m_up) > 0.0)
 		{
 			// we moved up only a fraction of the step height
-			m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
+			m_currentStepOffset = stepHeight * callback.m_closestHitFraction;
 			if (m_interpolateUp == true)
-				m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+				m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 			else
 				m_currentPosition = m_targetPosition;
 		}
-		m_verticalVelocity = 0.0;
-		m_verticalOffset = 0.0;
-	} else {
-		m_currentStepOffset = m_stepHeight;
+
+		btTransform& xform = m_ghostObject->getWorldTransform();
+		xform.setOrigin(m_currentPosition);
+		m_ghostObject->setWorldTransform(xform);
+
+		// fix penetration if we hit a ceiling for example
+		int numPenetrationLoops = 0;
+		m_touchingContact = false;
+		while (recoverFromPenetration(world))
+		{
+			numPenetrationLoops++;
+			m_touchingContact = true;
+			if (numPenetrationLoops > 4)
+			{
+				//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+				break;
+			}
+		}
+		m_targetPosition = m_ghostObject->getWorldTransform().getOrigin();
+		m_currentPosition = m_targetPosition;
+
+		if (m_verticalOffset > 0)
+		{
+			m_verticalOffset = 0.0;
+			m_verticalVelocity = 0.0;
+			m_currentStepOffset = m_stepHeight;
+		}
+	}
+	else
+	{
+		m_currentStepOffset = stepHeight;
 		m_currentPosition = m_targetPosition;
 	}
 }
 
-void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
+bool btKinematicCharacterController::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1)
+{
+	bool collides = (body0->getBroadphaseHandle()->m_collisionFilterGroup & body1->getBroadphaseHandle()->m_collisionFilterMask) != 0;
+	collides = collides && (body1->getBroadphaseHandle()->m_collisionFilterGroup & body0->getBroadphaseHandle()->m_collisionFilterMask);
+	return collides;
+}
+
+void btKinematicCharacterController::updateTargetPositionBasedOnCollision(const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
 {
 	btVector3 movementDirection = m_targetPosition - m_currentPosition;
 	btScalar movementLength = movementDirection.length();
-	if (movementLength>SIMD_EPSILON)
+	if (movementLength > SIMD_EPSILON)
 	{
 		movementDirection.normalize();
 
-		btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal);
+		btVector3 reflectDir = computeReflectionDirection(movementDirection, hitNormal);
 		reflectDir.normalize();
 
 		btVector3 parallelDir, perpindicularDir;
 
-		parallelDir = parallelComponent (reflectDir, hitNormal);
-		perpindicularDir = perpindicularComponent (reflectDir, hitNormal);
+		parallelDir = parallelComponent(reflectDir, hitNormal);
+		perpindicularDir = perpindicularComponent(reflectDir, hitNormal);
 
 		m_targetPosition = m_currentPosition;
-		if (0)//tangentMag != 0.0)
+		if (0)  //tangentMag != 0.0)
 		{
-			btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength);
-//			printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
-			m_targetPosition +=  parComponent;
+			btVector3 parComponent = parallelDir * btScalar(tangentMag * movementLength);
+			//			printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
+			m_targetPosition += parComponent;
 		}
 
 		if (normalMag != 0.0)
 		{
-			btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength);
-//			printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
+			btVector3 perpComponent = perpindicularDir * btScalar(normalMag * movementLength);
+			//			printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
 			m_targetPosition += perpComponent;
 		}
-	} else
+	}
+	else
 	{
-//		printf("movementLength don't normalize a zero vector\n");
+		//		printf("movementLength don't normalize a zero vector\n");
 	}
 }
 
-void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove)
+void btKinematicCharacterController::stepForwardAndStrafe(btCollisionWorld* collisionWorld, const btVector3& walkMove)
 {
 	// printf("m_normalizedDirection=%f,%f,%f\n",
 	// 	m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
 	// phase 2: forward and strafe
 	btTransform start, end;
+
 	m_targetPosition = m_currentPosition + walkMove;
 
-	start.setIdentity ();
-	end.setIdentity ();
-	
-	btScalar fraction = 1.0;
-	btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
-//	printf("distance2=%f\n",distance2);
+	start.setIdentity();
+	end.setIdentity();
 
-	if (m_touchingContact)
-	{
-		if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
-		{
-			//interferes with step movement
-			//updateTargetPositionBasedOnCollision (m_touchingNormal);
-		}
-	}
+	btScalar fraction = 1.0;
+	btScalar distance2 = (m_currentPosition - m_targetPosition).length2();
+	//	printf("distance2=%f\n",distance2);
 
 	int maxIter = 10;
 
 	while (fraction > btScalar(0.01) && maxIter-- > 0)
 	{
-		start.setOrigin (m_currentPosition);
-		end.setOrigin (m_targetPosition);
+		start.setOrigin(m_currentPosition);
+		end.setOrigin(m_targetPosition);
 		btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
 
-		btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
+		start.setRotation(m_currentOrientation);
+		end.setRotation(m_targetOrientation);
+
+		btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, sweepDirNegative, btScalar(0.0));
 		callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
 		callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
-
 		btScalar margin = m_convexShape->getMargin();
 		m_convexShape->setMargin(margin + m_addedMargin);
 
-
-		if (m_useGhostObjectSweepTest)
+		if (!(start == end))
 		{
-			m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-		} else
-		{
-			collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			if (m_useGhostObjectSweepTest)
+			{
+				m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			}
+			else
+			{
+				collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			}
 		}
-		
 		m_convexShape->setMargin(margin);
 
-		
 		fraction -= callback.m_closestHitFraction;
 
-		if (callback.hasHit())
-		{	
+		if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
+		{
 			// we moved only a fraction
 			//btScalar hitDistance;
 			//hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
 
-//			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+			//			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 
-			updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
+			updateTargetPositionBasedOnCollision(callback.m_hitNormalWorld);
 			btVector3 currentDir = m_targetPosition - m_currentPosition;
 			distance2 = currentDir.length2();
 			if (distance2 > SIMD_EPSILON)
@@ -397,115 +449,120 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 				{
 					break;
 				}
-			} else
+			}
+			else
 			{
-//				printf("currentDir: don't normalize a zero vector\n");
+				//				printf("currentDir: don't normalize a zero vector\n");
 				break;
 			}
-
-		} else {
-			// we moved whole way
+		}
+		else
+		{
 			m_currentPosition = m_targetPosition;
 		}
-
-	//	if (callback.m_closestHitFraction == 0.f)
-	//		break;
-
 	}
 }
 
-void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
+void btKinematicCharacterController::stepDown(btCollisionWorld* collisionWorld, btScalar dt)
 {
 	btTransform start, end, end_double;
 	bool runonce = false;
 
 	// phase 3: down
 	/*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
-	btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
+	btVector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep);
 	btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
-	btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; 
+	btVector3 gravity_drop = m_up * downVelocity; 
 	m_targetPosition -= (step_drop + gravity_drop);*/
 
 	btVector3 orig_position = m_targetPosition;
-	
-	btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
 
-	if(downVelocity > 0.0 && downVelocity > m_fallSpeed
-		&& (m_wasOnGround || !m_wasJumping))
+	btScalar downVelocity = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt;
+
+	if (m_verticalVelocity > 0.0)
+		return;
+
+	if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
 		downVelocity = m_fallSpeed;
 
-	btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+	btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
 	m_targetPosition -= step_drop;
 
-	btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
-        callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
-        callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
+	btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine);
+	callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
+	callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
-        btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
-        callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
-        callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
+	btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine);
+	callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
+	callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
 	while (1)
 	{
-		start.setIdentity ();
-		end.setIdentity ();
+		start.setIdentity();
+		end.setIdentity();
 
-		end_double.setIdentity ();
+		end_double.setIdentity();
 
-		start.setOrigin (m_currentPosition);
-		end.setOrigin (m_targetPosition);
+		start.setOrigin(m_currentPosition);
+		end.setOrigin(m_targetPosition);
+
+		start.setRotation(m_currentOrientation);
+		end.setRotation(m_targetOrientation);
 
 		//set double test for 2x the step drop, to check for a large drop vs small drop
-		end_double.setOrigin (m_targetPosition - step_drop);
+		end_double.setOrigin(m_targetPosition - step_drop);
 
 		if (m_useGhostObjectSweepTest)
 		{
-			m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 
-			if (!callback.hasHit())
+			if (!callback.hasHit() && m_ghostObject->hasContactResponse())
 			{
 				//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
-				m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+				m_ghostObject->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 			}
-		} else
+		}
+		else
 		{
-			collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 
-			if (!callback.hasHit())
-					{
-							//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
-							collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-					}
+			if (!callback.hasHit() && m_ghostObject->hasContactResponse())
+			{
+				//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
+				collisionWorld->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			}
 		}
-	
-		btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
-		bool has_hit = false;
+
+		btScalar downVelocity2 = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt;
+		bool has_hit;
 		if (bounce_fix == true)
-			has_hit = callback.hasHit() || callback2.hasHit();
+			has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
 		else
-			has_hit = callback2.hasHit();
+			has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback2.m_hitCollisionObject);
+
+		btScalar stepHeight = 0.0f;
+		if (m_verticalVelocity < 0.0)
+			stepHeight = m_stepHeight;
 
-		if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
-					&& (m_wasOnGround || !m_wasJumping))
+		if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false && (m_wasOnGround || !m_wasJumping))
 		{
 			//redo the velocity calculation when falling a small amount, for fast stairs motion
 			//for larger falls, use the smoother/slower interpolated movement by not touching the target position
 
 			m_targetPosition = orig_position;
-					downVelocity = m_stepHeight;
+			downVelocity = stepHeight;
 
-				btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+			step_drop = m_up * (m_currentStepOffset + downVelocity);
 			m_targetPosition -= step_drop;
 			runonce = true;
-			continue; //re-run previous tests
+			continue;  //re-run previous tests
 		}
 		break;
 	}
 
-	if (callback.hasHit() || runonce == true)
+	if ((m_ghostObject->hasContactResponse() && (callback.hasHit() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))) || runonce == true)
 	{
 		// we dropped a fraction of the height -> hit floor
-
 		btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
 
 		//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
@@ -513,32 +570,34 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		if (bounce_fix == true)
 		{
 			if (full_drop == true)
-                                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
-                        else
-                                //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
-                                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
+				m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+			else
+				//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
+				m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, fraction);
 		}
 		else
-			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+			m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 
 		full_drop = false;
 
 		m_verticalVelocity = 0.0;
 		m_verticalOffset = 0.0;
 		m_wasJumping = false;
-	} else {
+	}
+	else
+	{
 		// we dropped the full height
-		
+
 		full_drop = true;
 
 		if (bounce_fix == true)
 		{
-			downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
+			downVelocity = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt;
 			if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
 			{
-				m_targetPosition += step_drop; //undo previous target change
+				m_targetPosition += step_drop;  //undo previous target change
 				downVelocity = m_fallSpeed;
-				step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+				step_drop = m_up * (m_currentStepOffset + downVelocity);
 				m_targetPosition -= step_drop;
 			}
 		}
@@ -548,30 +607,22 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 	}
 }
 
-
-
-void btKinematicCharacterController::setWalkDirection
-(
-const btVector3& walkDirection
-)
+void btKinematicCharacterController::setWalkDirection(
+	const btVector3& walkDirection)
 {
 	m_useWalkDirection = true;
 	m_walkDirection = walkDirection;
 	m_normalizedDirection = getNormalizedVector(m_walkDirection);
 }
 
-
-
-void btKinematicCharacterController::setVelocityForTimeInterval
-(
-const btVector3& velocity,
-btScalar timeInterval
-)
+void btKinematicCharacterController::setVelocityForTimeInterval(
+	const btVector3& velocity,
+	btScalar timeInterval)
 {
-//	printf("setVelocity!\n");
-//	printf("  interval: %f\n", timeInterval);
-//	printf("  velocity: (%f, %f, %f)\n",
-//		 velocity.x(), velocity.y(), velocity.z());
+	//	printf("setVelocity!\n");
+	//	printf("  interval: %f\n", timeInterval);
+	//	printf("  velocity: (%f, %f, %f)\n",
+	//		 velocity.x(), velocity.y(), velocity.z());
 
 	m_useWalkDirection = false;
 	m_walkDirection = velocity;
@@ -579,93 +630,175 @@ btScalar timeInterval
 	m_velocityTimeInterval += timeInterval;
 }
 
-void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
+void btKinematicCharacterController::setAngularVelocity(const btVector3& velocity)
 {
-        m_verticalVelocity = 0.0;
-        m_verticalOffset = 0.0;
-        m_wasOnGround = false;
-        m_wasJumping = false;
-        m_walkDirection.setValue(0,0,0);
-        m_velocityTimeInterval = 0.0;
-
-        //clear pair cache
-        btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
-        while (cache->getOverlappingPairArray().size() > 0)
-        {
-                cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
-        }
+	m_AngVel = velocity;
 }
 
-void btKinematicCharacterController::warp (const btVector3& origin)
+const btVector3& btKinematicCharacterController::getAngularVelocity() const
 {
-	btTransform xform;
-	xform.setIdentity();
-	xform.setOrigin (origin);
-	m_ghostObject->setWorldTransform (xform);
+	return m_AngVel;
 }
 
-
-void btKinematicCharacterController::preStep (  btCollisionWorld* collisionWorld)
+void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity)
 {
-	
-	int numPenetrationLoops = 0;
-	m_touchingContact = false;
-	while (recoverFromPenetration (collisionWorld))
+	m_walkDirection = velocity;
+
+	// HACK: if we are moving in the direction of the up, treat it as a jump :(
+	if (m_walkDirection.length2() > 0)
 	{
-		numPenetrationLoops++;
-		m_touchingContact = true;
-		if (numPenetrationLoops > 4)
+		btVector3 w = velocity.normalized();
+		btScalar c = w.dot(m_up);
+		if (c != 0)
 		{
-			//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
-			break;
+			//there is a component in walkdirection for vertical velocity
+			btVector3 upComponent = m_up * (btSin(SIMD_HALF_PI - btAcos(c)) * m_walkDirection.length());
+			m_walkDirection -= upComponent;
+			m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();
+
+			if (c > 0.0f)
+			{
+				m_wasJumping = true;
+				m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
+			}
 		}
 	}
+	else
+		m_verticalVelocity = 0.0f;
+}
 
+btVector3 btKinematicCharacterController::getLinearVelocity() const
+{
+	return m_walkDirection + (m_verticalVelocity * m_up);
+}
+
+void btKinematicCharacterController::reset(btCollisionWorld* collisionWorld)
+{
+	m_verticalVelocity = 0.0;
+	m_verticalOffset = 0.0;
+	m_wasOnGround = false;
+	m_wasJumping = false;
+	m_walkDirection.setValue(0, 0, 0);
+	m_velocityTimeInterval = 0.0;
+
+	//clear pair cache
+	btHashedOverlappingPairCache* cache = m_ghostObject->getOverlappingPairCache();
+	while (cache->getOverlappingPairArray().size() > 0)
+	{
+		cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
+	}
+}
+
+void btKinematicCharacterController::warp(const btVector3& origin)
+{
+	btTransform xform;
+	xform.setIdentity();
+	xform.setOrigin(origin);
+	m_ghostObject->setWorldTransform(xform);
+}
+
+void btKinematicCharacterController::preStep(btCollisionWorld* collisionWorld)
+{
 	m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
 	m_targetPosition = m_currentPosition;
-//	printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
 
-	
+	m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
+	m_targetOrientation = m_currentOrientation;
+	//	printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
 }
 
-#include <stdio.h>
-
-void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
+void btKinematicCharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar dt)
 {
-//	printf("playerStep(): ");
-//	printf("  dt = %f", dt);
+	//	printf("playerStep(): ");
+	//	printf("  dt = %f", dt);
+
+	if (m_AngVel.length2() > 0.0f)
+	{
+		m_AngVel *= btPow(btScalar(1) - m_angularDamping, dt);
+	}
+
+	// integrate for angular velocity
+	if (m_AngVel.length2() > 0.0f)
+	{
+		btTransform xform;
+		xform = m_ghostObject->getWorldTransform();
+
+		btQuaternion rot(m_AngVel.normalized(), m_AngVel.length() * dt);
+
+		btQuaternion orn = rot * xform.getRotation();
+
+		xform.setRotation(orn);
+		m_ghostObject->setWorldTransform(xform);
+
+		m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
+		m_targetPosition = m_currentPosition;
+		m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
+		m_targetOrientation = m_currentOrientation;
+	}
 
 	// quick check...
-	if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) {
-//		printf("\n");
-		return;		// no motion
+	if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) 
+	{
+		//		printf("\n");
+		return;  // no motion
 	}
 
 	m_wasOnGround = onGround();
 
+	//btVector3 lvel = m_walkDirection;
+	//btScalar c = 0.0f;
+
+	if (m_walkDirection.length2() > 0)
+	{
+		// apply damping
+		m_walkDirection *= btPow(btScalar(1) - m_linearDamping, dt);
+	}
+
+	m_verticalVelocity *= btPow(btScalar(1) - m_linearDamping, dt);
+
 	// Update fall velocity.
 	m_verticalVelocity -= m_gravity * dt;
-	if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
+	if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
 	{
 		m_verticalVelocity = m_jumpSpeed;
 	}
-	if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
+	if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
 	{
 		m_verticalVelocity = -btFabs(m_fallSpeed);
 	}
 	m_verticalOffset = m_verticalVelocity * dt;
 
-
 	btTransform xform;
-	xform = m_ghostObject->getWorldTransform ();
-
-//	printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
-//	printf("walkSpeed=%f\n",walkSpeed);
-
-	stepUp (collisionWorld);
-	if (m_useWalkDirection) {
-		stepForwardAndStrafe (collisionWorld, m_walkDirection);
-	} else {
+	xform = m_ghostObject->getWorldTransform();
+
+	//	printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
+	//	printf("walkSpeed=%f\n",walkSpeed);
+
+	stepUp(collisionWorld);
+	//todo: Experimenting with behavior of controller when it hits a ceiling..
+	//bool hitUp = stepUp (collisionWorld);
+	//if (hitUp)
+	//{
+	//	m_verticalVelocity -= m_gravity * dt;
+	//	if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
+	//	{
+	//		m_verticalVelocity = m_jumpSpeed;
+	//	}
+	//	if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
+	//	{
+	//		m_verticalVelocity = -btFabs(m_fallSpeed);
+	//	}
+	//	m_verticalOffset = m_verticalVelocity * dt;
+
+	//	xform = m_ghostObject->getWorldTransform();
+	//}
+
+	if (m_useWalkDirection)
+	{
+		stepForwardAndStrafe(collisionWorld, m_walkDirection);
+	}
+	else
+	{
 		//printf("  time: %f", m_velocityTimeInterval);
 		// still have some time left for moving!
 		btScalar dtMoving =
@@ -680,42 +813,73 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 		// okay, step
 		stepForwardAndStrafe(collisionWorld, move);
 	}
-	stepDown (collisionWorld, dt);
-
+	stepDown(collisionWorld, dt);
+
+	//todo: Experimenting with max jump height
+	//if (m_wasJumping)
+	//{
+	//	btScalar ds = m_currentPosition[m_upAxis] - m_jumpPosition[m_upAxis];
+	//	if (ds > m_maxJumpHeight)
+	//	{
+	//		// substract the overshoot
+	//		m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight;
+
+	//		// max height was reached, so potential energy is at max
+	//		// and kinematic energy is 0, thus velocity is 0.
+	//		if (m_verticalVelocity > 0.0)
+	//			m_verticalVelocity = 0.0;
+	//	}
+	//}
 	// printf("\n");
 
-	xform.setOrigin (m_currentPosition);
-	m_ghostObject->setWorldTransform (xform);
+	xform.setOrigin(m_currentPosition);
+	m_ghostObject->setWorldTransform(xform);
+
+	int numPenetrationLoops = 0;
+	m_touchingContact = false;
+	while (recoverFromPenetration(collisionWorld))
+	{
+		numPenetrationLoops++;
+		m_touchingContact = true;
+		if (numPenetrationLoops > 4)
+		{
+			//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+			break;
+		}
+	}
 }
 
-void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
+void btKinematicCharacterController::setFallSpeed(btScalar fallSpeed)
 {
 	m_fallSpeed = fallSpeed;
 }
 
-void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
+void btKinematicCharacterController::setJumpSpeed(btScalar jumpSpeed)
 {
 	m_jumpSpeed = jumpSpeed;
+	m_SetjumpSpeed = m_jumpSpeed;
 }
 
-void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
+void btKinematicCharacterController::setMaxJumpHeight(btScalar maxJumpHeight)
 {
 	m_maxJumpHeight = maxJumpHeight;
 }
 
-bool btKinematicCharacterController::canJump () const
+bool btKinematicCharacterController::canJump() const
 {
 	return onGround();
 }
 
-void btKinematicCharacterController::jump ()
+void btKinematicCharacterController::jump(const btVector3& v)
 {
-	if (!canJump())
-		return;
-
+	m_jumpSpeed = v.length2() == 0 ? m_SetjumpSpeed : v.length();
 	m_verticalVelocity = m_jumpSpeed;
 	m_wasJumping = true;
 
+	m_jumpAxis = v.length2() == 0 ? m_up : v.normalized();
+
+	m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
+
 #if 0
 	currently no jumping.
 	btTransform xform;
@@ -727,14 +891,16 @@ void btKinematicCharacterController::jump ()
 #endif
 }
 
-void btKinematicCharacterController::setGravity(btScalar gravity)
+void btKinematicCharacterController::setGravity(const btVector3& gravity)
 {
-	m_gravity = gravity;
+	if (gravity.length2() > 0) setUpVector(-gravity);
+
+	m_gravity = gravity.length();
 }
 
-btScalar btKinematicCharacterController::getGravity() const
+btVector3 btKinematicCharacterController::getGravity() const
 {
-	return m_gravity;
+	return -m_gravity * m_up;
 }
 
 void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
@@ -748,16 +914,30 @@ btScalar btKinematicCharacterController::getMaxSlope() const
 	return m_maxSlopeRadians;
 }
 
-bool btKinematicCharacterController::onGround () const
+void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d)
+{
+	m_maxPenetrationDepth = d;
+}
+
+btScalar btKinematicCharacterController::getMaxPenetrationDepth() const
+{
+	return m_maxPenetrationDepth;
+}
+
+bool btKinematicCharacterController::onGround() const
 {
-	return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
+	return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON);
 }
 
+void btKinematicCharacterController::setStepHeight(btScalar h)
+{
+	m_stepHeight = h;
+}
 
 btVector3* btKinematicCharacterController::getUpAxisDirections()
 {
-	static btVector3 sUpAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
-	
+	static btVector3 sUpAxisDirection[3] = {btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f)};
+
 	return sUpAxisDirection;
 }
 
@@ -769,3 +949,48 @@ void btKinematicCharacterController::setUpInterpolate(bool value)
 {
 	m_interpolateUp = value;
 }
+
+void btKinematicCharacterController::setUp(const btVector3& up)
+{
+	if (up.length2() > 0 && m_gravity > 0.0f)
+	{
+		setGravity(-m_gravity * up.normalized());
+		return;
+	}
+
+	setUpVector(up);
+}
+
+void btKinematicCharacterController::setUpVector(const btVector3& up)
+{
+	if (m_up == up)
+		return;
+
+	btVector3 u = m_up;
+
+	if (up.length2() > 0)
+		m_up = up.normalized();
+	else
+		m_up = btVector3(0.0, 0.0, 0.0);
+
+	if (!m_ghostObject) return;
+	btQuaternion rot = getRotation(m_up, u);
+
+	//set orientation with new up
+	btTransform xform;
+	xform = m_ghostObject->getWorldTransform();
+	btQuaternion orn = rot.inverse() * xform.getRotation();
+	xform.setRotation(orn);
+	m_ghostObject->setWorldTransform(xform);
+}
+
+btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector3& v1) const
+{
+	if (v0.length2() == 0.0f || v1.length2() == 0.0f)
+	{
+		btQuaternion q;
+		return q;
+	}
+
+	return shortestArcQuatNormalize2(v0, v1);
+}
diff --git a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h
index add6f30a687..ff34fc871a5 100644
--- a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h
+++ b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_KINEMATIC_CHARACTER_CONTROLLER_H
 #define BT_KINEMATIC_CHARACTER_CONTROLLER_H
 
@@ -23,7 +22,6 @@ subject to the following restrictions:
 
 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 
-
 class btCollisionShape;
 class btConvexShape;
 class btRigidBody;
@@ -34,99 +32,111 @@ class btPairCachingGhostObject;
 ///btKinematicCharacterController is an object that supports a sliding motion in a world.
 ///It uses a ghost object and convex sweep test to test for upcoming collisions. This is combined with discrete collision detection to recover from penetrations.
 ///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user.
-ATTRIBUTE_ALIGNED16(class) btKinematicCharacterController : public btCharacterControllerInterface
+ATTRIBUTE_ALIGNED16(class)
+btKinematicCharacterController : public btCharacterControllerInterface
 {
 protected:
-
 	btScalar m_halfHeight;
-	
+
 	btPairCachingGhostObject* m_ghostObject;
-	btConvexShape*	m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
-	
+	btConvexShape* m_convexShape;  //is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
+
+	btScalar m_maxPenetrationDepth;
 	btScalar m_verticalVelocity;
 	btScalar m_verticalOffset;
 	btScalar m_fallSpeed;
 	btScalar m_jumpSpeed;
+	btScalar m_SetjumpSpeed;
 	btScalar m_maxJumpHeight;
-	btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
-	btScalar m_maxSlopeCosine;  // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
+	btScalar m_maxSlopeRadians;  // Slope angle that is set (used for returning the exact value)
+	btScalar m_maxSlopeCosine;   // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
 	btScalar m_gravity;
 
 	btScalar m_turnAngle;
-	
+
 	btScalar m_stepHeight;
 
-	btScalar	m_addedMargin;//@todo: remove this and fix the code
+	btScalar m_addedMargin;  //@todo: remove this and fix the code
 
 	///this is the desired walk direction, set by the user
-	btVector3	m_walkDirection;
-	btVector3	m_normalizedDirection;
+	btVector3 m_walkDirection;
+	btVector3 m_normalizedDirection;
+	btVector3 m_AngVel;
+
+	btVector3 m_jumpPosition;
 
 	//some internal variables
 	btVector3 m_currentPosition;
-	btScalar  m_currentStepOffset;
+	btScalar m_currentStepOffset;
 	btVector3 m_targetPosition;
 
+	btQuaternion m_currentOrientation;
+	btQuaternion m_targetOrientation;
+
 	///keep track of the contact manifolds
-	btManifoldArray	m_manifoldArray;
+	btManifoldArray m_manifoldArray;
 
 	bool m_touchingContact;
 	btVector3 m_touchingNormal;
 
-	bool  m_wasOnGround;
-	bool  m_wasJumping;
-	bool	m_useGhostObjectSweepTest;
-	bool	m_useWalkDirection;
-	btScalar	m_velocityTimeInterval;
-	int m_upAxis;
+	btScalar m_linearDamping;
+	btScalar m_angularDamping;
+
+	bool m_wasOnGround;
+	bool m_wasJumping;
+	bool m_useGhostObjectSweepTest;
+	bool m_useWalkDirection;
+	btScalar m_velocityTimeInterval;
+	btVector3 m_up;
+	btVector3 m_jumpAxis;
 
 	static btVector3* getUpAxisDirections();
-	bool  m_interpolateUp;
-	bool  full_drop;
-	bool  bounce_fix;
-
-	btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
-	btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
-	btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal);
-
-	bool recoverFromPenetration ( btCollisionWorld* collisionWorld);
-	void stepUp (btCollisionWorld* collisionWorld);
-	void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
-	void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
-	void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
-public:
+	bool m_interpolateUp;
+	bool full_drop;
+	bool bounce_fix;
+
+	btVector3 computeReflectionDirection(const btVector3& direction, const btVector3& normal);
+	btVector3 parallelComponent(const btVector3& direction, const btVector3& normal);
+	btVector3 perpindicularComponent(const btVector3& direction, const btVector3& normal);
+
+	bool recoverFromPenetration(btCollisionWorld * collisionWorld);
+	void stepUp(btCollisionWorld * collisionWorld);
+	void updateTargetPositionBasedOnCollision(const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
+	void stepForwardAndStrafe(btCollisionWorld * collisionWorld, const btVector3& walkMove);
+	void stepDown(btCollisionWorld * collisionWorld, btScalar dt);
+
+	virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1);
 
+	void setUpVector(const btVector3& up);
+
+	btQuaternion getRotation(btVector3 & v0, btVector3 & v1) const;
+
+public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
-	~btKinematicCharacterController ();
-	
+	btKinematicCharacterController(btPairCachingGhostObject * ghostObject, btConvexShape * convexShape, btScalar stepHeight, const btVector3& up = btVector3(1.0, 0.0, 0.0));
+	~btKinematicCharacterController();
 
 	///btActionInterface interface
-	virtual void updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime)
+	virtual void updateAction(btCollisionWorld * collisionWorld, btScalar deltaTime)
 	{
-		preStep ( collisionWorld);
-		playerStep (collisionWorld, deltaTime);
+		preStep(collisionWorld);
+		playerStep(collisionWorld, deltaTime);
 	}
-	
+
 	///btActionInterface interface
-	void	debugDraw(btIDebugDraw* debugDrawer);
+	void debugDraw(btIDebugDraw * debugDrawer);
 
-	void setUpAxis (int axis)
-	{
-		if (axis < 0)
-			axis = 0;
-		if (axis > 2)
-			axis = 2;
-		m_upAxis = axis;
-	}
+	void setUp(const btVector3& up);
+
+	const btVector3& getUp() { return m_up; }
 
 	/// This should probably be called setPositionIncrementPerSimulatorStep.
 	/// This is neither a direction nor a velocity, but the amount to
 	///	increment the position each simulation iteration, regardless
 	///	of dt.
 	/// This call will reset any velocity set by setVelocityForTimeInterval().
-	virtual void	setWalkDirection(const btVector3& walkDirection);
+	virtual void setWalkDirection(const btVector3& walkDirection);
 
 	/// Caller provides a velocity with which the character should move for
 	///	the given time period.  After the time period, velocity is reset
@@ -134,37 +144,57 @@ public:
 	/// This call will reset any walk direction set by setWalkDirection().
 	/// Negative time intervals will result in no motion.
 	virtual void setVelocityForTimeInterval(const btVector3& velocity,
-				btScalar timeInterval);
+											btScalar timeInterval);
+
+	virtual void setAngularVelocity(const btVector3& velocity);
+	virtual const btVector3& getAngularVelocity() const;
+
+	virtual void setLinearVelocity(const btVector3& velocity);
+	virtual btVector3 getLinearVelocity() const;
 
-	void reset ( btCollisionWorld* collisionWorld );
-	void warp (const btVector3& origin);
+	void setLinearDamping(btScalar d) { m_linearDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
+	btScalar getLinearDamping() const { return m_linearDamping; }
+	void setAngularDamping(btScalar d) { m_angularDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
+	btScalar getAngularDamping() const { return m_angularDamping; }
 
-	void preStep (  btCollisionWorld* collisionWorld);
-	void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
+	void reset(btCollisionWorld * collisionWorld);
+	void warp(const btVector3& origin);
 
-	void setFallSpeed (btScalar fallSpeed);
-	void setJumpSpeed (btScalar jumpSpeed);
-	void setMaxJumpHeight (btScalar maxJumpHeight);
-	bool canJump () const;
+	void preStep(btCollisionWorld * collisionWorld);
+	void playerStep(btCollisionWorld * collisionWorld, btScalar dt);
 
-	void jump ();
+	void setStepHeight(btScalar h);
+	btScalar getStepHeight() const { return m_stepHeight; }
+	void setFallSpeed(btScalar fallSpeed);
+	btScalar getFallSpeed() const { return m_fallSpeed; }
+	void setJumpSpeed(btScalar jumpSpeed);
+	btScalar getJumpSpeed() const { return m_jumpSpeed; }
+	void setMaxJumpHeight(btScalar maxJumpHeight);
+	bool canJump() const;
 
-	void setGravity(btScalar gravity);
-	btScalar getGravity() const;
+	void jump(const btVector3& v = btVector3(0, 0, 0));
+
+	void applyImpulse(const btVector3& v) { jump(v); }
+
+	void setGravity(const btVector3& gravity);
+	btVector3 getGravity() const;
 
 	/// The max slope determines the maximum angle that the controller can walk up.
 	/// The slope angle is measured in radians.
 	void setMaxSlope(btScalar slopeRadians);
 	btScalar getMaxSlope() const;
 
+	void setMaxPenetrationDepth(btScalar d);
+	btScalar getMaxPenetrationDepth() const;
+
 	btPairCachingGhostObject* getGhostObject();
-	void	setUseGhostSweepTest(bool useGhostObjectSweepTest)
+	void setUseGhostSweepTest(bool useGhostObjectSweepTest)
 	{
 		m_useGhostObjectSweepTest = useGhostObjectSweepTest;
 	}
 
-	bool onGround () const;
-	void setUpInterpolate (bool value);
+	bool onGround() const;
+	void setUpInterpolate(bool value);
 };
 
-#endif // BT_KINEMATIC_CHARACTER_CONTROLLER_H
+#endif  // BT_KINEMATIC_CHARACTER_CONTROLLER_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp
new file mode 100644
index 00000000000..27f76b8425c
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp
@@ -0,0 +1,1084 @@
+/*
+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 "btBatchedConstraints.h"
+
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btMinMax.h"
+#include "LinearMath/btStackAlloc.h"
+#include "LinearMath/btQuickprof.h"
+
+#include <string.h>  //for memset
+
+#include <cmath>
+
+const int kNoMerge = -1;
+
+bool btBatchedConstraints::s_debugDrawBatches = false;
+
+struct btBatchedConstraintInfo
+{
+	int constraintIndex;
+	int numConstraintRows;
+	int bodyIds[2];
+};
+
+struct btBatchInfo
+{
+	int numConstraints;
+	int mergeIndex;
+
+	btBatchInfo() : numConstraints(0), mergeIndex(kNoMerge) {}
+};
+
+bool btBatchedConstraints::validate(btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies) const
+{
+	//
+	// validate: for debugging only. Verify coloring of bodies, that no body is touched by more than one batch in any given phase
+	//
+	int errors = 0;
+	const int kUnassignedBatch = -1;
+
+	btAlignedObjectArray<int> bodyBatchId;
+	for (int iPhase = 0; iPhase < m_phases.size(); ++iPhase)
+	{
+		bodyBatchId.resizeNoInitialize(0);
+		bodyBatchId.resize(bodies.size(), kUnassignedBatch);
+		const Range& phase = m_phases[iPhase];
+		for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch)
+		{
+			const Range& batch = m_batches[iBatch];
+			for (int iiCons = batch.begin; iiCons < batch.end; ++iiCons)
+			{
+				int iCons = m_constraintIndices[iiCons];
+				const btSolverConstraint& cons = constraints->at(iCons);
+				const btSolverBody& bodyA = bodies[cons.m_solverBodyIdA];
+				const btSolverBody& bodyB = bodies[cons.m_solverBodyIdB];
+				if (!bodyA.internalGetInvMass().isZero())
+				{
+					int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdA];
+					if (thisBodyBatchId == kUnassignedBatch)
+					{
+						bodyBatchId[cons.m_solverBodyIdA] = iBatch;
+					}
+					else if (thisBodyBatchId != iBatch)
+					{
+						btAssert(!"dynamic body is used in 2 different batches in the same phase");
+						errors++;
+					}
+				}
+				if (!bodyB.internalGetInvMass().isZero())
+				{
+					int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdB];
+					if (thisBodyBatchId == kUnassignedBatch)
+					{
+						bodyBatchId[cons.m_solverBodyIdB] = iBatch;
+					}
+					else if (thisBodyBatchId != iBatch)
+					{
+						btAssert(!"dynamic body is used in 2 different batches in the same phase");
+						errors++;
+					}
+				}
+			}
+		}
+	}
+	return errors == 0;
+}
+
+static void debugDrawSingleBatch(const btBatchedConstraints* bc,
+								 btConstraintArray* constraints,
+								 const btAlignedObjectArray<btSolverBody>& bodies,
+								 int iBatch,
+								 const btVector3& color,
+								 const btVector3& offset)
+{
+	if (bc && bc->m_debugDrawer && iBatch < bc->m_batches.size())
+	{
+		const btBatchedConstraints::Range& b = bc->m_batches[iBatch];
+		for (int iiCon = b.begin; iiCon < b.end; ++iiCon)
+		{
+			int iCon = bc->m_constraintIndices[iiCon];
+			const btSolverConstraint& con = constraints->at(iCon);
+			int iBody0 = con.m_solverBodyIdA;
+			int iBody1 = con.m_solverBodyIdB;
+			btVector3 pos0 = bodies[iBody0].getWorldTransform().getOrigin() + offset;
+			btVector3 pos1 = bodies[iBody1].getWorldTransform().getOrigin() + offset;
+			bc->m_debugDrawer->drawLine(pos0, pos1, color);
+		}
+	}
+}
+
+static void debugDrawPhase(const btBatchedConstraints* bc,
+						   btConstraintArray* constraints,
+						   const btAlignedObjectArray<btSolverBody>& bodies,
+						   int iPhase,
+						   const btVector3& color0,
+						   const btVector3& color1,
+						   const btVector3& offset)
+{
+	BT_PROFILE("debugDrawPhase");
+	if (bc && bc->m_debugDrawer && iPhase < bc->m_phases.size())
+	{
+		const btBatchedConstraints::Range& phase = bc->m_phases[iPhase];
+		for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch)
+		{
+			float tt = float(iBatch - phase.begin) / float(btMax(1, phase.end - phase.begin - 1));
+			btVector3 col = lerp(color0, color1, tt);
+			debugDrawSingleBatch(bc, constraints, bodies, iBatch, col, offset);
+		}
+	}
+}
+
+static void debugDrawAllBatches(const btBatchedConstraints* bc,
+								btConstraintArray* constraints,
+								const btAlignedObjectArray<btSolverBody>& bodies)
+{
+	BT_PROFILE("debugDrawAllBatches");
+	if (bc && bc->m_debugDrawer && bc->m_phases.size() > 0)
+	{
+		btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
+		btVector3 bboxMax = -bboxMin;
+		for (int iBody = 0; iBody < bodies.size(); ++iBody)
+		{
+			const btVector3& pos = bodies[iBody].getWorldTransform().getOrigin();
+			bboxMin.setMin(pos);
+			bboxMax.setMax(pos);
+		}
+		btVector3 bboxExtent = bboxMax - bboxMin;
+		btVector3 offsetBase = btVector3(0, bboxExtent.y() * 1.1f, 0);
+		btVector3 offsetStep = btVector3(0, 0, bboxExtent.z() * 1.1f);
+		int numPhases = bc->m_phases.size();
+		for (int iPhase = 0; iPhase < numPhases; ++iPhase)
+		{
+			float b = float(iPhase) / float(numPhases - 1);
+			btVector3 color0 = btVector3(1, 0, b);
+			btVector3 color1 = btVector3(0, 1, b);
+			btVector3 offset = offsetBase + offsetStep * (float(iPhase) - float(numPhases - 1) * 0.5);
+			debugDrawPhase(bc, constraints, bodies, iPhase, color0, color1, offset);
+		}
+	}
+}
+
+static void initBatchedBodyDynamicFlags(btAlignedObjectArray<bool>* outBodyDynamicFlags, const btAlignedObjectArray<btSolverBody>& bodies)
+{
+	BT_PROFILE("initBatchedBodyDynamicFlags");
+	btAlignedObjectArray<bool>& bodyDynamicFlags = *outBodyDynamicFlags;
+	bodyDynamicFlags.resizeNoInitialize(bodies.size());
+	for (int i = 0; i < bodies.size(); ++i)
+	{
+		const btSolverBody& body = bodies[i];
+		bodyDynamicFlags[i] = (body.internalGetInvMass().x() > btScalar(0));
+	}
+}
+
+static int runLengthEncodeConstraintInfo(btBatchedConstraintInfo* outConInfos, int numConstraints)
+{
+	BT_PROFILE("runLengthEncodeConstraintInfo");
+	// detect and run-length encode constraint rows that repeat the same bodies
+	int iDest = 0;
+	int iSrc = 0;
+	while (iSrc < numConstraints)
+	{
+		const btBatchedConstraintInfo& srcConInfo = outConInfos[iSrc];
+		btBatchedConstraintInfo& conInfo = outConInfos[iDest];
+		conInfo.constraintIndex = iSrc;
+		conInfo.bodyIds[0] = srcConInfo.bodyIds[0];
+		conInfo.bodyIds[1] = srcConInfo.bodyIds[1];
+		while (iSrc < numConstraints && outConInfos[iSrc].bodyIds[0] == srcConInfo.bodyIds[0] && outConInfos[iSrc].bodyIds[1] == srcConInfo.bodyIds[1])
+		{
+			++iSrc;
+		}
+		conInfo.numConstraintRows = iSrc - conInfo.constraintIndex;
+		++iDest;
+	}
+	return iDest;
+}
+
+struct ReadSolverConstraintsLoop : public btIParallelForBody
+{
+	btBatchedConstraintInfo* m_outConInfos;
+	btConstraintArray* m_constraints;
+
+	ReadSolverConstraintsLoop(btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints)
+	{
+		m_outConInfos = outConInfos;
+		m_constraints = constraints;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		for (int i = iBegin; i < iEnd; ++i)
+		{
+			btBatchedConstraintInfo& conInfo = m_outConInfos[i];
+			const btSolverConstraint& con = m_constraints->at(i);
+			conInfo.bodyIds[0] = con.m_solverBodyIdA;
+			conInfo.bodyIds[1] = con.m_solverBodyIdB;
+			conInfo.constraintIndex = i;
+			conInfo.numConstraintRows = 1;
+		}
+	}
+};
+
+static int initBatchedConstraintInfo(btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints)
+{
+	BT_PROFILE("initBatchedConstraintInfo");
+	int numConstraints = constraints->size();
+	bool inParallel = true;
+	if (inParallel)
+	{
+		ReadSolverConstraintsLoop loop(outConInfos, constraints);
+		int grainSize = 1200;
+		btParallelFor(0, numConstraints, grainSize, loop);
+	}
+	else
+	{
+		for (int i = 0; i < numConstraints; ++i)
+		{
+			btBatchedConstraintInfo& conInfo = outConInfos[i];
+			const btSolverConstraint& con = constraints->at(i);
+			conInfo.bodyIds[0] = con.m_solverBodyIdA;
+			conInfo.bodyIds[1] = con.m_solverBodyIdB;
+			conInfo.constraintIndex = i;
+			conInfo.numConstraintRows = 1;
+		}
+	}
+	bool useRunLengthEncoding = true;
+	if (useRunLengthEncoding)
+	{
+		numConstraints = runLengthEncodeConstraintInfo(outConInfos, numConstraints);
+	}
+	return numConstraints;
+}
+
+static void expandConstraintRowsInPlace(int* constraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
+{
+	BT_PROFILE("expandConstraintRowsInPlace");
+	if (numConstraintRows > numConstraints)
+	{
+		// we walk the array in reverse to avoid overwriteing
+		for (int iCon = numConstraints - 1; iCon >= 0; --iCon)
+		{
+			const btBatchedConstraintInfo& conInfo = conInfos[iCon];
+			int iBatch = constraintBatchIds[iCon];
+			for (int i = conInfo.numConstraintRows - 1; i >= 0; --i)
+			{
+				int iDest = conInfo.constraintIndex + i;
+				btAssert(iDest >= iCon);
+				btAssert(iDest >= 0 && iDest < numConstraintRows);
+				constraintBatchIds[iDest] = iBatch;
+			}
+		}
+	}
+}
+
+static void expandConstraintRows(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
+{
+	BT_PROFILE("expandConstraintRows");
+	for (int iCon = 0; iCon < numConstraints; ++iCon)
+	{
+		const btBatchedConstraintInfo& conInfo = conInfos[iCon];
+		int iBatch = srcConstraintBatchIds[iCon];
+		for (int i = 0; i < conInfo.numConstraintRows; ++i)
+		{
+			int iDest = conInfo.constraintIndex + i;
+			btAssert(iDest >= iCon);
+			btAssert(iDest >= 0 && iDest < numConstraintRows);
+			destConstraintBatchIds[iDest] = iBatch;
+		}
+	}
+}
+
+struct ExpandConstraintRowsLoop : public btIParallelForBody
+{
+	int* m_destConstraintBatchIds;
+	const int* m_srcConstraintBatchIds;
+	const btBatchedConstraintInfo* m_conInfos;
+	int m_numConstraintRows;
+
+	ExpandConstraintRowsLoop(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraintRows)
+	{
+		m_destConstraintBatchIds = destConstraintBatchIds;
+		m_srcConstraintBatchIds = srcConstraintBatchIds;
+		m_conInfos = conInfos;
+		m_numConstraintRows = numConstraintRows;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		expandConstraintRows(m_destConstraintBatchIds, m_srcConstraintBatchIds + iBegin, m_conInfos + iBegin, iEnd - iBegin, m_numConstraintRows);
+	}
+};
+
+static void expandConstraintRowsMt(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
+{
+	BT_PROFILE("expandConstraintRowsMt");
+	ExpandConstraintRowsLoop loop(destConstraintBatchIds, srcConstraintBatchIds, conInfos, numConstraintRows);
+	int grainSize = 600;
+	btParallelFor(0, numConstraints, grainSize, loop);
+}
+
+static void initBatchedConstraintInfoArray(btAlignedObjectArray<btBatchedConstraintInfo>* outConInfos, btConstraintArray* constraints)
+{
+	BT_PROFILE("initBatchedConstraintInfoArray");
+	btAlignedObjectArray<btBatchedConstraintInfo>& conInfos = *outConInfos;
+	int numConstraints = constraints->size();
+	conInfos.resizeNoInitialize(numConstraints);
+
+	int newSize = initBatchedConstraintInfo(&outConInfos->at(0), constraints);
+	conInfos.resizeNoInitialize(newSize);
+}
+
+static void mergeSmallBatches(btBatchInfo* batches, int iBeginBatch, int iEndBatch, int minBatchSize, int maxBatchSize)
+{
+	BT_PROFILE("mergeSmallBatches");
+	for (int iBatch = iEndBatch - 1; iBatch >= iBeginBatch; --iBatch)
+	{
+		btBatchInfo& batch = batches[iBatch];
+		if (batch.mergeIndex == kNoMerge && batch.numConstraints > 0 && batch.numConstraints < minBatchSize)
+		{
+			for (int iDestBatch = iBatch - 1; iDestBatch >= iBeginBatch; --iDestBatch)
+			{
+				btBatchInfo& destBatch = batches[iDestBatch];
+				if (destBatch.mergeIndex == kNoMerge && (destBatch.numConstraints + batch.numConstraints) < maxBatchSize)
+				{
+					destBatch.numConstraints += batch.numConstraints;
+					batch.numConstraints = 0;
+					batch.mergeIndex = iDestBatch;
+					break;
+				}
+			}
+		}
+	}
+	// flatten mergeIndexes
+	// e.g. in case where A was merged into B and then B was merged into C, we need A to point to C instead of B
+	// Note: loop goes forward through batches because batches always merge from higher indexes to lower,
+	//     so by going from low to high it reduces the amount of trail-following
+	for (int iBatch = iBeginBatch; iBatch < iEndBatch; ++iBatch)
+	{
+		btBatchInfo& batch = batches[iBatch];
+		if (batch.mergeIndex != kNoMerge)
+		{
+			int iMergeDest = batches[batch.mergeIndex].mergeIndex;
+			// follow trail of merges to the end
+			while (iMergeDest != kNoMerge)
+			{
+				int iNext = batches[iMergeDest].mergeIndex;
+				if (iNext == kNoMerge)
+				{
+					batch.mergeIndex = iMergeDest;
+					break;
+				}
+				iMergeDest = iNext;
+			}
+		}
+	}
+}
+
+static void updateConstraintBatchIdsForMerges(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches)
+{
+	BT_PROFILE("updateConstraintBatchIdsForMerges");
+	// update batchIds to account for merges
+	for (int i = 0; i < numConstraints; ++i)
+	{
+		int iBatch = constraintBatchIds[i];
+		btAssert(iBatch < numBatches);
+		// if this constraint references a batch that was merged into another batch
+		if (batches[iBatch].mergeIndex != kNoMerge)
+		{
+			// update batchId
+			constraintBatchIds[i] = batches[iBatch].mergeIndex;
+		}
+	}
+}
+
+struct UpdateConstraintBatchIdsForMergesLoop : public btIParallelForBody
+{
+	int* m_constraintBatchIds;
+	const btBatchInfo* m_batches;
+	int m_numBatches;
+
+	UpdateConstraintBatchIdsForMergesLoop(int* constraintBatchIds, const btBatchInfo* batches, int numBatches)
+	{
+		m_constraintBatchIds = constraintBatchIds;
+		m_batches = batches;
+		m_numBatches = numBatches;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("UpdateConstraintBatchIdsForMergesLoop");
+		updateConstraintBatchIdsForMerges(m_constraintBatchIds + iBegin, iEnd - iBegin, m_batches, m_numBatches);
+	}
+};
+
+static void updateConstraintBatchIdsForMergesMt(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches)
+{
+	BT_PROFILE("updateConstraintBatchIdsForMergesMt");
+	UpdateConstraintBatchIdsForMergesLoop loop(constraintBatchIds, batches, numBatches);
+	int grainSize = 800;
+	btParallelFor(0, numConstraints, grainSize, loop);
+}
+
+inline bool BatchCompare(const btBatchedConstraints::Range& a, const btBatchedConstraints::Range& b)
+{
+	int lenA = a.end - a.begin;
+	int lenB = b.end - b.begin;
+	return lenA > lenB;
+}
+
+static void writeOutConstraintIndicesForRangeOfBatches(btBatchedConstraints* bc,
+													   const int* constraintBatchIds,
+													   int numConstraints,
+													   int* constraintIdPerBatch,
+													   int batchBegin,
+													   int batchEnd)
+{
+	BT_PROFILE("writeOutConstraintIndicesForRangeOfBatches");
+	for (int iCon = 0; iCon < numConstraints; ++iCon)
+	{
+		int iBatch = constraintBatchIds[iCon];
+		if (iBatch >= batchBegin && iBatch < batchEnd)
+		{
+			int iDestCon = constraintIdPerBatch[iBatch];
+			constraintIdPerBatch[iBatch] = iDestCon + 1;
+			bc->m_constraintIndices[iDestCon] = iCon;
+		}
+	}
+}
+
+struct WriteOutConstraintIndicesLoop : public btIParallelForBody
+{
+	btBatchedConstraints* m_batchedConstraints;
+	const int* m_constraintBatchIds;
+	int m_numConstraints;
+	int* m_constraintIdPerBatch;
+	int m_maxNumBatchesPerPhase;
+
+	WriteOutConstraintIndicesLoop(btBatchedConstraints* bc, const int* constraintBatchIds, int numConstraints, int* constraintIdPerBatch, int maxNumBatchesPerPhase)
+	{
+		m_batchedConstraints = bc;
+		m_constraintBatchIds = constraintBatchIds;
+		m_numConstraints = numConstraints;
+		m_constraintIdPerBatch = constraintIdPerBatch;
+		m_maxNumBatchesPerPhase = maxNumBatchesPerPhase;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("WriteOutConstraintIndicesLoop");
+		int batchBegin = iBegin * m_maxNumBatchesPerPhase;
+		int batchEnd = iEnd * m_maxNumBatchesPerPhase;
+		writeOutConstraintIndicesForRangeOfBatches(m_batchedConstraints,
+												   m_constraintBatchIds,
+												   m_numConstraints,
+												   m_constraintIdPerBatch,
+												   batchBegin,
+												   batchEnd);
+	}
+};
+
+static void writeOutConstraintIndicesMt(btBatchedConstraints* bc,
+										const int* constraintBatchIds,
+										int numConstraints,
+										int* constraintIdPerBatch,
+										int maxNumBatchesPerPhase,
+										int numPhases)
+{
+	BT_PROFILE("writeOutConstraintIndicesMt");
+	bool inParallel = true;
+	if (inParallel)
+	{
+		WriteOutConstraintIndicesLoop loop(bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase);
+		btParallelFor(0, numPhases, 1, loop);
+	}
+	else
+	{
+		for (int iCon = 0; iCon < numConstraints; ++iCon)
+		{
+			int iBatch = constraintBatchIds[iCon];
+			int iDestCon = constraintIdPerBatch[iBatch];
+			constraintIdPerBatch[iBatch] = iDestCon + 1;
+			bc->m_constraintIndices[iDestCon] = iCon;
+		}
+	}
+}
+
+static void writeGrainSizes(btBatchedConstraints* bc)
+{
+	typedef btBatchedConstraints::Range Range;
+	int numPhases = bc->m_phases.size();
+	bc->m_phaseGrainSize.resizeNoInitialize(numPhases);
+	int numThreads = btGetTaskScheduler()->getNumThreads();
+	for (int iPhase = 0; iPhase < numPhases; ++iPhase)
+	{
+		const Range& phase = bc->m_phases[iPhase];
+		int numBatches = phase.end - phase.begin;
+		float grainSize = std::floor((0.25f * numBatches / float(numThreads)) + 0.0f);
+		bc->m_phaseGrainSize[iPhase] = btMax(1, int(grainSize));
+	}
+}
+
+static void writeOutBatches(btBatchedConstraints* bc,
+							const int* constraintBatchIds,
+							int numConstraints,
+							const btBatchInfo* batches,
+							int* batchWork,
+							int maxNumBatchesPerPhase,
+							int numPhases)
+{
+	BT_PROFILE("writeOutBatches");
+	typedef btBatchedConstraints::Range Range;
+	bc->m_constraintIndices.reserve(numConstraints);
+	bc->m_batches.resizeNoInitialize(0);
+	bc->m_phases.resizeNoInitialize(0);
+
+	//int maxNumBatches = numPhases * maxNumBatchesPerPhase;
+	{
+		int* constraintIdPerBatch = batchWork;  // for each batch, keep an index into the next available slot in the m_constraintIndices array
+		int iConstraint = 0;
+		for (int iPhase = 0; iPhase < numPhases; ++iPhase)
+		{
+			int curPhaseBegin = bc->m_batches.size();
+			int iBegin = iPhase * maxNumBatchesPerPhase;
+			int iEnd = iBegin + maxNumBatchesPerPhase;
+			for (int i = iBegin; i < iEnd; ++i)
+			{
+				const btBatchInfo& batch = batches[i];
+				int curBatchBegin = iConstraint;
+				constraintIdPerBatch[i] = curBatchBegin;  // record the start of each batch in m_constraintIndices array
+				int numConstraints = batch.numConstraints;
+				iConstraint += numConstraints;
+				if (numConstraints > 0)
+				{
+					bc->m_batches.push_back(Range(curBatchBegin, iConstraint));
+				}
+			}
+			// if any batches were emitted this phase,
+			if (bc->m_batches.size() > curPhaseBegin)
+			{
+				// output phase
+				bc->m_phases.push_back(Range(curPhaseBegin, bc->m_batches.size()));
+			}
+		}
+
+		btAssert(iConstraint == numConstraints);
+		bc->m_constraintIndices.resizeNoInitialize(numConstraints);
+		writeOutConstraintIndicesMt(bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase, numPhases);
+	}
+	// for each phase
+	for (int iPhase = 0; iPhase < bc->m_phases.size(); ++iPhase)
+	{
+		// sort the batches from largest to smallest (can be helpful to some task schedulers)
+		const Range& curBatches = bc->m_phases[iPhase];
+		bc->m_batches.quickSortInternal(BatchCompare, curBatches.begin, curBatches.end - 1);
+	}
+	bc->m_phaseOrder.resize(bc->m_phases.size());
+	for (int i = 0; i < bc->m_phases.size(); ++i)
+	{
+		bc->m_phaseOrder[i] = i;
+	}
+	writeGrainSizes(bc);
+}
+
+//
+// PreallocatedMemoryHelper -- helper object for allocating a number of chunks of memory in a single contiguous block.
+//                             It is generally more efficient to do a single larger allocation than many smaller allocations.
+//
+// Example Usage:
+//
+//  btVector3* bodyPositions = NULL;
+//  btBatchedConstraintInfo* conInfos = NULL;
+//  {
+//    PreallocatedMemoryHelper<8> memHelper;
+//    memHelper.addChunk( (void**) &bodyPositions, sizeof( btVector3 ) * bodies.size() );
+//    memHelper.addChunk( (void**) &conInfos, sizeof( btBatchedConstraintInfo ) * numConstraints );
+//    void* memPtr = malloc( memHelper.getSizeToAllocate() );  // allocate the memory
+//    memHelper.setChunkPointers( memPtr );  // update pointers to chunks
+//  }
+template <int N>
+class PreallocatedMemoryHelper
+{
+	struct Chunk
+	{
+		void** ptr;
+		size_t size;
+	};
+	Chunk m_chunks[N];
+	int m_numChunks;
+
+public:
+	PreallocatedMemoryHelper() { m_numChunks = 0; }
+	void addChunk(void** ptr, size_t sz)
+	{
+		btAssert(m_numChunks < N);
+		if (m_numChunks < N)
+		{
+			Chunk& chunk = m_chunks[m_numChunks];
+			chunk.ptr = ptr;
+			chunk.size = sz;
+			m_numChunks++;
+		}
+	}
+	size_t getSizeToAllocate() const
+	{
+		size_t totalSize = 0;
+		for (int i = 0; i < m_numChunks; ++i)
+		{
+			totalSize += m_chunks[i].size;
+		}
+		return totalSize;
+	}
+	void setChunkPointers(void* mem) const
+	{
+		size_t totalSize = 0;
+		for (int i = 0; i < m_numChunks; ++i)
+		{
+			const Chunk& chunk = m_chunks[i];
+			char* chunkPtr = static_cast<char*>(mem) + totalSize;
+			*chunk.ptr = chunkPtr;
+			totalSize += chunk.size;
+		}
+	}
+};
+
+static btVector3 findMaxDynamicConstraintExtent(
+	btVector3* bodyPositions,
+	bool* bodyDynamicFlags,
+	btBatchedConstraintInfo* conInfos,
+	int numConstraints,
+	int numBodies)
+{
+	BT_PROFILE("findMaxDynamicConstraintExtent");
+	btVector3 consExtent = btVector3(1, 1, 1) * 0.001;
+	for (int iCon = 0; iCon < numConstraints; ++iCon)
+	{
+		const btBatchedConstraintInfo& con = conInfos[iCon];
+		int iBody0 = con.bodyIds[0];
+		int iBody1 = con.bodyIds[1];
+		btAssert(iBody0 >= 0 && iBody0 < numBodies);
+		btAssert(iBody1 >= 0 && iBody1 < numBodies);
+		// is it a dynamic constraint?
+		if (bodyDynamicFlags[iBody0] && bodyDynamicFlags[iBody1])
+		{
+			btVector3 delta = bodyPositions[iBody1] - bodyPositions[iBody0];
+			consExtent.setMax(delta.absolute());
+		}
+	}
+	return consExtent;
+}
+
+struct btIntVec3
+{
+	int m_ints[3];
+
+	SIMD_FORCE_INLINE const int& operator[](int i) const { return m_ints[i]; }
+	SIMD_FORCE_INLINE int& operator[](int i) { return m_ints[i]; }
+};
+
+struct AssignConstraintsToGridBatchesParams
+{
+	bool* bodyDynamicFlags;
+	btIntVec3* bodyGridCoords;
+	int numBodies;
+	btBatchedConstraintInfo* conInfos;
+	int* constraintBatchIds;
+	btIntVec3 gridChunkDim;
+	int maxNumBatchesPerPhase;
+	int numPhases;
+	int phaseMask;
+
+	AssignConstraintsToGridBatchesParams()
+	{
+		memset(this, 0, sizeof(*this));
+	}
+};
+
+static void assignConstraintsToGridBatches(const AssignConstraintsToGridBatchesParams& params, int iConBegin, int iConEnd)
+{
+	BT_PROFILE("assignConstraintsToGridBatches");
+	// (can be done in parallel)
+	for (int iCon = iConBegin; iCon < iConEnd; ++iCon)
+	{
+		const btBatchedConstraintInfo& con = params.conInfos[iCon];
+		int iBody0 = con.bodyIds[0];
+		int iBody1 = con.bodyIds[1];
+		int iPhase = iCon;  //iBody0; // pseudorandom choice to distribute evenly amongst phases
+		iPhase &= params.phaseMask;
+		int gridCoord[3];
+		// is it a dynamic constraint?
+		if (params.bodyDynamicFlags[iBody0] && params.bodyDynamicFlags[iBody1])
+		{
+			const btIntVec3& body0Coords = params.bodyGridCoords[iBody0];
+			const btIntVec3& body1Coords = params.bodyGridCoords[iBody1];
+			// for each dimension x,y,z,
+			for (int i = 0; i < 3; ++i)
+			{
+				int coordMin = btMin(body0Coords.m_ints[i], body1Coords.m_ints[i]);
+				int coordMax = btMax(body0Coords.m_ints[i], body1Coords.m_ints[i]);
+				if (coordMin != coordMax)
+				{
+					btAssert(coordMax == coordMin + 1);
+					if ((coordMin & 1) == 0)
+					{
+						iPhase &= ~(1 << i);  // force bit off
+					}
+					else
+					{
+						iPhase |= (1 << i);  // force bit on
+						iPhase &= params.phaseMask;
+					}
+				}
+				gridCoord[i] = coordMin;
+			}
+		}
+		else
+		{
+			if (!params.bodyDynamicFlags[iBody0])
+			{
+				iBody0 = con.bodyIds[1];
+			}
+			btAssert(params.bodyDynamicFlags[iBody0]);
+			const btIntVec3& body0Coords = params.bodyGridCoords[iBody0];
+			// for each dimension x,y,z,
+			for (int i = 0; i < 3; ++i)
+			{
+				gridCoord[i] = body0Coords.m_ints[i];
+			}
+		}
+		// calculate chunk coordinates
+		int chunkCoord[3];
+		btIntVec3 gridChunkDim = params.gridChunkDim;
+		// for each dimension x,y,z,
+		for (int i = 0; i < 3; ++i)
+		{
+			int coordOffset = (iPhase >> i) & 1;
+			chunkCoord[i] = (gridCoord[i] - coordOffset) / 2;
+			btClamp(chunkCoord[i], 0, gridChunkDim[i] - 1);
+			btAssert(chunkCoord[i] < gridChunkDim[i]);
+		}
+		int iBatch = iPhase * params.maxNumBatchesPerPhase + chunkCoord[0] + chunkCoord[1] * gridChunkDim[0] + chunkCoord[2] * gridChunkDim[0] * gridChunkDim[1];
+		btAssert(iBatch >= 0 && iBatch < params.maxNumBatchesPerPhase * params.numPhases);
+		params.constraintBatchIds[iCon] = iBatch;
+	}
+}
+
+struct AssignConstraintsToGridBatchesLoop : public btIParallelForBody
+{
+	const AssignConstraintsToGridBatchesParams* m_params;
+
+	AssignConstraintsToGridBatchesLoop(const AssignConstraintsToGridBatchesParams& params)
+	{
+		m_params = &params;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		assignConstraintsToGridBatches(*m_params, iBegin, iEnd);
+	}
+};
+
+//
+// setupSpatialGridBatchesMt -- generate batches using a uniform 3D grid
+//
+/*
+
+Bodies are treated as 3D points at their center of mass. We only consider dynamic bodies at this stage,
+because only dynamic bodies are mutated when a constraint is solved, thus subject to race conditions.
+
+1. Compute a bounding box around all dynamic bodies
+2. Compute the maximum extent of all dynamic constraints. Each dynamic constraint is treated as a line segment, and we need the size of
+   box that will fully enclose any single dynamic constraint
+
+3. Establish the cell size of our grid, the cell size in each dimension must be at least as large as the dynamic constraints max-extent,
+   so that no dynamic constraint can span more than 2 cells of our grid on any axis of the grid. The cell size should be adjusted
+   larger in order to keep the total number of cells from being excessively high
+
+Key idea: Given that each constraint spans 1 or 2 grid cells in each dimension, we can handle all constraints by processing
+          in chunks of 2x2x2 cells with 8 different 1-cell offsets ((0,0,0),(0,0,1),(0,1,0),(0,1,1),(1,0,0)...).
+          For each of the 8 offsets, we create a phase, and for each 2x2x2 chunk with dynamic constraints becomes a batch in that phase.
+
+4. Once the grid is established, we can calculate for each constraint which phase and batch it belongs in.
+
+5. Do a merge small batches on the batches of each phase separately, to try to even out the sizes of batches
+
+Optionally, we can "collapse" one dimension of our 3D grid to turn it into a 2D grid, which reduces the number of phases
+to 4. With fewer phases, there are more constraints per phase and this makes it easier to create batches of a useful size.
+*/
+//
+static void setupSpatialGridBatchesMt(
+	btBatchedConstraints* batchedConstraints,
+	btAlignedObjectArray<char>* scratchMemory,
+	btConstraintArray* constraints,
+	const btAlignedObjectArray<btSolverBody>& bodies,
+	int minBatchSize,
+	int maxBatchSize,
+	bool use2DGrid)
+{
+	BT_PROFILE("setupSpatialGridBatchesMt");
+	const int numPhases = 8;
+	int numConstraints = constraints->size();
+	int numConstraintRows = constraints->size();
+
+	const int maxGridChunkCount = 128;
+	int allocNumBatchesPerPhase = maxGridChunkCount;
+	int minNumBatchesPerPhase = 16;
+	int allocNumBatches = allocNumBatchesPerPhase * numPhases;
+
+	btVector3* bodyPositions = NULL;
+	bool* bodyDynamicFlags = NULL;
+	btIntVec3* bodyGridCoords = NULL;
+	btBatchInfo* batches = NULL;
+	int* batchWork = NULL;
+	btBatchedConstraintInfo* conInfos = NULL;
+	int* constraintBatchIds = NULL;
+	int* constraintRowBatchIds = NULL;
+	{
+		PreallocatedMemoryHelper<10> memHelper;
+		memHelper.addChunk((void**)&bodyPositions, sizeof(btVector3) * bodies.size());
+		memHelper.addChunk((void**)&bodyDynamicFlags, sizeof(bool) * bodies.size());
+		memHelper.addChunk((void**)&bodyGridCoords, sizeof(btIntVec3) * bodies.size());
+		memHelper.addChunk((void**)&batches, sizeof(btBatchInfo) * allocNumBatches);
+		memHelper.addChunk((void**)&batchWork, sizeof(int) * allocNumBatches);
+		memHelper.addChunk((void**)&conInfos, sizeof(btBatchedConstraintInfo) * numConstraints);
+		memHelper.addChunk((void**)&constraintBatchIds, sizeof(int) * numConstraints);
+		memHelper.addChunk((void**)&constraintRowBatchIds, sizeof(int) * numConstraintRows);
+		size_t scratchSize = memHelper.getSizeToAllocate();
+		// if we need to reallocate
+		if (scratchMemory->capacity() < scratchSize)
+		{
+			// allocate 6.25% extra to avoid repeated reallocs
+			scratchMemory->reserve(scratchSize + scratchSize / 16);
+		}
+		scratchMemory->resizeNoInitialize(scratchSize);
+		char* memPtr = &scratchMemory->at(0);
+		memHelper.setChunkPointers(memPtr);
+	}
+
+	numConstraints = initBatchedConstraintInfo(conInfos, constraints);
+
+	// compute bounding box around all dynamic bodies
+	// (could be done in parallel)
+	btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
+	btVector3 bboxMax = -bboxMin;
+	//int dynamicBodyCount = 0;
+	for (int i = 0; i < bodies.size(); ++i)
+	{
+		const btSolverBody& body = bodies[i];
+		btVector3 bodyPos = body.getWorldTransform().getOrigin();
+		bool isDynamic = (body.internalGetInvMass().x() > btScalar(0));
+		bodyPositions[i] = bodyPos;
+		bodyDynamicFlags[i] = isDynamic;
+		if (isDynamic)
+		{
+			//dynamicBodyCount++;
+			bboxMin.setMin(bodyPos);
+			bboxMax.setMax(bodyPos);
+		}
+	}
+
+	// find max extent of all dynamic constraints
+	// (could be done in parallel)
+	btVector3 consExtent = findMaxDynamicConstraintExtent(bodyPositions, bodyDynamicFlags, conInfos, numConstraints, bodies.size());
+
+	btVector3 gridExtent = bboxMax - bboxMin;
+
+	gridExtent.setMax(btVector3(btScalar(1), btScalar(1), btScalar(1)));
+
+	btVector3 gridCellSize = consExtent;
+	int gridDim[3];
+	gridDim[0] = int(1.0 + gridExtent.x() / gridCellSize.x());
+	gridDim[1] = int(1.0 + gridExtent.y() / gridCellSize.y());
+	gridDim[2] = int(1.0 + gridExtent.z() / gridCellSize.z());
+
+	// if we can collapse an axis, it will cut our number of phases in half which could be more efficient
+	int phaseMask = 7;
+	bool collapseAxis = use2DGrid;
+	if (collapseAxis)
+	{
+		// pick the smallest axis to collapse, leaving us with the greatest number of cells in our grid
+		int iAxisToCollapse = 0;
+		int axisDim = gridDim[iAxisToCollapse];
+		//for each dimension
+		for (int i = 0; i < 3; ++i)
+		{
+			if (gridDim[i] < axisDim)
+			{
+				iAxisToCollapse = i;
+				axisDim = gridDim[i];
+			}
+		}
+		// collapse it
+		gridCellSize[iAxisToCollapse] = gridExtent[iAxisToCollapse] * 2.0f;
+		phaseMask &= ~(1 << iAxisToCollapse);
+	}
+
+	int numGridChunks = 0;
+	btIntVec3 gridChunkDim;  // each chunk is 2x2x2 group of cells
+	while (true)
+	{
+		gridDim[0] = int(1.0 + gridExtent.x() / gridCellSize.x());
+		gridDim[1] = int(1.0 + gridExtent.y() / gridCellSize.y());
+		gridDim[2] = int(1.0 + gridExtent.z() / gridCellSize.z());
+		gridChunkDim[0] = btMax(1, (gridDim[0] + 0) / 2);
+		gridChunkDim[1] = btMax(1, (gridDim[1] + 0) / 2);
+		gridChunkDim[2] = btMax(1, (gridDim[2] + 0) / 2);
+		numGridChunks = gridChunkDim[0] * gridChunkDim[1] * gridChunkDim[2];
+		float nChunks = float(gridChunkDim[0]) * float(gridChunkDim[1]) * float(gridChunkDim[2]);  // suceptible to integer overflow
+		if (numGridChunks <= maxGridChunkCount && nChunks <= maxGridChunkCount)
+		{
+			break;
+		}
+		gridCellSize *= 1.25;  // should roughly cut numCells in half
+	}
+	btAssert(numGridChunks <= maxGridChunkCount);
+	int maxNumBatchesPerPhase = numGridChunks;
+
+	// for each dynamic body, compute grid coords
+	btVector3 invGridCellSize = btVector3(1, 1, 1) / gridCellSize;
+	// (can be done in parallel)
+	for (int iBody = 0; iBody < bodies.size(); ++iBody)
+	{
+		btIntVec3& coords = bodyGridCoords[iBody];
+		if (bodyDynamicFlags[iBody])
+		{
+			btVector3 v = (bodyPositions[iBody] - bboxMin) * invGridCellSize;
+			coords.m_ints[0] = int(v.x());
+			coords.m_ints[1] = int(v.y());
+			coords.m_ints[2] = int(v.z());
+			btAssert(coords.m_ints[0] >= 0 && coords.m_ints[0] < gridDim[0]);
+			btAssert(coords.m_ints[1] >= 0 && coords.m_ints[1] < gridDim[1]);
+			btAssert(coords.m_ints[2] >= 0 && coords.m_ints[2] < gridDim[2]);
+		}
+		else
+		{
+			coords.m_ints[0] = -1;
+			coords.m_ints[1] = -1;
+			coords.m_ints[2] = -1;
+		}
+	}
+
+	for (int iPhase = 0; iPhase < numPhases; ++iPhase)
+	{
+		int batchBegin = iPhase * maxNumBatchesPerPhase;
+		int batchEnd = batchBegin + maxNumBatchesPerPhase;
+		for (int iBatch = batchBegin; iBatch < batchEnd; ++iBatch)
+		{
+			btBatchInfo& batch = batches[iBatch];
+			batch = btBatchInfo();
+		}
+	}
+
+	{
+		AssignConstraintsToGridBatchesParams params;
+		params.bodyDynamicFlags = bodyDynamicFlags;
+		params.bodyGridCoords = bodyGridCoords;
+		params.numBodies = bodies.size();
+		params.conInfos = conInfos;
+		params.constraintBatchIds = constraintBatchIds;
+		params.gridChunkDim = gridChunkDim;
+		params.maxNumBatchesPerPhase = maxNumBatchesPerPhase;
+		params.numPhases = numPhases;
+		params.phaseMask = phaseMask;
+		bool inParallel = true;
+		if (inParallel)
+		{
+			AssignConstraintsToGridBatchesLoop loop(params);
+			int grainSize = 250;
+			btParallelFor(0, numConstraints, grainSize, loop);
+		}
+		else
+		{
+			assignConstraintsToGridBatches(params, 0, numConstraints);
+		}
+	}
+	for (int iCon = 0; iCon < numConstraints; ++iCon)
+	{
+		const btBatchedConstraintInfo& con = conInfos[iCon];
+		int iBatch = constraintBatchIds[iCon];
+		btBatchInfo& batch = batches[iBatch];
+		batch.numConstraints += con.numConstraintRows;
+	}
+
+	for (int iPhase = 0; iPhase < numPhases; ++iPhase)
+	{
+		// if phase is legit,
+		if (iPhase == (iPhase & phaseMask))
+		{
+			int iBeginBatch = iPhase * maxNumBatchesPerPhase;
+			int iEndBatch = iBeginBatch + maxNumBatchesPerPhase;
+			mergeSmallBatches(batches, iBeginBatch, iEndBatch, minBatchSize, maxBatchSize);
+		}
+	}
+	// all constraints have been assigned a batchId
+	updateConstraintBatchIdsForMergesMt(constraintBatchIds, numConstraints, batches, maxNumBatchesPerPhase * numPhases);
+
+	if (numConstraintRows > numConstraints)
+	{
+		expandConstraintRowsMt(&constraintRowBatchIds[0], &constraintBatchIds[0], &conInfos[0], numConstraints, numConstraintRows);
+	}
+	else
+	{
+		constraintRowBatchIds = constraintBatchIds;
+	}
+
+	writeOutBatches(batchedConstraints, constraintRowBatchIds, numConstraintRows, batches, batchWork, maxNumBatchesPerPhase, numPhases);
+	btAssert(batchedConstraints->validate(constraints, bodies));
+}
+
+static void setupSingleBatch(
+	btBatchedConstraints* bc,
+	int numConstraints)
+{
+	BT_PROFILE("setupSingleBatch");
+	typedef btBatchedConstraints::Range Range;
+
+	bc->m_constraintIndices.resize(numConstraints);
+	for (int i = 0; i < numConstraints; ++i)
+	{
+		bc->m_constraintIndices[i] = i;
+	}
+
+	bc->m_batches.resizeNoInitialize(0);
+	bc->m_phases.resizeNoInitialize(0);
+	bc->m_phaseOrder.resizeNoInitialize(0);
+	bc->m_phaseGrainSize.resizeNoInitialize(0);
+
+	if (numConstraints > 0)
+	{
+		bc->m_batches.push_back(Range(0, numConstraints));
+		bc->m_phases.push_back(Range(0, 1));
+		bc->m_phaseOrder.push_back(0);
+		bc->m_phaseGrainSize.push_back(1);
+	}
+}
+
+void btBatchedConstraints::setup(
+	btConstraintArray* constraints,
+	const btAlignedObjectArray<btSolverBody>& bodies,
+	BatchingMethod batchingMethod,
+	int minBatchSize,
+	int maxBatchSize,
+	btAlignedObjectArray<char>* scratchMemory)
+{
+	if (constraints->size() >= minBatchSize * 4)
+	{
+		bool use2DGrid = batchingMethod == BATCHING_METHOD_SPATIAL_GRID_2D;
+		setupSpatialGridBatchesMt(this, scratchMemory, constraints, bodies, minBatchSize, maxBatchSize, use2DGrid);
+		if (s_debugDrawBatches)
+		{
+			debugDrawAllBatches(this, constraints, bodies);
+		}
+	}
+	else
+	{
+		setupSingleBatch(this, constraints->size());
+	}
+}
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h
new file mode 100644
index 00000000000..5d982ca370c
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h
@@ -0,0 +1,62 @@
+/*
+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_BATCHED_CONSTRAINTS_H
+#define BT_BATCHED_CONSTRAINTS_H
+
+#include "LinearMath/btThreads.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
+#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h"
+
+class btIDebugDraw;
+
+struct btBatchedConstraints
+{
+	enum BatchingMethod
+	{
+		BATCHING_METHOD_SPATIAL_GRID_2D,
+		BATCHING_METHOD_SPATIAL_GRID_3D,
+		BATCHING_METHOD_COUNT
+	};
+	struct Range
+	{
+		int begin;
+		int end;
+
+		Range() : begin(0), end(0) {}
+		Range(int _beg, int _end) : begin(_beg), end(_end) {}
+	};
+
+	btAlignedObjectArray<int> m_constraintIndices;
+	btAlignedObjectArray<Range> m_batches;        // each batch is a range of indices in the m_constraintIndices array
+	btAlignedObjectArray<Range> m_phases;         // each phase is range of indices in the m_batches array
+	btAlignedObjectArray<char> m_phaseGrainSize;  // max grain size for each phase
+	btAlignedObjectArray<int> m_phaseOrder;       // phases can be done in any order, so we can randomize the order here
+	btIDebugDraw* m_debugDrawer;
+
+	static bool s_debugDrawBatches;
+
+	btBatchedConstraints() { m_debugDrawer = NULL; }
+	void setup(btConstraintArray* constraints,
+			   const btAlignedObjectArray<btSolverBody>& bodies,
+			   BatchingMethod batchingMethod,
+			   int minBatchSize,
+			   int maxBatchSize,
+			   btAlignedObjectArray<char>* scratchMemory);
+	bool validate(btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies) const;
+};
+
+#endif  // BT_BATCHED_CONSTRAINTS_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
index 09b7388b63e..ac046aa6eaf 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
@@ -15,49 +15,38 @@ subject to the following restrictions:
 Written by: Marcus Hennix
 */
 
-
 #include "btConeTwistConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
 #include "LinearMath/btMinMax.h"
+#include <cmath>
 #include <new>
 
-
-
 //#define CONETWIST_USE_OBSOLETE_SOLVER true
 #define CONETWIST_USE_OBSOLETE_SOLVER false
 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
 
-
 SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
 {
 	btVector3 vec = axis * invInertiaWorld;
 	return axis.dot(vec);
 }
 
-
-
-
-btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
-											 const btTransform& rbAFrame,const btTransform& rbBFrame)
-											 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
-											 m_angularOnly(false),
-											 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
+btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA, btRigidBody& rbB,
+											 const btTransform& rbAFrame, const btTransform& rbBFrame)
+	: btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA, rbB), m_rbAFrame(rbAFrame), m_rbBFrame(rbBFrame), m_angularOnly(false), m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
 {
 	init();
 }
 
-btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
-											:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
-											 m_angularOnly(false),
-											 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
+btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
+	: btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA), m_rbAFrame(rbAFrame), m_angularOnly(false), m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
 {
 	m_rbBFrame = m_rbAFrame;
 	m_rbBFrame.setOrigin(btVector3(0., 0., 0.));
-	init();	
+	init();
 }
 
-
 void btConeTwistConstraint::init()
 {
 	m_angularOnly = false;
@@ -75,30 +64,29 @@ void btConeTwistConstraint::init()
 	m_angCFM = btScalar(0.f);
 }
 
-
-void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
+void btConeTwistConstraint::getInfo1(btConstraintInfo1* info)
 {
 	if (m_useSolveConstraintObsolete)
 	{
 		info->m_numConstraintRows = 0;
 		info->nub = 0;
-	} 
+	}
 	else
 	{
 		info->m_numConstraintRows = 3;
 		info->nub = 3;
-		calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
-		if(m_solveSwingLimit)
+		calcAngleInfo2(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld());
+		if (m_solveSwingLimit)
 		{
 			info->m_numConstraintRows++;
 			info->nub--;
-			if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+			if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
 			{
 				info->m_numConstraintRows++;
 				info->nub--;
 			}
 		}
-		if(m_solveTwistLimit)
+		if (m_solveTwistLimit)
 		{
 			info->m_numConstraintRows++;
 			info->nub--;
@@ -106,90 +94,88 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
 	}
 }
 
-void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+void btConeTwistConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
 {
 	//always reserve 6 rows: object transform is not available on SPU
 	info->m_numConstraintRows = 6;
 	info->nub = 0;
-		
 }
-	
 
-void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
+void btConeTwistConstraint::getInfo2(btConstraintInfo2* info)
 {
-	getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+	getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld());
 }
 
-void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
+void btConeTwistConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB)
 {
-	calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
-	
+	calcAngleInfo2(transA, transB, invInertiaWorldA, invInertiaWorldB);
+
 	btAssert(!m_useSolveConstraintObsolete);
-    // set jacobian
-    info->m_J1linearAxis[0] = 1;
-    info->m_J1linearAxis[info->rowskip+1] = 1;
-    info->m_J1linearAxis[2*info->rowskip+2] = 1;
+	// set jacobian
+	info->m_J1linearAxis[0] = 1;
+	info->m_J1linearAxis[info->rowskip + 1] = 1;
+	info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
 	btVector3 a1 = transA.getBasis() * m_rbAFrame.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* angular1 = (btVector3*)(info->m_J1angularAxis + info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * info->rowskip);
 		btVector3 a1neg = -a1;
-		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    info->m_J2linearAxis[0] = -1;
-    info->m_J2linearAxis[info->rowskip+1] = -1;
-    info->m_J2linearAxis[2*info->rowskip+2] = -1;
+	info->m_J2linearAxis[0] = -1;
+	info->m_J2linearAxis[info->rowskip + 1] = -1;
+	info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
 	btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
 	{
 		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
-		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    // set right hand side
+	// set right hand side
 	btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
-    btScalar k = info->fps * linERP;
-    int j;
-	for (j=0; j<3; j++)
-    {
-        info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
-		info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
-		info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
-		if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
+	btScalar k = info->fps * linERP;
+	int j;
+	for (j = 0; j < 3; j++)
+	{
+		info->m_constraintError[j * info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
+		info->m_lowerLimit[j * info->rowskip] = -SIMD_INFINITY;
+		info->m_upperLimit[j * info->rowskip] = SIMD_INFINITY;
+		if (m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
 		{
-			info->cfm[j*info->rowskip] = m_linCFM;
+			info->cfm[j * info->rowskip] = m_linCFM;
 		}
-    }
+	}
 	int row = 3;
-    int srow = row * info->rowskip;
+	int srow = row * info->rowskip;
 	btVector3 ax1;
 	// angular limits
-	if(m_solveSwingLimit)
+	if (m_solveSwingLimit)
 	{
-		btScalar *J1 = info->m_J1angularAxis;
-		btScalar *J2 = info->m_J2angularAxis;
-		if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+		btScalar* J1 = info->m_J1angularAxis;
+		btScalar* J2 = info->m_J2angularAxis;
+		if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
 		{
-			btTransform trA = transA*m_rbAFrame;
+			btTransform trA = transA * m_rbAFrame;
 			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];
+			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 * m_relaxationFactor;
-			info->m_constraintError[srow] =   fact * m_swingAxis.dot(p);
-			info->m_constraintError[srow1] =  fact * m_swingAxis.dot(q);
+			info->m_constraintError[srow] = fact * m_swingAxis.dot(p);
+			info->m_constraintError[srow1] = fact * m_swingAxis.dot(q);
 			info->m_lowerLimit[srow] = -SIMD_INFINITY;
 			info->m_upperLimit[srow] = SIMD_INFINITY;
 			info->m_lowerLimit[srow1] = -SIMD_INFINITY;
@@ -199,16 +185,16 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
 		else
 		{
 			ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
-			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];
+			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 * m_biasFactor;
 
 			info->m_constraintError[srow] = k * m_swingCorrection;
-			if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
+			if (m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
 			{
 				info->cfm[srow] = m_angCFM;
 			}
@@ -218,36 +204,35 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
 			srow += info->rowskip;
 		}
 	}
-	if(m_solveTwistLimit)
+	if (m_solveTwistLimit)
 	{
 		ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
-		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* 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 * m_biasFactor;
 		info->m_constraintError[srow] = k * m_twistCorrection;
-		if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
+		if (m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
 		{
 			info->cfm[srow] = m_angCFM;
 		}
-		if(m_twistSpan > 0.0f)
+		if (m_twistSpan > 0.0f)
 		{
-
-			if(m_twistCorrection > 0.0f)
+			if (m_twistCorrection > 0.0f)
 			{
 				info->m_lowerLimit[srow] = 0;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
-			} 
+			}
 			else
 			{
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = 0;
-			} 
+			}
 		}
 		else
 		{
@@ -257,22 +242,20 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
 		srow += info->rowskip;
 	}
 }
-	
 
-
-void	btConeTwistConstraint::buildJacobian()
+void btConeTwistConstraint::buildJacobian()
 {
 	if (m_useSolveConstraintObsolete)
 	{
 		m_appliedImpulse = btScalar(0.);
 		m_accTwistLimitImpulse = btScalar(0.);
 		m_accSwingLimitImpulse = btScalar(0.);
-		m_accMotorImpulse = btVector3(0.,0.,0.);
+		m_accMotorImpulse = btVector3(0., 0., 0.);
 
 		if (!m_angularOnly)
 		{
-			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
+			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
 			btVector3 relPos = pivotBInW - pivotAInW;
 
 			btVector3 normal[3];
@@ -282,71 +265,68 @@ void	btConeTwistConstraint::buildJacobian()
 			}
 			else
 			{
-				normal[0].setValue(btScalar(1.0),0,0);
+				normal[0].setValue(btScalar(1.0), 0, 0);
 			}
 
 			btPlaneSpace1(normal[0], normal[1], normal[2]);
 
-			for (int i=0;i<3;i++)
+			for (int i = 0; i < 3; i++)
 			{
 				new (&m_jac[i]) btJacobianEntry(
-				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-				pivotAInW - m_rbA.getCenterOfMassPosition(),
-				pivotBInW - m_rbB.getCenterOfMassPosition(),
-				normal[i],
-				m_rbA.getInvInertiaDiagLocal(),
-				m_rbA.getInvMass(),
-				m_rbB.getInvInertiaDiagLocal(),
-				m_rbB.getInvMass());
+					m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+					m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+					pivotAInW - m_rbA.getCenterOfMassPosition(),
+					pivotBInW - m_rbB.getCenterOfMassPosition(),
+					normal[i],
+					m_rbA.getInvInertiaDiagLocal(),
+					m_rbA.getInvMass(),
+					m_rbB.getInvInertiaDiagLocal(),
+					m_rbB.getInvMass());
 			}
 		}
 
-		calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+		calcAngleInfo2(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld());
 	}
 }
 
-
-
-void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
+void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA, btSolverBody& bodyB, btScalar timeStep)
 {
-	#ifndef __SPU__
+#ifndef __SPU__
 	if (m_useSolveConstraintObsolete)
 	{
-		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
+		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
 
 		btScalar tau = btScalar(0.3);
 
 		//linear part
 		if (!m_angularOnly)
 		{
-			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
 			btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
 
 			btVector3 vel1;
-			bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
+			bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1, vel1);
 			btVector3 vel2;
-			bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
+			bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2, vel2);
 			btVector3 vel = vel1 - vel2;
 
-			for (int i=0;i<3;i++)
-			{		
+			for (int i = 0; i < 3; i++)
+			{
 				const btVector3& normal = m_jac[i].m_linearJointAxis;
 				btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
 
 				btScalar rel_vel;
 				rel_vel = normal.dot(vel);
 				//positional error (zeroth order error)
-				btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
-				btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
+				btScalar depth = -(pivotAInW - pivotBInW).dot(normal);  //this is the error projected on the normal
+				btScalar impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
 				m_appliedImpulse += impulse;
-				
+
 				btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
 				btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
-				bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
-				bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
-		
+				bodyA.internalApplyImpulse(normal * m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld() * ftorqueAxis1, impulse);
+				bodyB.internalApplyImpulse(normal * m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld() * ftorqueAxis2, -impulse);
 			}
 		}
 
@@ -356,13 +336,17 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 			// compute current and predicted transforms
 			btTransform trACur = m_rbA.getCenterOfMassTransform();
 			btTransform trBCur = m_rbB.getCenterOfMassTransform();
-			btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
-			btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
-			btTransform trAPred; trAPred.setIdentity(); 
-			btVector3 zerovec(0,0,0);
+			btVector3 omegaA;
+			bodyA.internalGetAngularVelocity(omegaA);
+			btVector3 omegaB;
+			bodyB.internalGetAngularVelocity(omegaB);
+			btTransform trAPred;
+			trAPred.setIdentity();
+			btVector3 zerovec(0, 0, 0);
 			btTransformUtil::integrateTransform(
 				trACur, zerovec, omegaA, timeStep, trAPred);
-			btTransform trBPred; trBPred.setIdentity(); 
+			btTransform trBPred;
+			trBPred.setIdentity();
 			btTransformUtil::integrateTransform(
 				trBCur, zerovec, omegaB, timeStep, trBPred);
 
@@ -374,7 +358,7 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 
 			// compute desired omegas in world
 			btVector3 omegaADes, omegaBDes;
-			
+
 			btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
 			btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
 
@@ -415,10 +399,10 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 				{
 					btScalar fMaxImpulse = m_maxMotorImpulse;
 					if (m_bNormalizedMotorStrength)
-						fMaxImpulse = fMaxImpulse/kAxisAInv;
+						fMaxImpulse = fMaxImpulse / kAxisAInv;
 
 					btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
-					btScalar  newUnclampedMag = newUnclampedAccImpulse.length();
+					btScalar newUnclampedMag = newUnclampedAccImpulse.length();
 					if (newUnclampedMag > fMaxImpulse)
 					{
 						newUnclampedAccImpulse.normalize();
@@ -428,31 +412,32 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 					m_accMotorImpulse += impulse;
 				}
 
-				btScalar  impulseMag  = impulse.length();
-				btVector3 impulseAxis =  impulse / impulseMag;
-
-				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
-				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+				btScalar impulseMag = impulse.length();
+				btVector3 impulseAxis = impulse / impulseMag;
 
+				bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * impulseAxis, impulseMag);
+				bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * impulseAxis, -impulseMag);
 			}
 		}
-		else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
+		else if (m_damping > SIMD_EPSILON)  // no motor: do a little damping
 		{
-			btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
-			btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
+			btVector3 angVelA;
+			bodyA.internalGetAngularVelocity(angVelA);
+			btVector3 angVelB;
+			bodyB.internalGetAngularVelocity(angVelB);
 			btVector3 relVel = angVelB - angVelA;
 			if (relVel.length2() > SIMD_EPSILON)
 			{
 				btVector3 relVelAxis = relVel.normalized();
-				btScalar m_kDamping =  btScalar(1.) /
-					(getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
-					 getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
+				btScalar m_kDamping = btScalar(1.) /
+									  (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
+									   getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
 				btVector3 impulse = m_damping * m_kDamping * relVel;
 
-				btScalar  impulseMag  = impulse.length();
+				btScalar impulseMag = impulse.length();
 				btVector3 impulseAxis = impulse / impulseMag;
-				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
-				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+				bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * impulseAxis, impulseMag);
+				bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * impulseAxis, -impulseMag);
 			}
 		}
 
@@ -467,7 +452,7 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 			// solve swing limit
 			if (m_solveSwingLimit)
 			{
-				btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
+				btScalar amplitude = m_swingLimitRatio * m_swingCorrection * m_biasFactor / timeStep;
 				btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
 				if (relSwingVel > 0)
 					amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
@@ -475,7 +460,7 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 
 				// Clamp the accumulated impulse
 				btScalar temp = m_accSwingLimitImpulse;
-				m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
+				m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0));
 				impulseMag = m_accSwingLimitImpulse - temp;
 
 				btVector3 impulse = m_swingAxis * impulseMag;
@@ -491,47 +476,41 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 				impulseMag = impulse.length();
 				btVector3 noTwistSwingAxis = impulse / impulseMag;
 
-				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
-				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
+				bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * noTwistSwingAxis, impulseMag);
+				bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * noTwistSwingAxis, -impulseMag);
 			}
 
-
 			// solve twist limit
 			if (m_solveTwistLimit)
 			{
-				btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
-				btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
-				if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
+				btScalar amplitude = m_twistLimitRatio * m_twistCorrection * m_biasFactor / timeStep;
+				btScalar relTwistVel = (angVelB - angVelA).dot(m_twistAxis);
+				if (relTwistVel > 0)  // only damp when moving towards limit (m_twistAxis flipping is important)
 					amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
 				btScalar impulseMag = amplitude * m_kTwist;
 
 				// Clamp the accumulated impulse
 				btScalar temp = m_accTwistLimitImpulse;
-				m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
+				m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0));
 				impulseMag = m_accTwistLimitImpulse - temp;
 
-		//		btVector3 impulse = m_twistAxis * impulseMag;
+				//		btVector3 impulse = m_twistAxis * impulseMag;
 
-				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
-				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
-			}		
+				bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * m_twistAxis, impulseMag);
+				bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * m_twistAxis, -impulseMag);
+			}
 		}
 	}
 #else
-btAssert(0);
-#endif //__SPU__
+	btAssert(0);
+#endif  //__SPU__
 }
 
-
-
-
-void	btConeTwistConstraint::updateRHS(btScalar	timeStep)
+void btConeTwistConstraint::updateRHS(btScalar timeStep)
 {
 	(void)timeStep;
-
 }
 
-
 #ifndef __SPU__
 void btConeTwistConstraint::calcAngleInfo()
 {
@@ -540,15 +519,15 @@ void btConeTwistConstraint::calcAngleInfo()
 	m_solveTwistLimit = false;
 	m_solveSwingLimit = false;
 
-	btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0);
-	btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0);
+	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);
 
-	btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
+	btScalar swing1 = btScalar(0.), swing2 = btScalar(0.);
 
-	btScalar swx=btScalar(0.),swy = btScalar(0.);
+	btScalar swx = btScalar(0.), swy = btScalar(0.);
 	btScalar thresh = btScalar(10.);
 	btScalar fact;
 
@@ -558,33 +537,33 @@ void btConeTwistConstraint::calcAngleInfo()
 		b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
 		swx = b2Axis1.dot(b1Axis1);
 		swy = b2Axis1.dot(b1Axis2);
-		swing1  = btAtan2Fast(swy, swx);
-		fact = (swy*swy + swx*swx) * thresh * thresh;
+		swing1 = btAtan2Fast(swy, swx);
+		fact = (swy * swy + swx * swx) * thresh * thresh;
 		fact = fact / (fact + btScalar(1.0));
-		swing1 *= fact; 
+		swing1 *= fact;
 	}
 
 	if (m_swingSpan2 >= btScalar(0.05f))
 	{
-		b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);			
+		b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
 		swx = b2Axis1.dot(b1Axis1);
 		swy = b2Axis1.dot(b1Axis3);
-		swing2  = btAtan2Fast(swy, swx);
-		fact = (swy*swy + swx*swx) * thresh * thresh;
+		swing2 = btAtan2Fast(swy, swx);
+		fact = (swy * swy + swx * swx) * thresh * thresh;
 		fact = fact / (fact + btScalar(1.0));
-		swing2 *= fact; 
+		swing2 *= fact;
 	}
 
-	btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);		
-	btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);	
-	btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
+	btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1);
+	btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2);
+	btScalar EllipseAngle = btFabs(swing1 * swing1) * RMaxAngle1Sq + btFabs(swing2 * swing2) * RMaxAngle2Sq;
 
 	if (EllipseAngle > 1.0f)
 	{
-		m_swingCorrection = EllipseAngle-1.0f;
+		m_swingCorrection = EllipseAngle - 1.0f;
 		m_solveSwingLimit = true;
 		// Calculate necessary axis & factors
-		m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
+		m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3));
 		m_swingAxis.normalize();
 		btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
 		m_swingAxis *= swingAxisSign;
@@ -594,14 +573,14 @@ void btConeTwistConstraint::calcAngleInfo()
 	if (m_twistSpan >= btScalar(0.))
 	{
 		btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
-		btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
-		btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
-		btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
+		btQuaternion rotationArc = shortestArcQuat(b2Axis1, b1Axis1);
+		btVector3 TwistRef = quatRotate(rotationArc, b2Axis2);
+		btScalar twist = btAtan2Fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
 		m_twistAngle = twist;
 
-//		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
+		//		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
 		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
-		if (twist <= -m_twistSpan*lockedFreeFactor)
+		if (twist <= -m_twistSpan * lockedFreeFactor)
 		{
 			m_twistCorrection = -(twist + m_twistSpan);
 			m_solveTwistLimit = true;
@@ -609,7 +588,7 @@ void btConeTwistConstraint::calcAngleInfo()
 			m_twistAxis.normalize();
 			m_twistAxis *= -1.0f;
 		}
-		else if (twist >  m_twistSpan*lockedFreeFactor)
+		else if (twist > m_twistSpan * lockedFreeFactor)
 		{
 			m_twistCorrection = (twist - m_twistSpan);
 			m_solveTwistLimit = true;
@@ -618,13 +597,11 @@ void btConeTwistConstraint::calcAngleInfo()
 		}
 	}
 }
-#endif //__SPU__
-
-static btVector3 vTwist(1,0,0); // twist axis in constraint's space
+#endif  //__SPU__
 
+static btVector3 vTwist(1, 0, 0);  // twist axis in constraint's space
 
-
-void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
+void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB)
 {
 	m_swingCorrection = btScalar(0.);
 	m_twistLimitSign = btScalar(0.);
@@ -632,7 +609,7 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 	m_solveSwingLimit = false;
 	// compute rotation of A wrt B (in constraint space)
 	if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
-	{	// it is assumed that setMotorTarget() was alredy called 
+	{  // it is assumed that setMotorTarget() was alredy called
 		// and motor target m_qTarget is within constraint limits
 		// TODO : split rotation to pure swing and pure twist
 		// compute desired transforms in world
@@ -641,23 +618,22 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 		btTransform trB = transB * m_rbBFrame;
 		btTransform trDeltaAB = trB * trPose * trA.inverse();
 		btQuaternion qDeltaAB = trDeltaAB.getRotation();
-		btVector3 swingAxis = 	btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
-		float swingAxisLen2 = swingAxis.length2();
-		if(btFuzzyZero(swingAxisLen2))
+		btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
+		btScalar swingAxisLen2 = swingAxis.length2();
+		if (btFuzzyZero(swingAxisLen2))
 		{
-		   return;
+			return;
 		}
 		m_swingAxis = swingAxis;
 		m_swingAxis.normalize();
 		m_swingCorrection = qDeltaAB.getAngle();
-		if(!btFuzzyZero(m_swingCorrection))
+		if (!btFuzzyZero(m_swingCorrection))
 		{
 			m_solveSwingLimit = true;
 		}
 		return;
 	}
 
-
 	{
 		// compute rotation of A wrt B (in constraint space)
 		btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
@@ -665,13 +641,17 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 		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();
+		btVector3 vConeNoTwist = quatRotate(qAB, vTwist);
+		vConeNoTwist.normalize();
+		btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist);
+		qABCone.normalize();
+		btQuaternion qABTwist = qABCone.inverse() * qAB;
+		qABTwist.normalize();
 
 		if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
 		{
-			btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
+			btScalar swingAngle, swingLimit = 0;
+			btVector3 swingAxis;
 			computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
 
 			if (swingAngle > swingLimit * m_limitSoftness)
@@ -684,9 +664,9 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 				m_swingLimitRatio = 1.f;
 				if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
 				{
-					m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
+					m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness) /
 										(swingLimit - swingLimit * m_limitSoftness);
-				}				
+				}
 
 				// swing correction tries to get back to soft limit
 				m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
@@ -694,14 +674,14 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 				// adjustment of swing axis (based on ellipse normal)
 				adjustSwingAxisToUseEllipseNormal(swingAxis);
 
-				// Calculate necessary axis & factors		
+				// Calculate necessary axis & factors
 				m_swingAxis = quatRotate(qB, -swingAxis);
 
-				m_twistAxisA.setValue(0,0,0);
+				m_twistAxisA.setValue(0, 0, 0);
 
-				m_kSwing =  btScalar(1.) /
-					(computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
-					 computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
+				m_kSwing = btScalar(1.) /
+						   (computeAngularImpulseDenominator(m_swingAxis, invInertiaWorldA) +
+							computeAngularImpulseDenominator(m_swingAxis, invInertiaWorldB));
 			}
 		}
 		else
@@ -717,9 +697,9 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 			btScalar x = ivB.dot(ivA);
 			btScalar y = ivB.dot(jvA);
 			btScalar z = ivB.dot(kvA);
-			if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
-			{ // fixed. We'll need to add one more row to constraint
-				if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
+			if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+			{  // fixed. We'll need to add one more row to constraint
+				if ((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
 				{
 					m_solveSwingLimit = true;
 					m_swingAxis = -ivB.cross(ivA);
@@ -727,47 +707,47 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 			}
 			else
 			{
-				if(m_swingSpan1 < m_fixThresh)
-				{ // hinge around Y axis
-//					if(!(btFuzzyZero(y)))
-					if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
+				if (m_swingSpan1 < m_fixThresh)
+				{  // hinge around Y axis
+					//					if(!(btFuzzyZero(y)))
+					if ((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
 					{
 						m_solveSwingLimit = true;
-						if(m_swingSpan2 >= m_fixThresh)
+						if (m_swingSpan2 >= m_fixThresh)
 						{
 							y = btScalar(0.f);
 							btScalar span2 = btAtan2(z, x);
-							if(span2 > m_swingSpan2)
+							if (span2 > m_swingSpan2)
 							{
 								x = btCos(m_swingSpan2);
 								z = btSin(m_swingSpan2);
 							}
-							else if(span2 < -m_swingSpan2)
+							else if (span2 < -m_swingSpan2)
 							{
-								x =  btCos(m_swingSpan2);
+								x = btCos(m_swingSpan2);
 								z = -btSin(m_swingSpan2);
 							}
 						}
 					}
 				}
 				else
-				{ // hinge around Z axis
-//					if(!btFuzzyZero(z))
-					if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y))))
+				{  // hinge around Z axis
+					//					if(!btFuzzyZero(z))
+					if ((!(btFuzzyZero(x))) || (!(btFuzzyZero(y))))
 					{
 						m_solveSwingLimit = true;
-						if(m_swingSpan1 >= m_fixThresh)
+						if (m_swingSpan1 >= m_fixThresh)
 						{
 							z = btScalar(0.f);
 							btScalar span1 = btAtan2(y, x);
-							if(span1 > m_swingSpan1)
+							if (span1 > m_swingSpan1)
 							{
 								x = btCos(m_swingSpan1);
 								y = btSin(m_swingSpan1);
 							}
-							else if(span1 < -m_swingSpan1)
+							else if (span1 < -m_swingSpan1)
 							{
-								x =  btCos(m_swingSpan1);
+								x = btCos(m_swingSpan1);
 								y = -btSin(m_swingSpan1);
 							}
 						}
@@ -778,10 +758,10 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 				target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
 				target.normalize();
 				m_swingAxis = -ivB.cross(target);
-                                m_swingCorrection = m_swingAxis.length();
+				m_swingCorrection = m_swingAxis.length();
 
-                                if (!btFuzzyZero(m_swingCorrection))
-                                    m_swingAxis.normalize();
+				if (!btFuzzyZero(m_swingCorrection))
+					m_swingAxis.normalize();
 			}
 		}
 
@@ -790,15 +770,15 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 			btVector3 twistAxis;
 			computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
 
-			if (m_twistAngle > m_twistSpan*m_limitSoftness)
+			if (m_twistAngle > m_twistSpan * m_limitSoftness)
 			{
 				m_solveTwistLimit = true;
 
 				m_twistLimitRatio = 1.f;
 				if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
 				{
-					m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
-										(m_twistSpan  - m_twistSpan * m_limitSoftness);
+					m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness) /
+										(m_twistSpan - m_twistSpan * m_limitSoftness);
 				}
 
 				// twist correction tries to get back to soft limit
@@ -807,8 +787,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 				m_twistAxis = quatRotate(qB, -twistAxis);
 
 				m_kTwist = btScalar(1.) /
-					(computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
-					 computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
+						   (computeAngularImpulseDenominator(m_twistAxis, invInertiaWorldA) +
+							computeAngularImpulseDenominator(m_twistAxis, invInertiaWorldB));
 			}
 
 			if (m_solveSwingLimit)
@@ -821,15 +801,13 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 	}
 }
 
-
-
 // given a cone rotation in constraint space, (pre: twist must already be removed)
 // this method computes its corresponding swing angle and axis.
 // more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
 void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
-												 btScalar& swingAngle, // out
-												 btVector3& vSwingAxis, // out
-												 btScalar& swingLimit) // out
+												 btScalar& swingAngle,   // out
+												 btVector3& vSwingAxis,  // out
+												 btScalar& swingLimit)   // out
 {
 	swingAngle = qCone.getAngle();
 	if (swingAngle > SIMD_EPSILON)
@@ -840,7 +818,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
         // non-zero twist?! this should never happen.
        btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
 #endif
-        
+
 		// Compute limit for given swing. tricky:
 		// Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
 		// (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
@@ -848,7 +826,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
 		// For starters, compute the direction from center to surface of ellipse.
 		// This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
 		// (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
-		btScalar xEllipse =  vSwingAxis.y();
+		btScalar xEllipse = vSwingAxis.y();
 		btScalar yEllipse = -vSwingAxis.z();
 
 		// Now, we use the slope of the vector (using x/yEllipse) and find the length
@@ -858,14 +836,14 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
 		//  a^2   b^2
 		// Do the math and it should be clear.
 
-		swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
+		swingLimit = m_swingSpan1;  // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
 		if (fabs(xEllipse) > SIMD_EPSILON)
 		{
-			btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
+			btScalar surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse);
 			btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
 			norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
 			btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
-			swingLimit = sqrt(swingLimit2);
+			swingLimit = std::sqrt(swingLimit2);
 		}
 
 		// test!
@@ -887,7 +865,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
 #if 0
         btAssert(0);
 #endif
- 	}
+	}
 }
 
 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
@@ -903,34 +881,34 @@ btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btSc
 	//  a^2   b^2
 	// Do the math and it should be clear.
 
-	float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
+	btScalar swingLimit = m_swingSpan1;  // if xEllipse == 0, just use axis b (1)
 	if (fabs(xEllipse) > SIMD_EPSILON)
 	{
-		btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
+		btScalar surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse);
 		btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
 		norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
 		btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
-		swingLimit = sqrt(swingLimit2);
+		swingLimit = std::sqrt(swingLimit2);
 	}
 
 	// convert into point in constraint space:
 	// note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
 	btVector3 vSwingAxis(0, xEllipse, -yEllipse);
 	btQuaternion qSwing(vSwingAxis, swingLimit);
-	btVector3 vPointInConstraintSpace(fLength,0,0);
+	btVector3 vPointInConstraintSpace(fLength, 0, 0);
 	return quatRotate(qSwing, vPointInConstraintSpace);
 }
 
 // given a twist rotation in constraint space, (pre: cone must already be removed)
 // this method computes its corresponding angle and axis.
 void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
-												  btScalar& twistAngle, // out
-												  btVector3& vTwistAxis) // out
+												  btScalar& twistAngle,   // out
+												  btVector3& vTwistAxis)  // out
 {
 	btQuaternion qMinTwist = qTwist;
 	twistAngle = qTwist.getAngle();
 
-	if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
+	if (twistAngle > SIMD_PI)  // long way around. flip quat and recalculate.
 	{
 		qMinTwist = -(qTwist);
 		twistAngle = qMinTwist.getAngle();
@@ -948,80 +926,79 @@ void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
 		vTwistAxis.normalize();
 }
 
-
 void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
 {
 	// the swing axis is computed as the "twist-free" cone rotation,
 	// but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
-	// so, if we're outside the limits, the closest way back inside the cone isn't 
+	// so, if we're outside the limits, the closest way back inside the cone isn't
 	// along the vector back to the center. better (and more stable) to use the ellipse normal.
 
 	// convert swing axis to direction from center to surface of ellipse
 	// (ie. rotate 2D vector by PI/2)
 	btScalar y = -vSwingAxis.z();
-	btScalar z =  vSwingAxis.y();
+	btScalar z = vSwingAxis.y();
 
 	// do the math...
-	if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
+	if (fabs(z) > SIMD_EPSILON)  // avoid division by 0. and we don't need an update if z == 0.
 	{
 		// compute gradient/normal of ellipse surface at current "point"
-		btScalar grad = y/z;
+		btScalar grad = y / z;
 		grad *= m_swingSpan2 / m_swingSpan1;
 
 		// adjust y/z to represent normal at point (instead of vector to point)
 		if (y > 0)
-			y =  fabs(grad * z);
+			y = fabs(grad * z);
 		else
 			y = -fabs(grad * z);
 
 		// convert ellipse direction back to swing axis
 		vSwingAxis.setZ(-y);
-		vSwingAxis.setY( z);
+		vSwingAxis.setY(z);
 		vSwingAxis.normalize();
 	}
 }
 
-
-
-void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
+void btConeTwistConstraint::setMotorTarget(const btQuaternion& q)
 {
 	//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);
+	//	btTransform trABCur = trBCur.inverse() * trACur;
+	//	btQuaternion qABCur = trABCur.getRotation();
+	//	btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
 	//btQuaternion qConstraintCur = trConstraintCur.getRotation();
 
 	btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
 	setMotorTargetInConstraintSpace(qConstraint);
 }
 
-
-void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
+void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion& q)
 {
 	m_qTarget = q;
 
 	// clamp motor target to within limits
 	{
-		btScalar softness = 1.f;//m_limitSoftness;
+		btScalar softness = 1.f;  //m_limitSoftness;
 
 		// split into twist and cone
 		btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
-		btQuaternion qTargetCone  = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
-		btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
+		btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted);
+		qTargetCone.normalize();
+		btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget;
+		qTargetTwist.normalize();
 
 		// clamp cone
 		if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
 		{
-			btScalar swingAngle, swingLimit; btVector3 swingAxis;
+			btScalar swingAngle, swingLimit;
+			btVector3 swingAxis;
 			computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
 
 			if (fabs(swingAngle) > SIMD_EPSILON)
 			{
-				if (swingAngle > swingLimit*softness)
-					swingAngle = swingLimit*softness;
-				else if (swingAngle < -swingLimit*softness)
-					swingAngle = -swingLimit*softness;
+				if (swingAngle > swingLimit * softness)
+					swingAngle = swingLimit * softness;
+				else if (swingAngle < -swingLimit * softness)
+					swingAngle = -swingLimit * softness;
 				qTargetCone = btQuaternion(swingAxis, swingAngle);
 			}
 		}
@@ -1029,16 +1006,17 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &
 		// clamp twist
 		if (m_twistSpan >= btScalar(0.05f))
 		{
-			btScalar twistAngle; btVector3 twistAxis;
+			btScalar twistAngle;
+			btVector3 twistAxis;
 			computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
 
 			if (fabs(twistAngle) > SIMD_EPSILON)
 			{
 				// eddy todo: limitSoftness used here???
-				if (twistAngle > m_twistSpan*softness)
-					twistAngle = m_twistSpan*softness;
-				else if (twistAngle < -m_twistSpan*softness)
-					twistAngle = -m_twistSpan*softness;
+				if (twistAngle > m_twistSpan * softness)
+					twistAngle = m_twistSpan * softness;
+				else if (twistAngle < -m_twistSpan * softness)
+					twistAngle = -m_twistSpan * softness;
 				qTargetTwist = btQuaternion(twistAxis, twistAngle);
 			}
 		}
@@ -1047,15 +1025,15 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &
 	}
 }
 
-///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///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 btConeTwistConstraint::setParam(int num, btScalar value, int axis)
 {
-	switch(num)
+	switch (num)
 	{
-		case BT_CONSTRAINT_ERP :
-		case BT_CONSTRAINT_STOP_ERP :
-			if((axis >= 0) && (axis < 3)) 
+		case BT_CONSTRAINT_ERP:
+		case BT_CONSTRAINT_STOP_ERP:
+			if ((axis >= 0) && (axis < 3))
 			{
 				m_linERP = value;
 				m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
@@ -1065,9 +1043,9 @@ void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
 				m_biasFactor = value;
 			}
 			break;
-		case BT_CONSTRAINT_CFM :
-		case BT_CONSTRAINT_STOP_CFM :
-			if((axis >= 0) && (axis < 3)) 
+		case BT_CONSTRAINT_CFM:
+		case BT_CONSTRAINT_STOP_CFM:
+			if ((axis >= 0) && (axis < 3))
 			{
 				m_linCFM = value;
 				m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
@@ -1085,19 +1063,19 @@ void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
 }
 
 ///return the local value of parameter
-btScalar btConeTwistConstraint::getParam(int num, int axis) const 
+btScalar btConeTwistConstraint::getParam(int num, int axis) const
 {
 	btScalar retVal = 0;
-	switch(num)
+	switch (num)
 	{
-		case BT_CONSTRAINT_ERP :
-		case BT_CONSTRAINT_STOP_ERP :
-			if((axis >= 0) && (axis < 3)) 
+		case BT_CONSTRAINT_ERP:
+		case BT_CONSTRAINT_STOP_ERP:
+			if ((axis >= 0) && (axis < 3))
 			{
 				btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
 				retVal = m_linERP;
 			}
-			else if((axis >= 3) && (axis < 6)) 
+			else if ((axis >= 3) && (axis < 6))
 			{
 				retVal = m_biasFactor;
 			}
@@ -1106,14 +1084,14 @@ btScalar btConeTwistConstraint::getParam(int num, int axis) const
 				btAssertConstrParams(0);
 			}
 			break;
-		case BT_CONSTRAINT_CFM :
-		case BT_CONSTRAINT_STOP_CFM :
-			if((axis >= 0) && (axis < 3)) 
+		case BT_CONSTRAINT_CFM:
+		case BT_CONSTRAINT_STOP_CFM:
+			if ((axis >= 0) && (axis < 3))
 			{
 				btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
 				retVal = m_linCFM;
 			}
-			else if((axis >= 3) && (axis < 6)) 
+			else if ((axis >= 3) && (axis < 6))
 			{
 				btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
 				retVal = m_angCFM;
@@ -1123,21 +1101,16 @@ btScalar btConeTwistConstraint::getParam(int num, int axis) const
 				btAssertConstrParams(0);
 			}
 			break;
-		default : 
+		default:
 			btAssertConstrParams(0);
 	}
 	return retVal;
 }
 
-
-void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
+void btConeTwistConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
 {
 	m_rbAFrame = frameA;
 	m_rbBFrame = frameB;
 	buildJacobian();
 	//calculateTransforms();
 }
-
- 
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
index b7636180c34..64f44df1cba 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
@@ -15,8 +15,6 @@ subject to the following restrictions:
 Written by: Marcus Hennix
 */
 
-
-
 /*
 Overview:
 
@@ -31,8 +29,6 @@ twist is along the x-axis,
 and swing 1 and 2 are along the z and y axes respectively.
 */
 
-
-
 #ifndef BT_CONETWISTCONSTRAINT_H
 #define BT_CONETWISTCONSTRAINT_H
 
@@ -41,13 +37,12 @@ and swing 1 and 2 are along the z and y axes respectively.
 #include "btTypedConstraint.h"
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btConeTwistConstraintData2	btConeTwistConstraintDoubleData
-#define btConeTwistConstraintDataName	"btConeTwistConstraintDoubleData"
+#define btConeTwistConstraintData2 btConeTwistConstraintDoubleData
+#define btConeTwistConstraintDataName "btConeTwistConstraintDoubleData"
 #else
-#define btConeTwistConstraintData2	btConeTwistConstraintData 
-#define btConeTwistConstraintDataName	"btConeTwistConstraintData" 
-#endif //BT_USE_DOUBLE_PRECISION
-
+#define btConeTwistConstraintData2 btConeTwistConstraintData
+#define btConeTwistConstraintDataName "btConeTwistConstraintData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 class btRigidBody;
 
@@ -59,103 +54,99 @@ enum btConeTwistFlags
 };
 
 ///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
-ATTRIBUTE_ALIGNED16(class) btConeTwistConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class)
+btConeTwistConstraint : public btTypedConstraint
 {
 #ifdef IN_PARALLELL_SOLVER
 public:
 #endif
-	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
+	btJacobianEntry m_jac[3];  //3 orthogonal linear constraints
 
-	btTransform m_rbAFrame; 
+	btTransform m_rbAFrame;
 	btTransform m_rbBFrame;
 
-	btScalar	m_limitSoftness;
-	btScalar	m_biasFactor;
-	btScalar	m_relaxationFactor;
+	btScalar m_limitSoftness;
+	btScalar m_biasFactor;
+	btScalar m_relaxationFactor;
 
-	btScalar	m_damping;
+	btScalar m_damping;
 
-	btScalar	m_swingSpan1;
-	btScalar	m_swingSpan2;
-	btScalar	m_twistSpan;
+	btScalar m_swingSpan1;
+	btScalar m_swingSpan2;
+	btScalar m_twistSpan;
 
-	btScalar	m_fixThresh;
+	btScalar m_fixThresh;
 
-	btVector3   m_swingAxis;
-	btVector3	m_twistAxis;
+	btVector3 m_swingAxis;
+	btVector3 m_twistAxis;
 
-	btScalar	m_kSwing;
-	btScalar	m_kTwist;
+	btScalar m_kSwing;
+	btScalar m_kTwist;
 
-	btScalar	m_twistLimitSign;
-	btScalar	m_swingCorrection;
-	btScalar	m_twistCorrection;
+	btScalar m_twistLimitSign;
+	btScalar m_swingCorrection;
+	btScalar m_twistCorrection;
 
-	btScalar	m_twistAngle;
+	btScalar m_twistAngle;
 
-	btScalar	m_accSwingLimitImpulse;
-	btScalar	m_accTwistLimitImpulse;
+	btScalar m_accSwingLimitImpulse;
+	btScalar m_accTwistLimitImpulse;
 
-	bool		m_angularOnly;
-	bool		m_solveTwistLimit;
-	bool		m_solveSwingLimit;
+	bool m_angularOnly;
+	bool m_solveTwistLimit;
+	bool m_solveSwingLimit;
 
-	bool	m_useSolveConstraintObsolete;
+	bool m_useSolveConstraintObsolete;
 
 	// not yet used...
-	btScalar	m_swingLimitRatio;
-	btScalar	m_twistLimitRatio;
-	btVector3   m_twistAxisA;
+	btScalar m_swingLimitRatio;
+	btScalar m_twistLimitRatio;
+	btVector3 m_twistAxisA;
 
 	// motor
-	bool		 m_bMotorEnabled;
-	bool		 m_bNormalizedMotorStrength;
+	bool m_bMotorEnabled;
+	bool m_bNormalizedMotorStrength;
 	btQuaternion m_qTarget;
-	btScalar	 m_maxMotorImpulse;
-	btVector3	 m_accMotorImpulse;
-	
+	btScalar m_maxMotorImpulse;
+	btVector3 m_accMotorImpulse;
+
 	// parameters
-	int			m_flags;
-	btScalar	m_linCFM;
-	btScalar	m_linERP;
-	btScalar	m_angCFM;
-	
-protected:
+	int m_flags;
+	btScalar m_linCFM;
+	btScalar m_linERP;
+	btScalar m_angCFM;
 
+protected:
 	void init();
 
-	void computeConeLimitInfo(const btQuaternion& qCone, // in
-		btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
+	void computeConeLimitInfo(const btQuaternion& qCone,                                           // in
+							  btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit);  // all outs
 
-	void computeTwistLimitInfo(const btQuaternion& qTwist, // in
-		btScalar& twistAngle, btVector3& vTwistAxis); // all outs
-
-	void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
+	void computeTwistLimitInfo(const btQuaternion& qTwist,                    // in
+							   btScalar& twistAngle, btVector3& vTwistAxis);  // all outs
 
+	void adjustSwingAxisToUseEllipseNormal(btVector3 & vSwingAxis) const;
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
-	
-	btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
+	btConeTwistConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
+
+	btConeTwistConstraint(btRigidBody & rbA, const btTransform& rbAFrame);
+
+	virtual void buildJacobian();
 
-	virtual void	buildJacobian();
+	virtual void getInfo1(btConstraintInfo1 * info);
 
-	virtual void getInfo1 (btConstraintInfo1* info);
+	void getInfo1NonVirtual(btConstraintInfo1 * info);
 
-	void	getInfo1NonVirtual(btConstraintInfo1* info);
-	
-	virtual void getInfo2 (btConstraintInfo2* info);
-	
-	void	getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
+	virtual void getInfo2(btConstraintInfo2 * info);
 
-	virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
+	void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB);
 
-    
-	void	updateRHS(btScalar	timeStep);
+	virtual void solveConstraintObsolete(btSolverBody & bodyA, btSolverBody & bodyB, btScalar timeStep);
 
+	void updateRHS(btScalar timeStep);
 
 	const btRigidBody& getRigidBodyA() const
 	{
@@ -166,64 +157,64 @@ public:
 		return m_rbB;
 	}
 
-	void	setAngularOnly(bool angularOnly)
+	void setAngularOnly(bool angularOnly)
 	{
 		m_angularOnly = angularOnly;
 	}
-	
-	bool    getAngularOnly() const
+
+	bool getAngularOnly() const
 	{
-	    return m_angularOnly;
+		return m_angularOnly;
 	}
 
-	void	setLimit(int limitIndex,btScalar limitValue)
+	void setLimit(int limitIndex, btScalar limitValue)
 	{
 		switch (limitIndex)
 		{
-		case 3:
+			case 3:
 			{
 				m_twistSpan = limitValue;
 				break;
 			}
-		case 4:
+			case 4:
 			{
 				m_swingSpan2 = limitValue;
 				break;
 			}
-		case 5:
+			case 5:
 			{
 				m_swingSpan1 = limitValue;
 				break;
 			}
-		default:
+			default:
 			{
 			}
 		};
 	}
 
-    btScalar getLimit(int limitIndex) const
+	btScalar getLimit(int limitIndex) const
 	{
 		switch (limitIndex)
 		{
-		case 3:
+			case 3:
 			{
 				return m_twistSpan;
 				break;
 			}
-		case 4:
+			case 4:
 			{
 				return m_swingSpan2;
 				break;
 			}
-		case 5:
+			case 5:
 			{
 				return m_swingSpan1;
 				break;
 			}
-		default:
+			default:
 			{
-			    btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint");
-			    return 0.0;
+				btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint");
+				return 0.0;
 			}
 		};
 	}
@@ -239,18 +230,18 @@ public:
 	// __relaxationFactor:
 	//		0->1, recommend to stay near 1.
 	//		the lower the value, the less the constraint will fight velocities which violate the angular limits.
-	void	setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+	void setLimit(btScalar _swingSpan1, btScalar _swingSpan2, btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
 	{
 		m_swingSpan1 = _swingSpan1;
 		m_swingSpan2 = _swingSpan2;
-		m_twistSpan  = _twistSpan;
+		m_twistSpan = _twistSpan;
 
-		m_limitSoftness =  _softness;
+		m_limitSoftness = _softness;
 		m_biasFactor = _biasFactor;
 		m_relaxationFactor = _relaxationFactor;
 	}
 
-	const btTransform& getAFrame() const { return m_rbAFrame; };	
+	const btTransform& getAFrame() const { return m_rbAFrame; };
 	const btTransform& getBFrame() const { return m_rbBFrame; };
 
 	inline int getSolveTwistLimit()
@@ -260,7 +251,7 @@ public:
 
 	inline int getSolveSwingLimit()
 	{
-		return m_solveTwistLimit;
+		return m_solveSwingLimit;
 	}
 
 	inline btScalar getTwistLimitSign()
@@ -269,7 +260,7 @@ public:
 	}
 
 	void calcAngleInfo();
-	void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
+	void calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB);
 
 	inline btScalar getSwingSpan1() const
 	{
@@ -308,8 +299,16 @@ public:
 	bool isMotorEnabled() const { return m_bMotorEnabled; }
 	btScalar getMaxMotorImpulse() const { return m_maxMotorImpulse; }
 	bool isMaxMotorImpulseNormalized() const { return m_bNormalizedMotorStrength; }
-	void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
-	void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
+	void setMaxMotorImpulse(btScalar maxMotorImpulse)
+	{
+		m_maxMotorImpulse = maxMotorImpulse;
+		m_bNormalizedMotorStrength = false;
+	}
+	void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse)
+	{
+		m_maxMotorImpulse = maxMotorImpulse;
+		m_bNormalizedMotorStrength = true;
+	}
 
 	btScalar getFixThresh() { return m_fixThresh; }
 	void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
@@ -318,17 +317,17 @@ public:
 	// q: the desired rotation of bodyA wrt bodyB.
 	// note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
 	// note: don't forget to enableMotor()
-	void setMotorTarget(const btQuaternion &q);
+	void setMotorTarget(const btQuaternion& q);
 	const btQuaternion& getMotorTarget() const { return m_qTarget; }
 
 	// same as above, but q is the desired rotation of frameA wrt frameB in constraint space
-	void setMotorTargetInConstraintSpace(const btQuaternion &q);
+	void setMotorTargetInConstraintSpace(const btQuaternion& q);
 
 	btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///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 void setParam(int num, btScalar value, int axis = -1);
 
 	virtual void setFrames(const btTransform& frameA, const btTransform& frameB);
 
@@ -342,84 +341,74 @@ public:
 		return m_rbBFrame;
 	}
 
-
 	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const;
+	virtual btScalar getParam(int num, int axis = -1) const;
 
 	int getFlags() const
 	{
 		return m_flags;
 	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
-
-	
-struct	btConeTwistConstraintDoubleData
+struct btConeTwistConstraintDoubleData
 {
-	btTypedConstraintDoubleData	m_typeConstraintData;
+	btTypedConstraintDoubleData m_typeConstraintData;
 	btTransformDoubleData m_rbAFrame;
 	btTransformDoubleData m_rbBFrame;
 
 	//limits
-	double	m_swingSpan1;
-	double	m_swingSpan2;
-	double	m_twistSpan;
-	double	m_limitSoftness;
-	double	m_biasFactor;
-	double	m_relaxationFactor;
-
-	double	m_damping;
-		
-	
-
+	double m_swingSpan1;
+	double m_swingSpan2;
+	double m_twistSpan;
+	double m_limitSoftness;
+	double m_biasFactor;
+	double m_relaxationFactor;
+
+	double m_damping;
 };
 
 #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 ///this structure is not used, except for loading pre-2.82 .bullet files
-struct	btConeTwistConstraintData
+struct btConeTwistConstraintData
 {
-	btTypedConstraintData	m_typeConstraintData;
+	btTypedConstraintData m_typeConstraintData;
 	btTransformFloatData m_rbAFrame;
 	btTransformFloatData m_rbBFrame;
 
 	//limits
-	float	m_swingSpan1;
-	float	m_swingSpan2;
-	float	m_twistSpan;
-	float	m_limitSoftness;
-	float	m_biasFactor;
-	float	m_relaxationFactor;
-
-	float	m_damping;
-		
-	char m_pad[4];
+	float m_swingSpan1;
+	float m_swingSpan2;
+	float m_twistSpan;
+	float m_limitSoftness;
+	float m_biasFactor;
+	float m_relaxationFactor;
 
+	float m_damping;
+
+	char m_pad[4];
 };
-#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+#endif  //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 //
 
-SIMD_FORCE_INLINE int	btConeTwistConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btConeTwistConstraintData2);
-
 }
 
-
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE const char*	btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*) dataBuffer;
-	btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
+	btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*)dataBuffer;
+	btTypedConstraint::serialize(&cone->m_typeConstraintData, serializer);
 
 	m_rbAFrame.serialize(cone->m_rbAFrame);
 	m_rbBFrame.serialize(cone->m_rbBFrame);
-	
+
 	cone->m_swingSpan1 = m_swingSpan1;
 	cone->m_swingSpan2 = m_swingSpan2;
 	cone->m_twistSpan = m_twistSpan;
@@ -431,5 +420,4 @@ SIMD_FORCE_INLINE const char*	btConeTwistConstraint::serialize(void* dataBuffer,
 	return btConeTwistConstraintDataName;
 }
 
-
-#endif //BT_CONETWISTCONSTRAINT_H
+#endif  //BT_CONETWISTCONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
index 890afe6da42..68a4a07a1da 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
@@ -26,40 +26,34 @@ struct btContactSolverInfo;
 struct btBroadphaseProxy;
 class btIDebugDraw;
 class btStackAlloc;
-class	btDispatcher;
+class btDispatcher;
 /// btConstraintSolver provides solver interface
 
-
 enum btConstraintSolverType
 {
-	BT_SEQUENTIAL_IMPULSE_SOLVER=1,
-	BT_MLCP_SOLVER=2,
-	BT_NNCG_SOLVER=4
+	BT_SEQUENTIAL_IMPULSE_SOLVER = 1,
+	BT_MLCP_SOLVER = 2,
+	BT_NNCG_SOLVER = 4,
+	BT_MULTIBODY_SOLVER = 8,
+	BT_BLOCK_SOLVER = 16,
 };
 
 class btConstraintSolver
 {
-
 public:
-
 	virtual ~btConstraintSolver() {}
-	
-	virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;}
+
+	virtual void prepareSolve(int /* numBodies */, int /* numManifolds */) { ; }
 
 	///solve a group of constraints
-	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer,btDispatcher* dispatcher) = 0;
+	virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, class btIDebugDraw* debugDrawer, btDispatcher* dispatcher) = 0;
 
-	virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */) {;}
+	virtual void allSolved(const btContactSolverInfo& /* info */, class btIDebugDraw* /* debugDrawer */) { ; }
 
 	///clear internal cached data and reset random seed
-	virtual	void	reset() = 0;
-
-	virtual btConstraintSolverType	getSolverType() const=0;
-
+	virtual void reset() = 0;
 
+	virtual btConstraintSolverType getSolverType() const = 0;
 };
 
-
-
-
-#endif //BT_CONSTRAINT_SOLVER_H
+#endif  //BT_CONSTRAINT_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
index 1098d0c96b6..4b22b2fff5b 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btContactConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btVector3.h"
@@ -22,44 +21,33 @@ subject to the following restrictions:
 #include "LinearMath/btMinMax.h"
 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
 
-
-
-btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
-:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
-	m_contactManifold(*contactManifold)
+btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold, btRigidBody& rbA, btRigidBody& rbB)
+	: btTypedConstraint(CONTACT_CONSTRAINT_TYPE, rbA, rbB),
+	  m_contactManifold(*contactManifold)
 {
-
 }
 
 btContactConstraint::~btContactConstraint()
 {
-
 }
 
-void	btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
+void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
 {
 	m_contactManifold = *contactManifold;
 }
 
-void btContactConstraint::getInfo1 (btConstraintInfo1* info)
+void btContactConstraint::getInfo1(btConstraintInfo1* info)
 {
-
 }
 
-void btContactConstraint::getInfo2 (btConstraintInfo2* info)
+void btContactConstraint::getInfo2(btConstraintInfo2* info)
 {
-
 }
 
-void	btContactConstraint::buildJacobian()
+void btContactConstraint::buildJacobian()
 {
-
 }
 
-
-
-
-
 #include "btContactConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btVector3.h"
@@ -68,64 +56,59 @@ void	btContactConstraint::buildJacobian()
 #include "LinearMath/btMinMax.h"
 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
 
-
-
 //response  between two dynamic objects without friction and no restitution, assuming 0 penetration depth
 btScalar resolveSingleCollision(
-        btRigidBody* body1,
-        btCollisionObject* colObj2,
-		const btVector3& contactPositionWorld,
-		const btVector3& contactNormalOnB,
-        const btContactSolverInfo& solverInfo,
-		btScalar distance)
+	btRigidBody* body1,
+	btCollisionObject* colObj2,
+	const btVector3& contactPositionWorld,
+	const btVector3& contactNormalOnB,
+	const btContactSolverInfo& solverInfo,
+	btScalar distance)
 {
 	btRigidBody* body2 = btRigidBody::upcast(colObj2);
-    
-	
-    const btVector3& normal = contactNormalOnB;
-
-    btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin(); 
-    btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin();
-    
-    btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1);
-	btVector3 vel2 = body2? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
-    btVector3 vel = vel1 - vel2;
-    btScalar rel_vel;
-    rel_vel = normal.dot(vel);
-    
-    btScalar combinedRestitution = 0.f;
-    btScalar restitution = combinedRestitution* -rel_vel;
-
-    btScalar positionalError = solverInfo.m_erp *-distance /solverInfo.m_timeStep ;
-    btScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping;
-	btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld,normal);
-	btScalar denom1 = body2? body2->computeImpulseDenominator(contactPositionWorld,normal) : 0.f;
+
+	const btVector3& normal = contactNormalOnB;
+
+	btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin();
+	btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin();
+
+	btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1);
+	btVector3 vel2 = body2 ? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0, 0, 0);
+	btVector3 vel = vel1 - vel2;
+	btScalar rel_vel;
+	rel_vel = normal.dot(vel);
+
+	btScalar combinedRestitution = 0.f;
+	btScalar restitution = combinedRestitution * -rel_vel;
+
+	btScalar positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep;
+	btScalar velocityError = -(1.0f + restitution) * rel_vel;  // * damping;
+	btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld, normal);
+	btScalar denom1 = body2 ? body2->computeImpulseDenominator(contactPositionWorld, normal) : 0.f;
 	btScalar relaxation = 1.f;
-	btScalar jacDiagABInv = relaxation/(denom0+denom1);
+	btScalar jacDiagABInv = relaxation / (denom0 + denom1);
 
-    btScalar penetrationImpulse = positionalError * jacDiagABInv;
-    btScalar velocityImpulse = velocityError * jacDiagABInv;
+	btScalar penetrationImpulse = positionalError * jacDiagABInv;
+	btScalar velocityImpulse = velocityError * jacDiagABInv;
 
-    btScalar normalImpulse = penetrationImpulse+velocityImpulse;
-    normalImpulse = 0.f > normalImpulse ? 0.f: normalImpulse;
+	btScalar normalImpulse = penetrationImpulse + velocityImpulse;
+	normalImpulse = 0.f > normalImpulse ? 0.f : normalImpulse;
 
-	body1->applyImpulse(normal*(normalImpulse), rel_pos1);
-    if (body2)
-		body2->applyImpulse(-normal*(normalImpulse), rel_pos2);
-    
-    return normalImpulse;
-}
+	body1->applyImpulse(normal * (normalImpulse), rel_pos1);
+	if (body2)
+		body2->applyImpulse(-normal * (normalImpulse), rel_pos2);
 
+	return normalImpulse;
+}
 
 //bilateral constraint between two dynamic objects
 void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
-                      btRigidBody& body2, const btVector3& pos2,
-                      btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
+							btRigidBody& body2, const btVector3& pos2,
+							btScalar distance, const btVector3& normal, btScalar& impulse, btScalar timeStep)
 {
 	(void)timeStep;
 	(void)distance;
 
-
 	btScalar normalLenSqr = normal.length2();
 	btAssert(btFabs(normalLenSqr) < btScalar(1.1));
 	if (normalLenSqr > btScalar(1.1))
@@ -133,45 +116,38 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
 		impulse = btScalar(0.);
 		return;
 	}
-	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
 	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
 	//this jacobian entry could be re-used for all iterations
-	
+
 	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
 	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
 	btVector3 vel = vel1 - vel2;
-	
 
-	   btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
-		body2.getCenterOfMassTransform().getBasis().transpose(),
-		rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
-		body2.getInvInertiaDiagLocal(),body2.getInvMass());
+	btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
+						body2.getCenterOfMassTransform().getBasis().transpose(),
+						rel_pos1, rel_pos2, normal, body1.getInvInertiaDiagLocal(), body1.getInvMass(),
+						body2.getInvInertiaDiagLocal(), body2.getInvMass());
 
 	btScalar jacDiagAB = jac.getDiagonal();
 	btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
-	
-	  btScalar rel_vel = jac.getRelativeVelocity(
+
+	btScalar rel_vel = jac.getRelativeVelocity(
 		body1.getLinearVelocity(),
 		body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
 		body2.getLinearVelocity(),
-		body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
-
-
+		body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
 
 	rel_vel = normal.dot(vel);
-	
+
 	//todo: move this into proper structure
 	btScalar contactDamping = btScalar(0.2);
 
 #ifdef ONLY_USE_LINEAR_MASS
 	btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
-	impulse = - contactDamping * rel_vel * massTerm;
-#else	
+	impulse = -contactDamping * rel_vel * massTerm;
+#else
 	btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
 	impulse = velocityImpulse;
 #endif
 }
-
-
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
index 477c79d1756..255489be991 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
@@ -22,18 +22,17 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 
 ///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface
-ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class)
+btContactConstraint : public btTypedConstraint
 {
 protected:
-
 	btPersistentManifold m_contactManifold;
 
-public:
-
-
-	btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);
+protected:
+	btContactConstraint(btPersistentManifold * contactManifold, btRigidBody & rbA, btRigidBody & rbB);
 
-	void	setContactManifold(btPersistentManifold* contactManifold);
+public:
+	void setContactManifold(btPersistentManifold * contactManifold);
 
 	btPersistentManifold* getContactManifold()
 	{
@@ -47,25 +46,20 @@ public:
 
 	virtual ~btContactConstraint();
 
-	virtual void getInfo1 (btConstraintInfo1* info);
+	virtual void getInfo1(btConstraintInfo1 * info);
 
-	virtual void getInfo2 (btConstraintInfo2* info);
+	virtual void getInfo2(btConstraintInfo2 * info);
 
 	///obsolete methods
-	virtual void	buildJacobian();
-
-
+	virtual void buildJacobian();
 };
 
 ///very basic collision resolution without friction
-btScalar resolveSingleCollision(btRigidBody* body1, class btCollisionObject* colObj2, const btVector3& contactPositionWorld,const btVector3& contactNormalOnB, const struct btContactSolverInfo& solverInfo,btScalar distance);
-
+btScalar resolveSingleCollision(btRigidBody* body1, class btCollisionObject* colObj2, const btVector3& contactPositionWorld, const btVector3& contactNormalOnB, const struct btContactSolverInfo& solverInfo, btScalar distance);
 
 ///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects
 void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
-                      btRigidBody& body2, const btVector3& pos2,
-                      btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep);
-
-
+							btRigidBody& body2, const btVector3& pos2,
+							btScalar distance, const btVector3& normal, btScalar& impulse, btScalar timeStep);
 
-#endif //BT_CONTACT_CONSTRAINT_H
+#endif  //BT_CONTACT_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
index a3a0fa6729f..3316403a873 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
@@ -18,7 +18,7 @@ subject to the following restrictions:
 
 #include "LinearMath/btScalar.h"
 
-enum	btSolverMode
+enum btSolverMode
 {
 	SOLVER_RANDMIZE_ORDER = 1,
 	SOLVER_FRICTION_SEPARATE = 2,
@@ -29,131 +29,149 @@ enum	btSolverMode
 	SOLVER_CACHE_FRIENDLY = 128,
 	SOLVER_SIMD = 256,
 	SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
-	SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
+	SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024,
+	SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048,
+	SOLVER_USE_ARTICULATED_WARMSTARTING = 4096,
 };
 
 struct btContactSolverInfoData
 {
-	
-
-	btScalar	m_tau;
-	btScalar	m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
-	btScalar	m_friction;
-	btScalar	m_timeStep;
-	btScalar	m_restitution;
-	int		m_numIterations;
-	btScalar	m_maxErrorReduction;
-	btScalar	m_sor;
-	btScalar	m_erp;//used as Baumgarte factor
-	btScalar	m_erp2;//used in Split Impulse
-	btScalar	m_globalCfm;//constraint force mixing
-	int			m_splitImpulse;
-	btScalar	m_splitImpulsePenetrationThreshold;
-	btScalar	m_splitImpulseTurnErp;
-	btScalar	m_linearSlop;
-	btScalar	m_warmstartingFactor;
-
-	int			m_solverMode;
-	int	m_restingContactRestitutionThreshold;
-	int			m_minimumSolverBatchSize;
-	btScalar	m_maxGyroscopicForce;
-	btScalar	m_singleAxisRollingFrictionThreshold;
-
-
+	btScalar m_tau;
+	btScalar m_damping;  //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	btScalar m_friction;
+	btScalar m_timeStep;
+	btScalar m_restitution;
+	int m_numIterations;
+	btScalar m_maxErrorReduction;
+	btScalar m_sor;          //successive over-relaxation term
+	btScalar m_erp;          //error reduction for non-contact constraints
+	btScalar m_erp2;         //error reduction for contact constraints
+	btScalar m_deformable_erp;          //error reduction for deformable constraints
+	btScalar m_deformable_cfm;          //constraint force mixing for deformable constraints
+	btScalar m_deformable_maxErrorReduction; // maxErrorReduction for deformable contact
+	btScalar m_globalCfm;    //constraint force mixing for contacts and non-contacts
+	btScalar m_frictionERP;  //error reduction for friction constraints
+	btScalar m_frictionCFM;  //constraint force mixing for friction constraints
+
+	int m_splitImpulse;
+	btScalar m_splitImpulsePenetrationThreshold;
+	btScalar m_splitImpulseTurnErp;
+	btScalar m_linearSlop;
+	btScalar m_warmstartingFactor;
+	btScalar m_articulatedWarmstartingFactor;
+	int m_solverMode;
+	int m_restingContactRestitutionThreshold;
+	int m_minimumSolverBatchSize;
+	btScalar m_maxGyroscopicForce;
+	btScalar m_singleAxisRollingFrictionThreshold;
+	btScalar m_leastSquaresResidualThreshold;
+	btScalar m_restitutionVelocityThreshold;
+	bool m_jointFeedbackInWorldSpace;
+	bool m_jointFeedbackInJointFrame;
+	int m_reportSolverAnalytics;
+	int m_numNonContactInnerIterations;
 };
 
 struct btContactSolverInfo : public btContactSolverInfoData
 {
-
-	
-
 	inline btContactSolverInfo()
 	{
 		m_tau = btScalar(0.6);
 		m_damping = btScalar(1.0);
 		m_friction = btScalar(0.3);
-		m_timeStep = btScalar(1.f/60.f);
+		m_timeStep = btScalar(1.f / 60.f);
 		m_restitution = btScalar(0.);
 		m_maxErrorReduction = btScalar(20.);
 		m_numIterations = 10;
 		m_erp = btScalar(0.2);
-		m_erp2 = btScalar(0.8);
+		m_erp2 = btScalar(0.2);
+		m_deformable_erp = btScalar(0.06);
+		m_deformable_cfm = btScalar(0.01);
+		m_deformable_maxErrorReduction = btScalar(0.1);
 		m_globalCfm = btScalar(0.);
+		m_frictionERP = btScalar(0.2);  //positional friction 'anchors' are disabled by default
+		m_frictionCFM = btScalar(0.);
 		m_sor = btScalar(1.);
 		m_splitImpulse = true;
 		m_splitImpulsePenetrationThreshold = -.04f;
 		m_splitImpulseTurnErp = 0.1f;
 		m_linearSlop = btScalar(0.0);
-		m_warmstartingFactor=btScalar(0.85);
+		m_warmstartingFactor = btScalar(0.85);
+		m_articulatedWarmstartingFactor = btScalar(0.85);
 		//m_solverMode =  SOLVER_USE_WARMSTARTING |  SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER;
-		m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
-		m_restingContactRestitutionThreshold = 2;//unused as of 2.81
-		m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
-		m_maxGyroscopicForce = 100.f; ///it is only used for 'explicit' version of gyroscopic force
-		m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
+		m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;  // | SOLVER_RANDMIZE_ORDER;
+		m_restingContactRestitutionThreshold = 2;              //unused as of 2.81
+		m_minimumSolverBatchSize = 128;                        //try to combine islands until the amount of constraints reaches this limit
+		m_maxGyroscopicForce = 100.f;                          ///it is only used for 'explicit' version of gyroscopic force
+		m_singleAxisRollingFrictionThreshold = 1e30f;          ///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
+		m_leastSquaresResidualThreshold = 0.f;
+		m_restitutionVelocityThreshold = 0.2f;  //if the relative velocity is below this threshold, there is zero restitution
+		m_jointFeedbackInWorldSpace = false;
+		m_jointFeedbackInJointFrame = false;
+		m_reportSolverAnalytics = 0;
+		m_numNonContactInnerIterations = 1;   // the number of inner iterations for solving motor constraint in a single iteration of the constraint solve
 	}
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct btContactSolverInfoDoubleData
 {
-	double		m_tau;
-	double		m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
-	double		m_friction;
-	double		m_timeStep;
-	double		m_restitution;
-	double		m_maxErrorReduction;
-	double		m_sor;
-	double		m_erp;//used as Baumgarte factor
-	double		m_erp2;//used in Split Impulse
-	double		m_globalCfm;//constraint force mixing
-	double		m_splitImpulsePenetrationThreshold;
-	double		m_splitImpulseTurnErp;
-	double		m_linearSlop;
-	double		m_warmstartingFactor;
-	double		m_maxGyroscopicForce;///it is only used for 'explicit' version of gyroscopic force
-	double		m_singleAxisRollingFrictionThreshold;
-
-	int			m_numIterations;
-	int			m_solverMode;
-	int			m_restingContactRestitutionThreshold;
-	int			m_minimumSolverBatchSize;
-	int			m_splitImpulse;
-	char		m_padding[4];
-
+	double m_tau;
+	double m_damping;  //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	double m_friction;
+	double m_timeStep;
+	double m_restitution;
+	double m_maxErrorReduction;
+	double m_sor;
+	double m_erp;        //used as Baumgarte factor
+	double m_erp2;       //used in Split Impulse
+	double m_globalCfm;  //constraint force mixing
+	double m_splitImpulsePenetrationThreshold;
+	double m_splitImpulseTurnErp;
+	double m_linearSlop;
+	double m_warmstartingFactor;
+	double m_articulatedWarmstartingFactor;
+	double m_maxGyroscopicForce;  ///it is only used for 'explicit' version of gyroscopic force
+	double m_singleAxisRollingFrictionThreshold;
+
+	int m_numIterations;
+	int m_solverMode;
+	int m_restingContactRestitutionThreshold;
+	int m_minimumSolverBatchSize;
+	int m_splitImpulse;
+	char m_padding[4];
 };
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct btContactSolverInfoFloatData
 {
-	float		m_tau;
-	float		m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
-	float		m_friction;
-	float		m_timeStep;
-
-	float		m_restitution;
-	float		m_maxErrorReduction;
-	float		m_sor;
-	float		m_erp;//used as Baumgarte factor
-
-	float		m_erp2;//used in Split Impulse
-	float		m_globalCfm;//constraint force mixing
-	float		m_splitImpulsePenetrationThreshold;
-	float		m_splitImpulseTurnErp;
-
-	float		m_linearSlop;
-	float		m_warmstartingFactor;
-	float		m_maxGyroscopicForce;
-	float		m_singleAxisRollingFrictionThreshold;
-
-	int			m_numIterations;
-	int			m_solverMode;
-	int			m_restingContactRestitutionThreshold;
-	int			m_minimumSolverBatchSize;
-
-	int			m_splitImpulse;
-	char		m_padding[4];
+	float m_tau;
+	float m_damping;  //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	float m_friction;
+	float m_timeStep;
+
+	float m_restitution;
+	float m_maxErrorReduction;
+	float m_sor;
+	float m_erp;  //used as Baumgarte factor
+
+	float m_erp2;       //used in Split Impulse
+	float m_globalCfm;  //constraint force mixing
+	float m_splitImpulsePenetrationThreshold;
+	float m_splitImpulseTurnErp;
+
+	float m_linearSlop;
+	float m_warmstartingFactor;
+	float m_articulatedWarmstartingFactor;
+	float m_maxGyroscopicForce;
+
+	float m_singleAxisRollingFrictionThreshold;
+	int m_numIterations;
+	int m_solverMode;
+	int m_restingContactRestitutionThreshold;
+
+	int m_minimumSolverBatchSize;
+	int m_splitImpulse;
+	
 };
 
-
-
-#endif //BT_CONTACT_SOLVER_INFO
+#endif  //BT_CONTACT_SOLVER_INFO
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
index 75d81cc08c2..bba102d9052 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
@@ -13,25 +13,20 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btFixedConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
 #include <new>
 
-
-btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
-:btGeneric6DofSpring2Constraint(rbA,rbB,frameInA,frameInB)
+btFixedConstraint::btFixedConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
+	: btGeneric6DofSpring2Constraint(rbA, rbB, frameInA, frameInB)
 {
-	setAngularLowerLimit(btVector3(0,0,0));
-	setAngularUpperLimit(btVector3(0,0,0));
-	setLinearLowerLimit(btVector3(0,0,0));
-	setLinearUpperLimit(btVector3(0,0,0));
+	setAngularLowerLimit(btVector3(0, 0, 0));
+	setAngularUpperLimit(btVector3(0, 0, 0));
+	setLinearLowerLimit(btVector3(0, 0, 0));
+	setLinearUpperLimit(btVector3(0, 0, 0));
 }
 
-
-
-
-btFixedConstraint::~btFixedConstraint ()
+btFixedConstraint::~btFixedConstraint()
 {
 }
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
index bff2008b283..6d474ea81de 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
@@ -18,16 +18,13 @@ subject to the following restrictions:
 
 #include "btGeneric6DofSpring2Constraint.h"
 
-
-ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btGeneric6DofSpring2Constraint
+ATTRIBUTE_ALIGNED16(class)
+btFixedConstraint : public btGeneric6DofSpring2Constraint
 {
-
 public:
-	btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
+	btFixedConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB);
 
-	
 	virtual ~btFixedConstraint();
-
 };
 
-#endif //BT_FIXED_CONSTRAINT_H
+#endif  //BT_FIXED_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp
index bcd457b6731..7535c52c05f 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp
@@ -17,38 +17,36 @@ subject to the following restrictions:
 
 #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(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 ()
+btGearConstraint::~btGearConstraint()
 {
 }
 
-void btGearConstraint::getInfo1 (btConstraintInfo1* info)
+void btGearConstraint::getInfo1(btConstraintInfo1* info)
 {
 	info->m_numConstraintRows = 1;
 	info->nub = 1;
 }
 
-void btGearConstraint::getInfo2 (btConstraintInfo2* info)
+void btGearConstraint::getInfo2(btConstraintInfo2* info)
 {
 	btVector3 globalAxisA, globalAxisB;
 
-	globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA;
-	globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB;
+	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];
-
+	info->m_J2angularAxis[0] = m_ratio * globalAxisB[0];
+	info->m_J2angularAxis[1] = m_ratio * globalAxisB[1];
+	info->m_J2angularAxis[2] = m_ratio * globalAxisB[2];
 }
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h
index f9afcb91211..64b15dfbce8 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h
@@ -13,45 +13,40 @@ subject to the following restrictions:
 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"
+#define btGearConstraintData btGearConstraintDoubleData
+#define btGearConstraintDataName "btGearConstraintDoubleData"
 #else
-#define btGearConstraintData	btGearConstraintFloatData
-#define btGearConstraintDataName	"btGearConstraintFloatData"
-#endif //BT_USE_DOUBLE_PRECISION
-
-
+#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;
+	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 ();
+	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);
+	virtual void getInfo1(btConstraintInfo1* info);
 
 	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo2 (btConstraintInfo2* info);
+	virtual void getInfo2(btConstraintInfo2* info);
 
-	void setAxisA(btVector3& axisA) 
+	void setAxisA(btVector3& axisA)
 	{
 		m_axisInA = axisA;
 	}
@@ -76,77 +71,76 @@ public:
 		return m_ratio;
 	}
 
-
-	virtual	void	setParam(int num, btScalar value, int axis = -1) 
+	virtual void setParam(int num, btScalar value, int axis = -1)
 	{
-		(void) num;
-		(void) value;
-		(void) axis;
+		(void)num;
+		(void)value;
+		(void)axis;
 		btAssert(0);
 	}
 
 	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const 
-	{ 
-		(void) num;
-		(void) axis;
+	virtual btScalar getParam(int num, int axis = -1) const
+	{
+		(void)num;
+		(void)axis;
 		btAssert(0);
 		return 0.f;
 	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
+	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;
+	btTypedConstraintFloatData m_typeConstraintData;
 
-	btVector3FloatData			m_axisInA;
-	btVector3FloatData			m_axisInB;
+	btVector3FloatData m_axisInA;
+	btVector3FloatData m_axisInB;
 
-	float							m_ratio;
-	char							m_padding[4];
+	float m_ratio;
+	char m_padding[4];
 };
 
 struct btGearConstraintDoubleData
 {
-	btTypedConstraintDoubleData	m_typeConstraintData;
+	btTypedConstraintDoubleData m_typeConstraintData;
 
-	btVector3DoubleData			m_axisInA;
-	btVector3DoubleData			m_axisInB;
+	btVector3DoubleData m_axisInA;
+	btVector3DoubleData m_axisInB;
 
-	double						m_ratio;
+	double m_ratio;
 };
 
-SIMD_FORCE_INLINE	int	btGearConstraint::calculateSerializeBufferSize() const
+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
+///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);
+	btTypedConstraint::serialize(&gear->m_typeConstraintData, serializer);
 
-	m_axisInA.serialize( gear->m_axisInA );
-	m_axisInB.serialize( gear->m_axisInB );
+	m_axisInA.serialize(gear->m_axisInA);
+	m_axisInB.serialize(gear->m_axisInB);
 
 	gear->m_ratio = m_ratio;
 
+	// Fill padding with zeros to appease msan.
+#ifndef BT_USE_DOUBLE_PRECISION
+	gear->m_padding[0] = 0;
+	gear->m_padding[1] = 0;
+	gear->m_padding[2] = 0;
+	gear->m_padding[3] = 0;
+#endif
+
 	return btGearConstraintDataName;
 }
 
-
-
-
-
-
-#endif //BT_GEAR_CONSTRAINT_H
+#endif  //BT_GEAR_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
index bc2b5a85df9..1f542035324 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
@@ -25,83 +25,61 @@ http://gimpact.sf.net
 #include "LinearMath/btTransformUtil.h"
 #include <new>
 
-
-
 #define D6_USE_OBSOLETE_METHOD false
 #define D6_USE_FRAME_OFFSET true
 
-
-
-
-
-
 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
-: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
-, m_frameInA(frameInA)
-, m_frameInB(frameInB),
-m_useLinearReferenceFrameA(useLinearReferenceFrameA),
-m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
-m_flags(0),
-m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
+	: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0), m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
 {
 	calculateTransforms();
 }
 
-
-
 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
-        : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
-		m_frameInB(frameInB),
-		m_useLinearReferenceFrameA(useLinearReferenceFrameB),
-		m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
-		m_flags(0),
-		m_useSolveConstraintObsolete(false)
+	: btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
+	  m_frameInB(frameInB),
+	  m_useLinearReferenceFrameA(useLinearReferenceFrameB),
+	  m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
+	  m_flags(0),
+	  m_useSolveConstraintObsolete(false)
 {
 	///not providing rigidbody A means implicitly using worldspace for body A
 	m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
 	calculateTransforms();
 }
 
-
-
-
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
 
-
-
 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
 {
-	int i = index%3;
-	int j = index/3;
+	int i = index % 3;
+	int j = index / 3;
 	return mat[i][j];
 }
 
-
-
 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
-bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
-bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
+bool matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz);
+bool 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);
+	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));
+			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[0] = -btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
 			xyz[1] = -SIMD_HALF_PI;
 			xyz[2] = btScalar(0.0);
 			return false;
@@ -110,7 +88,7 @@ bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
 	else
 	{
 		// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
-		xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+		xyz[0] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
 		xyz[1] = SIMD_HALF_PI;
 		xyz[2] = 0.0;
 	}
@@ -121,52 +99,49 @@ bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
 
 int btRotationalLimitMotor::testLimitValue(btScalar test_value)
 {
-	if(m_loLimit>m_hiLimit)
+	if (m_loLimit > m_hiLimit)
 	{
-		m_currentLimit = 0;//Free from violation
+		m_currentLimit = 0;  //Free from violation
 		return 0;
 	}
 	if (test_value < m_loLimit)
 	{
-		m_currentLimit = 1;//low limit violation
-		m_currentLimitError =  test_value - m_loLimit;
-		if(m_currentLimitError>SIMD_PI) 
-			m_currentLimitError-=SIMD_2_PI;
-		else if(m_currentLimitError<-SIMD_PI) 
-			m_currentLimitError+=SIMD_2_PI;
+		m_currentLimit = 1;  //low limit violation
+		m_currentLimitError = test_value - m_loLimit;
+		if (m_currentLimitError > SIMD_PI)
+			m_currentLimitError -= SIMD_2_PI;
+		else if (m_currentLimitError < -SIMD_PI)
+			m_currentLimitError += SIMD_2_PI;
 		return 1;
 	}
-	else if (test_value> m_hiLimit)
+	else if (test_value > m_hiLimit)
 	{
-		m_currentLimit = 2;//High limit violation
+		m_currentLimit = 2;  //High limit violation
 		m_currentLimitError = test_value - m_hiLimit;
-		if(m_currentLimitError>SIMD_PI) 
-			m_currentLimitError-=SIMD_2_PI;
-		else if(m_currentLimitError<-SIMD_PI) 
-			m_currentLimitError+=SIMD_2_PI;
+		if (m_currentLimitError > SIMD_PI)
+			m_currentLimitError -= SIMD_2_PI;
+		else if (m_currentLimitError < -SIMD_PI)
+			m_currentLimitError += SIMD_2_PI;
 		return 2;
 	};
 
-	m_currentLimit = 0;//Free from violation
+	m_currentLimit = 0;  //Free from violation
 	return 0;
-
 }
 
-
-
 btScalar btRotationalLimitMotor::solveAngularLimits(
-	btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
-	btRigidBody * body0, btRigidBody * body1 )
+	btScalar timeStep, btVector3& axis, btScalar jacDiagABInv,
+	btRigidBody* body0, btRigidBody* body1)
 {
-	if (needApplyTorques()==false) return 0.0f;
+	if (needApplyTorques() == false) return 0.0f;
 
 	btScalar target_velocity = m_targetVelocity;
 	btScalar maxMotorForce = m_maxMotorForce;
 
 	//current error correction
-	if (m_currentLimit!=0)
+	if (m_currentLimit != 0)
 	{
-		target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
+		target_velocity = -m_stopERP * m_currentLimitError / (timeStep);
 		maxMotorForce = m_maxLimitForce;
 	}
 
@@ -178,42 +153,37 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
 	btVector3 angVelB = body1->getAngularVelocity();
 
 	btVector3 vel_diff;
-	vel_diff = angVelA-angVelB;
-
-
+	vel_diff = angVelA - angVelB;
 
 	btScalar rel_vel = axis.dot(vel_diff);
 
 	// correction velocity
-	btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
+	btScalar motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel);
 
-
-	if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
+	if (motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON)
 	{
-		return 0.0f;//no need for applying force
+		return 0.0f;  //no need for applying force
 	}
 
-
 	// correction impulse
-	btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
+	btScalar unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv;
 
 	// clip correction impulse
 	btScalar clippedMotorImpulse;
 
 	///@todo: should clip against accumulated impulse
-	if (unclippedMotorImpulse>0.0f)
+	if (unclippedMotorImpulse > 0.0f)
 	{
-		clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
+		clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
 	}
 	else
 	{
-		clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
+		clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
 	}
 
-
 	// sort with accumulated impulses
-	btScalar	lo = btScalar(-BT_LARGE_FLOAT);
-	btScalar	hi = btScalar(BT_LARGE_FLOAT);
+	btScalar lo = btScalar(-BT_LARGE_FLOAT);
+	btScalar hi = btScalar(BT_LARGE_FLOAT);
 
 	btScalar oldaccumImpulse = m_accumulatedImpulse;
 	btScalar sum = oldaccumImpulse + clippedMotorImpulse;
@@ -227,59 +197,50 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
 	body1->applyTorqueImpulse(-motorImp);
 
 	return clippedMotorImpulse;
-
-
 }
 
 //////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
 
-
-
-
 //////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
 
-
 int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
 {
 	btScalar loLimit = m_lowerLimit[limitIndex];
 	btScalar hiLimit = m_upperLimit[limitIndex];
-	if(loLimit > hiLimit)
+	if (loLimit > hiLimit)
 	{
-		m_currentLimit[limitIndex] = 0;//Free from violation
+		m_currentLimit[limitIndex] = 0;  //Free from violation
 		m_currentLimitError[limitIndex] = btScalar(0.f);
 		return 0;
 	}
 
 	if (test_value < loLimit)
 	{
-		m_currentLimit[limitIndex] = 2;//low limit violation
-		m_currentLimitError[limitIndex] =  test_value - loLimit;
+		m_currentLimit[limitIndex] = 2;  //low limit violation
+		m_currentLimitError[limitIndex] = test_value - loLimit;
 		return 2;
 	}
-	else if (test_value> hiLimit)
+	else if (test_value > hiLimit)
 	{
-		m_currentLimit[limitIndex] = 1;//High limit violation
+		m_currentLimit[limitIndex] = 1;  //High limit violation
 		m_currentLimitError[limitIndex] = test_value - hiLimit;
 		return 1;
 	};
 
-	m_currentLimit[limitIndex] = 0;//Free from violation
+	m_currentLimit[limitIndex] = 0;  //Free from violation
 	m_currentLimitError[limitIndex] = btScalar(0.f);
 	return 0;
 }
 
-
-
 btScalar btTranslationalLimitMotor::solveLinearAxis(
 	btScalar timeStep,
 	btScalar jacDiagABInv,
-	btRigidBody& body1,const btVector3 &pointInA,
-	btRigidBody& body2,const btVector3 &pointInB,
+	btRigidBody& body1, const btVector3& pointInA,
+	btRigidBody& body2, const btVector3& pointInB,
 	int limit_index,
-	const btVector3 & axis_normal_on_a,
-	const btVector3 & anchorPos)
+	const btVector3& axis_normal_on_a,
+	const btVector3& anchorPos)
 {
-
 	///find relative velocity
 	//    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
 	//    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
@@ -292,14 +253,12 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
 
 	btScalar rel_vel = axis_normal_on_a.dot(vel);
 
-
-
 	/// apply displacement correction
 
 	//positional error (zeroth order error)
 	btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
-	btScalar	lo = btScalar(-BT_LARGE_FLOAT);
-	btScalar	hi = btScalar(BT_LARGE_FLOAT);
+	btScalar lo = btScalar(-BT_LARGE_FLOAT);
+	btScalar hi = btScalar(BT_LARGE_FLOAT);
 
 	btScalar minLimit = m_lowerLimit[limit_index];
 	btScalar maxLimit = m_upperLimit[limit_index];
@@ -312,7 +271,6 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
 			{
 				depth -= maxLimit;
 				lo = btScalar(0.);
-
 			}
 			else
 			{
@@ -329,10 +287,7 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
 		}
 	}
 
-	btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
-
-
-
+	btScalar normalImpulse = m_limitSoftness * (m_restitution * depth / timeStep - m_damping * rel_vel) * jacDiagABInv;
 
 	btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
 	btScalar sum = oldNormalImpulse + normalImpulse;
@@ -340,11 +295,9 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
 	normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
 
 	btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
-	body1.applyImpulse( impulse_vector, rel_pos1);
+	body1.applyImpulse(impulse_vector, rel_pos1);
 	body2.applyImpulse(-impulse_vector, rel_pos2);
 
-	
-
 	return normalImpulse;
 }
 
@@ -352,8 +305,8 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
 
 void btGeneric6DofConstraint::calculateAngleInfo()
 {
-	btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
-	matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
+	btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
+	matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff);
 	// 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]) :
 	//
@@ -378,31 +331,30 @@ void btGeneric6DofConstraint::calculateAngleInfo()
 	m_calculatedAxis[0].normalize();
 	m_calculatedAxis[1].normalize();
 	m_calculatedAxis[2].normalize();
-
 }
 
 void btGeneric6DofConstraint::calculateTransforms()
 {
-	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 }
 
-void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA, const btTransform& transB)
 {
 	m_calculatedTransformA = transA * m_frameInA;
 	m_calculatedTransformB = transB * m_frameInB;
 	calculateLinearInfo();
 	calculateAngleInfo();
-	if(m_useOffsetForConstraintFrame)
-	{	//  get weight factors depending on masses
+	if (m_useOffsetForConstraintFrame)
+	{  //  get weight factors depending on masses
 		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))
+		if (miS > btScalar(0.f))
 		{
 			m_factA = miB / miS;
 		}
-		else 
+		else
 		{
 			m_factA = btScalar(0.5f);
 		}
@@ -410,39 +362,32 @@ void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,cons
 	}
 }
 
-
-
 void btGeneric6DofConstraint::buildLinearJacobian(
-	btJacobianEntry & jacLinear,const btVector3 & normalWorld,
-	const btVector3 & pivotAInW,const btVector3 & pivotBInW)
+	btJacobianEntry& jacLinear, const btVector3& normalWorld,
+	const btVector3& pivotAInW, const btVector3& pivotBInW)
 {
 	new (&jacLinear) btJacobianEntry(
-        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-        pivotAInW - m_rbA.getCenterOfMassPosition(),
-        pivotBInW - m_rbB.getCenterOfMassPosition(),
-        normalWorld,
-        m_rbA.getInvInertiaDiagLocal(),
-        m_rbA.getInvMass(),
-        m_rbB.getInvInertiaDiagLocal(),
-        m_rbB.getInvMass());
+		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+		pivotAInW - m_rbA.getCenterOfMassPosition(),
+		pivotBInW - m_rbB.getCenterOfMassPosition(),
+		normalWorld,
+		m_rbA.getInvInertiaDiagLocal(),
+		m_rbA.getInvMass(),
+		m_rbB.getInvInertiaDiagLocal(),
+		m_rbB.getInvMass());
 }
 
-
-
 void btGeneric6DofConstraint::buildAngularJacobian(
-	btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
+	btJacobianEntry& jacAngular, const btVector3& jointAxisW)
 {
-	 new (&jacAngular)	btJacobianEntry(jointAxisW,
-                                      m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-                                      m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-                                      m_rbA.getInvInertiaDiagLocal(),
-                                      m_rbB.getInvInertiaDiagLocal());
-
+	new (&jacAngular) btJacobianEntry(jointAxisW,
+									  m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+									  m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+									  m_rbA.getInvInertiaDiagLocal(),
+									  m_rbB.getInvInertiaDiagLocal());
 }
 
-
-
 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 {
 	btScalar angle = m_calculatedAxisAngleDiff[axis_index];
@@ -453,23 +398,20 @@ bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 	return m_angularLimits[axis_index].needApplyTorques();
 }
 
-
-
 void btGeneric6DofConstraint::buildJacobian()
 {
 #ifndef __SPU__
 	if (m_useSolveConstraintObsolete)
 	{
-
 		// Clear accumulated impulses for the next simulation step
 		m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 		int i;
-		for(i = 0; i < 3; i++)
+		for (i = 0; i < 3; i++)
 		{
 			m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
 		}
 		//calculates transform
-		calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+		calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 
 		//  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
 		//  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
@@ -483,7 +425,7 @@ void btGeneric6DofConstraint::buildJacobian()
 
 		btVector3 normalWorld;
 		//linear part
-		for (i=0;i<3;i++)
+		for (i = 0; i < 3; i++)
 		{
 			if (m_linearLimits.isLimited(i))
 			{
@@ -493,56 +435,53 @@ void btGeneric6DofConstraint::buildJacobian()
 					normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
 
 				buildLinearJacobian(
-					m_jacLinear[i],normalWorld ,
-					pivotAInW,pivotBInW);
-
+					m_jacLinear[i], normalWorld,
+					pivotAInW, pivotBInW);
 			}
 		}
 
 		// angular part
-		for (i=0;i<3;i++)
+		for (i = 0; i < 3; i++)
 		{
 			//calculates error angle
 			if (testAngularLimitMotor(i))
 			{
 				normalWorld = this->getAxis(i);
 				// Create angular atom
-				buildAngularJacobian(m_jacAng[i],normalWorld);
+				buildAngularJacobian(m_jacAng[i], normalWorld);
 			}
 		}
-
 	}
-#endif //__SPU__
-
+#endif  //__SPU__
 }
 
-
-void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
+void btGeneric6DofConstraint::getInfo1(btConstraintInfo1* info)
 {
 	if (m_useSolveConstraintObsolete)
 	{
 		info->m_numConstraintRows = 0;
 		info->nub = 0;
-	} else
+	}
+	else
 	{
 		//prepare constraint
-		calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+		calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 		info->m_numConstraintRows = 0;
 		info->nub = 6;
 		int i;
 		//test linear limits
-		for(i = 0; i < 3; i++)
+		for (i = 0; i < 3; i++)
 		{
-			if(m_linearLimits.needApplyForce(i))
+			if (m_linearLimits.needApplyForce(i))
 			{
 				info->m_numConstraintRows++;
 				info->nub--;
 			}
 		}
 		//test angular limits
-		for (i=0;i<3 ;i++ )
+		for (i = 0; i < 3; i++)
 		{
-			if(testAngularLimitMotor(i))
+			if (testAngularLimitMotor(i))
 			{
 				info->m_numConstraintRows++;
 				info->nub--;
@@ -551,13 +490,14 @@ void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
 	}
 }
 
-void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+void btGeneric6DofConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
 {
 	if (m_useSolveConstraintObsolete)
 	{
 		info->m_numConstraintRows = 0;
 		info->nub = 0;
-	} else
+	}
+	else
 	{
 		//pre-allocate all 6
 		info->m_numConstraintRows = 6;
@@ -565,8 +505,7 @@ void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
 	}
 }
 
-
-void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
+void btGeneric6DofConstraint::getInfo2(btConstraintInfo2* info)
 {
 	btAssert(!m_useSolveConstraintObsolete);
 
@@ -577,136 +516,124 @@ void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
 	const btVector3& angVelA = m_rbA.getAngularVelocity();
 	const btVector3& angVelB = m_rbB.getAngularVelocity();
 
-	if(m_useOffsetForConstraintFrame)
-	{ // 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);
+	if (m_useOffsetForConstraintFrame)
+	{  // 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);
 	}
 	else
-	{ // leave old version for compatibility
-		int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
-		setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+	{  // leave old version for compatibility
+		int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 	}
-
 }
 
-
-void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+void btGeneric6DofConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
 {
-	
 	btAssert(!m_useSolveConstraintObsolete);
 	//prepare constraint
-	calculateTransforms(transA,transB);
+	calculateTransforms(transA, transB);
 
 	int i;
-	for (i=0;i<3 ;i++ )
+	for (i = 0; i < 3; i++)
 	{
 		testAngularLimitMotor(i);
 	}
 
-	if(m_useOffsetForConstraintFrame)
-	{ // 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);
+	if (m_useOffsetForConstraintFrame)
+	{  // 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);
 	}
 	else
-	{ // leave old version for compatibility
-		int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
-		setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+	{  // leave old version for compatibility
+		int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 	}
 }
 
-
-
-int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
 {
-//	int row = 0;
+	//	int row = 0;
 	//solve linear limits
 	btRotationalLimitMotor limot;
-	for (int i=0;i<3 ;i++ )
+	for (int i = 0; i < 3; i++)
 	{
-		if(m_linearLimits.needApplyForce(i))
-		{ // re-use rotational motor code
+		if (m_linearLimits.needApplyForce(i))
+		{  // re-use rotational motor code
 			limot.m_bounce = btScalar(0.f);
 			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_damping  = m_linearLimits.m_damping;
-			limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
-			limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
-			limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
-			limot.m_loLimit  = m_linearLimits.m_lowerLimit[i];
-			limot.m_maxLimitForce  = btScalar(0.f);
-			limot.m_maxMotorForce  = m_linearLimits.m_maxMotorForce[i];
-			limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
+			limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
+			limot.m_damping = m_linearLimits.m_damping;
+			limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
+			limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
+			limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
+			limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
+			limot.m_maxLimitForce = btScalar(0.f);
+			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_SHIFT);
-			limot.m_normalCFM	= (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
-			limot.m_stopCFM		= (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
-			limot.m_stopERP		= (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
-			if(m_useOffsetForConstraintFrame)
+			limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
+			limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
+			limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
+			if (m_useOffsetForConstraintFrame)
 			{
 				int indx1 = (i + 1) % 3;
 				int indx2 = (i + 2) % 3;
-				int rotAllowed = 1; // rotations around orthos to current axis
-				if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
+				int rotAllowed = 1;  // rotations around orthos to current axis
+				if (m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
 				{
 					rotAllowed = 0;
 				}
-				row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
+				row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
 			}
 			else
 			{
-				row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
+				row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0);
 			}
 		}
 	}
 	return row;
 }
 
-
-
-int btGeneric6DofConstraint::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 btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2* info, int row_offset, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
 {
-	btGeneric6DofConstraint * d6constraint = this;
+	btGeneric6DofConstraint* d6constraint = this;
 	int row = row_offset;
 	//solve angular limits
-	for (int i=0;i<3 ;i++ )
+	for (int i = 0; i < 3; i++)
 	{
-		if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
+		if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
 		{
 			btVector3 axis = d6constraint->getAxis(i);
 			int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
-			if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
+			if (!(flags & BT_6DOF_FLAGS_CFM_NORM))
 			{
 				m_angularLimits[i].m_normalCFM = info->cfm[0];
 			}
-			if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
+			if (!(flags & BT_6DOF_FLAGS_CFM_STOP))
 			{
 				m_angularLimits[i].m_stopCFM = info->cfm[0];
 			}
-			if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
+			if (!(flags & BT_6DOF_FLAGS_ERP_STOP))
 			{
 				m_angularLimits[i].m_stopERP = info->erp;
 			}
 			row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
-												transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
+										 transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
 		}
 	}
 
 	return row;
 }
 
-
-
-
-void	btGeneric6DofConstraint::updateRHS(btScalar	timeStep)
+void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
 {
 	(void)timeStep;
-
 }
 
-
 void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
 {
 	m_frameInA = frameA;
@@ -715,33 +642,27 @@ void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTrans
 	calculateTransforms();
 }
 
-
-
 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
 {
 	return m_calculatedAxis[axis_index];
 }
 
-
-btScalar	btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
+btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
 {
 	return m_calculatedLinearDiff[axisIndex];
 }
 
-
 btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
 {
 	return m_calculatedAxisAngleDiff[axisIndex];
 }
 
-
-
 void btGeneric6DofConstraint::calcAnchorPos(void)
 {
 	btScalar imA = m_rbA.getInvMass();
 	btScalar imB = m_rbB.getInvMass();
 	btScalar weight;
-	if(imB == btScalar(0.0))
+	if (imB == btScalar(0.0))
 	{
 		weight = btScalar(1.0);
 	}
@@ -755,43 +676,39 @@ void btGeneric6DofConstraint::calcAnchorPos(void)
 	return;
 }
 
-
-
 void btGeneric6DofConstraint::calculateLinearInfo()
 {
 	m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
 	m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
-	for(int i = 0; i < 3; i++)
+	for (int i = 0; i < 3; i++)
 	{
 		m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
 		m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
 	}
 }
 
-
-
 int btGeneric6DofConstraint::get_limit_motor_info2(
-	btRotationalLimitMotor * 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 srow = row * info->rowskip;
-    int powered = limot->m_enableMotor;
-    int limit = limot->m_currentLimit;
-    if (powered || limit)
-    {   // if the joint is powered, or has joint limits, add in the extra row
-        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))
-        {
+	btRotationalLimitMotor* 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 srow = row * info->rowskip;
+	bool powered = limot->m_enableMotor;
+	int limit = limot->m_currentLimit;
+	if (powered || limit)
+	{  // if the joint is powered, or has joint limits, add in the extra row
+		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))
+		{
 			if (m_useOffsetForConstraintFrame)
 			{
 				btVector3 tmpA, tmpB, relA, relB;
@@ -814,55 +731,56 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
 				relB = orthoB - totalDist * m_factB;
 				tmpA = relA.cross(ax1);
 				tmpB = relB.cross(ax1);
-				if(m_hasStaticBody && (!rotAllowed))
+				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];
-			} else
+				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];
+			}
+			else
 			{
-				btVector3 ltd;	// Linear Torque Decoupling vector
+				btVector3 ltd;  // Linear Torque Decoupling vector
 				btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
 				ltd = c.cross(ax1);
-				info->m_J1angularAxis[srow+0] = ltd[0];
-				info->m_J1angularAxis[srow+1] = ltd[1];
-				info->m_J1angularAxis[srow+2] = ltd[2];
+				info->m_J1angularAxis[srow + 0] = ltd[0];
+				info->m_J1angularAxis[srow + 1] = ltd[1];
+				info->m_J1angularAxis[srow + 2] = ltd[2];
 
 				c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
 				ltd = -c.cross(ax1);
-				info->m_J2angularAxis[srow+0] = ltd[0];
-				info->m_J2angularAxis[srow+1] = ltd[1];
-				info->m_J2angularAxis[srow+2] = ltd[2];
+				info->m_J2angularAxis[srow + 0] = ltd[0];
+				info->m_J2angularAxis[srow + 1] = ltd[1];
+				info->m_J2angularAxis[srow + 2] = ltd[2];
 			}
-        }
-        // if we're limited low and high simultaneously, the joint motor is
-        // ineffective
-        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
-        info->m_constraintError[srow] = btScalar(0.f);
-        if (powered)
-        {
+		}
+		// if we're limited low and high simultaneously, the joint motor is
+		// ineffective
+		if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
+		info->m_constraintError[srow] = btScalar(0.f);
+		if (powered)
+		{
 			info->cfm[srow] = limot->m_normalCFM;
-            if(!limit)
-            {
+			if (!limit)
+			{
 				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_stopERP);
+				btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
+												   limot->m_loLimit,
+												   limot->m_hiLimit,
+												   tag_vel,
+												   info->fps * limot->m_stopERP);
 				info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
-                info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
-                info->m_upperLimit[srow] = limot->m_maxMotorForce;
-            }
-        }
-        if(limit)
-        {
-            btScalar k = info->fps * limot->m_stopERP;
-			if(!rotational)
+				info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
+				info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
+			}
+		}
+		if (limit)
+		{
+			btScalar k = info->fps * limot->m_stopERP;
+			if (!rotational)
 			{
 				info->m_constraintError[srow] += k * limot->m_currentLimitError;
 			}
@@ -871,116 +789,112 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
 				info->m_constraintError[srow] += -k * limot->m_currentLimitError;
 			}
 			info->cfm[srow] = limot->m_stopCFM;
-            if (limot->m_loLimit == limot->m_hiLimit)
-            {   // limited low and high simultaneously
-                info->m_lowerLimit[srow] = -SIMD_INFINITY;
-                info->m_upperLimit[srow] = SIMD_INFINITY;
-            }
-            else
-            {
-                if (limit == 1)
-                {
-                    info->m_lowerLimit[srow] = 0;
-                    info->m_upperLimit[srow] = SIMD_INFINITY;
-                }
-                else
-                {
-                    info->m_lowerLimit[srow] = -SIMD_INFINITY;
-                    info->m_upperLimit[srow] = 0;
-                }
-                // deal with bounce
-                if (limot->m_bounce > 0)
-                {
-                    // calculate joint velocity
-                    btScalar vel;
-                    if (rotational)
-                    {
-                        vel = angVelA.dot(ax1);
-//make sure that if no body -> angVelB == zero vec
-//                        if (body1)
-                            vel -= angVelB.dot(ax1);
-                    }
-                    else
-                    {
-                        vel = linVelA.dot(ax1);
-//make sure that if no body -> angVelB == zero vec
-//                        if (body1)
-                            vel -= linVelB.dot(ax1);
-                    }
-                    // only apply bounce if the velocity is incoming, and if the
-                    // resulting c[] exceeds what we already have.
-                    if (limit == 1)
-                    {
-                        if (vel < 0)
-                        {
-                            btScalar newc = -limot->m_bounce* vel;
-                            if (newc > info->m_constraintError[srow]) 
+			if (limot->m_loLimit == limot->m_hiLimit)
+			{  // limited low and high simultaneously
+				info->m_lowerLimit[srow] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			}
+			else
+			{
+				if (limit == 1)
+				{
+					info->m_lowerLimit[srow] = 0;
+					info->m_upperLimit[srow] = SIMD_INFINITY;
+				}
+				else
+				{
+					info->m_lowerLimit[srow] = -SIMD_INFINITY;
+					info->m_upperLimit[srow] = 0;
+				}
+				// deal with bounce
+				if (limot->m_bounce > 0)
+				{
+					// calculate joint velocity
+					btScalar vel;
+					if (rotational)
+					{
+						vel = angVelA.dot(ax1);
+						//make sure that if no body -> angVelB == zero vec
+						//                        if (body1)
+						vel -= angVelB.dot(ax1);
+					}
+					else
+					{
+						vel = linVelA.dot(ax1);
+						//make sure that if no body -> angVelB == zero vec
+						//                        if (body1)
+						vel -= linVelB.dot(ax1);
+					}
+					// only apply bounce if the velocity is incoming, and if the
+					// resulting c[] exceeds what we already have.
+					if (limit == 1)
+					{
+						if (vel < 0)
+						{
+							btScalar newc = -limot->m_bounce * vel;
+							if (newc > info->m_constraintError[srow])
 								info->m_constraintError[srow] = newc;
-                        }
-                    }
-                    else
-                    {
-                        if (vel > 0)
-                        {
-                            btScalar newc = -limot->m_bounce * vel;
-                            if (newc < info->m_constraintError[srow]) 
+						}
+					}
+					else
+					{
+						if (vel > 0)
+						{
+							btScalar newc = -limot->m_bounce * vel;
+							if (newc < info->m_constraintError[srow])
 								info->m_constraintError[srow] = newc;
-                        }
-                    }
-                }
-            }
-        }
-        return 1;
-    }
-    else return 0;
+						}
+					}
+				}
+			}
+		}
+		return 1;
+	}
+	else
+		return 0;
 }
 
-
-
-
-
-
-	///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.
+///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 btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
 {
-	if((axis >= 0) && (axis < 3))
+	if ((axis >= 0) && (axis < 3))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			case BT_CONSTRAINT_STOP_ERP:
 				m_linearLimits.m_stopERP[axis] = value;
 				m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case BT_CONSTRAINT_STOP_CFM : 
+			case BT_CONSTRAINT_STOP_CFM:
 				m_linearLimits.m_stopCFM[axis] = value;
 				m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case BT_CONSTRAINT_CFM : 
+			case BT_CONSTRAINT_CFM:
 				m_linearLimits.m_normalCFM[axis] = value;
 				m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
-	else if((axis >=3) && (axis < 6))
+	else if ((axis >= 3) && (axis < 6))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			case BT_CONSTRAINT_STOP_ERP:
 				m_angularLimits[axis - 3].m_stopERP = value;
 				m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case BT_CONSTRAINT_STOP_CFM : 
+			case BT_CONSTRAINT_STOP_CFM:
 				m_angularLimits[axis - 3].m_stopCFM = value;
 				m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case BT_CONSTRAINT_CFM : 
+			case BT_CONSTRAINT_CFM:
 				m_angularLimits[axis - 3].m_normalCFM = value;
 				m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
@@ -990,47 +904,47 @@ void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
 	}
 }
 
-	///return the local value of parameter
-btScalar btGeneric6DofConstraint::getParam(int num, int axis) const 
+///return the local value of parameter
+btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
 {
 	btScalar retVal = 0;
-	if((axis >= 0) && (axis < 3))
+	if ((axis >= 0) && (axis < 3))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			case BT_CONSTRAINT_STOP_ERP:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_linearLimits.m_stopERP[axis];
 				break;
-			case BT_CONSTRAINT_STOP_CFM : 
+			case BT_CONSTRAINT_STOP_CFM:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_linearLimits.m_stopCFM[axis];
 				break;
-			case BT_CONSTRAINT_CFM : 
+			case BT_CONSTRAINT_CFM:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_linearLimits.m_normalCFM[axis];
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
-	else if((axis >=3) && (axis < 6))
+	else if ((axis >= 3) && (axis < 6))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			case BT_CONSTRAINT_STOP_ERP:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_angularLimits[axis - 3].m_stopERP;
 				break;
-			case BT_CONSTRAINT_STOP_CFM : 
+			case BT_CONSTRAINT_STOP_CFM:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_angularLimits[axis - 3].m_stopCFM;
 				break;
-			case BT_CONSTRAINT_CFM : 
+			case BT_CONSTRAINT_CFM:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_angularLimits[axis - 3].m_normalCFM;
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
@@ -1041,23 +955,21 @@ btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
 	return retVal;
 }
 
- 
-
-void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
+void btGeneric6DofConstraint::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
-	
+	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]);
-	
+	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();
 }
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
index bea8629c325..b9e762e175f 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
@@ -23,7 +23,6 @@ email: projectileman@yahoo.com
 http://gimpact.sf.net
 */
 
-
 #ifndef BT_GENERIC_6DOF_CONSTRAINT_H
 #define BT_GENERIC_6DOF_CONSTRAINT_H
 
@@ -33,96 +32,91 @@ http://gimpact.sf.net
 
 class btRigidBody;
 
-
-
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btGeneric6DofConstraintData2		btGeneric6DofConstraintDoubleData2
-#define btGeneric6DofConstraintDataName	"btGeneric6DofConstraintDoubleData2"
+#define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2
+#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2"
 #else
-#define btGeneric6DofConstraintData2		btGeneric6DofConstraintData
-#define btGeneric6DofConstraintDataName	"btGeneric6DofConstraintData"
-#endif //BT_USE_DOUBLE_PRECISION
-
+#define btGeneric6DofConstraintData2 btGeneric6DofConstraintData
+#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 //! Rotation Limit structure for generic joints
 class btRotationalLimitMotor
 {
 public:
-    //! limit_parameters
-    //!@{
-    btScalar m_loLimit;//!< joint limit
-    btScalar m_hiLimit;//!< joint limit
-    btScalar m_targetVelocity;//!< target motor velocity
-    btScalar m_maxMotorForce;//!< max force on motor
-    btScalar m_maxLimitForce;//!< max force on limit
-    btScalar m_damping;//!< Damping.
-    btScalar m_limitSoftness;//! Relaxation factor
-    btScalar m_normalCFM;//!< Constraint force mixing factor
-    btScalar m_stopERP;//!< Error tolerance factor when joint is at limit
-    btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
-    btScalar m_bounce;//!< restitution factor
-    bool m_enableMotor;
-
-    //!@}
-
-    //! temp_variables
-    //!@{
-    btScalar m_currentLimitError;//!  How much is violated this limit
-    btScalar m_currentPosition;     //!  current value of angle 
-    int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
-    btScalar m_accumulatedImpulse;
-    //!@}
-
-    btRotationalLimitMotor()
-    {
-    	m_accumulatedImpulse = 0.f;
-        m_targetVelocity = 0;
-        m_maxMotorForce = 0.1f;
-        m_maxLimitForce = 300.0f;
-        m_loLimit = 1.0f;
-        m_hiLimit = -1.0f;
+	//! limit_parameters
+	//!@{
+	btScalar m_loLimit;         //!< joint limit
+	btScalar m_hiLimit;         //!< joint limit
+	btScalar m_targetVelocity;  //!< target motor velocity
+	btScalar m_maxMotorForce;   //!< max force on motor
+	btScalar m_maxLimitForce;   //!< max force on limit
+	btScalar m_damping;         //!< Damping.
+	btScalar m_limitSoftness;   //! Relaxation factor
+	btScalar m_normalCFM;       //!< Constraint force mixing factor
+	btScalar m_stopERP;         //!< Error tolerance factor when joint is at limit
+	btScalar m_stopCFM;         //!< Constraint force mixing factor when joint is at limit
+	btScalar m_bounce;          //!< restitution factor
+	bool m_enableMotor;
+
+	//!@}
+
+	//! temp_variables
+	//!@{
+	btScalar m_currentLimitError;  //!  How much is violated this limit
+	btScalar m_currentPosition;    //!  current value of angle
+	int m_currentLimit;            //!< 0=free, 1=at lo limit, 2=at hi limit
+	btScalar m_accumulatedImpulse;
+	//!@}
+
+	btRotationalLimitMotor()
+	{
+		m_accumulatedImpulse = 0.f;
+		m_targetVelocity = 0;
+		m_maxMotorForce = 6.0f;
+		m_maxLimitForce = 300.0f;
+		m_loLimit = 1.0f;
+		m_hiLimit = -1.0f;
 		m_normalCFM = 0.f;
 		m_stopERP = 0.2f;
 		m_stopCFM = 0.f;
-        m_bounce = 0.0f;
-        m_damping = 1.0f;
-        m_limitSoftness = 0.5f;
-        m_currentLimit = 0;
-        m_currentLimitError = 0;
-        m_enableMotor = false;
-    }
-
-    btRotationalLimitMotor(const btRotationalLimitMotor & limot)
-    {
-        m_targetVelocity = limot.m_targetVelocity;
-        m_maxMotorForce = limot.m_maxMotorForce;
-        m_limitSoftness = limot.m_limitSoftness;
-        m_loLimit = limot.m_loLimit;
-        m_hiLimit = limot.m_hiLimit;
+		m_bounce = 0.0f;
+		m_damping = 1.0f;
+		m_limitSoftness = 0.5f;
+		m_currentLimit = 0;
+		m_currentLimitError = 0;
+		m_enableMotor = false;
+	}
+
+	btRotationalLimitMotor(const btRotationalLimitMotor& limot)
+	{
+		m_targetVelocity = limot.m_targetVelocity;
+		m_maxMotorForce = limot.m_maxMotorForce;
+		m_limitSoftness = limot.m_limitSoftness;
+		m_loLimit = limot.m_loLimit;
+		m_hiLimit = limot.m_hiLimit;
 		m_normalCFM = limot.m_normalCFM;
 		m_stopERP = limot.m_stopERP;
-		m_stopCFM =	limot.m_stopCFM;
-        m_bounce = limot.m_bounce;
-        m_currentLimit = limot.m_currentLimit;
-        m_currentLimitError = limot.m_currentLimitError;
-        m_enableMotor = limot.m_enableMotor;
-    }
-
-
+		m_stopCFM = limot.m_stopCFM;
+		m_bounce = limot.m_bounce;
+		m_currentLimit = limot.m_currentLimit;
+		m_currentLimitError = limot.m_currentLimitError;
+		m_enableMotor = limot.m_enableMotor;
+	}
 
 	//! Is limited
-    bool isLimited() const
-    {
-    	if(m_loLimit > m_hiLimit) return false;
-    	return true;
-    }
+	bool isLimited() const
+	{
+		if (m_loLimit > m_hiLimit) return false;
+		return true;
+	}
 
 	//! Need apply correction
-    bool needApplyTorques() const
-    {
-    	if(m_currentLimit == 0 && m_enableMotor == false) return false;
-    	return true;
-    }
+	bool needApplyTorques() const
+	{
+		if (m_currentLimit == 0 && m_enableMotor == false) return false;
+		return true;
+	}
 
 	//! calculates  error
 	/*!
@@ -131,104 +125,98 @@ public:
 	int testLimitValue(btScalar test_value);
 
 	//! apply the correction impulses for two bodies
-    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
-
+	btScalar solveAngularLimits(btScalar timeStep, btVector3& axis, btScalar jacDiagABInv, btRigidBody* body0, btRigidBody* body1);
 };
 
-
-
 class btTranslationalLimitMotor
 {
 public:
-	btVector3 m_lowerLimit;//!< the constraint lower limits
-    btVector3 m_upperLimit;//!< the constraint upper limits
-    btVector3 m_accumulatedImpulse;
-    //! Linear_Limit_parameters
-    //!@{
-    btScalar	m_limitSoftness;//!< Softness for linear limit
-    btScalar	m_damping;//!< Damping for linear limit
-    btScalar	m_restitution;//! Bounce parameter for linear limit
-	btVector3	m_normalCFM;//!< Constraint force mixing factor
-    btVector3	m_stopERP;//!< Error tolerance factor when joint is at limit
-	btVector3	m_stopCFM;//!< Constraint force mixing factor when joint is at limit
-    //!@}
-	bool		m_enableMotor[3];
-    btVector3	m_targetVelocity;//!< target motor velocity
-    btVector3	m_maxMotorForce;//!< max force on motor
-    btVector3	m_currentLimitError;//!  How much is violated this limit
-    btVector3	m_currentLinearDiff;//!  Current relative offset of constraint frames
-    int			m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
-
-    btTranslationalLimitMotor()
-    {
-    	m_lowerLimit.setValue(0.f,0.f,0.f);
-    	m_upperLimit.setValue(0.f,0.f,0.f);
-    	m_accumulatedImpulse.setValue(0.f,0.f,0.f);
+	btVector3 m_lowerLimit;  //!< the constraint lower limits
+	btVector3 m_upperLimit;  //!< the constraint upper limits
+	btVector3 m_accumulatedImpulse;
+	//! Linear_Limit_parameters
+	//!@{
+	btScalar m_limitSoftness;  //!< Softness for linear limit
+	btScalar m_damping;        //!< Damping for linear limit
+	btScalar m_restitution;    //! Bounce parameter for linear limit
+	btVector3 m_normalCFM;     //!< Constraint force mixing factor
+	btVector3 m_stopERP;       //!< Error tolerance factor when joint is at limit
+	btVector3 m_stopCFM;       //!< Constraint force mixing factor when joint is at limit
+							   //!@}
+	bool m_enableMotor[3];
+	btVector3 m_targetVelocity;     //!< target motor velocity
+	btVector3 m_maxMotorForce;      //!< max force on motor
+	btVector3 m_currentLimitError;  //!  How much is violated this limit
+	btVector3 m_currentLinearDiff;  //!  Current relative offset of constraint frames
+	int m_currentLimit[3];          //!< 0=free, 1=at lower limit, 2=at upper limit
+
+	btTranslationalLimitMotor()
+	{
+		m_lowerLimit.setValue(0.f, 0.f, 0.f);
+		m_upperLimit.setValue(0.f, 0.f, 0.f);
+		m_accumulatedImpulse.setValue(0.f, 0.f, 0.f);
 		m_normalCFM.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_limitSoftness = 0.7f;
-    	m_damping = btScalar(1.0f);
-    	m_restitution = btScalar(0.5f);
-		for(int i=0; i < 3; i++) 
+		m_limitSoftness = 0.7f;
+		m_damping = btScalar(1.0f);
+		m_restitution = btScalar(0.5f);
+		for (int i = 0; i < 3; i++)
 		{
 			m_enableMotor[i] = false;
 			m_targetVelocity[i] = btScalar(0.f);
 			m_maxMotorForce[i] = btScalar(0.f);
 		}
-    }
+	}
 
-    btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
-    {
-    	m_lowerLimit = other.m_lowerLimit;
-    	m_upperLimit = other.m_upperLimit;
-    	m_accumulatedImpulse = other.m_accumulatedImpulse;
+	btTranslationalLimitMotor(const btTranslationalLimitMotor& other)
+	{
+		m_lowerLimit = other.m_lowerLimit;
+		m_upperLimit = other.m_upperLimit;
+		m_accumulatedImpulse = other.m_accumulatedImpulse;
 
-    	m_limitSoftness = other.m_limitSoftness ;
-    	m_damping = other.m_damping;
-    	m_restitution = other.m_restitution;
+		m_limitSoftness = other.m_limitSoftness;
+		m_damping = other.m_damping;
+		m_restitution = other.m_restitution;
 		m_normalCFM = other.m_normalCFM;
 		m_stopERP = other.m_stopERP;
 		m_stopCFM = other.m_stopCFM;
 
-		for(int i=0; i < 3; i++) 
+		for (int i = 0; i < 3; i++)
 		{
 			m_enableMotor[i] = other.m_enableMotor[i];
 			m_targetVelocity[i] = other.m_targetVelocity[i];
 			m_maxMotorForce[i] = other.m_maxMotorForce[i];
 		}
-    }
+	}
 
-    //! Test limit
+	//! Test limit
 	/*!
     - free means upper < lower,
     - locked means upper == lower
     - limited means upper > lower
     - limitIndex: first 3 are linear, next 3 are angular
     */
-    inline bool	isLimited(int limitIndex) const
-    {
-       return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
-    }
-    inline bool needApplyForce(int limitIndex) const
-    {
-    	if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
-    	return true;
-    }
+	inline bool isLimited(int limitIndex) const
+	{
+		return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+	}
+	inline bool needApplyForce(int limitIndex) const
+	{
+		if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
+		return true;
+	}
 	int testLimitValue(int limitIndex, btScalar test_value);
 
-
-    btScalar solveLinearAxis(
-    	btScalar timeStep,
-        btScalar jacDiagABInv,
-        btRigidBody& body1,const btVector3 &pointInA,
-        btRigidBody& body2,const btVector3 &pointInB,
-        int limit_index,
-        const btVector3 & axis_normal_on_a,
-		const btVector3 & anchorPos);
-
-
+	btScalar solveLinearAxis(
+		btScalar timeStep,
+		btScalar jacDiagABInv,
+		btRigidBody& body1, const btVector3& pointInA,
+		btRigidBody& body2, const btVector3& pointInB,
+		int limit_index,
+		const btVector3& axis_normal_on_a,
+		const btVector3& anchorPos);
 };
 
 enum bt6DofFlags
@@ -237,8 +225,7 @@ enum bt6DofFlags
 	BT_6DOF_FLAGS_CFM_STOP = 2,
 	BT_6DOF_FLAGS_ERP_STOP = 4
 };
-#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
-
+#define BT_6DOF_FLAGS_AXIS_SHIFT 3  // bits per axis
 
 /// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 /*!
@@ -276,254 +263,245 @@ This brings support for limit parameters and motors. </li>
 </ul>
 
 */
-ATTRIBUTE_ALIGNED16(class) btGeneric6DofConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class)
+btGeneric6DofConstraint : public btTypedConstraint
 {
 protected:
-
 	//! relative_frames
-    //!@{
-	btTransform	m_frameInA;//!< the constraint space w.r.t body A
-    btTransform	m_frameInB;//!< the constraint space w.r.t body B
-    //!@}
+	//!@{
+	btTransform m_frameInA;  //!< the constraint space w.r.t body A
+	btTransform m_frameInB;  //!< the constraint space w.r.t body B
+	//!@}
 
-    //! Jacobians
-    //!@{
-    btJacobianEntry	m_jacLinear[3];//!< 3 orthogonal linear constraints
-    btJacobianEntry	m_jacAng[3];//!< 3 orthogonal angular constraints
-    //!@}
+	//! Jacobians
+	//!@{
+	btJacobianEntry m_jacLinear[3];  //!< 3 orthogonal linear constraints
+	btJacobianEntry m_jacAng[3];     //!< 3 orthogonal angular constraints
+	//!@}
 
 	//! Linear_Limit_parameters
-    //!@{
-    btTranslationalLimitMotor m_linearLimits;
-    //!@}
-
-
-    //! hinge_parameters
-    //!@{
-    btRotationalLimitMotor m_angularLimits[3];
+	//!@{
+	btTranslationalLimitMotor m_linearLimits;
 	//!@}
 
+	//! hinge_parameters
+	//!@{
+	btRotationalLimitMotor m_angularLimits[3];
+	//!@}
 
 protected:
-    //! temporal variables
-    //!@{
-    btScalar m_timeStep;
-    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;
-    
-	btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
+	//! temporal variables
+	//!@{
+	btScalar m_timeStep;
+	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;
 
-    bool	m_useLinearReferenceFrameA;
-	bool	m_useOffsetForConstraintFrame;
-    
-	int		m_flags;
+	btVector3 m_AnchorPos;  // point betwen pivots of bodies A and B to solve linear axes
 
-    //!@}
+	bool m_useLinearReferenceFrameA;
+	bool m_useOffsetForConstraintFrame;
 
-    btGeneric6DofConstraint&	operator=(btGeneric6DofConstraint&	other)
-    {
-        btAssert(0);
-        (void) other;
-        return *this;
-    }
+	int m_flags;
 
+	//!@}
 
-	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);
+	btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
+	{
+		btAssert(0);
+		(void)other;
+		return *this;
+	}
 
-	int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+	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);
 
-    void buildLinearJacobian(
-        btJacobianEntry & jacLinear,const btVector3 & normalWorld,
-        const btVector3 & pivotAInW,const btVector3 & pivotBInW);
+	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 buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
+	void buildLinearJacobian(
+		btJacobianEntry & jacLinear, const btVector3& normalWorld,
+		const btVector3& pivotAInW, const btVector3& pivotBInW);
+
+	void buildAngularJacobian(btJacobianEntry & jacAngular, const btVector3& jointAxisW);
 
 	// tests linear limits
 	void calculateLinearInfo();
 
 	//! calcs the euler angles between the two bodies.
-    void calculateAngleInfo();
-
-
+	void calculateAngleInfo();
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	///for backwards compatibility during the transition to 'getInfo/getInfo2'
-	bool		m_useSolveConstraintObsolete;
+	bool m_useSolveConstraintObsolete;
+
+	btGeneric6DofConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
+	btGeneric6DofConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
 
-    btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
-    btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
-    
 	//! Calcs global transform of the offsets
 	/*!
 	Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
 	\sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
 	*/
-    void calculateTransforms(const btTransform& transA,const btTransform& transB);
+	void calculateTransforms(const btTransform& transA, const btTransform& transB);
 
 	void calculateTransforms();
 
 	//! Gets the global transform of the offset for body A
-    /*!
+	/*!
     \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
     */
-    const btTransform & getCalculatedTransformA() const
-    {
-    	return m_calculatedTransformA;
-    }
+	const btTransform& getCalculatedTransformA() const
+	{
+		return m_calculatedTransformA;
+	}
 
-    //! Gets the global transform of the offset for body B
-    /*!
+	//! Gets the global transform of the offset for body B
+	/*!
     \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
     */
-    const btTransform & getCalculatedTransformB() const
-    {
-    	return m_calculatedTransformB;
-    }
-
-    const btTransform & getFrameOffsetA() const
-    {
-    	return m_frameInA;
-    }
-
-    const btTransform & getFrameOffsetB() const
-    {
-    	return m_frameInB;
-    }
+	const btTransform& getCalculatedTransformB() const
+	{
+		return m_calculatedTransformB;
+	}
 
+	const btTransform& getFrameOffsetA() const
+	{
+		return m_frameInA;
+	}
 
-    btTransform & getFrameOffsetA()
-    {
-    	return m_frameInA;
-    }
+	const btTransform& getFrameOffsetB() const
+	{
+		return m_frameInB;
+	}
 
-    btTransform & getFrameOffsetB()
-    {
-    	return m_frameInB;
-    }
+	btTransform& getFrameOffsetA()
+	{
+		return m_frameInA;
+	}
 
+	btTransform& getFrameOffsetB()
+	{
+		return m_frameInB;
+	}
 
 	//! performs Jacobian calculation, and also calculates angle differences and axis
-    virtual void	buildJacobian();
-
-	virtual void getInfo1 (btConstraintInfo1* info);
+	virtual void buildJacobian();
 
-	void getInfo1NonVirtual (btConstraintInfo1* info);
+	virtual void getInfo1(btConstraintInfo1 * info);
 
-	virtual void getInfo2 (btConstraintInfo2* info);
+	void getInfo1NonVirtual(btConstraintInfo1 * info);
 
-	void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+	virtual void getInfo2(btConstraintInfo2 * info);
 
+	void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
 
-    void	updateRHS(btScalar	timeStep);
+	void updateRHS(btScalar timeStep);
 
 	//! Get the rotation axis in global coordinates
 	/*!
 	\pre btGeneric6DofConstraint.buildJacobian must be called previously.
 	*/
-    btVector3 getAxis(int axis_index) const;
+	btVector3 getAxis(int axis_index) const;
 
-    //! Get the relative Euler angle
-    /*!
+	//! Get the relative Euler angle
+	/*!
 	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
 	*/
-    btScalar getAngle(int axis_index) const;
+	btScalar getAngle(int axis_index) const;
 
 	//! Get the relative position of the constraint pivot
-    /*!
+	/*!
 	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
 	*/
 	btScalar getRelativePivotPosition(int axis_index) const;
 
-	void setFrames(const btTransform & frameA, const btTransform & frameB);
+	void setFrames(const btTransform& frameA, const btTransform& frameB);
 
 	//! Test angular limit.
 	/*!
 	Calculates angular correction and returns true if limit needs to be corrected.
 	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
 	*/
-    bool testAngularLimitMotor(int axis_index);
+	bool testAngularLimitMotor(int axis_index);
 
-    void	setLinearLowerLimit(const btVector3& linearLower)
-    {
-    	m_linearLimits.m_lowerLimit = linearLower;
-    }
+	void setLinearLowerLimit(const btVector3& linearLower)
+	{
+		m_linearLimits.m_lowerLimit = linearLower;
+	}
 
-	void	getLinearLowerLimit(btVector3& linearLower) const
+	void getLinearLowerLimit(btVector3 & linearLower) const
 	{
 		linearLower = m_linearLimits.m_lowerLimit;
 	}
 
-	void	setLinearUpperLimit(const btVector3& linearUpper)
+	void setLinearUpperLimit(const btVector3& linearUpper)
 	{
 		m_linearLimits.m_upperLimit = linearUpper;
 	}
 
-	void	getLinearUpperLimit(btVector3& linearUpper) const
+	void getLinearUpperLimit(btVector3 & linearUpper) const
 	{
 		linearUpper = m_linearLimits.m_upperLimit;
 	}
 
-    void	setAngularLowerLimit(const btVector3& angularLower)
-    {
-		for(int i = 0; i < 3; i++) 
+	void setAngularLowerLimit(const btVector3& angularLower)
+	{
+		for (int i = 0; i < 3; i++)
 			m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
-    }
+	}
 
-	void	getAngularLowerLimit(btVector3& angularLower) const
+	void getAngularLowerLimit(btVector3 & angularLower) const
 	{
-		for(int i = 0; i < 3; i++) 
+		for (int i = 0; i < 3; i++)
 			angularLower[i] = m_angularLimits[i].m_loLimit;
 	}
 
-    void	setAngularUpperLimit(const btVector3& angularUpper)
-    {
-		for(int i = 0; i < 3; i++)
+	void setAngularUpperLimit(const btVector3& angularUpper)
+	{
+		for (int i = 0; i < 3; i++)
 			m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
-    }
+	}
 
-	void	getAngularUpperLimit(btVector3& angularUpper) const
+	void getAngularUpperLimit(btVector3 & angularUpper) const
 	{
-		for(int i = 0; i < 3; i++)
+		for (int i = 0; i < 3; i++)
 			angularUpper[i] = m_angularLimits[i].m_hiLimit;
 	}
 
 	//! Retrieves the angular limit informacion
-    btRotationalLimitMotor * getRotationalLimitMotor(int index)
-    {
-    	return &m_angularLimits[index];
-    }
-
-    //! Retrieves the  limit informacion
-    btTranslationalLimitMotor * getTranslationalLimitMotor()
-    {
-    	return &m_linearLimits;
-    }
-
-    //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
-    	{
+	btRotationalLimitMotor* getRotationalLimitMotor(int index)
+	{
+		return &m_angularLimits[index];
+	}
+
+	//! Retrieves the  limit informacion
+	btTranslationalLimitMotor* getTranslationalLimitMotor()
+	{
+		return &m_linearLimits;
+	}
+
+	//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;
-    	}
-    }
+			m_angularLimits[axis - 3].m_loLimit = lo;
+			m_angularLimits[axis - 3].m_hiLimit = hi;
+		}
+	}
 
 	//! Test limit
 	/*!
@@ -532,116 +510,106 @@ public:
     - limited means upper > lower
     - limitIndex: first 3 are linear, next 3 are angular
     */
-    bool	isLimited(int limitIndex) const
-    {
-    	if(limitIndex<3)
-    	{
+	bool isLimited(int limitIndex) const
+	{
+		if (limitIndex < 3)
+		{
 			return m_linearLimits.isLimited(limitIndex);
+		}
+		return m_angularLimits[limitIndex - 3].isLimited();
+	}
 
-    	}
-        return m_angularLimits[limitIndex-3].isLimited();
-    }
-
-	virtual void calcAnchorPos(void); // overridable
+	virtual void calcAnchorPos(void);  // overridable
 
-	int get_limit_motor_info2(	btRotationalLimitMotor * 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);
+	int get_limit_motor_info2(btRotationalLimitMotor * 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);
 
 	// access for UseFrameOffset
 	bool getUseFrameOffset() const { return m_useOffsetForConstraintFrame; }
 	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
-	
+
 	bool getUseLinearReferenceFrameA() const { return m_useLinearReferenceFrameA; }
 	void setUseLinearReferenceFrameA(bool linearReferenceFrameA) { m_useLinearReferenceFrameA = linearReferenceFrameA; }
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///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 void setParam(int num, btScalar value, int axis = -1);
 	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const;
+	virtual btScalar getParam(int num, int axis = -1) const;
 
-	void setAxis( const btVector3& axis1, const btVector3& axis2);
+	void setAxis(const btVector3& axis1, const btVector3& axis2);
 
-    	virtual	int getFlags() const
-    	{
-        	return m_flags;
+	virtual int getFlags() const
+	{
+		return m_flags;
 	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
-	
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
-
 struct btGeneric6DofConstraintData
 {
-	btTypedConstraintData	m_typeConstraintData;
-	btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTypedConstraintData m_typeConstraintData;
+	btTransformFloatData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransformFloatData m_rbBFrame;
-	
-	btVector3FloatData	m_linearUpperLimit;
-	btVector3FloatData	m_linearLowerLimit;
-
-	btVector3FloatData	m_angularUpperLimit;
-	btVector3FloatData	m_angularLowerLimit;
-	
-	int	m_useLinearReferenceFrameA;
+
+	btVector3FloatData m_linearUpperLimit;
+	btVector3FloatData m_linearLowerLimit;
+
+	btVector3FloatData m_angularUpperLimit;
+	btVector3FloatData m_angularLowerLimit;
+
+	int m_useLinearReferenceFrameA;
 	int m_useOffsetForConstraintFrame;
 };
 
 struct btGeneric6DofConstraintDoubleData2
 {
-	btTypedConstraintDoubleData	m_typeConstraintData;
-	btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTypedConstraintDoubleData m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransformDoubleData m_rbBFrame;
-	
-	btVector3DoubleData	m_linearUpperLimit;
-	btVector3DoubleData	m_linearLowerLimit;
-
-	btVector3DoubleData	m_angularUpperLimit;
-	btVector3DoubleData	m_angularLowerLimit;
-	
-	int	m_useLinearReferenceFrameA;
+
+	btVector3DoubleData m_linearUpperLimit;
+	btVector3DoubleData m_linearLowerLimit;
+
+	btVector3DoubleData m_angularUpperLimit;
+	btVector3DoubleData m_angularLowerLimit;
+
+	int m_useLinearReferenceFrameA;
 	int m_useOffsetForConstraintFrame;
 };
 
-SIMD_FORCE_INLINE	int	btGeneric6DofConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btGeneric6DofConstraintData2);
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-
 	btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer;
-	btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
+	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++)
+	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_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit;
+		dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit;
 		dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i];
 		dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i];
 	}
-	
-	dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
+
+	dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA ? 1 : 0;
 	dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
 
 	return btGeneric6DofConstraintDataName;
 }
 
-
-
-
-
-#endif //BT_GENERIC_6DOF_CONSTRAINT_H
+#endif  //BT_GENERIC_6DOF_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
index 31371944864..74a13c62493 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
@@ -37,67 +37,55 @@ email: projectileman@yahoo.com
 http://gimpact.sf.net
 */
 
-
-
 #include "btGeneric6DofSpring2Constraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
+#include <cmath>
 #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)
+	: 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)
+	: 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;
+	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)
+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);
+	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));
+			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[0] = -btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
 			xyz[1] = -SIMD_HALF_PI;
 			xyz[2] = btScalar(0.0);
 			return false;
@@ -106,32 +94,32 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat,btV
 	else
 	{
 		// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
-		xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+		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)
+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);
+	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));
+			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[0] = -btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
 			xyz[1] = btScalar(0.0);
 			xyz[2] = SIMD_HALF_PI;
 			return false;
@@ -139,33 +127,33 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btV
 	}
 	else
 	{
-		xyz[0] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
+		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)
+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);
+	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));
+			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[1] = -btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
 			xyz[2] = btScalar(0.0);
 			return false;
 		}
@@ -173,32 +161,32 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btV
 	else
 	{
 		xyz[0] = -SIMD_HALF_PI;
-		xyz[1] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+		xyz[1] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
 		xyz[2] = 0.0;
 	}
 	return false;
 }
 
-bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz)
+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);
+	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));
+			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[1] = -btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
 			xyz[2] = -SIMD_HALF_PI;
 			return false;
 		}
@@ -206,33 +194,33 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btV
 	else
 	{
 		xyz[0] = btScalar(0.0);
-		xyz[1] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
+		xyz[1] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
 		xyz[2] = SIMD_HALF_PI;
 	}
 	return false;
 }
 
-bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz)
+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);
+	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));
+			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));
+			xyz[2] = -btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
 			return false;
 		}
 	}
@@ -240,32 +228,32 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btV
 	{
 		xyz[0] = SIMD_HALF_PI;
 		xyz[1] = btScalar(0.0);
-		xyz[2] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
+		xyz[2] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
 	}
 	return false;
 }
 
-bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz)
+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);
+	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));
+			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));
+			xyz[2] = -btAtan2(btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 2));
 			return false;
 		}
 	}
@@ -273,23 +261,36 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btV
 	{
 		xyz[0] = btScalar(0.0);
 		xyz[1] = -SIMD_HALF_PI;
-		xyz[2] = btAtan2(-btGetMatrixElem(mat,1),-btGetMatrixElem(mat,2));
+		xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), -btGetMatrixElem(mat, 2));
 	}
 	return false;
 }
 
 void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 {
-	btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
+	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);
+		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]) :
@@ -307,14 +308,14 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 	// to the components of w and set that to 0.
 	switch (m_rotateOrder)
 	{
-	case RO_XYZ :
+		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
+			//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)
+			//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)	
+			// y' = N = axis2.cross(axis0)
 			// z' = z
 			//
 			// x" = X
@@ -324,7 +325,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 			//first rotate around z
 			//second rotate around y'= z.cross(X)
 			//third rotate around x" = X
-			//Original XYZ extrinsic rotation order. 
+			//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);
@@ -333,7 +334,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 			m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
 			break;
 		}
-	case RO_XZY :
+		case RO_XZY:
 		{
 			//planes: xz,ZY normals: y, X
 			//first rotate around y
@@ -346,7 +347,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 			m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
 			break;
 		}
-	case RO_YXZ :
+		case RO_YXZ:
 		{
 			//planes: yx,XZ normals: z, Y
 			//first rotate around z
@@ -359,7 +360,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 			m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
 			break;
 		}
-	case RO_YZX :
+		case RO_YZX:
 		{
 			//planes: yz,ZX normals: x, Y
 			//first rotate around x
@@ -372,7 +373,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 			m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
 			break;
 		}
-	case RO_ZXY :
+		case RO_ZXY:
 		{
 			//planes: zx,XY normals: y, Z
 			//first rotate around y
@@ -385,7 +386,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 			m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
 			break;
 		}
-	case RO_ZYX :
+		case RO_ZYX:
 		{
 			//planes: zy,YX normals: x, Z
 			//first rotate around x
@@ -398,22 +399,21 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 			m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
 			break;
 		}
-	default:
-		btAssert(false);
+		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());
+	calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 }
 
-void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA, const btTransform& transB)
 {
 	m_calculatedTransformA = transA * m_frameInA;
 	m_calculatedTransformB = transB * m_frameInB;
@@ -424,18 +424,17 @@ void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& tran
 	btScalar miB = getRigidBodyB().getInvMass();
 	m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
 	btScalar miS = miA + miB;
-	if(miS > btScalar(0.f))
+	if (miS > btScalar(0.f))
 	{
 		m_factA = miB / miS;
 	}
-	else 
+	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];
@@ -444,35 +443,37 @@ void btGeneric6DofSpring2Constraint::testAngularLimitMotor(int axis_index)
 	m_angularLimits[axis_index].testLimitValue(angle);
 }
 
-
-void btGeneric6DofSpring2Constraint::getInfo1 (btConstraintInfo1* info)
+void btGeneric6DofSpring2Constraint::getInfo1(btConstraintInfo1* info)
 {
 	//prepare constraint
-	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	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++)
+	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_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++ )
+	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_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)
+void btGeneric6DofSpring2Constraint::getInfo2(btConstraintInfo2* info)
 {
 	const btTransform& transA = m_rbA.getCenterOfMassTransform();
 	const btTransform& transB = m_rbB.getCenterOfMassTransform();
@@ -482,118 +483,138 @@ void btGeneric6DofSpring2Constraint::getInfo2 (btConstraintInfo2* info)
 	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 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)
+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++ )
+	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];
+		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_springStiffnessLimited = m_linearLimits.m_springStiffnessLimited[i];
-			limot.m_springDamping          = m_linearLimits.m_springDamping[i];
-			limot.m_springDampingLimited   = m_linearLimits.m_springDampingLimited[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];
+			limot.m_springDamping = m_linearLimits.m_springDamping[i];
+			limot.m_springDampingLimited = m_linearLimits.m_springDampingLimited[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_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
+			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 ) );
+								 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 )
+								 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);
-
+			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 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)
+	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);
+		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++ )
+	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)
+		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))
+			if (!(flags & BT_6DOF_FLAGS_CFM_STOP2))
 			{
 				m_angularLimits[i].m_stopCFM = info->cfm[0];
 			}
-			if(!(flags & BT_6DOF_FLAGS_ERP_STOP2))
+			if (!(flags & BT_6DOF_FLAGS_ERP_STOP2))
 			{
 				m_angularLimits[i].m_stopERP = info->erp;
 			}
-			if(!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
+			if (!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
 			{
 				m_angularLimits[i].m_motorCFM = info->cfm[0];
 			}
-			if(!(flags & BT_6DOF_FLAGS_ERP_MOTO2))
+			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);
+			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;
@@ -602,32 +623,31 @@ void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const
 	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++)
+	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)
+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;
+	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];
+	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];
+	J2[srow + 0] = -ax1[0];
+	J2[srow + 1] = -ax1[1];
+	J2[srow + 2] = -ax1[2];
 
-	if(!rotational)
+	if (!rotational)
 	{
 		btVector3 tmpA, tmpB, relA, relB;
 		// get vector from bodyB to frameB in WCS
@@ -636,40 +656,44 @@ void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * l
 		relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
 		tmpA = relA.cross(ax1);
 		tmpB = relB.cross(ax1);
-		if(m_hasStaticBody && (!rotAllowed))
+		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];
+		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)
+	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) 
+	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);
+		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 (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;
+		}
+		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;
 			}
 		}
@@ -679,16 +703,21 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		srow += info->rowskip;
 		++count;
 
-		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		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 (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;
+		}
+		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;
 			}
 		}
@@ -697,10 +726,10 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		info->cfm[srow] = limot->m_stopCFM;
 		srow += info->rowskip;
 		++count;
-	} else
-	if (limot->m_currentLimit==3) 
+	}
+	else if (limot->m_currentLimit == 3)
 	{
-		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		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;
@@ -711,16 +740,16 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 
 	if (limot->m_enableMotor && !limot->m_servoMotor)
 	{
-		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		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);
+		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->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
+		info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
 		info->cfm[srow] = limot->m_motorCFM;
 		srow += info->rowskip;
 		++count;
@@ -729,33 +758,48 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 	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 curServoTarget = limot->m_servoTarget;
+		if (rotational)
+		{
+			if (error > SIMD_PI)
+			{
+				error -= SIMD_2_PI;
+				curServoTarget += SIMD_2_PI;
+			}
+			if (error < -SIMD_PI)
+			{
+				error += SIMD_2_PI;
+				curServoTarget -= SIMD_2_PI;
+			}
+		}
+
+		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)
+		if (error != 0)
 		{
 			btScalar lowLimit;
 			btScalar hiLimit;
-			if(limot->m_loLimit > limot->m_hiLimit)
+			if (limot->m_loLimit > limot->m_hiLimit)
 			{
-				lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY;
-				hiLimit  = error < 0 ? limot->m_servoTarget :  SIMD_INFINITY;
+				lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY;
+				hiLimit = error < 0 ? curServoTarget : 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;
+				lowLimit = error > 0 && curServoTarget > limot->m_loLimit ? curServoTarget : limot->m_loLimit;
+				hiLimit = error < 0 && curServoTarget < limot->m_hiLimit ? curServoTarget : limot->m_hiLimit;
 			}
 			mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
-		} 
-		else 
+		}
+		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->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
+		info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
 		info->cfm[srow] = limot->m_motorCFM;
 		srow += info->rowskip;
 		++count;
@@ -764,7 +808,7 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 	if (limot->m_enableSpring)
 	{
 		btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint;
-		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		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)
@@ -777,51 +821,69 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		btScalar dt = BT_ONE / 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 vel;
+		if (rotational)
+		{
+			vel = angVelA.dot(ax1) - angVelB.dot(ax1);
+		}
+		else
+		{
+			btVector3 tanVelA = angVelA.cross(m_calculatedTransformA.getOrigin() - transA.getOrigin());
+			btVector3 tanVelB = angVelB.cross(m_calculatedTransformB.getOrigin() - transB.getOrigin());
+			vel = (linVelA + tanVelA).dot(ax1) - (linVelB + tanVelB).dot(ax1);
+		}
 		btScalar cfm = BT_ZERO;
 		btScalar mA = BT_ONE / m_rbA.getInvMass();
 		btScalar mB = BT_ONE / m_rbB.getInvMass();
-		if (rotational) {
+		if (rotational)
+		{
 			btScalar rrA = (m_calculatedTransformA.getOrigin() - transA.getOrigin()).length2();
 			btScalar rrB = (m_calculatedTransformB.getOrigin() - transB.getOrigin()).length2();
 			if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length();
 			if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length();
 		}
-		btScalar m = mA > mB ? mB : mA;
-		btScalar angularfreq = sqrt(ks / m);
-
+		btScalar m;
+		if (m_rbA.getInvMass() == 0) m = mB; else
+		if (m_rbB.getInvMass() == 0) m = mA; else
+			m = mA*mB / (mA + mB);
+		btScalar angularfreq = btSqrt(ks / m);
 
 		//limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
-		if(limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
+		if (limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
 		{
 			ks = BT_ONE / dt / dt / btScalar(16.0) * m;
 		}
 		//avoid damping that would blow up the spring
-		if(limot->m_springDampingLimited && kd * dt > m)
+		if (limot->m_springDampingLimited && kd * dt > m)
 		{
 			kd = m / dt;
 		}
 		btScalar fs = ks * error * dt;
 		btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
-		btScalar f = (fs+fd);
-
-	// after the spring force affecting the body(es) the new velocity will be
-	// vel + f / m * (rotational ? -1 : 1)
-	// so in theory this should be set here for m_constraintError
-	// (with m_constraintError we set a desired velocity for the affected body(es))
-	// however in practice any value is fine as long as it is greater then the "proper" velocity,
-	// because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
-	// so it is much simpler (and more robust) just to simply use inf (with the proper sign)
-	// you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)
-	// will we not request a velocity with the wrong direction ?
-	// and the answare is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
-	// so the sign of the force that is really matters
-	info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
+		btScalar f = (fs + fd);
+
+		// after the spring force affecting the body(es) the new velocity will be
+		// vel + f / m * (rotational ? -1 : 1)
+		// so in theory this should be set here for m_constraintError
+		// (with m_constraintError we set a desired velocity for the affected body(es))
+		// however in practice any value is fine as long as it is greater than the "proper" velocity,
+		// because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
+		// so it is much simpler (and more robust) just to simply use inf (with the proper sign)
+		// (Even with our best intent the "new" velocity is only an estimation. If we underestimate
+		// the "proper" velocity that will weaken the spring, however if we overestimate it, it doesn't
+		// matter, because the solver will limit it according the force limit)
+		// you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)
+		// will we not request a velocity with the wrong direction ?
+		// and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
+		// so the sign of the force that is really matters
+		if (m_flags & BT_6DOF_FLAGS_USE_INFINITE_ERROR)
+			info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
+		else
+			info->m_constraintError[srow] = vel + f / m * (rotational ? -1 : 1);
 
 		btScalar minf = f < fd ? f : fd;
 		btScalar maxf = f < fd ? fd : f;
-		if(!rotational)
+		if (!rotational)
 		{
 			info->m_lowerLimit[srow] = minf > 0 ? 0 : minf;
 			info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf;
@@ -840,56 +902,55 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 	return count;
 }
 
-
-//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+//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))
+	if ((axis >= 0) && (axis < 3))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			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 : 
+			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 : 
+			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 : 
+			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 : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
-	else if((axis >=3) && (axis < 6))
+	else if ((axis >= 3) && (axis < 6))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			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 : 
+			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 : 
+			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 : 
+			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 : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
@@ -900,54 +961,54 @@ void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis)
 }
 
 //return the local value of parameter
-btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const 
+btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const
 {
 	btScalar retVal = 0;
-	if((axis >= 0) && (axis < 3))
+	if ((axis >= 0) && (axis < 3))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			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 : 
+			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 : 
+			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 : 
+			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 : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
-	else if((axis >=3) && (axis < 6))
+	else if ((axis >= 3) && (axis < 6))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			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 : 
+			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 : 
+			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 : 
+			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 : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
@@ -958,31 +1019,29 @@ btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const
 	return retVal;
 }
 
- 
-
-void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1,const btVector3& axis2)
+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
-	
+	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]);
-	
+	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)
+	if (index < 3)
 		m_linearLimits.m_bounce[index] = bounce;
 	else
 		m_angularLimits[index - 3].m_bounce = bounce;
@@ -991,7 +1050,7 @@ void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce)
 void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3)
+	if (index < 3)
 		m_linearLimits.m_enableMotor[index] = onOff;
 	else
 		m_angularLimits[index - 3].m_enableMotor = onOff;
@@ -1000,7 +1059,7 @@ void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff)
 void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3)
+	if (index < 3)
 		m_linearLimits.m_servoMotor[index] = onOff;
 	else
 		m_angularLimits[index - 3].m_servoMotor = onOff;
@@ -1009,25 +1068,60 @@ void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
 void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3)
+	if (index < 3)
 		m_linearLimits.m_targetVelocity[index] = velocity;
 	else
 		m_angularLimits[index - 3].m_targetVelocity = velocity;
 }
 
-void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar target)
+void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOrg)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3)
-		m_linearLimits.m_servoTarget[index] = target;
+	if (index < 3)
+	{
+		m_linearLimits.m_servoTarget[index] = targetOrg;
+	}
 	else
+	{
+		//wrap between -PI and PI, see also
+		//https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi
+
+		btScalar target = targetOrg + SIMD_PI;
+		if (1)
+		{
+			btScalar m = target - SIMD_2_PI * std::floor(target / SIMD_2_PI);
+			// handle boundary cases resulted from floating-point cut off:
+			{
+				if (m >= SIMD_2_PI)
+				{
+					target = 0;
+				}
+				else
+				{
+					if (m < 0)
+					{
+						if (SIMD_2_PI + m == SIMD_2_PI)
+							target = 0;
+						else
+							target = SIMD_2_PI + m;
+					}
+					else
+					{
+						target = m;
+					}
+				}
+			}
+			target -= SIMD_PI;
+		}
+
 		m_angularLimits[index - 3].m_servoTarget = target;
+	}
 }
 
 void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3)
+	if (index < 3)
 		m_linearLimits.m_maxMotorForce[index] = force;
 	else
 		m_angularLimits[index - 3].m_maxMotorForce = force;
@@ -1036,19 +1130,22 @@ void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)
 void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3)
+	if (index < 3)
 		m_linearLimits.m_enableSpring[index] = onOff;
 	else
-		m_angularLimits[index - 3] .m_enableSpring = onOff;
+		m_angularLimits[index - 3].m_enableSpring = onOff;
 }
 
 void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3) {
+	if (index < 3)
+	{
 		m_linearLimits.m_springStiffness[index] = stiffness;
 		m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded;
-	} else {
+	}
+	else
+	{
 		m_angularLimits[index - 3].m_springStiffness = stiffness;
 		m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded;
 	}
@@ -1057,10 +1154,13 @@ void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness,
 void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3) {
+	if (index < 3)
+	{
 		m_linearLimits.m_springDamping[index] = damping;
 		m_linearLimits.m_springDampingLimited[index] = limitIfNeeded;
-	} else {
+	}
+	else
+	{
 		m_angularLimits[index - 3].m_springDamping = damping;
 		m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded;
 	}
@@ -1070,9 +1170,9 @@ void btGeneric6DofSpring2Constraint::setEquilibriumPoint()
 {
 	calculateTransforms();
 	int i;
-	for( i = 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 		m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
-	for(i = 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 		m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i];
 }
 
@@ -1080,35 +1180,38 @@ void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index)
 {
 	btAssert((index >= 0) && (index < 6));
 	calculateTransforms();
-	if (index<3)
+	if (index < 3)
 		m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
 	else
-		m_angularLimits[index - 3] .m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3];
+		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)
+	if (index < 3)
 		m_linearLimits.m_equilibriumPoint[index] = val;
 	else
-		m_angularLimits[index - 3] .m_equilibriumPoint = val;
+		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) {
+	if (m_loLimit > m_hiLimit)
+	{
 		m_currentLimit = 0;
 		m_currentLimitError = btScalar(0.f);
 	}
-	else if(m_loLimit == m_hiLimit) {
+	else if (m_loLimit == m_hiLimit)
+	{
 		m_currentLimitError = test_value - m_loLimit;
 		m_currentLimit = 3;
-	} else {
+	}
+	else
+	{
 		m_currentLimitError = test_value - m_loLimit;
 		m_currentLimitErrorHi = test_value - m_hiLimit;
 		m_currentLimit = 4;
@@ -1121,18 +1224,20 @@ void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_va
 {
 	btScalar loLimit = m_lowerLimit[limitIndex];
 	btScalar hiLimit = m_upperLimit[limitIndex];
-	if(loLimit > hiLimit) {
+	if (loLimit > hiLimit)
+	{
 		m_currentLimitError[limitIndex] = 0;
 		m_currentLimit[limitIndex] = 0;
 	}
-	else if(loLimit == hiLimit) {
+	else if (loLimit == hiLimit)
+	{
 		m_currentLimitError[limitIndex] = test_value - loLimit;
 		m_currentLimit[limitIndex] = 3;
-	} else {
+	}
+	else
+	{
 		m_currentLimitError[limitIndex] = test_value - loLimit;
 		m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
 		m_currentLimit[limitIndex] = 4;
 	}
 }
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
index ace4b3c29bf..c86dc373da6 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
@@ -37,7 +37,6 @@ email: projectileman@yahoo.com
 http://gimpact.sf.net
 */
 
-
 #ifndef BT_GENERIC_6DOF_CONSTRAINT2_H
 #define BT_GENERIC_6DOF_CONSTRAINT2_H
 
@@ -47,18 +46,17 @@ http://gimpact.sf.net
 
 class btRigidBody;
 
-
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btGeneric6DofSpring2ConstraintData2		btGeneric6DofSpring2ConstraintDoubleData2
-#define btGeneric6DofSpring2ConstraintDataName	"btGeneric6DofSpring2ConstraintDoubleData2"
+#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintDoubleData2
+#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintDoubleData2"
 #else
-#define btGeneric6DofSpring2ConstraintData2		btGeneric6DofSpring2ConstraintData
-#define btGeneric6DofSpring2ConstraintDataName	"btGeneric6DofSpring2ConstraintData"
-#endif //BT_USE_DOUBLE_PRECISION
+#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintData
+#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 enum RotateOrder
 {
-	RO_XYZ=0,
+	RO_XYZ = 0,
 	RO_XZY,
 	RO_YXZ,
 	RO_YZX,
@@ -69,9 +67,9 @@ enum RotateOrder
 class btRotationalLimitMotor2
 {
 public:
-// upper < lower means free
-// upper == lower means locked
-// upper > lower means limited
+	// upper < lower means free
+	// upper == lower means locked
+	// upper > lower means limited
 	btScalar m_loLimit;
 	btScalar m_hiLimit;
 	btScalar m_bounce;
@@ -79,95 +77,92 @@ public:
 	btScalar m_stopCFM;
 	btScalar m_motorERP;
 	btScalar m_motorCFM;
-	bool     m_enableMotor;
+	bool m_enableMotor;
 	btScalar m_targetVelocity;
 	btScalar m_maxMotorForce;
-	bool     m_servoMotor;
+	bool m_servoMotor;
 	btScalar m_servoTarget;
-	bool     m_enableSpring;
+	bool m_enableSpring;
 	btScalar m_springStiffness;
-	bool     m_springStiffnessLimited;
+	bool m_springStiffnessLimited;
 	btScalar m_springDamping;
-	bool     m_springDampingLimited;
+	bool m_springDampingLimited;
 	btScalar m_equilibriumPoint;
 
 	btScalar m_currentLimitError;
 	btScalar m_currentLimitErrorHi;
 	btScalar m_currentPosition;
-	int      m_currentLimit;
+	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_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 = 6.0f;
+		m_servoMotor = false;
+		m_servoTarget = 0;
+		m_enableSpring = false;
+		m_springStiffness = 0;
 		m_springStiffnessLimited = false;
-		m_springDamping          = 0;
-		m_springDampingLimited   = false;
-		m_equilibriumPoint       = 0;
+		m_springDamping = 0;
+		m_springDampingLimited = false;
+		m_equilibriumPoint = 0;
 
-		m_currentLimitError   = 0;
+		m_currentLimitError = 0;
 		m_currentLimitErrorHi = 0;
-		m_currentPosition     = 0;
-		m_currentLimit        = 0;
+		m_currentPosition = 0;
+		m_currentLimit = 0;
 	}
 
-	btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot)
+	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_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_springStiffnessLimited = limot.m_springStiffnessLimited;
-		m_springDamping          = limot.m_springDamping;
-		m_springDampingLimited   = limot.m_springDampingLimited;
-		m_equilibriumPoint       = limot.m_equilibriumPoint;
+		m_springDamping = limot.m_springDamping;
+		m_springDampingLimited = limot.m_springDampingLimited;
+		m_equilibriumPoint = limot.m_equilibriumPoint;
 
-		m_currentLimitError   = limot.m_currentLimitError;
+		m_currentLimitError = limot.m_currentLimitError;
 		m_currentLimitErrorHi = limot.m_currentLimitErrorHi;
-		m_currentPosition     = limot.m_currentPosition;
-		m_currentLimit        = limot.m_currentLimit;
+		m_currentPosition = limot.m_currentPosition;
+		m_currentLimit = limot.m_currentLimit;
 	}
 
-
 	bool isLimited()
 	{
-		if(m_loLimit > m_hiLimit) return false;
+		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
+	// upper < lower means free
+	// upper == lower means locked
+	// upper > lower means limited
 	btVector3 m_lowerLimit;
 	btVector3 m_upperLimit;
 	btVector3 m_bounce;
@@ -175,14 +170,14 @@ public:
 	btVector3 m_stopCFM;
 	btVector3 m_motorERP;
 	btVector3 m_motorCFM;
-	bool      m_enableMotor[3];
-	bool      m_servoMotor[3];
-	bool      m_enableSpring[3];
+	bool m_enableMotor[3];
+	bool m_servoMotor[3];
+	bool m_enableSpring[3];
 	btVector3 m_servoTarget;
 	btVector3 m_springStiffness;
-	bool      m_springStiffnessLimited[3];
+	bool m_springStiffnessLimited[3];
 	btVector3 m_springDamping;
-	bool      m_springDampingLimited[3];
+	bool m_springDampingLimited[3];
 	btVector3 m_equilibriumPoint;
 	btVector3 m_targetVelocity;
 	btVector3 m_maxMotorForce;
@@ -190,69 +185,69 @@ public:
 	btVector3 m_currentLimitError;
 	btVector3 m_currentLimitErrorHi;
 	btVector3 m_currentLinearDiff;
-	int       m_currentLimit[3];
+	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_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_enableMotor[i] = false;
+			m_servoMotor[i] = false;
+			m_enableSpring[i] = false;
+			m_servoTarget[i] = btScalar(0.f);
+			m_springStiffness[i] = btScalar(0.f);
 			m_springStiffnessLimited[i] = false;
-			m_springDamping[i]          = btScalar(0.f);
-			m_springDampingLimited[i]   = false;
-			m_equilibriumPoint[i]       = btScalar(0.f);
-			m_targetVelocity[i]         = btScalar(0.f);
-			m_maxMotorForce[i]          = btScalar(0.f);
-			
-			m_currentLimit[i]     = 0;
+			m_springDamping[i] = btScalar(0.f);
+			m_springDampingLimited[i] = false;
+			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 )
+	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_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;
+		m_currentLinearDiff = other.m_currentLinearDiff;
 
-		for(int i=0; i < 3; i++) 
+		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_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_springStiffnessLimited[i] = other.m_springStiffnessLimited[i];
-			m_springDamping[i]          = other.m_springDamping[i];
-			m_springDampingLimited[i]   = other.m_springDampingLimited[i];
-			m_equilibriumPoint[i]       = other.m_equilibriumPoint[i];
-			m_targetVelocity[i]         = other.m_targetVelocity[i];
-			m_maxMotorForce[i]          = other.m_maxMotorForce[i];
+			m_springDamping[i] = other.m_springDamping[i];
+			m_springDampingLimited[i] = other.m_springDampingLimited[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];
+			m_currentLimit[i] = other.m_currentLimit[i];
 		}
 	}
 
@@ -269,15 +264,15 @@ 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
+	BT_6DOF_FLAGS_ERP_MOTO2 = 8,
+	BT_6DOF_FLAGS_USE_INFINITE_ERROR = (1<<16)
 };
-#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis
-
+#define BT_6DOF_FLAGS_AXIS_SHIFT2 4  // bits per axis
 
-ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class)
+btGeneric6DofSpring2Constraint : public btTypedConstraint
 {
 protected:
-
 	btTransform m_frameInA;
 	btTransform m_frameInB;
 
@@ -290,53 +285,43 @@ protected:
 	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&)
+	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=(const 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);
+	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);
+	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);
 
 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);
+	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 void getInfo1(btConstraintInfo1 * info);
+	virtual void getInfo2(btConstraintInfo2 * info);
 	virtual int calculateSerializeBufferSize() const;
 	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 
@@ -344,19 +329,19 @@ public:
 	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(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; }
+	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& getCalculatedTransformB() const { return m_calculatedTransformB; }
 
-	const btTransform & getFrameOffsetA() const { return m_frameInA; }
-	const btTransform & getFrameOffsetB() const { return m_frameInB; }
+	const btTransform& getFrameOffsetA() const { return m_frameInA; }
+	const btTransform& getFrameOffsetB() const { return m_frameInB; }
 
-	btTransform & getFrameOffsetA() { return m_frameInA; }
-	btTransform & getFrameOffsetB() { 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]; }
@@ -367,58 +352,58 @@ public:
 	// 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 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 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 getLinearUpperLimit(btVector3 & linearUpper) { linearUpper = m_linearLimits.m_upperLimit; }
 
 	void setAngularLowerLimit(const btVector3& angularLower)
 	{
-		for(int i = 0; i < 3; i++) 
+		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++) 
+		for (int i = 0; i < 3; i++)
 			m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]);
 	}
 
-	void getAngularLowerLimit(btVector3& angularLower)
+	void getAngularLowerLimit(btVector3 & angularLower)
 	{
-		for(int i = 0; i < 3; i++) 
+		for (int i = 0; i < 3; i++)
 			angularLower[i] = m_angularLimits[i].m_loLimit;
 	}
 
-	void getAngularLowerLimitReversed(btVector3& angularLower)
+	void getAngularLowerLimitReversed(btVector3 & angularLower)
 	{
-		for(int i = 0; i < 3; i++)
+		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++)
+		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++)
+		for (int i = 0; i < 3; i++)
 			m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]);
 	}
 
-	void getAngularUpperLimit(btVector3& angularUpper)
+	void getAngularUpperLimit(btVector3 & angularUpper)
 	{
-		for(int i = 0; i < 3; i++)
+		for (int i = 0; i < 3; i++)
 			angularUpper[i] = m_angularLimits[i].m_hiLimit;
 	}
 
-	void getAngularUpperLimitReversed(btVector3& angularUpper)
+	void getAngularUpperLimitReversed(btVector3 & angularUpper)
 	{
-		for(int i = 0; i < 3; i++)
+		for (int i = 0; i < 3; i++)
 			angularUpper[i] = -m_angularLimits[i].m_loLimit;
 	}
 
@@ -426,7 +411,7 @@ public:
 
 	void setLimit(int axis, btScalar lo, btScalar hi)
 	{
-		if(axis<3)
+		if (axis < 3)
 		{
 			m_linearLimits.m_lowerLimit[axis] = lo;
 			m_linearLimits.m_upperLimit[axis] = hi;
@@ -435,14 +420,14 @@ public:
 		{
 			lo = btNormalizeAngle(lo);
 			hi = btNormalizeAngle(hi);
-			m_angularLimits[axis-3].m_loLimit = lo;
-			m_angularLimits[axis-3].m_hiLimit = 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)
+		if (axis < 3)
 		{
 			m_linearLimits.m_lowerLimit[axis] = lo;
 			m_linearLimits.m_upperLimit[axis] = hi;
@@ -451,46 +436,53 @@ public:
 		{
 			lo = btNormalizeAngle(lo);
 			hi = btNormalizeAngle(hi);
-			m_angularLimits[axis-3].m_hiLimit = -lo;
-			m_angularLimits[axis-3].m_loLimit = -hi;
+			m_angularLimits[axis - 3].m_hiLimit = -lo;
+			m_angularLimits[axis - 3].m_loLimit = -hi;
 		}
 	}
 
 	bool isLimited(int limitIndex)
 	{
-		if(limitIndex<3)
+		if (limitIndex < 3)
 		{
 			return m_linearLimits.isLimited(limitIndex);
 		}
-		return m_angularLimits[limitIndex-3].isLimited();
+		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 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 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, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely
-	void setDamping(int index, btScalar damping, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up
-	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 setStiffness(int index, btScalar stiffness, bool limitIfNeeded = true);  // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely
+	void setDamping(int index, btScalar damping, bool limitIfNeeded = true);      // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up
+	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). 
+	//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;
-};
 
+	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);
+};
 
 struct btGeneric6DofSpring2ConstraintData
 {
@@ -511,12 +503,12 @@ struct btGeneric6DofSpring2ConstraintData
 	btVector3FloatData m_linearSpringStiffness;
 	btVector3FloatData m_linearSpringDamping;
 	btVector3FloatData m_linearEquilibriumPoint;
-	char               m_linearEnableMotor[4];
-	char               m_linearServoMotor[4];
-	char               m_linearEnableSpring[4];
-	char               m_linearSpringStiffnessLimited[4];
-	char               m_linearSpringDampingLimited[4];
-	char               m_padding1[4];
+	char m_linearEnableMotor[4];
+	char m_linearServoMotor[4];
+	char m_linearEnableSpring[4];
+	char m_linearSpringStiffnessLimited[4];
+	char m_linearSpringDampingLimited[4];
+	char m_padding1[4];
 
 	btVector3FloatData m_angularUpperLimit;
 	btVector3FloatData m_angularLowerLimit;
@@ -531,13 +523,13 @@ struct btGeneric6DofSpring2ConstraintData
 	btVector3FloatData m_angularSpringStiffness;
 	btVector3FloatData m_angularSpringDamping;
 	btVector3FloatData m_angularEquilibriumPoint;
-	char               m_angularEnableMotor[4];
-	char               m_angularServoMotor[4];
-	char               m_angularEnableSpring[4];
-	char               m_angularSpringStiffnessLimited[4];
-	char               m_angularSpringDampingLimited[4];
+	char m_angularEnableMotor[4];
+	char m_angularServoMotor[4];
+	char m_angularEnableSpring[4];
+	char m_angularSpringStiffnessLimited[4];
+	char m_angularSpringDampingLimited[4];
 
-	int                m_rotateOrder;
+	int m_rotateOrder;
 };
 
 struct btGeneric6DofSpring2ConstraintDoubleData2
@@ -559,12 +551,12 @@ struct btGeneric6DofSpring2ConstraintDoubleData2
 	btVector3DoubleData m_linearSpringStiffness;
 	btVector3DoubleData m_linearSpringDamping;
 	btVector3DoubleData m_linearEquilibriumPoint;
-	char                m_linearEnableMotor[4];
-	char                m_linearServoMotor[4];
-	char                m_linearEnableSpring[4];
-	char                m_linearSpringStiffnessLimited[4];
-	char                m_linearSpringDampingLimited[4];
-	char                m_padding1[4];
+	char m_linearEnableMotor[4];
+	char m_linearServoMotor[4];
+	char m_linearEnableSpring[4];
+	char m_linearSpringStiffnessLimited[4];
+	char m_linearSpringDampingLimited[4];
+	char m_padding1[4];
 
 	btVector3DoubleData m_angularUpperLimit;
 	btVector3DoubleData m_angularLowerLimit;
@@ -579,13 +571,13 @@ struct btGeneric6DofSpring2ConstraintDoubleData2
 	btVector3DoubleData m_angularSpringStiffness;
 	btVector3DoubleData m_angularSpringDamping;
 	btVector3DoubleData m_angularEquilibriumPoint;
-	char                m_angularEnableMotor[4];
-	char                m_angularServoMotor[4];
-	char                m_angularEnableSpring[4];
-	char                m_angularSpringStiffnessLimited[4];
-	char                m_angularSpringDampingLimited[4];
+	char m_angularEnableMotor[4];
+	char m_angularServoMotor[4];
+	char m_angularEnableSpring[4];
+	char m_angularSpringStiffnessLimited[4];
+	char m_angularSpringDampingLimited[4];
 
-	int                 m_rotateOrder;
+	int m_rotateOrder;
 };
 
 SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const
@@ -596,79 +588,80 @@ SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSi
 SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer;
-	btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
+	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++)
+	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_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_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++)
+	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;
-		dof->m_angularSpringStiffnessLimited[i] = i < 3 ? ( m_angularLimits[i].m_springStiffnessLimited ? 1 : 0 ) : 0;
-		dof->m_angularSpringDampingLimited[i]   = i < 3 ? ( m_angularLimits[i].m_springDampingLimited ? 1 : 0 ) : 0;
+		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;
+		dof->m_angularSpringStiffnessLimited[i] = i < 3 ? (m_angularLimits[i].m_springStiffnessLimited ? 1 : 0) : 0;
+		dof->m_angularSpringDampingLimited[i] = i < 3 ? (m_angularLimits[i].m_springDampingLimited ? 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++)
+	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_linearSpringStiffnessLimited[i] = i < 3 ? ( m_linearLimits.m_springStiffnessLimited[i] ? 1 : 0 ) : 0;
-		dof->m_linearSpringDampingLimited[i]   = i < 3 ? ( m_linearLimits.m_springDampingLimited[i] ? 1 : 0 ) : 0;
+		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_linearSpringStiffnessLimited[i] = i < 3 ? (m_linearLimits.m_springStiffnessLimited[i] ? 1 : 0) : 0;
+		dof->m_linearSpringDampingLimited[i] = i < 3 ? (m_linearLimits.m_springDampingLimited[i] ? 1 : 0) : 0;
 	}
 
 	dof->m_rotateOrder = m_rotateOrder;
 
+	dof->m_padding1[0] = 0;
+	dof->m_padding1[1] = 0;
+	dof->m_padding1[2] = 0;
+	dof->m_padding1[3] = 0;
+
 	return btGeneric6DofSpring2ConstraintDataName;
 }
 
-
-
-
-
-#endif //BT_GENERIC_6DOF_CONSTRAINT_H
+#endif  //BT_GENERIC_6DOF_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp
index 6f765884ec0..8baf52bcd19 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp
@@ -17,26 +17,23 @@ subject to the following restrictions:
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
 
-
-btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
+btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
 	: btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
 {
-    init();
+	init();
 }
 
-
 btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
-        : btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB)
+	: btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB)
 {
-    init();
+	init();
 }
 
-
 void btGeneric6DofSpringConstraint::init()
 {
 	m_objectType = D6_SPRING_CONSTRAINT_TYPE;
 
-	for(int i = 0; i < 6; i++)
+	for (int i = 0; i < 6; i++)
 	{
 		m_springEnabled[i] = false;
 		m_equilibriumPoint[i] = btScalar(0.f);
@@ -45,12 +42,11 @@ void btGeneric6DofSpringConstraint::init()
 	}
 }
 
-
 void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
 {
 	btAssert((index >= 0) && (index < 6));
 	m_springEnabled[index] = onOff;
-	if(index < 3)
+	if (index < 3)
 	{
 		m_linearLimits.m_enableMotor[index] = onOff;
 	}
@@ -60,44 +56,38 @@ void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
 	}
 }
 
-
-
 void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
 {
 	btAssert((index >= 0) && (index < 6));
 	m_springStiffness[index] = stiffness;
 }
 
-
 void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping)
 {
 	btAssert((index >= 0) && (index < 6));
 	m_springDamping[index] = damping;
 }
 
-
 void btGeneric6DofSpringConstraint::setEquilibriumPoint()
 {
 	calculateTransforms();
 	int i;
 
-	for( i = 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 	{
 		m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
 	}
-	for(i = 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 	{
 		m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
 	}
 }
 
-
-
 void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
 {
 	btAssert((index >= 0) && (index < 6));
 	calculateTransforms();
-	if(index < 3)
+	if (index < 3)
 	{
 		m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
 	}
@@ -113,15 +103,14 @@ void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val)
 	m_equilibriumPoint[index] = val;
 }
 
-
 void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
 {
 	// it is assumed that calculateTransforms() have been called before this call
 	int i;
 	//btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
-	for(i = 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 	{
-		if(m_springEnabled[i])
+		if (m_springEnabled[i])
 		{
 			// get current position of constraint
 			btScalar currPos = m_calculatedLinearDiff[i];
@@ -130,28 +119,27 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf
 			// spring force is (delta * m_stiffness) according to Hooke's Law
 			btScalar force = delta * m_springStiffness[i];
 			btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations);
-			m_linearLimits.m_targetVelocity[i] =  velFactor * force;
-			m_linearLimits.m_maxMotorForce[i] =  btFabs(force) / info->fps;
+			m_linearLimits.m_targetVelocity[i] = velFactor * force;
+			m_linearLimits.m_maxMotorForce[i] = btFabs(force);
 		}
 	}
-	for(i = 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 	{
-		if(m_springEnabled[i + 3])
+		if (m_springEnabled[i + 3])
 		{
 			// get current position of constraint
 			btScalar currPos = m_calculatedAxisAngleDiff[i];
 			// calculate difference
-			btScalar delta = currPos - m_equilibriumPoint[i+3];
+			btScalar delta = currPos - m_equilibriumPoint[i + 3];
 			// spring force is (-delta * m_stiffness) according to Hooke's Law
-			btScalar force = -delta * m_springStiffness[i+3];
-			btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations);
+			btScalar force = -delta * m_springStiffness[i + 3];
+			btScalar velFactor = info->fps * m_springDamping[i + 3] / btScalar(info->m_numIterations);
 			m_angularLimits[i].m_targetVelocity = velFactor * force;
-			m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
+			m_angularLimits[i].m_maxMotorForce = btFabs(force);
 		}
 	}
 }
 
-
 void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
 {
 	// this will be called by constraint solver at the constraint setup stage
@@ -161,25 +149,21 @@ void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
 	btGeneric6DofConstraint::getInfo2(info);
 }
 
-
-void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
+void btGeneric6DofSpringConstraint::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
+	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]);
+	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();
+	calculateTransforms();
 }
-
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
index dac59c6889d..02b9d4d05d1 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
@@ -16,20 +16,17 @@ subject to the following restrictions:
 #ifndef BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
 #define BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
 
-
 #include "LinearMath/btVector3.h"
 #include "btTypedConstraint.h"
 #include "btGeneric6DofConstraint.h"
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btGeneric6DofSpringConstraintData2		btGeneric6DofSpringConstraintDoubleData2
-#define btGeneric6DofSpringConstraintDataName	"btGeneric6DofSpringConstraintDoubleData2"
+#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintDoubleData2
+#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintDoubleData2"
 #else
-#define btGeneric6DofSpringConstraintData2		btGeneric6DofSpringConstraintData
-#define btGeneric6DofSpringConstraintDataName	"btGeneric6DofSpringConstraintData"
-#endif //BT_USE_DOUBLE_PRECISION
-
-
+#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintData
+#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 /// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF
 
@@ -41,101 +38,98 @@ subject to the following restrictions:
 /// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] )
 /// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] )
 
-ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
+ATTRIBUTE_ALIGNED16(class)
+btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
 {
 protected:
-	bool		m_springEnabled[6];
-	btScalar	m_equilibriumPoint[6];
-	btScalar	m_springStiffness[6];
-	btScalar	m_springDamping[6]; // between 0 and 1 (1 == no damping)
+	bool m_springEnabled[6];
+	btScalar m_equilibriumPoint[6];
+	btScalar m_springStiffness[6];
+	btScalar m_springDamping[6];  // between 0 and 1 (1 == no damping)
 	void init();
-	void internalUpdateSprings(btConstraintInfo2* info);
-public: 
-	
+	void internalUpdateSprings(btConstraintInfo2 * info);
+
+public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-    btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
-    btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
+
+	btGeneric6DofSpringConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
+	btGeneric6DofSpringConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
 	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();           // 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);
 
 	bool isSpringEnabled(int index) const
 	{
-	    return m_springEnabled[index];
+		return m_springEnabled[index];
 	}
 
 	btScalar getStiffness(int index) const
 	{
-	    return m_springStiffness[index];
+		return m_springStiffness[index];
 	}
 
 	btScalar getDamping(int index) const
 	{
-	    return m_springDamping[index];
+		return m_springDamping[index];
 	}
 
 	btScalar getEquilibriumPoint(int index) const
 	{
-	    return m_equilibriumPoint[index];
+		return m_equilibriumPoint[index];
 	}
 
-	virtual void setAxis( const btVector3& axis1, const btVector3& axis2);
+	virtual void setAxis(const btVector3& axis1, const btVector3& axis2);
 
-	virtual void getInfo2 (btConstraintInfo2* info);
+	virtual void getInfo2(btConstraintInfo2 * info);
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
-
 struct btGeneric6DofSpringConstraintData
 {
-	btGeneric6DofConstraintData	m_6dofData;
-	
-	int			m_springEnabled[6];
-	float		m_equilibriumPoint[6];
-	float		m_springStiffness[6];
-	float		m_springDamping[6];
+	btGeneric6DofConstraintData m_6dofData;
+
+	int m_springEnabled[6];
+	float m_equilibriumPoint[6];
+	float m_springStiffness[6];
+	float m_springDamping[6];
 };
 
 struct btGeneric6DofSpringConstraintDoubleData2
 {
-	btGeneric6DofConstraintDoubleData2	m_6dofData;
-	
-	int			m_springEnabled[6];
-	double		m_equilibriumPoint[6];
-	double		m_springStiffness[6];
-	double		m_springDamping[6];
-};
+	btGeneric6DofConstraintDoubleData2 m_6dofData;
 
+	int m_springEnabled[6];
+	double m_equilibriumPoint[6];
+	double m_springStiffness[6];
+	double m_springDamping[6];
+};
 
-SIMD_FORCE_INLINE	int	btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btGeneric6DofSpringConstraintData2);
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char* btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btGeneric6DofSpringConstraintData2* dof = (btGeneric6DofSpringConstraintData2*)dataBuffer;
-	btGeneric6DofConstraint::serialize(&dof->m_6dofData,serializer);
+	btGeneric6DofConstraint::serialize(&dof->m_6dofData, serializer);
 
 	int i;
-	for (i=0;i<6;i++)
+	for (i = 0; i < 6; i++)
 	{
 		dof->m_equilibriumPoint[i] = m_equilibriumPoint[i];
 		dof->m_springDamping[i] = m_springDamping[i];
-		dof->m_springEnabled[i] = m_springEnabled[i]? 1 : 0;
+		dof->m_springEnabled[i] = m_springEnabled[i] ? 1 : 0;
 		dof->m_springStiffness[i] = m_springStiffness[i];
 	}
 	return btGeneric6DofSpringConstraintDataName;
 }
 
-#endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
-
+#endif  // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp
index 4be2aabe4d3..6507e1a0a74 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp
@@ -13,54 +13,49 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #include "btHinge2Constraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
 
-
-
 // constructor
 // anchor, axis1 and axis2 are in world coordinate system
 // axis1 must be orthogonal to axis2
 btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
-: btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(),RO_XYZ),
- m_anchor(anchor),
- m_axis1(axis1),
- m_axis2(axis2)
+	: btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), RO_XYZ),
+	  m_anchor(anchor),
+	  m_axis1(axis1),
+	  m_axis2(axis2)
 {
 	// build frame basis
 	// 6DOF constraint uses Euler angles and to define limits
 	// it is assumed that rotational order is :
 	// Z - first, allowed limits are (-PI,PI);
-	// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number 
+	// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
 	// used to prevent constraint from instability on poles;
 	// new position of X, allowed limits are (-PI,PI);
 	// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
 	// Build the frame in world coordinate system first
 	btVector3 zAxis = axis1.normalize();
 	btVector3 xAxis = axis2.normalize();
-	btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system
+	btVector3 yAxis = zAxis.cross(xAxis);  // 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]);
+	frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
+								 xAxis[1], yAxis[1], zAxis[1],
+								 xAxis[2], yAxis[2], zAxis[2]);
 	frameInW.setOrigin(anchor);
 	// now get constraint frame in local coordinate systems
 	m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
 	m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
 	// sei limits
 	setLinearLowerLimit(btVector3(0.f, 0.f, -1.f));
-	setLinearUpperLimit(btVector3(0.f, 0.f,  1.f));
+	setLinearUpperLimit(btVector3(0.f, 0.f, 1.f));
 	// like front wheels of a car
-	setAngularLowerLimit(btVector3(1.f,  0.f, -SIMD_HALF_PI * 0.5f)); 
-	setAngularUpperLimit(btVector3(-1.f, 0.f,  SIMD_HALF_PI * 0.5f));
+	setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f));
+	setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f));
 	// enable suspension
 	enableSpring(2, true);
 	setStiffness(2, SIMD_PI * SIMD_PI * 4.f);
 	setDamping(2, 0.01f);
 	setEquilibriumPoint();
 }
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h
index 06a8e3ecd14..95f604a8906 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h
@@ -16,32 +16,30 @@ subject to the following restrictions:
 #ifndef BT_HINGE2_CONSTRAINT_H
 #define BT_HINGE2_CONSTRAINT_H
 
-
-
 #include "LinearMath/btVector3.h"
 #include "btTypedConstraint.h"
 #include "btGeneric6DofSpring2Constraint.h"
 
-
-
 // Constraint similar to ODE Hinge2 Joint
 // has 3 degrees of frredom:
 // 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2)
 // 1 translational (along axis Z) with suspension spring
 
-ATTRIBUTE_ALIGNED16(class) btHinge2Constraint : public btGeneric6DofSpring2Constraint
+ATTRIBUTE_ALIGNED16(class)
+btHinge2Constraint : public btGeneric6DofSpring2Constraint
 {
 protected:
-	btVector3	m_anchor;
-	btVector3	m_axis1;
-	btVector3	m_axis2;
+	btVector3 m_anchor;
+	btVector3 m_axis1;
+	btVector3 m_axis2;
+
 public:
-		BT_DECLARE_ALIGNED_ALLOCATOR();
-		
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	// constructor
 	// anchor, axis1 and axis2 are in world coordinate system
 	// axis1 must be orthogonal to axis2
-    btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2);
+	btHinge2Constraint(btRigidBody & rbA, btRigidBody & rbB, btVector3 & anchor, btVector3 & axis1, btVector3 & axis2);
 	// access
 	const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
 	const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
@@ -51,10 +49,7 @@ public:
 	btScalar getAngle2() { return getAngle(0); }
 	// limits
 	void setUpperLimit(btScalar ang1max) { setAngularUpperLimit(btVector3(-1.f, 0.f, ang1max)); }
-	void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3( 1.f, 0.f, ang1min)); }
+	void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3(1.f, 0.f, ang1min)); }
 };
 
-
-
-#endif // BT_HINGE2_CONSTRAINT_H
-
+#endif  // BT_HINGE2_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
index 76a1509471e..aa6f69000d2 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btHingeConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
@@ -21,8 +20,6 @@ subject to the following restrictions:
 #include <new>
 #include "btSolverBody.h"
 
-
-
 //#define HINGE_USE_OBSOLETE_SOLVER false
 #define HINGE_USE_OBSOLETE_SOLVER false
 
@@ -30,59 +27,60 @@ subject to the following restrictions:
 
 #ifndef __SPU__
 
-
-
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
-									 const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
-									 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB,
+									 const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA)
+	: btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB),
 #ifdef _BT_USE_CENTER_LIMIT_
-									 m_limit(),
+	  m_limit(),
 #endif
-									 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_normalCFM(0),
-									 m_normalERP(0),
-									 m_stopCFM(0),
-									 m_stopERP(0)
+	  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_normalCFM(0),
+	  m_normalERP(0),
+	  m_stopCFM(0),
+	  m_stopERP(0)
 {
 	m_rbAFrame.getOrigin() = pivotInA;
-	
+
 	// since no frame is given, assume this to be zero angle and just pick rb transform axis
 	btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
 
 	btVector3 rbAxisA2;
 	btScalar projection = axisInA.dot(rbAxisA1);
-	if (projection >= 1.0f - SIMD_EPSILON) {
+	if (projection >= 1.0f - SIMD_EPSILON)
+	{
 		rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
 		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
-	} else if (projection <= -1.0f + SIMD_EPSILON) {
+	}
+	else if (projection <= -1.0f + SIMD_EPSILON)
+	{
 		rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
-		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);      
-	} else {
+		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
+	}
+	else
+	{
 		rbAxisA2 = axisInA.cross(rbAxisA1);
 		rbAxisA1 = rbAxisA2.cross(axisInA);
 	}
 
-	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
-									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
-									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+	m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
+								   rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
+								   rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
+
+	btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
+	btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
+	btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
 
-	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
-	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
-	btVector3 rbAxisB2 =  axisInB.cross(rbAxisB1);	
-	
 	m_rbBFrame.getOrigin() = pivotInB;
-	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
-									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
-									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
-	
-#ifndef	_BT_USE_CENTER_LIMIT_
+	m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
+								   rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
+								   rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
+
+#ifndef _BT_USE_CENTER_LIMIT_
 	//start with free
 	m_lowerLimit = btScalar(1.0f);
 	m_upperLimit = btScalar(-1.0f);
@@ -94,47 +92,44 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
 	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }
 
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
-:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA)
+	: btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
 #ifdef _BT_USE_CENTER_LIMIT_
-m_limit(),
+	  m_limit(),
 #endif
-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_normalCFM(0),
-m_normalERP(0),
-m_stopCFM(0),
-m_stopERP(0)
+	  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_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
 	// fixed axis in worldspace
 	btVector3 rbAxisA1, rbAxisA2;
 	btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
 
 	m_rbAFrame.getOrigin() = pivotInA;
-	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
-									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
-									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+	m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
+								   rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
+								   rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
 
 	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
 
-	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
-	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
+	btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
+	btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
 	btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
 
-
 	m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
-	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
-									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
-									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
-	
-#ifndef	_BT_USE_CENTER_LIMIT_
+	m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
+								   rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
+								   rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
+
+#ifndef _BT_USE_CENTER_LIMIT_
 	//start with free
 	m_lowerLimit = btScalar(1.0f);
 	m_upperLimit = btScalar(-1.0f);
@@ -146,26 +141,24 @@ m_stopERP(0)
 	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }
 
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
-								     const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
-:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB,
+									 const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
+	: btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB), m_rbAFrame(rbAFrame), m_rbBFrame(rbBFrame),
 #ifdef _BT_USE_CENTER_LIMIT_
-m_limit(),
+	  m_limit(),
 #endif
-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_normalCFM(0),
-m_normalERP(0),
-m_stopCFM(0),
-m_stopERP(0)
+	  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_normalCFM(0),
+	  m_normalERP(0),
+	  m_stopCFM(0),
+	  m_stopERP(0)
 {
-#ifndef	_BT_USE_CENTER_LIMIT_
+#ifndef _BT_USE_CENTER_LIMIT_
 	//start with free
 	m_lowerLimit = btScalar(1.0f);
 	m_upperLimit = btScalar(-1.0f);
@@ -175,30 +168,28 @@ m_stopERP(0)
 	m_solveLimit = false;
 #endif
 	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
-}			
-
-
+}
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
-:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
+	: btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_rbAFrame(rbAFrame), m_rbBFrame(rbAFrame),
 #ifdef _BT_USE_CENTER_LIMIT_
-m_limit(),
+	  m_limit(),
 #endif
-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_normalCFM(0),
-m_normalERP(0),
-m_stopCFM(0),
-m_stopERP(0)
+	  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_normalCFM(0),
+	  m_normalERP(0),
+	  m_stopCFM(0),
+	  m_stopERP(0)
 {
 	///not providing rigidbody B means implicitly using worldspace for body B
 
 	m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
-#ifndef	_BT_USE_CENTER_LIMIT_
+#ifndef _BT_USE_CENTER_LIMIT_
 	//start with free
 	m_lowerLimit = btScalar(1.0f);
 	m_upperLimit = btScalar(-1.0f);
@@ -210,9 +201,7 @@ m_stopERP(0)
 	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }
 
-
-
-void	btHingeConstraint::buildJacobian()
+void btHingeConstraint::buildJacobian()
 {
 	if (m_useSolveConstraintObsolete)
 	{
@@ -221,8 +210,8 @@ void	btHingeConstraint::buildJacobian()
 
 		if (!m_angularOnly)
 		{
-			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
+			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
 			btVector3 relPos = pivotBInW - pivotAInW;
 
 			btVector3 normal[3];
@@ -232,23 +221,23 @@ void	btHingeConstraint::buildJacobian()
 			}
 			else
 			{
-				normal[0].setValue(btScalar(1.0),0,0);
+				normal[0].setValue(btScalar(1.0), 0, 0);
 			}
 
 			btPlaneSpace1(normal[0], normal[1], normal[2]);
 
-			for (int i=0;i<3;i++)
+			for (int i = 0; i < 3; i++)
 			{
 				new (&m_jac[i]) btJacobianEntry(
-				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-				pivotAInW - m_rbA.getCenterOfMassPosition(),
-				pivotBInW - m_rbB.getCenterOfMassPosition(),
-				normal[i],
-				m_rbA.getInvInertiaDiagLocal(),
-				m_rbA.getInvMass(),
-				m_rbB.getInvInertiaDiagLocal(),
-				m_rbB.getInvMass());
+					m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+					m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+					pivotAInW - m_rbA.getCenterOfMassPosition(),
+					pivotBInW - m_rbB.getCenterOfMassPosition(),
+					normal[i],
+					m_rbA.getInvInertiaDiagLocal(),
+					m_rbA.getInvMass(),
+					m_rbB.getInvInertiaDiagLocal(),
+					m_rbB.getInvMass());
 			}
 		}
 
@@ -258,60 +247,55 @@ void	btHingeConstraint::buildJacobian()
 		//this is unused for now, it's a todo
 		btVector3 jointAxis0local;
 		btVector3 jointAxis1local;
-		
-		btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
+
+		btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2), jointAxis0local, jointAxis1local);
 
 		btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
 		btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
 		btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
-			
-		new (&m_jacAng[0])	btJacobianEntry(jointAxis0,
-			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbA.getInvInertiaDiagLocal(),
-			m_rbB.getInvInertiaDiagLocal());
-
-		new (&m_jacAng[1])	btJacobianEntry(jointAxis1,
-			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbA.getInvInertiaDiagLocal(),
-			m_rbB.getInvInertiaDiagLocal());
-
-		new (&m_jacAng[2])	btJacobianEntry(hingeAxisWorld,
-			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbA.getInvInertiaDiagLocal(),
-			m_rbB.getInvInertiaDiagLocal());
-
-			// clear accumulator
-			m_accLimitImpulse = btScalar(0.);
-
-			// test angular limit
-			testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
 
-		//Compute K = J*W*J' for hinge axis
-		btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
-		m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
-							 getRigidBodyB().computeAngularImpulseDenominator(axisA));
+		new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
+										   m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+										   m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+										   m_rbA.getInvInertiaDiagLocal(),
+										   m_rbB.getInvInertiaDiagLocal());
 
-	}
-}
+		new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
+										   m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+										   m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+										   m_rbA.getInvInertiaDiagLocal(),
+										   m_rbB.getInvInertiaDiagLocal());
 
+		new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld,
+										   m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+										   m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+										   m_rbA.getInvInertiaDiagLocal(),
+										   m_rbB.getInvInertiaDiagLocal());
 
-#endif //__SPU__
+		// clear accumulator
+		m_accLimitImpulse = btScalar(0.);
 
+		// test angular limit
+		testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 
-static inline btScalar btNormalizeAnglePositive(btScalar angle)
-{
-  return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI));
+		//Compute K = J*W*J' for hinge axis
+		btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+		m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+						   getRigidBodyB().computeAngularImpulseDenominator(axisA));
+	}
 }
 
+#endif  //__SPU__
 
+static inline btScalar btNormalizeAnglePositive(btScalar angle)
+{
+	return btFmod(btFmod(angle, btScalar(2.0 * SIMD_PI)) + btScalar(2.0 * SIMD_PI), btScalar(2.0 * SIMD_PI));
+}
 
 static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
 {
 	btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) -
-	btNormalizeAnglePositive(accAngle)));
+																btNormalizeAnglePositive(accAngle)));
 	return result;
 }
 
@@ -320,41 +304,36 @@ static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
 	btScalar tol(0.3);
 	btScalar result = btShortestAngularDistance(accAngle, curAngle);
 
-	  if (btFabs(result) > tol)
+	if (btFabs(result) > tol)
 		return curAngle;
-	  else
+	else
 		return accAngle + result;
 
 	return curAngle;
 }
 
-
 btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle()
 {
 	btScalar hingeAngle = getHingeAngle();
-	m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle);
+	m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, hingeAngle);
 	return m_accumulatedAngle;
 }
-void	btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
+void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
 {
-	m_accumulatedAngle  = accAngle;
+	m_accumulatedAngle = accAngle;
 }
 
 void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info)
 {
 	//update m_accumulatedAngle
 	btScalar curHingeAngle = getHingeAngle();
-	m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle);
+	m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, curHingeAngle);
 
 	btHingeConstraint::getInfo1(info);
-	
 }
 
-
 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
 {
-
-
 	if (m_useSolveConstraintObsolete)
 	{
 		info->m_numConstraintRows = 0;
@@ -362,17 +341,16 @@ void btHingeConstraint::getInfo1(btConstraintInfo1* info)
 	}
 	else
 	{
-		info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
-		info->nub = 1; 
+		info->m_numConstraintRows = 5;  // Fixed 3 linear + 2 angular
+		info->nub = 1;
 		//always add the row, to avoid computation (data is not available yet)
 		//prepare constraint
-		testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
-		if(getSolveLimit() || getEnableAngularMotor())
+		testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
+		if (getSolveLimit() || getEnableAngularMotor())
 		{
-			info->m_numConstraintRows++; // limit 3rd anguar as well
-			info->nub--; 
+			info->m_numConstraintRows++;  // limit 3rd anguar as well
+			info->nub--;
 		}
-
 	}
 }
 
@@ -386,41 +364,38 @@ void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
 	else
 	{
 		//always add the 'limit' row, to avoid computation (data is not available yet)
-		info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
-		info->nub = 0; 
+		info->m_numConstraintRows = 6;  // Fixed 3 linear + 2 angular
+		info->nub = 0;
 	}
 }
 
-void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
+void btHingeConstraint::getInfo2(btConstraintInfo2* info)
 {
-	if(m_useOffsetForConstraintFrame)
+	if (m_useOffsetForConstraintFrame)
 	{
-		getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+		getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity());
 	}
 	else
 	{
-		getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+		getInfo2Internal(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity());
 	}
 }
 
-
-void	btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+void btHingeConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
 {
 	///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now
-	testLimit(transA,transB);
+	testLimit(transA, transB);
 
-	getInfo2Internal(info,transA,transB,angVelA,angVelB);
+	getInfo2Internal(info, transA, transB, angVelA, angVelB);
 }
 
-
-void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
 {
-
 	btAssert(!m_useSolveConstraintObsolete);
 	int i, skip = info->rowskip;
 	// transforms in world space
-	btTransform trA = transA*m_rbAFrame;
-	btTransform trB = transB*m_rbBFrame;
+	btTransform trA = transA * m_rbAFrame;
+	btTransform trB = transB * m_rbBFrame;
 	// pivot point
 	btVector3 pivotAInW = trA.getOrigin();
 	btVector3 pivotBInW = trB.getOrigin();
@@ -448,7 +423,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 			info->m_constraintError[i*skip]=0.f;
 		}
 	}
-#endif //#if 0
+#endif  //#if 0
 	// linear (all fixed)
 
 	if (!m_angularOnly)
@@ -460,10 +435,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 		info->m_J2linearAxis[0] = -1;
 		info->m_J2linearAxis[skip + 1] = -1;
 		info->m_J2linearAxis[2 * skip + 2] = -1;
-	}	
-
-
-
+	}
 
 	btVector3 a1 = pivotAInW - transA.getOrigin();
 	{
@@ -471,22 +443,22 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
 		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
 		btVector3 a1neg = -a1;
-		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
 	btVector3 a2 = pivotBInW - transB.getOrigin();
 	{
 		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
 		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
 		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
-		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
 	// linear RHS
 	btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
 
-    btScalar k = info->fps * normalErp;
+	btScalar k = info->fps * normalErp;
 	if (!m_angularOnly)
 	{
-		for(i = 0; i < 3; i++)
+		for (i = 0; i < 3; i++)
 		{
 			info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
 		}
@@ -504,9 +476,9 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 	// get 2 orthos to hinge axis (X, Y)
 	btVector3 p = trA.getBasis().getColumn(0);
 	btVector3 q = trA.getBasis().getColumn(1);
-	// set the two hinge angular rows 
-    int s3 = 3 * info->rowskip;
-    int s4 = 4 * info->rowskip;
+	// set the two hinge angular rows
+	int s3 = 3 * info->rowskip;
+	int s4 = 4 * info->rowskip;
 
 	info->m_J1angularAxis[s3 + 0] = p[0];
 	info->m_J1angularAxis[s3 + 1] = p[1];
@@ -521,185 +493,172 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 	info->m_J2angularAxis[s4 + 0] = -q[0];
 	info->m_J2angularAxis[s4 + 1] = -q[1];
 	info->m_J2angularAxis[s4 + 2] = -q[2];
-    // compute the right hand side of the constraint equation. set relative
-    // body velocities along p and q to bring the hinge back into alignment.
-    // if ax1,ax2 are the unit length hinge axes as computed from body1 and
-    // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
-    // if `theta' is the angle between ax1 and ax2, we need an angular velocity
-    // along u to cover angle erp*theta in one step :
-    //   |angular_velocity| = angle/time = erp*theta / stepsize
-    //                      = (erp*fps) * theta
-    //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
-    //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
-    // ...as ax1 and ax2 are unit length. if theta is smallish,
-    // theta ~= sin(theta), so
-    //    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.
-    btVector3 ax2 = trB.getBasis().getColumn(2);
+	// compute the right hand side of the constraint equation. set relative
+	// body velocities along p and q to bring the hinge back into alignment.
+	// if ax1,ax2 are the unit length hinge axes as computed from body1 and
+	// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+	// if `theta' is the angle between ax1 and ax2, we need an angular velocity
+	// along u to cover angle erp*theta in one step :
+	//   |angular_velocity| = angle/time = erp*theta / stepsize
+	//                      = (erp*fps) * theta
+	//    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+	//                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+	// ...as ax1 and ax2 are unit length. if theta is smallish,
+	// theta ~= sin(theta), so
+	//    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.
+	btVector3 ax2 = trB.getBasis().getColumn(2);
 	btVector3 u = ax1.cross(ax2);
 	info->m_constraintError[s3] = k * u.dot(p);
 	info->m_constraintError[s4] = k * u.dot(q);
 	// check angular limits
-	int nrow = 4; // last filled row
+	int nrow = 4;  // last filled row
 	int srow;
 	btScalar limit_err = btScalar(0.0);
 	int limit = 0;
-	if(getSolveLimit())
+	if (getSolveLimit())
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
-	limit_err = m_limit.getCorrection() * m_referenceSign;
+#ifdef _BT_USE_CENTER_LIMIT_
+		limit_err = m_limit.getCorrection() * m_referenceSign;
 #else
-	limit_err = m_correction * m_referenceSign;
+		limit_err = m_correction * m_referenceSign;
 #endif
-	limit = (limit_err > btScalar(0.0)) ? 1 : 2;
-
+		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
 	}
 	// if the hinge has joint limits or motor, add in the extra row
-	int powered = 0;
-	if(getEnableAngularMotor())
-	{
-		powered = 1;
-	}
-	if(limit || powered) 
+	bool powered = getEnableAngularMotor();
+	if (limit || powered)
 	{
 		nrow++;
 		srow = nrow * info->rowskip;
-		info->m_J1angularAxis[srow+0] = ax1[0];
-		info->m_J1angularAxis[srow+1] = ax1[1];
-		info->m_J1angularAxis[srow+2] = ax1[2];
+		info->m_J1angularAxis[srow + 0] = ax1[0];
+		info->m_J1angularAxis[srow + 1] = ax1[1];
+		info->m_J1angularAxis[srow + 2] = ax1[2];
 
-		info->m_J2angularAxis[srow+0] = -ax1[0];
-		info->m_J2angularAxis[srow+1] = -ax1[1];
-		info->m_J2angularAxis[srow+2] = -ax1[2];
+		info->m_J2angularAxis[srow + 0] = -ax1[0];
+		info->m_J2angularAxis[srow + 1] = -ax1[1];
+		info->m_J2angularAxis[srow + 2] = -ax1[2];
 
 		btScalar lostop = getLowerLimit();
 		btScalar histop = getUpperLimit();
-		if(limit && (lostop == histop))
+		if (limit && (lostop == histop))
 		{  // the joint motor is ineffective
-			powered = 0;
+			powered = false;
 		}
 		info->m_constraintError[srow] = btScalar(0.0f);
 		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
-		if(powered)
+		if (powered)
 		{
-			if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
+			if (m_flags & BT_HINGE_FLAGS_CFM_NORM)
 			{
 				info->cfm[srow] = m_normalCFM;
 			}
 			btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
 			info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
-			info->m_lowerLimit[srow] = - m_maxMotorImpulse;
-			info->m_upperLimit[srow] =   m_maxMotorImpulse;
+			info->m_lowerLimit[srow] = -m_maxMotorImpulse;
+			info->m_upperLimit[srow] = m_maxMotorImpulse;
 		}
-		if(limit)
+		if (limit)
 		{
 			k = info->fps * currERP;
 			info->m_constraintError[srow] += k * limit_err;
-			if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
+			if (m_flags & BT_HINGE_FLAGS_CFM_STOP)
 			{
 				info->cfm[srow] = m_stopCFM;
 			}
-			if(lostop == histop) 
+			if (lostop == histop)
 			{
 				// limited low and high simultaneously
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
-			else if(limit == 1) 
-			{ // low limit
+			else if (limit == 1)
+			{  // low limit
 				info->m_lowerLimit[srow] = 0;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
-			else 
-			{ // high limit
+			else
+			{  // high limit
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = 0;
 			}
 			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 			btScalar bounce = m_limit.getRelaxationFactor();
 #else
 			btScalar bounce = m_relaxationFactor;
 #endif
-			if(bounce > btScalar(0.0))
+			if (bounce > btScalar(0.0))
 			{
 				btScalar vel = angVelA.dot(ax1);
 				vel -= angVelB.dot(ax1);
 				// only apply bounce if the velocity is incoming, and if the
 				// resulting c[] exceeds what we already have.
-				if(limit == 1)
-				{	// low limit
-					if(vel < 0)
+				if (limit == 1)
+				{  // low limit
+					if (vel < 0)
 					{
 						btScalar newc = -bounce * vel;
-						if(newc > info->m_constraintError[srow])
+						if (newc > info->m_constraintError[srow])
 						{
 							info->m_constraintError[srow] = newc;
 						}
 					}
 				}
 				else
-				{	// high limit - all those computations are reversed
-					if(vel > 0)
+				{  // high limit - all those computations are reversed
+					if (vel > 0)
 					{
 						btScalar newc = -bounce * vel;
-						if(newc < info->m_constraintError[srow])
+						if (newc < info->m_constraintError[srow])
 						{
 							info->m_constraintError[srow] = newc;
 						}
 					}
 				}
 			}
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 			info->m_constraintError[srow] *= m_limit.getBiasFactor();
 #else
 			info->m_constraintError[srow] *= m_biasFactor;
 #endif
-		} // if(limit)
-	} // if angular limit or powered
+		}  // if(limit)
+	}      // if angular limit or powered
 }
 
-
-void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
+void btHingeConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
 {
 	m_rbAFrame = frameA;
 	m_rbBFrame = frameB;
 	buildJacobian();
 }
 
-
-void	btHingeConstraint::updateRHS(btScalar	timeStep)
+void btHingeConstraint::updateRHS(btScalar timeStep)
 {
 	(void)timeStep;
-
 }
 
-
-
-
 btScalar btHingeConstraint::getHingeAngle()
 {
-	return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	return getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 }
 
-btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
+btScalar btHingeConstraint::getHingeAngle(const btTransform& transA, const btTransform& transB)
 {
-	const btVector3 refAxis0  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
-	const btVector3 refAxis1  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
+	const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
+	const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
 	const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
-//	btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+	//	btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
 	btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
 	return m_referenceSign * angle;
 }
 
-
-
-void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
+void btHingeConstraint::testLimit(const btTransform& transA, const btTransform& transB)
 {
 	// Compute limit information
-	m_hingeAngle = getHingeAngle(transA,transB);
-#ifdef	_BT_USE_CENTER_LIMIT_
+	m_hingeAngle = getHingeAngle(transA, transB);
+#ifdef _BT_USE_CENTER_LIMIT_
 	m_limit.test(m_hingeAngle);
 #else
 	m_correction = btScalar(0.);
@@ -713,7 +672,7 @@ void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& t
 			m_correction = (m_lowerLimit - m_hingeAngle);
 			m_limitSign = 1.0f;
 			m_solveLimit = true;
-		} 
+		}
 		else if (m_hingeAngle >= m_upperLimit)
 		{
 			m_correction = m_upperLimit - m_hingeAngle;
@@ -725,7 +684,6 @@ void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& t
 	return;
 }
 
-
 static btVector3 vHinge(0, 0, btScalar(1));
 
 void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
@@ -735,14 +693,15 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
 	qConstraint.normalize();
 
 	// extract "pure" hinge component
-	btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
+	btVector3 vNoHinge = quatRotate(qConstraint, vHinge);
+	vNoHinge.normalize();
 	btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
 	btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
 	qHinge.normalize();
 
 	// compute angular target, clamped to limits
 	btScalar targetAngle = qHinge.getAngle();
-	if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
+	if (targetAngle > SIMD_PI)  // long way around. flip quat and recalculate.
 	{
 		qHinge = -(qHinge);
 		targetAngle = qHinge.getAngle();
@@ -755,7 +714,7 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
 
 void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
 {
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 	m_limit.fit(targetAngle);
 #else
 	if (m_lowerLimit < m_upperLimit)
@@ -767,20 +726,18 @@ void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
 	}
 #endif
 	// compute angular velocity
-	btScalar curAngle  = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 	btScalar dAngle = targetAngle - curAngle;
 	m_motorTargetVelocity = dAngle / dt;
 }
 
-
-
-void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
 {
 	btAssert(!m_useSolveConstraintObsolete);
 	int i, s = info->rowskip;
 	// transforms in world space
-	btTransform trA = transA*m_rbAFrame;
-	btTransform trB = transB*m_rbBFrame;
+	btTransform trA = transA * m_rbAFrame;
+	btTransform trB = transB * m_rbBFrame;
 	// pivot point
 //	btVector3 pivotAInW = trA.getOrigin();
 //	btVector3 pivotBInW = trB.getOrigin();
@@ -793,11 +750,11 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
 	btScalar miS = miA + miB;
 	btScalar factA, factB;
-	if(miS > btScalar(0.f))
+	if (miS > btScalar(0.f))
 	{
 		factA = miB / miS;
 	}
-	else 
+	else
 	{
 		factA = btScalar(0.5f);
 	}
@@ -807,15 +764,21 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	btVector3 ax1A = trA.getBasis().getColumn(2);
 	btVector3 ax1B = trB.getBasis().getColumn(2);
 	btVector3 ax1 = ax1A * factA + ax1B * factB;
+	if (ax1.length2()<SIMD_EPSILON)
+	{
+		factA=0.f;
+		factB=1.f;
+		ax1 = ax1A * factA + ax1B * factB;
+	}
 	ax1.normalize();
-	// fill first 3 rows 
+	// fill first 3 rows
 	// we want: velA + wA x relA == velB + wB x relB
 	btTransform bodyA_trans = transA;
 	btTransform bodyB_trans = transB;
 	int s0 = 0;
 	int s1 = s;
 	int s2 = s * 2;
-	int nrow = 2; // last filled row
+	int nrow = 2;  // last filled row
 	btVector3 tmpA, tmpB, relA, relB, p, q;
 	// get vector from bodyB to frameB in WCS
 	relB = trB.getOrigin() - bodyB_trans.getOrigin();
@@ -834,7 +797,7 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	// now choose average ortho to hinge axis
 	p = orthoB * factA + orthoA * factB;
 	btScalar len2 = p.length2();
-	if(len2 > SIMD_EPSILON)
+	if (len2 > SIMD_EPSILON)
 	{
 		p /= btSqrt(len2);
 	}
@@ -847,44 +810,44 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	// fill three rows
 	tmpA = relA.cross(p);
 	tmpB = relB.cross(p);
-    for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
-    for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
+	for (i = 0; i < 3; i++) info->m_J1angularAxis[s0 + i] = tmpA[i];
+	for (i = 0; i < 3; i++) info->m_J2angularAxis[s0 + i] = -tmpB[i];
 	tmpA = relA.cross(q);
 	tmpB = relB.cross(q);
-	if(hasStaticBody && getSolveLimit())
-	{ // to make constraint between static and dynamic objects more rigid
+	if (hasStaticBody && getSolveLimit())
+	{  // to make constraint between static and dynamic objects more rigid
 		// remove wA (or wB) from equation if angular limit is hit
 		tmpB *= factB;
 		tmpA *= factA;
 	}
-	for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
-    for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
+	for (i = 0; i < 3; i++) info->m_J1angularAxis[s1 + i] = tmpA[i];
+	for (i = 0; i < 3; i++) info->m_J2angularAxis[s1 + i] = -tmpB[i];
 	tmpA = relA.cross(ax1);
 	tmpB = relB.cross(ax1);
-	if(hasStaticBody)
-	{ // to make constraint between static and dynamic objects more rigid
+	if (hasStaticBody)
+	{  // to make constraint between static and dynamic objects more rigid
 		// remove wA (or wB) from equation
 		tmpB *= factB;
 		tmpA *= factA;
 	}
-	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];
+	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 normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : 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++) info->m_J1linearAxis[s0+i] = p[i];
-		for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
-		for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
+		for (i = 0; i < 3; i++) info->m_J1linearAxis[s0 + i] = p[i];
+		for (i = 0; i < 3; i++) info->m_J1linearAxis[s1 + i] = q[i];
+		for (i = 0; i < 3; i++) info->m_J1linearAxis[s2 + i] = ax1[i];
 
-		for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i];
-		for (i=0; i<3; i++) info->m_J2linearAxis[s1+i] = -q[i];
-		for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -ax1[i];
+		for (i = 0; i < 3; i++) info->m_J2linearAxis[s0 + i] = -p[i];
+		for (i = 0; i < 3; i++) info->m_J2linearAxis[s1 + i] = -q[i];
+		for (i = 0; i < 3; i++) info->m_J2linearAxis[s2 + i] = -ax1[i];
+
+		// compute three elements of right hand side
 
-	// compute three elements of right hand side
-	
 		btScalar rhs = k * p.dot(ofs);
 		info->m_constraintError[s0] = rhs;
 		rhs = k * q.dot(ofs);
@@ -929,150 +892,144 @@ 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 * normalErp;//??
+	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);
 #endif
 	// check angular limits
-	nrow = 4; // last filled row
+	nrow = 4;  // last filled row
 	int srow;
 	btScalar limit_err = btScalar(0.0);
 	int limit = 0;
-	if(getSolveLimit())
+	if (getSolveLimit())
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
-	limit_err = m_limit.getCorrection() * m_referenceSign;
+#ifdef _BT_USE_CENTER_LIMIT_
+		limit_err = m_limit.getCorrection() * m_referenceSign;
 #else
-	limit_err = m_correction * m_referenceSign;
+		limit_err = m_correction * m_referenceSign;
 #endif
-	limit = (limit_err > btScalar(0.0)) ? 1 : 2;
-
+		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
 	}
 	// if the hinge has joint limits or motor, add in the extra row
-	int powered = 0;
-	if(getEnableAngularMotor())
-	{
-		powered = 1;
-	}
-	if(limit || powered) 
+	bool powered = getEnableAngularMotor();
+	if (limit || powered)
 	{
 		nrow++;
 		srow = nrow * info->rowskip;
-		info->m_J1angularAxis[srow+0] = ax1[0];
-		info->m_J1angularAxis[srow+1] = ax1[1];
-		info->m_J1angularAxis[srow+2] = ax1[2];
+		info->m_J1angularAxis[srow + 0] = ax1[0];
+		info->m_J1angularAxis[srow + 1] = ax1[1];
+		info->m_J1angularAxis[srow + 2] = ax1[2];
 
-		info->m_J2angularAxis[srow+0] = -ax1[0];
-		info->m_J2angularAxis[srow+1] = -ax1[1];
-		info->m_J2angularAxis[srow+2] = -ax1[2];
+		info->m_J2angularAxis[srow + 0] = -ax1[0];
+		info->m_J2angularAxis[srow + 1] = -ax1[1];
+		info->m_J2angularAxis[srow + 2] = -ax1[2];
 
 		btScalar lostop = getLowerLimit();
 		btScalar histop = getUpperLimit();
-		if(limit && (lostop == histop))
+		if (limit && (lostop == histop))
 		{  // the joint motor is ineffective
-			powered = 0;
+			powered = false;
 		}
 		info->m_constraintError[srow] = btScalar(0.0f);
 		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
-		if(powered)
+		if (powered)
 		{
-			if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
+			if (m_flags & BT_HINGE_FLAGS_CFM_NORM)
 			{
 				info->cfm[srow] = m_normalCFM;
 			}
 			btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
 			info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
-			info->m_lowerLimit[srow] = - m_maxMotorImpulse;
-			info->m_upperLimit[srow] =   m_maxMotorImpulse;
+			info->m_lowerLimit[srow] = -m_maxMotorImpulse;
+			info->m_upperLimit[srow] = m_maxMotorImpulse;
 		}
-		if(limit)
+		if (limit)
 		{
 			k = info->fps * currERP;
 			info->m_constraintError[srow] += k * limit_err;
-			if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
+			if (m_flags & BT_HINGE_FLAGS_CFM_STOP)
 			{
 				info->cfm[srow] = m_stopCFM;
 			}
-			if(lostop == histop) 
+			if (lostop == histop)
 			{
 				// limited low and high simultaneously
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
-			else if(limit == 1) 
-			{ // low limit
+			else if (limit == 1)
+			{  // low limit
 				info->m_lowerLimit[srow] = 0;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
-			else 
-			{ // high limit
+			else
+			{  // high limit
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = 0;
 			}
 			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 			btScalar bounce = m_limit.getRelaxationFactor();
 #else
 			btScalar bounce = m_relaxationFactor;
 #endif
-			if(bounce > btScalar(0.0))
+			if (bounce > btScalar(0.0))
 			{
 				btScalar vel = angVelA.dot(ax1);
 				vel -= angVelB.dot(ax1);
 				// only apply bounce if the velocity is incoming, and if the
 				// resulting c[] exceeds what we already have.
-				if(limit == 1)
-				{	// low limit
-					if(vel < 0)
+				if (limit == 1)
+				{  // low limit
+					if (vel < 0)
 					{
 						btScalar newc = -bounce * vel;
-						if(newc > info->m_constraintError[srow])
+						if (newc > info->m_constraintError[srow])
 						{
 							info->m_constraintError[srow] = newc;
 						}
 					}
 				}
 				else
-				{	// high limit - all those computations are reversed
-					if(vel > 0)
+				{  // high limit - all those computations are reversed
+					if (vel > 0)
 					{
 						btScalar newc = -bounce * vel;
-						if(newc < info->m_constraintError[srow])
+						if (newc < info->m_constraintError[srow])
 						{
 							info->m_constraintError[srow] = newc;
 						}
 					}
 				}
 			}
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 			info->m_constraintError[srow] *= m_limit.getBiasFactor();
 #else
 			info->m_constraintError[srow] *= m_biasFactor;
 #endif
-		} // if(limit)
-	} // if angular limit or powered
+		}  // if(limit)
+	}      // if angular limit or powered
 }
 
-
-///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///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 btHingeConstraint::setParam(int num, btScalar value, int axis)
 {
-	if((axis == -1) || (axis == 5))
+	if ((axis == -1) || (axis == 5))
 	{
-		switch(num)
-		{	
-			case BT_CONSTRAINT_STOP_ERP :
+		switch (num)
+		{
+			case BT_CONSTRAINT_STOP_ERP:
 				m_stopERP = value;
 				m_flags |= BT_HINGE_FLAGS_ERP_STOP;
 				break;
-			case BT_CONSTRAINT_STOP_CFM :
+			case BT_CONSTRAINT_STOP_CFM:
 				m_stopCFM = value;
 				m_flags |= BT_HINGE_FLAGS_CFM_STOP;
 				break;
-			case BT_CONSTRAINT_CFM :
+			case BT_CONSTRAINT_CFM:
 				m_normalCFM = value;
 				m_flags |= BT_HINGE_FLAGS_CFM_NORM;
 				break;
@@ -1080,7 +1037,7 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis)
 				m_normalERP = value;
 				m_flags |= BT_HINGE_FLAGS_ERP_NORM;
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
@@ -1091,22 +1048,22 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis)
 }
 
 ///return the local value of parameter
-btScalar btHingeConstraint::getParam(int num, int axis) const 
+btScalar btHingeConstraint::getParam(int num, int axis) const
 {
 	btScalar retVal = 0;
-	if((axis == -1) || (axis == 5))
+	if ((axis == -1) || (axis == 5))
 	{
-		switch(num)
-		{	
-			case BT_CONSTRAINT_STOP_ERP :
+		switch (num)
+		{
+			case BT_CONSTRAINT_STOP_ERP:
 				btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
 				retVal = m_stopERP;
 				break;
-			case BT_CONSTRAINT_STOP_CFM :
+			case BT_CONSTRAINT_STOP_CFM:
 				btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
 				retVal = m_stopCFM;
 				break;
-			case BT_CONSTRAINT_CFM :
+			case BT_CONSTRAINT_CFM:
 				btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
 				retVal = m_normalCFM;
 				break;
@@ -1114,7 +1071,7 @@ btScalar btHingeConstraint::getParam(int num, int axis) const
 				btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM);
 				retVal = m_normalERP;
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
@@ -1124,5 +1081,3 @@ btScalar btHingeConstraint::getParam(int num, int axis) const
 	}
 	return retVal;
 }
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
index f26e72105ba..c7509e30afb 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
@@ -20,7 +20,6 @@ subject to the following restrictions:
 
 #define _BT_USE_CENTER_LIMIT_ 1
 
-
 #include "LinearMath/btVector3.h"
 #include "btJacobianEntry.h"
 #include "btTypedConstraint.h"
@@ -28,14 +27,12 @@ subject to the following restrictions:
 class btRigidBody;
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btHingeConstraintData	btHingeConstraintDoubleData2 //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version
-#define btHingeConstraintDataName	"btHingeConstraintDoubleData2" 
+#define btHingeConstraintData btHingeConstraintDoubleData2  //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version
+#define btHingeConstraintDataName "btHingeConstraintDoubleData2"
 #else
-#define btHingeConstraintData	btHingeConstraintFloatData
-#define btHingeConstraintDataName	"btHingeConstraintFloatData"
-#endif //BT_USE_DOUBLE_PRECISION
-
-
+#define btHingeConstraintData btHingeConstraintFloatData
+#define btHingeConstraintDataName "btHingeConstraintFloatData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 enum btHingeFlags
 {
@@ -45,89 +42,83 @@ enum btHingeFlags
 	BT_HINGE_FLAGS_ERP_NORM = 8
 };
 
-
 /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 /// axis defines the orientation of the hinge axis
-ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class)
+btHingeConstraint : public btTypedConstraint
 {
 #ifdef IN_PARALLELL_SOLVER
 public:
 #endif
-	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
-	btJacobianEntry	m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
+	btJacobianEntry m_jac[3];     //3 orthogonal linear constraints
+	btJacobianEntry m_jacAng[3];  //2 orthogonal angular constraints+ 1 for limit/motor
 
-	btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTransform m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransform m_rbBFrame;
 
-	btScalar	m_motorTargetVelocity;
-	btScalar	m_maxMotorImpulse;
-
+	btScalar m_motorTargetVelocity;
+	btScalar m_maxMotorImpulse;
 
-#ifdef	_BT_USE_CENTER_LIMIT_
-	btAngularLimit	m_limit;
+#ifdef _BT_USE_CENTER_LIMIT_
+	btAngularLimit m_limit;
 #else
-	btScalar	m_lowerLimit;	
-	btScalar	m_upperLimit;	
-	btScalar	m_limitSign;
-	btScalar	m_correction;
+	btScalar m_lowerLimit;
+	btScalar m_upperLimit;
+	btScalar m_limitSign;
+	btScalar m_correction;
 
-	btScalar	m_limitSoftness; 
-	btScalar	m_biasFactor; 
-	btScalar	m_relaxationFactor; 
+	btScalar m_limitSoftness;
+	btScalar m_biasFactor;
+	btScalar m_relaxationFactor;
 
-	bool		m_solveLimit;
+	bool m_solveLimit;
 #endif
 
-	btScalar	m_kHinge;
-
+	btScalar m_kHinge;
 
-	btScalar	m_accLimitImpulse;
-	btScalar	m_hingeAngle;
-	btScalar	m_referenceSign;
+	btScalar m_accLimitImpulse;
+	btScalar m_hingeAngle;
+	btScalar m_referenceSign;
 
-	bool		m_angularOnly;
-	bool		m_enableAngularMotor;
-	bool		m_useSolveConstraintObsolete;
-	bool		m_useOffsetForConstraintFrame;
-	bool		m_useReferenceFrameA;
+	bool m_angularOnly;
+	bool m_enableAngularMotor;
+	bool m_useSolveConstraintObsolete;
+	bool m_useOffsetForConstraintFrame;
+	bool m_useReferenceFrameA;
 
-	btScalar	m_accMotorImpulse;
+	btScalar m_accMotorImpulse;
 
-	int			m_flags;
-	btScalar	m_normalCFM;
-	btScalar	m_normalERP;
-	btScalar	m_stopCFM;
-	btScalar	m_stopERP;
+	int m_flags;
+	btScalar m_normalCFM;
+	btScalar m_normalERP;
+	btScalar m_stopCFM;
+	btScalar m_stopERP;
 
-	
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false);
 
-	btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false);
-	
-	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
+	btHingeConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& pivotInA, const btVector3& pivotInB, const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA = false);
+
+	btHingeConstraint(btRigidBody & rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA = false);
 
-	btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
+	btHingeConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
 
+	btHingeConstraint(btRigidBody & rbA, const btTransform& rbAFrame, bool useReferenceFrameA = false);
 
-	virtual void	buildJacobian();
+	virtual void buildJacobian();
 
-	virtual void getInfo1 (btConstraintInfo1* info);
+	virtual void getInfo1(btConstraintInfo1 * info);
 
-	void getInfo1NonVirtual(btConstraintInfo1* info);
+	void getInfo1NonVirtual(btConstraintInfo1 * info);
 
-	virtual void getInfo2 (btConstraintInfo2* info);
+	virtual void getInfo2(btConstraintInfo2 * info);
 
-	void	getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
+	void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB);
 
-	void	getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
-	void	getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
-		
+	void getInfo2Internal(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB);
+	void getInfo2InternalUsingFrameOffset(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB);
 
-	void	updateRHS(btScalar	timeStep);
+	void updateRHS(btScalar timeStep);
 
 	const btRigidBody& getRigidBodyA() const
 	{
@@ -138,19 +129,19 @@ public:
 		return m_rbB;
 	}
 
-	btRigidBody& getRigidBodyA()	
-	{		
-		return m_rbA;	
-	}	
+	btRigidBody& getRigidBodyA()
+	{
+		return m_rbA;
+	}
 
-	btRigidBody& getRigidBodyB()	
-	{		
-		return m_rbB;	
+	btRigidBody& getRigidBodyB()
+	{
+		return m_rbB;
 	}
 
 	btTransform& getFrameOffsetA()
 	{
-	return m_rbAFrame;
+		return m_rbAFrame;
 	}
 
 	btTransform& getFrameOffsetB()
@@ -159,15 +150,15 @@ public:
 	}
 
 	void setFrames(const btTransform& frameA, const btTransform& frameB);
-	
-	void	setAngularOnly(bool angularOnly)
+
+	void setAngularOnly(bool angularOnly)
 	{
 		m_angularOnly = angularOnly;
 	}
 
-	void	enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse)
+	void enableAngularMotor(bool enableMotor, btScalar targetVelocity, btScalar maxMotorImpulse)
 	{
-		m_enableAngularMotor  = enableMotor;
+		m_enableAngularMotor = enableMotor;
 		m_motorTargetVelocity = targetVelocity;
 		m_maxMotorImpulse = maxMotorImpulse;
 	}
@@ -175,29 +166,28 @@ public:
 	// extra motor API, including ability to set a target rotation (as opposed to angular velocity)
 	// note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
 	//       maintain a given angular target.
-	void enableMotor(bool enableMotor) 	{ m_enableAngularMotor = enableMotor; }
+	void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; }
 	void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
 	void setMotorTargetVelocity(btScalar motorTargetVelocity) { m_motorTargetVelocity = motorTargetVelocity; }
-	void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
+	void setMotorTarget(const btQuaternion& qAinB, btScalar dt);  // qAinB is rotation of body A wrt body B.
 	void setMotorTarget(btScalar targetAngle, btScalar dt);
 
-
-	void	setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+	void setLimit(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 		m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor);
 #else
 		m_lowerLimit = btNormalizeAngle(low);
 		m_upperLimit = btNormalizeAngle(high);
-		m_limitSoftness =  _softness;
+		m_limitSoftness = _softness;
 		m_biasFactor = _biasFactor;
 		m_relaxationFactor = _relaxationFactor;
 #endif
 	}
-	
+
 	btScalar getLimitSoftness() const
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 		return m_limit.getSoftness();
 #else
 		return m_limitSoftness;
@@ -206,7 +196,7 @@ public:
 
 	btScalar getLimitBiasFactor() const
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 		return m_limit.getBiasFactor();
 #else
 		return m_biasFactor;
@@ -215,112 +205,110 @@ public:
 
 	btScalar getLimitRelaxationFactor() const
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 		return m_limit.getRelaxationFactor();
 #else
 		return m_relaxationFactor;
 #endif
 	}
 
-	void	setAxis(btVector3& axisInA)
+	void setAxis(btVector3 & axisInA)
 	{
 		btVector3 rbAxisA1, rbAxisA2;
 		btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
 		btVector3 pivotInA = m_rbAFrame.getOrigin();
-//		m_rbAFrame.getOrigin() = pivotInA;
-		m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
-										rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
-										rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+		//		m_rbAFrame.getOrigin() = pivotInA;
+		m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
+									   rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
+									   rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
 
 		btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;
 
-		btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
-		btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
+		btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
+		btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
 		btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
 
 		m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA));
 
-		m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
-										rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
-										rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
+		m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
+									   rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
+									   rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
 		m_rbBFrame.getBasis() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis();
-
 	}
 
-    bool hasLimit() const {
-#ifdef  _BT_USE_CENTER_LIMIT_
-        return m_limit.getHalfRange() > 0;
+	bool hasLimit() const
+	{
+#ifdef _BT_USE_CENTER_LIMIT_
+		return m_limit.getHalfRange() > 0;
 #else
-        return m_lowerLimit <= m_upperLimit;
+		return m_lowerLimit <= m_upperLimit;
 #endif
-    }
+	}
 
-	btScalar	getLowerLimit() const
+	btScalar getLowerLimit() const
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
-	return m_limit.getLow();
+#ifdef _BT_USE_CENTER_LIMIT_
+		return m_limit.getLow();
 #else
-	return m_lowerLimit;
+		return m_lowerLimit;
 #endif
 	}
 
-	btScalar	getUpperLimit() const
+	btScalar getUpperLimit() const
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
-	return m_limit.getHigh();
-#else		
-	return m_upperLimit;
+#ifdef _BT_USE_CENTER_LIMIT_
+		return m_limit.getHigh();
+#else
+		return m_upperLimit;
 #endif
 	}
 
-
 	///The getHingeAngle gives the hinge angle in range [-PI,PI]
 	btScalar getHingeAngle();
 
-	btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
+	btScalar getHingeAngle(const btTransform& transA, const btTransform& transB);
 
-	void testLimit(const btTransform& transA,const btTransform& transB);
+	void testLimit(const btTransform& transA, const btTransform& transB);
 
-
-	const btTransform& getAFrame() const { return m_rbAFrame; };	
+	const btTransform& getAFrame() const { return m_rbAFrame; };
 	const btTransform& getBFrame() const { return m_rbBFrame; };
 
-	btTransform& getAFrame() { return m_rbAFrame; };	
+	btTransform& getAFrame() { return m_rbAFrame; };
 	btTransform& getBFrame() { return m_rbBFrame; };
 
 	inline int getSolveLimit()
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
-	return m_limit.isLimit();
+#ifdef _BT_USE_CENTER_LIMIT_
+		return m_limit.isLimit();
 #else
-	return m_solveLimit;
+		return m_solveLimit;
 #endif
 	}
 
 	inline btScalar getLimitSign()
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
-	return m_limit.getSign();
+#ifdef _BT_USE_CENTER_LIMIT_
+		return m_limit.getSign();
 #else
 		return m_limitSign;
 #endif
 	}
 
-	inline bool getAngularOnly() 
-	{ 
-		return m_angularOnly; 
+	inline bool getAngularOnly()
+	{
+		return m_angularOnly;
 	}
-	inline bool getEnableAngularMotor() 
-	{ 
-		return m_enableAngularMotor; 
+	inline bool getEnableAngularMotor()
+	{
+		return m_enableAngularMotor;
 	}
-	inline btScalar getMotorTargetVelosity() 
-	{ 
-		return m_motorTargetVelocity; 
+	inline btScalar getMotorTargetVelocity()
+	{
+		return m_motorTargetVelocity;
 	}
-	inline btScalar getMaxMotorImpulse() 
-	{ 
-		return m_maxMotorImpulse; 
+	inline btScalar getMaxMotorImpulse()
+	{
+		return m_maxMotorImpulse;
 	}
 	// access for UseFrameOffset
 	bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
@@ -329,143 +317,132 @@ public:
 	bool getUseReferenceFrameA() const { return m_useReferenceFrameA; }
 	void setUseReferenceFrameA(bool useReferenceFrameA) { m_useReferenceFrameA = useReferenceFrameA; }
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///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 void setParam(int num, btScalar value, int axis = -1);
 	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const;
-	
-	virtual	int getFlags() const
+	virtual btScalar getParam(int num, int axis = -1) const;
+
+	virtual int getFlags() const
 	{
-  	    return m_flags;
+		return m_flags;
 	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
-
 //only for backward compatibility
 #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 ///this structure is not used, except for loading pre-2.82 .bullet files
-struct	btHingeConstraintDoubleData
+struct btHingeConstraintDoubleData
 {
-	btTypedConstraintData	m_typeConstraintData;
-	btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTypedConstraintData m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransformDoubleData m_rbBFrame;
-	int			m_useReferenceFrameA;
-	int			m_angularOnly;
-	int			m_enableAngularMotor;
-	float	m_motorTargetVelocity;
-	float	m_maxMotorImpulse;
-
-	float	m_lowerLimit;
-	float	m_upperLimit;
-	float	m_limitSoftness;
-	float	m_biasFactor;
-	float	m_relaxationFactor;
-
+	int m_useReferenceFrameA;
+	int m_angularOnly;
+	int m_enableAngularMotor;
+	float m_motorTargetVelocity;
+	float m_maxMotorImpulse;
+
+	float m_lowerLimit;
+	float m_upperLimit;
+	float m_limitSoftness;
+	float m_biasFactor;
+	float m_relaxationFactor;
 };
-#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+#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
+ATTRIBUTE_ALIGNED16(class)
+btHingeAccumulatedAngleConstraint : public btHingeConstraint
 {
 protected:
-	btScalar	m_accumulatedAngle;
-public:
+	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 )
+
+	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();
+		m_accumulatedAngle = getHingeAngle();
 	}
 
-	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false)
-	:btHingeConstraint(rbA,pivotInA,axisInA, useReferenceFrameA)
+	btHingeAccumulatedAngleConstraint(btRigidBody & rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA = false)
+		: btHingeConstraint(rbA, pivotInA, axisInA, useReferenceFrameA)
 	{
-		m_accumulatedAngle=getHingeAngle();
+		m_accumulatedAngle = getHingeAngle();
 	}
-	
-	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false)
-	:btHingeConstraint(rbA,rbB, rbAFrame, rbBFrame, useReferenceFrameA )
+
+	btHingeAccumulatedAngleConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false)
+		: btHingeConstraint(rbA, rbB, rbAFrame, rbBFrame, useReferenceFrameA)
 	{
-		m_accumulatedAngle=getHingeAngle();
+		m_accumulatedAngle = getHingeAngle();
 	}
 
-	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false)
-	:btHingeConstraint(rbA,rbAFrame, useReferenceFrameA )
+	btHingeAccumulatedAngleConstraint(btRigidBody & rbA, const btTransform& rbAFrame, bool useReferenceFrameA = false)
+		: btHingeConstraint(rbA, rbAFrame, useReferenceFrameA)
 	{
-		m_accumulatedAngle=getHingeAngle();
+		m_accumulatedAngle = getHingeAngle();
 	}
 	btScalar getAccumulatedHingeAngle();
-	void	setAccumulatedHingeAngle(btScalar accAngle);
-	virtual void getInfo1 (btConstraintInfo1* info);
-
+	void setAccumulatedHingeAngle(btScalar accAngle);
+	virtual void getInfo1(btConstraintInfo1 * info);
 };
 
-struct	btHingeConstraintFloatData
+struct btHingeConstraintFloatData
 {
-	btTypedConstraintData	m_typeConstraintData;
-	btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTypedConstraintData m_typeConstraintData;
+	btTransformFloatData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransformFloatData m_rbBFrame;
-	int			m_useReferenceFrameA;
-	int			m_angularOnly;
-	
-	int			m_enableAngularMotor;
-	float	m_motorTargetVelocity;
-	float	m_maxMotorImpulse;
-
-	float	m_lowerLimit;
-	float	m_upperLimit;
-	float	m_limitSoftness;
-	float	m_biasFactor;
-	float	m_relaxationFactor;
-
+	int m_useReferenceFrameA;
+	int m_angularOnly;
+
+	int m_enableAngularMotor;
+	float m_motorTargetVelocity;
+	float m_maxMotorImpulse;
+
+	float m_lowerLimit;
+	float m_upperLimit;
+	float m_limitSoftness;
+	float m_biasFactor;
+	float m_relaxationFactor;
 };
 
-
-
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btHingeConstraintDoubleData2
+struct btHingeConstraintDoubleData2
 {
-	btTypedConstraintDoubleData	m_typeConstraintData;
-	btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTypedConstraintDoubleData m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransformDoubleData m_rbBFrame;
-	int			m_useReferenceFrameA;
-	int			m_angularOnly;
-	int			m_enableAngularMotor;
-	double		m_motorTargetVelocity;
-	double		m_maxMotorImpulse;
-
-	double		m_lowerLimit;
-	double		m_upperLimit;
-	double		m_limitSoftness;
-	double		m_biasFactor;
-	double		m_relaxationFactor;
-	char	m_padding1[4];
-
+	int m_useReferenceFrameA;
+	int m_angularOnly;
+	int m_enableAngularMotor;
+	double m_motorTargetVelocity;
+	double m_maxMotorImpulse;
+
+	double m_lowerLimit;
+	double m_upperLimit;
+	double m_limitSoftness;
+	double m_biasFactor;
+	double m_relaxationFactor;
+	char m_padding1[4];
 };
 
-
-
-
-SIMD_FORCE_INLINE	int	btHingeConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btHingeConstraintData);
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
-	btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer);
+	btTypedConstraint::serialize(&hingeData->m_typeConstraintData, serializer);
 
 	m_rbAFrame.serialize(hingeData->m_rbAFrame);
 	m_rbBFrame.serialize(hingeData->m_rbBFrame);
@@ -475,7 +452,7 @@ SIMD_FORCE_INLINE	const char*	btHingeConstraint::serialize(void* dataBuffer, btS
 	hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
 	hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
 	hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 	hingeData->m_lowerLimit = float(m_limit.getLow());
 	hingeData->m_upperLimit = float(m_limit.getHigh());
 	hingeData->m_limitSoftness = float(m_limit.getSoftness());
@@ -489,7 +466,15 @@ SIMD_FORCE_INLINE	const char*	btHingeConstraint::serialize(void* dataBuffer, btS
 	hingeData->m_relaxationFactor = float(m_relaxationFactor);
 #endif
 
+	// Fill padding with zeros to appease msan.
+#ifdef BT_USE_DOUBLE_PRECISION
+	hingeData->m_padding1[0] = 0;
+	hingeData->m_padding1[1] = 0;
+	hingeData->m_padding1[2] = 0;
+	hingeData->m_padding1[3] = 0;
+#endif
+
 	return btHingeConstraintDataName;
 }
 
-#endif //BT_HINGECONSTRAINT_H
+#endif  //BT_HINGECONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
index 125580d1998..438456fe512 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
@@ -18,7 +18,6 @@ subject to the following restrictions:
 
 #include "LinearMath/btMatrix3x3.h"
 
-
 //notes:
 // Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
 // which makes the btJacobianEntry memory layout 16 bytes
@@ -27,25 +26,26 @@ subject to the following restrictions:
 /// Jacobian entry is an abstraction that allows to describe constraints
 /// it can be used in combination with a constraint solver
 /// Can be used to relate the effect of an impulse to the constraint error
-ATTRIBUTE_ALIGNED16(class) btJacobianEntry
+ATTRIBUTE_ALIGNED16(class)
+btJacobianEntry
 {
 public:
-	btJacobianEntry() {};
+	btJacobianEntry(){};
 	//constraint between two different rigidbodies
 	btJacobianEntry(
 		const btMatrix3x3& world2A,
 		const btMatrix3x3& world2B,
-		const btVector3& rel_pos1,const btVector3& rel_pos2,
+		const btVector3& rel_pos1, const btVector3& rel_pos2,
 		const btVector3& jointAxis,
-		const btVector3& inertiaInvA, 
+		const btVector3& inertiaInvA,
 		const btScalar massInvA,
 		const btVector3& inertiaInvB,
 		const btScalar massInvB)
-		:m_linearJointAxis(jointAxis)
+		: m_linearJointAxis(jointAxis)
 	{
-		m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
-		m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
-		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_aJ = world2A * (rel_pos1.cross(m_linearJointAxis));
+		m_bJ = world2B * (rel_pos2.cross(-m_linearJointAxis));
+		m_0MinvJt = inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
 		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
 
@@ -54,33 +54,31 @@ public:
 
 	//angular constraint between two different rigidbodies
 	btJacobianEntry(const btVector3& jointAxis,
-		const btMatrix3x3& world2A,
-		const btMatrix3x3& world2B,
-		const btVector3& inertiaInvA,
-		const btVector3& inertiaInvB)
-		:m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
+					const btMatrix3x3& world2A,
+					const btMatrix3x3& world2B,
+					const btVector3& inertiaInvA,
+					const btVector3& inertiaInvB)
+		: m_linearJointAxis(btVector3(btScalar(0.), btScalar(0.), btScalar(0.)))
 	{
-		m_aJ= world2A*jointAxis;
-		m_bJ = world2B*-jointAxis;
-		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_aJ = world2A * jointAxis;
+		m_bJ = world2B * -jointAxis;
+		m_0MinvJt = inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
-		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+		m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
 
 		btAssert(m_Adiag > btScalar(0.0));
 	}
 
 	//angular constraint between two different rigidbodies
 	btJacobianEntry(const btVector3& axisInA,
-		const btVector3& axisInB,
-		const btVector3& inertiaInvA,
-		const btVector3& inertiaInvB)
-		: m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
-		, m_aJ(axisInA)
-		, m_bJ(-axisInB)
+					const btVector3& axisInB,
+					const btVector3& inertiaInvA,
+					const btVector3& inertiaInvB)
+		: m_linearJointAxis(btVector3(btScalar(0.), btScalar(0.), btScalar(0.))), m_aJ(axisInA), m_bJ(-axisInB)
 	{
-		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_0MinvJt = inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
-		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+		m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
 
 		btAssert(m_Adiag > btScalar(0.0));
 	}
@@ -88,25 +86,25 @@ public:
 	//constraint on one rigidbody
 	btJacobianEntry(
 		const btMatrix3x3& world2A,
-		const btVector3& rel_pos1,const btVector3& rel_pos2,
+		const btVector3& rel_pos1, const btVector3& rel_pos2,
 		const btVector3& jointAxis,
-		const btVector3& inertiaInvA, 
+		const btVector3& inertiaInvA,
 		const btScalar massInvA)
-		:m_linearJointAxis(jointAxis)
+		: m_linearJointAxis(jointAxis)
 	{
-		m_aJ= world2A*(rel_pos1.cross(jointAxis));
-		m_bJ = world2A*(rel_pos2.cross(-jointAxis));
-		m_0MinvJt	= inertiaInvA * m_aJ;
-		m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
+		m_aJ = world2A * (rel_pos1.cross(jointAxis));
+		m_bJ = world2A * (rel_pos2.cross(-jointAxis));
+		m_0MinvJt = inertiaInvA * m_aJ;
+		m_1MinvJt = btVector3(btScalar(0.), btScalar(0.), btScalar(0.));
 		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
 
 		btAssert(m_Adiag > btScalar(0.0));
 	}
 
-	btScalar	getDiagonal() const { return m_Adiag; }
+	btScalar getDiagonal() const { return m_Adiag; }
 
 	// for two constraints on the same rigidbody (for example vehicle friction)
-	btScalar	getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
+	btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
 	{
 		const btJacobianEntry& jacA = *this;
 		btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
@@ -114,42 +112,39 @@ public:
 		return lin + ang;
 	}
 
-	
-
 	// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
-	btScalar	getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const
+	btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA, const btScalar massInvB) const
 	{
 		const btJacobianEntry& jacA = *this;
 		btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
 		btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
 		btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
-		btVector3 lin0 = massInvA * lin ;
+		btVector3 lin0 = massInvA * lin;
 		btVector3 lin1 = massInvB * lin;
-		btVector3 sum = ang0+ang1+lin0+lin1;
-		return sum[0]+sum[1]+sum[2];
+		btVector3 sum = ang0 + ang1 + lin0 + lin1;
+		return sum[0] + sum[1] + sum[2];
 	}
 
-	btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
+	btScalar getRelativeVelocity(const btVector3& linvelA, const btVector3& angvelA, const btVector3& linvelB, const btVector3& angvelB)
 	{
 		btVector3 linrel = linvelA - linvelB;
-		btVector3 angvela  = angvelA * m_aJ;
-		btVector3 angvelb  = angvelB * m_bJ;
+		btVector3 angvela = angvelA * m_aJ;
+		btVector3 angvelb = angvelB * m_bJ;
 		linrel *= m_linearJointAxis;
 		angvela += angvelb;
 		angvela += linrel;
-		btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
+		btScalar rel_vel2 = angvela[0] + angvela[1] + angvela[2];
 		return rel_vel2 + SIMD_EPSILON;
 	}
-//private:
+	//private:
 
-	btVector3	m_linearJointAxis;
-	btVector3	m_aJ;
-	btVector3	m_bJ;
-	btVector3	m_0MinvJt;
-	btVector3	m_1MinvJt;
+	btVector3 m_linearJointAxis;
+	btVector3 m_aJ;
+	btVector3 m_bJ;
+	btVector3 m_0MinvJt;
+	btVector3 m_1MinvJt;
 	//Optimization: can be stored in the w/last component of one of the vectors
-	btScalar	m_Adiag;
-
+	btScalar m_Adiag;
 };
 
-#endif //BT_JACOBIAN_ENTRY_H
+#endif  //BT_JACOBIAN_ENTRY_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp
index f110cd48081..ccf89160491 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp
@@ -15,14 +15,9 @@ subject to the following restrictions:
 
 #include "btNNCGConstraintSolver.h"
 
-
-
-
-
-
-btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+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);
+	btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
 	m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
 	m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
@@ -37,38 +32,39 @@ btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject*
 	return val;
 }
 
-btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
+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)
+		if (1)  // uncomment this for a bit less random ((iteration & 7) == 0)
 		{
-
-			for (int j=0; j<numNonContactPool; ++j) {
+			for (int j = 0; j < numNonContactPool; ++j)
+			{
 				int tmp = m_orderNonContactConstraintPool[j];
-				int swapi = btRandInt2(j+1);
+				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)
+			//contact/friction constraints are not solved more than
+			if (iteration < infoGlobal.m_numIterations)
 			{
-				for (int j=0; j<numConstraintPool; ++j) {
+				for (int j = 0; j < numConstraintPool; ++j)
+				{
 					int tmp = m_orderTmpConstraintPool[j];
-					int swapi = btRandInt2(j+1);
+					int swapi = btRandInt2(j + 1);
 					m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
 					m_orderTmpConstraintPool[swapi] = tmp;
 				}
 
-				for (int j=0; j<numFrictionPool; ++j) {
+				for (int j = 0; j < numFrictionPool; ++j)
+				{
 					int tmp = m_orderFrictionConstraintPool[j];
-					int swapi = btRandInt2(j+1);
+					int swapi = btRandInt2(j + 1);
 					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
 					m_orderFrictionConstraintPool[swapi] = tmp;
 				}
@@ -76,53 +72,40 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 		}
 	}
 
-
 	btScalar deltaflengthsqr = 0;
-
-	if (infoGlobal.m_solverMode & SOLVER_SIMD)
 	{
-		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
 		{
 			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-			if (iteration < constraint.m_overrideNumSolverIterations) 
+			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);
+				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 (m_onlyForNoneContact)
 	{
-		if (iteration==0) 
+		if (iteration == 0)
+		{
+			for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = m_deltafNC[j];
+		}
+		else
 		{
-			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) 
+			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++) m_pNC[j] = 0;
+			}
+			else
 			{
-				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+				for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
 				{
 					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-					if (iteration < constraint.m_overrideNumSolverIterations) 
+					if (iteration < constraint.m_overrideNumSolverIterations)
 					{
 						btScalar additionaldeltaimpulse = beta * m_pNC[j];
 						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
@@ -130,8 +113,8 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 						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);
+						body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
 					}
 				}
 			}
@@ -139,300 +122,225 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 		m_deltafLengthSqrPrev = deltaflengthsqr;
 	}
 
-
-
-	if (infoGlobal.m_solverMode & SOLVER_SIMD)
 	{
-
-		if (iteration< infoGlobal.m_numIterations)
+		if (iteration < infoGlobal.m_numIterations)
 		{
-			for (int j=0;j<numConstraints;j++)
+			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);
+					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);
+					constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
 				}
 			}
 
-			///solve all contact constraints using SIMD, if available
+			///solve all contact constraints
 			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;
+				int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
 
-				for (int c=0;c<numPoolConstraints;c++)
+				for (int c = 0; c < numPoolConstraints; c++)
 				{
-					btScalar totalImpulse =0;
+					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);						
+						btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 						m_deltafC[c] = deltaf;
-						deltaflengthsqr += deltaf*deltaf;
+						deltaflengthsqr += deltaf * deltaf;
 						totalImpulse = solveManifold.m_appliedImpulse;
 					}
 					bool applyFriction = true;
 					if (applyFriction)
 					{
 						{
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
 
-							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
-
-							if (totalImpulse>btScalar(0))
+							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;
+								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[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]];
 
-							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
-
-							if (totalImpulse>btScalar(0))
+							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[c * multiplier + 1] = deltaf;
+								deltaflengthsqr += deltaf * deltaf;
+							}
+							else
 							{
-								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;
+								m_deltafCF[c * multiplier + 1] = 0;
 							}
 						}
 					}
 				}
-
 			}
-			else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			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++)
+				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);
+					btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 					m_deltafC[j] = deltaf;
-					deltaflengthsqr += deltaf*deltaf;
+					deltaflengthsqr += deltaf * deltaf;
 				}
 
-
-
-				///solve all friction constraints, using SIMD, if available
+				///solve all friction constraints
 
 				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-				for (j=0;j<numFrictionPoolConstraints;j++)
+				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))
+					if (totalImpulse > btScalar(0))
 					{
-						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+						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);
+						btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 						m_deltafCF[j] = deltaf;
-						deltaflengthsqr += deltaf*deltaf;
-					} else {
+						deltaflengthsqr += deltaf * deltaf;
+					}
+					else
+					{
 						m_deltafCF[j] = 0;
 					}
 				}
+			}
 
-
+			{
 				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-				for (j=0;j<numRollingFrictionPoolConstraints;j++)
+				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))
+					if (totalImpulse > btScalar(0))
 					{
-						btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-						if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
+						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);
+						btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
 						m_deltafCRF[j] = deltaf;
-						deltaflengthsqr += deltaf*deltaf;
-					} else {
+						deltaflengthsqr += deltaf * deltaf;
+					}
+					else
+					{
 						m_deltafCRF[j] = 0;
 					}
 				}
-
-
 			}
 		}
+	}
 
-
-
-	} else
+	if (!m_onlyForNoneContact)
 	{
-
-		if (iteration< infoGlobal.m_numIterations)
+		if (iteration == 0)
 		{
-			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;
-				}
-			}
+			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];
+			for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++) m_pCRF[j] = m_deltafCRF[j];
 		}
-	}
-
-
-
-
-	if (!m_onlyForNoneContact) 
-	{
-		if (iteration==0) 
+		else
 		{
-			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 ) 
+			// 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_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
+				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;
+				for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++) m_pCRF[j] = 0;
 			}
-		} 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++)
+			else
+			{
+				for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
 				{
 					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-					if (iteration < constraint.m_overrideNumSolverIterations) {
+					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);
+						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++)
+				for (int j = 0; j < m_tmpSolverContactConstraintPool.size(); j++)
 				{
 					btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					if (iteration< infoGlobal.m_numIterations) {
+					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);
+						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++)
+				for (int j = 0; j < m_tmpSolverContactFrictionConstraintPool.size(); j++)
 				{
 					btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-					if (iteration< infoGlobal.m_numIterations) {
+					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);
+						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++)
+				{
+					for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++)
 					{
 						btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-						if (iteration< infoGlobal.m_numIterations) {
+						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);
+							body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
+							body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
 						}
 					}
 				}
@@ -444,7 +352,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 	return deltaflengthsqr;
 }
 
-btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
+btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
 {
 	m_pNC.resizeNoInitialize(0);
 	m_pC.resizeNoInitialize(0);
@@ -458,6 +366,3 @@ btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject
 
 	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
 }
-
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h
index a300929cd5c..c84f274a991 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h
@@ -18,33 +18,30 @@ subject to the following restrictions:
 
 #include "btSequentialImpulseConstraintSolver.h"
 
-ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
+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
+	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
+	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 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);
+	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) {}
@@ -57,8 +54,4 @@ public:
 	bool m_onlyForNoneContact;
 };
 
-
-
-
-#endif //BT_NNCG_CONSTRAINT_SOLVER_H
-
+#endif  //BT_NNCG_CONSTRAINT_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
index 3c0430b903f..ad399dc57f7 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
@@ -13,217 +13,193 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btPoint2PointConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include <new>
 
-
-
-
-
-btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
-m_flags(0),
-m_useSolveConstraintObsolete(false)
+btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB)
+	: btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0), m_useSolveConstraintObsolete(false)
 {
-
 }
 
-
-btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
-m_flags(0),
-m_useSolveConstraintObsolete(false)
+btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA, const btVector3& pivotInA)
+	: btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA), m_pivotInA(pivotInA), m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), m_flags(0), m_useSolveConstraintObsolete(false)
 {
-	
 }
 
-void	btPoint2PointConstraint::buildJacobian()
+void btPoint2PointConstraint::buildJacobian()
 {
-
 	///we need it for both methods
 	{
 		m_appliedImpulse = btScalar(0.);
 
-		btVector3	normal(0,0,0);
+		btVector3 normal(0, 0, 0);
 
-		for (int i=0;i<3;i++)
+		for (int i = 0; i < 3; i++)
 		{
 			normal[i] = 1;
 			new (&m_jac[i]) btJacobianEntry(
-			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
-			m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
-			normal,
-			m_rbA.getInvInertiaDiagLocal(),
-			m_rbA.getInvMass(),
-			m_rbB.getInvInertiaDiagLocal(),
-			m_rbB.getInvMass());
-		normal[i] = 0;
+				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbA.getCenterOfMassTransform() * m_pivotInA - m_rbA.getCenterOfMassPosition(),
+				m_rbB.getCenterOfMassTransform() * m_pivotInB - m_rbB.getCenterOfMassPosition(),
+				normal,
+				m_rbA.getInvInertiaDiagLocal(),
+				m_rbA.getInvMass(),
+				m_rbB.getInvInertiaDiagLocal(),
+				m_rbB.getInvMass());
+			normal[i] = 0;
 		}
 	}
-
-
 }
 
-void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
+void btPoint2PointConstraint::getInfo1(btConstraintInfo1* info)
 {
 	getInfo1NonVirtual(info);
 }
 
-void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+void btPoint2PointConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
 {
 	if (m_useSolveConstraintObsolete)
 	{
 		info->m_numConstraintRows = 0;
 		info->nub = 0;
-	} else
+	}
+	else
 	{
 		info->m_numConstraintRows = 3;
 		info->nub = 3;
 	}
 }
 
-
-
-
-void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
+void btPoint2PointConstraint::getInfo2(btConstraintInfo2* info)
 {
-	getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 }
 
-void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
+void btPoint2PointConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
 {
 	btAssert(!m_useSolveConstraintObsolete);
 
-	 //retrieve matrices
+	//retrieve matrices
 
 	// anchor points in global coordinates with respect to body PORs.
-   
-    // set jacobian
-    info->m_J1linearAxis[0] = 1;
-	info->m_J1linearAxis[info->rowskip+1] = 1;
-	info->m_J1linearAxis[2*info->rowskip+2] = 1;
 
-	btVector3 a1 = body0_trans.getBasis()*getPivotInA();
+	// set jacobian
+	info->m_J1linearAxis[0] = 1;
+	info->m_J1linearAxis[info->rowskip + 1] = 1;
+	info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
+
+	btVector3 a1 = body0_trans.getBasis() * getPivotInA();
 	{
 		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+		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);
+		a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    
+
 	info->m_J2linearAxis[0] = -1;
-    info->m_J2linearAxis[info->rowskip+1] = -1;
-    info->m_J2linearAxis[2*info->rowskip+2] = -1;
-	
-	btVector3 a2 = body1_trans.getBasis()*getPivotInB();
-   
+	info->m_J2linearAxis[info->rowskip + 1] = -1;
+	info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
+
+	btVector3 a2 = body1_trans.getBasis() * getPivotInB();
+
 	{
-	//	btVector3 a2n = -a2;
+		//	btVector3 a2n = -a2;
 		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
-		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    
-
 
-    // set right hand side
+	// set right hand side
 	btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
-    btScalar k = info->fps * currERP;
-    int j;
-	for (j=0; j<3; j++)
-    {
-        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
+	btScalar k = info->fps * currERP;
+	int j;
+	for (j = 0; j < 3; j++)
+	{
+		info->m_constraintError[j * info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
 		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
-    }
-	if(m_flags & BT_P2P_FLAGS_CFM)
+	}
+	if (m_flags & BT_P2P_FLAGS_CFM)
 	{
-		for (j=0; j<3; j++)
+		for (j = 0; j < 3; j++)
 		{
-			info->cfm[j*info->rowskip] = m_cfm;
+			info->cfm[j * info->rowskip] = m_cfm;
 		}
 	}
 
-	btScalar impulseClamp = m_setting.m_impulseClamp;//
-	for (j=0; j<3; j++)
-    {
+	btScalar impulseClamp = m_setting.m_impulseClamp;  //
+	for (j = 0; j < 3; j++)
+	{
 		if (m_setting.m_impulseClamp > 0)
 		{
-			info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
-			info->m_upperLimit[j*info->rowskip] = impulseClamp;
+			info->m_lowerLimit[j * info->rowskip] = -impulseClamp;
+			info->m_upperLimit[j * info->rowskip] = impulseClamp;
 		}
 	}
 	info->m_damping = m_setting.m_damping;
-	
 }
 
-
-
-void	btPoint2PointConstraint::updateRHS(btScalar	timeStep)
+void btPoint2PointConstraint::updateRHS(btScalar timeStep)
 {
 	(void)timeStep;
-
 }
 
-///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///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 btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
 {
-	if(axis != -1)
+	if (axis != -1)
 	{
 		btAssertConstrParams(0);
 	}
 	else
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_ERP :
-			case BT_CONSTRAINT_STOP_ERP :
-				m_erp = value; 
+			case BT_CONSTRAINT_ERP:
+			case BT_CONSTRAINT_STOP_ERP:
+				m_erp = value;
 				m_flags |= BT_P2P_FLAGS_ERP;
 				break;
-			case BT_CONSTRAINT_CFM :
-			case BT_CONSTRAINT_STOP_CFM :
-				m_cfm = value; 
+			case BT_CONSTRAINT_CFM:
+			case BT_CONSTRAINT_STOP_CFM:
+				m_cfm = value;
 				m_flags |= BT_P2P_FLAGS_CFM;
 				break;
-			default: 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
 }
 
 ///return the local value of parameter
-btScalar btPoint2PointConstraint::getParam(int num, int axis) const 
+btScalar btPoint2PointConstraint::getParam(int num, int axis) const
 {
 	btScalar retVal(SIMD_INFINITY);
-	if(axis != -1)
+	if (axis != -1)
 	{
 		btAssertConstrParams(0);
 	}
 	else
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_ERP :
-			case BT_CONSTRAINT_STOP_ERP :
+			case BT_CONSTRAINT_ERP:
+			case BT_CONSTRAINT_STOP_ERP:
 				btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
-				retVal = m_erp; 
+				retVal = m_erp;
 				break;
-			case BT_CONSTRAINT_CFM :
-			case BT_CONSTRAINT_STOP_CFM :
+			case BT_CONSTRAINT_CFM:
+			case BT_CONSTRAINT_STOP_CFM:
 				btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
-				retVal = m_cfm; 
+				retVal = m_cfm;
 				break;
-			default: 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
 	return retVal;
 }
-	
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
index 8fa03d719d7..4717e198004 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
@@ -22,26 +22,24 @@ subject to the following restrictions:
 
 class btRigidBody;
 
-
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btPoint2PointConstraintData2	btPoint2PointConstraintDoubleData2
-#define btPoint2PointConstraintDataName	"btPoint2PointConstraintDoubleData2"
+#define btPoint2PointConstraintData2 btPoint2PointConstraintDoubleData2
+#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData2"
 #else
-#define btPoint2PointConstraintData2	btPoint2PointConstraintFloatData
-#define btPoint2PointConstraintDataName	"btPoint2PointConstraintFloatData"
-#endif //BT_USE_DOUBLE_PRECISION
+#define btPoint2PointConstraintData2 btPoint2PointConstraintFloatData
+#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
-struct	btConstraintSetting
+struct btConstraintSetting
 {
-	btConstraintSetting()	:
-		m_tau(btScalar(0.3)),
-		m_damping(btScalar(1.)),
-		m_impulseClamp(btScalar(0.))
+	btConstraintSetting() : m_tau(btScalar(0.3)),
+							m_damping(btScalar(1.)),
+							m_impulseClamp(btScalar(0.))
 	{
 	}
-	btScalar		m_tau;
-	btScalar		m_damping;
-	btScalar		m_impulseClamp;
+	btScalar m_tau;
+	btScalar m_damping;
+	btScalar m_impulseClamp;
 };
 
 enum btPoint2PointFlags
@@ -51,52 +49,51 @@ enum btPoint2PointFlags
 };
 
 /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
-ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class)
+btPoint2PointConstraint : public btTypedConstraint
 {
 #ifdef IN_PARALLELL_SOLVER
 public:
 #endif
-	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
-	
-	btVector3	m_pivotInA;
-	btVector3	m_pivotInB;
-	
-	int			m_flags;
-	btScalar	m_erp;
-	btScalar	m_cfm;
-	
-public:
+	btJacobianEntry m_jac[3];  //3 orthogonal linear constraints
+
+	btVector3 m_pivotInA;
+	btVector3 m_pivotInB;
+
+	int m_flags;
+	btScalar m_erp;
+	btScalar m_cfm;
 
+public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	///for backwards compatibility during the transition to 'getInfo/getInfo2'
-	bool		m_useSolveConstraintObsolete;
-
-	btConstraintSetting	m_setting;
+	bool m_useSolveConstraintObsolete;
 
-	btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);
+	btConstraintSetting m_setting;
 
-	btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
+	btPoint2PointConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& pivotInA, const btVector3& pivotInB);
 
+	btPoint2PointConstraint(btRigidBody & rbA, const btVector3& pivotInA);
 
-	virtual void	buildJacobian();
+	virtual void buildJacobian();
 
-	virtual void getInfo1 (btConstraintInfo1* info);
+	virtual void getInfo1(btConstraintInfo1 * info);
 
-	void getInfo1NonVirtual (btConstraintInfo1* info);
+	void getInfo1NonVirtual(btConstraintInfo1 * info);
 
-	virtual void getInfo2 (btConstraintInfo2* info);
+	virtual void getInfo2(btConstraintInfo2 * info);
 
-	void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans);
+	void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& body0_trans, const btTransform& body1_trans);
 
-	void	updateRHS(btScalar	timeStep);
+	void updateRHS(btScalar timeStep);
 
-	void	setPivotA(const btVector3& pivotA)
+	void setPivotA(const btVector3& pivotA)
 	{
 		m_pivotInA = pivotA;
 	}
 
-	void	setPivotB(const btVector3& pivotB)
+	void setPivotB(const btVector3& pivotB)
 	{
 		m_pivotInB = pivotB;
 	}
@@ -111,70 +108,66 @@ public:
 		return m_pivotInB;
 	}
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///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 void setParam(int num, btScalar value, int axis = -1);
 	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const;
-	
-	virtual	int getFlags() const
+	virtual btScalar getParam(int num, int axis = -1) const;
+
+	virtual int getFlags() const
 	{
-        	return m_flags;
-    	}
+		return m_flags;
+	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btPoint2PointConstraintFloatData
+struct btPoint2PointConstraintFloatData
 {
-	btTypedConstraintData	m_typeConstraintData;
-	btVector3FloatData	m_pivotInA;
-	btVector3FloatData	m_pivotInB;
+	btTypedConstraintData m_typeConstraintData;
+	btVector3FloatData m_pivotInA;
+	btVector3FloatData m_pivotInB;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btPoint2PointConstraintDoubleData2
+struct btPoint2PointConstraintDoubleData2
 {
-	btTypedConstraintDoubleData	m_typeConstraintData;
-	btVector3DoubleData	m_pivotInA;
-	btVector3DoubleData	m_pivotInB;
+	btTypedConstraintDoubleData m_typeConstraintData;
+	btVector3DoubleData m_pivotInA;
+	btVector3DoubleData m_pivotInB;
 };
 
 #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 ///this structure is not used, except for loading pre-2.82 .bullet files
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btPoint2PointConstraintDoubleData
+struct btPoint2PointConstraintDoubleData
 {
-	btTypedConstraintData	m_typeConstraintData;
-	btVector3DoubleData	m_pivotInA;
-	btVector3DoubleData	m_pivotInB;
+	btTypedConstraintData m_typeConstraintData;
+	btVector3DoubleData m_pivotInA;
+	btVector3DoubleData m_pivotInB;
 };
-#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+#endif  //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 
-
-SIMD_FORCE_INLINE	int	btPoint2PointConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btPoint2PointConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btPoint2PointConstraintData2);
-
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char* btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btPoint2PointConstraintData2* p2pData = (btPoint2PointConstraintData2*)dataBuffer;
 
-	btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
+	btTypedConstraint::serialize(&p2pData->m_typeConstraintData, serializer);
 	m_pivotInA.serialize(p2pData->m_pivotInA);
 	m_pivotInB.serialize(p2pData->m_pivotInB);
 
 	return btPoint2PointConstraintDataName;
 }
 
-#endif //BT_POINT2POINTCONSTRAINT_H
+#endif  //BT_POINT2POINTCONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index 8da572bf7d8..d2641c582f1 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -14,7 +14,9 @@ subject to the following restrictions:
 */
 
 //#define COMPUTE_IMPULSE_DENOM 1
-//#define BT_ADDITIONAL_DEBUG
+#ifdef BT_DEBUG
+#	define BT_ADDITIONAL_DEBUG
+#endif
 
 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
 
@@ -33,24 +35,24 @@ subject to the following restrictions:
 //#include "btSolverBody.h"
 //#include "btSolverConstraint.h"
 #include "LinearMath/btAlignedObjectArray.h"
-#include <string.h> //for memset
+#include <string.h>  //for memset
 
-int		gNumSplitImpulseRecoveries = 0;
+int gNumSplitImpulseRecoveries = 0;
 
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
-
+//#define VERBOSE_RESIDUAL_PRINTF 1
 ///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)
+static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, 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());
+	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
+	const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity());
+	const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity());
 
 	//	const btScalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
-	deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
-	deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
 
 	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
 	if (sum < c.m_lowerLimit)
@@ -68,21 +70,20 @@ static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolver
 		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);
+	bodyA.internalApplyImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+	bodyB.internalApplyImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 
-	return deltaImpulse;
+	return deltaImpulse * (1. / c.m_jacDiagABInv);
 }
 
-
-static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
+static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, 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());
+	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
+	const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity());
+	const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity());
 
-	deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
-	deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
 	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
 	if (sum < c.m_lowerLimit)
 	{
@@ -93,70 +94,67 @@ static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSol
 	{
 		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);
+	bodyA.internalApplyImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+	bodyB.internalApplyImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 
-	return deltaImpulse;
+	return deltaImpulse * (1. / c.m_jacDiagABInv);
 }
 
-
-
 #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 )
+#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 ) ) );
+	__m128 result = _mm_mul_ps(vec0, vec1);
+	return _mm_add_ps(btVecSplat(result, 0), _mm_add_ps(btVecSplat(result, 1), btVecSplat(result, 2)));
 }
 
-#if defined (BT_ALLOW_SSE4)
+#if defined(BT_ALLOW_SSE4)
 #include <intrin.h>
 
-#define USE_FMA					1
-#define USE_FMA3_INSTEAD_FMA4	1
-#define USE_SSE4_DOT			1
+#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))
+#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)
+#define DOT_PRODUCT(a, b) SSE4_DP(a, b)
 #else
-#define DOT_PRODUCT(a, b)		btSimdDot3(a, b)
+#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)
+#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
+#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)
+#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)
+#define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c)
 #endif
-#else // USE_FMA
+#else  // USE_FMA
 // c + a*b
-#define FMADD(a, b, c)		_mm_add_ps(c, _mm_mul_ps(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))
+#define FMNADD(a, b, c) _mm_sub_ps(c, _mm_mul_ps(a, b))
 #endif
 #endif
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
-static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
+static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
 	__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 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
 	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));
+	__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.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);
@@ -169,54 +167,52 @@ static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1,
 	__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 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
+	__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, bodyB.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));
-	return deltaImpulse;
+	bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
+	bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
+	bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
+	bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
+	return deltaImpulse.m_floats[0] / c.m_jacDiagABInv;
 }
 
-
 // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
-static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
+static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, 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;
+#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, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
+	const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.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);
+	bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128);
+	bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128);
+	bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128);
+	bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128);
+	btSimdScalar deltaImp = deltaImpulse;
+	return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
 #else
-	return gResolveSingleConstraintRowGeneric_sse2(body1,body2,c);
+	return gResolveSingleConstraintRowGeneric_sse2(bodyA, bodyB, c);
 #endif
 }
 
-
-
-static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
+static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
 	__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 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
 	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));
+	__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.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);
@@ -226,211 +222,209 @@ static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bod
 	__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 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
+	__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.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));
-	return deltaImpulse;
+	bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
+	bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
+	bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
+	bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
+	return deltaImpulse.m_floats[0] / c.m_jacDiagABInv;
 }
 
-
 // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
-static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
+static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, 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;
+	__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, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
+	const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.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);
+	bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128);
+	bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128);
+	bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128);
+	bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128);
+	btSimdScalar deltaImp = deltaImpulse;
+	return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
 #else
-	return gResolveSingleConstraintRowLowerLimit_sse2(body1,body2,c);
-#endif //BT_ALLOW_SSE4
+	return gResolveSingleConstraintRowLowerLimit_sse2(bodyA, bodyB, c);
+#endif  //BT_ALLOW_SSE4
 }
 
+#endif  //USE_SIMD
 
-#endif //USE_SIMD
-
-
-
-btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
-#ifdef USE_SIMD
-	return m_resolveSingleConstraintRowGeneric(body1, body2, c);
-#else
-	return resolveSingleConstraintRowGeneric(body1,body2,c);
-#endif
+	return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c);
 }
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
-btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
-	return gResolveSingleConstraintRowGeneric_scalar_reference(body1, body2, c);
+	return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c);
 }
 
-btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
-#ifdef USE_SIMD
-	return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
-#else
-	return resolveSingleConstraintRowLowerLimit(body1,body2,c);
-#endif
+	return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c);
 }
 
-
-btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
-	return gResolveSingleConstraintRowLowerLimit_scalar_reference(body1,body2,c);
+	return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c);
 }
 
-
-void	btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
-        btSolverBody& body1,
-        btSolverBody& body2,
-        const btSolverConstraint& c)
+static btScalar gResolveSplitPenetrationImpulse_scalar_reference(
+	btSolverBody& bodyA,
+	btSolverBody& bodyB,
+	const btSolverConstraint& c)
 {
-		if (c.m_rhsPenetration)
-        {
-			gNumSplitImpulseRecoveries++;
-			btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
-			const btScalar deltaVel1Dotn	=	c.m_contactNormal1.dot(body1.internalGetPushVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
-			const btScalar deltaVel2Dotn	=	c.m_contactNormal2.dot(body2.internalGetPushVelocity())		+ c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
-
-			deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
-			deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;
-			const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
-			if (sum < c.m_lowerLimit)
-			{
-				deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
-				c.m_appliedPushImpulse = c.m_lowerLimit;
-			}
-			else
-			{
-				c.m_appliedPushImpulse = sum;
-			}
-			body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
-			body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
-        }
+	btScalar deltaImpulse = 0.f;
+
+	if (c.m_rhsPenetration)
+	{
+		gNumSplitImpulseRecoveries++;
+		deltaImpulse = c.m_rhsPenetration - btScalar(c.m_appliedPushImpulse) * c.m_cfm;
+		const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetTurnVelocity());
+		const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetTurnVelocity());
+
+		deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
+		deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
+		const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
+		if (sum < c.m_lowerLimit)
+		{
+			deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse;
+			c.m_appliedPushImpulse = c.m_lowerLimit;
+		}
+		else
+		{
+			c.m_appliedPushImpulse = sum;
+		}
+		bodyA.internalApplyPushImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+		bodyB.internalApplyPushImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+	}
+	return deltaImpulse * (1. / c.m_jacDiagABInv);
 }
 
- void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
 #ifdef USE_SIMD
 	if (!c.m_rhsPenetration)
-		return;
+		return 0.f;
 
 	gNumSplitImpulseRecoveries++;
 
 	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
-	__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_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
-	__m128 deltaVel1Dotn	=	_mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
-	__m128 deltaVel2Dotn	=	_mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().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_appliedPushImpulse = _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 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_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse), _mm_set1_ps(c.m_cfm)));
+	__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetTurnVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetTurnVelocity().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_appliedPushImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
+	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
+	__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128);
 	__m128 impulseMagnitude = deltaImpulse;
-	body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
-	body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
-	body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
-	body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+	bodyA.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyA.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
+	bodyA.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyA.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
+	bodyB.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyB.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
+	bodyB.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyB.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
+	btSimdScalar deltaImp = deltaImpulse;
+	return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
 #else
-	resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
+	return gResolveSplitPenetrationImpulse_scalar_reference(bodyA, bodyB, c);
 #endif
 }
 
+btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
+{
+	m_btSeed2 = 0;
+	m_cachedSolverMode = 0;
+	setupSolverFunctions(false);
+}
 
- btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
-	 : m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference),
-	 m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference),
-	 m_btSeed2(0)
- {
+void btSequentialImpulseConstraintSolver::setupSolverFunctions(bool useSimd)
+{
+	m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_scalar_reference;
+	m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_scalar_reference;
+	m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_scalar_reference;
 
+	if (useSimd)
+	{
 #ifdef USE_SIMD
-	 m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
-	 m_resolveSingleConstraintRowLowerLimit=gResolveSingleConstraintRowLowerLimit_sse2;
-#endif //USE_SIMD
+		m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
+		m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse2;
+		m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_sse2;
 
 #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()
- {
- }
+		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
+#endif  //USE_SIMD
+	}
+}
 
- btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()
- {
-	 return gResolveSingleConstraintRowGeneric_scalar_reference;
- }
+btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
+{
+}
 
- btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()
- {
-	 return gResolveSingleConstraintRowLowerLimit_scalar_reference;
- }
+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;
- }
+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
+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()
 {
-	m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
+	m_btSeed2 = (1664525L * m_btSeed2 + 1013904223L) & 0xffffffff;
 	return m_btSeed2;
 }
 
-
-
 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
-int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
+int btSequentialImpulseConstraintSolver::btRandInt2(int n)
 {
 	// seems good; xor-fold and modulus
 	const unsigned long un = static_cast<unsigned long>(n);
@@ -438,15 +432,20 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
 
 	// note: probably more aggressive than it needs to be -- might be
 	//       able to get away without one or two of the innermost branches.
-	if (un <= 0x00010000UL) {
+	if (un <= 0x00010000UL)
+	{
 		r ^= (r >> 16);
-		if (un <= 0x00000100UL) {
+		if (un <= 0x00000100UL)
+		{
 			r ^= (r >> 8);
-			if (un <= 0x00000010UL) {
+			if (un <= 0x00000010UL)
+			{
 				r ^= (r >> 4);
-				if (un <= 0x00000004UL) {
+				if (un <= 0x00000004UL)
+				{
 					r ^= (r >> 2);
-					if (un <= 0x00000002UL) {
+					if (un <= 0x00000002UL)
+					{
 						r ^= (r >> 1);
 					}
 				}
@@ -454,66 +453,56 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
 		}
 	}
 
-	return (int) (r % un);
+	return (int)(r % un);
 }
 
-
-
-void	btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep)
+void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep)
 {
+	btRigidBody* rb = collisionObject ? btRigidBody::upcast(collisionObject) : 0;
 
-	btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
-
-	solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
-	solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
-	solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
-	solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
+	solverBody->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
+	solverBody->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f);
+	solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
+	solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
 
 	if (rb)
 	{
 		solverBody->m_worldTransform = rb->getWorldTransform();
-		solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor());
+		solverBody->internalSetInvMass(btVector3(rb->getInvMass(), rb->getInvMass(), rb->getInvMass()) * rb->getLinearFactor());
 		solverBody->m_originalBody = rb;
 		solverBody->m_angularFactor = rb->getAngularFactor();
 		solverBody->m_linearFactor = rb->getLinearFactor();
 		solverBody->m_linearVelocity = rb->getLinearVelocity();
 		solverBody->m_angularVelocity = rb->getAngularVelocity();
-		solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep;
-		solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ;
-
-	} else
-	{
+		solverBody->m_externalForceImpulse = rb->getTotalForce() * rb->getInvMass() * timeStep;
+		solverBody->m_externalTorqueImpulse = rb->getTotalTorque() * rb->getInvInertiaTensorWorld() * timeStep;
+	}
+	else
+		{
 		solverBody->m_worldTransform.setIdentity();
-		solverBody->internalSetInvMass(btVector3(0,0,0));
+		solverBody->internalSetInvMass(btVector3(0, 0, 0));
 		solverBody->m_originalBody = 0;
-		solverBody->m_angularFactor.setValue(1,1,1);
-		solverBody->m_linearFactor.setValue(1,1,1);
-		solverBody->m_linearVelocity.setValue(0,0,0);
-		solverBody->m_angularVelocity.setValue(0,0,0);
-		solverBody->m_externalForceImpulse.setValue(0,0,0);
-		solverBody->m_externalTorqueImpulse.setValue(0,0,0);
-	}
-
-
-}
-
-
-
-
-
+		solverBody->m_angularFactor.setValue(1, 1, 1);
+		solverBody->m_linearFactor.setValue(1, 1, 1);
+		solverBody->m_linearVelocity.setValue(0, 0, 0);
+		solverBody->m_angularVelocity.setValue(0, 0, 0);
+		solverBody->m_externalForceImpulse.setValue(0, 0, 0);
+		solverBody->m_externalTorqueImpulse.setValue(0, 0, 0);
+					}
+				}
 
-btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
+btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
 {
+	//printf("rel_vel =%f\n", rel_vel);
+	if (btFabs(rel_vel) < velocityThreshold)
+		return 0.;
+
 	btScalar rest = restitution * -rel_vel;
 	return rest;
 }
 
-
-
-void	btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
+void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj, btVector3& frictionDirection, int frictionMode)
 {
-
-
 	if (colObj && colObj->hasAnisotropicFriction(frictionMode))
 	{
 		// transform to local coordinates
@@ -524,21 +513,15 @@ void	btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb
 		// ... and transform it back to global coordinates
 		frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
 	}
-
 }
 
-
-
-
-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)
+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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
 {
-
-
 	btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
 	btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
 
 	btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
-	btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
+	btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
 
 	solverConstraint.m_solverBodyIdA = solverBodyIdA;
 	solverConstraint.m_solverBodyIdB = solverBodyIdB;
@@ -554,21 +537,23 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 		solverConstraint.m_contactNormal1 = normalAxis;
 		btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
 		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor();
-	}else
+		solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld() * ftorqueAxis1 * body0->getAngularFactor();
+	}
+	else
 	{
 		solverConstraint.m_contactNormal1.setZero();
 		solverConstraint.m_relpos1CrossNormal.setZero();
-		solverConstraint.m_angularComponentA .setZero();
+		solverConstraint.m_angularComponentA.setZero();
 	}
 
-	if (body1)
+	if (bodyA)
 	{
 		solverConstraint.m_contactNormal2 = -normalAxis;
 		btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
 		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor();
-	} else
+		solverConstraint.m_angularComponentB = bodyA->getInvInertiaTensorWorld() * ftorqueAxis1 * bodyA->getAngularFactor();
+	}
+	else
 	{
 		solverConstraint.m_contactNormal2.setZero();
 		solverConstraint.m_relpos2CrossNormal.setZero();
@@ -581,60 +566,63 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 		btScalar denom1 = 0.f;
 		if (body0)
 		{
-			vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+			vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
 			denom0 = body0->getInvMass() + normalAxis.dot(vec);
 		}
-		if (body1)
+		if (bodyA)
 		{
-			vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
-			denom1 = body1->getInvMass() + normalAxis.dot(vec);
+			vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
+			denom1 = bodyA->getInvMass() + normalAxis.dot(vec);
 		}
-		btScalar denom = relaxation/(denom0+denom1);
+		btScalar denom = relaxation / (denom0 + denom1);
 		solverConstraint.m_jacDiagABInv = denom;
 	}
 
 	{
-
-
 		btScalar rel_vel;
-		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
-			+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
-		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
-			+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
+		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(bodyA ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(bodyA ? solverBodyB.m_angularVelocity : btVector3(0, 0, 0));
 
-		rel_vel = vel1Dotn+vel2Dotn;
+		rel_vel = vel1Dotn + vel2Dotn;
 
-//		btScalar positionalError = 0.f;
+		//		btScalar positionalError = 0.f;
 
-		btScalar velocityError =  desiredVelocity - rel_vel;
+		btScalar velocityError = desiredVelocity - rel_vel;
 		btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
-		solverConstraint.m_rhs = velocityImpulse;
+
+		btScalar penetrationImpulse = btScalar(0);
+
+		if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
+		{
+			btScalar distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(normalAxis);
+			btScalar positionalError = -distance * infoGlobal.m_frictionERP / infoGlobal.m_timeStep;
+			penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
+		}
+
+		solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
 		solverConstraint.m_rhsPenetration = 0.f;
 		solverConstraint.m_cfm = cfmSlip;
 		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
 		solverConstraint.m_upperLimit = solverConstraint.m_friction;
-
 	}
 }
 
-btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
 {
 	btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
 	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
-							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
+		colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
 
-
-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,
-									btScalar desiredVelocity, btScalar cfmSlip)
+void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
+	btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
+	btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
+	btScalar desiredVelocity, btScalar cfmSlip)
 
 {
-	btVector3 normalAxis(0,0,0);
-
+	btVector3 normalAxis(0, 0, 0);
 
 	solverConstraint.m_contactNormal1 = normalAxis;
 	solverConstraint.m_contactNormal2 = -normalAxis;
@@ -642,12 +630,12 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolv
 	btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
 
 	btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
-	btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
+	btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
 
 	solverConstraint.m_solverBodyIdA = solverBodyIdA;
 	solverConstraint.m_solverBodyIdB = solverBodyIdB;
 
-	solverConstraint.m_friction = cp.m_combinedRollingFriction;
+	solverConstraint.m_friction = combinedTorsionalFriction;
 	solverConstraint.m_originalContactPoint = 0;
 
 	solverConstraint.m_appliedImpulse = 0.f;
@@ -656,66 +644,117 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolv
 	{
 		btVector3 ftorqueAxis1 = -normalAxis1;
 		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld() * ftorqueAxis1 * body0->getAngularFactor() : btVector3(0, 0, 0);
 	}
 	{
 		btVector3 ftorqueAxis1 = normalAxis1;
 		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_angularComponentB = bodyA ? bodyA->getInvInertiaTensorWorld() * ftorqueAxis1 * bodyA->getAngularFactor() : btVector3(0, 0, 0);
 	}
 
-
 	{
-		btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
-		btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
+		btVector3 iMJaA = body0 ? body0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0);
+		btVector3 iMJaB = bodyA ? bodyA->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0);
 		btScalar sum = 0;
 		sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
 		sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
-		solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
+		solverConstraint.m_jacDiagABInv = btScalar(1.) / sum;
 	}
 
 	{
-
-
 		btScalar rel_vel;
-		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
-			+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
-		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
-			+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
+		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(bodyA ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(bodyA ? solverBodyB.m_angularVelocity : btVector3(0, 0, 0));
 
-		rel_vel = vel1Dotn+vel2Dotn;
+		rel_vel = vel1Dotn + vel2Dotn;
 
-//		btScalar positionalError = 0.f;
+		//		btScalar positionalError = 0.f;
 
-		btSimdScalar velocityError =  desiredVelocity - rel_vel;
-		btSimdScalar	velocityImpulse = velocityError * btSimdScalar(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;
-
 	}
 }
 
-
-
-
-
-
-
-
-btSolverConstraint&	btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
 {
 	btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
-	setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
-							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
+	setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction, rel_pos1, rel_pos2,
+		colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
 
-
-int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep)
+int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body, btScalar timeStep)
 {
+#if BT_THREADSAFE
+	int solverBodyId = -1;
+	const bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
+	const bool isStaticOrKinematic = body.isStaticOrKinematicObject();
+	const bool isKinematic = body.isKinematicObject();
+	if (isRigidBodyType && !isStaticOrKinematic)
+	{
+		// dynamic body
+		// Dynamic bodies can only be in one island, so it's safe to write to the companionId
+		solverBodyId = body.getCompanionId();
+		if (solverBodyId < 0)
+		{
+			solverBodyId = m_tmpSolverBodyPool.size();
+			btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(&solverBody, &body, timeStep);
+			body.setCompanionId(solverBodyId);
+		}
+	}
+	else if (isRigidBodyType && isKinematic)
+	{
+		//
+		// NOTE: must test for kinematic before static because some kinematic objects also
+		//   identify as "static"
+		//
+		// Kinematic bodies can be in multiple islands at once, so it is a
+		// race condition to write to them, so we use an alternate method
+		// to record the solverBodyId
+		int uniqueId = body.getWorldArrayIndex();
+		const int INVALID_SOLVER_BODY_ID = -1;
+		if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size())
+		{
+			m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
+		}
+		solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
+		// if no table entry yet,
+		if (solverBodyId == INVALID_SOLVER_BODY_ID)
+		{
+			// create a table entry for this body
+			solverBodyId = m_tmpSolverBodyPool.size();
+			btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(&solverBody, &body, timeStep);
+			m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId] = solverBodyId;
+		}
+	}
+	else
+	{
+		bool isMultiBodyType = (body.getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK);
+		// Incorrectly set collision object flags can degrade performance in various ways.
+		if (!isMultiBodyType)
+		{
+			btAssert(body.isStaticOrKinematicObject());
+		}
+		//it could be a multibody link collider
+		// all fixed bodies (inf mass) get mapped to a single solver id
+		if (m_fixedBodyId < 0)
+		{
+			m_fixedBodyId = m_tmpSolverBodyPool.size();
+			btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(&fixedBody, 0, timeStep);
+		}
+		solverBodyId = m_fixedBodyId;
+	}
+	btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size());
+	return solverBodyId;
+#else   // BT_THREADSAFE
 
 	int solverBodyIdA = -1;
 
@@ -723,8 +762,9 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 	{
 		//body has already been converted
 		solverBodyIdA = body.getCompanionId();
-        btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
-	} else
+		btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
+	}
+	else
 	{
 		btRigidBody* rb = btRigidBody::upcast(&body);
 		//convert both active and kinematic objects (for their velocity)
@@ -732,273 +772,253 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 		{
 			solverBodyIdA = m_tmpSolverBodyPool.size();
 			btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-			initSolverBody(&solverBody,&body,timeStep);
+			initSolverBody(&solverBody, &body, timeStep);
 			body.setCompanionId(solverBodyIdA);
-		} else
+		}
+		else
 		{
-
-			if (m_fixedBodyId<0)
+			if (m_fixedBodyId < 0)
 			{
 				m_fixedBodyId = m_tmpSolverBodyPool.size();
 				btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
-				initSolverBody(&fixedBody,0,timeStep);
+				initSolverBody(&fixedBody, 0, timeStep);
 			}
 			return m_fixedBodyId;
-//			return 0;//assume first one is a fixed solver body
+			//			return 0;//assume first one is a fixed solver body
 		}
 	}
 
 	return solverBodyIdA;
-
+#endif  // BT_THREADSAFE
 }
 #include <stdio.h>
 
-
 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
-																 int solverBodyIdA, int solverBodyIdB,
-																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
-																 btScalar& relaxation,
-																 const btVector3& rel_pos1, const btVector3& rel_pos2)
+	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];
-
-			btRigidBody* rb0 = bodyA->m_originalBody;
-			btRigidBody* rb1 = bodyB->m_originalBody;
-
-//			btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
-//			btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
-			//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
-			//rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
-
-			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);
-			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
-					btVector3 vec;
-					btScalar denom0 = 0.f;
-					btScalar denom1 = 0.f;
-					if (rb0)
-					{
-						vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
-						denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
-					}
-					if (rb1)
-					{
-						vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
-						denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
-					}
-#endif //COMPUTE_IMPULSE_DENOM
-
-					btScalar denom = relaxation/(denom0+denom1);
-					solverConstraint.m_jacDiagABInv = denom;
-				}
-
-				if (rb0)
-				{
-					solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
-					solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-				} else
-				{
-					solverConstraint.m_contactNormal1.setZero();
-					solverConstraint.m_relpos1CrossNormal.setZero();
-				}
-				if (rb1)
-				{
-					solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
-					solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
-				}else
-				{
-					solverConstraint.m_contactNormal2.setZero();
-					solverConstraint.m_relpos2CrossNormal.setZero();
-				}
-
-				btScalar restitution = 0.f;
-				btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
-
-				{
-					btVector3 vel1,vel2;
+	btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+	btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
 
-					vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
-					vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+	btRigidBody* rb0 = bodyA->m_originalBody;
+	btRigidBody* rb1 = bodyB->m_originalBody;
 
-	//			btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
-					btVector3 vel  = vel1 - vel2;
-					btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
+	//			btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
+	//			btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
+	//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
+	//rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
 
+	relaxation = infoGlobal.m_sor;
+	btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep;
 
+	//cfm = 1 /       ( dt * kp + kd )
+	//erp = dt * kp / ( dt * kp + kd )
 
-					solverConstraint.m_friction = cp.m_combinedFriction;
+	btScalar cfm = infoGlobal.m_globalCfm;
+	btScalar erp = infoGlobal.m_erp2;
 
+	if ((cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP))
+	{
+		if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM)
+			cfm = cp.m_contactCFM;
+		if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP)
+			erp = cp.m_contactERP;
+	}
+	else
+	{
+		if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
+		{
+			btScalar denom = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1);
+			if (denom < SIMD_EPSILON)
+			{
+				denom = SIMD_EPSILON;
+			}
+			cfm = btScalar(1) / denom;
+			erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
+		}
+	}
 
-					restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
-					if (restitution <= btScalar(0.))
-					{
-						restitution = 0.f;
-					};
-				}
+	cfm *= invTimeStep;
 
+	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);
+	solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
 
-				///warm starting (or zero if disabled)
-				if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-				{
-					solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-					if (rb0)
-						bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-					if (rb1)
-						bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
-				} else
-				{
-					solverConstraint.m_appliedImpulse = 0.f;
-				}
+	{
+#ifdef COMPUTE_IMPULSE_DENOM
+		btScalar denom0 = rb0->computeImpulseDenominator(pos1, cp.m_normalWorldOnB);
+		btScalar denom1 = rb1->computeImpulseDenominator(pos2, cp.m_normalWorldOnB);
+#else
+		btVector3 vec;
+		btScalar denom0 = 0.f;
+		btScalar denom1 = 0.f;
+		if (rb0)
+		{
+			vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
+			denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+		}
+		if (rb1)
+		{
+			vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
+			denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+		}
+#endif  //COMPUTE_IMPULSE_DENOM
 
-				solverConstraint.m_appliedPushImpulse = 0.f;
+		btScalar denom = relaxation / (denom0 + denom1 + cfm);
+		solverConstraint.m_jacDiagABInv = denom;
+	}
 
-				{
+	if (rb0)
+	{
+		solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
+		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+	}
+	else
+	{
+		solverConstraint.m_contactNormal1.setZero();
+		solverConstraint.m_relpos1CrossNormal.setZero();
+	}
+	if (rb1)
+	{
+		solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
+		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+	}
+	else
+	{
+		solverConstraint.m_contactNormal2.setZero();
+		solverConstraint.m_relpos2CrossNormal.setZero();
+	}
 
-					btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0);
-					btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0);
-					btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0);
-					btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0);
+	btScalar restitution = 0.f;
+	btScalar penetration = cp.getDistance() + infoGlobal.m_linearSlop;
 
+	{
+		btVector3 vel1, vel2;
 
-					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)
-						+ solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB);
-					btScalar rel_vel = vel1Dotn+vel2Dotn;
+		vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0, 0, 0);
+		vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0, 0, 0);
 
-					btScalar positionalError = 0.f;
-					btScalar	velocityError = restitution - rel_vel;// * damping;
+		//			btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+		btVector3 vel = vel1 - vel2;
+		btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
 
+		solverConstraint.m_friction = cp.m_combinedFriction;
 
-					btScalar erp = infoGlobal.m_erp2;
-					if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-					{
-						erp = infoGlobal.m_erp;
-					}
+		restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
+		if (restitution <= btScalar(0.))
+		{
+			restitution = 0.f;
+		};
+	}
 
-					if (penetration>0)
-					{
-						positionalError = 0;
+	///warm starting (or zero if disabled)
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
+		solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+		if (rb0)
+			bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
+		if (rb1)
+			bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
+	}
+	else
+	{
+		solverConstraint.m_appliedImpulse = 0.f;
+	}
 
-						velocityError -= penetration / infoGlobal.m_timeStep;
-					} else
-					{
-						positionalError = -penetration * erp/infoGlobal.m_timeStep;
-					}
+	solverConstraint.m_appliedPushImpulse = 0.f;
 
-					btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
-					btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+	{
+		btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse : btVector3(0, 0, 0);
+		btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse : btVector3(0, 0, 0);
+		btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse : btVector3(0, 0, 0);
+		btVector3 externalTorqueImpulseB = bodyB->m_originalBody ? bodyB->m_externalTorqueImpulse : btVector3(0, 0, 0);
 
-					if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-					{
-						//combine position and velocity into rhs
-						solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
-						solverConstraint.m_rhsPenetration = 0.f;
+		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) + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity + externalTorqueImpulseB);
+		btScalar rel_vel = vel1Dotn + vel2Dotn;
 
-					} else
-					{
-						//split position and velocity into rhs and m_rhsPenetration
-						solverConstraint.m_rhs = velocityImpulse;
-						solverConstraint.m_rhsPenetration = penetrationImpulse;
-					}
-					solverConstraint.m_cfm = 0.f;
-					solverConstraint.m_lowerLimit = 0;
-					solverConstraint.m_upperLimit = 1e10f;
-				}
+		btScalar positionalError = 0.f;
+		btScalar velocityError = restitution - rel_vel;  // * damping;
 
+		if (penetration > 0)
+		{
+			positionalError = 0;
 
+			velocityError -= penetration * invTimeStep;
+		}
+		else
+		{
+			positionalError = -penetration * erp * invTimeStep;
+		}
 
+		btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
+		btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
 
+		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		{
+			//combine position and velocity into rhs
+			solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;  //-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
+			solverConstraint.m_rhsPenetration = 0.f;
+		}
+		else
+		{
+			//split position and velocity into rhs and m_rhsPenetration
+			solverConstraint.m_rhs = velocityImpulse;
+			solverConstraint.m_rhsPenetration = penetrationImpulse;
+		}
+		solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
+		solverConstraint.m_lowerLimit = 0;
+		solverConstraint.m_upperLimit = 1e10f;
+	}
 }
 
-
-
-void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
-																		int solverBodyIdA, int solverBodyIdB,
-																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
+void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverConstraint& solverConstraint,
+	int solverBodyIdA, int solverBodyIdB,
+	btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
 {
-
-	btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
-	btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
-
-	btRigidBody* rb0 = bodyA->m_originalBody;
-	btRigidBody* rb1 = bodyB->m_originalBody;
-
 	{
 		btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
-		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-		{
-			frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
-			if (rb0)
-				bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
-			if (rb1)
-				bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
-		} else
-		{
-			frictionConstraint1.m_appliedImpulse = 0.f;
-		}
+		
+		frictionConstraint1.m_appliedImpulse = 0.f;
 	}
 
 	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 	{
-		btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
-		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-		{
-			frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2  * infoGlobal.m_warmstartingFactor;
-			if (rb0)
-				bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
-			if (rb1)
-				bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
-		} else
-		{
-			frictionConstraint2.m_appliedImpulse = 0.f;
-		}
+		btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
+		
+		frictionConstraint2.m_appliedImpulse = 0.f;
 	}
 }
 
-
-
-
-void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
+void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal)
 {
-	btCollisionObject* colObj0=0,*colObj1=0;
+	btCollisionObject *colObj0 = 0, *colObj1 = 0;
 
 	colObj0 = (btCollisionObject*)manifold->getBody0();
 	colObj1 = (btCollisionObject*)manifold->getBody1();
 
-	int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
-	int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
+	int solverBodyIdA = getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
+	int solverBodyIdB = getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
 
-//	btRigidBody* bodyA = btRigidBody::upcast(colObj0);
-//	btRigidBody* bodyB = btRigidBody::upcast(colObj1);
+	//	btRigidBody* bodyA = btRigidBody::upcast(colObj0);
+	//	btRigidBody* bodyB = btRigidBody::upcast(colObj1);
 
 	btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
 	btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
 
-
-
 	///avoid collision response between two static objects
 	if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
 		return;
 
-	int rollingFriction=1;
-	for (int j=0;j<manifold->getNumContacts();j++)
+	int rollingFriction = 1;
+	for (int j = 0; j < manifold->getNumContacts(); j++)
 	{
-
 		btManifoldPoint& cp = manifold->getContactPoint(j);
 
 		if (cp.getDistance() <= manifold->getContactProcessingThreshold())
@@ -1007,11 +1027,8 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			btVector3 rel_pos2;
 			btScalar relaxation;
 
-
 			int frictionIndex = m_tmpSolverContactConstraintPool.size();
 			btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
-			btRigidBody* rb0 = btRigidBody::upcast(colObj0);
-			btRigidBody* rb1 = btRigidBody::upcast(colObj1);
 			solverConstraint.m_solverBodyIdA = solverBodyIdA;
 			solverConstraint.m_solverBodyIdB = solverBodyIdB;
 
@@ -1023,61 +1040,42 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
 			rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
 
-			btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
-			btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+			btVector3 vel1;
+			btVector3 vel2;
 
-			solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1);
-			solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 );
+			solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1, vel1);
+			solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2, vel2);
 
-			btVector3 vel  = vel1 - 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();
-
 			/////setup the friction constraints
 
 			solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
 
-			btVector3 angVelA(0,0,0),angVelB(0,0,0);
-			if (rb0)
-				angVelA = rb0->getAngularVelocity();
-			if (rb1)
-				angVelB = rb1->getAngularVelocity();
-			btVector3 relAngVel = angVelB-angVelA;
-
-			if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
+			if ((cp.m_combinedRollingFriction > 0.f) && (rollingFriction > 0))
 			{
-				//only a single rollingFriction per manifold
-				rollingFriction--;
-				if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
-				{
-					relAngVel.normalize();
-					applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					if (relAngVel.length()>0.001)
-						addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
-				} else
 				{
-					addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-					btVector3 axis0,axis1;
-					btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
-					applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					if (axis0.length()>0.001)
-						addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-					if (axis1.length()>0.001)
-						addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
+					addTorsionalFrictionConstraint(cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedSpinningFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+					btVector3 axis0, axis1;
+					btPlaneSpace1(cp.m_normalWorldOnB, axis0, axis1);
+					axis0.normalize();
+					axis1.normalize();
+
+					applyAnisotropicFriction(colObj0, axis0, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj1, axis0, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj0, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj1, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					if (axis0.length() > 0.001)
+						addTorsionalFrictionConstraint(axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
+							cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+					if (axis1.length() > 0.001)
+						addTorsionalFrictionConstraint(axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
+							cp.m_combinedRollingFriction, 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
@@ -1090,408 +1088,409 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
 			///
 			///The user can manually override the friction directions for certain contacts using a contact callback,
-			///and set the cp.m_lateralFrictionInitialized to true
+			///and use contactPoint.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
 			///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
 			///this will give a conveyor belt effect
 			///
-			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+
+			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
 			{
 				cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
 				btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
 				if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
 				{
-					cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
-					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel);
+					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 
-					if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
-						cp.m_lateralFrictionDir2.normalize();//??
-						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+						cp.m_lateralFrictionDir2.normalize();  //??
+						applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+						applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+						addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 					}
-
-				} else
+				}
+				else
 				{
-					btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+					btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
 
-					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
-						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+						applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+						applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+						addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 					}
 
-
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
 					{
-						cp.m_lateralFrictionInitialized = true;
+						cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
 					}
 				}
-
-			} else
+			}
+			else
 			{
-				addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
+				addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
 
 				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-					addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
-
+					addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
+				}
+			setFrictionConstraintImpulse(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
 			}
-			setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
-
-
-
-
 		}
 	}
-}
 
-void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
+void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
 {
 	int i;
 	btPersistentManifold* manifold = 0;
-//			btCollisionObject* colObj0=0,*colObj1=0;
-
+	//			btCollisionObject* colObj0=0,*colObj1=0;
 
-	for (i=0;i<numManifolds;i++)
+	for (i = 0; i < numManifolds; i++)
 	{
 		manifold = manifoldPtr[i];
-		convertContact(manifold,infoGlobal);
+		convertContact(manifold, infoGlobal);
 	}
 }
 
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* currentConstraintRow,
+	btTypedConstraint* constraint,
+	const btTypedConstraint::btConstraintInfo1& info1,
+	int solverBodyIdA,
+	int solverBodyIdB,
+	const btContactSolverInfo& infoGlobal)
 {
-	m_fixedBodyId = -1;
-	BT_PROFILE("solveGroupCacheFriendlySetup");
-	(void)debugDrawer;
+	const btRigidBody& rbA = constraint->getRigidBodyA();
+	const btRigidBody& rbB = constraint->getRigidBodyB();
 
-	m_maxOverrideNumSolverIterations = 0;
+	const btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
+	const btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
 
-#ifdef BT_ADDITIONAL_DEBUG
-	 //make sure that dynamic bodies exist for all (enabled) constraints
-	for (int i=0;i<numConstraints;i++)
+	int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
+	if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations)
+		m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
+
+	for (int j = 0; j < info1.m_numConstraintRows; j++)
 	{
-		btTypedConstraint* constraint = constraints[i];
-		if (constraint->isEnabled())
+		memset(&currentConstraintRow[j], 0, sizeof(btSolverConstraint));
+		currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
+		currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
+		currentConstraintRow[j].m_appliedImpulse = 0.f;
+		currentConstraintRow[j].m_appliedPushImpulse = 0.f;
+		currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
+		currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
+		currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
+	}
+
+	// these vectors are already cleared in initSolverBody, no need to redundantly clear again
+	btAssert(bodyAPtr->getDeltaLinearVelocity().isZero());
+	btAssert(bodyAPtr->getDeltaAngularVelocity().isZero());
+	btAssert(bodyAPtr->getPushVelocity().isZero());
+	btAssert(bodyAPtr->getTurnVelocity().isZero());
+	btAssert(bodyBPtr->getDeltaLinearVelocity().isZero());
+	btAssert(bodyBPtr->getDeltaAngularVelocity().isZero());
+	btAssert(bodyBPtr->getPushVelocity().isZero());
+	btAssert(bodyBPtr->getTurnVelocity().isZero());
+	//bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+	//bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
+	//bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
+	//bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
+	//bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+	//bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
+	//bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
+	//bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
+
+	btTypedConstraint::btConstraintInfo2 info2;
+	info2.fps = 1.f / infoGlobal.m_timeStep;
+	info2.erp = infoGlobal.m_erp;
+	info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
+	info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
+	info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
+	info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
+	info2.rowskip = sizeof(btSolverConstraint) / sizeof(btScalar);  //check this
+																	///the size of btSolverConstraint needs be a multiple of btScalar
+	btAssert(info2.rowskip * sizeof(btScalar) == sizeof(btSolverConstraint));
+	info2.m_constraintError = &currentConstraintRow->m_rhs;
+	currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
+	info2.m_damping = infoGlobal.m_damping;
+	info2.cfm = &currentConstraintRow->m_cfm;
+	info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
+	info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
+	info2.m_numIterations = infoGlobal.m_numIterations;
+	constraint->getInfo2(&info2);
+
+	///finalize the constraint setup
+	for (int j = 0; j < info1.m_numConstraintRows; j++)
+	{
+		btSolverConstraint& solverConstraint = currentConstraintRow[j];
+
+		if (solverConstraint.m_upperLimit >= constraint->getBreakingImpulseThreshold())
 		{
-			if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
-			{
-				bool found=false;
-				for (int b=0;b<numBodies;b++)
-				{
+			solverConstraint.m_upperLimit = constraint->getBreakingImpulseThreshold();
+		}
 
-					if (&constraint->getRigidBodyA()==bodies[b])
-					{
-						found = true;
-						break;
-					}
-				}
-				btAssert(found);
-			}
-			if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
-			{
-				bool found=false;
-				for (int b=0;b<numBodies;b++)
-				{
-					if (&constraint->getRigidBodyB()==bodies[b])
-					{
-						found = true;
-						break;
-					}
-				}
-				btAssert(found);
-			}
+		if (solverConstraint.m_lowerLimit <= -constraint->getBreakingImpulseThreshold())
+		{
+			solverConstraint.m_lowerLimit = -constraint->getBreakingImpulseThreshold();
+		}
+
+		solverConstraint.m_originalContactPoint = constraint;
+
+		{
+			const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
+			solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld() * ftorqueAxis1 * constraint->getRigidBodyA().getAngularFactor();
+		}
+		{
+			const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
+			solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld() * ftorqueAxis2 * constraint->getRigidBodyB().getAngularFactor();
+		}
+
+		{
+			btVector3 iMJlA = solverConstraint.m_contactNormal1 * rbA.getInvMass();
+			btVector3 iMJaA = rbA.getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal;
+			btVector3 iMJlB = solverConstraint.m_contactNormal2 * rbB.getInvMass();  //sign of normal?
+			btVector3 iMJaB = rbB.getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal;
+
+			btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
+			sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
+			sum += iMJlB.dot(solverConstraint.m_contactNormal2);
+			sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
+			btScalar fsum = btFabs(sum);
+			btAssert(fsum > SIMD_EPSILON);
+			btScalar sorRelaxation = 1.f;  //todo: get from globalInfo?
+			solverConstraint.m_jacDiagABInv = fsum > SIMD_EPSILON ? sorRelaxation / sum : 0.f;
 		}
+
+		{
+			btScalar rel_vel;
+			btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0, 0, 0);
+			btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0, 0, 0);
+
+			btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0, 0, 0);
+			btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalTorqueImpulse : btVector3(0, 0, 0);
+
+			btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity() + externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity() + externalTorqueImpulseA);
+
+			btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity() + externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity() + externalTorqueImpulseB);
+
+			rel_vel = vel1Dotn + vel2Dotn;
+			btScalar restitution = 0.f;
+			btScalar positionalError = solverConstraint.m_rhs;  //already filled in by getConstraintInfo2
+			btScalar velocityError = restitution - rel_vel * info2.m_damping;
+			btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
+			btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
+			solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
+			solverConstraint.m_appliedImpulse = 0.f;
+		}
+	}
+}
+
+void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("convertJoints");
+	for (int j = 0; j < numConstraints; j++)
+	{
+		btTypedConstraint* constraint = constraints[j];
+		constraint->buildJacobian();
+		constraint->internalSetAppliedImpulse(0.0f);
 	}
-    //make sure that dynamic bodies exist for all contact manifolds
-    for (int i=0;i<numManifolds;i++)
-    {
-        if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
-        {
-            bool found=false;
-            for (int b=0;b<numBodies;b++)
-            {
-
-                if (manifoldPtr[i]->getBody0()==bodies[b])
-                {
-                    found = true;
-                    break;
-                }
-            }
-            btAssert(found);
-        }
-        if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
-        {
-            bool found=false;
-            for (int b=0;b<numBodies;b++)
-            {
-                if (manifoldPtr[i]->getBody1()==bodies[b])
-                {
-                    found = true;
-                    break;
-                }
-            }
-            btAssert(found);
-        }
-    }
-#endif //BT_ADDITIONAL_DEBUG
 
+	int totalNumRows = 0;
 
+	m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
+	//calculate the total number of contraint rows
+	for (int i = 0; i < numConstraints; i++)
+	{
+		btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+		btJointFeedback* fb = constraints[i]->getJointFeedback();
+		if (fb)
+		{
+			fb->m_appliedForceBodyA.setZero();
+			fb->m_appliedTorqueBodyA.setZero();
+			fb->m_appliedForceBodyB.setZero();
+			fb->m_appliedTorqueBodyB.setZero();
+		}
+
+		if (constraints[i]->isEnabled())
+		{
+			constraints[i]->getInfo1(&info1);
+		}
+		else
+		{
+			info1.m_numConstraintRows = 0;
+			info1.nub = 0;
+		}
+		totalNumRows += info1.m_numConstraintRows;
+	}
+	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
+
+	///setup the btSolverConstraints
+	int currentRow = 0;
+
+	for (int i = 0; i < numConstraints; i++)
+	{
+		const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+
+		if (info1.m_numConstraintRows)
+		{
+			btAssert(currentRow < totalNumRows);
+
+			btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
+			btTypedConstraint* constraint = constraints[i];
+			btRigidBody& rbA = constraint->getRigidBodyA();
+			btRigidBody& rbB = constraint->getRigidBodyB();
+
+			int solverBodyIdA = getOrInitSolverBody(rbA, infoGlobal.m_timeStep);
+			int solverBodyIdB = getOrInitSolverBody(rbB, infoGlobal.m_timeStep);
+
+			convertJoint(currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal);
+		}
+		currentRow += info1.m_numConstraintRows;
+	}
+}
+
+void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("convertBodies");
 	for (int i = 0; i < numBodies; i++)
 	{
 		bodies[i]->setCompanionId(-1);
 	}
+#if BT_THREADSAFE
+	m_kinematicBodyUniqueIdToSolverBodyTable.resize(0);
+#endif  // BT_THREADSAFE
 
-
-	m_tmpSolverBodyPool.reserve(numBodies+1);
+	m_tmpSolverBodyPool.reserve(numBodies + 1);
 	m_tmpSolverBodyPool.resize(0);
 
 	//btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
-    //initSolverBody(&fixedBody,0);
+	//initSolverBody(&fixedBody,0);
 
-	//convert all bodies
-
-
-	for (int i=0;i<numBodies;i++)
+	for (int i = 0; i < numBodies; i++)
 	{
-		int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
+		int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
 
 		btRigidBody* body = btRigidBody::upcast(bodies[i]);
 		if (body && body->getInvMass())
 		{
 			btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
-			btVector3 gyroForce (0,0,0);
-			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
+			btVector3 gyroForce(0, 0, 0);
+			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
 			{
 				gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
-				solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
+				solverBody.m_externalTorqueImpulse -= gyroForce * body->getInvInertiaTensorWorld() * infoGlobal.m_timeStep;
 			}
-			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
+			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
 			{
 				gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
 				solverBody.m_externalTorqueImpulse += gyroForce;
 			}
-			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
+			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
 			{
 				gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
 				solverBody.m_externalTorqueImpulse += gyroForce;
-
 			}
-			
-
 		}
 	}
+}
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
+	m_fixedBodyId = -1;
+	BT_PROFILE("solveGroupCacheFriendlySetup");
+	(void)debugDrawer;
 
-	if (1)
+	// if solver mode has changed,
+	if (infoGlobal.m_solverMode != m_cachedSolverMode)
 	{
-		int j;
-		for (j=0;j<numConstraints;j++)
-		{
-			btTypedConstraint* constraint = constraints[j];
-			constraint->buildJacobian();
-			constraint->internalSetAppliedImpulse(0.0f);
-		}
+		// update solver functions to use SIMD or non-SIMD
+		bool useSimd = !!(infoGlobal.m_solverMode & SOLVER_SIMD);
+		setupSolverFunctions(useSimd);
+		m_cachedSolverMode = infoGlobal.m_solverMode;
 	}
+	m_maxOverrideNumSolverIterations = 0;
 
-	//btRigidBody* rb0=0,*rb1=0;
-
-	//if (1)
+#ifdef BT_ADDITIONAL_DEBUG
+	//make sure that dynamic bodies exist for all (enabled) constraints
+	for (int i = 0; i < numConstraints; i++)
 	{
+		btTypedConstraint* constraint = constraints[i];
+		if (constraint->isEnabled())
 		{
-
-			int totalNumRows = 0;
-			int i;
-
-			m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
-			//calculate the total number of contraint rows
-			for (i=0;i<numConstraints;i++)
+			if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
 			{
-				btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
-				btJointFeedback* fb = constraints[i]->getJointFeedback();
-				if (fb)
+				bool found = false;
+				for (int b = 0; b < numBodies; b++)
 				{
-					fb->m_appliedForceBodyA.setZero();
-					fb->m_appliedTorqueBodyA.setZero();
-					fb->m_appliedForceBodyB.setZero();
-					fb->m_appliedTorqueBodyB.setZero();
+					if (&constraint->getRigidBodyA() == bodies[b])
+					{
+						found = true;
+						break;
+					}
 				}
-
-				if (constraints[i]->isEnabled())
+				btAssert(found);
+			}
+			if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
+			{
+				bool found = false;
+				for (int b = 0; b < numBodies; b++)
 				{
+					if (&constraint->getRigidBodyB() == bodies[b])
+					{
+						found = true;
+						break;
+					}
 				}
-				if (constraints[i]->isEnabled())
-				{
-					constraints[i]->getInfo1(&info1);
-				} else
+				btAssert(found);
+			}
+		}
+	}
+	//make sure that dynamic bodies exist for all contact manifolds
+	for (int i = 0; i < numManifolds; i++)
+	{
+		if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
+		{
+			bool found = false;
+			for (int b = 0; b < numBodies; b++)
+			{
+				if (manifoldPtr[i]->getBody0() == bodies[b])
 				{
-					info1.m_numConstraintRows = 0;
-					info1.nub = 0;
+					found = true;
+					break;
 				}
-				totalNumRows += info1.m_numConstraintRows;
 			}
-			m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
-
-
-			///setup the btSolverConstraints
-			int currentRow = 0;
-
-			for (i=0;i<numConstraints;i++)
+			btAssert(found);
+		}
+		if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
+		{
+			bool found = false;
+			for (int b = 0; b < numBodies; b++)
 			{
-				const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
-
-				if (info1.m_numConstraintRows)
+				if (manifoldPtr[i]->getBody1() == bodies[b])
 				{
-					btAssert(currentRow<totalNumRows);
-
-					btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
-					btTypedConstraint* constraint = constraints[i];
-					btRigidBody& rbA = constraint->getRigidBodyA();
-					btRigidBody& rbB = constraint->getRigidBodyB();
-
-					int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep);
-                    int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep);
-
-                    btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
-                    btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
-
-
-
-
-					int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
-					if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
-						m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
-
-
-					int j;
-					for ( j=0;j<info1.m_numConstraintRows;j++)
-					{
-						memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
-						currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
-						currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
-						currentConstraintRow[j].m_appliedImpulse = 0.f;
-						currentConstraintRow[j].m_appliedPushImpulse = 0.f;
-						currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
-						currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
-						currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
-					}
-
-					bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
-					bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
-					bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
-					bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
-					bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
-					bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
-					bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
-					bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
-
-
-					btTypedConstraint::btConstraintInfo2 info2;
-					info2.fps = 1.f/infoGlobal.m_timeStep;
-					info2.erp = infoGlobal.m_erp;
-					info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
-					info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
-					info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
-					info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
-					info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
-					///the size of btSolverConstraint needs be a multiple of btScalar
-		            btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
-					info2.m_constraintError = &currentConstraintRow->m_rhs;
-					currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
-					info2.m_damping = infoGlobal.m_damping;
-					info2.cfm = &currentConstraintRow->m_cfm;
-					info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
-					info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
-					info2.m_numIterations = infoGlobal.m_numIterations;
-					constraints[i]->getInfo2(&info2);
-
-					///finalize the constraint setup
-					for ( j=0;j<info1.m_numConstraintRows;j++)
-					{
-						btSolverConstraint& solverConstraint = currentConstraintRow[j];
-
-						if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
-						{
-							solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
-						}
-
-						if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
-						{
-							solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
-						}
-
-						solverConstraint.m_originalContactPoint = constraint;
-
-						{
-							const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
-							solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
-						}
-						{
-							const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
-							solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
-						}
-
-						{
-							btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass();
-							btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
-							btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal?
-							btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
-
-							btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
-							sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
-							sum += iMJlB.dot(solverConstraint.m_contactNormal2);
-							sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
-							btScalar fsum = btFabs(sum);
-							btAssert(fsum > SIMD_EPSILON);
-							solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
-						}
-
-
-
-						{
-							btScalar rel_vel;
-							btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
-							btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0);
-
-							btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0);
-							btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
-
-							btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
-												+ solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
-
-							btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
-																+ solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
-
-							rel_vel = vel1Dotn+vel2Dotn;
-							btScalar restitution = 0.f;
-							btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
-							btScalar	velocityError = restitution - rel_vel * info2.m_damping;
-							btScalar	penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
-							btScalar	velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
-							solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
-							solverConstraint.m_appliedImpulse = 0.f;
-
-
-						}
-					}
+					found = true;
+					break;
 				}
-				currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
 			}
+			btAssert(found);
 		}
+	}
+#endif  //BT_ADDITIONAL_DEBUG
 
-		convertContacts(manifoldPtr,numManifolds,infoGlobal);
+	//convert all bodies
+	convertBodies(bodies, numBodies, infoGlobal);
 
-	}
+	convertJoints(constraints, numConstraints, infoGlobal);
 
-//	btContactSolverInfo info = infoGlobal;
+	convertContacts(manifoldPtr, numManifolds, infoGlobal);
 
+	//	btContactSolverInfo info = infoGlobal;
 
 	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
 	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
@@ -1500,34 +1499,34 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 	///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
 	m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
 	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-		m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
+		m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2);
 	else
 		m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
 
 	m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
 	{
 		int i;
-		for (i=0;i<numNonContactPool;i++)
+		for (i = 0; i < numNonContactPool; i++)
 		{
 			m_orderNonContactConstraintPool[i] = i;
 		}
-		for (i=0;i<numConstraintPool;i++)
+		for (i = 0; i < numConstraintPool; i++)
 		{
 			m_orderTmpConstraintPool[i] = i;
 		}
-		for (i=0;i<numFrictionPool;i++)
+		for (i = 0; i < numFrictionPool; i++)
 		{
 			m_orderFrictionConstraintPool[i] = i;
 		}
 	}
 
 	return 0.f;
-
 }
 
-
-btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
+btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/)
 {
+	BT_PROFILE("solveSingleIteration");
+	btScalar leastSquaresResidual = 0.f;
 
 	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
 	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
@@ -1535,29 +1534,31 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 
 	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
 	{
-		if (1)			// uncomment this for a bit less random ((iteration & 7) == 0)
+		if (1)  // uncomment this for a bit less random ((iteration & 7) == 0)
 		{
-
-			for (int j=0; j<numNonContactPool; ++j) {
+			for (int j = 0; j < numNonContactPool; ++j)
+			{
 				int tmp = m_orderNonContactConstraintPool[j];
-				int swapi = btRandInt2(j+1);
+				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)
+			if (iteration < infoGlobal.m_numIterations)
 			{
-				for (int j=0; j<numConstraintPool; ++j) {
+				for (int j = 0; j < numConstraintPool; ++j)
+				{
 					int tmp = m_orderTmpConstraintPool[j];
-					int swapi = btRandInt2(j+1);
+					int swapi = btRandInt2(j + 1);
 					m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
 					m_orderTmpConstraintPool[swapi] = tmp;
 				}
 
-				for (int j=0; j<numFrictionPool; ++j) {
+				for (int j = 0; j < numFrictionPool; ++j)
+				{
 					int tmp = m_orderFrictionConstraintPool[j];
-					int swapi = btRandInt2(j+1);
+					int swapi = btRandInt2(j + 1);
 					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
 					m_orderFrictionConstraintPool[swapi] = tmp;
 				}
@@ -1565,313 +1566,248 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 		}
 	}
 
-	if (infoGlobal.m_solverMode & SOLVER_SIMD)
+	///solve all joint constraints
+	for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
 	{
-		///solve all joint constraints, using SIMD, if available
-		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+		if (iteration < constraint.m_overrideNumSolverIterations)
 		{
-			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-			if (iteration < constraint.m_overrideNumSolverIterations)
-				resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+			btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint);
+			leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 		}
+	}
 
-		if (iteration< infoGlobal.m_numIterations)
+	if (iteration < infoGlobal.m_numIterations)
+	{
+		for (int j = 0; j < numConstraints; j++)
 		{
-			for (int j=0;j<numConstraints;j++)
+			if (constraints[j]->isEnabled())
 			{
-				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);
-				}
+				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]];
-						resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-						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;
-
-								resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-							}
-						}
-
-						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;
-
-								resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-							}
-						}
-					}
-				}
+		///solve all contact constraints
+		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;
 
-			}
-			else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			for (int c = 0; c < numPoolConstraints; c++)
 			{
-				//solve the friction constraints after all contact constraints, don't interleave them
-				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-				int j;
+				btScalar totalImpulse = 0;
 
-				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);
+					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
+					btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+					leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 
+					totalImpulse = solveManifold.m_appliedImpulse;
 				}
-
-
-
-				///solve all friction constraints, using SIMD, if available
-
-				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-				for (j=0;j<numFrictionPoolConstraints;j++)
+				bool applyFriction = true;
+				if (applyFriction)
 				{
-					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);
-					}
-				}
+						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;
 
-				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-				for (j=0;j<numRollingFrictionPoolConstraints;j++)
-				{
+							btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+							leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+						}
+					}
 
-					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-					btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
-					if (totalImpulse>btScalar(0))
+					if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
 					{
-						btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-						if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
-							rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+						btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]];
 
-						rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
-						rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+						if (totalImpulse > btScalar(0))
+						{
+							solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+							solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 
-						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+							btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+							leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+						}
 					}
 				}
-
-
 			}
 		}
-	} else
-	{
-		//non-SIMD version
-		///solve all joint constraints
-		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
-		{
-			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-			if (iteration < constraint.m_overrideNumSolverIterations)
-				resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
-		}
-
-		if (iteration< infoGlobal.m_numIterations)
+		else  //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
 		{
-			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
+			//solve the friction constraints after all contact constraints, don't interleave them
 			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-			for (int j=0;j<numPoolConstraints;j++)
+			int j;
+
+			for (j = 0; j < numPoolConstraints; j++)
 			{
 				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-				resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+				btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+				leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 			}
+
 			///solve all friction constraints
+
 			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-			for (int j=0;j<numFrictionPoolConstraints;j++)
+			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))
+				if (totalImpulse > btScalar(0))
 				{
-					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-					solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+					solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+					solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 
-					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+					leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 				}
 			}
+		}
 
-			int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-			for (int j=0;j<numRollingFrictionPoolConstraints;j++)
+		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))
 			{
-				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;
+				btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+				if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
+					rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
 
-					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
-					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+				rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+				rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
 
-					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
-				}
+				btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
+				leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 			}
 		}
 	}
-	return 0.f;
+	return leastSquaresResidual;
 }
 
-
-void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
+	BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
 	int iteration;
 	if (infoGlobal.m_splitImpulse)
 	{
-		if (infoGlobal.m_solverMode & SOLVER_SIMD)
 		{
-			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+			for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
 			{
+				btScalar leastSquaresResidual = 0.f;
 				{
 					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 					int j;
-					for (j=0;j<numPoolConstraints;j++)
+					for (j = 0; j < numPoolConstraints; j++)
 					{
 						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
 
-						resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+						leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 					}
 				}
-			}
-		}
-		else
-		{
-			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
-			{
+				if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
 				{
-					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-					int j;
-					for (j=0;j<numPoolConstraints;j++)
-					{
-						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-
-						resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					}
+#ifdef VERBOSE_RESIDUAL_PRINTF
+					printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
+#endif
+					break;
 				}
 			}
 		}
 	}
 }
 
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	BT_PROFILE("solveGroupCacheFriendlyIterations");
 
 	{
 		///this is a special step to resolve penetrations (just for contacts)
-		solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+		solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
-		int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
+		int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
 
-		for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
-		//for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
+		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);
-		}
+			m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
+			if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
+			{
+#ifdef VERBOSE_RESIDUAL_PRINTF
+				printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
+#endif
+				m_analyticsData.m_numSolverCalls++;
+				m_analyticsData.m_numIterationsUsed = iteration+1;
+				m_analyticsData.m_islandId = -2;
+				if (numBodies>0)
+					m_analyticsData.m_islandId = bodies[0]->getCompanionId();
+				m_analyticsData.m_numBodies = numBodies;
+				m_analyticsData.m_numContactManifolds = numManifolds;
+				m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
+				break;
+			}
+		}
 	}
 	return 0.f;
 }
 
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
+void btSequentialImpulseConstraintSolver::writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
 {
-	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-	int i,j;
-
-	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	for (int j = iBegin; j < iEnd; j++)
 	{
-		for (j=0;j<numPoolConstraints;j++)
-		{
-			const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
-			btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
-			btAssert(pt);
-			pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
+		const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
+		btManifoldPoint* pt = (btManifoldPoint*)solveManifold.m_originalContactPoint;
+		btAssert(pt);
+		pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
 		//	float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-			//	printf("pt->m_appliedImpulseLateral1 = %f\n", f);
-			pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[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_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
-			}
-			//do a callback here?
+		//	printf("pt->m_appliedImpulseLateral1 = %f\n", f);
+		pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[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_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse;
 		}
+		//do a callback here?
 	}
+}
 
-	numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
-	for (j=0;j<numPoolConstraints;j++)
+void btSequentialImpulseConstraintSolver::writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+	for (int j = iBegin; j < iEnd; j++)
 	{
 		const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
 		btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
 		btJointFeedback* fb = constr->getJointFeedback();
 		if (fb)
 		{
-			fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
-			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 ???? */
-
+			fb->m_appliedForceBodyA += solverConstr.m_contactNormal1 * solverConstr.m_appliedImpulse * constr->getRigidBodyA().getLinearFactor() / infoGlobal.m_timeStep;
+			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);
-		if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
+		if (btFabs(solverConstr.m_appliedImpulse) >= constr->getBreakingImpulseThreshold())
 		{
 			constr->setEnabled(false);
 		}
 	}
+}
 
-
-
-	for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+void btSequentialImpulseConstraintSolver::writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+	for (int i = iBegin; i < iEnd; i++)
 	{
 		btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
 		if (body)
@@ -1882,11 +1818,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 				m_tmpSolverBodyPool[i].writebackVelocity();
 
 			m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
-				m_tmpSolverBodyPool[i].m_linearVelocity+
+				m_tmpSolverBodyPool[i].m_linearVelocity +
 				m_tmpSolverBodyPool[i].m_externalForceImpulse);
 
 			m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
-				m_tmpSolverBodyPool[i].m_angularVelocity+
+				m_tmpSolverBodyPool[i].m_angularVelocity +
 				m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
 
 			if (infoGlobal.m_splitImpulse)
@@ -1895,6 +1831,19 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 			m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
 		}
 	}
+}
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("solveGroupCacheFriendlyFinish");
+
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
+		writeBackContacts(0, m_tmpSolverContactConstraintPool.size(), infoGlobal);
+	}
+
+	writeBackJoints(0, m_tmpSolverNonContactConstraintPool.size(), infoGlobal);
+	writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal);
 
 	m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
 	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
@@ -1905,25 +1854,22 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 	return 0.f;
 }
 
-
-
 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
-btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btDispatcher* /*dispatcher*/)
+btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer, btDispatcher* /*dispatcher*/)
 {
-
 	BT_PROFILE("solveGroup");
 	//you need to provide at least some bodies
 
-	solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
+	solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
-	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
+	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
 	solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
 
 	return 0.f;
 }
 
-void	btSequentialImpulseConstraintSolver::reset()
+void btSequentialImpulseConstraintSolver::reset()
 {
 	m_btSeed2 = 0;
 }
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
index a6029180983..f3ef02fccc7 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
@@ -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.
@@ -27,121 +27,161 @@ class btCollisionObject;
 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
 #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
 
-typedef btSimdScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
+typedef btScalar (*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
+
+struct btSolverAnalyticsData
+{
+	btSolverAnalyticsData()
+	{
+		m_numSolverCalls = 0;
+		m_numIterationsUsed = -1;
+		m_remainingLeastSquaresResidual = -1;
+		m_islandId = -2;
+	}
+	int m_islandId;
+	int m_numBodies;
+	int m_numContactManifolds;
+	int m_numSolverCalls;
+	int m_numIterationsUsed;
+	double m_remainingLeastSquaresResidual;
+};
 
 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
-ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver
+ATTRIBUTE_ALIGNED16(class)
+btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
+	
+
 protected:
-	btAlignedObjectArray<btSolverBody>      m_tmpSolverBodyPool;
-	btConstraintArray			m_tmpSolverContactConstraintPool;
-	btConstraintArray			m_tmpSolverNonContactConstraintPool;
-	btConstraintArray			m_tmpSolverContactFrictionConstraintPool;
-	btConstraintArray			m_tmpSolverContactRollingFrictionConstraintPool;
-
-	btAlignedObjectArray<int>	m_orderTmpConstraintPool;
-	btAlignedObjectArray<int>	m_orderNonContactConstraintPool;
-	btAlignedObjectArray<int>	m_orderFrictionConstraintPool;
+	btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
+	btConstraintArray m_tmpSolverContactConstraintPool;
+	btConstraintArray m_tmpSolverNonContactConstraintPool;
+	btConstraintArray m_tmpSolverContactFrictionConstraintPool;
+	btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
+
+	btAlignedObjectArray<int> m_orderTmpConstraintPool;
+	btAlignedObjectArray<int> m_orderNonContactConstraintPool;
+	btAlignedObjectArray<int> m_orderFrictionConstraintPool;
 	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
-	int							m_maxOverrideNumSolverIterations;
+	int m_maxOverrideNumSolverIterations;
 	int m_fixedBodyId;
+	// When running solvers on multiple threads, a race condition exists for Kinematic objects that
+	// participate in more than one solver.
+	// The getOrInitSolverBody() function writes the companionId of each body (storing the index of the solver body
+	// for the current solver). For normal dynamic bodies it isn't an issue because they can only be in one island
+	// (and therefore one thread) at a time. But kinematic bodies can be in multiple islands at once.
+	// To avoid this race condition, this solver does not write the companionId, instead it stores the solver body
+	// index in this solver-local table, indexed by the uniqueId of the body.
+	btAlignedObjectArray<int> m_kinematicBodyUniqueIdToSolverBodyTable;  // only used for multithreading
 
 	btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
 	btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
+	btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse;
+	int m_cachedSolverMode;  // used to check if SOLVER_SIMD flag has been changed
+	void setupSolverFunctions(bool useSimd);
 
-	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, 
-									btScalar desiredVelocity=0., btScalar cfmSlip=0.);
+	btScalar m_leastSquaresResidual;
 
-	void setupRollingFrictionConstraint(	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=0., btScalar cfmSlip=0.);
+	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,
+		const btContactSolverInfo& infoGlobal,
+		btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
 
-	btSolverConstraint&	addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
-	btSolverConstraint&	addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
+	void setupTorsionalFrictionConstraint(btSolverConstraint & solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
+		btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
+		btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
+		btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
 
-	
-	void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, 
-								const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
+	btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
+	btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar torsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity = 0, btScalar cfmSlip = 0.f);
+
+	void setupContactConstraint(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
+		const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
 
-	static void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
+	static void applyAnisotropicFriction(btCollisionObject * colObj, btVector3 & frictionDirection, int frictionMode);
 
-	void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, 
-										 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
+	void setFrictionConstraintImpulse(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB,
+		btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
 
 	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
-	unsigned long	m_btSeed2;
+	unsigned long m_btSeed2;
 
-	
-	btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
+	btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold);
+
+	virtual void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
 
-	virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
+	void convertContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
 
-	void	convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
+	virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
+	void convertJoint(btSolverConstraint * currentConstraintRow, btTypedConstraint * constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
 
+	virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
 
-	void	resolveSplitPenetrationSIMD(
-     btSolverBody& bodyA,btSolverBody& bodyB,
-        const btSolverConstraint& contactConstraint);
+	btScalar resolveSplitPenetrationSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
+	{
+		return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
+	}
 
-	void	resolveSplitPenetrationImpulseCacheFriendly(
-       btSolverBody& bodyA,btSolverBody& bodyB,
-        const btSolverConstraint& contactConstraint);
+	btScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
+	{
+		return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
+	}
 
 	//internal method
-	int		getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
-	void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
-
-	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:
-	
-	
-	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
-	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+	int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep);
+	void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep);
+
+	btScalar resolveSingleConstraintRowGeneric(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
+	btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
+	btScalar resolveSingleConstraintRowLowerLimit(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
+	btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
+	btScalar resolveSplitPenetrationImpulse(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
+	{
+		return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
+	}
 
-	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);
+protected:
+	void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
+	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	btSequentialImpulseConstraintSolver();
 	virtual ~btSequentialImpulseConstraintSolver();
 
-	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
-		
+	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();
-	
+	virtual void reset();
+
 	unsigned long btRand2();
 
-	int btRandInt2 (int n);
+	int btRandInt2(int n);
 
-	void	setRandSeed(unsigned long seed)
+	void setRandSeed(unsigned long seed)
 	{
 		m_btSeed2 = seed;
 	}
-	unsigned long	getRandSeed() const
+	unsigned long getRandSeed() const
 	{
 		return m_btSeed2;
 	}
 
-	
-	virtual btConstraintSolverType	getSolverType() const
+	virtual btConstraintSolverType getSolverType() const
 	{
 		return BT_SEQUENTIAL_IMPULSE_SOLVER;
 	}
 
-	btSingleConstraintRowSolver	getActiveConstraintRowSolverGeneric()
+	btSingleConstraintRowSolver getActiveConstraintRowSolverGeneric()
 	{
 		return m_resolveSingleConstraintRowGeneric;
 	}
@@ -149,7 +189,7 @@ public:
 	{
 		m_resolveSingleConstraintRowGeneric = rowSolver;
 	}
-	btSingleConstraintRowSolver	getActiveConstraintRowSolverLowerLimit()
+	btSingleConstraintRowSolver getActiveConstraintRowSolverLowerLimit()
 	{
 		return m_resolveSingleConstraintRowLowerLimit;
 	}
@@ -158,19 +198,18 @@ public:
 		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();
+	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();
+	btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
+	btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
+	btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
+	btSolverAnalyticsData m_analyticsData;
 };
 
-
-
-
-#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
-
+#endif  //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp
new file mode 100644
index 00000000000..2718da4a501
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp
@@ -0,0 +1,1554 @@
+/*
+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 "btSequentialImpulseConstraintSolverMt.h"
+
+#include "LinearMath/btQuickprof.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+
+bool btSequentialImpulseConstraintSolverMt::s_allowNestedParallelForLoops = false;  // some task schedulers don't like nested loops
+int btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching = 250;
+int btSequentialImpulseConstraintSolverMt::s_minBatchSize = 50;
+int btSequentialImpulseConstraintSolverMt::s_maxBatchSize = 100;
+btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_contactBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D;
+btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_jointBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D;
+
+btSequentialImpulseConstraintSolverMt::btSequentialImpulseConstraintSolverMt()
+{
+	m_numFrictionDirections = 1;
+	m_useBatching = false;
+	m_useObsoleteJointConstraints = false;
+}
+
+btSequentialImpulseConstraintSolverMt::~btSequentialImpulseConstraintSolverMt()
+{
+}
+
+void btSequentialImpulseConstraintSolverMt::setupBatchedContactConstraints()
+{
+	BT_PROFILE("setupBatchedContactConstraints");
+	m_batchedContactConstraints.setup(&m_tmpSolverContactConstraintPool,
+									  m_tmpSolverBodyPool,
+									  s_contactBatchingMethod,
+									  s_minBatchSize,
+									  s_maxBatchSize,
+									  &m_scratchMemory);
+}
+
+void btSequentialImpulseConstraintSolverMt::setupBatchedJointConstraints()
+{
+	BT_PROFILE("setupBatchedJointConstraints");
+	m_batchedJointConstraints.setup(&m_tmpSolverNonContactConstraintPool,
+									m_tmpSolverBodyPool,
+									s_jointBatchingMethod,
+									s_minBatchSize,
+									s_maxBatchSize,
+									&m_scratchMemory);
+}
+
+void btSequentialImpulseConstraintSolverMt::internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal)
+{
+	btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[iContactConstraint];
+
+	btVector3 rel_pos1;
+	btVector3 rel_pos2;
+	btScalar relaxation;
+
+	int solverBodyIdA = contactConstraint.m_solverBodyIdA;
+	int solverBodyIdB = contactConstraint.m_solverBodyIdB;
+
+	btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+	btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+
+	btRigidBody* colObj0 = solverBodyA->m_originalBody;
+	btRigidBody* colObj1 = solverBodyB->m_originalBody;
+
+	btManifoldPoint& cp = *static_cast<btManifoldPoint*>(contactConstraint.m_originalContactPoint);
+
+	const btVector3& pos1 = cp.getPositionWorldOnA();
+	const btVector3& pos2 = cp.getPositionWorldOnB();
+
+	rel_pos1 = pos1 - solverBodyA->getWorldTransform().getOrigin();
+	rel_pos2 = pos2 - solverBodyB->getWorldTransform().getOrigin();
+
+	btVector3 vel1;
+	btVector3 vel2;
+
+	solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1, vel1);
+	solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2, vel2);
+
+	btVector3 vel = vel1 - vel2;
+	btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
+
+	setupContactConstraint(contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
+
+	// setup rolling friction constraints
+	int rollingFrictionIndex = m_rollingFrictionIndexTable[iContactConstraint];
+	if (rollingFrictionIndex >= 0)
+	{
+		btSolverConstraint& spinningFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[rollingFrictionIndex];
+		btAssert(spinningFrictionConstraint.m_frictionIndex == iContactConstraint);
+		setupTorsionalFrictionConstraint(spinningFrictionConstraint,
+										 cp.m_normalWorldOnB,
+										 solverBodyIdA,
+										 solverBodyIdB,
+										 cp,
+										 cp.m_combinedSpinningFriction,
+										 rel_pos1,
+										 rel_pos2,
+										 colObj0,
+										 colObj1,
+										 relaxation,
+										 0.0f,
+										 0.0f);
+		btVector3 axis[2];
+		btPlaneSpace1(cp.m_normalWorldOnB, axis[0], axis[1]);
+		axis[0].normalize();
+		axis[1].normalize();
+
+		applyAnisotropicFriction(colObj0, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+		applyAnisotropicFriction(colObj1, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+		applyAnisotropicFriction(colObj0, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+		applyAnisotropicFriction(colObj1, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+		// put the largest axis first
+		if (axis[1].length2() > axis[0].length2())
+		{
+			btSwap(axis[0], axis[1]);
+		}
+		const btScalar kRollingFrictionThreshold = 0.001f;
+		for (int i = 0; i < 2; ++i)
+		{
+			int iRollingFric = rollingFrictionIndex + 1 + i;
+			btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric];
+			btAssert(rollingFrictionConstraint.m_frictionIndex == iContactConstraint);
+			btVector3 dir = axis[i];
+			if (dir.length() > kRollingFrictionThreshold)
+			{
+				setupTorsionalFrictionConstraint(rollingFrictionConstraint,
+												 dir,
+												 solverBodyIdA,
+												 solverBodyIdB,
+												 cp,
+												 cp.m_combinedRollingFriction,
+												 rel_pos1,
+												 rel_pos2,
+												 colObj0,
+												 colObj1,
+												 relaxation,
+												 0.0f,
+												 0.0f);
+			}
+			else
+			{
+				rollingFrictionConstraint.m_frictionIndex = -1;  // disable constraint
+			}
+		}
+	}
+
+	// setup friction constraints
+	//	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
+	{
+		///Bullet has several options to set the friction directions
+		///By default, each contact has only a single friction direction that is recomputed automatically very frame
+		///based on the relative linear velocity.
+		///If the relative velocity it zero, it will automatically compute a friction direction.
+
+		///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
+		///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
+		///
+		///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
+		///
+		///The user can manually override the friction directions for certain contacts using a contact callback,
+		///and set the cp.m_lateralFrictionInitialized to true
+		///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
+		///this will give a conveyor belt effect
+		///
+		btSolverConstraint* frictionConstraint1 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex];
+		btAssert(frictionConstraint1->m_frictionIndex == iContactConstraint);
+
+		btSolverConstraint* frictionConstraint2 = NULL;
+		if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
+		{
+			frictionConstraint2 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex + 1];
+			btAssert(frictionConstraint2->m_frictionIndex == iContactConstraint);
+		}
+
+		if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
+		{
+			cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+			btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+			if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
+			{
+				cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel);
+				applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+				applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+				setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+
+				if (frictionConstraint2)
+				{
+					cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+					cp.m_lateralFrictionDir2.normalize();  //??
+					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+				}
+			}
+			else
+			{
+				btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
+
+				applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+				applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+				setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+
+				if (frictionConstraint2)
+				{
+					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+				}
+
+				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
+				{
+					cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
+				}
+			}
+		}
+		else
+		{
+			setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
+			if (frictionConstraint2)
+			{
+				setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
+			}
+		}
+	}
+
+	setFrictionConstraintImpulse(contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
+}
+
+struct SetupContactConstraintsLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+	const btContactSolverInfo* m_infoGlobal;
+
+	SetupContactConstraintsLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, const btContactSolverInfo& infoGlobal)
+	{
+		m_solver = solver;
+		m_bc = bc;
+		m_infoGlobal = &infoGlobal;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("SetupContactConstraintsLoop");
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			for (int i = batch.begin; i < batch.end; ++i)
+			{
+				int iContact = m_bc->m_constraintIndices[i];
+				m_solver->internalSetupContactConstraints(iContact, *m_infoGlobal);
+			}
+		}
+	}
+};
+
+void btSequentialImpulseConstraintSolverMt::setupAllContactConstraints(const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("setupAllContactConstraints");
+	if (m_useBatching)
+	{
+		const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+		SetupContactConstraintsLoop loop(this, &batchedCons, infoGlobal);
+		for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+		{
+			int iPhase = batchedCons.m_phaseOrder[iiPhase];
+			const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+			int grainSize = 1;
+			btParallelFor(phase.begin, phase.end, grainSize, loop);
+		}
+	}
+	else
+	{
+		for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); ++i)
+		{
+			internalSetupContactConstraints(i, infoGlobal);
+		}
+	}
+}
+
+int btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btCollisionObject& body, btScalar timeStep)
+{
+	//
+	// getOrInitSolverBody is threadsafe only for a single thread per solver (with potentially multiple solvers)
+	//
+	// getOrInitSolverBodyThreadsafe -- attempts to be fully threadsafe (however may affect determinism)
+	//
+	int solverBodyId = -1;
+	bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
+	if (isRigidBodyType && !body.isStaticOrKinematicObject())
+	{
+		// dynamic body
+		// Dynamic bodies can only be in one island, so it's safe to write to the companionId
+		solverBodyId = body.getCompanionId();
+		if (solverBodyId < 0)
+		{
+			m_bodySolverArrayMutex.lock();
+			// now that we have the lock, check again
+			solverBodyId = body.getCompanionId();
+			if (solverBodyId < 0)
+			{
+				solverBodyId = m_tmpSolverBodyPool.size();
+				btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+				initSolverBody(&solverBody, &body, timeStep);
+				body.setCompanionId(solverBodyId);
+			}
+			m_bodySolverArrayMutex.unlock();
+		}
+	}
+	else if (isRigidBodyType && body.isKinematicObject())
+	{
+		//
+		// NOTE: must test for kinematic before static because some kinematic objects also
+		//   identify as "static"
+		//
+		// Kinematic bodies can be in multiple islands at once, so it is a
+		// race condition to write to them, so we use an alternate method
+		// to record the solverBodyId
+		int uniqueId = body.getWorldArrayIndex();
+		const int INVALID_SOLVER_BODY_ID = -1;
+		if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId)
+		{
+			m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock();
+			// now that we have the lock, check again
+			if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId)
+			{
+				m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
+			}
+			m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock();
+		}
+		solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
+		// if no table entry yet,
+		if (INVALID_SOLVER_BODY_ID == solverBodyId)
+		{
+			// need to acquire both locks
+			m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock();
+			m_bodySolverArrayMutex.lock();
+			// now that we have the lock, check again
+			solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
+			if (INVALID_SOLVER_BODY_ID == solverBodyId)
+			{
+				// create a table entry for this body
+				solverBodyId = m_tmpSolverBodyPool.size();
+				btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+				initSolverBody(&solverBody, &body, timeStep);
+				m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId] = solverBodyId;
+			}
+			m_bodySolverArrayMutex.unlock();
+			m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock();
+		}
+	}
+	else
+	{
+		// all fixed bodies (inf mass) get mapped to a single solver id
+		if (m_fixedBodyId < 0)
+		{
+			m_bodySolverArrayMutex.lock();
+			// now that we have the lock, check again
+			if (m_fixedBodyId < 0)
+			{
+				m_fixedBodyId = m_tmpSolverBodyPool.size();
+				btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+				initSolverBody(&fixedBody, 0, timeStep);
+			}
+			m_bodySolverArrayMutex.unlock();
+		}
+		solverBodyId = m_fixedBodyId;
+	}
+	btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size());
+	return solverBodyId;
+}
+
+void btSequentialImpulseConstraintSolverMt::internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("internalCollectContactManifoldCachedInfo");
+	for (int i = 0; i < numManifolds; ++i)
+	{
+		btContactManifoldCachedInfo* cachedInfo = &cachedInfoArray[i];
+		btPersistentManifold* manifold = manifoldPtr[i];
+		btCollisionObject* colObj0 = (btCollisionObject*)manifold->getBody0();
+		btCollisionObject* colObj1 = (btCollisionObject*)manifold->getBody1();
+
+		int solverBodyIdA = getOrInitSolverBodyThreadsafe(*colObj0, infoGlobal.m_timeStep);
+		int solverBodyIdB = getOrInitSolverBodyThreadsafe(*colObj1, infoGlobal.m_timeStep);
+
+		cachedInfo->solverBodyIds[0] = solverBodyIdA;
+		cachedInfo->solverBodyIds[1] = solverBodyIdB;
+		cachedInfo->numTouchingContacts = 0;
+
+		btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+		btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+
+		// A contact manifold between 2 static object should not exist!
+		// check the collision flags of your objects if this assert fires.
+		// Incorrectly set collision object flags can degrade performance in various ways.
+		btAssert(!m_tmpSolverBodyPool[solverBodyIdA].m_invMass.isZero() || !m_tmpSolverBodyPool[solverBodyIdB].m_invMass.isZero());
+
+		int iContact = 0;
+		for (int j = 0; j < manifold->getNumContacts(); j++)
+		{
+			btManifoldPoint& cp = manifold->getContactPoint(j);
+
+			if (cp.getDistance() <= manifold->getContactProcessingThreshold())
+			{
+				cachedInfo->contactPoints[iContact] = &cp;
+				cachedInfo->contactHasRollingFriction[iContact] = (cp.m_combinedRollingFriction > 0.f);
+				iContact++;
+			}
+		}
+		cachedInfo->numTouchingContacts = iContact;
+	}
+}
+
+struct CollectContactManifoldCachedInfoLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray;
+	btPersistentManifold** m_manifoldPtr;
+	const btContactSolverInfo* m_infoGlobal;
+
+	CollectContactManifoldCachedInfoLoop(btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, const btContactSolverInfo& infoGlobal)
+	{
+		m_solver = solver;
+		m_cachedInfoArray = cachedInfoArray;
+		m_manifoldPtr = manifoldPtr;
+		m_infoGlobal = &infoGlobal;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalCollectContactManifoldCachedInfo(m_cachedInfoArray + iBegin, m_manifoldPtr + iBegin, iEnd - iBegin, *m_infoGlobal);
+	}
+};
+
+void btSequentialImpulseConstraintSolverMt::internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds)
+{
+	BT_PROFILE("internalAllocContactConstraints");
+	// possibly parallel part
+	for (int iManifold = 0; iManifold < numManifolds; ++iManifold)
+	{
+		const btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[iManifold];
+		int contactIndex = cachedInfo.contactIndex;
+		int frictionIndex = contactIndex * m_numFrictionDirections;
+		int rollingFrictionIndex = cachedInfo.rollingFrictionIndex;
+		for (int i = 0; i < cachedInfo.numTouchingContacts; i++)
+		{
+			btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[contactIndex];
+			contactConstraint.m_solverBodyIdA = cachedInfo.solverBodyIds[0];
+			contactConstraint.m_solverBodyIdB = cachedInfo.solverBodyIds[1];
+			contactConstraint.m_originalContactPoint = cachedInfo.contactPoints[i];
+
+			// allocate the friction constraints
+			contactConstraint.m_frictionIndex = frictionIndex;
+			for (int iDir = 0; iDir < m_numFrictionDirections; ++iDir)
+			{
+				btSolverConstraint& frictionConstraint = m_tmpSolverContactFrictionConstraintPool[frictionIndex];
+				frictionConstraint.m_frictionIndex = contactIndex;
+				frictionIndex++;
+			}
+
+			// allocate rolling friction constraints
+			if (cachedInfo.contactHasRollingFriction[i])
+			{
+				m_rollingFrictionIndexTable[contactIndex] = rollingFrictionIndex;
+				// allocate 3 (although we may use only 2 sometimes)
+				for (int i = 0; i < 3; i++)
+				{
+					m_tmpSolverContactRollingFrictionConstraintPool[rollingFrictionIndex].m_frictionIndex = contactIndex;
+					rollingFrictionIndex++;
+				}
+			}
+			else
+			{
+				// indicate there is no rolling friction for this contact point
+				m_rollingFrictionIndexTable[contactIndex] = -1;
+			}
+			contactIndex++;
+		}
+	}
+}
+
+struct AllocContactConstraintsLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray;
+
+	AllocContactConstraintsLoop(btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray)
+	{
+		m_solver = solver;
+		m_cachedInfoArray = cachedInfoArray;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalAllocContactConstraints(m_cachedInfoArray + iBegin, iEnd - iBegin);
+	}
+};
+
+void btSequentialImpulseConstraintSolverMt::allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("allocAllContactConstraints");
+	btAlignedObjectArray<btContactManifoldCachedInfo> cachedInfoArray;  // = m_manifoldCachedInfoArray;
+	cachedInfoArray.resizeNoInitialize(numManifolds);
+	if (/* DISABLES CODE */ (false))
+	{
+		// sequential
+		internalCollectContactManifoldCachedInfo(&cachedInfoArray[0], manifoldPtr, numManifolds, infoGlobal);
+	}
+	else
+	{
+		// may alter ordering of bodies which affects determinism
+		CollectContactManifoldCachedInfoLoop loop(this, &cachedInfoArray[0], manifoldPtr, infoGlobal);
+		int grainSize = 200;
+		btParallelFor(0, numManifolds, grainSize, loop);
+	}
+
+	{
+		// serial part
+		int numContacts = 0;
+		int numRollingFrictionConstraints = 0;
+		for (int iManifold = 0; iManifold < numManifolds; ++iManifold)
+		{
+			btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[iManifold];
+			cachedInfo.contactIndex = numContacts;
+			cachedInfo.rollingFrictionIndex = numRollingFrictionConstraints;
+			numContacts += cachedInfo.numTouchingContacts;
+			for (int i = 0; i < cachedInfo.numTouchingContacts; ++i)
+			{
+				if (cachedInfo.contactHasRollingFriction[i])
+				{
+					numRollingFrictionConstraints += 3;
+				}
+			}
+		}
+		{
+			BT_PROFILE("allocPools");
+			if (m_tmpSolverContactConstraintPool.capacity() < numContacts)
+			{
+				// if we need to reallocate, reserve some extra so we don't have to reallocate again next frame
+				int extraReserve = numContacts / 16;
+				m_tmpSolverContactConstraintPool.reserve(numContacts + extraReserve);
+				m_rollingFrictionIndexTable.reserve(numContacts + extraReserve);
+				m_tmpSolverContactFrictionConstraintPool.reserve((numContacts + extraReserve) * m_numFrictionDirections);
+				m_tmpSolverContactRollingFrictionConstraintPool.reserve(numRollingFrictionConstraints + extraReserve);
+			}
+			m_tmpSolverContactConstraintPool.resizeNoInitialize(numContacts);
+			m_rollingFrictionIndexTable.resizeNoInitialize(numContacts);
+			m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(numContacts * m_numFrictionDirections);
+			m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(numRollingFrictionConstraints);
+		}
+	}
+	{
+		AllocContactConstraintsLoop loop(this, &cachedInfoArray[0]);
+		int grainSize = 200;
+		btParallelFor(0, numManifolds, grainSize, loop);
+	}
+}
+
+void btSequentialImpulseConstraintSolverMt::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
+{
+	if (!m_useBatching)
+	{
+		btSequentialImpulseConstraintSolver::convertContacts(manifoldPtr, numManifolds, infoGlobal);
+		return;
+	}
+	BT_PROFILE("convertContacts");
+	if (numManifolds > 0)
+	{
+		if (m_fixedBodyId < 0)
+		{
+			m_fixedBodyId = m_tmpSolverBodyPool.size();
+			btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(&fixedBody, 0, infoGlobal.m_timeStep);
+		}
+		allocAllContactConstraints(manifoldPtr, numManifolds, infoGlobal);
+		if (m_useBatching)
+		{
+			setupBatchedContactConstraints();
+		}
+		setupAllContactConstraints(infoGlobal);
+	}
+}
+
+void btSequentialImpulseConstraintSolverMt::internalInitMultipleJoints(btTypedConstraint** constraints, int iBegin, int iEnd)
+{
+	BT_PROFILE("internalInitMultipleJoints");
+	for (int i = iBegin; i < iEnd; i++)
+	{
+		btTypedConstraint* constraint = constraints[i];
+		btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+		if (constraint->isEnabled())
+		{
+			constraint->buildJacobian();
+			constraint->internalSetAppliedImpulse(0.0f);
+			btJointFeedback* fb = constraint->getJointFeedback();
+			if (fb)
+			{
+				fb->m_appliedForceBodyA.setZero();
+				fb->m_appliedTorqueBodyA.setZero();
+				fb->m_appliedForceBodyB.setZero();
+				fb->m_appliedTorqueBodyB.setZero();
+			}
+			constraint->getInfo1(&info1);
+		}
+		else
+		{
+			info1.m_numConstraintRows = 0;
+			info1.nub = 0;
+		}
+	}
+}
+
+struct InitJointsLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	btTypedConstraint** m_constraints;
+
+	InitJointsLoop(btSequentialImpulseConstraintSolverMt* solver, btTypedConstraint** constraints)
+	{
+		m_solver = solver;
+		m_constraints = constraints;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalInitMultipleJoints(m_constraints, iBegin, iEnd);
+	}
+};
+
+void btSequentialImpulseConstraintSolverMt::internalConvertMultipleJoints(const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("internalConvertMultipleJoints");
+	for (int i = iBegin; i < iEnd; ++i)
+	{
+		const JointParams& jointParams = jointParamsArray[i];
+		int currentRow = jointParams.m_solverConstraint;
+		if (currentRow != -1)
+		{
+			const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+			btAssert(currentRow < m_tmpSolverNonContactConstraintPool.size());
+			btAssert(info1.m_numConstraintRows > 0);
+
+			btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
+			btTypedConstraint* constraint = constraints[i];
+
+			convertJoint(currentConstraintRow, constraint, info1, jointParams.m_solverBodyA, jointParams.m_solverBodyB, infoGlobal);
+		}
+	}
+}
+
+struct ConvertJointsLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& m_jointParamsArray;
+	btTypedConstraint** m_srcConstraints;
+	const btContactSolverInfo& m_infoGlobal;
+
+	ConvertJointsLoop(btSequentialImpulseConstraintSolverMt* solver,
+					  const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& jointParamsArray,
+					  btTypedConstraint** srcConstraints,
+					  const btContactSolverInfo& infoGlobal) : m_jointParamsArray(jointParamsArray),
+															   m_infoGlobal(infoGlobal)
+	{
+		m_solver = solver;
+		m_srcConstraints = srcConstraints;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalConvertMultipleJoints(m_jointParamsArray, m_srcConstraints, iBegin, iEnd, m_infoGlobal);
+	}
+};
+
+void btSequentialImpulseConstraintSolverMt::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
+{
+	if (!m_useBatching)
+	{
+		btSequentialImpulseConstraintSolver::convertJoints(constraints, numConstraints, infoGlobal);
+		return;
+	}
+	BT_PROFILE("convertJoints");
+	bool parallelJointSetup = true;
+	m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
+	if (parallelJointSetup)
+	{
+		InitJointsLoop loop(this, constraints);
+		int grainSize = 40;
+		btParallelFor(0, numConstraints, grainSize, loop);
+	}
+	else
+	{
+		internalInitMultipleJoints(constraints, 0, numConstraints);
+	}
+
+	int totalNumRows = 0;
+	btAlignedObjectArray<JointParams> jointParamsArray;
+	jointParamsArray.resizeNoInitialize(numConstraints);
+
+	//calculate the total number of contraint rows
+	for (int i = 0; i < numConstraints; i++)
+	{
+		btTypedConstraint* constraint = constraints[i];
+
+		JointParams& params = jointParamsArray[i];
+		const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+
+		if (info1.m_numConstraintRows)
+		{
+			params.m_solverConstraint = totalNumRows;
+			params.m_solverBodyA = getOrInitSolverBody(constraint->getRigidBodyA(), infoGlobal.m_timeStep);
+			params.m_solverBodyB = getOrInitSolverBody(constraint->getRigidBodyB(), infoGlobal.m_timeStep);
+		}
+		else
+		{
+			params.m_solverConstraint = -1;
+		}
+		totalNumRows += info1.m_numConstraintRows;
+	}
+	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
+
+	///setup the btSolverConstraints
+	if (parallelJointSetup)
+	{
+		ConvertJointsLoop loop(this, jointParamsArray, constraints, infoGlobal);
+		int grainSize = 20;
+		btParallelFor(0, numConstraints, grainSize, loop);
+	}
+	else
+	{
+		internalConvertMultipleJoints(jointParamsArray, constraints, 0, numConstraints, infoGlobal);
+	}
+	setupBatchedJointConstraints();
+}
+
+void btSequentialImpulseConstraintSolverMt::internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("internalConvertBodies");
+	for (int i = iBegin; i < iEnd; i++)
+	{
+		btCollisionObject* obj = bodies[i];
+		obj->setCompanionId(i);
+		btSolverBody& solverBody = m_tmpSolverBodyPool[i];
+		initSolverBody(&solverBody, obj, infoGlobal.m_timeStep);
+
+		btRigidBody* body = btRigidBody::upcast(obj);
+		if (body && body->getInvMass())
+		{
+			btVector3 gyroForce(0, 0, 0);
+			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
+			{
+				gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
+				solverBody.m_externalTorqueImpulse -= gyroForce * body->getInvInertiaTensorWorld() * infoGlobal.m_timeStep;
+			}
+			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
+			{
+				gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
+				solverBody.m_externalTorqueImpulse += gyroForce;
+			}
+			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
+			{
+				gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
+				solverBody.m_externalTorqueImpulse += gyroForce;
+			}
+		}
+	}
+}
+
+struct ConvertBodiesLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	btCollisionObject** m_bodies;
+	int m_numBodies;
+	const btContactSolverInfo& m_infoGlobal;
+
+	ConvertBodiesLoop(btSequentialImpulseConstraintSolverMt* solver,
+					  btCollisionObject** bodies,
+					  int numBodies,
+					  const btContactSolverInfo& infoGlobal) : m_infoGlobal(infoGlobal)
+	{
+		m_solver = solver;
+		m_bodies = bodies;
+		m_numBodies = numBodies;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalConvertBodies(m_bodies, iBegin, iEnd, m_infoGlobal);
+	}
+};
+
+void btSequentialImpulseConstraintSolverMt::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("convertBodies");
+	m_kinematicBodyUniqueIdToSolverBodyTable.resize(0);
+
+	m_tmpSolverBodyPool.resizeNoInitialize(numBodies + 1);
+
+	m_fixedBodyId = numBodies;
+	{
+		btSolverBody& fixedBody = m_tmpSolverBodyPool[m_fixedBodyId];
+		initSolverBody(&fixedBody, NULL, infoGlobal.m_timeStep);
+	}
+
+	bool parallelBodySetup = true;
+	if (parallelBodySetup)
+	{
+		ConvertBodiesLoop loop(this, bodies, numBodies, infoGlobal);
+		int grainSize = 40;
+		btParallelFor(0, numBodies, grainSize, loop);
+	}
+	else
+	{
+		internalConvertBodies(bodies, 0, numBodies, infoGlobal);
+	}
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySetup(
+	btCollisionObject** bodies,
+	int numBodies,
+	btPersistentManifold** manifoldPtr,
+	int numManifolds,
+	btTypedConstraint** constraints,
+	int numConstraints,
+	const btContactSolverInfo& infoGlobal,
+	btIDebugDraw* debugDrawer)
+{
+	m_numFrictionDirections = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
+	m_useBatching = false;
+	if (numManifolds >= s_minimumContactManifoldsForBatching &&
+		(s_allowNestedParallelForLoops || !btThreadsAreRunning()))
+	{
+		m_useBatching = true;
+		m_batchedContactConstraints.m_debugDrawer = debugDrawer;
+		m_batchedJointConstraints.m_debugDrawer = debugDrawer;
+	}
+	btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies,
+																	  numBodies,
+																	  manifoldPtr,
+																	  numManifolds,
+																	  constraints,
+																	  numConstraints,
+																	  infoGlobal,
+																	  debugDrawer);
+	return 0.0f;
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactSplitPenetrationImpulseConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
+{
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
+	{
+		int iCons = consIndices[iiCons];
+		const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iCons];
+		btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
+		btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
+		btScalar residual = resolveSplitPenetrationImpulse(bodyA, bodyB, solveManifold);
+		leastSquaresResidual += residual * residual;
+	}
+	return leastSquaresResidual;
+}
+
+struct ContactSplitPenetrationImpulseSolverLoop : public btIParallelSumBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+
+	ContactSplitPenetrationImpulseSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
+	{
+		m_solver = solver;
+		m_bc = bc;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("ContactSplitPenetrationImpulseSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleContactSplitPenetrationImpulseConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
+		}
+		return sum;
+	}
+};
+
+void btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
+	BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
+	if (infoGlobal.m_splitImpulse)
+	{
+		for (int iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
+		{
+			btScalar leastSquaresResidual = 0.f;
+			if (m_useBatching)
+			{
+				const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+				ContactSplitPenetrationImpulseSolverLoop loop(this, &batchedCons);
+				btScalar leastSquaresResidual = 0.f;
+				for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+				{
+					int iPhase = batchedCons.m_phaseOrder[iiPhase];
+					const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+					int grainSize = batchedCons.m_phaseGrainSize[iPhase];
+					leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+				}
+			}
+			else
+			{
+				// non-batched
+				leastSquaresResidual = resolveMultipleContactSplitPenetrationImpulseConstraints(m_orderTmpConstraintPool, 0, m_tmpSolverContactConstraintPool.size());
+			}
+			if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
+			{
+#ifdef VERBOSE_RESIDUAL_PRINTF
+				printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
+#endif
+				break;
+			}
+		}
+	}
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
+	if (!m_useBatching)
+	{
+		return btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+	}
+	BT_PROFILE("solveSingleIterationMt");
+	btScalar leastSquaresResidual = 0.f;
+
+	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+	{
+		if (1)  // uncomment this for a bit less random ((iteration & 7) == 0)
+		{
+			randomizeConstraintOrdering(iteration, infoGlobal.m_numIterations);
+		}
+	}
+
+	{
+		///solve all joint constraints
+		leastSquaresResidual += resolveAllJointConstraints(iteration);
+
+		if (iteration < infoGlobal.m_numIterations)
+		{
+			// this loop is only used for cone-twist constraints,
+			// it would be nice to skip this loop if none of the constraints need it
+			if (m_useObsoleteJointConstraints)
+			{
+				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);
+					}
+				}
+			}
+
+			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
+			{
+				// solve all contact, contact-friction, and rolling friction constraints interleaved
+				leastSquaresResidual += resolveAllContactConstraintsInterleaved();
+			}
+			else  //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			{
+				// don't interleave them
+				// solve all contact constraints
+				leastSquaresResidual += resolveAllContactConstraints();
+
+				// solve all contact friction constraints
+				leastSquaresResidual += resolveAllContactFrictionConstraints();
+
+				// solve all rolling friction constraints
+				leastSquaresResidual += resolveAllRollingFrictionConstraints();
+			}
+		}
+	}
+	return leastSquaresResidual;
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleJointConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration)
+{
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
+	{
+		int iCons = consIndices[iiCons];
+		const btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[iCons];
+		if (iteration < constraint.m_overrideNumSolverIterations)
+		{
+			btSolverBody& bodyA = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+			btSolverBody& bodyB = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+			btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, constraint);
+			leastSquaresResidual += residual * residual;
+		}
+	}
+	return leastSquaresResidual;
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
+{
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
+	{
+		int iCons = consIndices[iiCons];
+		const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iCons];
+		btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
+		btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
+		btScalar residual = resolveSingleConstraintRowLowerLimit(bodyA, bodyB, solveManifold);
+		leastSquaresResidual += residual * residual;
+	}
+	return leastSquaresResidual;
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
+{
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
+	{
+		int iContact = consIndices[iiCons];
+		btScalar totalImpulse = m_tmpSolverContactConstraintPool[iContact].m_appliedImpulse;
+
+		// apply sliding friction
+		if (totalImpulse > 0.0f)
+		{
+			int iBegin = iContact * m_numFrictionDirections;
+			int iEnd = iBegin + m_numFrictionDirections;
+			for (int iFriction = iBegin; iFriction < iEnd; ++iFriction)
+			{
+				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[iFriction++];
+				btAssert(solveManifold.m_frictionIndex == iContact);
+
+				solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+				solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
+
+				btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
+				btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
+				btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, solveManifold);
+				leastSquaresResidual += residual * residual;
+			}
+		}
+	}
+	return leastSquaresResidual;
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactRollingFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
+{
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
+	{
+		int iContact = consIndices[iiCons];
+		int iFirstRollingFriction = m_rollingFrictionIndexTable[iContact];
+		if (iFirstRollingFriction >= 0)
+		{
+			btScalar totalImpulse = m_tmpSolverContactConstraintPool[iContact].m_appliedImpulse;
+			// apply rolling friction
+			if (totalImpulse > 0.0f)
+			{
+				int iBegin = iFirstRollingFriction;
+				int iEnd = iBegin + 3;
+				for (int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric)
+				{
+					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric];
+					if (rollingFrictionConstraint.m_frictionIndex != iContact)
+					{
+						break;
+					}
+					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+					if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
+					{
+						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+					}
+
+					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+					btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
+					leastSquaresResidual += residual * residual;
+				}
+			}
+		}
+	}
+	return leastSquaresResidual;
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraintsInterleaved(const btAlignedObjectArray<int>& contactIndices,
+																							 int batchBegin,
+																							 int batchEnd)
+{
+	btScalar leastSquaresResidual = 0.f;
+	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+
+	for (int iiCons = batchBegin; iiCons < batchEnd; iiCons++)
+	{
+		btScalar totalImpulse = 0;
+		int iContact = contactIndices[iiCons];
+		// apply penetration constraint
+		{
+			const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iContact];
+			btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+			leastSquaresResidual += residual * residual;
+			totalImpulse = solveManifold.m_appliedImpulse;
+		}
+
+		// apply sliding friction
+		if (totalImpulse > 0.0f)
+		{
+			int iBegin = iContact * m_numFrictionDirections;
+			int iEnd = iBegin + m_numFrictionDirections;
+			for (int iFriction = iBegin; iFriction < iEnd; ++iFriction)
+			{
+				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[iFriction];
+				btAssert(solveManifold.m_frictionIndex == iContact);
+
+				solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+				solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
+
+				btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
+				btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
+				btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, solveManifold);
+				leastSquaresResidual += residual * residual;
+			}
+		}
+
+		// apply rolling friction
+		int iFirstRollingFriction = m_rollingFrictionIndexTable[iContact];
+		if (totalImpulse > 0.0f && iFirstRollingFriction >= 0)
+		{
+			int iBegin = iFirstRollingFriction;
+			int iEnd = iBegin + 3;
+			for (int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric)
+			{
+				btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric];
+				if (rollingFrictionConstraint.m_frictionIndex != iContact)
+				{
+					break;
+				}
+				btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+				if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
+				{
+					rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+				}
+
+				rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+				rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+				btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
+				leastSquaresResidual += residual * residual;
+			}
+		}
+	}
+	return leastSquaresResidual;
+}
+
+void btSequentialImpulseConstraintSolverMt::randomizeBatchedConstraintOrdering(btBatchedConstraints* batchedConstraints)
+{
+	btBatchedConstraints& bc = *batchedConstraints;
+	// randomize ordering of phases
+	for (int ii = 1; ii < bc.m_phaseOrder.size(); ++ii)
+	{
+		int iSwap = btRandInt2(ii + 1);
+		bc.m_phaseOrder.swap(ii, iSwap);
+	}
+
+	// for each batch,
+	for (int iBatch = 0; iBatch < bc.m_batches.size(); ++iBatch)
+	{
+		// randomize ordering of constraints within the batch
+		const btBatchedConstraints::Range& batch = bc.m_batches[iBatch];
+		for (int iiCons = batch.begin; iiCons < batch.end; ++iiCons)
+		{
+			int iSwap = batch.begin + btRandInt2(iiCons - batch.begin + 1);
+			btAssert(iSwap >= batch.begin && iSwap < batch.end);
+			bc.m_constraintIndices.swap(iiCons, iSwap);
+		}
+	}
+}
+
+void btSequentialImpulseConstraintSolverMt::randomizeConstraintOrdering(int iteration, int numIterations)
+{
+	// randomize ordering of joint constraints
+	randomizeBatchedConstraintOrdering(&m_batchedJointConstraints);
+
+	//contact/friction constraints are not solved more than numIterations
+	if (iteration < numIterations)
+	{
+		randomizeBatchedConstraintOrdering(&m_batchedContactConstraints);
+	}
+}
+
+struct JointSolverLoop : public btIParallelSumBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+	int m_iteration;
+
+	JointSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, int iteration)
+	{
+		m_solver = solver;
+		m_bc = bc;
+		m_iteration = iteration;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("JointSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleJointConstraints(m_bc->m_constraintIndices, batch.begin, batch.end, m_iteration);
+		}
+		return sum;
+	}
+};
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveAllJointConstraints(int iteration)
+{
+	BT_PROFILE("resolveAllJointConstraints");
+	const btBatchedConstraints& batchedCons = m_batchedJointConstraints;
+	JointSolverLoop loop(this, &batchedCons, iteration);
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+	{
+		int iPhase = batchedCons.m_phaseOrder[iiPhase];
+		const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+		int grainSize = 1;
+		leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+	}
+	return leastSquaresResidual;
+}
+
+struct ContactSolverLoop : public btIParallelSumBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+
+	ContactSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
+	{
+		m_solver = solver;
+		m_bc = bc;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("ContactSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleContactConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
+		}
+		return sum;
+	}
+};
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraints()
+{
+	BT_PROFILE("resolveAllContactConstraints");
+	const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+	ContactSolverLoop loop(this, &batchedCons);
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+	{
+		int iPhase = batchedCons.m_phaseOrder[iiPhase];
+		const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+		int grainSize = batchedCons.m_phaseGrainSize[iPhase];
+		leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+	}
+	return leastSquaresResidual;
+}
+
+struct ContactFrictionSolverLoop : public btIParallelSumBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+
+	ContactFrictionSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
+	{
+		m_solver = solver;
+		m_bc = bc;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("ContactFrictionSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleContactFrictionConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
+		}
+		return sum;
+	}
+};
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactFrictionConstraints()
+{
+	BT_PROFILE("resolveAllContactFrictionConstraints");
+	const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+	ContactFrictionSolverLoop loop(this, &batchedCons);
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+	{
+		int iPhase = batchedCons.m_phaseOrder[iiPhase];
+		const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+		int grainSize = batchedCons.m_phaseGrainSize[iPhase];
+		leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+	}
+	return leastSquaresResidual;
+}
+
+struct InterleavedContactSolverLoop : public btIParallelSumBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+
+	InterleavedContactSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
+	{
+		m_solver = solver;
+		m_bc = bc;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("InterleavedContactSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleContactConstraintsInterleaved(m_bc->m_constraintIndices, batch.begin, batch.end);
+		}
+		return sum;
+	}
+};
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraintsInterleaved()
+{
+	BT_PROFILE("resolveAllContactConstraintsInterleaved");
+	const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+	InterleavedContactSolverLoop loop(this, &batchedCons);
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+	{
+		int iPhase = batchedCons.m_phaseOrder[iiPhase];
+		const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+		int grainSize = 1;
+		leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+	}
+	return leastSquaresResidual;
+}
+
+struct ContactRollingFrictionSolverLoop : public btIParallelSumBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+
+	ContactRollingFrictionSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
+	{
+		m_solver = solver;
+		m_bc = bc;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("ContactFrictionSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleContactRollingFrictionConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
+		}
+		return sum;
+	}
+};
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveAllRollingFrictionConstraints()
+{
+	BT_PROFILE("resolveAllRollingFrictionConstraints");
+	btScalar leastSquaresResidual = 0.f;
+	//
+	// We do not generate batches for rolling friction constraints. We assume that
+	// one of two cases is true:
+	//
+	//  1. either most bodies in the simulation have rolling friction, in which case we can use the
+	//     batches for contacts and use a lookup table to translate contact indices to rolling friction
+	//     (ignoring any contact indices that don't map to a rolling friction constraint). As long as
+	//     most contacts have a corresponding rolling friction constraint, this should parallelize well.
+	//
+	//  -OR-
+	//
+	//  2. few bodies in the simulation have rolling friction, so it is not worth trying to use the
+	//     batches from contacts as most of the contacts won't have corresponding rolling friction
+	//     constraints and most threads would end up doing very little work. Most of the time would
+	//     go to threading overhead, so we don't bother with threading.
+	//
+	int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+	if (numRollingFrictionPoolConstraints >= m_tmpSolverContactConstraintPool.size())
+	{
+		// use batching if there are many rolling friction constraints
+		const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+		ContactRollingFrictionSolverLoop loop(this, &batchedCons);
+		btScalar leastSquaresResidual = 0.f;
+		for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+		{
+			int iPhase = batchedCons.m_phaseOrder[iiPhase];
+			const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+			int grainSize = 1;
+			leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+		}
+	}
+	else
+	{
+		// no batching, also ignores SOLVER_RANDMIZE_ORDER
+		for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
+		{
+			btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+			if (rollingFrictionConstraint.m_frictionIndex >= 0)
+			{
+				btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+				if (totalImpulse > 0.0f)
+				{
+					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+					if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
+						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+
+					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+					btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
+					leastSquaresResidual += residual * residual;
+				}
+			}
+		}
+	}
+	return leastSquaresResidual;
+}
+
+void btSequentialImpulseConstraintSolverMt::internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("internalWriteBackContacts");
+	writeBackContacts(iBegin, iEnd, infoGlobal);
+	//for ( int iContact = iBegin; iContact < iEnd; ++iContact)
+	//{
+	//    const btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[ iContact ];
+	//    btManifoldPoint* pt = (btManifoldPoint*) contactConstraint.m_originalContactPoint;
+	//    btAssert( pt );
+	//    pt->m_appliedImpulse = contactConstraint.m_appliedImpulse;
+	//    pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex ].m_appliedImpulse;
+	//    if ( m_numFrictionDirections == 2 )
+	//    {
+	//        pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex + 1 ].m_appliedImpulse;
+	//    }
+	//}
+}
+
+void btSequentialImpulseConstraintSolverMt::internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("internalWriteBackJoints");
+	writeBackJoints(iBegin, iEnd, infoGlobal);
+}
+
+void btSequentialImpulseConstraintSolverMt::internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("internalWriteBackBodies");
+	writeBackBodies(iBegin, iEnd, infoGlobal);
+}
+
+struct WriteContactPointsLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btContactSolverInfo* m_infoGlobal;
+
+	WriteContactPointsLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal)
+	{
+		m_solver = solver;
+		m_infoGlobal = &infoGlobal;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalWriteBackContacts(iBegin, iEnd, *m_infoGlobal);
+	}
+};
+
+struct WriteJointsLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btContactSolverInfo* m_infoGlobal;
+
+	WriteJointsLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal)
+	{
+		m_solver = solver;
+		m_infoGlobal = &infoGlobal;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalWriteBackJoints(iBegin, iEnd, *m_infoGlobal);
+	}
+};
+
+struct WriteBodiesLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btContactSolverInfo* m_infoGlobal;
+
+	WriteBodiesLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal)
+	{
+		m_solver = solver;
+		m_infoGlobal = &infoGlobal;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalWriteBackBodies(iBegin, iEnd, *m_infoGlobal);
+	}
+};
+
+btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("solveGroupCacheFriendlyFinish");
+
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
+		WriteContactPointsLoop loop(this, infoGlobal);
+		int grainSize = 500;
+		btParallelFor(0, m_tmpSolverContactConstraintPool.size(), grainSize, loop);
+	}
+
+	{
+		WriteJointsLoop loop(this, infoGlobal);
+		int grainSize = 400;
+		btParallelFor(0, m_tmpSolverNonContactConstraintPool.size(), grainSize, loop);
+	}
+	{
+		WriteBodiesLoop loop(this, infoGlobal);
+		int grainSize = 100;
+		btParallelFor(0, m_tmpSolverBodyPool.size(), grainSize, loop);
+	}
+
+	m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
+
+	m_tmpSolverBodyPool.resizeNoInitialize(0);
+	return 0.f;
+}
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h
new file mode 100644
index 00000000000..1861ddd7d76
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h
@@ -0,0 +1,150 @@
+/*
+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_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
+#define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
+
+#include "btSequentialImpulseConstraintSolver.h"
+#include "btBatchedConstraints.h"
+#include "LinearMath/btThreads.h"
+
+///
+/// btSequentialImpulseConstraintSolverMt
+///
+///  A multithreaded variant of the sequential impulse constraint solver. The constraints to be solved are grouped into
+///  batches and phases where each batch of constraints within a given phase can be solved in parallel with the rest.
+///  Ideally we want as few phases as possible, and each phase should have many batches, and all of the batches should
+///  have about the same number of constraints.
+///  This method works best on a large island of many constraints.
+///
+///  Supports all of the features of the normal sequential impulse solver such as:
+///    - split penetration impulse
+///    - rolling friction
+///    - interleaving constraints
+///    - warmstarting
+///    - 2 friction directions
+///    - randomized constraint ordering
+///    - early termination when leastSquaresResidualThreshold is satisfied
+///
+///  When the SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS flag is enabled, unlike the normal SequentialImpulse solver,
+///  the rolling friction is interleaved as well.
+///  Interleaving the contact penetration constraints with friction reduces the number of parallel loops that need to be done,
+///  which reduces threading overhead so it can be a performance win, however, it does seem to produce a less stable simulation,
+///  at least on stacks of blocks.
+///
+///  When the SOLVER_RANDMIZE_ORDER flag is enabled, the ordering of phases, and the ordering of constraints within each batch
+///  is randomized, however it does not swap constraints between batches.
+///  This is to avoid regenerating the batches for each solver iteration which would be quite costly in performance.
+///
+///  Note that a non-zero leastSquaresResidualThreshold could possibly affect the determinism of the simulation
+///  if the task scheduler's parallelSum operation is non-deterministic. The parallelSum operation can be non-deterministic
+///  because floating point addition is not associative due to rounding errors.
+///  The task scheduler can and should ensure that the result of any parallelSum operation is deterministic.
+///
+ATTRIBUTE_ALIGNED16(class)
+btSequentialImpulseConstraintSolverMt : public btSequentialImpulseConstraintSolver
+{
+public:
+	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE;
+	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE;
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE;
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
+
+	// temp struct used to collect info from persistent manifolds into a cache-friendly struct using multiple threads
+	struct btContactManifoldCachedInfo
+	{
+		static const int MAX_NUM_CONTACT_POINTS = 4;
+
+		int numTouchingContacts;
+		int solverBodyIds[2];
+		int contactIndex;
+		int rollingFrictionIndex;
+		bool contactHasRollingFriction[MAX_NUM_CONTACT_POINTS];
+		btManifoldPoint* contactPoints[MAX_NUM_CONTACT_POINTS];
+	};
+	// temp struct used for setting up joint constraints in parallel
+	struct JointParams
+	{
+		int m_solverConstraint;
+		int m_solverBodyA;
+		int m_solverBodyB;
+	};
+	void internalInitMultipleJoints(btTypedConstraint * *constraints, int iBegin, int iEnd);
+	void internalConvertMultipleJoints(const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+
+	// parameters to control batching
+	static bool s_allowNestedParallelForLoops;        // whether to allow nested parallel operations
+	static int s_minimumContactManifoldsForBatching;  // don't even try to batch if fewer manifolds than this
+	static btBatchedConstraints::BatchingMethod s_contactBatchingMethod;
+	static btBatchedConstraints::BatchingMethod s_jointBatchingMethod;
+	static int s_minBatchSize;  // desired number of constraints per batch
+	static int s_maxBatchSize;
+
+protected:
+	static const int CACHE_LINE_SIZE = 64;
+
+	btBatchedConstraints m_batchedContactConstraints;
+	btBatchedConstraints m_batchedJointConstraints;
+	int m_numFrictionDirections;
+	bool m_useBatching;
+	bool m_useObsoleteJointConstraints;
+	btAlignedObjectArray<btContactManifoldCachedInfo> m_manifoldCachedInfoArray;
+	btAlignedObjectArray<int> m_rollingFrictionIndexTable;  // lookup table mapping contact index to rolling friction index
+	btSpinMutex m_bodySolverArrayMutex;
+	char m_antiFalseSharingPadding[CACHE_LINE_SIZE];  // padding to keep mutexes in separate cachelines
+	btSpinMutex m_kinematicBodyUniqueIdToSolverBodyTableMutex;
+	btAlignedObjectArray<char> m_scratchMemory;
+
+	virtual void randomizeConstraintOrdering(int iteration, int numIterations);
+	virtual btScalar resolveAllJointConstraints(int iteration);
+	virtual btScalar resolveAllContactConstraints();
+	virtual btScalar resolveAllContactFrictionConstraints();
+	virtual btScalar resolveAllContactConstraintsInterleaved();
+	virtual btScalar resolveAllRollingFrictionConstraints();
+
+	virtual void setupBatchedContactConstraints();
+	virtual void setupBatchedJointConstraints();
+	virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
+	virtual void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
+	virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
+
+	int getOrInitSolverBodyThreadsafe(btCollisionObject & body, btScalar timeStep);
+	void allocAllContactConstraints(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
+	void setupAllContactConstraints(const btContactSolverInfo& infoGlobal);
+	void randomizeBatchedConstraintOrdering(btBatchedConstraints * batchedConstraints);
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btSequentialImpulseConstraintSolverMt();
+	virtual ~btSequentialImpulseConstraintSolverMt();
+
+	btScalar resolveMultipleJointConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration);
+	btScalar resolveMultipleContactConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
+	btScalar resolveMultipleContactSplitPenetrationImpulseConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
+	btScalar resolveMultipleContactFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
+	btScalar resolveMultipleContactRollingFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
+	btScalar resolveMultipleContactConstraintsInterleaved(const btAlignedObjectArray<int>& contactIndices, int batchBegin, int batchEnd);
+
+	void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo * cachedInfoArray, btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
+	void internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds);
+	void internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal);
+	void internalConvertBodies(btCollisionObject * *bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	void internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+};
+
+#endif  //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
index f8f81bfe6fa..cac5302a73e 100644..100755
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
@@ -18,8 +18,6 @@ Added by Roman Ponomarev (rponom@gmail.com)
 April 04, 2008
 */
 
-
-
 #include "btSliderConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
@@ -29,10 +27,10 @@ April 04, 2008
 
 void btSliderConstraint::initParams()
 {
-    m_lowerLinLimit = btScalar(1.0);
-    m_upperLinLimit = btScalar(-1.0);
-    m_lowerAngLimit = btScalar(0.);
-    m_upperAngLimit = btScalar(0.);
+	m_lowerLinLimit = btScalar(1.0);
+	m_upperLinLimit = btScalar(-1.0);
+	m_lowerAngLimit = btScalar(0.);
+	m_upperAngLimit = btScalar(0.);
 	m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
 	m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
 	m_dampingDirLin = btScalar(0.);
@@ -59,13 +57,13 @@ void btSliderConstraint::initParams()
 	m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM;
 
 	m_poweredLinMotor = false;
-    m_targetLinMotorVelocity = btScalar(0.);
-    m_maxLinMotorForce = btScalar(0.);
+	m_targetLinMotorVelocity = btScalar(0.);
+	m_maxLinMotorForce = btScalar(0.);
 	m_accumulatedLinMotorImpulse = btScalar(0.0);
 
 	m_poweredAngMotor = false;
-    m_targetAngMotorVelocity = btScalar(0.);
-    m_maxAngMotorForce = btScalar(0.);
+	m_targetAngMotorVelocity = btScalar(0.);
+	m_maxAngMotorForce = btScalar(0.);
 	m_accumulatedAngMotorImpulse = btScalar(0.0);
 
 	m_flags = 0;
@@ -73,43 +71,32 @@ void btSliderConstraint::initParams()
 
 	m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME;
 
-	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 }
 
-
-
-
-
 btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
-        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB),
-		m_useSolveConstraintObsolete(false),
-		m_frameInA(frameInA),
-        m_frameInB(frameInB),
-		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+	: btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB),
+	  m_useSolveConstraintObsolete(false),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB),
+	  m_useLinearReferenceFrameA(useLinearReferenceFrameA)
 {
 	initParams();
 }
 
-
-
 btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA)
-        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB),
-		m_useSolveConstraintObsolete(false),
-		m_frameInB(frameInB),
-		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+	: btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB),
+	  m_useSolveConstraintObsolete(false),
+	  m_frameInB(frameInB),
+	  m_useLinearReferenceFrameA(useLinearReferenceFrameA)
 {
 	///not providing rigidbody A means implicitly using worldspace for body A
 	m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
-//	m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
+	//	m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
 
 	initParams();
 }
 
-
-
-
-
-
 void btSliderConstraint::getInfo1(btConstraintInfo1* info)
 {
 	if (m_useSolveConstraintObsolete)
@@ -119,46 +106,39 @@ void btSliderConstraint::getInfo1(btConstraintInfo1* info)
 	}
 	else
 	{
-		info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
-		info->nub = 2; 
+		info->m_numConstraintRows = 4;  // Fixed 2 linear + 2 angular
+		info->nub = 2;
 		//prepare constraint
-		calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+		calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 		testAngLimits();
 		testLinLimits();
-		if(getSolveLinLimit() || getPoweredLinMotor())
+		if (getSolveLinLimit() || getPoweredLinMotor())
 		{
-			info->m_numConstraintRows++; // limit 3rd linear as well
-			info->nub--; 
+			info->m_numConstraintRows++;  // limit 3rd linear as well
+			info->nub--;
 		}
-		if(getSolveAngLimit() || getPoweredAngMotor())
+		if (getSolveAngLimit() || getPoweredAngMotor())
 		{
-			info->m_numConstraintRows++; // limit 3rd angular as well
-			info->nub--; 
+			info->m_numConstraintRows++;  // limit 3rd angular as well
+			info->nub--;
 		}
 	}
 }
 
 void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
 {
-
-	info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used)
-	info->nub = 0; 
+	info->m_numConstraintRows = 6;  // Fixed 2 linear + 2 angular + 1 limit (even if not used)
+	info->nub = 0;
 }
 
 void btSliderConstraint::getInfo2(btConstraintInfo2* info)
 {
-	getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
+	getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(), m_rbB.getLinearVelocity(), m_rbA.getInvMass(), m_rbB.getInvMass());
 }
 
-
-
-
-
-
-
-void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+void btSliderConstraint::calculateTransforms(const btTransform& transA, const btTransform& transB)
 {
-	if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
+	if (m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
 	{
 		m_calculatedTransformA = transA * m_frameInA;
 		m_calculatedTransformB = transB * m_frameInB;
@@ -170,8 +150,8 @@ void btSliderConstraint::calculateTransforms(const btTransform& transA,const btT
 	}
 	m_realPivotAInW = m_calculatedTransformA.getOrigin();
 	m_realPivotBInW = m_calculatedTransformB.getOrigin();
-	m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
-	if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
+	m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0);  // along X
+	if (m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
 	{
 		m_delta = m_realPivotBInW - m_realPivotAInW;
 	}
@@ -180,30 +160,28 @@ void btSliderConstraint::calculateTransforms(const btTransform& transA,const btT
 		m_delta = m_realPivotAInW - m_realPivotBInW;
 	}
 	m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
-    btVector3 normalWorld;
-    int i;
-    //linear part
-    for(i = 0; i < 3; i++)
-    {
+	btVector3 normalWorld;
+	int i;
+	//linear part
+	for (i = 0; i < 3; i++)
+	{
 		normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
 		m_depth[i] = m_delta.dot(normalWorld);
-    }
+	}
 }
- 
-
 
 void btSliderConstraint::testLinLimits(void)
 {
 	m_solveLinLim = false;
 	m_linPos = m_depth[0];
-	if(m_lowerLinLimit <= m_upperLinLimit)
+	if (m_lowerLinLimit <= m_upperLinLimit)
 	{
-		if(m_depth[0] > m_upperLinLimit)
+		if (m_depth[0] > m_upperLinLimit)
 		{
 			m_depth[0] -= m_upperLinLimit;
 			m_solveLinLim = true;
 		}
-		else if(m_depth[0] < m_lowerLinLimit)
+		else if (m_depth[0] < m_lowerLinLimit)
 		{
 			m_depth[0] -= m_lowerLinLimit;
 			m_solveLinLim = true;
@@ -219,27 +197,25 @@ void btSliderConstraint::testLinLimits(void)
 	}
 }
 
-
-
 void btSliderConstraint::testAngLimits(void)
 {
 	m_angDepth = btScalar(0.);
 	m_solveAngLim = false;
-	if(m_lowerAngLimit <= m_upperAngLimit)
+	if (m_lowerAngLimit <= m_upperAngLimit)
 	{
 		const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
 		const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
 		const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
-//		btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));  
-		btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0));  
+		//		btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
+		btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0));
 		rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
 		m_angPos = rot;
-		if(rot < m_lowerAngLimit)
+		if (rot < m_lowerAngLimit)
 		{
 			m_angDepth = rot - m_lowerAngLimit;
 			m_solveAngLim = true;
-		} 
-		else if(rot > m_upperAngLimit)
+		}
+		else if (rot > m_upperAngLimit)
 		{
 			m_angDepth = rot - m_upperAngLimit;
 			m_solveAngLim = true;
@@ -255,8 +231,6 @@ btVector3 btSliderConstraint::getAncorInA(void)
 	return ancorInA;
 }
 
-
-
 btVector3 btSliderConstraint::getAncorInB(void)
 {
 	btVector3 ancorInB;
@@ -264,17 +238,16 @@ btVector3 btSliderConstraint::getAncorInB(void)
 	return ancorInB;
 }
 
-
-void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass  )
+void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, btScalar rbAinvMass, btScalar rbBinvMass)
 {
 	const btTransform& trA = getCalculatedTransformA();
 	const btTransform& trB = getCalculatedTransformB();
-	
+
 	btAssert(!m_useSolveConstraintObsolete);
 	int i, s = info->rowskip;
-	
+
 	btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
-	
+
 	// difference between frames in WCS
 	btVector3 ofs = trB.getOrigin() - trA.getOrigin();
 	// now get weight factors depending on masses
@@ -283,11 +256,11 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 	bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
 	btScalar miS = miA + miB;
 	btScalar factA, factB;
-	if(miS > btScalar(0.f))
+	if (miS > btScalar(0.f))
 	{
 		factA = miB / miS;
 	}
-	else 
+	else
 	{
 		factA = btScalar(0.5f);
 	}
@@ -295,17 +268,17 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 	btVector3 ax1, p, q;
 	btVector3 ax1A = trA.getBasis().getColumn(0);
 	btVector3 ax1B = trB.getBasis().getColumn(0);
-	if(m_useOffsetForConstraintFrame)
+	if (m_useOffsetForConstraintFrame)
 	{
 		// get the desired direction of slider axis
 		// as weighted sum of X-orthos of frameA and frameB in WCS
 		ax1 = ax1A * factA + ax1B * factB;
 		ax1.normalize();
 		// construct two orthos to slider axis
-		btPlaneSpace1 (ax1, p, q);
+		btPlaneSpace1(ax1, p, q);
 	}
 	else
-	{ // old way - use frameA
+	{  // old way - use frameA
 		ax1 = trA.getBasis().getColumn(0);
 		// get 2 orthos to slider axis (Y, Z)
 		p = trA.getBasis().getColumn(1);
@@ -322,16 +295,16 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 	info->m_J1angularAxis[0] = p[0];
 	info->m_J1angularAxis[1] = p[1];
 	info->m_J1angularAxis[2] = p[2];
-	info->m_J1angularAxis[s+0] = q[0];
-	info->m_J1angularAxis[s+1] = q[1];
-	info->m_J1angularAxis[s+2] = q[2];
+	info->m_J1angularAxis[s + 0] = q[0];
+	info->m_J1angularAxis[s + 1] = q[1];
+	info->m_J1angularAxis[s + 2] = q[2];
 
 	info->m_J2angularAxis[0] = -p[0];
 	info->m_J2angularAxis[1] = -p[1];
 	info->m_J2angularAxis[2] = -p[2];
-	info->m_J2angularAxis[s+0] = -q[0];
-	info->m_J2angularAxis[s+1] = -q[1];
-	info->m_J2angularAxis[s+2] = -q[2];
+	info->m_J2angularAxis[s + 0] = -q[0];
+	info->m_J2angularAxis[s + 1] = -q[1];
+	info->m_J2angularAxis[s + 2] = -q[2];
 	// compute the right hand side of the constraint equation. set relative
 	// body velocities along p and q to bring the slider back into alignment.
 	// if ax1A,ax1B are the unit length slider axes as computed from bodyA and
@@ -347,26 +320,25 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 	//    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.
-//	btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
+	//	btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
 	btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
 	btScalar k = info->fps * currERP;
 
 	btVector3 u = ax1A.cross(ax1B);
 	info->m_constraintError[0] = k * u.dot(p);
 	info->m_constraintError[s] = k * u.dot(q);
-	if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
+	if (m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
 	{
 		info->cfm[0] = m_cfmOrthoAng;
 		info->cfm[s] = m_cfmOrthoAng;
 	}
 
-	int nrow = 1; // last filled row
+	int nrow = 1;  // last filled row
 	int srow;
 	btScalar limit_err;
 	int limit;
-	int powered;
 
-	// next two rows. 
+	// next two rows.
 	// we want: velA + wA x relA == velB + wB x relB ... but this would
 	// result in three equations, so we project along two orthos to the slider axis
 
@@ -376,8 +348,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 	int s2 = nrow * s;
 	nrow++;
 	int s3 = nrow * s;
-	btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0);
-	if(m_useOffsetForConstraintFrame)
+	btVector3 tmpA(0, 0, 0), tmpB(0, 0, 0), relA(0, 0, 0), relB(0, 0, 0), c(0, 0, 0);
+	if (m_useOffsetForConstraintFrame)
 	{
 		// get vector from bodyB to frameB in WCS
 		relB = trB.getOrigin() - bodyB_trans.getOrigin();
@@ -399,7 +371,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 		// now choose average ortho to slider axis
 		p = orthoB * factA + orthoA * factB;
 		btScalar len2 = p.length2();
-		if(len2 > SIMD_EPSILON)
+		if (len2 > SIMD_EPSILON)
 		{
 			p /= btSqrt(len2);
 		}
@@ -412,38 +384,38 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 		// fill two rows
 		tmpA = relA.cross(p);
 		tmpB = relB.cross(p);
-		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];
+		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];
 		tmpA = relA.cross(q);
 		tmpB = relB.cross(q);
-		if(hasStaticBody && getSolveAngLimit())
-		{ // to make constraint between static and dynamic objects more rigid
+		if (hasStaticBody && getSolveAngLimit())
+		{  // to make constraint between static and dynamic objects more rigid
 			// remove wA (or wB) from equation if angular limit is hit
 			tmpB *= factB;
 			tmpA *= factA;
 		}
-		for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i];
-		for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
-		for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
-		for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
-		for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i];
-		for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i];
+		for (i = 0; i < 3; i++) info->m_J1angularAxis[s3 + i] = tmpA[i];
+		for (i = 0; i < 3; i++) info->m_J2angularAxis[s3 + i] = -tmpB[i];
+		for (i = 0; i < 3; i++) info->m_J1linearAxis[s2 + i] = p[i];
+		for (i = 0; i < 3; i++) info->m_J1linearAxis[s3 + i] = q[i];
+		for (i = 0; i < 3; i++) info->m_J2linearAxis[s2 + i] = -p[i];
+		for (i = 0; i < 3; i++) info->m_J2linearAxis[s3 + i] = -q[i];
 	}
 	else
-	{	// old way - maybe incorrect if bodies are not on the slider axis
+	{  // old way - maybe incorrect if bodies are not on the slider axis
 		// see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
 		c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
 		btVector3 tmp = c.cross(p);
-		for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
-		for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
+		for (i = 0; i < 3; i++) info->m_J1angularAxis[s2 + i] = factA * tmp[i];
+		for (i = 0; i < 3; i++) info->m_J2angularAxis[s2 + i] = factB * tmp[i];
 		tmp = c.cross(q);
-		for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
-		for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
+		for (i = 0; i < 3; i++) info->m_J1angularAxis[s3 + i] = factA * tmp[i];
+		for (i = 0; i < 3; i++) info->m_J2angularAxis[s3 + i] = factB * tmp[i];
 
-		for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
-		for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
-		for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i];
-		for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i];
+		for (i = 0; i < 3; i++) info->m_J1linearAxis[s2 + i] = p[i];
+		for (i = 0; i < 3; i++) info->m_J1linearAxis[s3 + i] = q[i];
+		for (i = 0; i < 3; i++) info->m_J2linearAxis[s2 + i] = -p[i];
+		for (i = 0; i < 3; i++) info->m_J2linearAxis[s3 + i] = -q[i];
 	}
 	// compute two elements of right hand side
 
@@ -455,37 +427,32 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 	info->m_constraintError[s2] = rhs;
 	rhs = k * q.dot(ofs);
 	info->m_constraintError[s3] = rhs;
-	if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
+	if (m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
 	{
 		info->cfm[s2] = m_cfmOrthoLin;
 		info->cfm[s3] = m_cfmOrthoLin;
 	}
 
-
 	// check linear limits
 	limit_err = btScalar(0.0);
 	limit = 0;
-	if(getSolveLinLimit())
+	if (getSolveLinLimit())
 	{
-		limit_err = getLinDepth() *  signFact;
+		limit_err = getLinDepth() * signFact;
 		limit = (limit_err > btScalar(0.0)) ? 2 : 1;
 	}
-	powered = 0;
-	if(getPoweredLinMotor())
-	{
-		powered = 1;
-	}
+	bool powered = getPoweredLinMotor();
 	// if the slider has joint limits or motor, add in the extra row
-	if (limit || powered) 
+	if (limit || powered)
 	{
 		nrow++;
 		srow = nrow * info->rowskip;
-		info->m_J1linearAxis[srow+0] = ax1[0];
-		info->m_J1linearAxis[srow+1] = ax1[1];
-		info->m_J1linearAxis[srow+2] = ax1[2];
-		info->m_J2linearAxis[srow+0] = -ax1[0];
-		info->m_J2linearAxis[srow+1] = -ax1[1];
-		info->m_J2linearAxis[srow+2] = -ax1[2];
+		info->m_J1linearAxis[srow + 0] = ax1[0];
+		info->m_J1linearAxis[srow + 1] = ax1[1];
+		info->m_J1linearAxis[srow + 2] = ax1[2];
+		info->m_J2linearAxis[srow + 0] = -ax1[0];
+		info->m_J2linearAxis[srow + 1] = -ax1[1];
+		info->m_J2linearAxis[srow + 2] = -ax1[2];
 		// linear torque decoupling step:
 		//
 		// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
@@ -493,46 +460,46 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 		// constraint force is applied at must lie along the same ax1 axis.
 		// a torque couple will result in limited slider-jointed free
 		// bodies from gaining angular momentum.
-		if(m_useOffsetForConstraintFrame)
+		if (m_useOffsetForConstraintFrame)
 		{
 			// this is needed only when bodyA and bodyB are both dynamic.
-			if(!hasStaticBody)
+			if (!hasStaticBody)
 			{
 				tmpA = relA.cross(ax1);
 				tmpB = relB.cross(ax1);
-				info->m_J1angularAxis[srow+0] = tmpA[0];
-				info->m_J1angularAxis[srow+1] = tmpA[1];
-				info->m_J1angularAxis[srow+2] = tmpA[2];
-				info->m_J2angularAxis[srow+0] = -tmpB[0];
-				info->m_J2angularAxis[srow+1] = -tmpB[1];
-				info->m_J2angularAxis[srow+2] = -tmpB[2];
+				info->m_J1angularAxis[srow + 0] = tmpA[0];
+				info->m_J1angularAxis[srow + 1] = tmpA[1];
+				info->m_J1angularAxis[srow + 2] = tmpA[2];
+				info->m_J2angularAxis[srow + 0] = -tmpB[0];
+				info->m_J2angularAxis[srow + 1] = -tmpB[1];
+				info->m_J2angularAxis[srow + 2] = -tmpB[2];
 			}
 		}
 		else
-		{ // The old way. May be incorrect if bodies are not on the slider axis
-			btVector3 ltd;	// Linear Torque Decoupling vector (a torque)
+		{                   // The old way. May be incorrect if bodies are not on the slider axis
+			btVector3 ltd;  // Linear Torque Decoupling vector (a torque)
 			ltd = c.cross(ax1);
-			info->m_J1angularAxis[srow+0] = factA*ltd[0];
-			info->m_J1angularAxis[srow+1] = factA*ltd[1];
-			info->m_J1angularAxis[srow+2] = factA*ltd[2];
-			info->m_J2angularAxis[srow+0] = factB*ltd[0];
-			info->m_J2angularAxis[srow+1] = factB*ltd[1];
-			info->m_J2angularAxis[srow+2] = factB*ltd[2];
+			info->m_J1angularAxis[srow + 0] = factA * ltd[0];
+			info->m_J1angularAxis[srow + 1] = factA * ltd[1];
+			info->m_J1angularAxis[srow + 2] = factA * ltd[2];
+			info->m_J2angularAxis[srow + 0] = factB * ltd[0];
+			info->m_J2angularAxis[srow + 1] = factB * ltd[1];
+			info->m_J2angularAxis[srow + 2] = factB * ltd[2];
 		}
 		// right-hand part
 		btScalar lostop = getLowerLinLimit();
 		btScalar histop = getUpperLinLimit();
-		if(limit && (lostop == histop))
+		if (limit && (lostop == histop))
 		{  // the joint motor is ineffective
-			powered = 0;
+			powered = false;
 		}
 		info->m_constraintError[srow] = 0.;
 		info->m_lowerLimit[srow] = 0.;
 		info->m_upperLimit[srow] = 0.;
 		currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
-		if(powered)
+		if (powered)
 		{
-			if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
+			if (m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
 			{
 				info->cfm[srow] = m_cfmDirLin;
 			}
@@ -542,41 +509,41 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 			info->m_lowerLimit[srow] += -getMaxLinMotorForce() / info->fps;
 			info->m_upperLimit[srow] += getMaxLinMotorForce() / info->fps;
 		}
-		if(limit)
+		if (limit)
 		{
 			k = info->fps * currERP;
 			info->m_constraintError[srow] += k * limit_err;
-			if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
+			if (m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
 			{
 				info->cfm[srow] = m_cfmLimLin;
 			}
-			if(lostop == histop) 
-			{	// limited low and high simultaneously
+			if (lostop == histop)
+			{  // limited low and high simultaneously
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
-			else if(limit == 1) 
-			{ // low limit
+			else if (limit == 1)
+			{  // low limit
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = 0;
 			}
-			else 
-			{ // high limit
+			else
+			{  // high limit
 				info->m_lowerLimit[srow] = 0;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
 			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
 			btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
-			if(bounce > btScalar(0.0))
+			if (bounce > btScalar(0.0))
 			{
 				btScalar vel = linVelA.dot(ax1);
 				vel -= linVelB.dot(ax1);
 				vel *= signFact;
 				// only apply bounce if the velocity is incoming, and if the
 				// resulting c[] exceeds what we already have.
-				if(limit == 1)
-				{	// low limit
-					if(vel < 0)
+				if (limit == 1)
+				{  // low limit
+					if (vel < 0)
 					{
 						btScalar newc = -bounce * vel;
 						if (newc > info->m_constraintError[srow])
@@ -586,11 +553,11 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 					}
 				}
 				else
-				{ // high limit - all those computations are reversed
-					if(vel > 0)
+				{  // high limit - all those computations are reversed
+					if (vel > 0)
 					{
 						btScalar newc = -bounce * vel;
-						if(newc < info->m_constraintError[srow]) 
+						if (newc < info->m_constraintError[srow])
 						{
 							info->m_constraintError[srow] = newc;
 						}
@@ -598,44 +565,40 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 				}
 			}
 			info->m_constraintError[srow] *= getSoftnessLimLin();
-		} // if(limit)
-	} // if linear limit
+		}  // if(limit)
+	}      // if linear limit
 	// check angular limits
 	limit_err = btScalar(0.0);
 	limit = 0;
-	if(getSolveAngLimit())
+	if (getSolveAngLimit())
 	{
 		limit_err = getAngDepth();
 		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
 	}
 	// if the slider has joint limits, add in the extra row
-	powered = 0;
-	if(getPoweredAngMotor())
-	{
-		powered = 1;
-	}
-	if(limit || powered) 
+	powered = getPoweredAngMotor();
+	if (limit || powered)
 	{
 		nrow++;
 		srow = nrow * info->rowskip;
-		info->m_J1angularAxis[srow+0] = ax1[0];
-		info->m_J1angularAxis[srow+1] = ax1[1];
-		info->m_J1angularAxis[srow+2] = ax1[2];
+		info->m_J1angularAxis[srow + 0] = ax1[0];
+		info->m_J1angularAxis[srow + 1] = ax1[1];
+		info->m_J1angularAxis[srow + 2] = ax1[2];
 
-		info->m_J2angularAxis[srow+0] = -ax1[0];
-		info->m_J2angularAxis[srow+1] = -ax1[1];
-		info->m_J2angularAxis[srow+2] = -ax1[2];
+		info->m_J2angularAxis[srow + 0] = -ax1[0];
+		info->m_J2angularAxis[srow + 1] = -ax1[1];
+		info->m_J2angularAxis[srow + 2] = -ax1[2];
 
 		btScalar lostop = getLowerAngLimit();
 		btScalar histop = getUpperAngLimit();
-		if(limit && (lostop == histop))
+		if (limit && (lostop == histop))
 		{  // the joint motor is ineffective
-			powered = 0;
+			powered = false;
 		}
 		currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
-		if(powered)
+		if (powered)
 		{
-			if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
+			if (m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
 			{
 				info->cfm[srow] = m_cfmDirAng;
 			}
@@ -644,55 +607,55 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 			info->m_lowerLimit[srow] = -getMaxAngMotorForce() / info->fps;
 			info->m_upperLimit[srow] = getMaxAngMotorForce() / info->fps;
 		}
-		if(limit)
+		if (limit)
 		{
 			k = info->fps * currERP;
 			info->m_constraintError[srow] += k * limit_err;
-			if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
+			if (m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
 			{
 				info->cfm[srow] = m_cfmLimAng;
 			}
-			if(lostop == histop) 
+			if (lostop == histop)
 			{
 				// limited low and high simultaneously
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
-			else if(limit == 1) 
-			{ // low limit
+			else if (limit == 1)
+			{  // low limit
 				info->m_lowerLimit[srow] = 0;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
-			else 
-			{ // high limit
+			else
+			{  // high limit
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = 0;
 			}
 			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
 			btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
-			if(bounce > btScalar(0.0))
+			if (bounce > btScalar(0.0))
 			{
 				btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
 				vel -= m_rbB.getAngularVelocity().dot(ax1);
 				// only apply bounce if the velocity is incoming, and if the
 				// resulting c[] exceeds what we already have.
-				if(limit == 1)
-				{	// low limit
-					if(vel < 0)
+				if (limit == 1)
+				{  // low limit
+					if (vel < 0)
 					{
 						btScalar newc = -bounce * vel;
-						if(newc > info->m_constraintError[srow])
+						if (newc > info->m_constraintError[srow])
 						{
 							info->m_constraintError[srow] = newc;
 						}
 					}
 				}
 				else
-				{	// high limit - all those computations are reversed
-					if(vel > 0)
+				{  // high limit - all those computations are reversed
+					if (vel > 0)
 					{
 						btScalar newc = -bounce * vel;
-						if(newc < info->m_constraintError[srow])
+						if (newc < info->m_constraintError[srow])
 						{
 							info->m_constraintError[srow] = newc;
 						}
@@ -700,165 +663,161 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 				}
 			}
 			info->m_constraintError[srow] *= getSoftnessLimAng();
-		} // if(limit)
-	} // if angular limit or powered
+		}  // if(limit)
+	}      // if angular limit or powered
 }
 
-
-///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///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 btSliderConstraint::setParam(int num, btScalar value, int axis)
 {
-	switch(num)
+	switch (num)
 	{
-	case BT_CONSTRAINT_STOP_ERP :
-		if(axis < 1)
-		{
-			m_softnessLimLin = value;
-			m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN;
-		}
-		else if(axis < 3)
-		{
-			m_softnessOrthoLin = value;
-			m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN;
-		}
-		else if(axis == 3)
-		{
-			m_softnessLimAng = value;
-			m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG;
-		}
-		else if(axis < 6)
-		{
-			m_softnessOrthoAng = value;
-			m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG;
-		}
-		else
-		{
-			btAssertConstrParams(0);
-		}
-		break;
-	case BT_CONSTRAINT_CFM :
-		if(axis < 1)
-		{
-			m_cfmDirLin = value;
-			m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN;
-		}
-		else if(axis == 3)
-		{
-			m_cfmDirAng = value;
-			m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG;
-		}
-		else
-		{
-			btAssertConstrParams(0);
-		}
-		break;
-	case BT_CONSTRAINT_STOP_CFM :
-		if(axis < 1)
-		{
-			m_cfmLimLin = value;
-			m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN;
-		}
-		else if(axis < 3)
-		{
-			m_cfmOrthoLin = value;
-			m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN;
-		}
-		else if(axis == 3)
-		{
-			m_cfmLimAng = value;
-			m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG;
-		}
-		else if(axis < 6)
-		{
-			m_cfmOrthoAng = value;
-			m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG;
-		}
-		else
-		{
-			btAssertConstrParams(0);
-		}
-		break;
+		case BT_CONSTRAINT_STOP_ERP:
+			if (axis < 1)
+			{
+				m_softnessLimLin = value;
+				m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN;
+			}
+			else if (axis < 3)
+			{
+				m_softnessOrthoLin = value;
+				m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN;
+			}
+			else if (axis == 3)
+			{
+				m_softnessLimAng = value;
+				m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG;
+			}
+			else if (axis < 6)
+			{
+				m_softnessOrthoAng = value;
+				m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG;
+			}
+			else
+			{
+				btAssertConstrParams(0);
+			}
+			break;
+		case BT_CONSTRAINT_CFM:
+			if (axis < 1)
+			{
+				m_cfmDirLin = value;
+				m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN;
+			}
+			else if (axis == 3)
+			{
+				m_cfmDirAng = value;
+				m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG;
+			}
+			else
+			{
+				btAssertConstrParams(0);
+			}
+			break;
+		case BT_CONSTRAINT_STOP_CFM:
+			if (axis < 1)
+			{
+				m_cfmLimLin = value;
+				m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN;
+			}
+			else if (axis < 3)
+			{
+				m_cfmOrthoLin = value;
+				m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN;
+			}
+			else if (axis == 3)
+			{
+				m_cfmLimAng = value;
+				m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG;
+			}
+			else if (axis < 6)
+			{
+				m_cfmOrthoAng = value;
+				m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG;
+			}
+			else
+			{
+				btAssertConstrParams(0);
+			}
+			break;
 	}
 }
 
 ///return the local value of parameter
-btScalar btSliderConstraint::getParam(int num, int axis) const 
+btScalar btSliderConstraint::getParam(int num, int axis) const
 {
 	btScalar retVal(SIMD_INFINITY);
-	switch(num)
+	switch (num)
 	{
-	case BT_CONSTRAINT_STOP_ERP :
-		if(axis < 1)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN);
-			retVal = m_softnessLimLin;
-		}
-		else if(axis < 3)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN);
-			retVal = m_softnessOrthoLin;
-		}
-		else if(axis == 3)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG);
-			retVal = m_softnessLimAng;
-		}
-		else if(axis < 6)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG);
-			retVal = m_softnessOrthoAng;
-		}
-		else
-		{
-			btAssertConstrParams(0);
-		}
-		break;
-	case BT_CONSTRAINT_CFM :
-		if(axis < 1)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN);
-			retVal = m_cfmDirLin;
-		}
-		else if(axis == 3)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG);
-			retVal = m_cfmDirAng;
-		}
-		else
-		{
-			btAssertConstrParams(0);
-		}
-		break;
-	case BT_CONSTRAINT_STOP_CFM :
-		if(axis < 1)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN);
-			retVal = m_cfmLimLin;
-		}
-		else if(axis < 3)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN);
-			retVal = m_cfmOrthoLin;
-		}
-		else if(axis == 3)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG);
-			retVal = m_cfmLimAng;
-		}
-		else if(axis < 6)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG);
-			retVal = m_cfmOrthoAng;
-		}
-		else
-		{
-			btAssertConstrParams(0);
-		}
-		break;
+		case BT_CONSTRAINT_STOP_ERP:
+			if (axis < 1)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN);
+				retVal = m_softnessLimLin;
+			}
+			else if (axis < 3)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN);
+				retVal = m_softnessOrthoLin;
+			}
+			else if (axis == 3)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG);
+				retVal = m_softnessLimAng;
+			}
+			else if (axis < 6)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG);
+				retVal = m_softnessOrthoAng;
+			}
+			else
+			{
+				btAssertConstrParams(0);
+			}
+			break;
+		case BT_CONSTRAINT_CFM:
+			if (axis < 1)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN);
+				retVal = m_cfmDirLin;
+			}
+			else if (axis == 3)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG);
+				retVal = m_cfmDirAng;
+			}
+			else
+			{
+				btAssertConstrParams(0);
+			}
+			break;
+		case BT_CONSTRAINT_STOP_CFM:
+			if (axis < 1)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN);
+				retVal = m_cfmLimLin;
+			}
+			else if (axis < 3)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN);
+				retVal = m_cfmOrthoLin;
+			}
+			else if (axis == 3)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG);
+				retVal = m_cfmLimAng;
+			}
+			else if (axis < 6)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG);
+				retVal = m_cfmOrthoAng;
+			}
+			else
+			{
+				btAssertConstrParams(0);
+			}
+			break;
 	}
 	return retVal;
 }
-
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
index 628ada4cfb1..75ca34e978f 100644..100755
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
@@ -25,29 +25,26 @@ TODO:
 #ifndef BT_SLIDER_CONSTRAINT_H
 #define BT_SLIDER_CONSTRAINT_H
 
+#include "LinearMath/btScalar.h"  //for BT_USE_DOUBLE_PRECISION
+
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btSliderConstraintData2		btSliderConstraintDoubleData
-#define btSliderConstraintDataName  "btSliderConstraintDoubleData"
+#define btSliderConstraintData2 btSliderConstraintDoubleData
+#define btSliderConstraintDataName "btSliderConstraintDoubleData"
 #else
-#define btSliderConstraintData2		btSliderConstraintData 
-#define btSliderConstraintDataName	"btSliderConstraintData"
-#endif //BT_USE_DOUBLE_PRECISION
+#define btSliderConstraintData2 btSliderConstraintData
+#define btSliderConstraintDataName "btSliderConstraintData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 #include "LinearMath/btVector3.h"
 #include "btJacobianEntry.h"
 #include "btTypedConstraint.h"
 
-
-
 class btRigidBody;
 
-
-
-#define SLIDER_CONSTRAINT_DEF_SOFTNESS		(btScalar(1.0))
-#define SLIDER_CONSTRAINT_DEF_DAMPING		(btScalar(1.0))
-#define SLIDER_CONSTRAINT_DEF_RESTITUTION	(btScalar(0.7))
-#define SLIDER_CONSTRAINT_DEF_CFM			(btScalar(0.f))
-
+#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0))
+#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0))
+#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7))
+#define SLIDER_CONSTRAINT_DEF_CFM (btScalar(0.f))
 
 enum btSliderFlags
 {
@@ -65,15 +62,15 @@ enum btSliderFlags
 	BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11)
 };
 
-
-ATTRIBUTE_ALIGNED16(class) btSliderConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class)
+btSliderConstraint : public btTypedConstraint
 {
 protected:
 	///for backwards compatibility during the transition to 'getInfo/getInfo2'
-	bool		m_useSolveConstraintObsolete;
-	bool		m_useOffsetForConstraintFrame;
-	btTransform	m_frameInA;
-    btTransform	m_frameInB;
+	bool m_useSolveConstraintObsolete;
+	bool m_useOffsetForConstraintFrame;
+	btTransform m_frameInA;
+	btTransform m_frameInB;
 	// use frameA fo define limits, if true
 	bool m_useLinearReferenceFrameA;
 	// linear limits
@@ -117,21 +114,21 @@ protected:
 	btScalar m_restitutionOrthoAng;
 	btScalar m_dampingOrthoAng;
 	btScalar m_cfmOrthoAng;
-	
+
 	// for interlal use
 	bool m_solveLinLim;
 	bool m_solveAngLim;
 
 	int m_flags;
 
-	btJacobianEntry	m_jacLin[3];
-	btScalar		m_jacLinDiagABInv[3];
+	btJacobianEntry m_jacLin[3];
+	btScalar m_jacLinDiagABInv[3];
 
-    btJacobianEntry	m_jacAng[3];
+	btJacobianEntry m_jacAng[3];
 
 	btScalar m_timeStep;
-    btTransform m_calculatedTransformA;
-    btTransform m_calculatedTransformB;
+	btTransform m_calculatedTransformA;
+	btTransform m_calculatedTransformB;
 
 	btVector3 m_sliderAxis;
 	btVector3 m_realPivotAInW;
@@ -148,57 +145,57 @@ protected:
 	btScalar m_angDepth;
 	btScalar m_kAngle;
 
-	bool	 m_poweredLinMotor;
-    btScalar m_targetLinMotorVelocity;
-    btScalar m_maxLinMotorForce;
-    btScalar m_accumulatedLinMotorImpulse;
-	
-	bool	 m_poweredAngMotor;
-    btScalar m_targetAngMotorVelocity;
-    btScalar m_maxAngMotorForce;
-    btScalar m_accumulatedAngMotorImpulse;
-
-	//------------------------    
+	bool m_poweredLinMotor;
+	btScalar m_targetLinMotorVelocity;
+	btScalar m_maxLinMotorForce;
+	btScalar m_accumulatedLinMotorImpulse;
+
+	bool m_poweredAngMotor;
+	btScalar m_targetAngMotorVelocity;
+	btScalar m_maxAngMotorForce;
+	btScalar m_accumulatedAngMotorImpulse;
+
+	//------------------------
 	void initParams();
+
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	// constructors
-    btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
-    btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
+	btSliderConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
+	btSliderConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
 
 	// overrides
 
-    virtual void getInfo1 (btConstraintInfo1* info);
+	virtual void getInfo1(btConstraintInfo1 * info);
 
-	void getInfo1NonVirtual(btConstraintInfo1* info);
-	
-	virtual void getInfo2 (btConstraintInfo2* info);
+	void getInfo1NonVirtual(btConstraintInfo1 * info);
 
-	void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
+	virtual void getInfo2(btConstraintInfo2 * info);
 
+	void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, btScalar rbAinvMass, btScalar rbBinvMass);
 
 	// access
-    const btRigidBody& getRigidBodyA() const { return m_rbA; }
-    const btRigidBody& getRigidBodyB() const { return m_rbB; }
-    const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
-    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; }
-    btScalar getLowerLinLimit() { return m_lowerLinLimit; }
-    void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
-    btScalar getUpperLinLimit() { return m_upperLinLimit; }
-    void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
-    btScalar getLowerAngLimit() { return m_lowerAngLimit; }
-    void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
-    btScalar getUpperAngLimit() { return m_upperAngLimit; }
-    void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
+	const btRigidBody& getRigidBodyA() const { return m_rbA; }
+	const btRigidBody& getRigidBodyB() const { return m_rbB; }
+	const btTransform& getCalculatedTransformA() const { return m_calculatedTransformA; }
+	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; }
+	btScalar getLowerLinLimit() { return m_lowerLinLimit; }
+	void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
+	btScalar getUpperLinLimit() { return m_upperLinLimit; }
+	void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
+	btScalar getLowerAngLimit() { return m_lowerAngLimit; }
+	void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
+	btScalar getUpperAngLimit() { return m_upperAngLimit; }
+	void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
 	bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
 	btScalar getSoftnessDirLin() { return m_softnessDirLin; }
 	btScalar getRestitutionDirLin() { return m_restitutionDirLin; }
-	btScalar getDampingDirLin() { return m_dampingDirLin ; }
+	btScalar getDampingDirLin() { return m_dampingDirLin; }
 	btScalar getSoftnessDirAng() { return m_softnessDirAng; }
 	btScalar getRestitutionDirAng() { return m_restitutionDirAng; }
 	btScalar getDampingDirAng() { return m_dampingDirAng; }
@@ -247,8 +244,6 @@ public:
 
 	btScalar getLinearPos() const { return m_linPos; }
 	btScalar getAngularPos() const { return m_angPos; }
-	
-	
 
 	// access for ODE solver
 	bool getSolveLinLimit() { return m_solveLinLim; }
@@ -256,9 +251,9 @@ public:
 	bool getSolveAngLimit() { return m_solveAngLim; }
 	btScalar getAngDepth() { return m_angDepth; }
 	// shared code used by ODE solver
-	void	calculateTransforms(const btTransform& transA,const btTransform& transB);
-	void	testLinLimits();
-	void	testAngLimits();
+	void calculateTransforms(const btTransform& transA, const btTransform& transB);
+	void testLinLimits();
+	void testAngLimits();
 	// access for PE Solver
 	btVector3 getAncorInA();
 	btVector3 getAncorInB();
@@ -266,84 +261,75 @@ public:
 	bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
 	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
 
-	void setFrames(const btTransform& frameA, const btTransform& frameB) 
-	{ 
-		m_frameInA=frameA; 
-		m_frameInB=frameB;
-		calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	void setFrames(const btTransform& frameA, const btTransform& frameB)
+	{
+		m_frameInA = frameA;
+		m_frameInB = frameB;
+		calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 		buildJacobian();
-	} 
-
+	}
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///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 void setParam(int num, btScalar value, int axis = -1);
 	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const;
-	
-	virtual	int getFlags() const
-    	{
+	virtual btScalar getParam(int num, int axis = -1) const;
+
+	virtual int getFlags() const
+	{
 		return m_flags;
 	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	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;
-
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
-
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 
-
 struct btSliderConstraintData
 {
-	btTypedConstraintData	m_typeConstraintData;
-	btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTypedConstraintData m_typeConstraintData;
+	btTransformFloatData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransformFloatData m_rbBFrame;
-	
-	float	m_linearUpperLimit;
-	float	m_linearLowerLimit;
 
-	float	m_angularUpperLimit;
-	float	m_angularLowerLimit;
+	float m_linearUpperLimit;
+	float m_linearLowerLimit;
 
-	int	m_useLinearReferenceFrameA;
-	int m_useOffsetForConstraintFrame;
+	float m_angularUpperLimit;
+	float m_angularLowerLimit;
 
+	int m_useLinearReferenceFrameA;
+	int m_useOffsetForConstraintFrame;
 };
 
-
 struct btSliderConstraintDoubleData
 {
-	btTypedConstraintDoubleData	m_typeConstraintData;
-	btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTypedConstraintDoubleData m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransformDoubleData m_rbBFrame;
-	
-	double	m_linearUpperLimit;
-	double	m_linearLowerLimit;
 
-	double	m_angularUpperLimit;
-	double	m_angularLowerLimit;
+	double m_linearUpperLimit;
+	double m_linearLowerLimit;
 
-	int	m_useLinearReferenceFrameA;
-	int m_useOffsetForConstraintFrame;
+	double m_angularUpperLimit;
+	double m_angularLowerLimit;
 
+	int m_useLinearReferenceFrameA;
+	int m_useOffsetForConstraintFrame;
 };
 
-SIMD_FORCE_INLINE		int	btSliderConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btSliderConstraintData2);
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-
-	btSliderConstraintData2* sliderData = (btSliderConstraintData2*) dataBuffer;
-	btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
+	btSliderConstraintData2* sliderData = (btSliderConstraintData2*)dataBuffer;
+	btTypedConstraint::serialize(&sliderData->m_typeConstraintData, serializer);
 
 	m_frameInA.serialize(sliderData->m_rbAFrame);
 	m_frameInB.serialize(sliderData->m_rbBFrame);
@@ -360,7 +346,4 @@ SIMD_FORCE_INLINE	const char*	btSliderConstraint::serialize(void* dataBuffer, bt
 	return btSliderConstraintDataName;
 }
 
-
-
-#endif //BT_SLIDER_CONSTRAINT_H
-
+#endif  //BT_SLIDER_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
index 0c7dbd668bb..1ea20edcb2b 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
@@ -13,43 +13,38 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #include "btSolve2LinearConstraint.h"
 
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btVector3.h"
 #include "btJacobianEntry.h"
 
-
 void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
-												   btRigidBody* body1,
-		btRigidBody* body2,
-
-						const btMatrix3x3& world2A,
-						const btMatrix3x3& world2B,
-						
-						const btVector3& invInertiaADiag,
-						const btScalar invMassA,
-						const btVector3& linvelA,const btVector3& angvelA,
-						const btVector3& rel_posA1,
-						const btVector3& invInertiaBDiag,
-						const btScalar invMassB,
-						const btVector3& linvelB,const btVector3& angvelB,
-						const btVector3& rel_posA2,
-
-					  btScalar depthA, const btVector3& normalA, 
-					  const btVector3& rel_posB1,const btVector3& rel_posB2,
-					  btScalar depthB, const btVector3& normalB, 
-					  btScalar& imp0,btScalar& imp1)
+	btRigidBody* body1,
+	btRigidBody* body2,
+
+	const btMatrix3x3& world2A,
+	const btMatrix3x3& world2B,
+
+	const btVector3& invInertiaADiag,
+	const btScalar invMassA,
+	const btVector3& linvelA, const btVector3& angvelA,
+	const btVector3& rel_posA1,
+	const btVector3& invInertiaBDiag,
+	const btScalar invMassB,
+	const btVector3& linvelB, const btVector3& angvelB,
+	const btVector3& rel_posA2,
+
+	btScalar depthA, const btVector3& normalA,
+	const btVector3& rel_posB1, const btVector3& rel_posB2,
+	btScalar depthB, const btVector3& normalB,
+	btScalar& imp0, btScalar& imp1)
 {
 	(void)linvelA;
 	(void)linvelB;
 	(void)angvelB;
 	(void)angvelA;
 
-
-
 	imp0 = btScalar(0.);
 	imp1 = btScalar(0.);
 
@@ -59,86 +54,76 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
 
 	btAssert(len < SIMD_EPSILON);
 
-
 	//this jacobian entry could be re-used for all iterations
-	btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
-		invInertiaBDiag,invMassB);
-	btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
-		invInertiaBDiag,invMassB);
-	
+	btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
+						 invInertiaBDiag, invMassB);
+	btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
+						 invInertiaBDiag, invMassB);
+
 	//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
 	//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
 
-	const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
-	const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
+	const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1));
+	const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1));
 
-//	btScalar penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
+	//	btScalar penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
 	btScalar massTerm = btScalar(1.) / (invMassA + invMassB);
 
-
 	// calculate rhs (or error) terms
-	const btScalar dv0 = depthA  * m_tau * massTerm - vel0 * m_damping;
-	const btScalar dv1 = depthB  * m_tau * massTerm - vel1 * m_damping;
-
+	const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
+	const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
 
 	// dC/dv * dv = -C
-	
+
 	// jacobian * impulse = -error
 	//
 
 	//impulse = jacobianInverse * -error
 
 	// inverting 2x2 symmetric system (offdiagonal are equal!)
-	// 
+	//
 
+	btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB);
+	btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag);
 
-	btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
-	btScalar	invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
-	
 	//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
 	//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
 
 	imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
-	imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+	imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet;
 
 	//[a b]								  [d -c]
 	//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
 
 	//[jA nD] * [imp0] = [dv0]
 	//[nD jB]   [imp1]   [dv1]
-
 }
 
-
-
 void btSolve2LinearConstraint::resolveBilateralPairConstraint(
-						btRigidBody* body1,
-						btRigidBody* body2,
-						const btMatrix3x3& world2A,
-						const btMatrix3x3& world2B,
-						
-						const btVector3& invInertiaADiag,
-						const btScalar invMassA,
-						const btVector3& linvelA,const btVector3& angvelA,
-						const btVector3& rel_posA1,
-						const btVector3& invInertiaBDiag,
-						const btScalar invMassB,
-						const btVector3& linvelB,const btVector3& angvelB,
-						const btVector3& rel_posA2,
-
-					  btScalar depthA, const btVector3& normalA, 
-					  const btVector3& rel_posB1,const btVector3& rel_posB2,
-					  btScalar depthB, const btVector3& normalB, 
-					  btScalar& imp0,btScalar& imp1)
+	btRigidBody* body1,
+	btRigidBody* body2,
+	const btMatrix3x3& world2A,
+	const btMatrix3x3& world2B,
+
+	const btVector3& invInertiaADiag,
+	const btScalar invMassA,
+	const btVector3& linvelA, const btVector3& angvelA,
+	const btVector3& rel_posA1,
+	const btVector3& invInertiaBDiag,
+	const btScalar invMassB,
+	const btVector3& linvelB, const btVector3& angvelB,
+	const btVector3& rel_posA2,
+
+	btScalar depthA, const btVector3& normalA,
+	const btVector3& rel_posB1, const btVector3& rel_posB2,
+	btScalar depthB, const btVector3& normalB,
+	btScalar& imp0, btScalar& imp1)
 {
-
 	(void)linvelA;
 	(void)linvelB;
 	(void)angvelA;
 	(void)angvelB;
 
-
-
 	imp0 = btScalar(0.);
 	imp1 = btScalar(0.);
 
@@ -148,42 +133,40 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
 
 	btAssert(len < SIMD_EPSILON);
 
-
 	//this jacobian entry could be re-used for all iterations
-	btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
-		invInertiaBDiag,invMassB);
-	btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
-		invInertiaBDiag,invMassB);
-	
+	btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
+						 invInertiaBDiag, invMassB);
+	btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
+						 invInertiaBDiag, invMassB);
+
 	//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
 	//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
 
-	const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
-	const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
+	const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1));
+	const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1));
 
 	// calculate rhs (or error) terms
-	const btScalar dv0 = depthA  * m_tau - vel0 * m_damping;
-	const btScalar dv1 = depthB  * m_tau - vel1 * m_damping;
+	const btScalar dv0 = depthA * m_tau - vel0 * m_damping;
+	const btScalar dv1 = depthB * m_tau - vel1 * m_damping;
 
 	// dC/dv * dv = -C
-	
+
 	// jacobian * impulse = -error
 	//
 
 	//impulse = jacobianInverse * -error
 
 	// inverting 2x2 symmetric system (offdiagonal are equal!)
-	// 
+	//
 
+	btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB);
+	btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag);
 
-	btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
-	btScalar	invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
-	
 	//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
 	//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
 
 	imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
-	imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+	imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet;
 
 	//[a b]								  [d -c]
 	//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
@@ -191,9 +174,9 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
 	//[jA nD] * [imp0] = [dv0]
 	//[nD jB]   [imp1]   [dv1]
 
-	if ( imp0 > btScalar(0.0))
+	if (imp0 > btScalar(0.0))
 	{
-		if ( imp1 > btScalar(0.0) )
+		if (imp1 > btScalar(0.0))
 		{
 			//both positive
 		}
@@ -203,9 +186,10 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
 
 			// now imp0>0 imp1<0
 			imp0 = dv0 / jacA.getDiagonal();
-			if ( imp0 > btScalar(0.0) )
+			if (imp0 > btScalar(0.0))
 			{
-			} else
+			}
+			else
 			{
 				imp0 = btScalar(0.);
 			}
@@ -216,24 +200,25 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
 		imp0 = btScalar(0.);
 
 		imp1 = dv1 / jacB.getDiagonal();
-		if ( imp1 <= btScalar(0.0) )
+		if (imp1 <= btScalar(0.0))
 		{
 			imp1 = btScalar(0.);
 			// now imp0>0 imp1<0
 			imp0 = dv0 / jacA.getDiagonal();
-			if ( imp0 > btScalar(0.0) )
+			if (imp0 > btScalar(0.0))
 			{
-			} else
+			}
+			else
 			{
 				imp0 = btScalar(0.);
 			}
-		} else
+		}
+		else
 		{
 		}
 	}
 }
 
-
 /*
 void btSolve2LinearConstraint::resolveAngularConstraint(	const btMatrix3x3& invInertiaAWS,
 											const btScalar invMassA,
@@ -252,4 +237,3 @@ void btSolve2LinearConstraint::resolveAngularConstraint(	const btMatrix3x3& invI
 
 }
 */
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
index e8bfabf864e..fca8ecec81d 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
@@ -19,20 +19,16 @@ subject to the following restrictions:
 #include "LinearMath/btMatrix3x3.h"
 #include "LinearMath/btVector3.h"
 
-
 class btRigidBody;
 
-
-
 /// constraint class used for lateral tyre friction.
-class	btSolve2LinearConstraint
+class btSolve2LinearConstraint
 {
-	btScalar	m_tau;
-	btScalar	m_damping;
+	btScalar m_tau;
+	btScalar m_damping;
 
 public:
-
-	btSolve2LinearConstraint(btScalar tau,btScalar damping)
+	btSolve2LinearConstraint(btScalar tau, btScalar damping)
 	{
 		m_tau = tau;
 		m_damping = damping;
@@ -40,52 +36,51 @@ public:
 	//
 	// solve unilateral constraint (equality, direct method)
 	//
-	void resolveUnilateralPairConstraint(		
-														   btRigidBody* body0,
+	void resolveUnilateralPairConstraint(
+		btRigidBody* body0,
 		btRigidBody* body1,
 
 		const btMatrix3x3& world2A,
-						const btMatrix3x3& world2B,
-						
-						const btVector3& invInertiaADiag,
-						const btScalar invMassA,
-						const btVector3& linvelA,const btVector3& angvelA,
-						const btVector3& rel_posA1,
-						const btVector3& invInertiaBDiag,
-						const btScalar invMassB,
-						const btVector3& linvelB,const btVector3& angvelB,
-						const btVector3& rel_posA2,
-
-					  btScalar depthA, const btVector3& normalA, 
-					  const btVector3& rel_posB1,const btVector3& rel_posB2,
-					  btScalar depthB, const btVector3& normalB, 
-					  btScalar& imp0,btScalar& imp1);
-
+		const btMatrix3x3& world2B,
+
+		const btVector3& invInertiaADiag,
+		const btScalar invMassA,
+		const btVector3& linvelA, const btVector3& angvelA,
+		const btVector3& rel_posA1,
+		const btVector3& invInertiaBDiag,
+		const btScalar invMassB,
+		const btVector3& linvelB, const btVector3& angvelB,
+		const btVector3& rel_posA2,
+
+		btScalar depthA, const btVector3& normalA,
+		const btVector3& rel_posB1, const btVector3& rel_posB2,
+		btScalar depthB, const btVector3& normalB,
+		btScalar& imp0, btScalar& imp1);
 
 	//
 	// solving 2x2 lcp problem (inequality, direct solution )
 	//
 	void resolveBilateralPairConstraint(
-			btRigidBody* body0,
-						btRigidBody* body1,
+		btRigidBody* body0,
+		btRigidBody* body1,
 		const btMatrix3x3& world2A,
-						const btMatrix3x3& world2B,
-						
-						const btVector3& invInertiaADiag,
-						const btScalar invMassA,
-						const btVector3& linvelA,const btVector3& angvelA,
-						const btVector3& rel_posA1,
-						const btVector3& invInertiaBDiag,
-						const btScalar invMassB,
-						const btVector3& linvelB,const btVector3& angvelB,
-						const btVector3& rel_posA2,
-
-					  btScalar depthA, const btVector3& normalA, 
-					  const btVector3& rel_posB1,const btVector3& rel_posB2,
-					  btScalar depthB, const btVector3& normalB, 
-					  btScalar& imp0,btScalar& imp1);
-
-/*
+		const btMatrix3x3& world2B,
+
+		const btVector3& invInertiaADiag,
+		const btScalar invMassA,
+		const btVector3& linvelA, const btVector3& angvelA,
+		const btVector3& rel_posA1,
+		const btVector3& invInertiaBDiag,
+		const btScalar invMassB,
+		const btVector3& linvelB, const btVector3& angvelB,
+		const btVector3& rel_posA2,
+
+		btScalar depthA, const btVector3& normalA,
+		const btVector3& rel_posB1, const btVector3& rel_posB2,
+		btScalar depthB, const btVector3& normalB,
+		btScalar& imp0, btScalar& imp1);
+
+	/*
 	void resolveAngularConstraint(	const btMatrix3x3& invInertiaAWS,
 						const btScalar invMassA,
 						const btVector3& linvelA,const btVector3& angvelA,
@@ -101,7 +96,6 @@ public:
 					  btScalar& imp0,btScalar& imp1);
 
 */
-
 };
 
-#endif //BT_SOLVE_2LINEAR_CONSTRAINT_H
+#endif  //BT_SOLVE_2LINEAR_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
index 8e4456e617a..409aa8a08cb 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
@@ -16,7 +16,7 @@ subject to the following restrictions:
 #ifndef BT_SOLVER_BODY_H
 #define BT_SOLVER_BODY_H
 
-class	btRigidBody;
+class btRigidBody;
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btMatrix3x3.h"
 
@@ -26,108 +26,99 @@ class	btRigidBody;
 ///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
 #ifdef BT_USE_SSE
 #define USE_SIMD 1
-#endif //
-
+#endif  //
 
 #ifdef USE_SIMD
 
-struct	btSimdScalar
+struct btSimdScalar
 {
-	SIMD_FORCE_INLINE	btSimdScalar()
+	SIMD_FORCE_INLINE btSimdScalar()
 	{
-
 	}
-/* workaround for clang 3.4 ( == apple clang 5.1 ) issue, friction would fail with forced inlining */
-#if (defined(__clang__) && defined(__apple_build_version__) &&  (__clang_major__ == 5) && (__clang_minor__ == 1)) \
-|| (defined(__clang__) && !defined(__apple_build_version__) && (__clang_major__ == 3) && (__clang_minor__ == 4))
-	inline __attribute__ ((noinline)) btSimdScalar(float	fl)
-#else
-	SIMD_FORCE_INLINE	btSimdScalar(float	fl)
-#endif
-	:m_vec128 (_mm_set1_ps(fl))
+
+	SIMD_FORCE_INLINE btSimdScalar(float fl)
+		: m_vec128(_mm_set1_ps(fl))
 	{
 	}
 
-	SIMD_FORCE_INLINE	btSimdScalar(__m128 v128)
-		:m_vec128(v128)
+	SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
+		: m_vec128(v128)
 	{
 	}
-	union
-	{
-		__m128		m_vec128;
-		float		m_floats[4];
-		int			m_ints[4];
-		btScalar	m_unusedPadding;
+	union {
+		__m128 m_vec128;
+		float m_floats[4];
+		int m_ints[4];
+		btScalar m_unusedPadding;
 	};
-	SIMD_FORCE_INLINE	__m128	get128()
+	SIMD_FORCE_INLINE __m128 get128()
 	{
 		return m_vec128;
 	}
 
-	SIMD_FORCE_INLINE	const __m128	get128() const
+	SIMD_FORCE_INLINE const __m128 get128() const
 	{
 		return m_vec128;
 	}
 
-	SIMD_FORCE_INLINE	void	set128(__m128 v128)
+	SIMD_FORCE_INLINE void set128(__m128 v128)
 	{
 		m_vec128 = v128;
 	}
 
-	SIMD_FORCE_INLINE	operator       __m128()       
-	{ 
-		return m_vec128; 
-	}
-	SIMD_FORCE_INLINE	operator const __m128() const 
-	{ 
-		return m_vec128; 
+	SIMD_FORCE_INLINE operator __m128()
+	{
+		return m_vec128;
 	}
-	
-	SIMD_FORCE_INLINE	operator float() const 
-	{ 
-		return m_floats[0]; 
+	SIMD_FORCE_INLINE operator const __m128() const
+	{
+		return m_vec128;
 	}
 
+	SIMD_FORCE_INLINE operator float() const
+	{
+		return m_floats[0];
+	}
 };
 
 ///@brief Return the elementwise product of two btSimdScalar
-SIMD_FORCE_INLINE btSimdScalar 
-operator*(const btSimdScalar& v1, const btSimdScalar& v2) 
+SIMD_FORCE_INLINE btSimdScalar
+operator*(const btSimdScalar& v1, const btSimdScalar& v2)
 {
-	return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
+	return btSimdScalar(_mm_mul_ps(v1.get128(), v2.get128()));
 }
 
 ///@brief Return the elementwise product of two btSimdScalar
-SIMD_FORCE_INLINE btSimdScalar 
-operator+(const btSimdScalar& v1, const btSimdScalar& v2) 
+SIMD_FORCE_INLINE btSimdScalar
+operator+(const btSimdScalar& v1, const btSimdScalar& v2)
 {
-	return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
+	return btSimdScalar(_mm_add_ps(v1.get128(), v2.get128()));
 }
 
-
 #else
 #define btSimdScalar btScalar
 #endif
 
 ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
-ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
+ATTRIBUTE_ALIGNED16(struct)
+btSolverBody
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	btTransform		m_worldTransform;
-	btVector3		m_deltaLinearVelocity;
-	btVector3		m_deltaAngularVelocity;
-	btVector3		m_angularFactor;
-	btVector3		m_linearFactor;
-	btVector3		m_invMass;
-	btVector3		m_pushVelocity;
-	btVector3		m_turnVelocity;
-	btVector3		m_linearVelocity;
-	btVector3		m_angularVelocity;
-	btVector3		m_externalForceImpulse;
-	btVector3		m_externalTorqueImpulse;
-
-	btRigidBody*	m_originalBody;
-	void	setWorldTransform(const btTransform& worldTransform)
+	btTransform m_worldTransform;
+	btVector3 m_deltaLinearVelocity;
+	btVector3 m_deltaAngularVelocity;
+	btVector3 m_angularFactor;
+	btVector3 m_linearFactor;
+	btVector3 m_invMass;
+	btVector3 m_pushVelocity;
+	btVector3 m_turnVelocity;
+	btVector3 m_linearVelocity;
+	btVector3 m_angularVelocity;
+	btVector3 m_externalForceImpulse;
+	btVector3 m_externalTorqueImpulse;
+
+	btRigidBody* m_originalBody;
+	void setWorldTransform(const btTransform& worldTransform)
 	{
 		m_worldTransform = worldTransform;
 	}
@@ -136,56 +127,50 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 	{
 		return m_worldTransform;
 	}
-	
-	
 
-	SIMD_FORCE_INLINE void	getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity ) const
+	SIMD_FORCE_INLINE void getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity) const
 	{
 		if (m_originalBody)
-			velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity+m_externalTorqueImpulse).cross(rel_pos);
+			velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity + m_externalTorqueImpulse).cross(rel_pos);
 		else
-			velocity.setValue(0,0,0);
+			velocity.setValue(0, 0, 0);
 	}
 
-
-	SIMD_FORCE_INLINE void	getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
+	SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity) const
 	{
 		if (m_originalBody)
-			velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
+			velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
 		else
-			velocity.setValue(0,0,0);
+			velocity.setValue(0, 0, 0);
 	}
 
-	SIMD_FORCE_INLINE void	getAngularVelocity(btVector3& angVel) const
+	SIMD_FORCE_INLINE void getAngularVelocity(btVector3 & angVel) const
 	{
 		if (m_originalBody)
-			angVel =m_angularVelocity+m_deltaAngularVelocity;
+			angVel = m_angularVelocity + m_deltaAngularVelocity;
 		else
-			angVel.setValue(0,0,0);
+			angVel.setValue(0, 0, 0);
 	}
 
-
 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent, const btScalar impulseMagnitude)
 	{
 		if (m_originalBody)
 		{
-			m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
-			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+			m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
+			m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 		}
 	}
 
-	SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent, btScalar impulseMagnitude)
 	{
 		if (m_originalBody)
 		{
-			m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
-			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+			m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor;
+			m_turnVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 		}
 	}
 
-
-
 	const btVector3& getDeltaLinearVelocity() const
 	{
 		return m_deltaLinearVelocity;
@@ -196,20 +181,19 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 		return m_deltaAngularVelocity;
 	}
 
-	const btVector3& getPushVelocity() const 
+	const btVector3& getPushVelocity() const
 	{
 		return m_pushVelocity;
 	}
 
-	const btVector3& getTurnVelocity() const 
+	const btVector3& getTurnVelocity() const
 	{
 		return m_turnVelocity;
 	}
 
-
 	////////////////////////////////////////////////
 	///some internal methods, don't use them
-		
+
 	btVector3& internalGetDeltaLinearVelocity()
 	{
 		return m_deltaLinearVelocity;
@@ -234,7 +218,7 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 	{
 		m_invMass = invMass;
 	}
-	
+
 	btVector3& internalGetPushVelocity()
 	{
 		return m_pushVelocity;
@@ -245,67 +229,57 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 		return m_turnVelocity;
 	}
 
-	SIMD_FORCE_INLINE void	internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
+	SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity) const
 	{
-		velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
+		velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
 	}
 
-	SIMD_FORCE_INLINE void	internalGetAngularVelocity(btVector3& angVel) const
+	SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3 & angVel) const
 	{
-		angVel = m_angularVelocity+m_deltaAngularVelocity;
+		angVel = m_angularVelocity + m_deltaAngularVelocity;
 	}
 
-
 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent, const btScalar impulseMagnitude)
 	{
 		if (m_originalBody)
 		{
-			m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
-			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+			m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
+			m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 		}
 	}
-		
-	
-	
 
-	void	writebackVelocity()
+	void writebackVelocity()
 	{
 		if (m_originalBody)
 		{
-			m_linearVelocity +=m_deltaLinearVelocity;
+			m_linearVelocity += m_deltaLinearVelocity;
 			m_angularVelocity += m_deltaAngularVelocity;
-			
+
 			//m_originalBody->setCompanionId(-1);
 		}
 	}
 
-
-	void	writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp)
+	void writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp)
 	{
-        (void) timeStep;
+		(void)timeStep;
 		if (m_originalBody)
 		{
 			m_linearVelocity += m_deltaLinearVelocity;
 			m_angularVelocity += m_deltaAngularVelocity;
-			
+
 			//correct the position/orientation based on push/turn recovery
 			btTransform newTransform;
-			if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
+			if (m_pushVelocity[0] != 0.f || m_pushVelocity[1] != 0 || m_pushVelocity[2] != 0 || m_turnVelocity[0] != 0.f || m_turnVelocity[1] != 0 || m_turnVelocity[2] != 0)
 			{
-			//	btQuaternion orn = m_worldTransform.getRotation();
-				btTransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
+				//	btQuaternion orn = m_worldTransform.getRotation();
+				btTransformUtil::integrateTransform(m_worldTransform, m_pushVelocity, m_turnVelocity * splitImpulseTurnErp, timeStep, newTransform);
 				m_worldTransform = newTransform;
 			}
 			//m_worldTransform.setRotation(orn);
 			//m_originalBody->setCompanionId(-1);
 		}
 	}
-	
-
-
 };
 
-#endif //BT_SOLVER_BODY_H
-
-
+#endif  //BT_SOLVER_BODY_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
index 5515e6b311c..c7938df8675 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
@@ -16,7 +16,7 @@ subject to the following restrictions:
 #ifndef BT_SOLVER_CONSTRAINT_H
 #define BT_SOLVER_CONSTRAINT_H
 
-class	btRigidBody;
+class btRigidBody;
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btMatrix3x3.h"
 #include "btJacobianEntry.h"
@@ -25,56 +25,50 @@ class	btRigidBody;
 //#define NO_FRICTION_TANGENTIALS 1
 #include "btSolverBody.h"
 
-
 ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
-ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
+ATTRIBUTE_ALIGNED16(struct)
+btSolverConstraint
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btVector3		m_relpos1CrossNormal;
-	btVector3		m_contactNormal1;
-
-	btVector3		m_relpos2CrossNormal;
-	btVector3		m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
-
-	btVector3		m_angularComponentA;
-	btVector3		m_angularComponentB;
-	
-	mutable btSimdScalar	m_appliedPushImpulse;
-	mutable btSimdScalar	m_appliedImpulse;
-
-	btScalar	m_friction;
-	btScalar	m_jacDiagABInv;
-	btScalar		m_rhs;
-	btScalar		m_cfm;
-	
-    btScalar		m_lowerLimit;
-	btScalar		m_upperLimit;
-	btScalar		m_rhsPenetration;
-    union
-	{
-		void*		m_originalContactPoint;
-		btScalar	m_unusedPadding4;
-		int			m_numRowsForNonContactConstraint;
+	btVector3 m_relpos1CrossNormal;
+	btVector3 m_contactNormal1;
+
+	btVector3 m_relpos2CrossNormal;
+	btVector3 m_contactNormal2;  //usually m_contactNormal2 == -m_contactNormal1, but not always
+
+	btVector3 m_angularComponentA;
+	btVector3 m_angularComponentB;
+
+	mutable btSimdScalar m_appliedPushImpulse;
+	mutable btSimdScalar m_appliedImpulse;
+
+	btScalar m_friction;
+	btScalar m_jacDiagABInv;
+	btScalar m_rhs;
+	btScalar m_cfm;
+
+	btScalar m_lowerLimit;
+	btScalar m_upperLimit;
+	btScalar m_rhsPenetration;
+	union {
+		void* m_originalContactPoint;
+		btScalar m_unusedPadding4;
+		int m_numRowsForNonContactConstraint;
 	};
 
-	int	m_overrideNumSolverIterations;
-    int			m_frictionIndex;
+	int m_overrideNumSolverIterations;
+	int m_frictionIndex;
 	int m_solverBodyIdA;
 	int m_solverBodyIdB;
 
-    
-	enum		btSolverConstraintType
+	enum btSolverConstraintType
 	{
 		BT_SOLVER_CONTACT_1D = 0,
 		BT_SOLVER_FRICTION_1D
 	};
 };
 
-typedef btAlignedObjectArray<btSolverConstraint>	btConstraintArray;
-
-
-#endif //BT_SOLVER_CONSTRAINT_H
-
-
+typedef btAlignedObjectArray<btSolverConstraint> btConstraintArray;
 
+#endif  //BT_SOLVER_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
index 736a64a1c94..ebe679c449d 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
@@ -13,69 +13,63 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btTypedConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btSerializer.h"
 
-
-#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
+#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.05f)
 
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
-:btTypedObject(type),
-m_userConstraintType(-1),
-m_userConstraintPtr((void*)-1),
-m_breakingImpulseThreshold(SIMD_INFINITY),
-m_isEnabled(true),
-m_needsFeedback(false),
-m_overrideNumSolverIterations(-1),
-m_rbA(rbA),
-m_rbB(getFixedBody()),
-m_appliedImpulse(btScalar(0.)),
-m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
-m_jointFeedback(0)
+	: btTypedObject(type),
+	  m_userConstraintType(-1),
+	  m_userConstraintPtr((void*)-1),
+	  m_breakingImpulseThreshold(SIMD_INFINITY),
+	  m_isEnabled(true),
+	  m_needsFeedback(false),
+	  m_overrideNumSolverIterations(-1),
+	  m_rbA(rbA),
+	  m_rbB(getFixedBody()),
+	  m_appliedImpulse(btScalar(0.)),
+	  m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
+	  m_jointFeedback(0)
 {
 }
 
-
-btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
-:btTypedObject(type),
-m_userConstraintType(-1),
-m_userConstraintPtr((void*)-1),
-m_breakingImpulseThreshold(SIMD_INFINITY),
-m_isEnabled(true),
-m_needsFeedback(false),
-m_overrideNumSolverIterations(-1),
-m_rbA(rbA),
-m_rbB(rbB),
-m_appliedImpulse(btScalar(0.)),
-m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
-m_jointFeedback(0)
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA, btRigidBody& rbB)
+	: btTypedObject(type),
+	  m_userConstraintType(-1),
+	  m_userConstraintPtr((void*)-1),
+	  m_breakingImpulseThreshold(SIMD_INFINITY),
+	  m_isEnabled(true),
+	  m_needsFeedback(false),
+	  m_overrideNumSolverIterations(-1),
+	  m_rbA(rbA),
+	  m_rbB(rbB),
+	  m_appliedImpulse(btScalar(0.)),
+	  m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
+	  m_jointFeedback(0)
 {
 }
 
-
-
-
 btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
 {
-	if(lowLim > uppLim)
+	if (lowLim > uppLim)
 	{
 		return btScalar(1.0f);
 	}
-	else if(lowLim == uppLim)
+	else if (lowLim == uppLim)
 	{
 		return btScalar(0.0f);
 	}
 	btScalar lim_fact = btScalar(1.0f);
 	btScalar delta_max = vel / timeFact;
-	if(delta_max < btScalar(0.0f))
+	if (delta_max < btScalar(0.0f))
 	{
-		if((pos >= lowLim) && (pos < (lowLim - delta_max)))
+		if ((pos >= lowLim) && (pos < (lowLim - delta_max)))
 		{
 			lim_fact = (lowLim - pos) / delta_max;
 		}
-		else if(pos  < lowLim)
+		else if (pos < lowLim)
 		{
 			lim_fact = btScalar(0.0f);
 		}
@@ -84,13 +78,13 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal
 			lim_fact = btScalar(1.0f);
 		}
 	}
-	else if(delta_max > btScalar(0.0f))
+	else if (delta_max > btScalar(0.0f))
 	{
-		if((pos <= uppLim) && (pos > (uppLim - delta_max)))
+		if ((pos <= uppLim) && (pos > (uppLim - delta_max)))
 		{
 			lim_fact = (uppLim - pos) / delta_max;
 		}
-		else if(pos  > uppLim)
+		else if (pos > uppLim)
 		{
 			lim_fact = btScalar(0.0f);
 		}
@@ -101,19 +95,19 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal
 	}
 	else
 	{
-			lim_fact = btScalar(0.0f);
+		lim_fact = btScalar(0.0f);
 	}
 	return lim_fact;
 }
 
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btTypedConstraintData2* tcd = (btTypedConstraintData2*) dataBuffer;
+	btTypedConstraintData2* tcd = (btTypedConstraintData2*)dataBuffer;
 
 	tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
 	tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
-	char* name = (char*) serializer->findNameForPointer(this);
+	char* name = (char*)serializer->findNameForPointer(this);
 	tcd->m_name = (char*)serializer->getUniquePointer(name);
 	if (tcd->m_name)
 	{
@@ -124,10 +118,10 @@ const char*	btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
 	tcd->m_needsFeedback = m_needsFeedback;
 	tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations;
 	tcd->m_breakingImpulseThreshold = m_breakingImpulseThreshold;
-	tcd->m_isEnabled = m_isEnabled? 1: 0;
-	
-	tcd->m_userConstraintId =m_userConstraintId;
-	tcd->m_userConstraintType =m_userConstraintType;
+	tcd->m_isEnabled = m_isEnabled ? 1 : 0;
+
+	tcd->m_userConstraintId = m_userConstraintId;
+	tcd->m_userConstraintType = m_userConstraintType;
 
 	tcd->m_appliedImpulse = m_appliedImpulse;
 	tcd->m_dbgDrawSize = m_dbgDrawSize;
@@ -135,10 +129,10 @@ const char*	btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
 	tcd->m_disableCollisionsBetweenLinkedBodies = false;
 
 	int i;
-	for (i=0;i<m_rbA.getNumConstraintRefs();i++)
+	for (i = 0; i < m_rbA.getNumConstraintRefs(); i++)
 		if (m_rbA.getConstraintRef(i) == this)
 			tcd->m_disableCollisionsBetweenLinkedBodies = true;
-	for (i=0;i<m_rbB.getNumConstraintRefs();i++)
+	for (i = 0; i < m_rbB.getNumConstraintRefs(); i++)
 		if (m_rbB.getConstraintRef(i) == this)
 			tcd->m_disableCollisionsBetweenLinkedBodies = true;
 
@@ -147,17 +141,16 @@ const char*	btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
 
 btRigidBody& btTypedConstraint::getFixedBody()
 {
-	static btRigidBody s_fixed(0, 0,0);
-	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+	static btRigidBody s_fixed(0, 0, 0);
+	s_fixed.setMassProps(btScalar(0.), btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
 	return s_fixed;
 }
 
-
 void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor)
 {
 	m_halfRange = (high - low) / 2.0f;
 	m_center = btNormalizeAngle(low + m_halfRange);
-	m_softness =  _softness;
+	m_softness = _softness;
 	m_biasFactor = _biasFactor;
 	m_relaxationFactor = _relaxationFactor;
 }
@@ -174,7 +167,7 @@ void btAngularLimit::test(const btScalar angle)
 		if (deviation < -m_halfRange)
 		{
 			m_solveLimit = true;
-			m_correction = - (deviation + m_halfRange);
+			m_correction = -(deviation + m_halfRange);
 			m_sign = +1.0f;
 		}
 		else if (deviation > m_halfRange)
@@ -186,7 +179,6 @@ void btAngularLimit::test(const btScalar angle)
 	}
 }
 
-
 btScalar btAngularLimit::getError() const
 {
 	return m_correction * m_sign;
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
index b55db0c1bfb..d30f3dee5c5 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
@@ -16,26 +16,24 @@ subject to the following restrictions:
 #ifndef BT_TYPED_CONSTRAINT_H
 #define BT_TYPED_CONSTRAINT_H
 
-
 #include "LinearMath/btScalar.h"
 #include "btSolverConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btTypedConstraintData2		btTypedConstraintDoubleData
-#define btTypedConstraintDataName	"btTypedConstraintDoubleData"
+#define btTypedConstraintData2 btTypedConstraintDoubleData
+#define btTypedConstraintDataName "btTypedConstraintDoubleData"
 #else
-#define btTypedConstraintData2 		btTypedConstraintFloatData
-#define btTypedConstraintDataName  "btTypedConstraintFloatData" 
-#endif //BT_USE_DOUBLE_PRECISION
-
+#define btTypedConstraintData2 btTypedConstraintFloatData
+#define btTypedConstraintDataName "btTypedConstraintFloatData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 class btSerializer;
 
 //Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
 enum btTypedConstraintType
 {
-	POINT2POINT_CONSTRAINT_TYPE=3,
+	POINT2POINT_CONSTRAINT_TYPE = 3,
 	HINGE_CONSTRAINT_TYPE,
 	CONETWIST_CONSTRAINT_TYPE,
 	D6_CONSTRAINT_TYPE,
@@ -48,91 +46,88 @@ enum btTypedConstraintType
 	MAX_CONSTRAINT_TYPE
 };
 
-
 enum btConstraintParams
 {
-	BT_CONSTRAINT_ERP=1,
+	BT_CONSTRAINT_ERP = 1,
 	BT_CONSTRAINT_STOP_ERP,
 	BT_CONSTRAINT_CFM,
 	BT_CONSTRAINT_STOP_CFM
 };
 
 #if 1
-	#define btAssertConstrParams(_par) btAssert(_par) 
+#define btAssertConstrParams(_par) btAssert(_par)
 #else
-	#define btAssertConstrParams(_par)
+#define btAssertConstrParams(_par)
 #endif
 
-
-ATTRIBUTE_ALIGNED16(struct)	btJointFeedback
+ATTRIBUTE_ALIGNED16(struct)
+btJointFeedback
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	btVector3	m_appliedForceBodyA;
-	btVector3	m_appliedTorqueBodyA;
-	btVector3	m_appliedForceBodyB;
-	btVector3	m_appliedTorqueBodyB;
+	btVector3 m_appliedForceBodyA;
+	btVector3 m_appliedTorqueBodyA;
+	btVector3 m_appliedForceBodyB;
+	btVector3 m_appliedTorqueBodyB;
 };
 
-
 ///TypedConstraint is the baseclass for Bullet constraints and vehicles
-ATTRIBUTE_ALIGNED16(class) btTypedConstraint : public btTypedObject
+ATTRIBUTE_ALIGNED16(class)
+btTypedConstraint : public btTypedObject
 {
-	int	m_userConstraintType;
+	int m_userConstraintType;
 
-	union
-	{
-		int	m_userConstraintId;
+	union {
+		int m_userConstraintId;
 		void* m_userConstraintPtr;
 	};
 
-	btScalar	m_breakingImpulseThreshold;
-	bool		m_isEnabled;
-	bool		m_needsFeedback;
-	int			m_overrideNumSolverIterations;
+	btScalar m_breakingImpulseThreshold;
+	bool m_isEnabled;
+	bool m_needsFeedback;
+	int m_overrideNumSolverIterations;
 
-
-	btTypedConstraint&	operator=(btTypedConstraint&	other)
+	btTypedConstraint& operator=(btTypedConstraint& other)
 	{
 		btAssert(0);
-		(void) other;
+		(void)other;
 		return *this;
 	}
 
 protected:
-	btRigidBody&	m_rbA;
-	btRigidBody&	m_rbB;
-	btScalar	m_appliedImpulse;
-	btScalar	m_dbgDrawSize;
-	btJointFeedback*	m_jointFeedback;
+	btRigidBody& m_rbA;
+	btRigidBody& m_rbB;
+	btScalar m_appliedImpulse;
+	btScalar m_dbgDrawSize;
+	btJointFeedback* m_jointFeedback;
 
 	///internal method used by the constraint solver, don't use them directly
 	btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
-	
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	virtual ~btTypedConstraint() {};
-	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
-	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
+	virtual ~btTypedConstraint(){};
+	btTypedConstraint(btTypedConstraintType type, btRigidBody & rbA);
+	btTypedConstraint(btTypedConstraintType type, btRigidBody & rbA, btRigidBody & rbB);
 
-	struct btConstraintInfo1 {
-		int m_numConstraintRows,nub;
+	struct btConstraintInfo1
+	{
+		int m_numConstraintRows, nub;
 	};
 
 	static btRigidBody& getFixedBody();
 
-	struct btConstraintInfo2 {
+	struct btConstraintInfo2
+	{
 		// integrator parameters: frames per second (1/stepsize), default error
 		// reduction parameter (0..1).
-		btScalar fps,erp;
+		btScalar fps, erp;
 
 		// for the first and second body, pointers to two (linear and angular)
 		// n*3 jacobian sub matrices, stored by rows. these matrices will have
 		// been initialized to 0 on entry. if the second body is zero then the
 		// J2xx pointers may be 0.
-		btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
+		btScalar *m_J1linearAxis, *m_J1angularAxis, *m_J2linearAxis, *m_J2angularAxis;
 
 		// elements to jump from one row to the next in J's
 		int rowskip;
@@ -140,24 +135,19 @@ public:
 		// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
 		// "constraint force mixing" vector. c is set to zero on entry, cfm is
 		// set to a constant value (typically very small or zero) value on entry.
-		btScalar *m_constraintError,*cfm;
+		btScalar *m_constraintError, *cfm;
 
 		// lo and hi limits for variables (set to -/+ infinity on entry).
-		btScalar *m_lowerLimit,*m_upperLimit;
+		btScalar *m_lowerLimit, *m_upperLimit;
 
-		// findex vector for variables. see the LCP solver interface for a
-		// description of what this does. this is set to -1 on entry.
-		// note that the returned indexes are relative to the first index of
-		// the constraint.
-		int *findex;
 		// number of solver iterations
 		int m_numIterations;
 
 		//damping of the velocity
-		btScalar	m_damping;
+		btScalar m_damping;
 	};
 
-	int	getOverrideNumSolverIterations() const
+	int getOverrideNumSolverIterations() const
 	{
 		return m_overrideNumSolverIterations;
 	}
@@ -170,60 +160,57 @@ public:
 	}
 
 	///internal method used by the constraint solver, don't use them directly
-	virtual void	buildJacobian() {};
+	virtual void buildJacobian(){};
 
 	///internal method used by the constraint solver, don't use them directly
-	virtual	void	setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
+	virtual void setupSolverConstraint(btConstraintArray & ca, int solverBodyA, int solverBodyB, btScalar timeStep)
 	{
-        (void)ca;
-        (void)solverBodyA;
-        (void)solverBodyB;
-        (void)timeStep;
+		(void)ca;
+		(void)solverBodyA;
+		(void)solverBodyB;
+		(void)timeStep;
 	}
-	
+
 	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo1 (btConstraintInfo1* info)=0;
+	virtual void getInfo1(btConstraintInfo1 * info) = 0;
 
 	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo2 (btConstraintInfo2* info)=0;
+	virtual void getInfo2(btConstraintInfo2 * info) = 0;
 
 	///internal method used by the constraint solver, don't use them directly
-	void	internalSetAppliedImpulse(btScalar appliedImpulse)
+	void internalSetAppliedImpulse(btScalar appliedImpulse)
 	{
 		m_appliedImpulse = appliedImpulse;
 	}
 	///internal method used by the constraint solver, don't use them directly
-	btScalar	internalGetAppliedImpulse()
+	btScalar internalGetAppliedImpulse()
 	{
 		return m_appliedImpulse;
 	}
 
-
-	btScalar	getBreakingImpulseThreshold() const
+	btScalar getBreakingImpulseThreshold() const
 	{
-		return 	m_breakingImpulseThreshold;
+		return m_breakingImpulseThreshold;
 	}
 
-	void	setBreakingImpulseThreshold(btScalar threshold)
+	void setBreakingImpulseThreshold(btScalar threshold)
 	{
 		m_breakingImpulseThreshold = threshold;
 	}
 
-	bool	isEnabled() const
+	bool isEnabled() const
 	{
 		return m_isEnabled;
 	}
 
-	void	setEnabled(bool enabled)
+	void setEnabled(bool enabled)
 	{
-		m_isEnabled=enabled;
+		m_isEnabled = enabled;
 	}
 
-
 	///internal method used by the constraint solver, don't use them directly
-	virtual	void	solveConstraintObsolete(btSolverBody& /*bodyA*/,btSolverBody& /*bodyB*/,btScalar	/*timeStep*/) {};
+	virtual void solveConstraintObsolete(btSolverBody& /*bodyA*/, btSolverBody& /*bodyB*/, btScalar /*timeStep*/){};
 
-	
 	const btRigidBody& getRigidBodyA() const
 	{
 		return m_rbA;
@@ -233,7 +220,7 @@ public:
 		return m_rbB;
 	}
 
-	btRigidBody& getRigidBodyA() 
+	btRigidBody& getRigidBodyA()
 	{
 		return m_rbA;
 	}
@@ -244,15 +231,15 @@ public:
 
 	int getUserConstraintType() const
 	{
-		return m_userConstraintType ;
+		return m_userConstraintType;
 	}
 
-	void	setUserConstraintType(int userConstraintType)
+	void setUserConstraintType(int userConstraintType)
 	{
 		m_userConstraintType = userConstraintType;
 	};
 
-	void	setUserConstraintId(int uid)
+	void setUserConstraintId(int uid)
 	{
 		m_userConstraintId = uid;
 	}
@@ -262,17 +249,17 @@ public:
 		return m_userConstraintId;
 	}
 
-	void	setUserConstraintPtr(void* ptr)
+	void setUserConstraintPtr(void* ptr)
 	{
 		m_userConstraintPtr = ptr;
 	}
 
-	void*	getUserConstraintPtr()
+	void* getUserConstraintPtr()
 	{
 		return m_userConstraintPtr;
 	}
 
-	void	setJointFeedback(btJointFeedback* jointFeedback)
+	void setJointFeedback(btJointFeedback * jointFeedback)
 	{
 		m_jointFeedback = jointFeedback;
 	}
@@ -287,37 +274,36 @@ public:
 		return m_jointFeedback;
 	}
 
-
 	int getUid() const
 	{
-		return m_userConstraintId;   
-	} 
+		return m_userConstraintId;
+	}
 
-	bool	needsFeedback() const
+	bool needsFeedback() const
 	{
 		return m_needsFeedback;
 	}
 
 	///enableFeedback will allow to read the applied linear and angular impulse
 	///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
-	void	enableFeedback(bool needsFeedback)
+	void enableFeedback(bool needsFeedback)
 	{
 		m_needsFeedback = needsFeedback;
 	}
 
-	///getAppliedImpulse is an estimated total applied impulse. 
+	///getAppliedImpulse is an estimated total applied impulse.
 	///This feedback could be used to determine breaking constraints or playing sounds.
-	btScalar	getAppliedImpulse() const
+	btScalar getAppliedImpulse() const
 	{
 		btAssert(m_needsFeedback);
 		return m_appliedImpulse;
 	}
 
-	btTypedConstraintType getConstraintType () const
+	btTypedConstraintType getConstraintType() const
 	{
 		return btTypedConstraintType(m_objectType);
 	}
-	
+
 	void setDbgDrawSize(btScalar dbgDrawSize)
 	{
 		m_dbgDrawSize = dbgDrawSize;
@@ -327,35 +313,34 @@ public:
 		return m_dbgDrawSize;
 	}
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///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) = 0;
+	virtual void setParam(int num, btScalar value, int axis = -1) = 0;
 
 	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const = 0;
-	
-	virtual	int	calculateSerializeBufferSize() const;
+	virtual btScalar getParam(int num, int axis = -1) const = 0;
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+	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;
 };
 
-// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits 
+// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
 // all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
 SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
 {
-	if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
+	if (angleLowerLimitInRadians >= angleUpperLimitInRadians)
 	{
 		return angleInRadians;
 	}
-	else if(angleInRadians < angleLowerLimitInRadians)
+	else if (angleInRadians < angleLowerLimitInRadians)
 	{
 		btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
 		btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
 		return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
 	}
-	else if(angleInRadians > angleUpperLimitInRadians)
+	else if (angleInRadians > angleUpperLimitInRadians)
 	{
 		btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
 		btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
@@ -367,6 +352,8 @@ SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScal
 	}
 }
 
+// clang-format off
+
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btTypedConstraintFloatData
 {
@@ -390,6 +377,8 @@ struct	btTypedConstraintFloatData
 	
 };
 
+
+
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 
 #define BT_BACKWARDS_COMPATIBLE_SERIALIZATION
@@ -441,18 +430,17 @@ struct	btTypedConstraintDoubleData
 	
 };
 
+// clang-format on
 
-SIMD_FORCE_INLINE	int	btTypedConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btTypedConstraintData2);
 }
 
-
-
 class btAngularLimit
 {
 private:
-	btScalar 
+	btScalar
 		m_center,
 		m_halfRange,
 		m_softness,
@@ -467,15 +455,16 @@ private:
 public:
 	/// Default constructor initializes limit as inactive, allowing free constraint movement
 	btAngularLimit()
-		:m_center(0.0f),
-		m_halfRange(-1.0f),
-		m_softness(0.9f),
-		m_biasFactor(0.3f),
-		m_relaxationFactor(1.0f),
-		m_correction(0.0f),
-		m_sign(0.0f),
-		m_solveLimit(false)
-	{}
+		: m_center(0.0f),
+		  m_halfRange(-1.0f),
+		  m_softness(0.9f),
+		  m_biasFactor(0.3f),
+		  m_relaxationFactor(1.0f),
+		  m_correction(0.0f),
+		  m_sign(0.0f),
+		  m_solveLimit(false)
+	{
+	}
 
 	/// Sets all limit's parameters.
 	/// When low > high limit becomes inactive.
@@ -504,13 +493,13 @@ public:
 		return m_relaxationFactor;
 	}
 
-	/// Returns correction value evaluated when test() was invoked 
+	/// Returns correction value evaluated when test() was invoked
 	inline btScalar getCorrection() const
 	{
 		return m_correction;
 	}
 
-	/// Returns sign value evaluated when test() was invoked 
+	/// Returns sign value evaluated when test() was invoked
 	inline btScalar getSign() const
 	{
 		return m_sign;
@@ -538,9 +527,6 @@ public:
 	btScalar getLow() const;
 
 	btScalar getHigh() const;
-
 };
 
-
-
-#endif //BT_TYPED_CONSTRAINT_H
+#endif  //BT_TYPED_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
index b009f41aec8..42ed1fbb878 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
@@ -13,43 +13,38 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #include "btUniversalConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
 
-
-
 #define UNIV_EPS btScalar(0.01f)
 
-
 // constructor
 // anchor, axis1 and axis2 are in world coordinate system
 // axis1 must be orthogonal to axis2
 btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2)
-: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
- m_anchor(anchor),
- m_axis1(axis1),
- m_axis2(axis2)
+	: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
+	  m_anchor(anchor),
+	  m_axis1(axis1),
+	  m_axis2(axis2)
 {
 	// build frame basis
 	// 6DOF constraint uses Euler angles and to define limits
 	// it is assumed that rotational order is :
 	// Z - first, allowed limits are (-PI,PI);
-	// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number 
+	// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
 	// used to prevent constraint from instability on poles;
 	// new position of X, allowed limits are (-PI,PI);
 	// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
 	// Build the frame in world coordinate system first
 	btVector3 zAxis = m_axis1.normalize();
 	btVector3 yAxis = m_axis2.normalize();
-	btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
+	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]);
+	frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
+								 xAxis[1], yAxis[1], zAxis[1],
+								 xAxis[2], yAxis[2], zAxis[2]);
 	frameInW.setOrigin(anchor);
 	// now get constraint frame in local coordinate systems
 	m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
@@ -58,30 +53,28 @@ btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB,
 	setLinearLowerLimit(btVector3(0., 0., 0.));
 	setLinearUpperLimit(btVector3(0., 0., 0.));
 	setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS));
-	setAngularUpperLimit(btVector3(0.f,  SIMD_HALF_PI - UNIV_EPS,  SIMD_PI - UNIV_EPS));
+	setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS));
 }
 
-void btUniversalConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
+void btUniversalConstraint::setAxis(const btVector3& axis1, const btVector3& axis2)
 {
-  m_axis1 = axis1;
-  m_axis2 = axis2;
+	m_axis1 = axis1;
+	m_axis2 = axis2;
 
 	btVector3 zAxis = axis1.normalized();
 	btVector3 yAxis = axis2.normalized();
-	btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
+	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]);
+	frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
+								 xAxis[1], yAxis[1], zAxis[1],
+								 xAxis[2], yAxis[2], zAxis[2]);
 	frameInW.setOrigin(m_anchor);
 
 	// now get constraint frame in local coordinate systems
 	m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
 	m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
 
-  calculateTransforms();
+	calculateTransforms();
 }
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
index 9e708410430..8c24d93a641 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
@@ -16,35 +16,32 @@ subject to the following restrictions:
 #ifndef BT_UNIVERSAL_CONSTRAINT_H
 #define BT_UNIVERSAL_CONSTRAINT_H
 
-
-
 #include "LinearMath/btVector3.h"
 #include "btTypedConstraint.h"
 #include "btGeneric6DofConstraint.h"
 
-
-
 /// Constraint similar to ODE Universal Joint
 /// has 2 rotatioonal degrees of freedom, similar to Euler rotations around Z (axis 1)
 /// and Y (axis 2)
-/// Description from ODE manual : 
-/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular. 
+/// Description from ODE manual :
+/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular.
 /// In other words, rotation of the two bodies about the direction perpendicular to the two axes will be equal."
 
-ATTRIBUTE_ALIGNED16(class) btUniversalConstraint : public btGeneric6DofConstraint
+ATTRIBUTE_ALIGNED16(class)
+btUniversalConstraint : public btGeneric6DofConstraint
 {
 protected:
-	btVector3	m_anchor;
-	btVector3	m_axis1;
-	btVector3	m_axis2;
+	btVector3 m_anchor;
+	btVector3 m_axis1;
+	btVector3 m_axis2;
+
 public:
-	
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	// constructor
 	// anchor, axis1 and axis2 are in world coordinate system
 	// axis1 must be orthogonal to axis2
-    btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2);
+	btUniversalConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2);
 	// access
 	const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
 	const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
@@ -56,10 +53,7 @@ public:
 	void setUpperLimit(btScalar ang1max, btScalar ang2max) { setAngularUpperLimit(btVector3(0.f, ang1max, ang2max)); }
 	void setLowerLimit(btScalar ang1min, btScalar ang2min) { setAngularLowerLimit(btVector3(0.f, ang1min, ang2min)); }
 
-	void setAxis( const btVector3& axis1, const btVector3& axis2);
+	void setAxis(const btVector3& axis1, const btVector3& axis2);
 };
 
-
-
-#endif // BT_UNIVERSAL_CONSTRAINT_H
-
+#endif  // BT_UNIVERSAL_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
deleted file mode 100644
index e1f69afe101..00000000000
--- a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
+++ /dev/null
@@ -1,469 +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/btConvexHullComputer.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/Gimpact/btTriangleShapeEx.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]);
-	btTriangleShapeEx 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]);
-	btTriangleShapeEx 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;	
-}
-
-// Convex hull
-plConvexHull plConvexHullCompute(float (*coords)[3], int count)
-{
-	btConvexHullComputer *computer = new btConvexHullComputer;
-	computer->compute(reinterpret_cast< float* >(coords),
-					  sizeof(*coords), count, 0, 0);
-	return reinterpret_cast<plConvexHull>(computer);
-}
-
-void plConvexHullDelete(plConvexHull hull)
-{
-	btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
-	delete computer;
-}
-
-int plConvexHullNumVertices(plConvexHull hull)
-{
-	btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
-	return computer->vertices.size();
-}
-
-int plConvexHullNumFaces(plConvexHull hull)
-{
-	btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
-	return computer->faces.size();
-}
-
-void plConvexHullGetVertex(plConvexHull hull, int n, float coords[3],
-						   int *original_index)
-{
-	btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
-	const btVector3 &v(computer->vertices[n]);
-	coords[0] = v[0];
-	coords[1] = v[1];
-	coords[2] = v[2];
-	(*original_index) = computer->original_vertex_index[n];
-}
-
-int plConvexHullGetFaceSize(plConvexHull hull, int n)
-{
-	btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
-	const btConvexHullComputer::Edge *e_orig, *e;
-	int count;
-
-	for (e_orig = &computer->edges[computer->faces[n]], e = e_orig, count = 0;
-		 count == 0 || e != e_orig;
-		 e = e->getNextEdgeOfFace(), count++);
-	return count;
-}
-
-void plConvexHullGetFaceVertices(plConvexHull hull, int n, int *vertices)
-{
-	btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
-	const btConvexHullComputer::Edge *e_orig, *e;
-	int count;
-
-	for (e_orig = &computer->edges[computer->faces[n]], e = e_orig, count = 0;
-		 count == 0 || e != e_orig;
-		 e = e->getNextEdgeOfFace(), count++)
-	{
-		vertices[count] = e->getTargetVertex();
-	}
-}
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h b/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h
index e1fea3a49c0..b5cac56cdca 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h
@@ -26,21 +26,16 @@ class btCollisionWorld;
 class btActionInterface
 {
 protected:
-
 	static btRigidBody& getFixedBody();
-	
-	
-public:
 
+public:
 	virtual ~btActionInterface()
 	{
 	}
 
-	virtual void updateAction( btCollisionWorld* collisionWorld, btScalar deltaTimeStep)=0;
+	virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTimeStep) = 0;
 
 	virtual void debugDraw(btIDebugDraw* debugDrawer) = 0;
-
 };
 
-#endif //_BT_ACTION_INTERFACE_H
-
+#endif  //_BT_ACTION_INTERFACE_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index 361a054ec69..fb15ae31eb5 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btDiscreteDynamicsWorld.h"
 
 //collision detection
@@ -38,11 +37,9 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
 
-
 #include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
 
-
 #include "BulletDynamics/Dynamics/btActionInterface.h"
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btMotionState.h"
@@ -56,57 +53,52 @@ int startHit=2;
 int firstHit=startHit;
 #endif
 
-SIMD_FORCE_INLINE	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
+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();
+	islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
 	return islandId;
-
 }
 
-
 class btSortConstraintOnIslandPredicate
 {
-	public:
-
-		bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const
-		{
-			int rIslandId0,lIslandId0;
-			rIslandId0 = btGetConstraintIslandId(rhs);
-			lIslandId0 = btGetConstraintIslandId(lhs);
-			return lIslandId0 < rIslandId0;
-		}
+public:
+	bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const
+	{
+		int rIslandId0, lIslandId0;
+		rIslandId0 = btGetConstraintIslandId(rhs);
+		lIslandId0 = btGetConstraintIslandId(lhs);
+		return lIslandId0 < rIslandId0;
+	}
 };
 
 struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
 {
-	btContactSolverInfo*	m_solverInfo;
-	btConstraintSolver*		m_solver;
-	btTypedConstraint**		m_sortedConstraints;
-	int						m_numConstraints;
-	btIDebugDraw*			m_debugDrawer;
-	btDispatcher*			m_dispatcher;
+	btContactSolverInfo* m_solverInfo;
+	btConstraintSolver* m_solver;
+	btTypedConstraint** m_sortedConstraints;
+	int m_numConstraints;
+	btIDebugDraw* m_debugDrawer;
+	btDispatcher* m_dispatcher;
 
 	btAlignedObjectArray<btCollisionObject*> m_bodies;
 	btAlignedObjectArray<btPersistentManifold*> m_manifolds;
 	btAlignedObjectArray<btTypedConstraint*> m_constraints;
 
-
 	InplaceSolverIslandCallback(
-		btConstraintSolver*	solver,
+		btConstraintSolver* solver,
 		btStackAlloc* stackAlloc,
 		btDispatcher* dispatcher)
-		:m_solverInfo(NULL),
-		m_solver(solver),
-		m_sortedConstraints(NULL),
-		m_numConstraints(0),
-		m_debugDrawer(NULL),
-		m_dispatcher(dispatcher)
+		: m_solverInfo(NULL),
+		  m_solver(solver),
+		  m_sortedConstraints(NULL),
+		  m_numConstraints(0),
+		  m_debugDrawer(NULL),
+		  m_dispatcher(dispatcher)
 	{
-
 	}
 
 	InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
@@ -116,34 +108,34 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 		return *this;
 	}
 
-	SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints,	int	numConstraints,	btIDebugDraw* debugDrawer)
+	SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btIDebugDraw* debugDrawer)
 	{
 		btAssert(solverInfo);
 		m_solverInfo = solverInfo;
 		m_sortedConstraints = sortedConstraints;
 		m_numConstraints = numConstraints;
 		m_debugDrawer = debugDrawer;
-		m_bodies.resize (0);
-		m_manifolds.resize (0);
-		m_constraints.resize (0);
+		m_bodies.resize(0);
+		m_manifolds.resize(0);
+		m_constraints.resize(0);
 	}
 
-
-	virtual	void	processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
+	virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId)
 	{
-		if (islandId<0)
+		if (islandId < 0)
 		{
 			///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
-			m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
-		} else
+			m_solver->solveGroup(bodies, numBodies, manifolds, numManifolds, &m_sortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
+		}
+		else
 		{
-				//also add all non-contact constraints/joints for this island
+			//also add all non-contact constraints/joints for this island
 			btTypedConstraint** startConstraint = 0;
 			int numCurConstraints = 0;
 			int i;
 
 			//find the first constraint for this island
-			for (i=0;i<m_numConstraints;i++)
+			for (i = 0; i < m_numConstraints; i++)
 			{
 				if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
 				{
@@ -152,7 +144,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 				}
 			}
 			//count the number of constraints in this island
-			for (;i<m_numConstraints;i++)
+			for (; i < m_numConstraints; i++)
 			{
 				if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
 				{
@@ -160,91 +152,87 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 				}
 			}
 
-			if (m_solverInfo->m_minimumSolverBatchSize<=1)
+			if (m_solverInfo->m_minimumSolverBatchSize <= 1)
 			{
-				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
-			} else
+				m_solver->solveGroup(bodies, numBodies, manifolds, numManifolds, startConstraint, numCurConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
+			}
+			else
 			{
-
-				for (i=0;i<numBodies;i++)
+				for (i = 0; i < numBodies; i++)
 					m_bodies.push_back(bodies[i]);
-				for (i=0;i<numManifolds;i++)
+				for (i = 0; i < numManifolds; i++)
 					m_manifolds.push_back(manifolds[i]);
-				for (i=0;i<numCurConstraints;i++)
+				for (i = 0; i < numCurConstraints; i++)
 					m_constraints.push_back(startConstraint[i]);
-				if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
+				if ((m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize)
 				{
 					processConstraints();
-				} else
+				}
+				else
 				{
 					//printf("deferred\n");
 				}
 			}
 		}
 	}
-	void	processConstraints()
+	void processConstraints()
 	{
+		btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
+		btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
+		btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
 
-		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_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);
 		m_constraints.resize(0);
-
 	}
-
 };
 
-
-
-btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
-:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
-m_sortedConstraints	(),
-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_latencyMotionStateInterpolation(true)
+btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+	: btDynamicsWorld(dispatcher, pairCache, collisionConfiguration),
+	  m_sortedConstraints(),
+	  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_latencyMotionStateInterpolation(true)
 
 {
 	if (!m_constraintSolver)
 	{
-		void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
+		void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver), 16);
 		m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;
 		m_ownsConstraintSolver = true;
-	} else
+	}
+	else
 	{
 		m_ownsConstraintSolver = false;
 	}
 
 	{
-		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16);
+		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager), 16);
 		m_islandManager = new (mem) btSimulationIslandManager();
 	}
 
 	m_ownsIslandManager = true;
 
 	{
-		void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback),16);
-		m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (m_constraintSolver, 0, dispatcher);
+		void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback), 16);
+		m_solverIslandCallback = new (mem) InplaceSolverIslandCallback(m_constraintSolver, 0, dispatcher);
 	}
 }
 
-
 btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
 {
 	//only delete it when we created it
 	if (m_ownsIslandManager)
 	{
 		m_islandManager->~btSimulationIslandManager();
-		btAlignedFree( m_islandManager);
+		btAlignedFree(m_islandManager);
 	}
 	if (m_solverIslandCallback)
 	{
@@ -253,18 +241,17 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
 	}
 	if (m_ownsConstraintSolver)
 	{
-
 		m_constraintSolver->~btConstraintSolver();
 		btAlignedFree(m_constraintSolver);
 	}
 }
 
-void	btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
+void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
 {
-///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
-///to switch status _after_ adding kinematic objects to the world
-///fix it for Bullet 3.x release
-	for (int i=0;i<m_collisionObjects.size();i++)
+	///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
+	///to switch status _after_ adding kinematic objects to the world
+	///fix it for Bullet 3.x release
+	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -277,10 +264,9 @@ void	btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
 			}
 		}
 	}
-
 }
 
-void	btDiscreteDynamicsWorld::debugDrawWorld()
+void btDiscreteDynamicsWorld::debugDrawWorld()
 {
 	BT_PROFILE("debugDrawWorld");
 
@@ -290,43 +276,40 @@ void	btDiscreteDynamicsWorld::debugDrawWorld()
 	if (getDebugDrawer())
 	{
 		int mode = getDebugDrawer()->getDebugMode();
-		if(mode  & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
+		if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
 		{
 			drawConstraints = true;
 		}
 	}
-	if(drawConstraints)
+	if (drawConstraints)
 	{
-		for(int i = getNumConstraints()-1; i>=0 ;i--)
+		for (int i = getNumConstraints() - 1; i >= 0; i--)
 		{
 			btTypedConstraint* constraint = getConstraint(i);
 			debugDrawConstraint(constraint);
 		}
 	}
 
-
-
-    if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals)))
+	if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals)))
 	{
 		int i;
 
 		if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
 		{
-			for (i=0;i<m_actions.size();i++)
+			for (i = 0; i < m_actions.size(); i++)
 			{
 				m_actions[i]->debugDraw(m_debugDrawer);
 			}
 		}
 	}
-    if (getDebugDrawer())
-        getDebugDrawer()->flushLines();
-
+	if (getDebugDrawer())
+		getDebugDrawer()->flushLines();
 }
 
-void	btDiscreteDynamicsWorld::clearForces()
+void btDiscreteDynamicsWorld::clearForces()
 {
 	///@todo: iterate over awake simulation islands!
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
 	{
 		btRigidBody* body = m_nonStaticRigidBodies[i];
 		//need to check if next line is ok
@@ -336,10 +319,10 @@ void	btDiscreteDynamicsWorld::clearForces()
 }
 
 ///apply gravity, call this once per timestep
-void	btDiscreteDynamicsWorld::applyGravity()
+void btDiscreteDynamicsWorld::applyGravity()
 {
 	///@todo: iterate over awake simulation islands!
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
 	{
 		btRigidBody* body = m_nonStaticRigidBodies[i];
 		if (body->isActive())
@@ -349,8 +332,7 @@ void	btDiscreteDynamicsWorld::applyGravity()
 	}
 }
 
-
-void	btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
+void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
 {
 	btAssert(body);
 
@@ -363,32 +345,32 @@ void	btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
 		{
 			btTransform interpolatedTransform;
 			btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
-				body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),
-				(m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(),
-				interpolatedTransform);
+												body->getInterpolationLinearVelocity(), body->getInterpolationAngularVelocity(),
+												(m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime * body->getHitFraction(),
+												interpolatedTransform);
 			body->getMotionState()->setWorldTransform(interpolatedTransform);
 		}
 	}
 }
 
-
-void	btDiscreteDynamicsWorld::synchronizeMotionStates()
+void btDiscreteDynamicsWorld::synchronizeMotionStates()
 {
-	BT_PROFILE("synchronizeMotionStates");
+	//	BT_PROFILE("synchronizeMotionStates");
 	if (m_synchronizeAllMotionStates)
 	{
 		//iterate  over all collision objects
-		for ( int i=0;i<m_collisionObjects.size();i++)
+		for (int i = 0; i < m_collisionObjects.size(); i++)
 		{
 			btCollisionObject* colObj = m_collisionObjects[i];
 			btRigidBody* body = btRigidBody::upcast(colObj);
 			if (body)
 				synchronizeSingleMotionState(body);
 		}
-	} else
+	}
+	else
 	{
 		//iterate over all active rigid bodies
-		for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+		for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
 		{
 			btRigidBody* body = m_nonStaticRigidBodies[i];
 			if (body->isActive())
@@ -397,13 +379,10 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 	}
 }
 
-
-int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
+int btDiscreteDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
 {
 	startProfiling(timeStep);
 
-	BT_PROFILE("stepSimulation");
-
 	int numSimulationSubSteps = 0;
 
 	if (maxSubSteps)
@@ -413,10 +392,11 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 		m_localTime += timeStep;
 		if (m_localTime >= fixedTimeStep)
 		{
-			numSimulationSubSteps = int( m_localTime / fixedTimeStep);
+			numSimulationSubSteps = int(m_localTime / fixedTimeStep);
 			m_localTime -= numSimulationSubSteps * fixedTimeStep;
 		}
-	} else
+	}
+	else
 	{
 		//variable timestep
 		fixedTimeStep = timeStep;
@@ -426,7 +406,8 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 		{
 			numSimulationSubSteps = 0;
 			maxSubSteps = 0;
-		} else
+		}
+		else
 		{
 			numSimulationSubSteps = 1;
 			maxSubSteps = 1;
@@ -436,28 +417,25 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 	//process some debugging flags
 	if (getDebugDrawer())
 	{
-		btIDebugDraw* debugDrawer = getDebugDrawer ();
+		btIDebugDraw* debugDrawer = getDebugDrawer();
 		gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
 	}
 	if (numSimulationSubSteps)
 	{
-
 		//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
-		int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
+		int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
 
-		saveKinematicState(fixedTimeStep*clampedSimulationSteps);
+		saveKinematicState(fixedTimeStep * clampedSimulationSteps);
 
 		applyGravity();
 
-
-
-		for (int i=0;i<clampedSimulationSteps;i++)
+		for (int i = 0; i < clampedSimulationSteps; i++)
 		{
 			internalSingleStepSimulation(fixedTimeStep);
 			synchronizeMotionStates();
 		}
-
-	} else
+	}
+	else
 	{
 		synchronizeMotionStates();
 	}
@@ -466,17 +444,17 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 
 #ifndef BT_NO_PROFILE
 	CProfileManager::Increment_Frame_Counter();
-#endif //BT_NO_PROFILE
+#endif  //BT_NO_PROFILE
 
 	return numSimulationSubSteps;
 }
 
-void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
+void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
-
 	BT_PROFILE("internalSingleStepSimulation");
 
-	if(0 != m_internalPreTickCallback) {
+	if (0 != m_internalPreTickCallback)
+	{
 		(*m_internalPreTickCallback)(this, timeStep);
 	}
 
@@ -489,19 +467,15 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 	dispatchInfo.m_stepCount = 0;
 	dispatchInfo.m_debugDraw = getDebugDrawer();
 
-
-    createPredictiveContacts(timeStep);
+	createPredictiveContacts(timeStep);
 
 	///perform collision detection
 	performDiscreteCollisionDetection();
 
 	calculateSimulationIslands();
 
-
 	getSolverInfo().m_timeStep = timeStep;
 
-
-
 	///solve contact and other joint constraints
 	solveConstraints(getSolverInfo());
 
@@ -514,37 +488,38 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 	///update vehicle simulation
 	updateActions(timeStep);
 
-	updateActivationState( timeStep );
+	updateActivationState(timeStep);
 
-	if(0 != m_internalTickCallback) {
+	if (0 != m_internalTickCallback)
+	{
 		(*m_internalTickCallback)(this, timeStep);
 	}
 }
 
-void	btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
+void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
 {
 	m_gravity = gravity;
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
 	{
 		btRigidBody* body = m_nonStaticRigidBodies[i];
-		if (body->isActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
+		if (body->isActive() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY))
 		{
 			body->setGravity(gravity);
 		}
 	}
 }
 
-btVector3 btDiscreteDynamicsWorld::getGravity () const
+btVector3 btDiscreteDynamicsWorld::getGravity() const
 {
 	return m_gravity;
 }
 
-void	btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
+void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask)
 {
-	btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask);
+	btCollisionWorld::addCollisionObject(collisionObject, collisionFilterGroup, collisionFilterMask);
 }
 
-void	btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
 {
 	btRigidBody* body = btRigidBody::upcast(collisionObject);
 	if (body)
@@ -553,16 +528,15 @@ void	btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collision
 		btCollisionWorld::removeCollisionObject(collisionObject);
 }
 
-void	btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
+void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
 {
 	m_nonStaticRigidBodies.remove(body);
 	btCollisionWorld::removeCollisionObject(body);
 }
 
-
-void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
+void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
 {
-	if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
+	if (!body->isStaticOrKinematicObject() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY))
 	{
 		body->setGravity(m_gravity);
 	}
@@ -572,22 +546,23 @@ void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
 		if (!body->isStaticObject())
 		{
 			m_nonStaticRigidBodies.push_back(body);
-		} else
+		}
+		else
 		{
 			body->setActivationState(ISLAND_SLEEPING);
 		}
 
 		bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
-		short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
-		short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+		int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
+		int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
 
-		addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
+		addCollisionObject(body, collisionFilterGroup, collisionFilterMask);
 	}
 }
 
-void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
+void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask)
 {
-	if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
+	if (!body->isStaticOrKinematicObject() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY))
 	{
 		body->setGravity(m_gravity);
 	}
@@ -598,31 +573,29 @@ void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short
 		{
 			m_nonStaticRigidBodies.push_back(body);
 		}
-		 else
+		else
 		{
 			body->setActivationState(ISLAND_SLEEPING);
 		}
-		addCollisionObject(body,group,mask);
+		addCollisionObject(body, group, mask);
 	}
 }
 
-
-void	btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
+void btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
 {
 	BT_PROFILE("updateActions");
 
-	for ( int i=0;i<m_actions.size();i++)
+	for (int i = 0; i < m_actions.size(); i++)
 	{
-		m_actions[i]->updateAction( this, timeStep);
+		m_actions[i]->updateAction(this, timeStep);
 	}
 }
 
-
-void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
+void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 {
 	BT_PROFILE("updateActivationState");
 
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
 	{
 		btRigidBody* body = m_nonStaticRigidBodies[i];
 		if (body)
@@ -634,32 +607,33 @@ void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 				if (body->isStaticOrKinematicObject())
 				{
 					body->setActivationState(ISLAND_SLEEPING);
-				} else
+				}
+				else
 				{
 					if (body->getActivationState() == ACTIVE_TAG)
-						body->setActivationState( WANTS_DEACTIVATION );
+						body->setActivationState(WANTS_DEACTIVATION);
 					if (body->getActivationState() == ISLAND_SLEEPING)
 					{
-						body->setAngularVelocity(btVector3(0,0,0));
-						body->setLinearVelocity(btVector3(0,0,0));
+						body->setAngularVelocity(btVector3(0, 0, 0));
+						body->setLinearVelocity(btVector3(0, 0, 0));
 					}
-
 				}
-			} else
+			}
+			else
 			{
 				if (body->getActivationState() != DISABLE_DEACTIVATION)
-					body->setActivationState( ACTIVE_TAG );
+					body->setActivationState(ACTIVE_TAG);
 			}
 		}
 	}
 }
 
-void	btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
+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());
-    
+	//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);
@@ -667,105 +641,98 @@ void	btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool d
 	}
 }
 
-void	btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
+void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
 {
 	m_constraints.remove(constraint);
 	constraint->getRigidBodyA().removeConstraintRef(constraint);
 	constraint->getRigidBodyB().removeConstraintRef(constraint);
 }
 
-void	btDiscreteDynamicsWorld::addAction(btActionInterface* action)
+void btDiscreteDynamicsWorld::addAction(btActionInterface* action)
 {
 	m_actions.push_back(action);
 }
 
-void	btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
+void btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
 {
 	m_actions.remove(action);
 }
 
-
-void	btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
+void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
 {
 	addAction(vehicle);
 }
 
-void	btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
+void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
 {
 	removeAction(vehicle);
 }
 
-void	btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
+void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
 {
 	addAction(character);
 }
 
-void	btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
+void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
 {
 	removeAction(character);
 }
 
-
-
-
-void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
+void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
 	BT_PROFILE("solveConstraints");
 
-	m_sortedConstraints.resize( m_constraints.size());
+	m_sortedConstraints.resize(m_constraints.size());
 	int i;
-	for (i=0;i<getNumConstraints();i++)
+	for (i = 0; i < getNumConstraints(); i++)
 	{
 		m_sortedConstraints[i] = m_constraints[i];
 	}
 
-//	btAssert(0);
-
-
+	//	btAssert(0);
 
 	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
 
 	btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
 
-	m_solverIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer());
+	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);
+	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverIslandCallback);
 
 	m_solverIslandCallback->processConstraints();
 
 	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
 }
 
-
-void	btDiscreteDynamicsWorld::calculateSimulationIslands()
+void btDiscreteDynamicsWorld::calculateSimulationIslands()
 {
 	BT_PROFILE("calculateSimulationIslands");
 
-	getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
+	getSimulationIslandManager()->updateActivationState(getCollisionWorld(), getCollisionWorld()->getDispatcher());
 
-    {
-        //merge islands based on speculative contact manifolds too
-        for (int i=0;i<this->m_predictiveManifolds.size();i++)
-        {
-            btPersistentManifold* manifold = m_predictiveManifolds[i];
+	{
+		//merge islands based on speculative contact manifolds too
+		for (int i = 0; i < this->m_predictiveManifolds.size(); i++)
+		{
+			btPersistentManifold* manifold = m_predictiveManifolds[i];
 
-            const btCollisionObject* colObj0 = manifold->getBody0();
-            const btCollisionObject* colObj1 = manifold->getBody1();
+			const btCollisionObject* colObj0 = manifold->getBody0();
+			const btCollisionObject* colObj1 = manifold->getBody1();
 
-            if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
-                ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
-            {
-				getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
-            }
-        }
-    }
+			if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
+				((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
+			{
+				getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
+			}
+		}
+	}
 
 	{
 		int i;
 		int numConstraints = int(m_constraints.size());
-		for (i=0;i< numConstraints ; i++ )
+		for (i = 0; i < numConstraints; i++)
 		{
 			btTypedConstraint* constraint = m_constraints[i];
 			if (constraint->isEnabled())
@@ -776,7 +743,7 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 				if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
 					((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
 				{
-					getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
+					getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
 				}
 			}
 		}
@@ -784,51 +751,44 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 
 	//Store the island id in each body
 	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
-
-
 }
 
-
-
-
 class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
 {
 public:
-
 	btCollisionObject* m_me;
 	btScalar m_allowedPenetration;
 	btOverlappingPairCache* m_pairCache;
 	btDispatcher* m_dispatcher;
 
 public:
-	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
-	  btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
-		m_me(me),
-		m_allowedPenetration(0.0f),
-		m_pairCache(pairCache),
-		m_dispatcher(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),
+																																										   m_pairCache(pairCache),
+																																										   m_dispatcher(dispatcher)
 	{
 	}
 
-	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
+	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace)
 	{
 		if (convexResult.m_hitCollisionObject == m_me)
 			return 1.0f;
 
 		//ignore result if there is no contact response
-		if(!convexResult.m_hitCollisionObject->hasContactResponse())
+		if (!convexResult.m_hitCollisionObject->hasContactResponse())
 			return 1.0f;
 
-		btVector3 linVelA,linVelB;
-		linVelA = m_convexToWorld-m_convexFromWorld;
-		linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin();
+		btVector3 linVelA, linVelB;
+		linVelA = m_convexToWorld - m_convexFromWorld;
+		linVelB = btVector3(0, 0, 0);  //toB.getOrigin()-fromB.getOrigin();
 
-		btVector3 relativeVelocity = (linVelA-linVelB);
+		btVector3 relativeVelocity = (linVelA - linVelB);
 		//don't report time of impact for motion away from the contact normal (or causes minor penetration)
-		if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration)
+		if (convexResult.m_hitNormalLocal.dot(relativeVelocity) >= -m_allowedPenetration)
 			return 1.f;
 
-		return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
+		return ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
 	}
 
 	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
@@ -840,11 +800,22 @@ public:
 		///don't do CCD when the collision filters are not matching
 		if (!ClosestConvexResultCallback::needsCollision(proxy0))
 			return false;
+		if (m_pairCache->getOverlapFilterCallback()) {
+			btBroadphaseProxy* proxy1 = m_me->getBroadphaseHandle();
+			bool collides = m_pairCache->needsBroadphaseCollision(proxy0, proxy1);
+			if (!collides)
+			{
+				return false;
+			}
+		}
 
-		btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
+		btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;
+
+		if (!m_dispatcher->needsCollision(m_me, otherObj))
+			return false;
 
 		//call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
-		if (m_dispatcher->needsResponse(m_me,otherObj))
+		if (m_dispatcher->needsResponse(m_me, otherObj))
 		{
 #if 0
 			///don't do CCD when there are already contact points (touching contact/penetration)
@@ -870,41 +841,24 @@ public:
 
 		return false;
 	}
-
-
 };
 
 ///internal debugging variable. this value shouldn't be too high
-int gNumClampedCcdMotions=0;
+int gNumClampedCcdMotions = 0;
 
-
-void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
+void btDiscreteDynamicsWorld::createPredictiveContactsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep)
 {
-	BT_PROFILE("createPredictiveContacts");
-
-	{
-		BT_PROFILE("release predictive contact manifolds");
-
-		for (int i=0;i<m_predictiveManifolds.size();i++)
-		{
-			btPersistentManifold* manifold = m_predictiveManifolds[i];
-			this->m_dispatcher1->releaseManifold(manifold);
-		}
-		m_predictiveManifolds.clear();
-	}
-
 	btTransform predictedTrans;
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for (int i = 0; i < numBodies; i++)
 	{
-		btRigidBody* body = m_nonStaticRigidBodies[i];
+		btRigidBody* body = bodies[i];
 		body->setHitFraction(1.f);
 
 		if (body->isActive() && (!body->isStaticOrKinematicObject()))
 		{
-
 			body->predictIntegratedTransform(timeStep, predictedTrans);
 
-			btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
+			btScalar squareMotion = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()).length2();
 
 			if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
 			{
@@ -916,81 +870,97 @@ void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 					class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
 					{
 					public:
-
-						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
-						  btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
+						StaticOnlyCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me, fromA, toA, pairCache, dispatcher)
 						{
 						}
 
-					  	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+						virtual bool needsCollision(btBroadphaseProxy* proxy0) const
 						{
-							btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
+							btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;
 							if (!otherObj->isStaticOrKinematicObject())
 								return false;
 							return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
 						}
 					};
 
-					StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
+					StaticOnlyCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
 #else
-					btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
+					btClosestNotMeConvexResultCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
 #endif
 					//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
-					btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
-					sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;
+					btSphereShape tmpSphere(body->getCcdSweptSphereRadius());  //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+					sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration;
 
 					sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
-					sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
+					sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
 					btTransform modifiedPredictedTrans = predictedTrans;
 					modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
 
-					convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
+					convexSweepTest(&tmpSphere, body->getWorldTransform(), modifiedPredictedTrans, sweepResults);
 					if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
 					{
-
-						btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction;
+						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);
+						btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body, sweepResults.m_hitCollisionObject);
+						btMutexLock(&m_predictiveManifoldsMutex);
 						m_predictiveManifolds.push_back(manifold);
+						btMutexUnlock(&m_predictiveManifoldsMutex);
 
-						btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec;
-						btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB;
+						btVector3 worldPointB = body->getWorldTransform().getOrigin() + distVec;
+						btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse() * worldPointB;
 
-						btManifoldPoint newPoint(btVector3(0,0,0), localPointB,sweepResults.m_hitNormalWorld,distance);
+						btManifoldPoint newPoint(btVector3(0, 0, 0), localPointB, sweepResults.m_hitNormalWorld, distance);
 
 						bool isPredictive = true;
 						int index = manifold->addManifoldPoint(newPoint, isPredictive);
 						btManifoldPoint& pt = manifold->getContactPoint(index);
 						pt.m_combinedRestitution = 0;
-						pt.m_combinedFriction = btManifoldResult::calculateCombinedFriction(body,sweepResults.m_hitCollisionObject);
+						pt.m_combinedFriction = gCalculateCombinedFrictionCallback(body, sweepResults.m_hitCollisionObject);
 						pt.m_positionWorldOnA = body->getWorldTransform().getOrigin();
 						pt.m_positionWorldOnB = worldPointB;
-
 					}
 				}
 			}
 		}
 	}
 }
-void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
+
+void btDiscreteDynamicsWorld::releasePredictiveContacts()
+{
+	BT_PROFILE("release predictive contact manifolds");
+
+	for (int i = 0; i < m_predictiveManifolds.size(); i++)
+	{
+		btPersistentManifold* manifold = m_predictiveManifolds[i];
+		this->m_dispatcher1->releaseManifold(manifold);
+	}
+	m_predictiveManifolds.clear();
+}
+
+void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
+{
+	BT_PROFILE("createPredictiveContacts");
+	releasePredictiveContacts();
+	if (m_nonStaticRigidBodies.size() > 0)
+	{
+		createPredictiveContactsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep);
+	}
+}
+
+void btDiscreteDynamicsWorld::integrateTransformsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep)
 {
-	BT_PROFILE("integrateTransforms");
 	btTransform predictedTrans;
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for (int i = 0; i < numBodies; i++)
 	{
-		btRigidBody* body = m_nonStaticRigidBodies[i];
+		btRigidBody* body = bodies[i];
 		body->setHitFraction(1.f);
 
 		if (body->isActive() && (!body->isStaticOrKinematicObject()))
 		{
-
 			body->predictIntegratedTransform(timeStep, predictedTrans);
 
-			btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
-
-
+			btScalar squareMotion = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()).length2();
 
 			if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
 			{
@@ -1002,43 +972,40 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 					class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
 					{
 					public:
-
-						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
-						  btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
+						StaticOnlyCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me, fromA, toA, pairCache, dispatcher)
 						{
 						}
 
-					  	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+						virtual bool needsCollision(btBroadphaseProxy* proxy0) const
 						{
-							btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
+							btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;
 							if (!otherObj->isStaticOrKinematicObject())
 								return false;
 							return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
 						}
 					};
 
-					StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
+					StaticOnlyCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
 #else
-					btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
+					btClosestNotMeConvexResultCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
 #endif
 					//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
-					btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
-					sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;
+					btSphereShape tmpSphere(body->getCcdSweptSphereRadius());  //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+					sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration;
 
 					sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
-					sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
+					sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
 					btTransform modifiedPredictedTrans = predictedTrans;
 					modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
 
-					convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
+					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);
+						body->predictIntegratedTransform(timeStep * body->getHitFraction(), predictedTrans);
 						body->setHitFraction(0.f);
-						body->proceedToTransform( predictedTrans);
+						body->proceedToTransform(predictedTrans);
 
 #if 0
 						btVector3 linVel = body->getLinearVelocity();
@@ -1065,40 +1032,45 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 						//btScalar depth = 0.f;
 						//appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
 
-
 #endif
 
-        				continue;
+						continue;
 					}
 				}
 			}
 
-
-			body->proceedToTransform( predictedTrans);
-
+			body->proceedToTransform(predictedTrans);
 		}
+	}
+}
 
+void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
+{
+	BT_PROFILE("integrateTransforms");
+	if (m_nonStaticRigidBodies.size() > 0)
+	{
+		integrateTransformsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep);
 	}
 
 	///this should probably be switched on by default, but it is not well tested yet
 	if (m_applySpeculativeContactRestitution)
 	{
 		BT_PROFILE("apply speculative contact restitution");
-		for (int i=0;i<m_predictiveManifolds.size();i++)
+		for (int i = 0; i < m_predictiveManifolds.size(); i++)
 		{
 			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++)
+			for (int p = 0; p < manifold->getNumContacts(); p++)
 			{
 				const btManifoldPoint& pt = manifold->getContactPoint(p);
-				btScalar combinedRestitution = btManifoldResult::calculateCombinedRestitution(body0, body1);
+				btScalar combinedRestitution = gCalculateCombinedRestitutionCallback(body0, body1);
 
-				if (combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
+				if (combinedRestitution > 0 && pt.m_appliedImpulse != 0.f)
 				//if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
 				{
-					btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution;
+					btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse * combinedRestitution;
 
 					const btVector3& pos1 = pt.getPositionWorldOnA();
 					const btVector3& pos2 = pt.getPositionWorldOnB();
@@ -1107,25 +1079,19 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 					btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin();
 
 					if (body0)
-						body0->applyImpulse(imp,rel_pos0);
+						body0->applyImpulse(imp, rel_pos0);
 					if (body1)
-						body1->applyImpulse(-imp,rel_pos1);
+						body1->applyImpulse(-imp, rel_pos1);
 				}
 			}
 		}
 	}
-
 }
 
-
-
-
-
-
-void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
 	BT_PROFILE("predictUnconstraintMotion");
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
 	{
 		btRigidBody* body = m_nonStaticRigidBodies[i];
 		if (!body->isStaticOrKinematicObject())
@@ -1134,179 +1100,171 @@ void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 
 			body->applyDamping(timeStep);
 
-			body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
+			body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform());
 		}
 	}
 }
 
-
-void	btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
+void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
 {
 	(void)timeStep;
 
 #ifndef BT_NO_PROFILE
 	CProfileManager::Reset();
-#endif //BT_NO_PROFILE
-
+#endif  //BT_NO_PROFILE
 }
 
-
-
-
-
-
 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 {
 	bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
 	bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
 	btScalar dbgDrawSize = constraint->getDbgDrawSize();
-	if(dbgDrawSize <= btScalar(0.f))
+	if (dbgDrawSize <= btScalar(0.f))
 	{
 		return;
 	}
 
-	switch(constraint->getConstraintType())
+	switch (constraint->getConstraintType())
 	{
 		case POINT2POINT_CONSTRAINT_TYPE:
+		{
+			btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
+			btTransform tr;
+			tr.setIdentity();
+			btVector3 pivot = p2pC->getPivotInA();
+			pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
+			tr.setOrigin(pivot);
+			getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			// that ideally should draw the same frame
+			pivot = p2pC->getPivotInB();
+			pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
+			tr.setOrigin(pivot);
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+		}
+		break;
+		case HINGE_CONSTRAINT_TYPE:
+		{
+			btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
+			btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			btScalar minAng = pHinge->getLowerLimit();
+			btScalar maxAng = pHinge->getUpperLimit();
+			if (minAng == maxAng)
 			{
-				btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
-				btTransform tr;
-				tr.setIdentity();
-				btVector3 pivot = p2pC->getPivotInA();
-				pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
-				tr.setOrigin(pivot);
-				getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				// that ideally should draw the same frame
-				pivot = p2pC->getPivotInB();
-				pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
-				tr.setOrigin(pivot);
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				break;
 			}
-			break;
-		case HINGE_CONSTRAINT_TYPE:
+			bool drawSect = true;
+			if (!pHinge->hasLimit())
 			{
-				btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
-				btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				btScalar minAng = pHinge->getLowerLimit();
-				btScalar maxAng = pHinge->getUpperLimit();
-				if(minAng == maxAng)
-				{
-					break;
-				}
-				bool drawSect = true;
-				if(!pHinge->hasLimit())
-				{
-					minAng = btScalar(0.f);
-					maxAng = SIMD_2_PI;
-					drawSect = false;
-				}
-				if(drawLimits)
-				{
-					btVector3& center = tr.getOrigin();
-					btVector3 normal = tr.getBasis().getColumn(2);
-					btVector3 axis = tr.getBasis().getColumn(0);
-					getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
-				}
+				minAng = btScalar(0.f);
+				maxAng = SIMD_2_PI;
+				drawSect = false;
 			}
-			break;
+			if (drawLimits)
+			{
+				btVector3& center = tr.getOrigin();
+				btVector3 normal = tr.getBasis().getColumn(2);
+				btVector3 axis = tr.getBasis().getColumn(0);
+				getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0, 0, 0), drawSect);
+			}
+		}
+		break;
 		case CONETWIST_CONSTRAINT_TYPE:
+		{
+			btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
+			btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			if (drawLimits)
 			{
-				btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
-				btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				if(drawLimits)
+				//const btScalar length = btScalar(5);
+				const btScalar length = dbgDrawSize;
+				static int nSegments = 8 * 4;
+				btScalar fAngleInRadians = btScalar(2. * 3.1415926) * (btScalar)(nSegments - 1) / btScalar(nSegments);
+				btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
+				pPrev = tr * pPrev;
+				for (int i = 0; i < nSegments; i++)
 				{
-					//const btScalar length = btScalar(5);
-					const btScalar length = dbgDrawSize;
-					static int nSegments = 8*4;
-					btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments);
-					btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
-					pPrev = tr * pPrev;
-					for (int i=0; i<nSegments; i++)
-					{
-						fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);
-						btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
-						pCur = tr * pCur;
-						getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));
+					fAngleInRadians = btScalar(2. * 3.1415926) * (btScalar)i / btScalar(nSegments);
+					btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
+					pCur = tr * pCur;
+					getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0, 0, 0));
 
-						if (i%(nSegments/8) == 0)
-							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));
-					if(useFrameB)
-					{
-						tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
-					}
-					else
-					{
-						tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
-					}
-					btVector3 pivot = tr.getOrigin();
-					btVector3 normal = tr.getBasis().getColumn(0);
-					btVector3 axis1 = tr.getBasis().getColumn(1);
-					getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);
+					if (i % (nSegments / 8) == 0)
+						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));
+				if (useFrameB)
+				{
+					tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
+				}
+				else
+				{
+					tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
+				}
+				btVector3 pivot = tr.getOrigin();
+				btVector3 normal = tr.getBasis().getColumn(0);
+				btVector3 axis1 = tr.getBasis().getColumn(1);
+				getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, btVector3(0, 0, 0), true);
 			}
-			break;
+		}
+		break;
 		case D6_SPRING_CONSTRAINT_TYPE:
 		case D6_CONSTRAINT_TYPE:
+		{
+			btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
+			btTransform tr = p6DOF->getCalculatedTransformA();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			tr = p6DOF->getCalculatedTransformB();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			if (drawLimits)
 			{
-				btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
-				btTransform tr = p6DOF->getCalculatedTransformA();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				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();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				if(drawLimits)
+				btVector3 normal = -tr.getBasis().getColumn(0);
+				btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
+				btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
+				if (minFi > maxFi)
 				{
-					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));
+					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;
+		}
+		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:
 		{
@@ -1324,9 +1282,12 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 					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));
+					if (minTh <= maxTh)
+					{
+						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);
@@ -1335,9 +1296,9 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 					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];
+					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;
@@ -1359,42 +1320,38 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 			break;
 		}
 		case SLIDER_CONSTRAINT_TYPE:
+		{
+			btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
+			btTransform tr = pSlider->getCalculatedTransformA();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			tr = pSlider->getCalculatedTransformB();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			if (drawLimits)
 			{
-				btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
-				btTransform tr = pSlider->getCalculatedTransformA();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				tr = pSlider->getCalculatedTransformB();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				if(drawLimits)
-				{
-					btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();
-					btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
-					btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
-					getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
-					btVector3 normal = tr.getBasis().getColumn(0);
-					btVector3 axis = tr.getBasis().getColumn(1);
-					btScalar a_min = pSlider->getLowerAngLimit();
-					btScalar a_max = pSlider->getUpperAngLimit();
-					const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
-					getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
-				}
+				btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();
+				btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
+				btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
+				getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
+				btVector3 normal = tr.getBasis().getColumn(0);
+				btVector3 axis = tr.getBasis().getColumn(1);
+				btScalar a_min = pSlider->getLowerAngLimit();
+				btScalar a_max = pSlider->getUpperAngLimit();
+				const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
+				getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0, 0, 0), true);
 			}
-			break;
-		default :
+		}
+		break;
+		default:
 			break;
 	}
 	return;
 }
 
-
-
-
-
-void	btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
+void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 {
 	if (m_ownsConstraintSolver)
 	{
-		btAlignedFree( m_constraintSolver);
+		btAlignedFree(m_constraintSolver);
 	}
 	m_ownsConstraintSolver = false;
 	m_constraintSolver = solver;
@@ -1406,8 +1363,7 @@ btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
 	return m_constraintSolver;
 }
 
-
-int		btDiscreteDynamicsWorld::getNumConstraints() const
+int btDiscreteDynamicsWorld::getNumConstraints() const
 {
 	return int(m_constraints.size());
 }
@@ -1420,90 +1376,85 @@ const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
 	return m_constraints[index];
 }
 
-
-
-void	btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer)
+void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer)
 {
 	int i;
 	//serialize all collision objects
-	for (i=0;i<m_collisionObjects.size();i++)
+	for (i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
 		{
 			int len = colObj->calculateSerializeBufferSize();
-			btChunk* chunk = serializer->allocate(len,1);
+			btChunk* chunk = serializer->allocate(len, 1);
 			const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
-			serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj);
+			serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, colObj);
 		}
 	}
 
-	for (i=0;i<m_constraints.size();i++)
+	for (i = 0; i < m_constraints.size(); i++)
 	{
 		btTypedConstraint* constraint = m_constraints[i];
 		int size = constraint->calculateSerializeBufferSize();
-		btChunk* chunk = serializer->allocate(size,1);
-		const char* structType = constraint->serialize(chunk->m_oldPtr,serializer);
-		serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint);
+		btChunk* chunk = serializer->allocate(size, 1);
+		const char* structType = constraint->serialize(chunk->m_oldPtr, serializer);
+		serializer->finalizeChunk(chunk, structType, BT_CONSTRAINT_CODE, constraint);
 	}
 }
 
-
-
-
-void	btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serializer)
+void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serializer)
 {
 #ifdef BT_USE_DOUBLE_PRECISION
-		int len = sizeof(btDynamicsWorldDoubleData);
-		btChunk* chunk = serializer->allocate(len,1);
-		btDynamicsWorldDoubleData* worldInfo = (btDynamicsWorldDoubleData*)chunk->m_oldPtr;
-#else//BT_USE_DOUBLE_PRECISION
-		int len = sizeof(btDynamicsWorldFloatData);
-		btChunk* chunk = serializer->allocate(len,1);
-		btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr;
-#endif//BT_USE_DOUBLE_PRECISION
-
-		memset(worldInfo ,0x00,len);
-
-		m_gravity.serialize(worldInfo->m_gravity);
-		worldInfo->m_solverInfo.m_tau = getSolverInfo().m_tau;
-		worldInfo->m_solverInfo.m_damping = getSolverInfo().m_damping;
-		worldInfo->m_solverInfo.m_friction = getSolverInfo().m_friction;
-		worldInfo->m_solverInfo.m_timeStep = getSolverInfo().m_timeStep;
-
-		worldInfo->m_solverInfo.m_restitution = getSolverInfo().m_restitution;
-		worldInfo->m_solverInfo.m_maxErrorReduction = getSolverInfo().m_maxErrorReduction;
-		worldInfo->m_solverInfo.m_sor = getSolverInfo().m_sor;
-		worldInfo->m_solverInfo.m_erp = getSolverInfo().m_erp;
-
-		worldInfo->m_solverInfo.m_erp2 = getSolverInfo().m_erp2;
-		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;
-
+	int len = sizeof(btDynamicsWorldDoubleData);
+	btChunk* chunk = serializer->allocate(len, 1);
+	btDynamicsWorldDoubleData* worldInfo = (btDynamicsWorldDoubleData*)chunk->m_oldPtr;
+#else   //BT_USE_DOUBLE_PRECISION
+	int len = sizeof(btDynamicsWorldFloatData);
+	btChunk* chunk = serializer->allocate(len, 1);
+	btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr;
+#endif  //BT_USE_DOUBLE_PRECISION
+
+	memset(worldInfo, 0x00, len);
+
+	m_gravity.serialize(worldInfo->m_gravity);
+	worldInfo->m_solverInfo.m_tau = getSolverInfo().m_tau;
+	worldInfo->m_solverInfo.m_damping = getSolverInfo().m_damping;
+	worldInfo->m_solverInfo.m_friction = getSolverInfo().m_friction;
+	worldInfo->m_solverInfo.m_timeStep = getSolverInfo().m_timeStep;
+
+	worldInfo->m_solverInfo.m_restitution = getSolverInfo().m_restitution;
+	worldInfo->m_solverInfo.m_maxErrorReduction = getSolverInfo().m_maxErrorReduction;
+	worldInfo->m_solverInfo.m_sor = getSolverInfo().m_sor;
+	worldInfo->m_solverInfo.m_erp = getSolverInfo().m_erp;
+
+	worldInfo->m_solverInfo.m_erp2 = getSolverInfo().m_erp2;
+	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
-		const char* structType = "btDynamicsWorldFloatData";
-#endif//BT_USE_DOUBLE_PRECISION
-		serializer->finalizeChunk(chunk,structType,BT_DYNAMICSWORLD_CODE,worldInfo);
+	const char* structType = "btDynamicsWorldDoubleData";
+#else   //BT_USE_DOUBLE_PRECISION
+	const char* structType = "btDynamicsWorldFloatData";
+#endif  //BT_USE_DOUBLE_PRECISION
+	serializer->finalizeChunk(chunk, structType, BT_DYNAMICSWORLD_CODE, worldInfo);
 }
 
-void	btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
+void btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
 {
-
 	serializer->startSerialization();
 
 	serializeDynamicsWorldInfo(serializer);
@@ -1512,6 +1463,7 @@ void	btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializeRigidBodies(serializer);
 
+	serializeContactManifolds(serializer);
+
 	serializer->finishSerialization();
 }
-
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
index dd3d1c3660c..73607c61fd5 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
@@ -13,12 +13,10 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_DISCRETE_DYNAMICS_WORLD_H
 #define BT_DISCRETE_DYNAMICS_WORLD_H
 
 #include "btDynamicsWorld.h"
-
 class btDispatcher;
 class btOverlappingPairCache;
 class btConstraintSolver;
@@ -27,159 +25,161 @@ class btTypedConstraint;
 class btActionInterface;
 class btPersistentManifold;
 class btIDebugDraw;
+
 struct InplaceSolverIslandCallback;
 
 #include "LinearMath/btAlignedObjectArray.h"
-
+#include "LinearMath/btThreads.h"
 
 ///btDiscreteDynamicsWorld provides discrete rigid body simulation
 ///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
-ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorld : public btDynamicsWorld
+ATTRIBUTE_ALIGNED16(class)
+btDiscreteDynamicsWorld : public btDynamicsWorld
 {
 protected:
-	
-    btAlignedObjectArray<btTypedConstraint*>	m_sortedConstraints;
-	InplaceSolverIslandCallback* 	m_solverIslandCallback;
+	btAlignedObjectArray<btTypedConstraint*> m_sortedConstraints;
+	InplaceSolverIslandCallback* m_solverIslandCallback;
 
-	btConstraintSolver*	m_constraintSolver;
+	btConstraintSolver* m_constraintSolver;
 
-	btSimulationIslandManager*	m_islandManager;
+	btSimulationIslandManager* m_islandManager;
 
 	btAlignedObjectArray<btTypedConstraint*> m_constraints;
 
 	btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies;
 
-	btVector3	m_gravity;
+	btVector3 m_gravity;
 
 	//for variable timesteps
-	btScalar	m_localTime;
-	btScalar	m_fixedTimeStep;
+	btScalar m_localTime;
+	btScalar m_fixedTimeStep;
 	//for variable timesteps
 
-	bool	m_ownsIslandManager;
-	bool	m_ownsConstraintSolver;
-	bool	m_synchronizeAllMotionStates;
-	bool	m_applySpeculativeContactRestitution;
+	bool m_ownsIslandManager;
+	bool m_ownsConstraintSolver;
+	bool m_synchronizeAllMotionStates;
+	bool m_applySpeculativeContactRestitution;
 
-	btAlignedObjectArray<btActionInterface*>	m_actions;
-	
-	int	m_profileTimings;
+	btAlignedObjectArray<btActionInterface*> m_actions;
 
-	bool	m_latencyMotionStateInterpolation;
+	int m_profileTimings;
 
-	btAlignedObjectArray<btPersistentManifold*>	m_predictiveManifolds;
+	bool m_latencyMotionStateInterpolation;
 
-	virtual void	predictUnconstraintMotion(btScalar timeStep);
-	
-	virtual void	integrateTransforms(btScalar timeStep);
-		
-	virtual void	calculateSimulationIslands();
+	btAlignedObjectArray<btPersistentManifold*> m_predictiveManifolds;
+	btSpinMutex m_predictiveManifoldsMutex;  // used to synchronize threads creating predictive contacts
 
-	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
-	
-	virtual void	updateActivationState(btScalar timeStep);
+	virtual void predictUnconstraintMotion(btScalar timeStep);
+
+	void integrateTransformsInternal(btRigidBody * *bodies, int numBodies, btScalar timeStep);  // can be called in parallel
+	virtual void integrateTransforms(btScalar timeStep);
 
-	void	updateActions(btScalar timeStep);
+	virtual void calculateSimulationIslands();
 
-	void	startProfiling(btScalar timeStep);
+	
 
-	virtual void	internalSingleStepSimulation( btScalar timeStep);
+	virtual void updateActivationState(btScalar timeStep);
 
-	void	createPredictiveContacts(btScalar timeStep);
+	void updateActions(btScalar timeStep);
 
-	virtual void	saveKinematicState(btScalar timeStep);
+	void startProfiling(btScalar timeStep);
 
-	void	serializeRigidBodies(btSerializer* serializer);
+	virtual void internalSingleStepSimulation(btScalar timeStep);
 
-	void	serializeDynamicsWorldInfo(btSerializer* serializer);
+	void releasePredictiveContacts();
+	void createPredictiveContactsInternal(btRigidBody * *bodies, int numBodies, btScalar timeStep);  // can be called in parallel
+	virtual void createPredictiveContacts(btScalar timeStep);
 
-public:
+	virtual void saveKinematicState(btScalar timeStep);
 
+	void serializeRigidBodies(btSerializer * serializer);
 
+	void serializeDynamicsWorldInfo(btSerializer * serializer);
+    
+public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
-	btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+	btDiscreteDynamicsWorld(btDispatcher * dispatcher, btBroadphaseInterface * pairCache, btConstraintSolver * constraintSolver, btCollisionConfiguration * collisionConfiguration);
 
 	virtual ~btDiscreteDynamicsWorld();
 
 	///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
-	virtual int	stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
+	virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
 
-
-	virtual void	synchronizeMotionStates();
+    virtual void solveConstraints(btContactSolverInfo & solverInfo);
+    
+	virtual void synchronizeMotionStates();
 
 	///this can be useful to synchronize a single rigid body -> graphics object
-	void	synchronizeSingleMotionState(btRigidBody* body);
+	void synchronizeSingleMotionState(btRigidBody * body);
 
-	virtual void	addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
+	virtual void addConstraint(btTypedConstraint * constraint, bool disableCollisionsBetweenLinkedBodies = false);
 
-	virtual void	removeConstraint(btTypedConstraint* constraint);
+	virtual void removeConstraint(btTypedConstraint * constraint);
 
-	virtual void	addAction(btActionInterface*);
+	virtual void addAction(btActionInterface*);
 
-	virtual void	removeAction(btActionInterface*);
-	
-	btSimulationIslandManager*	getSimulationIslandManager()
+	virtual void removeAction(btActionInterface*);
+
+	btSimulationIslandManager* getSimulationIslandManager()
 	{
 		return m_islandManager;
 	}
 
-	const btSimulationIslandManager*	getSimulationIslandManager() const 
+	const btSimulationIslandManager* getSimulationIslandManager() const
 	{
 		return m_islandManager;
 	}
 
-	btCollisionWorld*	getCollisionWorld()
+	btCollisionWorld* getCollisionWorld()
 	{
 		return this;
 	}
 
-	virtual void	setGravity(const btVector3& gravity);
+	virtual void setGravity(const btVector3& gravity);
 
-	virtual btVector3 getGravity () const;
+	virtual btVector3 getGravity() const;
 
-	virtual void	addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+	virtual void addCollisionObject(btCollisionObject * collisionObject, int collisionFilterGroup = btBroadphaseProxy::StaticFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
 
-	virtual void	addRigidBody(btRigidBody* body);
+	virtual void addRigidBody(btRigidBody * body);
 
-	virtual void	addRigidBody(btRigidBody* body, short group, short mask);
+	virtual void addRigidBody(btRigidBody * body, int group, int mask);
 
-	virtual void	removeRigidBody(btRigidBody* body);
+	virtual void removeRigidBody(btRigidBody * body);
 
 	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
-	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
+	virtual void removeCollisionObject(btCollisionObject * collisionObject);
 
+	virtual void debugDrawConstraint(btTypedConstraint * constraint);
 
-	virtual void	debugDrawConstraint(btTypedConstraint* constraint);
+	virtual void debugDrawWorld();
 
-	virtual void	debugDrawWorld();
-
-	virtual void	setConstraintSolver(btConstraintSolver* solver);
+	virtual void setConstraintSolver(btConstraintSolver * solver);
 
 	virtual btConstraintSolver* getConstraintSolver();
-	
-	virtual	int		getNumConstraints() const;
 
-	virtual btTypedConstraint* getConstraint(int index)	;
+	virtual int getNumConstraints() const;
+
+	virtual btTypedConstraint* getConstraint(int index);
 
 	virtual const btTypedConstraint* getConstraint(int index) const;
 
-	
-	virtual btDynamicsWorldType	getWorldType() const
+	virtual btDynamicsWorldType getWorldType() const
 	{
 		return BT_DISCRETE_DYNAMICS_WORLD;
 	}
-	
+
 	///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
-	virtual void	clearForces();
+	virtual void clearForces();
 
 	///apply gravity, call this once per timestep
-	virtual void	applyGravity();
+	virtual void applyGravity();
 
-	virtual void	setNumTasks(int numTasks)
+	virtual void setNumTasks(int numTasks)
 	{
-        (void) numTasks;
+		(void)numTasks;
 	}
 
 	///obsolete, use updateActions instead
@@ -189,15 +189,15 @@ public:
 	}
 
 	///obsolete, use addAction instead
-	virtual void	addVehicle(btActionInterface* vehicle);
+	virtual void addVehicle(btActionInterface * vehicle);
 	///obsolete, use removeAction instead
-	virtual void	removeVehicle(btActionInterface* vehicle);
+	virtual void removeVehicle(btActionInterface * vehicle);
 	///obsolete, use addAction instead
-	virtual void	addCharacter(btActionInterface* character);
+	virtual void addCharacter(btActionInterface * character);
 	///obsolete, use removeAction instead
-	virtual void	removeCharacter(btActionInterface* character);
+	virtual void removeCharacter(btActionInterface * character);
 
-	void	setSynchronizeAllMotionStates(bool synchronizeAll)
+	void setSynchronizeAllMotionStates(bool synchronizeAll)
 	{
 		m_synchronizeAllMotionStates = synchronizeAll;
 	}
@@ -210,18 +210,18 @@ public:
 	{
 		m_applySpeculativeContactRestitution = enable;
 	}
-	
+
 	bool getApplySpeculativeContactRestitution() const
 	{
 		return m_applySpeculativeContactRestitution;
 	}
 
 	///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo)
-	virtual	void	serialize(btSerializer* serializer);
+	virtual void serialize(btSerializer * serializer);
 
 	///Interpolate motion state between previous and current transform, instead of current and next transform.
 	///This can relieve discontinuities in the rendering, due to penetrations
-	void setLatencyMotionStateInterpolation(bool latencyInterpolation )
+	void setLatencyMotionStateInterpolation(bool latencyInterpolation)
 	{
 		m_latencyMotionStateInterpolation = latencyInterpolation;
 	}
@@ -229,6 +229,16 @@ public:
 	{
 		return m_latencyMotionStateInterpolation;
 	}
+    
+    btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies()
+    {
+        return m_nonStaticRigidBodies;
+    }
+    
+    const btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies() const
+    {
+        return m_nonStaticRigidBodies;
+    }
 };
 
-#endif //BT_DISCRETE_DYNAMICS_WORLD_H
+#endif  //BT_DISCRETE_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp
new file mode 100644
index 00000000000..8207b471356
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp
@@ -0,0 +1,263 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+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,
+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 "btDiscreteDynamicsWorldMt.h"
+
+//collision detection
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "btSimulationIslandManagerMt.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btQuickprof.h"
+
+//rigidbody & constraints
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
+#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"
+
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "BulletDynamics/Dynamics/btActionInterface.h"
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btMotionState.h"
+
+#include "LinearMath/btSerializer.h"
+
+///
+/// btConstraintSolverPoolMt
+///
+
+btConstraintSolverPoolMt::ThreadSolver* btConstraintSolverPoolMt::getAndLockThreadSolver()
+{
+	int i = 0;
+#if BT_THREADSAFE
+	i = btGetCurrentThreadIndex() % m_solvers.size();
+#endif  // #if BT_THREADSAFE
+	while (true)
+	{
+		ThreadSolver& solver = m_solvers[i];
+		if (solver.mutex.tryLock())
+		{
+			return &solver;
+		}
+		// failed, try the next one
+		i = (i + 1) % m_solvers.size();
+	}
+	return NULL;
+}
+
+void btConstraintSolverPoolMt::init(btConstraintSolver** solvers, int numSolvers)
+{
+	m_solverType = BT_SEQUENTIAL_IMPULSE_SOLVER;
+	m_solvers.resize(numSolvers);
+	for (int i = 0; i < numSolvers; ++i)
+	{
+		m_solvers[i].solver = solvers[i];
+	}
+	if (numSolvers > 0)
+	{
+		m_solverType = solvers[0]->getSolverType();
+	}
+}
+
+// create the solvers for me
+btConstraintSolverPoolMt::btConstraintSolverPoolMt(int numSolvers)
+{
+	btAlignedObjectArray<btConstraintSolver*> solvers;
+	solvers.reserve(numSolvers);
+	for (int i = 0; i < numSolvers; ++i)
+	{
+		btConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
+		solvers.push_back(solver);
+	}
+	init(&solvers[0], numSolvers);
+}
+
+// pass in fully constructed solvers (destructor will delete them)
+btConstraintSolverPoolMt::btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers)
+{
+	init(solvers, numSolvers);
+}
+
+btConstraintSolverPoolMt::~btConstraintSolverPoolMt()
+{
+	// delete all solvers
+	for (int i = 0; i < m_solvers.size(); ++i)
+	{
+		ThreadSolver& solver = m_solvers[i];
+		delete solver.solver;
+		solver.solver = NULL;
+	}
+}
+
+///solve a group of constraints
+btScalar btConstraintSolverPoolMt::solveGroup(btCollisionObject** bodies,
+											  int numBodies,
+											  btPersistentManifold** manifolds,
+											  int numManifolds,
+											  btTypedConstraint** constraints,
+											  int numConstraints,
+											  const btContactSolverInfo& info,
+											  btIDebugDraw* debugDrawer,
+											  btDispatcher* dispatcher)
+{
+	ThreadSolver* ts = getAndLockThreadSolver();
+	ts->solver->solveGroup(bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
+	ts->mutex.unlock();
+	return 0.0f;
+}
+
+void btConstraintSolverPoolMt::reset()
+{
+	for (int i = 0; i < m_solvers.size(); ++i)
+	{
+		ThreadSolver& solver = m_solvers[i];
+		solver.mutex.lock();
+		solver.solver->reset();
+		solver.mutex.unlock();
+	}
+}
+
+///
+/// btDiscreteDynamicsWorldMt
+///
+
+btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
+													 btBroadphaseInterface* pairCache,
+													 btConstraintSolverPoolMt* solverPool,
+													 btConstraintSolver* constraintSolverMt,
+													 btCollisionConfiguration* collisionConfiguration)
+	: btDiscreteDynamicsWorld(dispatcher, pairCache, solverPool, collisionConfiguration)
+{
+	if (m_ownsIslandManager)
+	{
+		m_islandManager->~btSimulationIslandManager();
+		btAlignedFree(m_islandManager);
+	}
+	{
+		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt), 16);
+		btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt();
+		im->setMinimumSolverBatchSize(m_solverInfo.m_minimumSolverBatchSize);
+		m_islandManager = im;
+	}
+	m_constraintSolverMt = constraintSolverMt;
+}
+
+btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt()
+{
+}
+
+void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo)
+{
+	BT_PROFILE("solveConstraints");
+
+	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
+
+	/// solve all the constraints for this island
+	btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
+	btSimulationIslandManagerMt::SolverParams solverParams;
+	solverParams.m_solverPool = m_constraintSolver;
+	solverParams.m_solverMt = m_constraintSolverMt;
+	solverParams.m_solverInfo = &solverInfo;
+	solverParams.m_debugDrawer = m_debugDrawer;
+	solverParams.m_dispatcher = getCollisionWorld()->getDispatcher();
+	im->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, solverParams);
+
+	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
+}
+
+struct UpdaterUnconstrainedMotion : public btIParallelForBody
+{
+	btScalar timeStep;
+	btRigidBody** rigidBodies;
+
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		for (int i = iBegin; i < iEnd; ++i)
+		{
+			btRigidBody* body = rigidBodies[i];
+			if (!body->isStaticOrKinematicObject())
+			{
+				//don't integrate/update velocities here, it happens in the constraint solver
+				body->applyDamping(timeStep);
+				body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform());
+			}
+		}
+	}
+};
+
+void btDiscreteDynamicsWorldMt::predictUnconstraintMotion(btScalar timeStep)
+{
+	BT_PROFILE("predictUnconstraintMotion");
+	if (m_nonStaticRigidBodies.size() > 0)
+	{
+		UpdaterUnconstrainedMotion update;
+		update.timeStep = timeStep;
+		update.rigidBodies = &m_nonStaticRigidBodies[0];
+		int grainSize = 50;  // num of iterations per task for task scheduler
+		btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update);
+	}
+}
+
+void btDiscreteDynamicsWorldMt::createPredictiveContacts(btScalar timeStep)
+{
+	BT_PROFILE("createPredictiveContacts");
+	releasePredictiveContacts();
+	if (m_nonStaticRigidBodies.size() > 0)
+	{
+		UpdaterCreatePredictiveContacts update;
+		update.world = this;
+		update.timeStep = timeStep;
+		update.rigidBodies = &m_nonStaticRigidBodies[0];
+		int grainSize = 50;  // num of iterations per task for task scheduler
+		btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update);
+	}
+}
+
+void btDiscreteDynamicsWorldMt::integrateTransforms(btScalar timeStep)
+{
+	BT_PROFILE("integrateTransforms");
+	if (m_nonStaticRigidBodies.size() > 0)
+	{
+		UpdaterIntegrateTransforms update;
+		update.world = this;
+		update.timeStep = timeStep;
+		update.rigidBodies = &m_nonStaticRigidBodies[0];
+		int grainSize = 50;  // num of iterations per task for task scheduler
+		btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update);
+	}
+}
+
+int btDiscreteDynamicsWorldMt::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
+{
+	int numSubSteps = btDiscreteDynamicsWorld::stepSimulation(timeStep, maxSubSteps, fixedTimeStep);
+	if (btITaskScheduler* scheduler = btGetTaskScheduler())
+	{
+		// tell Bullet's threads to sleep, so other threads can run
+		scheduler->sleepWorkerThreadsHint();
+	}
+	return numSubSteps;
+}
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h
new file mode 100644
index 00000000000..dccf35d7a7a
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h
@@ -0,0 +1,131 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+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, 
+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_DISCRETE_DYNAMICS_WORLD_MT_H
+#define BT_DISCRETE_DYNAMICS_WORLD_MT_H
+
+#include "btDiscreteDynamicsWorld.h"
+#include "btSimulationIslandManagerMt.h"
+#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
+
+///
+/// btConstraintSolverPoolMt - masquerades as a constraint solver, but really it is a threadsafe pool of them.
+///
+///  Each solver in the pool is protected by a mutex.  When solveGroup is called from a thread,
+///  the pool looks for a solver that isn't being used by another thread, locks it, and dispatches the
+///  call to the solver.
+///  So long as there are at least as many solvers as there are hardware threads, it should never need to
+///  spin wait.
+///
+class btConstraintSolverPoolMt : public btConstraintSolver
+{
+public:
+	// create the solvers for me
+	explicit btConstraintSolverPoolMt(int numSolvers);
+
+	// pass in fully constructed solvers (destructor will delete them)
+	btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers);
+
+	virtual ~btConstraintSolverPoolMt();
+
+	///solve a group of constraints
+	virtual btScalar solveGroup(btCollisionObject** bodies,
+								int numBodies,
+								btPersistentManifold** manifolds,
+								int numManifolds,
+								btTypedConstraint** constraints,
+								int numConstraints,
+								const btContactSolverInfo& info,
+								btIDebugDraw* debugDrawer,
+								btDispatcher* dispatcher) BT_OVERRIDE;
+
+	virtual void reset() BT_OVERRIDE;
+	virtual btConstraintSolverType getSolverType() const BT_OVERRIDE { return m_solverType; }
+
+private:
+	const static size_t kCacheLineSize = 128;
+	struct ThreadSolver
+	{
+		btConstraintSolver* solver;
+		btSpinMutex mutex;
+		char _cachelinePadding[kCacheLineSize - sizeof(btSpinMutex) - sizeof(void*)];  // keep mutexes from sharing a cache line
+	};
+	btAlignedObjectArray<ThreadSolver> m_solvers;
+	btConstraintSolverType m_solverType;
+
+	ThreadSolver* getAndLockThreadSolver();
+	void init(btConstraintSolver** solvers, int numSolvers);
+};
+
+///
+/// btDiscreteDynamicsWorldMt -- a version of DiscreteDynamicsWorld with some minor changes to support
+///                              solving simulation islands on multiple threads.
+///
+///  Should function exactly like btDiscreteDynamicsWorld.
+///  Also 3 methods that iterate over all of the rigidbodies can run in parallel:
+///     - predictUnconstraintMotion
+///     - integrateTransforms
+///     - createPredictiveContacts
+///
+ATTRIBUTE_ALIGNED16(class)
+btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld
+{
+protected:
+	btConstraintSolver* m_constraintSolverMt;
+
+	virtual void solveConstraints(btContactSolverInfo & solverInfo) BT_OVERRIDE;
+
+	virtual void predictUnconstraintMotion(btScalar timeStep) BT_OVERRIDE;
+
+	struct UpdaterCreatePredictiveContacts : public btIParallelForBody
+	{
+		btScalar timeStep;
+		btRigidBody** rigidBodies;
+		btDiscreteDynamicsWorldMt* world;
+
+		void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+		{
+			world->createPredictiveContactsInternal(&rigidBodies[iBegin], iEnd - iBegin, timeStep);
+		}
+	};
+	virtual void createPredictiveContacts(btScalar timeStep) BT_OVERRIDE;
+
+	struct UpdaterIntegrateTransforms : public btIParallelForBody
+	{
+		btScalar timeStep;
+		btRigidBody** rigidBodies;
+		btDiscreteDynamicsWorldMt* world;
+
+		void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+		{
+			world->integrateTransformsInternal(&rigidBodies[iBegin], iEnd - iBegin, timeStep);
+		}
+	};
+	virtual void integrateTransforms(btScalar timeStep) BT_OVERRIDE;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btDiscreteDynamicsWorldMt(btDispatcher * dispatcher,
+							  btBroadphaseInterface * pairCache,
+							  btConstraintSolverPoolMt * solverPool,        // Note this should be a solver-pool for multi-threading
+							  btConstraintSolver * constraintSolverMt,      // single multi-threaded solver for large islands (or NULL)
+							  btCollisionConfiguration * collisionConfiguration);
+	virtual ~btDiscreteDynamicsWorldMt();
+
+	virtual int stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) BT_OVERRIDE;
+};
+
+#endif  //BT_DISCRETE_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
index 35dd1400fe7..3c55234a8a9 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
@@ -24,144 +24,151 @@ class btActionInterface;
 class btConstraintSolver;
 class btDynamicsWorld;
 
-
 /// Type for the callback for each tick
-typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep);
+typedef void (*btInternalTickCallback)(btDynamicsWorld* world, btScalar timeStep);
 
 enum btDynamicsWorldType
 {
-	BT_SIMPLE_DYNAMICS_WORLD=1,
-	BT_DISCRETE_DYNAMICS_WORLD=2,
-	BT_CONTINUOUS_DYNAMICS_WORLD=3,
-	BT_SOFT_RIGID_DYNAMICS_WORLD=4,
-	BT_GPU_DYNAMICS_WORLD=5
+	BT_SIMPLE_DYNAMICS_WORLD = 1,
+	BT_DISCRETE_DYNAMICS_WORLD = 2,
+	BT_CONTINUOUS_DYNAMICS_WORLD = 3,
+	BT_SOFT_RIGID_DYNAMICS_WORLD = 4,
+	BT_GPU_DYNAMICS_WORLD = 5,
+	BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6,
+    BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD = 7
 };
 
 ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.
 class btDynamicsWorld : public btCollisionWorld
 {
-
 protected:
-		btInternalTickCallback m_internalTickCallback;
-		btInternalTickCallback m_internalPreTickCallback;
-		void*	m_worldUserInfo;
+	btInternalTickCallback m_internalTickCallback;
+	btInternalTickCallback m_internalPreTickCallback;
+	void* m_worldUserInfo;
 
-		btContactSolverInfo	m_solverInfo;
+	btContactSolverInfo m_solverInfo;
 
 public:
-		
+	btDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* broadphase, btCollisionConfiguration* collisionConfiguration)
+		: btCollisionWorld(dispatcher, broadphase, collisionConfiguration), m_internalTickCallback(0), m_internalPreTickCallback(0), m_worldUserInfo(0)
+	{
+	}
 
-		btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration)
-		:btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0),m_internalPreTickCallback(0), m_worldUserInfo(0)
-		{
-		}
+	virtual ~btDynamicsWorld()
+	{
+	}
 
-		virtual ~btDynamicsWorld()
-		{
-		}
-		
-		///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds.
-		///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'.
-		///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'.
-		///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant.
-		virtual int		stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
-			
-		virtual void	debugDrawWorld() = 0;
-				
-		virtual void	addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) 
-		{ 
-			(void)constraint; (void)disableCollisionsBetweenLinkedBodies;
-		}
+	///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds.
+	///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'.
+	///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'.
+	///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant.
+	virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)) = 0;
 
-		virtual void	removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
+	virtual void debugDrawWorld() = 0;
 
-		virtual void	addAction(btActionInterface* action) = 0;
+	virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies = false)
+	{
+		(void)constraint;
+		(void)disableCollisionsBetweenLinkedBodies;
+	}
 
-		virtual void	removeAction(btActionInterface* action) = 0;
+	virtual void removeConstraint(btTypedConstraint* constraint) { (void)constraint; }
 
-		//once a rigidbody is added to the dynamics world, it will get this gravity assigned
-		//existing rigidbodies in the world get gravity assigned too, during this method
-		virtual void	setGravity(const btVector3& gravity) = 0;
-		virtual btVector3 getGravity () const = 0;
+	virtual void addAction(btActionInterface* action) = 0;
 
-		virtual void	synchronizeMotionStates() = 0;
+	virtual void removeAction(btActionInterface* action) = 0;
 
-		virtual void	addRigidBody(btRigidBody* body) = 0;
+	//once a rigidbody is added to the dynamics world, it will get this gravity assigned
+	//existing rigidbodies in the world get gravity assigned too, during this method
+	virtual void setGravity(const btVector3& gravity) = 0;
+	virtual btVector3 getGravity() const = 0;
 
-		virtual void	addRigidBody(btRigidBody* body, short group, short mask) = 0;
+	virtual void synchronizeMotionStates() = 0;
 
-		virtual void	removeRigidBody(btRigidBody* body) = 0;
+	virtual void addRigidBody(btRigidBody* body) = 0;
 
-		virtual void	setConstraintSolver(btConstraintSolver* solver) = 0;
+	virtual void addRigidBody(btRigidBody* body, int group, int mask) = 0;
 
-		virtual btConstraintSolver* getConstraintSolver() = 0;
-		
-		virtual	int		getNumConstraints() const {	return 0;		}
-		
-		virtual btTypedConstraint* getConstraint(int index)		{	(void)index;		return 0;		}
-		
-		virtual const btTypedConstraint* getConstraint(int index) const	{	(void)index;	return 0;	}
+	virtual void removeRigidBody(btRigidBody* body) = 0;
 
-		virtual btDynamicsWorldType	getWorldType() const=0;
+	virtual void setConstraintSolver(btConstraintSolver* solver) = 0;
 
-		virtual void	clearForces() = 0;
+	virtual btConstraintSolver* getConstraintSolver() = 0;
 
-		/// Set the callback for when an internal tick (simulation substep) happens, optional user info
-		void setInternalTickCallback(btInternalTickCallback cb,	void* worldUserInfo=0,bool isPreTick=false) 
-		{ 
-			if (isPreTick)
-			{
-				m_internalPreTickCallback = cb;
-			} else
-			{
-				m_internalTickCallback = cb; 
-			}
-			m_worldUserInfo = worldUserInfo;
-		}
+	virtual int getNumConstraints() const { return 0; }
 
-		void	setWorldUserInfo(void* worldUserInfo)
-		{
-			m_worldUserInfo = worldUserInfo;
-		}
+	virtual btTypedConstraint* getConstraint(int index)
+	{
+		(void)index;
+		return 0;
+	}
+
+	virtual const btTypedConstraint* getConstraint(int index) const
+	{
+		(void)index;
+		return 0;
+	}
 
-		void*	getWorldUserInfo() const
+	virtual btDynamicsWorldType getWorldType() const = 0;
+
+	virtual void clearForces() = 0;
+
+	/// Set the callback for when an internal tick (simulation substep) happens, optional user info
+	void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo = 0, bool isPreTick = false)
+	{
+		if (isPreTick)
 		{
-			return m_worldUserInfo;
+			m_internalPreTickCallback = cb;
 		}
-
-		btContactSolverInfo& getSolverInfo()
+		else
 		{
-			return m_solverInfo;
+			m_internalTickCallback = cb;
 		}
-
-
-		///obsolete, use addAction instead.
-		virtual void	addVehicle(btActionInterface* vehicle) {(void)vehicle;}
-		///obsolete, use removeAction instead
-		virtual void	removeVehicle(btActionInterface* vehicle) {(void)vehicle;}
-		///obsolete, use addAction instead.
-		virtual void	addCharacter(btActionInterface* character) {(void)character;}
-		///obsolete, use removeAction instead
-		virtual void	removeCharacter(btActionInterface* character) {(void)character;}
-
-
+		m_worldUserInfo = worldUserInfo;
+	}
+
+	void setWorldUserInfo(void* worldUserInfo)
+	{
+		m_worldUserInfo = worldUserInfo;
+	}
+
+	void* getWorldUserInfo() const
+	{
+		return m_worldUserInfo;
+	}
+
+	btContactSolverInfo& getSolverInfo()
+	{
+		return m_solverInfo;
+	}
+
+	const btContactSolverInfo& getSolverInfo() const
+	{
+		return m_solverInfo;
+	}
+
+	///obsolete, use addAction instead.
+	virtual void addVehicle(btActionInterface* vehicle) { (void)vehicle; }
+	///obsolete, use removeAction instead
+	virtual void removeVehicle(btActionInterface* vehicle) { (void)vehicle; }
+	///obsolete, use addAction instead.
+	virtual void addCharacter(btActionInterface* character) { (void)character; }
+	///obsolete, use removeAction instead
+	virtual void removeCharacter(btActionInterface* character) { (void)character; }
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct btDynamicsWorldDoubleData
 {
-	btContactSolverInfoDoubleData	m_solverInfo;
-	btVector3DoubleData	m_gravity;
+	btContactSolverInfoDoubleData m_solverInfo;
+	btVector3DoubleData m_gravity;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct btDynamicsWorldFloatData
 {
-	btContactSolverInfoFloatData	m_solverInfo;
-	btVector3FloatData	m_gravity;
+	btContactSolverInfoFloatData m_solverInfo;
+	btVector3FloatData m_gravity;
 };
 
-
-#endif //BT_DYNAMICS_WORLD_H
-
-
+#endif  //BT_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
index a7882684bf1..27fdead7617 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
@@ -22,36 +22,34 @@ subject to the following restrictions:
 #include "LinearMath/btSerializer.h"
 
 //'temporarily' global variables
-btScalar	gDeactivationTime = btScalar(2.);
-bool	gDisableDeactivation = false;
+btScalar gDeactivationTime = btScalar(2.);
+bool gDisableDeactivation = false;
 static int uniqueId = 0;
 
-
 btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
 {
 	setupRigidBody(constructionInfo);
 }
 
-btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia)
+btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia)
 {
-	btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia);
+	btRigidBodyConstructionInfo cinfo(mass, motionState, collisionShape, localInertia);
 	setupRigidBody(cinfo);
 }
 
-void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
+void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
 {
-
-	m_internalType=CO_RIGID_BODY;
+	m_internalType = CO_RIGID_BODY;
 
 	m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
-	m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
-	m_angularFactor.setValue(1,1,1);
-	m_linearFactor.setValue(1,1,1);
+	m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+	m_angularFactor.setValue(1, 1, 1);
+	m_linearFactor.setValue(1, 1, 1);
 	m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-    setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
+		setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
 
 	m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
 	m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
@@ -67,46 +65,44 @@ void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
 	if (m_optionalMotionState)
 	{
 		m_optionalMotionState->getWorldTransform(m_worldTransform);
-	} else
+	}
+	else
 	{
 		m_worldTransform = constructionInfo.m_startWorldTransform;
 	}
 
 	m_interpolationWorldTransform = m_worldTransform;
-	m_interpolationLinearVelocity.setValue(0,0,0);
-	m_interpolationAngularVelocity.setValue(0,0,0);
-	
+	m_interpolationLinearVelocity.setValue(0, 0, 0);
+	m_interpolationAngularVelocity.setValue(0, 0, 0);
+
 	//moved to btCollisionObject
 	m_friction = constructionInfo.m_friction;
 	m_rollingFriction = constructionInfo.m_rollingFriction;
+	m_spinningFriction = constructionInfo.m_spinningFriction;
+
 	m_restitution = constructionInfo.m_restitution;
 
-	setCollisionShape( constructionInfo.m_collisionShape );
+	setCollisionShape(constructionInfo.m_collisionShape);
 	m_debugBodyId = uniqueId++;
-	
+
 	setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
 	updateInertiaTensor();
 
 	m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY;
 
-
 	m_deltaLinearVelocity.setZero();
 	m_deltaAngularVelocity.setZero();
-	m_invMass = m_inverseMass*m_linearFactor;
+	m_invMass = m_inverseMass * m_linearFactor;
 	m_pushVelocity.setZero();
 	m_turnVelocity.setZero();
-
-	
-
 }
 
-
-void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
+void btRigidBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform)
 {
-	btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
+	btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform);
 }
 
-void			btRigidBody::saveKinematicState(btScalar timeStep)
+void btRigidBody::saveKinematicState(btScalar timeStep)
 {
 	//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
 	if (timeStep != btScalar(0.))
@@ -114,25 +110,22 @@ void			btRigidBody::saveKinematicState(btScalar timeStep)
 		//if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
 		if (getMotionState())
 			getMotionState()->getWorldTransform(m_worldTransform);
-		btVector3 linVel,angVel;
-		
-		btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
+		btVector3 linVel, angVel;
+
+		btTransformUtil::calculateVelocity(m_interpolationWorldTransform, m_worldTransform, timeStep, m_linearVelocity, m_angularVelocity);
 		m_interpolationLinearVelocity = m_linearVelocity;
 		m_interpolationAngularVelocity = m_angularVelocity;
 		m_interpolationWorldTransform = m_worldTransform;
 		//printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
 	}
 }
-	
-void	btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
+
+void btRigidBody::getAabb(btVector3& aabbMin, btVector3& aabbMax) const
 {
-	getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
+	getCollisionShape()->getAabb(m_worldTransform, aabbMin, aabbMax);
 }
 
-
-
-
-void btRigidBody::setGravity(const btVector3& acceleration) 
+void btRigidBody::setGravity(const btVector3& acceleration)
 {
 	if (m_inverseMass != btScalar(0.0))
 	{
@@ -141,33 +134,29 @@ void btRigidBody::setGravity(const btVector3& acceleration)
 	m_gravity_acceleration = acceleration;
 }
 
-
-
-
-
-
 void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
 {
-	m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
-	m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+#ifdef BT_USE_OLD_DAMPING_METHOD
+	m_linearDamping = btMax(lin_damping, btScalar(0.0));
+	m_angularDamping = btMax(ang_damping, btScalar(0.0));
+#else
+	m_linearDamping = btClamped(lin_damping, btScalar(0.0), btScalar(1.0));
+	m_angularDamping = btClamped(ang_damping, btScalar(0.0), btScalar(1.0));
+#endif
 }
 
-
-
-
 ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
-void			btRigidBody::applyDamping(btScalar timeStep)
+void btRigidBody::applyDamping(btScalar timeStep)
 {
 	//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
 	//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
 
-//#define USE_OLD_DAMPING_METHOD 1
-#ifdef USE_OLD_DAMPING_METHOD
-	m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
-	m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+#ifdef BT_USE_OLD_DAMPING_METHOD
+	m_linearVelocity *= btMax((btScalar(1.0) - timeStep * m_linearDamping), btScalar(0.0));
+	m_angularVelocity *= btMax((btScalar(1.0) - timeStep * m_angularDamping), btScalar(0.0));
 #else
-	m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep);
-	m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep);
+	m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep);
+	m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep);
 #endif
 
 	if (m_additionalDamping)
@@ -180,7 +169,6 @@ void			btRigidBody::applyDamping(btScalar timeStep)
 			m_angularVelocity *= m_additionalDampingFactor;
 			m_linearVelocity *= m_additionalDampingFactor;
 		}
-	
 
 		btScalar speed = m_linearVelocity.length();
 		if (speed < m_linearDamping)
@@ -189,10 +177,11 @@ void			btRigidBody::applyDamping(btScalar timeStep)
 			if (speed > dampVel)
 			{
 				btVector3 dir = m_linearVelocity.normalized();
-				m_linearVelocity -=  dir * dampVel;
-			} else
+				m_linearVelocity -= dir * dampVel;
+			}
+			else
 			{
-				m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+				m_linearVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 			}
 		}
 
@@ -203,30 +192,36 @@ void			btRigidBody::applyDamping(btScalar timeStep)
 			if (angSpeed > angDampVel)
 			{
 				btVector3 dir = m_angularVelocity.normalized();
-				m_angularVelocity -=  dir * angDampVel;
-			} else
+				m_angularVelocity -= dir * angDampVel;
+			}
+			else
 			{
-				m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+				m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 			}
 		}
 	}
 }
 
-
 void btRigidBody::applyGravity()
 {
 	if (isStaticOrKinematicObject())
 		return;
-	
-	applyCentralForce(m_gravity);	
 
+	applyCentralForce(m_gravity);
+}
+
+void btRigidBody::clearGravity()
+{
+    if (isStaticOrKinematicObject())
+        return;
+    
+    applyCentralForce(-m_gravity);
 }
 
 void btRigidBody::proceedToTransform(const btTransform& newTrans)
 {
-	setCenterOfMassTransform( newTrans );
+	setCenterOfMassTransform(newTrans);
 }
-	
 
 void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
 {
@@ -234,7 +229,8 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
 	{
 		m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
 		m_inverseMass = btScalar(0.);
-	} else
+	}
+	else
 	{
 		m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
 		m_inverseMass = btScalar(1.0) / mass;
@@ -242,50 +238,45 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
 
 	//Fg = m * a
 	m_gravity = mass * m_gravity_acceleration;
-	
-	m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
-				   inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
-				   inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
 
-	m_invMass = m_linearFactor*m_inverseMass;
+	m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
+							   inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
+							   inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
+
+	m_invMass = m_linearFactor * m_inverseMass;
 }
 
-	
-void btRigidBody::updateInertiaTensor() 
+void btRigidBody::updateInertiaTensor()
 {
 	m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
 }
 
-
-
 btVector3 btRigidBody::getLocalInertia() const
 {
-
 	btVector3 inertiaLocal;
 	const btVector3 inertia = m_invInertiaLocal;
 	inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
-		inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
-		inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
+						  inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
+						  inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
 	return inertiaLocal;
 }
 
 inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt,
-	const btMatrix3x3 &I)
+							  const btMatrix3x3& I)
 {
-	const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0);
+	const btVector3 w2 = I * w1 + w1.cross(I * w1) * dt - (T * dt + I * w0);
 	return w2;
 }
 
 inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt,
-	const btMatrix3x3 &I)
+									 const btMatrix3x3& I)
 {
-
 	btMatrix3x3 w1x, Iw1x;
-	const btVector3 Iwi = (I*w1);
+	const btVector3 Iwi = (I * w1);
 	w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]);
 	Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]);
 
-	const btMatrix3x3 dfw1 = I + (w1x*I - Iw1x)*dt;
+	const btMatrix3x3 dfw1 = I + (w1x * I - Iw1x) * dt;
 	return dfw1;
 }
 
@@ -293,58 +284,55 @@ btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForc
 {
 	btVector3 inertiaLocal = getLocalInertia();
 	btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
-	btVector3 tmp = inertiaTensorWorld*getAngularVelocity();
+	btVector3 tmp = inertiaTensorWorld * getAngularVelocity();
 	btVector3 gf = getAngularVelocity().cross(tmp);
 	btScalar l2 = gf.length2();
-	if (l2>maxGyroscopicForce*maxGyroscopicForce)
+	if (l2 > maxGyroscopicForce * maxGyroscopicForce)
 	{
-		gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce;
+		gf *= btScalar(1.) / btSqrt(l2) * maxGyroscopicForce;
 	}
 	return gf;
 }
 
-
 btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const
-{	
+{
 	btVector3 idl = getLocalInertia();
 	btVector3 omega1 = getAngularVelocity();
 	btQuaternion q = getWorldTransform().getRotation();
-	
+
 	// Convert to body coordinates
 	btVector3 omegab = quatRotate(q.inverse(), omega1);
 	btMatrix3x3 Ib;
-	Ib.setValue(idl.x(),0,0,
-				0,idl.y(),0,
-				0,0,idl.z());
-	
-	btVector3 ibo = Ib*omegab;
+	Ib.setValue(idl.x(), 0, 0,
+				0, idl.y(), 0,
+				0, 0, idl.z());
+
+	btVector3 ibo = Ib * omegab;
 
 	// Residual vector
 	btVector3 f = step * omegab.cross(ibo);
-	
+
 	btMatrix3x3 skew0;
 	omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]);
-	btVector3 om = Ib*omegab;
+	btVector3 om = Ib * omegab;
 	btMatrix3x3 skew1;
-	om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]);
-	
+	om.getSkewSymmetricMatrix(&skew1[0], &skew1[1], &skew1[2]);
+
 	// Jacobian
-	btMatrix3x3 J = Ib +  (skew0*Ib - skew1)*step;
-	
-//	btMatrix3x3 Jinv = J.inverse();
-//	btVector3 omega_div = Jinv*f;
+	btMatrix3x3 J = Ib + (skew0 * Ib - skew1) * step;
+
+	//	btMatrix3x3 Jinv = J.inverse();
+	//	btVector3 omega_div = Jinv*f;
 	btVector3 omega_div = J.solve33(f);
-	
+
 	// Single Newton-Raphson update
-	omegab = omegab - omega_div;//Solve33(J, f);
+	omegab = omegab - omega_div;  //Solve33(J, f);
 	// Back to world coordinates
-	btVector3 omega2 = quatRotate(q,omegab);
-	btVector3 gf = omega2-omega1;
+	btVector3 omega2 = quatRotate(q, omegab);
+	btVector3 gf = omega2 - omega1;
 	return gf;
 }
 
-
-
 btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const
 {
 	// use full newton-euler equations.  common practice to drop the wxIw term. want it for better tumbling behavior.
@@ -359,7 +347,7 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) con
 		m_worldTransform.getBasis().transpose();
 
 	// use newtons method to find implicit solution for new angular velocity (w')
-	// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 
+	// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0
 	// df/dw' = I + 1xIw'*step + w'xI*step
 
 	btVector3 w1 = w0;
@@ -381,8 +369,7 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) con
 	return gf;
 }
 
-
-void btRigidBody::integrateVelocities(btScalar step) 
+void btRigidBody::integrateVelocities(btScalar step)
 {
 	if (isStaticOrKinematicObject())
 		return;
@@ -391,30 +378,31 @@ void btRigidBody::integrateVelocities(btScalar step)
 	m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
 
 #define MAX_ANGVEL SIMD_HALF_PI
-	/// clamp angular velocity. collision calculations will fail on higher angular velocities	
+	/// clamp angular velocity. collision calculations will fail on higher angular velocities
 	btScalar angvel = m_angularVelocity.length();
-	if (angvel*step > MAX_ANGVEL)
+	if (angvel * step > MAX_ANGVEL)
 	{
-		m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
+		m_angularVelocity *= (MAX_ANGVEL / step) / angvel;
 	}
-
+	#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+	clampVelocity(m_angularVelocity);
+	#endif
 }
 
 btQuaternion btRigidBody::getOrientation() const
 {
-		btQuaternion orn;
-		m_worldTransform.getBasis().getRotation(orn);
-		return orn;
+	btQuaternion orn;
+	m_worldTransform.getBasis().getRotation(orn);
+	return orn;
 }
-	
-	
+
 void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
 {
-
 	if (isKinematicObject())
 	{
 		m_interpolationWorldTransform = m_worldTransform;
-	} else
+	}
+	else
 	{
 		m_interpolationWorldTransform = xform;
 	}
@@ -424,51 +412,59 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
 	updateInertiaTensor();
 }
 
-
-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
+int btRigidBody::calculateSerializeBufferSize() const
 {
 	int sz = sizeof(btRigidBodyData);
 	return sz;
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
 {
-	btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer;
+	btRigidBodyData* rbd = (btRigidBodyData*)dataBuffer;
 
 	btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
 
@@ -490,19 +486,20 @@ const char*	btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali
 	rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
 	rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
 	rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
-	rbd->m_linearSleepingThreshold=m_linearSleepingThreshold;
+	rbd->m_linearSleepingThreshold = m_linearSleepingThreshold;
 	rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
 
+	// Fill padding with zeros to appease msan.
+#ifdef BT_USE_DOUBLE_PRECISION
+	memset(rbd->m_padding, 0, sizeof(rbd->m_padding));
+#endif
+
 	return btRigidBodyDataName;
 }
 
-
-
 void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
 {
-	btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1);
+	btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(), 1);
 	const char* structType = serialize(chunk->m_oldPtr, serializer);
-	serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this);
+	serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, (void*)this);
 }
-
-
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
index c2f8c5d64ae..7442dd1e6a6 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
@@ -25,206 +25,197 @@ class btCollisionShape;
 class btMotionState;
 class btTypedConstraint;
 
-
 extern btScalar gDeactivationTime;
 extern bool gDisableDeactivation;
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btRigidBodyData	btRigidBodyDoubleData
-#define btRigidBodyDataName	"btRigidBodyDoubleData"
+#define btRigidBodyData btRigidBodyDoubleData
+#define btRigidBodyDataName "btRigidBodyDoubleData"
 #else
-#define btRigidBodyData	btRigidBodyFloatData
-#define btRigidBodyDataName	"btRigidBodyFloatData"
-#endif //BT_USE_DOUBLE_PRECISION
-
+#define btRigidBodyData btRigidBodyFloatData
+#define btRigidBodyDataName "btRigidBodyFloatData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
-enum	btRigidBodyFlags
+enum btRigidBodyFlags
 {
 	BT_DISABLE_WORLD_GRAVITY = 1,
 	///BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards.
 	///and it BT_ENABLE_GYROPSCOPIC_FORCE becomes equivalent to BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
 	///See Demos/GyroscopicDemo and computeGyroscopicImpulseImplicit
 	BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT = 2,
-	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD=4,
-	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY=8,
+	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD = 4,
+	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY = 8,
 	BT_ENABLE_GYROPSCOPIC_FORCE = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY,
 };
 
-
 ///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape.
 ///It is recommended for performance and memory use to share btCollisionShape objects whenever possible.
-///There are 3 types of rigid bodies: 
+///There are 3 types of rigid bodies:
 ///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics.
 ///- B) Fixed objects with zero mass. They are not moving (basically collision objects)
-///- C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform.
+///- C) Kinematic objects, which are objects without mass, but the user can move them. There is one-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform.
 ///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time.
 ///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects)
-class btRigidBody  : public btCollisionObject
+class btRigidBody : public btCollisionObject
 {
-
-	btMatrix3x3	m_invInertiaTensorWorld;
-	btVector3		m_linearVelocity;
-	btVector3		m_angularVelocity;
-	btScalar		m_inverseMass;
-	btVector3		m_linearFactor;
-
-	btVector3		m_gravity;	
-	btVector3		m_gravity_acceleration;
-	btVector3		m_invInertiaLocal;
-	btVector3		m_totalForce;
-	btVector3		m_totalTorque;
-	
-	btScalar		m_linearDamping;
-	btScalar		m_angularDamping;
-
-	bool			m_additionalDamping;
-	btScalar		m_additionalDampingFactor;
-	btScalar		m_additionalLinearDampingThresholdSqr;
-	btScalar		m_additionalAngularDampingThresholdSqr;
-	btScalar		m_additionalAngularDampingFactor;
-
-
-	btScalar		m_linearSleepingThreshold;
-	btScalar		m_angularSleepingThreshold;
+	btMatrix3x3 m_invInertiaTensorWorld;
+	btVector3 m_linearVelocity;
+	btVector3 m_angularVelocity;
+	btScalar m_inverseMass;
+	btVector3 m_linearFactor;
+
+	btVector3 m_gravity;
+	btVector3 m_gravity_acceleration;
+	btVector3 m_invInertiaLocal;
+	btVector3 m_totalForce;
+	btVector3 m_totalTorque;
+
+	btScalar m_linearDamping;
+	btScalar m_angularDamping;
+
+	bool m_additionalDamping;
+	btScalar m_additionalDampingFactor;
+	btScalar m_additionalLinearDampingThresholdSqr;
+	btScalar m_additionalAngularDampingThresholdSqr;
+	btScalar m_additionalAngularDampingFactor;
+
+	btScalar m_linearSleepingThreshold;
+	btScalar m_angularSleepingThreshold;
 
 	//m_optionalMotionState allows to automatic synchronize the world transform for active objects
-	btMotionState*	m_optionalMotionState;
+	btMotionState* m_optionalMotionState;
 
 	//keep track of typed constraints referencing this rigid body, to disable collision between linked bodies
 	btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
 
-	int				m_rigidbodyFlags;
-	
-	int				m_debugBodyId;
-	
-
-protected:
+	int m_rigidbodyFlags;
 
-	ATTRIBUTE_ALIGNED16(btVector3		m_deltaLinearVelocity);
-	btVector3		m_deltaAngularVelocity;
-	btVector3		m_angularFactor;
-	btVector3		m_invMass;
-	btVector3		m_pushVelocity;
-	btVector3		m_turnVelocity;
+	int m_debugBodyId;
 
+protected:
+	ATTRIBUTE_ALIGNED16(btVector3 m_deltaLinearVelocity);
+	btVector3 m_deltaAngularVelocity;
+	btVector3 m_angularFactor;
+	btVector3 m_invMass;
+	btVector3 m_pushVelocity;
+	btVector3 m_turnVelocity;
 
 public:
-
-
 	///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body.
 	///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument)
-	///You can use the motion state to synchronize the world transform between physics and graphics objects. 
+	///You can use the motion state to synchronize the world transform between physics and graphics objects.
 	///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state,
 	///m_startWorldTransform is only used when you don't provide a motion state.
-	struct	btRigidBodyConstructionInfo
+	struct btRigidBodyConstructionInfo
 	{
-		btScalar			m_mass;
+		btScalar m_mass;
 
 		///When a motionState is provided, the rigid body will initialize its world transform from the motion state
 		///In this case, m_startWorldTransform is ignored.
-		btMotionState*		m_motionState;
-		btTransform	m_startWorldTransform;
+		btMotionState* m_motionState;
+		btTransform m_startWorldTransform;
 
-		btCollisionShape*	m_collisionShape;
-		btVector3			m_localInertia;
-		btScalar			m_linearDamping;
-		btScalar			m_angularDamping;
+		btCollisionShape* m_collisionShape;
+		btVector3 m_localInertia;
+		btScalar m_linearDamping;
+		btScalar m_angularDamping;
 
 		///best simulation results when friction is non-zero
-		btScalar			m_friction;
+		btScalar m_friction;
 		///the m_rollingFriction prevents rounded shapes, such as spheres, cylinders and capsules from rolling forever.
 		///See Bullet/Demos/RollingFrictionDemo for usage
-		btScalar			m_rollingFriction;
+		btScalar m_rollingFriction;
+		btScalar m_spinningFriction;  //torsional friction around contact normal
+
 		///best simulation results using zero restitution.
-		btScalar			m_restitution;
+		btScalar m_restitution;
 
-		btScalar			m_linearSleepingThreshold;
-		btScalar			m_angularSleepingThreshold;
+		btScalar m_linearSleepingThreshold;
+		btScalar m_angularSleepingThreshold;
 
 		//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
 		//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
-		bool				m_additionalDamping;
-		btScalar			m_additionalDampingFactor;
-		btScalar			m_additionalLinearDampingThresholdSqr;
-		btScalar			m_additionalAngularDampingThresholdSqr;
-		btScalar			m_additionalAngularDampingFactor;
-
-		btRigidBodyConstructionInfo(	btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
-		m_mass(mass),
-			m_motionState(motionState),
-			m_collisionShape(collisionShape),
-			m_localInertia(localInertia),
-			m_linearDamping(btScalar(0.)),
-			m_angularDamping(btScalar(0.)),
-			m_friction(btScalar(0.5)),
-			m_rollingFriction(btScalar(0)),
-			m_restitution(btScalar(0.)),
-			m_linearSleepingThreshold(btScalar(0.8)),
-			m_angularSleepingThreshold(btScalar(1.f)),
-			m_additionalDamping(false),
-			m_additionalDampingFactor(btScalar(0.005)),
-			m_additionalLinearDampingThresholdSqr(btScalar(0.01)),
-			m_additionalAngularDampingThresholdSqr(btScalar(0.01)),
-			m_additionalAngularDampingFactor(btScalar(0.01))
+		bool m_additionalDamping;
+		btScalar m_additionalDampingFactor;
+		btScalar m_additionalLinearDampingThresholdSqr;
+		btScalar m_additionalAngularDampingThresholdSqr;
+		btScalar m_additionalAngularDampingFactor;
+
+		btRigidBodyConstructionInfo(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia = btVector3(0, 0, 0)) : m_mass(mass),
+																																									   m_motionState(motionState),
+																																									   m_collisionShape(collisionShape),
+																																									   m_localInertia(localInertia),
+																																									   m_linearDamping(btScalar(0.)),
+																																									   m_angularDamping(btScalar(0.)),
+																																									   m_friction(btScalar(0.5)),
+																																									   m_rollingFriction(btScalar(0)),
+																																									   m_spinningFriction(btScalar(0)),
+																																									   m_restitution(btScalar(0.)),
+																																									   m_linearSleepingThreshold(btScalar(0.8)),
+																																									   m_angularSleepingThreshold(btScalar(1.f)),
+																																									   m_additionalDamping(false),
+																																									   m_additionalDampingFactor(btScalar(0.005)),
+																																									   m_additionalLinearDampingThresholdSqr(btScalar(0.01)),
+																																									   m_additionalAngularDampingThresholdSqr(btScalar(0.01)),
+																																									   m_additionalAngularDampingFactor(btScalar(0.01))
 		{
 			m_startWorldTransform.setIdentity();
 		}
 	};
 
 	///btRigidBody constructor using construction info
-	btRigidBody(	const btRigidBodyConstructionInfo& constructionInfo);
+	btRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
 
-	///btRigidBody constructor for backwards compatibility. 
+	///btRigidBody constructor for backwards compatibility.
 	///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo)
-	btRigidBody(	btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0));
-
+	btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia = btVector3(0, 0, 0));
 
 	virtual ~btRigidBody()
-        { 
-                //No constraints should point to this rigidbody
-		//Remove constraints from the dynamics world before you delete the related rigidbodies. 
-                btAssert(m_constraintRefs.size()==0); 
-        }
+	{
+		//No constraints should point to this rigidbody
+		//Remove constraints from the dynamics world before you delete the related rigidbodies.
+		btAssert(m_constraintRefs.size() == 0);
+	}
 
 protected:
-
 	///setupRigidBody is only used internally by the constructor
-	void	setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
+	void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
 
 public:
+	void proceedToTransform(const btTransform& newTrans);
 
-	void			proceedToTransform(const btTransform& newTrans); 
-	
 	///to keep collision detection and dynamics separate we don't store a rigidbody pointer
 	///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
-	static const btRigidBody*	upcast(const btCollisionObject* colObj)
+	static const btRigidBody* upcast(const btCollisionObject* colObj)
 	{
-		if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
+		if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
 			return (const btRigidBody*)colObj;
 		return 0;
 	}
-	static btRigidBody*	upcast(btCollisionObject* colObj)
+	static btRigidBody* upcast(btCollisionObject* colObj)
 	{
-		if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
+		if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
 			return (btRigidBody*)colObj;
 		return 0;
 	}
-	
+
 	/// continuous collision detection needs prediction
-	void			predictIntegratedTransform(btScalar step, btTransform& predictedTransform) ;
-	
-	void			saveKinematicState(btScalar step);
-	
-	void			applyGravity();
-	
-	void			setGravity(const btVector3& acceleration);  
+	void predictIntegratedTransform(btScalar step, btTransform& predictedTransform);
+
+	void saveKinematicState(btScalar step);
+
+	void applyGravity();
+    
+    void clearGravity();
 
-	const btVector3&	getGravity() const
+	void setGravity(const btVector3& acceleration);
+
+	const btVector3& getGravity() const
 	{
 		return m_gravity_acceleration;
 	}
 
-	void			setDamping(btScalar lin_damping, btScalar ang_damping);
+	void setDamping(btScalar lin_damping, btScalar ang_damping);
 
 	btScalar getLinearDamping() const
 	{
@@ -246,18 +237,20 @@ public:
 		return m_angularSleepingThreshold;
 	}
 
-	void			applyDamping(btScalar timeStep);
+	void applyDamping(btScalar timeStep);
 
-	SIMD_FORCE_INLINE const btCollisionShape*	getCollisionShape() const {
+	SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const
+	{
 		return m_collisionShape;
 	}
 
-	SIMD_FORCE_INLINE btCollisionShape*	getCollisionShape() {
-			return m_collisionShape;
+	SIMD_FORCE_INLINE btCollisionShape* getCollisionShape()
+	{
+		return m_collisionShape;
 	}
-	
-	void			setMassProps(btScalar mass, const btVector3& inertia);
-	
+
+	void setMassProps(btScalar mass, const btVector3& inertia);
+
 	const btVector3& getLinearFactor() const
 	{
 		return m_linearFactor;
@@ -265,20 +258,22 @@ public:
 	void setLinearFactor(const btVector3& linearFactor)
 	{
 		m_linearFactor = linearFactor;
-		m_invMass = m_linearFactor*m_inverseMass;
+		m_invMass = m_linearFactor * m_inverseMass;
 	}
-	btScalar		getInvMass() const { return m_inverseMass; }
-	const btMatrix3x3& getInvInertiaTensorWorld() const { 
-		return m_invInertiaTensorWorld; 
+	btScalar getInvMass() const { return m_inverseMass; }
+	btScalar getMass() const { return m_inverseMass == btScalar(0.) ? btScalar(0.) : btScalar(1.0) / m_inverseMass; }
+	const btMatrix3x3& getInvInertiaTensorWorld() const
+	{
+		return m_invInertiaTensorWorld;
 	}
-		
-	void			integrateVelocities(btScalar step);
 
-	void			setCenterOfMassTransform(const btTransform& xform);
+	void integrateVelocities(btScalar step);
+
+	void setCenterOfMassTransform(const btTransform& xform);
 
-	void			applyCentralForce(const btVector3& force)
+	void applyCentralForce(const btVector3& force)
 	{
-		m_totalForce += force*m_linearFactor;
+		m_totalForce += force * m_linearFactor;
 	}
 
 	const btVector3& getTotalForce() const
@@ -290,90 +285,176 @@ public:
 	{
 		return m_totalTorque;
 	};
-    
+
 	const btVector3& getInvInertiaDiagLocal() const
 	{
 		return m_invInertiaLocal;
 	};
 
-	void	setInvInertiaDiagLocal(const btVector3& diagInvInertia)
+	void setInvInertiaDiagLocal(const btVector3& diagInvInertia)
 	{
 		m_invInertiaLocal = diagInvInertia;
 	}
 
-	void	setSleepingThresholds(btScalar linear,btScalar angular)
+	void setSleepingThresholds(btScalar linear, btScalar angular)
 	{
 		m_linearSleepingThreshold = linear;
 		m_angularSleepingThreshold = angular;
 	}
 
-	void	applyTorque(const btVector3& torque)
+	void applyTorque(const btVector3& torque)
 	{
-		m_totalTorque += torque*m_angularFactor;
+		m_totalTorque += torque * m_angularFactor;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_totalTorque);
+		#endif
 	}
-	
-	void	applyForce(const btVector3& force, const btVector3& rel_pos) 
+
+	void applyForce(const btVector3& force, const btVector3& rel_pos)
 	{
 		applyCentralForce(force);
-		applyTorque(rel_pos.cross(force*m_linearFactor));
+		applyTorque(rel_pos.cross(force * m_linearFactor));
 	}
-	
+
 	void applyCentralImpulse(const btVector3& impulse)
 	{
-		m_linearVelocity += impulse *m_linearFactor * m_inverseMass;
+		m_linearVelocity += impulse * m_linearFactor * m_inverseMass;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_linearVelocity);
+		#endif
 	}
-	
-  	void applyTorqueImpulse(const btVector3& torque)
+
+	void applyTorqueImpulse(const btVector3& torque)
 	{
-			m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+		m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_angularVelocity);
+		#endif
 	}
-	
-	void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) 
+
+	void applyImpulse(const btVector3& impulse, const btVector3& rel_pos)
 	{
 		if (m_inverseMass != btScalar(0.))
 		{
 			applyCentralImpulse(impulse);
 			if (m_angularFactor)
 			{
-				applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor));
+				applyTorqueImpulse(rel_pos.cross(impulse * m_linearFactor));
 			}
 		}
 	}
+    
+    void applyPushImpulse(const btVector3& impulse, const btVector3& rel_pos)
+    {
+        if (m_inverseMass != btScalar(0.))
+        {
+            applyCentralPushImpulse(impulse);
+            if (m_angularFactor)
+            {
+                applyTorqueTurnImpulse(rel_pos.cross(impulse * m_linearFactor));
+            }
+        }
+    }
+    
+    btVector3 getPushVelocity() const
+    {
+        return m_pushVelocity;
+    }
+    
+    btVector3 getTurnVelocity() const
+    {
+        return m_turnVelocity;
+    }
+    
+    void setPushVelocity(const btVector3& v)
+    {
+        m_pushVelocity = v;
+    }
+
+    #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+    void clampVelocity(btVector3& v) const {
+        v.setX(
+            fmax(-BT_CLAMP_VELOCITY_TO,
+                 fmin(BT_CLAMP_VELOCITY_TO, v.getX()))
+        );
+        v.setY(
+            fmax(-BT_CLAMP_VELOCITY_TO,
+                 fmin(BT_CLAMP_VELOCITY_TO, v.getY()))
+        );
+        v.setZ(
+            fmax(-BT_CLAMP_VELOCITY_TO,
+                 fmin(BT_CLAMP_VELOCITY_TO, v.getZ()))
+        );
+    }
+    #endif
+
+    void setTurnVelocity(const btVector3& v)
+    {
+        m_turnVelocity = v;
+        #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+        clampVelocity(m_turnVelocity);
+        #endif
+    }
+    
+    void applyCentralPushImpulse(const btVector3& impulse)
+    {
+        m_pushVelocity += impulse * m_linearFactor * m_inverseMass;
+        #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+        clampVelocity(m_pushVelocity);
+        #endif
+    }
+    
+    void applyTorqueTurnImpulse(const btVector3& torque)
+    {
+        m_turnVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+        #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+        clampVelocity(m_turnVelocity);
+        #endif
+    }
 
-	void clearForces() 
+	void clearForces()
 	{
 		m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 		m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	}
-	
-	void updateInertiaTensor();    
-	
-	const btVector3&     getCenterOfMassPosition() const { 
-		return m_worldTransform.getOrigin(); 
+
+	void updateInertiaTensor();
+
+	const btVector3& getCenterOfMassPosition() const
+	{
+		return m_worldTransform.getOrigin();
 	}
 	btQuaternion getOrientation() const;
-	
-	const btTransform&  getCenterOfMassTransform() const { 
-		return m_worldTransform; 
+
+	const btTransform& getCenterOfMassTransform() const
+	{
+		return m_worldTransform;
 	}
-	const btVector3&   getLinearVelocity() const { 
-		return m_linearVelocity; 
+	const btVector3& getLinearVelocity() const
+	{
+		return m_linearVelocity;
 	}
-	const btVector3&    getAngularVelocity() const { 
-		return m_angularVelocity; 
+	const btVector3& getAngularVelocity() const
+	{
+		return m_angularVelocity;
 	}
-	
 
 	inline void setLinearVelocity(const btVector3& lin_vel)
-	{ 
+	{
 		m_updateRevision++;
-		m_linearVelocity = lin_vel; 
+		m_linearVelocity = lin_vel;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_linearVelocity);
+		#endif
 	}
 
-	inline void setAngularVelocity(const btVector3& ang_vel) 
-	{ 
+	inline void setAngularVelocity(const btVector3& ang_vel)
+	{
 		m_updateRevision++;
-		m_angularVelocity = ang_vel; 
+		m_angularVelocity = ang_vel;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_angularVelocity);
+		#endif
 	}
 
 	btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const
@@ -384,19 +465,20 @@ public:
 		//for kinematic objects, we could also use use:
 		//		return 	(m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
 	}
+    
+    btVector3 getPushVelocityInLocalPoint(const btVector3& rel_pos) const
+    {
+        //we also calculate lin/ang velocity for kinematic objects
+        return m_pushVelocity + m_turnVelocity.cross(rel_pos);
+    }
 
-	void translate(const btVector3& v) 
+	void translate(const btVector3& v)
 	{
-		m_worldTransform.getOrigin() += v; 
+		m_worldTransform.getOrigin() += v;
 	}
 
-	
-	void	getAabb(btVector3& aabbMin,btVector3& aabbMax) const;
-
-
+	void getAabb(btVector3& aabbMin, btVector3& aabbMax) const;
 
-
-	
 	SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const
 	{
 		btVector3 r0 = pos - getCenterOfMassPosition();
@@ -406,7 +488,6 @@ public:
 		btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0);
 
 		return m_inverseMass + normal.dot(vec);
-
 	}
 
 	SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const
@@ -415,26 +496,25 @@ public:
 		return axis.dot(vec);
 	}
 
-	SIMD_FORCE_INLINE void	updateDeactivation(btScalar timeStep)
+	SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep)
 	{
-		if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
+		if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
 			return;
 
-		if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) &&
-			(getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold))
+		if ((getLinearVelocity().length2() < m_linearSleepingThreshold * m_linearSleepingThreshold) &&
+			(getAngularVelocity().length2() < m_angularSleepingThreshold * m_angularSleepingThreshold))
 		{
 			m_deactivationTime += timeStep;
-		} else
+		}
+		else
 		{
-			m_deactivationTime=btScalar(0.);
+			m_deactivationTime = btScalar(0.);
 			setActivationState(0);
 		}
-
 	}
 
-	SIMD_FORCE_INLINE bool	wantsSleeping()
+	SIMD_FORCE_INLINE bool wantsSleeping()
 	{
-
 		if (getActivationState() == DISABLE_DEACTIVATION)
 			return false;
 
@@ -442,41 +522,39 @@ public:
 		if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
 			return false;
 
-		if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
+		if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
 			return true;
 
-		if (m_deactivationTime> gDeactivationTime)
+		if (m_deactivationTime > gDeactivationTime)
 		{
 			return true;
 		}
 		return false;
 	}
 
-
-	
-	const btBroadphaseProxy*	getBroadphaseProxy() const
+	const btBroadphaseProxy* getBroadphaseProxy() const
 	{
 		return m_broadphaseHandle;
 	}
-	btBroadphaseProxy*	getBroadphaseProxy() 
+	btBroadphaseProxy* getBroadphaseProxy()
 	{
 		return m_broadphaseHandle;
 	}
-	void	setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy)
+	void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy)
 	{
 		m_broadphaseHandle = broadphaseProxy;
 	}
 
 	//btMotionState allows to automatic synchronize the world transform for active objects
-	btMotionState*	getMotionState()
+	btMotionState* getMotionState()
 	{
 		return m_optionalMotionState;
 	}
-	const btMotionState*	getMotionState() const
+	const btMotionState* getMotionState() const
 	{
 		return m_optionalMotionState;
 	}
-	void	setMotionState(btMotionState* motionState)
+	void setMotionState(btMotionState* motionState)
 	{
 		m_optionalMotionState = motionState;
 		if (m_optionalMotionState)
@@ -484,33 +562,31 @@ public:
 	}
 
 	//for experimental overriding of friction/contact solver func
-	int	m_contactSolverType;
-	int	m_frictionSolverType;
+	int m_contactSolverType;
+	int m_frictionSolverType;
 
-	void	setAngularFactor(const btVector3& angFac)
+	void setAngularFactor(const btVector3& angFac)
 	{
 		m_updateRevision++;
 		m_angularFactor = angFac;
 	}
 
-	void	setAngularFactor(btScalar angFac)
+	void setAngularFactor(btScalar angFac)
 	{
 		m_updateRevision++;
-		m_angularFactor.setValue(angFac,angFac,angFac);
+		m_angularFactor.setValue(angFac, angFac, angFac);
 	}
-	const btVector3&	getAngularFactor() const
+	const btVector3& getAngularFactor() const
 	{
 		return m_angularFactor;
 	}
 
 	//is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase?
-	bool	isInWorld() const
+	bool isInWorld() const
 	{
 		return (getBroadphaseProxy() != 0);
 	}
 
-	virtual bool checkCollideWithOverride(const  btCollisionObject* co) const;
-
 	void addConstraintRef(btTypedConstraint* c);
 	void removeConstraintRef(btTypedConstraint* c);
 
@@ -524,7 +600,7 @@ public:
 		return m_constraintRefs.size();
 	}
 
-	void	setFlags(int flags)
+	void setFlags(int flags)
 	{
 		m_rigidbodyFlags = flags;
 	}
@@ -534,12 +610,9 @@ public:
 		return m_rigidbodyFlags;
 	}
 
-
-	
-
 	///perform implicit force computation in world space
 	btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const;
-	
+
 	///perform implicit force computation in body space (inertial frame)
 	btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const;
 
@@ -549,70 +622,66 @@ public:
 
 	///////////////////////////////////////////////
 
-	virtual	int	calculateSerializeBufferSize()	const;
+	virtual int calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer,  class btSerializer* serializer) const;
+	virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
 
 	virtual void serializeSingleObject(class btSerializer* serializer) const;
-
 };
 
 //@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btRigidBodyFloatData
+struct btRigidBodyFloatData
 {
-	btCollisionObjectFloatData	m_collisionObjectData;
-	btMatrix3x3FloatData		m_invInertiaTensorWorld;
-	btVector3FloatData		m_linearVelocity;
-	btVector3FloatData		m_angularVelocity;
-	btVector3FloatData		m_angularFactor;
-	btVector3FloatData		m_linearFactor;
-	btVector3FloatData		m_gravity;	
-	btVector3FloatData		m_gravity_acceleration;
-	btVector3FloatData		m_invInertiaLocal;
-	btVector3FloatData		m_totalForce;
-	btVector3FloatData		m_totalTorque;
-	float					m_inverseMass;
-	float					m_linearDamping;
-	float					m_angularDamping;
-	float					m_additionalDampingFactor;
-	float					m_additionalLinearDampingThresholdSqr;
-	float					m_additionalAngularDampingThresholdSqr;
-	float					m_additionalAngularDampingFactor;
-	float					m_linearSleepingThreshold;
-	float					m_angularSleepingThreshold;
-	int						m_additionalDamping;
+	btCollisionObjectFloatData m_collisionObjectData;
+	btMatrix3x3FloatData m_invInertiaTensorWorld;
+	btVector3FloatData m_linearVelocity;
+	btVector3FloatData m_angularVelocity;
+	btVector3FloatData m_angularFactor;
+	btVector3FloatData m_linearFactor;
+	btVector3FloatData m_gravity;
+	btVector3FloatData m_gravity_acceleration;
+	btVector3FloatData m_invInertiaLocal;
+	btVector3FloatData m_totalForce;
+	btVector3FloatData m_totalTorque;
+	float m_inverseMass;
+	float m_linearDamping;
+	float m_angularDamping;
+	float m_additionalDampingFactor;
+	float m_additionalLinearDampingThresholdSqr;
+	float m_additionalAngularDampingThresholdSqr;
+	float m_additionalAngularDampingFactor;
+	float m_linearSleepingThreshold;
+	float m_angularSleepingThreshold;
+	int m_additionalDamping;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btRigidBodyDoubleData
+struct btRigidBodyDoubleData
 {
-	btCollisionObjectDoubleData	m_collisionObjectData;
-	btMatrix3x3DoubleData		m_invInertiaTensorWorld;
-	btVector3DoubleData		m_linearVelocity;
-	btVector3DoubleData		m_angularVelocity;
-	btVector3DoubleData		m_angularFactor;
-	btVector3DoubleData		m_linearFactor;
-	btVector3DoubleData		m_gravity;	
-	btVector3DoubleData		m_gravity_acceleration;
-	btVector3DoubleData		m_invInertiaLocal;
-	btVector3DoubleData		m_totalForce;
-	btVector3DoubleData		m_totalTorque;
-	double					m_inverseMass;
-	double					m_linearDamping;
-	double					m_angularDamping;
-	double					m_additionalDampingFactor;
-	double					m_additionalLinearDampingThresholdSqr;
-	double					m_additionalAngularDampingThresholdSqr;
-	double					m_additionalAngularDampingFactor;
-	double					m_linearSleepingThreshold;
-	double					m_angularSleepingThreshold;
-	int						m_additionalDamping;
-	char	m_padding[4];
+	btCollisionObjectDoubleData m_collisionObjectData;
+	btMatrix3x3DoubleData m_invInertiaTensorWorld;
+	btVector3DoubleData m_linearVelocity;
+	btVector3DoubleData m_angularVelocity;
+	btVector3DoubleData m_angularFactor;
+	btVector3DoubleData m_linearFactor;
+	btVector3DoubleData m_gravity;
+	btVector3DoubleData m_gravity_acceleration;
+	btVector3DoubleData m_invInertiaLocal;
+	btVector3DoubleData m_totalForce;
+	btVector3DoubleData m_totalTorque;
+	double m_inverseMass;
+	double m_linearDamping;
+	double m_angularDamping;
+	double m_additionalDampingFactor;
+	double m_additionalLinearDampingThresholdSqr;
+	double m_additionalAngularDampingThresholdSqr;
+	double m_additionalAngularDampingFactor;
+	double m_linearSleepingThreshold;
+	double m_angularSleepingThreshold;
+	int m_additionalDamping;
+	char m_padding[4];
 };
 
-
-
-#endif //BT_RIGIDBODY_H
-
+#endif  //BT_RIGIDBODY_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
index 35dd38840f6..8103390fb1d 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
@@ -21,47 +21,40 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 
-
 /*
   Make sure this dummy function never changes so that it
   can be used by probes that are checking whether the
   library is actually installed.
 */
-extern "C" 
+extern "C"
 {
-	void btBulletDynamicsProbe ();
-	void btBulletDynamicsProbe () {}
+	void btBulletDynamicsProbe();
+	void btBulletDynamicsProbe() {}
 }
 
-
-
-
-btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
-:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
-m_constraintSolver(constraintSolver),
-m_ownsConstraintSolver(false),
-m_gravity(0,0,-10)
+btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+	: btDynamicsWorld(dispatcher, pairCache, collisionConfiguration),
+	  m_constraintSolver(constraintSolver),
+	  m_ownsConstraintSolver(false),
+	  m_gravity(0, 0, -10)
 {
-
 }
 
-
 btSimpleDynamicsWorld::~btSimpleDynamicsWorld()
 {
 	if (m_ownsConstraintSolver)
-		btAlignedFree( m_constraintSolver);
+		btAlignedFree(m_constraintSolver);
 }
 
-int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
+int btSimpleDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
 {
 	(void)fixedTimeStep;
 	(void)maxSubSteps;
 
-
 	///apply gravity, predict motion
 	predictUnconstraintMotion(timeStep);
 
-	btDispatcherInfo&	dispatchInfo = getDispatchInfo();
+	btDispatcherInfo& dispatchInfo = getDispatchInfo();
 	dispatchInfo.m_timeStep = timeStep;
 	dispatchInfo.m_stepCount = 0;
 	dispatchInfo.m_debugDraw = getDebugDrawer();
@@ -74,17 +67,17 @@ int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
 	if (numManifolds)
 	{
 		btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer();
-		
+
 		btContactSolverInfo infoGlobal;
 		infoGlobal.m_timeStep = timeStep;
-		m_constraintSolver->prepareSolve(0,numManifolds);
-		m_constraintSolver->solveGroup(&getCollisionObjectArray()[0],getNumCollisionObjects(),manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_dispatcher1);
-		m_constraintSolver->allSolved(infoGlobal,m_debugDrawer);
+		m_constraintSolver->prepareSolve(0, numManifolds);
+		m_constraintSolver->solveGroup(&getCollisionObjectArray()[0], getNumCollisionObjects(), manifoldPtr, numManifolds, 0, 0, infoGlobal, m_debugDrawer, m_dispatcher1);
+		m_constraintSolver->allSolved(infoGlobal, m_debugDrawer);
 	}
 
 	///integrate transforms
 	integrateTransforms(timeStep);
-		
+
 	updateAabbs();
 
 	synchronizeMotionStates();
@@ -92,29 +85,27 @@ int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
 	clearForces();
 
 	return 1;
-
 }
 
-void	btSimpleDynamicsWorld::clearForces()
+void btSimpleDynamicsWorld::clearForces()
 {
 	///@todo: iterate over awake simulation islands!
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
-		
+
 		btRigidBody* body = btRigidBody::upcast(colObj);
 		if (body)
 		{
 			body->clearForces();
 		}
 	}
-}	
-
+}
 
-void	btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
+void btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
 {
 	m_gravity = gravity;
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -125,17 +116,17 @@ void	btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
 	}
 }
 
-btVector3 btSimpleDynamicsWorld::getGravity () const
+btVector3 btSimpleDynamicsWorld::getGravity() const
 {
 	return m_gravity;
 }
 
-void	btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
+void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
 {
 	btCollisionWorld::removeCollisionObject(body);
 }
 
-void	btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
 {
 	btRigidBody* body = btRigidBody::upcast(collisionObject);
 	if (body)
@@ -144,8 +135,7 @@ void	btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionOb
 		btCollisionWorld::removeCollisionObject(collisionObject);
 }
 
-
-void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
+void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
 {
 	body->setGravity(m_gravity);
 
@@ -155,37 +145,32 @@ void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
 	}
 }
 
-void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
+void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask)
 {
 	body->setGravity(m_gravity);
 
 	if (body->getCollisionShape())
 	{
-		addCollisionObject(body,group,mask);
+		addCollisionObject(body, group, mask);
 	}
 }
 
-
-void	btSimpleDynamicsWorld::debugDrawWorld()
+void btSimpleDynamicsWorld::debugDrawWorld()
 {
-
 }
-				
-void	btSimpleDynamicsWorld::addAction(btActionInterface* action)
-{
 
+void btSimpleDynamicsWorld::addAction(btActionInterface* action)
+{
 }
 
-void	btSimpleDynamicsWorld::removeAction(btActionInterface* action)
+void btSimpleDynamicsWorld::removeAction(btActionInterface* action)
 {
-
 }
 
-
-void	btSimpleDynamicsWorld::updateAabbs()
+void btSimpleDynamicsWorld::updateAabbs()
 {
 	btTransform predictedTrans;
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -193,19 +178,19 @@ void	btSimpleDynamicsWorld::updateAabbs()
 		{
 			if (body->isActive() && (!body->isStaticObject()))
 			{
-				btVector3 minAabb,maxAabb;
-				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+				btVector3 minAabb, maxAabb;
+				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb);
 				btBroadphaseInterface* bp = getBroadphase();
-				bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
+				bp->setAabb(body->getBroadphaseHandle(), minAabb, maxAabb, m_dispatcher1);
 			}
 		}
 	}
 }
 
-void	btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
+void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
 	btTransform predictedTrans;
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -214,17 +199,15 @@ void	btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
 			if (body->isActive() && (!body->isStaticObject()))
 			{
 				body->predictIntegratedTransform(timeStep, predictedTrans);
-				body->proceedToTransform( predictedTrans);
+				body->proceedToTransform(predictedTrans);
 			}
 		}
 	}
 }
 
-
-
-void	btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -235,20 +218,19 @@ void	btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 				if (body->isActive())
 				{
 					body->applyGravity();
-					body->integrateVelocities( timeStep);
+					body->integrateVelocities(timeStep);
 					body->applyDamping(timeStep);
-					body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
+					body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform());
 				}
 			}
 		}
 	}
 }
 
-
-void	btSimpleDynamicsWorld::synchronizeMotionStates()
+void btSimpleDynamicsWorld::synchronizeMotionStates()
 {
 	///@todo: iterate over awake simulation islands!
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -260,11 +242,9 @@ void	btSimpleDynamicsWorld::synchronizeMotionStates()
 			}
 		}
 	}
-
 }
 
-
-void	btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
+void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 {
 	if (m_ownsConstraintSolver)
 	{
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
index d48d2e39c4d..12be231c7f1 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
@@ -27,63 +27,58 @@ class btConstraintSolver;
 class btSimpleDynamicsWorld : public btDynamicsWorld
 {
 protected:
+	btConstraintSolver* m_constraintSolver;
 
-	btConstraintSolver*	m_constraintSolver;
+	bool m_ownsConstraintSolver;
 
-	bool	m_ownsConstraintSolver;
-
-	void	predictUnconstraintMotion(btScalar timeStep);
-	
-	void	integrateTransforms(btScalar timeStep);
-		
-	btVector3	m_gravity;
-	
-public:
+	void predictUnconstraintMotion(btScalar timeStep);
 
+	void integrateTransforms(btScalar timeStep);
 
+	btVector3 m_gravity;
 
+public:
 	///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver
-	btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+	btSimpleDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration);
 
 	virtual ~btSimpleDynamicsWorld();
-		
+
 	///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead
-	virtual int	stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
+	virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
 
-	virtual void	setGravity(const btVector3& gravity);
+	virtual void setGravity(const btVector3& gravity);
 
-	virtual btVector3 getGravity () const;
+	virtual btVector3 getGravity() const;
 
-	virtual void	addRigidBody(btRigidBody* body);
+	virtual void addRigidBody(btRigidBody* body);
 
-	virtual void	addRigidBody(btRigidBody* body, short group, short mask);
+	virtual void addRigidBody(btRigidBody* body, int group, int mask);
 
-	virtual void	removeRigidBody(btRigidBody* body);
+	virtual void removeRigidBody(btRigidBody* body);
 
-	virtual void	debugDrawWorld();
-				
-	virtual void	addAction(btActionInterface* action);
+	virtual void debugDrawWorld();
 
-	virtual void	removeAction(btActionInterface* action);
+	virtual void addAction(btActionInterface* action);
+
+	virtual void removeAction(btActionInterface* action);
 
 	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
-	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
-	
-	virtual void	updateAabbs();
+	virtual void removeCollisionObject(btCollisionObject* collisionObject);
+
+	virtual void updateAabbs();
 
-	virtual void	synchronizeMotionStates();
+	virtual void synchronizeMotionStates();
 
-	virtual void	setConstraintSolver(btConstraintSolver* solver);
+	virtual void setConstraintSolver(btConstraintSolver* solver);
 
 	virtual btConstraintSolver* getConstraintSolver();
 
-	virtual btDynamicsWorldType	getWorldType() const
+	virtual btDynamicsWorldType getWorldType() const
 	{
 		return BT_SIMPLE_DYNAMICS_WORLD;
 	}
 
-	virtual void	clearForces();
-
+	virtual void clearForces();
 };
 
-#endif //BT_SIMPLE_DYNAMICS_WORLD_H
+#endif  //BT_SIMPLE_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp
new file mode 100644
index 00000000000..772b7742023
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp
@@ -0,0 +1,696 @@
+/*
+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 "LinearMath/btScalar.h"
+#include "LinearMath/btThreads.h"
+#include "btSimulationIslandManagerMt.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h"  // for s_minimumContactManifoldsForBatching
+
+//#include <stdio.h>
+#include "LinearMath/btQuickprof.h"
+
+SIMD_FORCE_INLINE int calcBatchCost(int bodies, int manifolds, int constraints)
+{
+	// rough estimate of the cost of a batch, used for merging
+	int batchCost = bodies + 8 * manifolds + 4 * constraints;
+	return batchCost;
+}
+
+SIMD_FORCE_INLINE int calcBatchCost(const btSimulationIslandManagerMt::Island* island)
+{
+	return calcBatchCost(island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size());
+}
+
+btSimulationIslandManagerMt::btSimulationIslandManagerMt()
+{
+	m_minimumSolverBatchSize = calcBatchCost(0, 128, 0);
+	m_batchIslandMinBodyCount = 32;
+	m_islandDispatch = parallelIslandDispatch;
+	m_batchIsland = NULL;
+}
+
+btSimulationIslandManagerMt::~btSimulationIslandManagerMt()
+{
+	for (int i = 0; i < m_allocatedIslands.size(); ++i)
+	{
+		delete m_allocatedIslands[i];
+	}
+	m_allocatedIslands.resize(0);
+	m_activeIslands.resize(0);
+	m_freeIslands.resize(0);
+}
+
+inline int getIslandId(const btPersistentManifold* lhs)
+{
+	const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
+	const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
+	int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag();
+	return islandId;
+}
+
+SIMD_FORCE_INLINE int btGetConstraintIslandId1(const btTypedConstraint* lhs)
+{
+	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
+	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
+	int islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
+	return islandId;
+}
+
+/// function object that routes calls to operator<
+class IslandBatchSizeSortPredicate
+{
+public:
+	bool operator()(const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs) const
+	{
+		int lCost = calcBatchCost(lhs);
+		int rCost = calcBatchCost(rhs);
+		return lCost > rCost;
+	}
+};
+
+class IslandBodyCapacitySortPredicate
+{
+public:
+	bool operator()(const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs) const
+	{
+		return lhs->bodyArray.capacity() > rhs->bodyArray.capacity();
+	}
+};
+
+void btSimulationIslandManagerMt::Island::append(const Island& other)
+{
+	// append bodies
+	for (int i = 0; i < other.bodyArray.size(); ++i)
+	{
+		bodyArray.push_back(other.bodyArray[i]);
+	}
+	// append manifolds
+	for (int i = 0; i < other.manifoldArray.size(); ++i)
+	{
+		manifoldArray.push_back(other.manifoldArray[i]);
+	}
+	// append constraints
+	for (int i = 0; i < other.constraintArray.size(); ++i)
+	{
+		constraintArray.push_back(other.constraintArray[i]);
+	}
+}
+
+bool btIsBodyInIsland(const btSimulationIslandManagerMt::Island& island, const btCollisionObject* obj)
+{
+	for (int i = 0; i < island.bodyArray.size(); ++i)
+	{
+		if (island.bodyArray[i] == obj)
+		{
+			return true;
+		}
+	}
+	return false;
+}
+
+void btSimulationIslandManagerMt::initIslandPools()
+{
+	// reset island pools
+	int numElem = getUnionFind().getNumElements();
+	m_lookupIslandFromId.resize(numElem);
+	for (int i = 0; i < m_lookupIslandFromId.size(); ++i)
+	{
+		m_lookupIslandFromId[i] = NULL;
+	}
+	m_activeIslands.resize(0);
+	m_freeIslands.resize(0);
+	// check whether allocated islands are sorted by body capacity (largest to smallest)
+	int lastCapacity = 0;
+	bool isSorted = true;
+	for (int i = 0; i < m_allocatedIslands.size(); ++i)
+	{
+		Island* island = m_allocatedIslands[i];
+		int cap = island->bodyArray.capacity();
+		if (cap > lastCapacity)
+		{
+			isSorted = false;
+			break;
+		}
+		lastCapacity = cap;
+	}
+	if (!isSorted)
+	{
+		m_allocatedIslands.quickSort(IslandBodyCapacitySortPredicate());
+	}
+
+	m_batchIsland = NULL;
+	// mark all islands free (but avoid deallocation)
+	for (int i = 0; i < m_allocatedIslands.size(); ++i)
+	{
+		Island* island = m_allocatedIslands[i];
+		island->bodyArray.resize(0);
+		island->manifoldArray.resize(0);
+		island->constraintArray.resize(0);
+		island->id = -1;
+		island->isSleeping = true;
+		m_freeIslands.push_back(island);
+	}
+}
+
+btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland(int id)
+{
+	btAssert(id >= 0);
+	btAssert(id < m_lookupIslandFromId.size());
+	Island* island = m_lookupIslandFromId[id];
+	if (island == NULL)
+	{
+		// search for existing island
+		for (int i = 0; i < m_activeIslands.size(); ++i)
+		{
+			if (m_activeIslands[i]->id == id)
+			{
+				island = m_activeIslands[i];
+				break;
+			}
+		}
+		m_lookupIslandFromId[id] = island;
+	}
+	return island;
+}
+
+btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::allocateIsland(int id, int numBodies)
+{
+	Island* island = NULL;
+	int allocSize = numBodies;
+	if (numBodies < m_batchIslandMinBodyCount)
+	{
+		if (m_batchIsland)
+		{
+			island = m_batchIsland;
+			m_lookupIslandFromId[id] = island;
+			// if we've made a large enough batch,
+			if (island->bodyArray.size() + numBodies >= m_batchIslandMinBodyCount)
+			{
+				// next time start a new batch
+				m_batchIsland = NULL;
+			}
+			return island;
+		}
+		else
+		{
+			// need to allocate a batch island
+			allocSize = m_batchIslandMinBodyCount * 2;
+		}
+	}
+	btAlignedObjectArray<Island*>& freeIslands = m_freeIslands;
+
+	// search for free island
+	if (freeIslands.size() > 0)
+	{
+		// try to reuse a previously allocated island
+		int iFound = freeIslands.size();
+		// linear search for smallest island that can hold our bodies
+		for (int i = freeIslands.size() - 1; i >= 0; --i)
+		{
+			if (freeIslands[i]->bodyArray.capacity() >= allocSize)
+			{
+				iFound = i;
+				island = freeIslands[i];
+				island->id = id;
+				break;
+			}
+		}
+		// if found, shrink array while maintaining ordering
+		if (island)
+		{
+			int iDest = iFound;
+			int iSrc = iDest + 1;
+			while (iSrc < freeIslands.size())
+			{
+				freeIslands[iDest++] = freeIslands[iSrc++];
+			}
+			freeIslands.pop_back();
+		}
+	}
+	if (island == NULL)
+	{
+		// no free island found, allocate
+		island = new Island();  // TODO: change this to use the pool allocator
+		island->id = id;
+		island->bodyArray.reserve(allocSize);
+		m_allocatedIslands.push_back(island);
+	}
+	m_lookupIslandFromId[id] = island;
+	if (numBodies < m_batchIslandMinBodyCount)
+	{
+		m_batchIsland = island;
+	}
+	m_activeIslands.push_back(island);
+	return island;
+}
+
+void btSimulationIslandManagerMt::buildIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld)
+{
+	BT_PROFILE("buildIslands");
+
+	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+
+	//we are going to sort the unionfind array, and store the element id in the size
+	//afterwards, we clean unionfind, to make sure no-one uses it anymore
+
+	getUnionFind().sortIslands();
+	int numElem = getUnionFind().getNumElements();
+
+	int endIslandIndex = 1;
+	int startIslandIndex;
+
+	//update the sleeping state for bodies, if all are sleeping
+	for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
+	{
+		int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+		for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
+		{
+		}
+
+		//int numSleeping = 0;
+
+		bool allSleeping = true;
+
+		int idx;
+		for (idx = startIslandIndex; idx < endIslandIndex; idx++)
+		{
+			int i = getUnionFind().getElement(idx).m_sz;
+
+			btCollisionObject* colObj0 = collisionObjects[i];
+			if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+			{
+				//				printf("error in island management\n");
+			}
+
+			btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+			if (colObj0->getIslandTag() == islandId)
+			{
+				if (colObj0->getActivationState() == ACTIVE_TAG ||
+					colObj0->getActivationState() == DISABLE_DEACTIVATION)
+				{
+					allSleeping = false;
+					break;
+				}
+			}
+		}
+
+		if (allSleeping)
+		{
+			int idx;
+			for (idx = startIslandIndex; idx < endIslandIndex; idx++)
+			{
+				int i = getUnionFind().getElement(idx).m_sz;
+				btCollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+				{
+					//					printf("error in island management\n");
+				}
+
+				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+				if (colObj0->getIslandTag() == islandId)
+				{
+					colObj0->setActivationState(ISLAND_SLEEPING);
+				}
+			}
+		}
+		else
+		{
+			int idx;
+			for (idx = startIslandIndex; idx < endIslandIndex; idx++)
+			{
+				int i = getUnionFind().getElement(idx).m_sz;
+
+				btCollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+				{
+					//					printf("error in island management\n");
+				}
+
+				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+				if (colObj0->getIslandTag() == islandId)
+				{
+					if (colObj0->getActivationState() == ISLAND_SLEEPING)
+					{
+						colObj0->setActivationState(WANTS_DEACTIVATION);
+						colObj0->setDeactivationTime(0.f);
+					}
+				}
+			}
+		}
+	}
+}
+
+void btSimulationIslandManagerMt::addBodiesToIslands(btCollisionWorld* collisionWorld)
+{
+	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+	int endIslandIndex = 1;
+	int startIslandIndex;
+	int numElem = getUnionFind().getNumElements();
+
+	// create explicit islands and add bodies to each
+	for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
+	{
+		int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+
+		// find end index
+		for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
+		{
+		}
+		// check if island is sleeping
+		bool islandSleeping = true;
+		for (int iElem = startIslandIndex; iElem < endIslandIndex; iElem++)
+		{
+			int i = getUnionFind().getElement(iElem).m_sz;
+			btCollisionObject* colObj = collisionObjects[i];
+			if (colObj->isActive())
+			{
+				islandSleeping = false;
+			}
+		}
+		if (!islandSleeping)
+		{
+			// want to count the number of bodies before allocating the island to optimize memory usage of the Island structures
+			int numBodies = endIslandIndex - startIslandIndex;
+			Island* island = allocateIsland(islandId, numBodies);
+			island->isSleeping = false;
+
+			// add bodies to island
+			for (int iElem = startIslandIndex; iElem < endIslandIndex; iElem++)
+			{
+				int i = getUnionFind().getElement(iElem).m_sz;
+				btCollisionObject* colObj = collisionObjects[i];
+				island->bodyArray.push_back(colObj);
+			}
+		}
+	}
+}
+
+void btSimulationIslandManagerMt::addManifoldsToIslands(btDispatcher* dispatcher)
+{
+	// walk all the manifolds, activating bodies touched by kinematic objects, and add each manifold to its Island
+	int maxNumManifolds = dispatcher->getNumManifolds();
+	for (int i = 0; i < maxNumManifolds; i++)
+	{
+		btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
+
+		const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
+		const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
+
+		///@todo: check sleeping conditions!
+		if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
+			((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
+		{
+			//kinematic objects don't merge islands, but wake up all connected objects
+			if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
+			{
+				if (colObj0->hasContactResponse())
+					colObj1->activate();
+			}
+			if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
+			{
+				if (colObj1->hasContactResponse())
+					colObj0->activate();
+			}
+			//filtering for response
+			if (dispatcher->needsResponse(colObj0, colObj1))
+			{
+				// scatter manifolds into various islands
+				int islandId = getIslandId(manifold);
+				// if island not sleeping,
+				if (Island* island = getIsland(islandId))
+				{
+					island->manifoldArray.push_back(manifold);
+				}
+			}
+		}
+	}
+}
+
+void btSimulationIslandManagerMt::addConstraintsToIslands(btAlignedObjectArray<btTypedConstraint*>& constraints)
+{
+	// walk constraints
+	for (int i = 0; i < constraints.size(); i++)
+	{
+		// scatter constraints into various islands
+		btTypedConstraint* constraint = constraints[i];
+		if (constraint->isEnabled())
+		{
+			int islandId = btGetConstraintIslandId1(constraint);
+			// if island is not sleeping,
+			if (Island* island = getIsland(islandId))
+			{
+				island->constraintArray.push_back(constraint);
+			}
+		}
+	}
+}
+
+void btSimulationIslandManagerMt::mergeIslands()
+{
+	// sort islands in order of decreasing batch size
+	m_activeIslands.quickSort(IslandBatchSizeSortPredicate());
+
+	// merge small islands to satisfy minimum batch size
+	// find first small batch island
+	int destIslandIndex = m_activeIslands.size();
+	for (int i = 0; i < m_activeIslands.size(); ++i)
+	{
+		Island* island = m_activeIslands[i];
+		int batchSize = calcBatchCost(island);
+		if (batchSize < m_minimumSolverBatchSize)
+		{
+			destIslandIndex = i;
+			break;
+		}
+	}
+	int lastIndex = m_activeIslands.size() - 1;
+	while (destIslandIndex < lastIndex)
+	{
+		// merge islands from the back of the list
+		Island* island = m_activeIslands[destIslandIndex];
+		int numBodies = island->bodyArray.size();
+		int numManifolds = island->manifoldArray.size();
+		int numConstraints = island->constraintArray.size();
+		int firstIndex = lastIndex;
+		// figure out how many islands we want to merge and find out how many bodies, manifolds and constraints we will have
+		while (true)
+		{
+			Island* src = m_activeIslands[firstIndex];
+			numBodies += src->bodyArray.size();
+			numManifolds += src->manifoldArray.size();
+			numConstraints += src->constraintArray.size();
+			int batchCost = calcBatchCost(numBodies, numManifolds, numConstraints);
+			if (batchCost >= m_minimumSolverBatchSize)
+			{
+				break;
+			}
+			if (firstIndex - 1 == destIslandIndex)
+			{
+				break;
+			}
+			firstIndex--;
+		}
+		// reserve space for these pointers to minimize reallocation
+		island->bodyArray.reserve(numBodies);
+		island->manifoldArray.reserve(numManifolds);
+		island->constraintArray.reserve(numConstraints);
+		// merge islands
+		for (int i = firstIndex; i <= lastIndex; ++i)
+		{
+			island->append(*m_activeIslands[i]);
+		}
+		// shrink array to exclude the islands that were merged from
+		m_activeIslands.resize(firstIndex);
+		lastIndex = firstIndex - 1;
+		destIslandIndex++;
+	}
+}
+
+void btSimulationIslandManagerMt::solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams)
+{
+	btPersistentManifold** manifolds = island.manifoldArray.size() ? &island.manifoldArray[0] : NULL;
+	btTypedConstraint** constraintsPtr = island.constraintArray.size() ? &island.constraintArray[0] : NULL;
+	solver->solveGroup(&island.bodyArray[0],
+					   island.bodyArray.size(),
+					   manifolds,
+					   island.manifoldArray.size(),
+					   constraintsPtr,
+					   island.constraintArray.size(),
+					   *solverParams.m_solverInfo,
+					   solverParams.m_debugDrawer,
+					   solverParams.m_dispatcher);
+}
+
+void btSimulationIslandManagerMt::serialIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams)
+{
+	BT_PROFILE("serialIslandDispatch");
+	// serial dispatch
+	btAlignedObjectArray<Island*>& islands = *islandsPtr;
+	btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool;
+	for (int i = 0; i < islands.size(); ++i)
+	{
+		solveIsland(solver, *islands[i], solverParams);
+	}
+}
+
+struct UpdateIslandDispatcher : public btIParallelForBody
+{
+	btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& m_islandsPtr;
+	const btSimulationIslandManagerMt::SolverParams& m_solverParams;
+
+	UpdateIslandDispatcher(btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams)
+		: m_islandsPtr(islandsPtr), m_solverParams(solverParams)
+	{
+	}
+
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		btConstraintSolver* solver = m_solverParams.m_solverPool;
+		for (int i = iBegin; i < iEnd; ++i)
+		{
+			btSimulationIslandManagerMt::Island* island = m_islandsPtr[i];
+			btSimulationIslandManagerMt::solveIsland(solver, *island, m_solverParams);
+		}
+	}
+};
+
+void btSimulationIslandManagerMt::parallelIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams)
+{
+	BT_PROFILE("parallelIslandDispatch");
+	//
+	// if there are islands with many contacts, it may be faster to submit these
+	// large islands *serially* to a single parallel constraint solver, and then later
+	// submit the remaining smaller islands in parallel to multiple sequential solvers.
+	//
+	// Some task schedulers do not deal well with nested parallelFor loops. One implementation
+	// of OpenMP was actually slower than doing everything single-threaded. Intel TBB
+	// on the other hand, seems to do a pretty respectable job with it.
+	//
+	// When solving islands in parallel, the worst case performance happens when there
+	// is one very large island and then perhaps a smattering of very small
+	// islands -- one worker thread takes the large island and the remaining workers
+	// tear through the smaller islands and then sit idle waiting for the first worker
+	// to finish. Solving islands in parallel works best when there are numerous small
+	// islands, roughly equal in size.
+	//
+	// By contrast, the other approach -- the parallel constraint solver -- is only
+	// able to deliver a worthwhile speedup when the island is large. For smaller islands,
+	// it is difficult to extract a useful amount of parallelism -- the overhead of grouping
+	// the constraints into batches and sending the batches to worker threads can nullify
+	// any gains from parallelism.
+	//
+
+	UpdateIslandDispatcher dispatcher(*islandsPtr, solverParams);
+	// We take advantage of the fact the islands are sorted in order of decreasing size
+	int iBegin = 0;
+	if (solverParams.m_solverMt)
+	{
+		while (iBegin < islandsPtr->size())
+		{
+			btSimulationIslandManagerMt::Island* island = (*islandsPtr)[iBegin];
+			if (island->manifoldArray.size() < btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching)
+			{
+				// OK to submit the rest of the array in parallel
+				break;
+			}
+			// serial dispatch to parallel solver for large islands (if any)
+			solveIsland(solverParams.m_solverMt, *island, solverParams);
+			++iBegin;
+		}
+	}
+	// parallel dispatch to sequential solvers for rest
+	btParallelFor(iBegin, islandsPtr->size(), 1, dispatcher);
+}
+
+///@todo: this is random access, it can be walked 'cache friendly'!
+void btSimulationIslandManagerMt::buildAndProcessIslands(btDispatcher* dispatcher,
+														 btCollisionWorld* collisionWorld,
+														 btAlignedObjectArray<btTypedConstraint*>& constraints,
+														 const SolverParams& solverParams)
+{
+	BT_PROFILE("buildAndProcessIslands");
+	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+
+	buildIslands(dispatcher, collisionWorld);
+
+	if (!getSplitIslands())
+	{
+		btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer();
+		int maxNumManifolds = dispatcher->getNumManifolds();
+
+		for (int i = 0; i < maxNumManifolds; i++)
+		{
+			btPersistentManifold* manifold = manifolds[i];
+
+			const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
+			const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
+
+			///@todo: check sleeping conditions!
+			if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
+				((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
+			{
+				//kinematic objects don't merge islands, but wake up all connected objects
+				if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
+				{
+					if (colObj0->hasContactResponse())
+						colObj1->activate();
+				}
+				if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
+				{
+					if (colObj1->hasContactResponse())
+						colObj0->activate();
+				}
+			}
+		}
+		btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[0] : NULL;
+		btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool;
+		solver->solveGroup(&collisionObjects[0],
+						   collisionObjects.size(),
+						   manifolds,
+						   maxNumManifolds,
+						   constraintsPtr,
+						   constraints.size(),
+						   *solverParams.m_solverInfo,
+						   solverParams.m_debugDrawer,
+						   solverParams.m_dispatcher);
+	}
+	else
+	{
+		initIslandPools();
+
+		//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
+		addBodiesToIslands(collisionWorld);
+		addManifoldsToIslands(dispatcher);
+		addConstraintsToIslands(constraints);
+
+		// m_activeIslands array should now contain all non-sleeping Islands, and each Island should
+		// have all the necessary bodies, manifolds and constraints.
+
+		// if we want to merge islands with small batch counts,
+		if (m_minimumSolverBatchSize > 1)
+		{
+			mergeIslands();
+		}
+		// dispatch islands to solver
+		m_islandDispatch(&m_activeIslands, solverParams);
+	}
+}
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h
new file mode 100644
index 00000000000..ab73a899f14
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h
@@ -0,0 +1,112 @@
+/*
+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_SIMULATION_ISLAND_MANAGER_MT_H
+#define BT_SIMULATION_ISLAND_MANAGER_MT_H
+
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+
+class btTypedConstraint;
+class btConstraintSolver;
+struct btContactSolverInfo;
+class btIDebugDraw;
+
+///
+/// SimulationIslandManagerMt -- Multithread capable version of SimulationIslandManager
+///                       Splits the world up into islands which can be solved in parallel.
+///                       In order to solve islands in parallel, an IslandDispatch function
+///                       must be provided which will dispatch calls to multiple threads.
+///                       The amount of parallelism that can be achieved depends on the number
+///                       of islands. If only a single island exists, then no parallelism is
+///                       possible.
+///
+class btSimulationIslandManagerMt : public btSimulationIslandManager
+{
+public:
+	struct Island
+	{
+		// a simulation island consisting of bodies, manifolds and constraints,
+		// to be passed into a constraint solver.
+		btAlignedObjectArray<btCollisionObject*> bodyArray;
+		btAlignedObjectArray<btPersistentManifold*> manifoldArray;
+		btAlignedObjectArray<btTypedConstraint*> constraintArray;
+		int id;  // island id
+		bool isSleeping;
+
+		void append(const Island& other);  // add bodies, manifolds, constraints to my own
+	};
+	struct SolverParams
+	{
+		btConstraintSolver* m_solverPool;
+		btConstraintSolver* m_solverMt;
+		btContactSolverInfo* m_solverInfo;
+		btIDebugDraw* m_debugDrawer;
+		btDispatcher* m_dispatcher;
+	};
+	static void solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams);
+
+	typedef void (*IslandDispatchFunc)(btAlignedObjectArray<Island*>* islands, const SolverParams& solverParams);
+	static void serialIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams);
+	static void parallelIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams);
+
+protected:
+	btAlignedObjectArray<Island*> m_allocatedIslands;    // owner of all Islands
+	btAlignedObjectArray<Island*> m_activeIslands;       // islands actively in use
+	btAlignedObjectArray<Island*> m_freeIslands;         // islands ready to be reused
+	btAlignedObjectArray<Island*> m_lookupIslandFromId;  // big lookup table to map islandId to Island pointer
+	Island* m_batchIsland;
+	int m_minimumSolverBatchSize;
+	int m_batchIslandMinBodyCount;
+	IslandDispatchFunc m_islandDispatch;
+
+	Island* getIsland(int id);
+	virtual Island* allocateIsland(int id, int numBodies);
+	virtual void initIslandPools();
+	virtual void addBodiesToIslands(btCollisionWorld* collisionWorld);
+	virtual void addManifoldsToIslands(btDispatcher* dispatcher);
+	virtual void addConstraintsToIslands(btAlignedObjectArray<btTypedConstraint*>& constraints);
+	virtual void mergeIslands();
+
+public:
+	btSimulationIslandManagerMt();
+	virtual ~btSimulationIslandManagerMt();
+
+	virtual void buildAndProcessIslands(btDispatcher* dispatcher,
+										btCollisionWorld* collisionWorld,
+										btAlignedObjectArray<btTypedConstraint*>& constraints,
+										const SolverParams& solverParams);
+
+	virtual void buildIslands(btDispatcher* dispatcher, btCollisionWorld* colWorld);
+
+	int getMinimumSolverBatchSize() const
+	{
+		return m_minimumSolverBatchSize;
+	}
+	void setMinimumSolverBatchSize(int sz)
+	{
+		m_minimumSolverBatchSize = sz;
+	}
+	IslandDispatchFunc getIslandDispatchFunction() const
+	{
+		return m_islandDispatch;
+	}
+	// allow users to set their own dispatch function for multithreaded dispatch
+	void setIslandDispatchFunction(IslandDispatchFunc func)
+	{
+		m_islandDispatch = func;
+	}
+};
+
+#endif  //BT_SIMULATION_ISLAND_MANAGER_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp
index aea8c683732..9862bd2e228 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -21,37 +21,38 @@
  
  */
 
-
 #include "btMultiBody.h"
 #include "btMultiBodyLink.h"
 #include "btMultiBodyLinkCollider.h"
 #include "btMultiBodyJointFeedback.h"
 #include "LinearMath/btTransformUtil.h"
 #include "LinearMath/btSerializer.h"
-// #define INCLUDE_GYRO_TERM 
+//#include "Bullet3Common/b3Logging.h"
+// #define INCLUDE_GYRO_TERM
 
-///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals
-bool gJointFeedbackInWorldSpace = false;
-bool gJointFeedbackInJointFrame = false;
 
-namespace {
-    const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)
-    const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds
+namespace
+{
+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 btMultiBody::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;
 }
 
-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;
-    }
+namespace
+{
 
-/*
+
+#if 0
     void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
                                  const btVector3 &displacement,
                                  const btVector3 &top_in,
@@ -81,52 +82,65 @@ namespace {
 		top_out = a_top.cross(b_top);
 		bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
 	}
-*/
-}
+#endif
 
+}  // namespace
 
 //
 // Implementation of class btMultiBody
 //
 
 btMultiBody::btMultiBody(int n_links,
-                     btScalar mass,
-                     const btVector3 &inertia,
-                     bool fixedBase,
-                     bool canSleep,
-		     bool /*deprecatedUseMultiDof*/)
-    : 
-    	m_baseCollider(0),
-		m_baseName(0),
-    	m_basePos(0,0,0),
-    	m_baseQuat(0, 0, 0, 1),
-      m_baseMass(mass),
-      m_baseInertia(inertia),
-    
-		m_fixedBase(fixedBase),
-		m_awake(true),
-		m_canSleep(canSleep),
-		m_sleepTimer(0),
-		
-		m_linearDamping(0.04f),
-		m_angularDamping(0.04f),
-		m_useGyroTerm(true),
-			m_maxAppliedImpulse(1000.f),
-		m_maxCoordinateVelocity(100.f),
-			m_hasSelfCollision(true),		
-		__posUpdated(false),
-			m_dofCount(0),
-		m_posVarCnt(0),
-		m_useRK4(false), 	
-		m_useGlobalVelocities(false),
-		m_internalNeedsJointFeedback(false)
+						 btScalar mass,
+						 const btVector3 &inertia,
+						 bool fixedBase,
+						 bool canSleep,
+						 bool /*deprecatedUseMultiDof*/)
+	: m_baseCollider(0),
+	  m_baseName(0),
+	  m_basePos(0, 0, 0),
+	  m_baseQuat(0, 0, 0, 1),
+      m_basePos_interpolate(0, 0, 0),
+      m_baseQuat_interpolate(0, 0, 0, 1),
+	  m_baseMass(mass),
+	  m_baseInertia(inertia),
+
+	  m_fixedBase(fixedBase),
+	  m_awake(true),
+	  m_canSleep(canSleep),
+	  m_canWakeup(true),
+	  m_sleepTimer(0),
+	  m_userObjectPointer(0),
+	  m_userIndex2(-1),
+	  m_userIndex(-1),
+	  m_companionId(-1),
+	  m_linearDamping(0.04f),
+	  m_angularDamping(0.04f),
+	  m_useGyroTerm(true),
+	  m_maxAppliedImpulse(1000.f),
+	  m_maxCoordinateVelocity(100.f),
+	  m_hasSelfCollision(true),
+	  __posUpdated(false),
+	  m_dofCount(0),
+	  m_posVarCnt(0),
+	  m_useRK4(false),
+	  m_useGlobalVelocities(false),
+	  m_internalNeedsJointFeedback(false)
 {
+	m_cachedInertiaTopLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	m_cachedInertiaTopRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	m_cachedInertiaLowerLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	m_cachedInertiaLowerRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	m_cachedInertiaValid = false;
+
 	m_links.resize(n_links);
 	m_matrixBuf.resize(n_links + 1);
 
+	m_baseForce.setValue(0, 0, 0);
+	m_baseTorque.setValue(0, 0, 0);
 
-    m_baseForce.setValue(0, 0, 0);
-    m_baseTorque.setValue(0, 0, 0);
+	clearConstraintForces();
+	clearForcesAndTorques();
 }
 
 btMultiBody::~btMultiBody()
@@ -134,129 +148,125 @@ btMultiBody::~btMultiBody()
 }
 
 void btMultiBody::setupFixed(int i,
-						   btScalar mass,
-						   const btVector3 &inertia,
-						   int parent,
-						   const btQuaternion &rotParentToThis,
-						   const btVector3 &parentComToThisPivotOffset,
-                           const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/)
+							 btScalar mass,
+							 const btVector3 &inertia,
+							 int parent,
+							 const btQuaternion &rotParentToThis,
+							 const btVector3 &parentComToThisPivotOffset,
+							 const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/)
 {
-	
 	m_links[i].m_mass = mass;
-    m_links[i].m_inertiaLocal = inertia;
-    m_links[i].m_parent = parent;
-    m_links[i].m_zeroRotParentToThis = rotParentToThis;
+	m_links[i].m_inertiaLocal = inertia;
+	m_links[i].m_parent = parent;
+	m_links[i].setAxisTop(0, 0., 0., 0.);
+	m_links[i].setAxisBottom(0, btVector3(0, 0, 0));
+	m_links[i].m_zeroRotParentToThis = rotParentToThis;
 	m_links[i].m_dVector = thisPivotToThisComOffset;
-    m_links[i].m_eVector = parentComToThisPivotOffset;    
+	m_links[i].m_eVector = parentComToThisPivotOffset;
 
 	m_links[i].m_jointType = btMultibodyLink::eFixed;
 	m_links[i].m_dofCount = 0;
 	m_links[i].m_posVarCount = 0;
 
-	m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;    
-	
+	m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+
 	m_links[i].updateCacheMultiDof();
 
 	updateLinksDofOffsets();
-
 }
 
-
 void btMultiBody::setupPrismatic(int i,
-                               btScalar mass,
-                               const btVector3 &inertia,
-                               int parent,
-                               const btQuaternion &rotParentToThis,
-                               const btVector3 &jointAxis,
-                               const btVector3 &parentComToThisPivotOffset,
-							   const btVector3 &thisPivotToThisComOffset,
-							   bool disableParentCollision)
+								 btScalar mass,
+								 const btVector3 &inertia,
+								 int parent,
+								 const btQuaternion &rotParentToThis,
+								 const btVector3 &jointAxis,
+								 const btVector3 &parentComToThisPivotOffset,
+								 const btVector3 &thisPivotToThisComOffset,
+								 bool disableParentCollision)
 {
 	m_dofCount += 1;
 	m_posVarCnt += 1;
-	
-    m_links[i].m_mass = mass;
-    m_links[i].m_inertiaLocal = inertia;
-    m_links[i].m_parent = parent;
-    m_links[i].m_zeroRotParentToThis = rotParentToThis;
-    m_links[i].setAxisTop(0, 0., 0., 0.);
-    m_links[i].setAxisBottom(0, jointAxis);
-    m_links[i].m_eVector = parentComToThisPivotOffset;
+
+	m_links[i].m_mass = mass;
+	m_links[i].m_inertiaLocal = inertia;
+	m_links[i].m_parent = parent;
+	m_links[i].m_zeroRotParentToThis = rotParentToThis;
+	m_links[i].setAxisTop(0, 0., 0., 0.);
+	m_links[i].setAxisBottom(0, jointAxis);
+	m_links[i].m_eVector = parentComToThisPivotOffset;
 	m_links[i].m_dVector = thisPivotToThisComOffset;
-    m_links[i].m_cachedRotParentToThis = rotParentToThis;
+	m_links[i].m_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_posVarCount = 1;
 	m_links[i].m_jointPos[0] = 0.f;
 	m_links[i].m_jointTorque[0] = 0.f;
 
 	if (disableParentCollision)
-		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
 	//
-	
+
 	m_links[i].updateCacheMultiDof();
-	
+
 	updateLinksDofOffsets();
 }
 
 void btMultiBody::setupRevolute(int i,
-                              btScalar mass,
-                              const btVector3 &inertia,
-                              int parent,
-                              const btQuaternion &rotParentToThis,
-                              const btVector3 &jointAxis,
-                              const btVector3 &parentComToThisPivotOffset,
-                              const btVector3 &thisPivotToThisComOffset,
-							  bool disableParentCollision)
+								btScalar mass,
+								const btVector3 &inertia,
+								int parent,
+								const btQuaternion &rotParentToThis,
+								const btVector3 &jointAxis,
+								const btVector3 &parentComToThisPivotOffset,
+								const btVector3 &thisPivotToThisComOffset,
+								bool disableParentCollision)
 {
 	m_dofCount += 1;
 	m_posVarCnt += 1;
-	
-    m_links[i].m_mass = mass;
-    m_links[i].m_inertiaLocal = inertia;
-    m_links[i].m_parent = parent;
-    m_links[i].m_zeroRotParentToThis = rotParentToThis;
-    m_links[i].setAxisTop(0, jointAxis);
-    m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
-    m_links[i].m_dVector = thisPivotToThisComOffset;
-    m_links[i].m_eVector = parentComToThisPivotOffset;
+
+	m_links[i].m_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_posVarCount = 1;
 	m_links[i].m_jointPos[0] = 0.f;
 	m_links[i].m_jointTorque[0] = 0.f;
 
 	if (disableParentCollision)
-		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
-    //
+		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+	//
 	m_links[i].updateCacheMultiDof();
 	//
 	updateLinksDofOffsets();
 }
 
-
-
 void btMultiBody::setupSpherical(int i,
-						   btScalar mass,
-						   const btVector3 &inertia,
-						   int parent,
-						   const btQuaternion &rotParentToThis,
-						   const btVector3 &parentComToThisPivotOffset,
-						   const btVector3 &thisPivotToThisComOffset,
-						   bool disableParentCollision)
+								 btScalar mass,
+								 const btVector3 &inertia,
+								 int parent,
+								 const btQuaternion &rotParentToThis,
+								 const btVector3 &parentComToThisPivotOffset,
+								 const btVector3 &thisPivotToThisComOffset,
+								 bool disableParentCollision)
 {
-	
 	m_dofCount += 3;
 	m_posVarCnt += 4;
 
 	m_links[i].m_mass = mass;
-    m_links[i].m_inertiaLocal = inertia;
-    m_links[i].m_parent = parent;
-    m_links[i].m_zeroRotParentToThis = rotParentToThis;    
-    m_links[i].m_dVector = thisPivotToThisComOffset;
-    m_links[i].m_eVector = parentComToThisPivotOffset;    
+	m_links[i].m_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;
@@ -267,323 +277,369 @@ void btMultiBody::setupSpherical(int i,
 	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_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].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
 	//
-	m_links[i].updateCacheMultiDof();	
+	m_links[i].updateCacheMultiDof();
 	//
 	updateLinksDofOffsets();
 }
 
 void btMultiBody::setupPlanar(int i,
-						   btScalar mass,
-						   const btVector3 &inertia,
-						   int parent,
-						   const btQuaternion &rotParentToThis,
-						   const btVector3 &rotationAxis,
-						   const btVector3 &parentComToThisComOffset,						   
-						   bool disableParentCollision)
+							  btScalar mass,
+							  const btVector3 &inertia,
+							  int parent,
+							  const btQuaternion &rotParentToThis,
+							  const btVector3 &rotationAxis,
+							  const btVector3 &parentComToThisComOffset,
+							  bool disableParentCollision)
 {
-	
 	m_dofCount += 3;
 	m_posVarCnt += 3;
 
 	m_links[i].m_mass = mass;
-    m_links[i].m_inertiaLocal = inertia;
-    m_links[i].m_parent = parent;
-    m_links[i].m_zeroRotParentToThis = rotParentToThis;    
+	m_links[i].m_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;
+	m_links[i].m_eVector = parentComToThisComOffset;
 
 	//
-	static btVector3 vecNonParallelToRotAxis(1, 0, 0);
-	if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
+	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 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]);
+	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].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].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+	//
 	m_links[i].updateCacheMultiDof();
 	//
 	updateLinksDofOffsets();
+
+	m_links[i].setAxisBottom(1, m_links[i].getAxisBottom(1).normalized());
+	m_links[i].setAxisBottom(2, m_links[i].getAxisBottom(2).normalized());
 }
 
 void btMultiBody::finalizeMultiDof()
 {
 	m_deltaV.resize(0);
 	m_deltaV.resize(6 + m_dofCount);
-	m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount);			//m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
-	m_vectorBuf.resize(2 * m_dofCount);													//two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
-
+    m_splitV.resize(0);
+    m_splitV.resize(6 + m_dofCount);
+	m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount);  //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
+	m_vectorBuf.resize(2 * m_dofCount);                                           //two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
+	m_matrixBuf.resize(m_links.size() + 1);
+	for (int i = 0; i < m_vectorBuf.size(); i++)
+	{
+		m_vectorBuf[i].setValue(0, 0, 0);
+	}
 	updateLinksDofOffsets();
 }
-	
-int btMultiBody::getParent(int i) const
+
+int btMultiBody::getParent(int link_num) const
 {
-    return m_links[i].m_parent;
+	return m_links[link_num].m_parent;
 }
 
 btScalar btMultiBody::getLinkMass(int i) const
 {
-    return m_links[i].m_mass;
+	return m_links[i].m_mass;
 }
 
-const btVector3 & btMultiBody::getLinkInertia(int i) const
+const btVector3 &btMultiBody::getLinkInertia(int i) const
 {
-    return m_links[i].m_inertiaLocal;
+	return m_links[i].m_inertiaLocal;
 }
 
 btScalar btMultiBody::getJointPos(int i) const
 {
-    return m_links[i].m_jointPos[0];
+	return m_links[i].m_jointPos[0];
 }
 
 btScalar btMultiBody::getJointVel(int i) const
 {
-    return m_realBuf[6 + m_links[i].m_dofOffset];
+	return m_realBuf[6 + m_links[i].m_dofOffset];
 }
 
-btScalar * btMultiBody::getJointPosMultiDof(int i)
+btScalar *btMultiBody::getJointPosMultiDof(int i)
 {
 	return &m_links[i].m_jointPos[0];
 }
 
-btScalar * btMultiBody::getJointVelMultiDof(int i)
+btScalar *btMultiBody::getJointVelMultiDof(int i)
 {
 	return &m_realBuf[6 + m_links[i].m_dofOffset];
 }
 
-const btScalar * btMultiBody::getJointPosMultiDof(int i) const 
+const btScalar *btMultiBody::getJointPosMultiDof(int i) const
 {
 	return &m_links[i].m_jointPos[0];
 }
 
-const btScalar * btMultiBody::getJointVelMultiDof(int i) const 
+const btScalar *btMultiBody::getJointVelMultiDof(int i) const
 {
 	return &m_realBuf[6 + m_links[i].m_dofOffset];
 }
 
-
 void btMultiBody::setJointPos(int i, btScalar q)
 {
-    m_links[i].m_jointPos[0] = q;
-    m_links[i].updateCacheMultiDof();
+	m_links[i].m_jointPos[0] = q;
+	m_links[i].updateCacheMultiDof();
+}
+
+
+void btMultiBody::setJointPosMultiDof(int i, const double *q)
+{
+	for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
+		m_links[i].m_jointPos[pos] = (btScalar)q[pos];
+
+	m_links[i].updateCacheMultiDof();
 }
 
-void btMultiBody::setJointPosMultiDof(int i, btScalar *q)
+void btMultiBody::setJointPosMultiDof(int i, const float *q)
 {
-	for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
-		m_links[i].m_jointPos[pos] = q[pos];
+	for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
+		m_links[i].m_jointPos[pos] = (btScalar)q[pos];
 
-    m_links[i].updateCacheMultiDof();
+	m_links[i].updateCacheMultiDof();
 }
 
+
+
 void btMultiBody::setJointVel(int i, btScalar qdot)
 {
-    m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
+	m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
+}
+
+void btMultiBody::setJointVelMultiDof(int i, const double *qdot)
+{
+	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];
 }
 
-void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot)
+void btMultiBody::setJointVelMultiDof(int i, const float* qdot)
 {
-	for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof];
+	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];
 }
 
-const btVector3 & btMultiBody::getRVector(int i) const
+const btVector3 &btMultiBody::getRVector(int i) const
 {
-    return m_links[i].m_cachedRVector;
+	return m_links[i].m_cachedRVector;
 }
 
-const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
+const btQuaternion &btMultiBody::getParentToLocalRot(int i) const
 {
-    return m_links[i].m_cachedRotParentToThis;
+	return m_links[i].m_cachedRotParentToThis;
+}
+
+const btVector3 &btMultiBody::getInterpolateRVector(int i) const
+{
+    return m_links[i].m_cachedRVector_interpolate;
+}
+
+const btQuaternion &btMultiBody::getInterpolateParentToLocalRot(int i) const
+{
+    return m_links[i].m_cachedRotParentToThis_interpolate;
 }
 
 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);
-    }
+	btAssert(i >= -1);
+	btAssert(i < m_links.size());
+	if ((i < -1) || (i >= m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
+	}
+
+	btVector3 result = local_pos;
+	while (i != -1)
+	{
+		// 'result' is in frame i. transform it to frame parent(i)
+		result += getRVector(i);
+		result = quatRotate(getParentToLocalRot(i).inverse(), result);
+		i = getParent(i);
+	}
 
-    // 'result' is now in the base frame. transform it to world frame
-    result = quatRotate(getWorldToBaseRot().inverse() ,result);
-    result += getBasePos();
+	// 'result' is now in the base frame. transform it to world frame
+	result = quatRotate(getWorldToBaseRot().inverse(), result);
+	result += getBasePos();
 
-    return result;
+	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);
-    }
+	btAssert(i >= -1);
+	btAssert(i < m_links.size());
+	if ((i < -1) || (i >= m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
+	}
+
+	if (i == -1)
+	{
+		// world to base
+		return quatRotate(getWorldToBaseRot(), (world_pos - getBasePos()));
+	}
+	else
+	{
+		// find position in parent frame, then transform to current frame
+		return quatRotate(getParentToLocalRot(i), worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
+	}
 }
 
 btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
 {
-    btVector3 result = local_dir;
-    while (i != -1) {
-        result = quatRotate(getParentToLocalRot(i).inverse() , result);
-        i = getParent(i);
-    }
-    result = quatRotate(getWorldToBaseRot().inverse() , result);
-    return result;
+	btAssert(i >= -1);
+	btAssert(i < m_links.size());
+	if ((i < -1) || (i >= m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
+	}
+
+	btVector3 result = local_dir;
+	while (i != -1)
+	{
+		result = quatRotate(getParentToLocalRot(i).inverse(), result);
+		i = getParent(i);
+	}
+	result = quatRotate(getWorldToBaseRot().inverse(), result);
+	return result;
 }
 
 btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
 {
-    if (i == -1) {
-        return quatRotate(getWorldToBaseRot(), world_dir);
-    } else {
-        return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir));
-    }
+	btAssert(i >= -1);
+	btAssert(i < m_links.size());
+	if ((i < -1) || (i >= m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
+	}
+
+	if (i == -1)
+	{
+		return quatRotate(getWorldToBaseRot(), world_dir);
+	}
+	else
+	{
+		return quatRotate(getParentToLocalRot(i), worldDirToLocal(getParent(i), world_dir));
+	}
 }
 
-void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
+btMatrix3x3 btMultiBody::localFrameToWorld(int i, const btMatrix3x3 &local_frame) 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);
-    }
+	btMatrix3x3 result = local_frame;
+	btVector3 frameInWorld0 = localDirToWorld(i, local_frame.getColumn(0));
+	btVector3 frameInWorld1 = localDirToWorld(i, local_frame.getColumn(1));
+	btVector3 frameInWorld2 = localDirToWorld(i, local_frame.getColumn(2));
+	result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
+	return result;
 }
 
-btScalar btMultiBody::getKineticEnergy() const
+void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) 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]);
-    }
+	// Calculates the velocities of each link (and the base) in its local frame
+	const btQuaternion& base_rot = getWorldToBaseRot();
+	omega[0] = quatRotate(base_rot, getBaseOmega());
+	vel[0] = quatRotate(base_rot, getBaseVel());
 
-    return 0.5f * result;
-}
+	for (int i = 0; i < num_links; ++i)
+	{
+		const btMultibodyLink& link = getLink(i);
+		const int parent = link.m_parent;
 
-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])));
-    }
+		// transform parent vel into this frame, store in omega[i+1], vel[i+1]
+		spatialTransform(btMatrix3x3(link.m_cachedRotParentToThis), link.m_cachedRVector,
+			omega[parent + 1], vel[parent + 1],
+			omega[i + 1], vel[i + 1]);
 
-    return result;
+		// now add qidot * shat_i
+		const btScalar* jointVel = getJointVelMultiDof(i);
+		for (int dof = 0; dof < link.m_dofCount; ++dof)
+		{
+			omega[i + 1] += jointVel[dof] * link.getAxisTop(dof);
+			vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof);
+		}
+	}
 }
 
+
 void btMultiBody::clearConstraintForces()
 {
 	m_baseConstraintForce.setValue(0, 0, 0);
 	m_baseConstraintTorque.setValue(0, 0, 0);
 
-
-    for (int i = 0; i < getNumLinks(); ++i) {
-        m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
-        m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
-    }
+	for (int i = 0; i < getNumLinks(); ++i)
+	{
+		m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
+		m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
+	}
 }
 void btMultiBody::clearForcesAndTorques()
 {
-    m_baseForce.setValue(0, 0, 0);
-    m_baseTorque.setValue(0, 0, 0);
+	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);
+	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) 
+	for (int i = 0; i < 6 + getNumDofs(); ++i)
 	{
 		m_realBuf[i] = 0.f;
 	}
 }
 void btMultiBody::addLinkForce(int i, const btVector3 &f)
 {
-    m_links[i].m_appliedForce += f;
+	m_links[i].m_appliedForce += f;
 }
 
 void btMultiBody::addLinkTorque(int i, const btVector3 &t)
 {
-    m_links[i].m_appliedTorque += t;
+	m_links[i].m_appliedTorque += t;
 }
 
 void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f)
 {
-    m_links[i].m_appliedConstraintForce += f;
+	m_links[i].m_appliedConstraintForce += f;
 }
 
 void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t)
 {
-    m_links[i].m_appliedConstraintTorque += t;
+	m_links[i].m_appliedConstraintTorque += t;
 }
 
-
-
 void btMultiBody::addJointTorque(int i, btScalar Q)
 {
-    m_links[i].m_jointTorque[0] += Q;
+	m_links[i].m_jointTorque[0] += Q;
 }
 
 void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q)
@@ -593,70 +649,72 @@ void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q)
 
 void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q)
 {
-	for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+	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
+const btVector3 &btMultiBody::getLinkForce(int i) const
 {
-    return m_links[i].m_appliedForce;
+	return m_links[i].m_appliedForce;
 }
 
-const btVector3 & btMultiBody::getLinkTorque(int i) const
+const btVector3 &btMultiBody::getLinkTorque(int i) const
 {
-    return m_links[i].m_appliedTorque;
+	return m_links[i].m_appliedTorque;
 }
 
 btScalar btMultiBody::getJointTorque(int i) const
 {
-    return m_links[i].m_jointTorque[0];
+	return m_links[i].m_jointTorque[0];
 }
 
-btScalar * btMultiBody::getJointTorqueMultiDof(int i)
+btScalar *btMultiBody::getJointTorqueMultiDof(int i)
 {
-    return &m_links[i].m_jointTorque[0];
+	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?
+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;
+	btVector3 row0 = btVector3(
+		v0.x() * v1.x(),
+		v0.x() * v1.y(),
+		v0.x() * v1.z());
+	btVector3 row1 = btVector3(
+		v0.y() * v1.x(),
+		v0.y() * v1.y(),
+		v0.y() * v1.z());
+	btVector3 row2 = btVector3(
+		v0.z() * v1.x(),
+		v0.z() * v1.y(),
+		v0.z() * v1.z());
+
+	btMatrix3x3 m(row0[0], row0[1], row0[2],
+				  row1[0], row1[1], row1[2],
+				  row2[0], row2[1], row2[2]);
+	return m;
 }
 
 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
 //
 
 void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
-                               btAlignedObjectArray<btScalar> &scratch_r,
-                               btAlignedObjectArray<btVector3> &scratch_v,
-                               btAlignedObjectArray<btMatrix3x3> &scratch_m,
-				bool isConstraintPass)
+    btAlignedObjectArray<btScalar> &scratch_r,
+    btAlignedObjectArray<btVector3> &scratch_v,
+    btAlignedObjectArray<btMatrix3x3> &scratch_m,
+	bool isConstraintPass,
+	bool jointFeedbackInWorldSpace,
+	bool jointFeedbackInJointFrame)
 {
-    // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
-    // and the base linear & angular accelerations.
+	// 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).
+	// 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.
 
-    // output should point to an array of 6 + num_links reals.
-    // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
-    // num_links joint acceleration values.
-    
 	// We added support for multi degree of freedom (multi dof) joints.
 	// In addition we also can compute the joint reaction forces. This is performed in a second pass,
 	// so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver)
@@ -665,96 +723,96 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 	int num_links = getNumLinks();
 
-    const btScalar DAMPING_K1_LINEAR = m_linearDamping;
+	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;
+	const btScalar DAMPING_K2_ANGULAR = m_angularDamping;
 
-    btVector3 base_vel = getBaseVel();
-    btVector3 base_omega = getBaseOmega();
+	const btVector3 base_vel = getBaseVel();
+	const btVector3 base_omega = getBaseOmega();
 
-    // Temporary matrices/vectors -- use scratch space from caller
-    // so that we don't have to keep reallocating every frame
+	// 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);
+	scratch_r.resize(2 * m_dofCount + 7);  //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)	
+	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;
+	// 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;
+	// 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];
+	// 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];
+	// 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;
+	// 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];
+	// 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;
+	//aux variables
+	btSpatialMotionVector spatJointVel;         //spatial velocity due to the joint motion (i.e. without predecessors' influence)
+	btScalar D[36];                             //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
+	btScalar invD_times_Y[6];                   //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
+	btSpatialMotionVector result;               //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
+	btScalar Y_minus_hT_a[6];                   //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
+	btSpatialForceVector spatForceVecTemps[6];  //6 temporary spatial force vectors
+	btSpatialTransformationMatrix fromParent;   //spatial transform from parent to child
+	btSymmetricSpatialDyad dyadTemp;            //inertia matrix temp
+	btSpatialTransformationMatrix fromWorld;
 	fromWorld.m_trnVec.setZero();
 	/////////////////
 
-    // ptr to the joint accel part of the output
-    btScalar * joint_accel = output + 6;
+	// 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.
+	// Start of the algorithm proper.
 
-    rot_from_parent[0] = btMatrix3x3(m_baseQuat);				//m_baseQuat assumed to be alias!?
+	// 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) 
-	{        
+	if (m_fixedBase)
+	{
 		zeroAccSpatFrc[0].setZero();
-    }
-	else 
+	}
+	else
 	{
-		btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
-		btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
-		//external forces		
-		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));	
+		const btVector3 &baseForce = isConstraintPass ? m_baseConstraintForce : m_baseForce;
+		const btVector3 &baseTorque = isConstraintPass ? m_baseConstraintTorque : m_baseTorque;
+		//external forces
+		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
 
 		//adding damping terms (only)
-		btScalar linDampMult = 1., angDampMult = 1.;
-		zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()),
-								   linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm()));
+		const btScalar linDampMult = 1., angDampMult = 1.;
+		zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
+									linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
 
 		//
 		//p += vhat x Ihat vhat - done in a simpler way
@@ -762,67 +820,66 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			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];
+	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];
+	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]);
+		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)
+		// vhat_i += qidot * shat_i
+		if (!m_useGlobalVelocities)
 		{
 			spatJointVel.setZero();
 
-			for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
+			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;
+			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_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]);		
+		// we can now calculate chat_i
+		spatVel[i + 1].cross(spatJointVel, spatCoriolisAcc[i]);
 
-        // calculate zhat_i^A
+		// calculate zhat_i^A
 		//
-		//external forces		
-		btVector3 linkAppliedForce = isConstraintPass? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
-		btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
- 
-		zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce ));
-	
+		//external forces
+		btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
+		btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
+
+		zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
+
 #if 0	
 		{
 
@@ -840,27 +897,26 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 		//
 		//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
+		zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()),
+										linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
+
+		// calculate Ihat_i^A
 		//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
-		spatInertia[i+1].setMatrix(	btMatrix3x3(0,0,0,0,0,0,0,0,0),
-									//
-									btMatrix3x3(m_links[i].m_mass, 0, 0,
-												0, m_links[i].m_mass, 0,
-												0, 0, m_links[i].m_mass),
-									//
-									btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
-												0, m_links[i].m_inertiaLocal[1], 0,
-												0, 0, m_links[i].m_inertiaLocal[2])
-								);
+		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()));
+		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();
@@ -871,63 +927,65 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 		//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("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)
+	}
+
+	// '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;
+		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)
+		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];
+			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)
-			;
+			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)
+		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)
+			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];
+				const 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)
+		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];
+				if (D[0] >= SIMD_EPSILON)
+				{
+					invDi[0] = 1.0f / D[0];
+				}
+				else
+				{
+					invDi[0] = 0;
+				}
 				break;
 			}
 			case btMultibodyLink::eSpherical:
 			case btMultibodyLink::ePlanar:
 			{
-				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();
+				const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
+				const btMatrix3x3 invD3x3(D3x3.inverse());
 
 				//unroll the loop?
-				for(int row = 0; row < 3; ++row)
+				for (int row = 0; row < 3; ++row)
 				{
-					for(int col = 0; col < 3; ++col)
-					{						
+					for (int col = 0; col < 3; ++col)
+					{
 						invDi[row * 3 + col] = invD3x3[row][col];
 					}
 				}
@@ -936,85 +994,82 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			}
 			default:
 			{
-			
 			}
 		}
 
 		//determine h*D^{-1}
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		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];
-				//				
+			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			{
+				const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
+				//
 				spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
 			}
 		}
 
-		dyadTemp = spatInertia[i+1];
+		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];
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			const 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)
+		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)
+			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];				
-			}	
+				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] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i];
+
+		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];			
+			spatForceVecTemps[0] += hDof * invD_times_Y[dof];
 		}
-		
+
 		fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
-		
-		zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
-    }
 
+		zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
+	}
 
-    // Second 'upward' loop
-    // (part of TreeForwardDynamics in Mirtich)
+	// Second 'upward' loop
+	// (part of TreeForwardDynamics in Mirtich)
 
-    if (m_fixedBase) 
+	if (m_fixedBase)
 	{
-        spatAcc[0].setZero();
-    } 
-	else 
+		spatAcc[0].setZero();
+	}
+	else
 	{
-        if (num_links > 0) 
+		if (num_links > 0)
 		{
+			m_cachedInertiaValid = true;
 			m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
 			m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
 			m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
-			m_cachedInertiaLowerRight= spatInertia[0].m_topLeftMat.transpose();
+			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) 
+	}
+
+	// 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
@@ -1022,79 +1077,79 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 		//	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;
+		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]);
 
-		fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
-		
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		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);			
+			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);
 		}
 
-		btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
+		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];		
+		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];
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+			spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
 
 		if (m_links[i].m_jointFeedback)
 		{
 			m_internalNeedsJointFeedback = true;
 
-			btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
-			btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
+			btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
+			btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;
 
-			if (gJointFeedbackInJointFrame)
+			if (jointFeedbackInJointFrame)
 			{
 				//shift the reaction forces to the joint frame
 				//linear (force) component is the same
 				//shift the angular (torque, moment) component using the relative position,  m_links[i].m_dVector
-				 angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
+				angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
 			}
-			
 
-			if (gJointFeedbackInWorldSpace)
+			if (jointFeedbackInWorldSpace)
 			{
 				if (isConstraintPass)
 				{
- m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
-                                        m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
-				} else
+					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
+					m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
+				}
+				else
 				{
-					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
-					m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
+					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
+					m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
 				}
-			} else
+			}
+			else
 			{
 				if (isConstraintPass)
 				{
-					  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;                        
-                                m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
-
+					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
+					m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
 				}
 				else
 				{
-				m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
-				m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
-				}		
-			}	
+					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
+					m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
+				}
+			}
+		}
 	}
 
-    }
-
-    // transform base accelerations back to the world frame.
-    btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
+	// transform base accelerations back to the world frame.
+	const 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()));
+	const 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];
@@ -1117,26 +1172,25 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	//printf("]\n");
 	/////////////////
 
-    // Final step: add the accelerations (times dt) to the velocities.
+	// Final step: add the accelerations (times dt) to the velocities.
 
 	if (!isConstraintPass)
 	{
-	if(dt > 0.)
-		applyDeltaVeeMultiDof(output, dt);
-
+		if (dt > 0.)
+			applyDeltaVeeMultiDof(output, dt);
 	}
 	/////
 	//btScalar angularThres = 1;
-	//btScalar maxAngVel = 0.;		
+	//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.)
@@ -1153,128 +1207,173 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	/////
 
 	/////////////////////
-	if(m_useGlobalVelocities)
+	if (m_useGlobalVelocities)
 	{
-		for (int i = 0; i < num_links; ++i) 
+		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]);
+
+			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			
+			// vhat_i += qidot * shat_i
 			spatJointVel.setZero();
 
-			for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
+			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;
 
+			// 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(spatVel[i + 1], m_links[i].m_absFrameTotVelocity);
 			fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
 		}
 	}
-	
 }
 
-
-
-void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const
+void btMultiBody::solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const
 {
 	int num_links = getNumLinks();
 	///solve I * x = rhs, so the result = invI * rhs
-    if (num_links == 0) 
+	if (num_links == 0)
 	{
 		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
-        result[0] = rhs_bot[0] / m_baseInertia[0];
-        result[1] = rhs_bot[1] / m_baseInertia[1];
-        result[2] = rhs_bot[2] / m_baseInertia[2];
-        result[3] = rhs_top[0] / m_baseMass;
-        result[4] = rhs_top[1] / m_baseMass;
-        result[5] = rhs_top[2] / m_baseMass;
-    } else 
+
+		if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
+		{
+			result[0] = rhs_bot[0] / m_baseInertia[0];
+			result[1] = rhs_bot[1] / m_baseInertia[1];
+			result[2] = rhs_bot[2] / m_baseInertia[2];
+		}
+		else
+		{
+			result[0] = 0;
+			result[1] = 0;
+			result[2] = 0;
+		}
+		if (m_baseMass >= SIMD_EPSILON)
+		{
+			result[3] = rhs_top[0] / m_baseMass;
+			result[4] = rhs_top[1] / m_baseMass;
+			result[5] = rhs_top[2] / m_baseMass;
+		}
+		else
+		{
+			result[3] = 0;
+			result[4] = 0;
+			result[5] = 0;
+		}
+	}
+	else
 	{
+		if (!m_cachedInertiaValid)
+		{
+			for (int i = 0; i < 6; i++)
+			{
+				result[i] = 0.f;
+			}
+			return;
+		}
 		/// Special routine for calculating the inverse of a spatial inertia matrix
 		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
-		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
+		btMatrix3x3 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;
+		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];
+			btVector3 vtop = invI_upper_left * rhs_top;
+			btVector3 tmp;
+			tmp = invIupper_right * rhs_bot;
+			vtop += tmp;
+			btVector3 vbot = invI_lower_left * rhs_top;
+			tmp = invI_lower_right * rhs_bot;
+			vbot += tmp;
+			result[0] = vtop[0];
+			result[1] = vtop[1];
+			result[2] = vtop[2];
+			result[3] = vbot[0];
+			result[4] = vbot[1];
+			result[5] = vbot[2];
 		}
-		
-    }
+	}
 }
 void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const
 {
 	int num_links = getNumLinks();
 	///solve I * x = rhs, so the result = invI * rhs
-    if (num_links == 0) 
+	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 
+		if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
+		{
+			result.setAngular(rhs.getAngular() / m_baseInertia);
+		}
+		else
+		{
+			result.setAngular(btVector3(0, 0, 0));
+		}
+		if (m_baseMass >= SIMD_EPSILON)
+		{
+			result.setLinear(rhs.getLinear() / m_baseMass);
+		}
+		else
+		{
+			result.setLinear(btVector3(0, 0, 0));
+		}
+	}
+	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;
+		if (!m_cachedInertiaValid)
+		{
+			result.setLinear(btVector3(0, 0, 0));
+			result.setAngular(btVector3(0, 0, 0));
+			result.setVector(btVector3(0, 0, 0), btVector3(0, 0, 0));
+			return;
+		}
+		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f;
 		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
 		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
 		tmp = invIupper_right * m_cachedInertiaLowerRight;
 		btMatrix3x3 invI_upper_left = (tmp * Binv);
 		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
-		tmp = m_cachedInertiaTopLeft  * invI_upper_left;
-		tmp[0][0]-= 1.0;
-		tmp[1][1]-= 1.0;
-		tmp[2][2]-= 1.0;
+		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);		  
+			btVector3 vtop = invI_upper_left * rhs.getLinear();
+			btVector3 tmp;
+			tmp = invIupper_right * rhs.getAngular();
+			vtop += tmp;
+			btVector3 vbot = invI_lower_left * rhs.getLinear();
+			tmp = invI_lower_right * rhs.getAngular();
+			vbot += tmp;
+			result.setVector(vtop, vbot);
 		}
-		
-    }
+	}
 }
 
 void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
@@ -1293,155 +1392,152 @@ void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, in
 }
 
 void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
-                                       btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
+												 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
+	// 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);	    
+	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];
+	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;
+	// 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];
+	// 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;
+	// 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; 
+	// 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;	
+	btScalar invD_times_Y[6];                   //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
+	btSpatialMotionVector result;               //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
+	btScalar Y_minus_hT_a[6];                   //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
+	btSpatialForceVector spatForceVecTemps[6];  //6 temporary spatial force vectors
+	btSpatialTransformationMatrix fromParent;
 	/////////////////
 
-    // First 'upward' loop.
-    // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
-	
+	// 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) 
+	// -- set to force/torque on the base, zero otherwise
+	if (m_fixedBase)
+	{
+		zeroAccSpatFrc[0].setZero();
+	}
+	else
 	{
-        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) 
+		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();
-    }    
+		zeroAccSpatFrc[i + 1].setZero();
+	}
 
 	// 'Downward' loop.
-    // (part of TreeForwardDynamics in Mirtich.)
-    for (int i = num_links - 1; i >= 0; --i)
+	// (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;
+		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)
+		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])
-											;
+			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)
+		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)
+			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];				
-			}	
+				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)
+		// 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];		
+			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;
 
+		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)
+	// Second 'upward' loop
+	// (part of TreeForwardDynamics in Mirtich)
 
-    if (m_fixedBase) 
+	if (m_fixedBase)
 	{
-        spatAcc[0].setZero();
-    } 
-	else 
+		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)
+	// 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;
+		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)
+		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);
+			//
+			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]);
+		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];      
-    }
+		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();
+	// 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();
+	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];
@@ -1453,20 +1549,169 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 	//printf("]\n");
 	/////////////////
 }
+void btMultiBody::predictPositionsMultiDof(btScalar dt)
+{
+    int num_links = getNumLinks();
+    // step position by adding dt * velocity
+    //btVector3 v = getBaseVel();
+    //m_basePos += dt * v;
+    //
+    btScalar *pBasePos;
+    btScalar *pBaseVel = &m_realBuf[3];  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
+    
+    // reset to current position
+    for (int i = 0; i < 3; ++i)
+    {
+        m_basePos_interpolate[i] = m_basePos[i];
+    }
+    pBasePos = m_basePos_interpolate;
+    
+    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;
 
+    // reset to current orientation
+    for (int i = 0; i < 4; ++i)
+    {
+        m_baseQuat_interpolate[i] = m_baseQuat[i];
+    }
+    pBaseQuat = m_baseQuat_interpolate;
 
-
+    btScalar *pBaseOmega = &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
+    //
+    btQuaternion baseQuat;
+    baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+    btVector3 baseOmega;
+    baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
+    pQuatUpdateFun(baseOmega, baseQuat, true, dt);
+    pBaseQuat[0] = baseQuat.x();
+    pBaseQuat[1] = baseQuat.y();
+    pBaseQuat[2] = baseQuat.z();
+    pBaseQuat[3] = baseQuat.w();
+
+    // Finally we can update m_jointPos for each of the m_links
+    for (int i = 0; i < num_links; ++i)
+    {
+        btScalar *pJointPos;
+        pJointPos = &m_links[i].m_jointPos_interpolate[0];
+        
+        btScalar *pJointVel = getJointVelMultiDof(i);
+        
+        switch (m_links[i].m_jointType)
+        {
+            case btMultibodyLink::ePrismatic:
+            case btMultibodyLink::eRevolute:
+            {
+                //reset to current pos
+                pJointPos[0] = m_links[i].m_jointPos[0];
+                btScalar jointVel = pJointVel[0];
+                pJointPos[0] += dt * jointVel;
+                break;
+            }
+            case btMultibodyLink::eSpherical:
+            {
+                //reset to current pos
+
+                for (int j = 0; j < 4; ++j)
+                {
+                    pJointPos[j] = m_links[i].m_jointPos[j];
+                }
+                
+                btVector3 jointVel;
+                jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
+                btQuaternion jointOri;
+                jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
+                pQuatUpdateFun(jointVel, jointOri, false, dt);
+                pJointPos[0] = jointOri.x();
+                pJointPos[1] = jointOri.y();
+                pJointPos[2] = jointOri.z();
+                pJointPos[3] = jointOri.w();
+                break;
+            }
+            case btMultibodyLink::ePlanar:
+            {
+                for (int j = 0; j < 3; ++j)
+                {
+                    pJointPos[j] = m_links[i].m_jointPos[j];
+                }
+                pJointPos[0] += dt * getJointVelMultiDof(i)[0];
+                
+                btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
+                btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
+                pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
+                pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
+                break;
+            }
+            default:
+            {
+            }
+        }
+        
+        m_links[i].updateInterpolationCacheMultiDof();
+    }
+}
 
 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;
+	// 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)
-	//	
+    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];
@@ -1476,92 +1721,102 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 	struct
 	{
 		//"exponential map" based on btTransformUtil::integrateTransform(..)
-		void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
+		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	
-			
+			//!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			
+			if (!baseBody)
+				angvel = quatRotate(quat, omega);  //if quat is not m_baseQuat, it is alibi => ok
 			else
 				angvel = omega;
-		
-			btScalar fAngle = angvel.length(); 		
+
+			btScalar fAngle = angvel.length();
 			//limit the angular motion
 			if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
 			{
-				fAngle = btScalar(0.5)*SIMD_HALF_PI / dt;
+				fAngle = btScalar(0.5) * SIMD_HALF_PI / dt;
 			}
 
-			if ( fAngle < btScalar(0.001) )
+			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 );
+				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 );
+				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();			
-		
+
+			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]);
+    btScalar *pBaseQuat = pq ? pq : m_baseQuat;
+	btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
+	//
+	btQuaternion baseQuat;
+	baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+	btVector3 baseOmega;
+	baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
 	pQuatUpdateFun(baseOmega, baseQuat, true, dt);
 	pBaseQuat[0] = baseQuat.x();
 	pBaseQuat[1] = baseQuat.y();
 	pBaseQuat[2] = baseQuat.z();
 	pBaseQuat[3] = baseQuat.w();
 
-
 	//printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
 	//printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
 	//printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
 
-	if(pq)		
+	if (pq)
 		pq += 7;
-	if(pqd)
+	if (pqd)
 		pqd += 6;
 
 	// Finally we can update m_jointPos for each of the m_links
-    for (int i = 0; i < num_links; ++i) 
+	for (int i = 0; i < num_links; ++i)
 	{
-		btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);		
+        btScalar *pJointPos;
+        pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
+        
 		btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
 
-		switch(m_links[i].m_jointType)
+		switch (m_links[i].m_jointType)
 		{
 			case btMultibodyLink::ePrismatic:
 			case btMultibodyLink::eRevolute:
 			{
-				btScalar jointVel = pJointVel[0];	
+                //reset to current pos
+				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]);
+                //reset to current pos
+				btVector3 jointVel;
+				jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
+				btQuaternion jointOri;
+				jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
 				pQuatUpdateFun(jointVel, jointOri, false, dt);
-				pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
+				pJointPos[0] = jointOri.x();
+				pJointPos[1] = jointOri.y();
+				pJointPos[2] = jointOri.z();
+				pJointPos[3] = jointOri.w();
 				break;
 			}
 			case btMultibodyLink::ePlanar:
@@ -1578,122 +1833,135 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 			default:
 			{
 			}
-
 		}
 
 		m_links[i].updateCacheMultiDof(pq);
 
-		if(pq)		
+		if (pq)
 			pq += m_links[i].m_posVarCount;
-		if(pqd)
+		if (pqd)
 			pqd += m_links[i].m_dofCount;
-    }
+	}
 }
 
 void btMultiBody::fillConstraintJacobianMultiDof(int link,
-                                    const btVector3 &contact_point,
-									const btVector3 &normal_ang,
-                                    const btVector3 &normal_lin,
-                                    btScalar *jac,
-                                    btAlignedObjectArray<btScalar> &scratch_r,
-                                    btAlignedObjectArray<btVector3> &scratch_v,
-                                    btAlignedObjectArray<btMatrix3x3> &scratch_m) const
-{
-    // temporary space
+												 const btVector3 &contact_point,
+												 const btVector3 &normal_ang,
+												 const btVector3 &normal_lin,
+												 btScalar *jac,
+												 btAlignedObjectArray<btScalar> &scratch_r1,
+												 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];
+	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;
+
+    scratch_r1.resize(m_dofCount+num_links);
+    btScalar * results = m_dofCount > 0 ? &scratch_r1[0] : 0;
+    btScalar* links = num_links? &scratch_r1[m_dofCount] : 0;
+    int numLinksChildToRoot=0;
+    int l = link;
+    while (l != -1)
+    {
+        links[numLinksChildToRoot++]=l;
+        l = m_links[l].m_parent;
+    }
+    
+	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 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);
+	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];
+	// 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_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) 
+	// 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) {
+		jac[i] = 0;
+	}
 
-            // 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];
+	// Qdot coefficients, if necessary.
+	if (num_links > 0 && link > -1)
+	{
+        // TODO: (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 a = 0; a < numLinksChildToRoot; a++)
+        {
+            int i = links[numLinksChildToRoot-1-a];
+        	// 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;
+			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)
+			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));
+					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));
+					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));
+					results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0));
+					results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(1));
+					results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(2));
+
+					results[m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));
+					results[m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(1));
+					results[m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(2));
 
 					break;
 				}
 				case btMultibodyLink::ePlanar:
 				{
-					results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
-					results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
-					results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
+					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;
 				}
@@ -1701,246 +1969,318 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link,
 				{
 				}
 			}
-            
-        }
+		}
 
-        // Now copy through to output.
+		// Now copy through to output.
 		//printf("jac[%d] = ", link);
-        while (link != -1) 
+		while (link != -1)
 		{
-			for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
+			for (int dof = 0; dof < m_links[link].m_dofCount; ++dof)
 			{
 				jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
 				//printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
 			}
-            
+
 			link = m_links[link].m_parent;
-        }
+		}
 		//printf("]\n");
-    }
+	}
 }
 
-
 void btMultiBody::wakeUp()
 {
-    m_awake = true;
+	m_sleepTimer = 0;
+	m_awake = true;
 }
 
 void btMultiBody::goToSleep()
 {
-    m_awake = false;
+	m_awake = false;
 }
 
 void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
 {
 	extern bool gDisableDeactivation;
-    if (!m_canSleep || 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;
+	
+
+	// motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
+	btScalar motion = 0;
 	{
-		for (int i = 0; i < 6 + m_dofCount; ++i) 		
+		for (int i = 0; i < 6 + m_dofCount; ++i)
 			motion += m_realBuf[i] * m_realBuf[i];
 	}
-	
 
-    if (motion < SLEEP_EPSILON) {
-        m_sleepTimer += timestep;
-        if (m_sleepTimer > SLEEP_TIMEOUT) {
-            goToSleep();
-        }
-    } else {
-        m_sleepTimer = 0;
-		if (!m_awake)
-			wakeUp();
-    }
+	if (motion < SLEEP_EPSILON)
+	{
+		m_sleepTimer += timestep;
+		if (m_sleepTimer > SLEEP_TIMEOUT)
+		{
+			goToSleep();
+		}
+	}
+	else
+	{
+		m_sleepTimer = 0;
+		if (m_canWakeup)
+		{
+			if (!m_awake)
+				wakeUp();
+		}
+	}
 }
 
-
-void	btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
+void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
 {
-	
 	int num_links = getNumLinks();
 
 	// Cached 3x3 rotation matrices from parent frame to this frame.
-	btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0];
+	btMatrix3x3 *rot_from_parent = (btMatrix3x3 *)&m_matrixBuf[0];
 
-	rot_from_parent[0] = btMatrix3x3(m_baseQuat);				//m_baseQuat assumed to be alias!?
-	
-	for (int i = 0; i < num_links; ++i) 
+	rot_from_parent[0] = btMatrix3x3(m_baseQuat);  //m_baseQuat assumed to be alias!?
+
+	for (int i = 0; i < num_links; ++i)
 	{
-		rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
+		rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
 	}
-		
+
 	int nLinks = getNumLinks();
 	///base + num m_links
-	world_to_local.resize(nLinks+1);
-	local_origin.resize(nLinks+1);
+	world_to_local.resize(nLinks + 1);
+	local_origin.resize(nLinks + 1);
 
 	world_to_local[0] = getWorldToBaseRot();
 	local_origin[0] = getBasePos();
-	
-	for (int k=0;k<getNumLinks();k++)
+
+	for (int k = 0; k < getNumLinks(); k++)
 	{
 		const int parent = getParent(k);
-		world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
-		local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
+		world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];
+		local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));
 	}
 
-	for (int link=0;link<getNumLinks();link++)
+	for (int link = 0; link < getNumLinks(); link++)
 	{
-		int index = link+1;
+		int index = link + 1;
 
 		btVector3 posr = local_origin[index];
-		btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
+		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]));
+		tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
 		getLink(link).m_cachedWorldTransform = tr;
-			
 	}
-
 }
 
-void	btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
+void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
 {
-	world_to_local.resize(getNumLinks()+1);
-	local_origin.resize(getNumLinks()+1);
-	
+	world_to_local.resize(getNumLinks() + 1);
+	local_origin.resize(getNumLinks() + 1);
+
 	world_to_local[0] = getWorldToBaseRot();
 	local_origin[0] = getBasePos();
-	
+
 	if (getBaseCollider())
 	{
 		btVector3 posr = local_origin[0];
 		//	float pos[4]={posr.x(),posr.y(),posr.z(),1};
-		btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
+		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]));
-		
+		tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+
 		getBaseCollider()->setWorldTransform(tr);
-		
+        getBaseCollider()->setInterpolationWorldTransform(tr);
 	}
-	
-	for (int k=0;k<getNumLinks();k++)
+
+	for (int k = 0; k < getNumLinks(); k++)
 	{
 		const int parent = getParent(k);
-		world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
-		local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
+		world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];
+		local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));
 	}
-	
-	
-	for (int m=0;m<getNumLinks();m++)
+
+	for (int m = 0; m < getNumLinks(); m++)
 	{
-		btMultiBodyLinkCollider* col = getLink(m).m_collider;
+		btMultiBodyLinkCollider *col = getLink(m).m_collider;
 		if (col)
 		{
 			int link = col->m_link;
 			btAssert(link == m);
-			
-			int index = link+1;
-			
+
+			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()};
+			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]));
-			
+			tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+
 			col->setWorldTransform(tr);
+            col->setInterpolationWorldTransform(tr);
 		}
 	}
 }
 
-int	btMultiBody::calculateSerializeBufferSize()	const
+void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
+{
+    world_to_local.resize(getNumLinks() + 1);
+    local_origin.resize(getNumLinks() + 1);
+    
+    world_to_local[0] = getInterpolateWorldToBaseRot();
+    local_origin[0] = getInterpolateBasePos();
+    
+    if (getBaseCollider())
+    {
+        btVector3 posr = local_origin[0];
+        //    float pos[4]={posr.x(),posr.y(),posr.z(),1};
+        btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
+        btTransform tr;
+        tr.setIdentity();
+        tr.setOrigin(posr);
+        tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+        
+        getBaseCollider()->setInterpolationWorldTransform(tr);
+    }
+    
+    for (int k = 0; k < getNumLinks(); k++)
+    {
+        const int parent = getParent(k);
+        world_to_local[k + 1] = getInterpolateParentToLocalRot(k) * world_to_local[parent + 1];
+        local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getInterpolateRVector(k)));
+    }
+    
+    for (int m = 0; m < getNumLinks(); m++)
+    {
+        btMultiBodyLinkCollider *col = getLink(m).m_collider;
+        if (col)
+        {
+            int link = col->m_link;
+            btAssert(link == m);
+            
+            int index = link + 1;
+            
+            btVector3 posr = local_origin[index];
+            //            float pos[4]={posr.x(),posr.y(),posr.z(),1};
+            btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
+            btTransform tr;
+            tr.setIdentity();
+            tr.setOrigin(posr);
+            tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+            
+            col->setInterpolationWorldTransform(tr);
+        }
+    }
+}
+
+int btMultiBody::calculateSerializeBufferSize() const
 {
 	int sz = sizeof(btMultiBodyData);
 	return sz;
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *serializer) const
 {
-		btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer;
-		getBaseWorldTransform().serialize(mbd->m_baseWorldTransform);
-		mbd->m_baseMass = this->getBaseMass();
-		getBaseInertia().serialize(mbd->m_baseInertia);
+	btMultiBodyData *mbd = (btMultiBodyData *)dataBuffer;
+	getBasePos().serialize(mbd->m_baseWorldPosition);
+	getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation);
+	getBaseVel().serialize(mbd->m_baseLinearVelocity);
+	getBaseOmega().serialize(mbd->m_baseAngularVelocity);
+
+	mbd->m_baseMass = this->getBaseMass();
+	getBaseInertia().serialize(mbd->m_baseInertia);
+	{
+		char *name = (char *)serializer->findNameForPointer(m_baseName);
+		mbd->m_baseName = (char *)serializer->getUniquePointer(name);
+		if (mbd->m_baseName)
 		{
-			char* name = (char*) serializer->findNameForPointer(m_baseName);
-			mbd->m_baseName = (char*)serializer->getUniquePointer(name);
-			if (mbd->m_baseName)
-			{
-				serializer->serializeName(name);
-			}
+			serializer->serializeName(name);
 		}
-		mbd->m_numLinks = this->getNumLinks();
-		if (mbd->m_numLinks)
+	}
+	mbd->m_numLinks = this->getNumLinks();
+	if (mbd->m_numLinks)
+	{
+		int sz = sizeof(btMultiBodyLinkData);
+		int numElem = mbd->m_numLinks;
+		btChunk *chunk = serializer->allocate(sz, numElem);
+		btMultiBodyLinkData *memPtr = (btMultiBodyLinkData *)chunk->m_oldPtr;
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
-			int sz = sizeof(btMultiBodyLinkData);
-			int numElem = mbd->m_numLinks;
-			btChunk* chunk = serializer->allocate(sz,numElem);
-			btMultiBodyLinkData* memPtr = (btMultiBodyLinkData*)chunk->m_oldPtr;
-			for (int i=0;i<numElem;i++,memPtr++)
+			memPtr->m_jointType = getLink(i).m_jointType;
+			memPtr->m_dofCount = getLink(i).m_dofCount;
+			memPtr->m_posVarCount = getLink(i).m_posVarCount;
+
+			getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
+
+			getLink(i).m_absFrameTotVelocity.m_topVec.serialize(memPtr->m_absFrameTotVelocityTop);
+			getLink(i).m_absFrameTotVelocity.m_bottomVec.serialize(memPtr->m_absFrameTotVelocityBottom);
+			getLink(i).m_absFrameLocVelocity.m_topVec.serialize(memPtr->m_absFrameLocVelocityTop);
+			getLink(i).m_absFrameLocVelocity.m_bottomVec.serialize(memPtr->m_absFrameLocVelocityBottom);
+
+			memPtr->m_linkMass = getLink(i).m_mass;
+			memPtr->m_parentIndex = getLink(i).m_parent;
+			memPtr->m_jointDamping = getLink(i).m_jointDamping;
+			memPtr->m_jointFriction = getLink(i).m_jointFriction;
+			memPtr->m_jointLowerLimit = getLink(i).m_jointLowerLimit;
+			memPtr->m_jointUpperLimit = getLink(i).m_jointUpperLimit;
+			memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce;
+			memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity;
+
+			getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset);
+			getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
+			getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
+			btAssert(memPtr->m_dofCount <= 3);
+			for (int dof = 0; dof < getLink(i).m_dofCount; dof++)
 			{
+				getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
+				getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);
 
-				memPtr->m_jointType = getLink(i).m_jointType;
-				memPtr->m_dofCount = getLink(i).m_dofCount;
-				memPtr->m_posVarCount = getLink(i).m_posVarCount;
-				
-				getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
-				memPtr->m_linkMass = getLink(i).m_mass;
-				memPtr->m_parentIndex = getLink(i).m_parent;
-				getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
-				getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
-				getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
-				btAssert(memPtr->m_dofCount<=3);
-				for (int dof = 0;dof<getLink(i).m_dofCount;dof++)
-				{
-					getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
-					getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);
-					
-					memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
-					memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
+				memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
+				memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
+			}
+			int numPosVar = getLink(i).m_posVarCount;
+			for (int posvar = 0; posvar < numPosVar; posvar++)
+			{
+				memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
+			}
 
-				}
-				int numPosVar = getLink(i).m_posVarCount;
-				for (int posvar = 0; posvar < numPosVar;posvar++)
+			{
+				char *name = (char *)serializer->findNameForPointer(m_links[i].m_linkName);
+				memPtr->m_linkName = (char *)serializer->getUniquePointer(name);
+				if (memPtr->m_linkName)
 				{
-					memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
+					serializer->serializeName(name);
 				}
-				
-				
+			}
+			{
+				char *name = (char *)serializer->findNameForPointer(m_links[i].m_jointName);
+				memPtr->m_jointName = (char *)serializer->getUniquePointer(name);
+				if (memPtr->m_jointName)
 				{
-					char* name = (char*) serializer->findNameForPointer(m_links[i].m_linkName);
-					memPtr->m_linkName = (char*)serializer->getUniquePointer(name);
-					if (memPtr->m_linkName)
-					{
-						serializer->serializeName(name);
-					}
+					serializer->serializeName(name);
 				}
-				{
-					char* name = (char*) serializer->findNameForPointer(m_links[i].m_jointName);
-					memPtr->m_jointName = (char*)serializer->getUniquePointer(name);
-					if (memPtr->m_jointName)
-					{
-						serializer->serializeName(name);
-					}
-				}
-				memPtr->m_linkCollider = (btCollisionObjectData*)serializer->getUniquePointer(getLink(i).m_collider);
-
 			}
-			serializer->finalizeChunk(chunk,btMultiBodyLinkDataName,BT_ARRAY_CODE,(void*) &m_links[0]);
+			memPtr->m_linkCollider = (btCollisionObjectData *)serializer->getUniquePointer(getLink(i).m_collider);
 		}
-		mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
+		serializer->finalizeChunk(chunk, btMultiBodyLinkDataName, BT_ARRAY_CODE, (void *)&m_links[0]);
+	}
+	mbd->m_links = mbd->m_numLinks ? (btMultiBodyLinkData *)serializer->getUniquePointer((void *)&m_links[0]) : 0;
+
+	// Fill padding with zeros to appease msan.
+#ifdef BT_USE_DOUBLE_PRECISION
+	memset(mbd->m_padding, 0, sizeof(mbd->m_padding));
+#endif
 
-		return btMultiBodyDataName;
+	return btMultiBodyDataName;
 }
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h
index 8fbe6cda827..f2acfab9b24 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -21,7 +21,6 @@
  
  */
 
-
 #ifndef BT_MULTIBODY_H
 #define BT_MULTIBODY_H
 
@@ -31,170 +30,193 @@
 #include "LinearMath/btMatrix3x3.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
-
 ///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms
 #ifdef BT_USE_DOUBLE_PRECISION
-	#define btMultiBodyData	btMultiBodyDoubleData
-	#define btMultiBodyDataName	"btMultiBodyDoubleData"
-	#define btMultiBodyLinkData btMultiBodyLinkDoubleData
-	#define btMultiBodyLinkDataName	"btMultiBodyLinkDoubleData"
+#define btMultiBodyData btMultiBodyDoubleData
+#define btMultiBodyDataName "btMultiBodyDoubleData"
+#define btMultiBodyLinkData btMultiBodyLinkDoubleData
+#define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData"
 #else
-	#define btMultiBodyData	btMultiBodyFloatData
-	#define btMultiBodyDataName	"btMultiBodyFloatData"
-	#define btMultiBodyLinkData btMultiBodyLinkFloatData
-	#define btMultiBodyLinkDataName	"btMultiBodyLinkFloatData"
-#endif //BT_USE_DOUBLE_PRECISION
+#define btMultiBodyData btMultiBodyFloatData
+#define btMultiBodyDataName "btMultiBodyFloatData"
+#define btMultiBodyLinkData btMultiBodyLinkFloatData
+#define btMultiBodyLinkDataName "btMultiBodyLinkFloatData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 #include "btMultiBodyLink.h"
 class btMultiBodyLinkCollider;
 
-ATTRIBUTE_ALIGNED16(class) btMultiBody 
+ATTRIBUTE_ALIGNED16(class)
+btMultiBody
 {
 public:
-
-    
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-    //
-    // initialization
-    //
-    
-	btMultiBody(int n_links,             // NOT including the base
-		btScalar mass,                // mass of base
-		const btVector3 &inertia,    // inertia of base, in base frame; assumed diagonal
-		bool fixedBase,           // whether the base is fixed (true) or can move (false)
-		bool canSleep, bool deprecatedMultiDof=true);
+	//
+	// initialization
+	//
 
+	btMultiBody(int n_links,               // NOT including the base
+				btScalar mass,             // mass of base
+				const btVector3 &inertia,  // inertia of base, in base frame; assumed diagonal
+				bool fixedBase,            // whether the base is fixed (true) or can move (false)
+				bool canSleep, bool deprecatedMultiDof = true);
 
 	virtual ~btMultiBody();
-    
+
 	//note: fixed link collision with parent is always disabled
-	void setupFixed(int linkIndex,
-						   btScalar mass,
-						   const btVector3 &inertia,
-						   int parent,
-						   const btQuaternion &rotParentToThis,
-						   const btVector3 &parentComToThisPivotOffset,
-                           const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true);
-
-						
+	void setupFixed(int i, //linkIndex
+					btScalar mass,
+					const btVector3 &inertia,
+					int parent,
+					const btQuaternion &rotParentToThis,
+					const btVector3 &parentComToThisPivotOffset,
+					const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision = true);
+
 	void setupPrismatic(int i,
-                               btScalar mass,
-                               const btVector3 &inertia,
-                               int parent,
-                               const btQuaternion &rotParentToThis,
-                               const btVector3 &jointAxis,
-                               const btVector3 &parentComToThisPivotOffset,
-							   const btVector3 &thisPivotToThisComOffset,
-							   bool disableParentCollision);
-
-    void setupRevolute(int linkIndex,            // 0 to num_links-1
-                       btScalar mass,
-                       const btVector3 &inertia,
-                       int parentIndex,
-                       const btQuaternion &rotParentToThis,  // rotate points in parent frame to this frame, when q = 0
-                       const btVector3 &jointAxis,    // in my frame
-                       const btVector3 &parentComToThisPivotOffset,    // vector from parent COM to joint axis, in PARENT frame
-                       const btVector3 &thisPivotToThisComOffset,       // vector from joint axis to my COM, in MY frame
-					   bool disableParentCollision=false);
-
-	void setupSpherical(int linkIndex,											// 0 to num_links-1
-                       btScalar mass,
-                       const btVector3 &inertia,
-                       int parent,
-                       const btQuaternion &rotParentToThis,		// rotate points in parent frame to this frame, when q = 0                       
-                       const btVector3 &parentComToThisPivotOffset,			// vector from parent COM to joint axis, in PARENT frame
-                       const btVector3 &thisPivotToThisComOffset,				// vector from joint axis to my COM, in MY frame
-					   bool disableParentCollision=false);		
-
-	void setupPlanar(int i,											// 0 to num_links-1
-                       btScalar mass,
-                       const btVector3 &inertia,
-                       int parent,
-                       const btQuaternion &rotParentToThis,		// rotate points in parent frame to this frame, when q = 0                       
-					   const btVector3 &rotationAxis,
-                       const btVector3 &parentComToThisComOffset,			// vector from parent COM to this COM, in PARENT frame                       
-					   bool disableParentCollision=false);		
-	
-	const btMultibodyLink& getLink(int index) const
+						btScalar mass,
+						const btVector3 &inertia,
+						int parent,
+						const btQuaternion &rotParentToThis,
+						const btVector3 &jointAxis,
+						const btVector3 &parentComToThisPivotOffset,
+						const btVector3 &thisPivotToThisComOffset,
+						bool disableParentCollision);
+
+	void setupRevolute(int i,  // 0 to num_links-1
+					   btScalar mass,
+					   const btVector3 &inertia,
+					   int parentIndex,
+					   const btQuaternion &rotParentToThis,          // rotate points in parent frame to this frame, when q = 0
+					   const btVector3 &jointAxis,                   // in my frame
+					   const btVector3 &parentComToThisPivotOffset,  // vector from parent COM to joint axis, in PARENT frame
+					   const btVector3 &thisPivotToThisComOffset,    // vector from joint axis to my COM, in MY frame
+					   bool disableParentCollision = false);
+
+	void setupSpherical(int i,  // linkIndex, 0 to num_links-1
+						btScalar mass,
+						const btVector3 &inertia,
+						int parent,
+						const btQuaternion &rotParentToThis,          // rotate points in parent frame to this frame, when q = 0
+						const btVector3 &parentComToThisPivotOffset,  // vector from parent COM to joint axis, in PARENT frame
+						const btVector3 &thisPivotToThisComOffset,    // vector from joint axis to my COM, in MY frame
+						bool disableParentCollision = false);
+
+	void setupPlanar(int i,  // 0 to num_links-1
+					 btScalar mass,
+					 const btVector3 &inertia,
+					 int parent,
+					 const btQuaternion &rotParentToThis,  // rotate points in parent frame to this frame, when q = 0
+					 const btVector3 &rotationAxis,
+					 const btVector3 &parentComToThisComOffset,  // vector from parent COM to this COM, in PARENT frame
+					 bool disableParentCollision = false);
+
+	const btMultibodyLink &getLink(int index) const
 	{
 		return m_links[index];
 	}
 
-	btMultibodyLink& getLink(int index)
+	btMultibodyLink &getLink(int index)
 	{
 		return m_links[index];
 	}
 
-
-	void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base
+	void setBaseCollider(btMultiBodyLinkCollider * collider)  //collider can be NULL to disable collision for the base
 	{
 		m_baseCollider = collider;
 	}
-	const btMultiBodyLinkCollider* getBaseCollider() const
+	const btMultiBodyLinkCollider *getBaseCollider() const
 	{
 		return m_baseCollider;
 	}
-	btMultiBodyLinkCollider* getBaseCollider()
+	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
-    //
+	const btMultiBodyLinkCollider *getLinkCollider(int index) const
+	{
+		if (index >= 0 && index < getNumLinks())
+		{
+			return getLink(index).m_collider;
+		}
+		return 0;
+	}
+
+	btMultiBodyLinkCollider *getLinkCollider(int index)
+	{
+		if (index >= 0 && index < getNumLinks())
+		{
+			return getLink(index).m_collider;
+		}
+		return 0;
+	}
 
-    int getNumLinks() const { return m_links.size(); }
+	//
+	// 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)
-    //
+	btScalar getBaseMass() const { return m_baseMass; }
+	const btVector3 &getBaseInertia() const { return m_baseInertia; }
+	btScalar getLinkMass(int i) const;
+	const btVector3 &getLinkInertia(int i) const;
 
-    void setBaseMass(btScalar mass) { m_baseMass = mass; }
-    void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; }
+	//
+	// 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
-    //
+	//
+	// 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 
+	const btVector3 &getBasePos() 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
+		return m_basePos; 
+	}  // in world frame
+	const btVector3 getBaseVel() const
+	{
+		return btVector3(m_realBuf[3], m_realBuf[4], m_realBuf[5]);
+	}  // in world frame
+	const btQuaternion &getWorldToBaseRot() const
+	{
+		return m_baseQuat;
+	}
+    
+    const btVector3 &getInterpolateBasePos() const
+    {
+        return m_basePos_interpolate;
+    }  // in world frame
+    const btQuaternion &getInterpolateWorldToBaseRot() const
+    {
+        return m_baseQuat_interpolate;
+    }
+    
+    // rotates world vectors into base frame
+	btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); }  // in world frame
 
-    void setBasePos(const btVector3 &pos) 
-	{ 
-		m_basePos = pos; 
+	void setBasePos(const btVector3 &pos)
+	{
+		m_basePos = pos;
+        m_basePos_interpolate = pos;
 	}
 
-	void setBaseWorldTransform(const btTransform& tr)
+	void setBaseWorldTransform(const btTransform &tr)
 	{
 		setBasePos(tr.getOrigin());
 		setWorldToBaseRot(tr.getRotation().inverse());
-
 	}
 
 	btTransform getBaseWorldTransform() const
@@ -205,186 +227,212 @@ public:
 		return tr;
 	}
 
-    void setBaseVel(const btVector3 &vel) 
-	{ 
-
-		m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2]; 
+	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 setWorldToBaseRot(const btQuaternion &rot)
+	{
+		m_baseQuat = rot;  //m_baseQuat asumed to ba alias!?
+        m_baseQuat_interpolate = rot;
 	}
-    void setBaseOmega(const btVector3 &omega) 
-	{ 
-		m_realBuf[0]=omega[0]; 
-		m_realBuf[1]=omega[1]; 
-		m_realBuf[2]=omega[2]; 
+	void 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)
+	//
 
-    //
-    // 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);
+	btScalar getJointPos(int i) const;
+	btScalar getJointVel(int i) const;
 
-	const btScalar * getJointVelMultiDof(int i) const ;
-	const btScalar * getJointPosMultiDof(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);	
+	const btScalar *getJointVelMultiDof(int i) const;
+	const btScalar *getJointPosMultiDof(int i) const;
 
+	void setJointPos(int i, btScalar q);
+	void setJointVel(int i, btScalar qdot);
+	void setJointPosMultiDof(int i, const double *q);
+	void setJointVelMultiDof(int i, const double *qdot);
+	void setJointPosMultiDof(int i, const float *q);
+	void setJointVelMultiDof(int i, const float *qdot);
 
-
-    //
-    // direct access to velocities as a vector of 6 + num_links elements.
-    // (omega first, then v, then joint velocities.)
-    //
-    const btScalar * getVelocityVector() const 
-	{ 
-		return &m_realBuf[0]; 
+	//
+	// 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() 
+    
+    const btScalar *getDeltaVelocityVector() const
+    {
+        return &m_deltaV[0];
+    }
+    
+    const btScalar *getSplitVelocityVector() const
+    {
+        return &m_splitV[0];
+    }
+	/*    btScalar * getVelocityVector() 
 	{ 
 		return &real_buf[0]; 
 	}
-  */  
+  */
 
-    //
-    // get the frames of reference (positions and orientations) of the child m_links
-    // (i = 0 to num_links-1)
-    //
+	//
+	// get the frames of reference (positions and orientations) of the child m_links
+	// (i = 0 to num_links-1)
+	//
 
-    const btVector3 & getRVector(int i) const;   // vector from COM(parent(i)) to COM(i), in frame i's coords
-    const btQuaternion & getParentToLocalRot(int i) const;   // rotates vectors in frame parent(i) to vectors in frame i.
+	const btVector3 &getRVector(int i) const;              // vector from COM(parent(i)) to COM(i), in frame i's coords
+	const btQuaternion &getParentToLocalRot(int i) const;  // rotates vectors in frame parent(i) to vectors in frame i.
+    const btVector3 &getInterpolateRVector(int i) const;              // vector from COM(parent(i)) to COM(i), in frame i's coords
+    const btQuaternion &getInterpolateParentToLocalRot(int i) const;  // rotates vectors in frame parent(i) to vectors in frame i.
 
+	//
+	// transform vectors in local frame of link i to world frame (or vice versa)
+	//
+	btVector3 localPosToWorld(int i, const btVector3 &local_pos) const;
+	btVector3 localDirToWorld(int i, const btVector3 &local_dir) const;
+	btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const;
+	btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const;
 
-    //
-    // transform vectors in local frame of link i to world frame (or vice versa)
-    //
-    btVector3 localPosToWorld(int i, const btVector3 &vec) const;
-    btVector3 localDirToWorld(int i, const btVector3 &vec) const;
-    btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
-    btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
-    
+	//
+	// transform a frame in local coordinate to a frame in world coordinate
+	//
+	btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) 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.
-    //
+	//
+	// set external forces and torques. Note all external forces/torques are given in the WORLD frame.
+	//
 
-    void clearForcesAndTorques();
-   void clearConstraintForces();
+	void clearForcesAndTorques();
+	void clearConstraintForces();
 
 	void clearVelocities();
 
-    void addBaseForce(const btVector3 &f) 
-	{ 
-		m_baseForce += f; 
+	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 addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
+	void addLinkForce(int i, const btVector3 &f);
+	void addLinkTorque(int i, const btVector3 &t);
 
- void addBaseConstraintForce(const btVector3 &f)
-        {
-                m_baseConstraintForce += f;
-        }
-    void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; }
-    void addLinkConstraintForce(int i, const btVector3 &f);
-    void addLinkConstraintTorque(int i, const btVector3 &t);
-       
+	void addBaseConstraintForce(const btVector3 &f)
+	{
+		m_baseConstraintForce += f;
+	}
+	void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; }
+	void addLinkConstraintForce(int i, const btVector3 &f);
+	void addLinkConstraintTorque(int i, const btVector3 &t);
 
-void addJointTorque(int i, btScalar Q);
+	void addJointTorque(int i, btScalar Q);
 	void addJointTorqueMultiDof(int i, int dof, btScalar Q);
 	void addJointTorqueMultiDof(int i, const btScalar *Q);
 
-    const btVector3 & getBaseForce() const { return m_baseForce; }
-    const btVector3 & getBaseTorque() const { return m_baseTorque; }
-    const btVector3 & getLinkForce(int i) const;
-    const btVector3 & getLinkTorque(int i) const;
-    btScalar getJointTorque(int i) const;
-	btScalar * getJointTorqueMultiDof(int i);
-
-
-    //
-    // dynamics routines.
-    //
-
-    // timestep the velocities (given the external forces/torques set using addBaseForce etc).
-    // also sets up caches for calcAccelerationDeltas.
-    //
-    // Note: the caller must provide three vectors which are used as
-    // temporary scratch space. The idea here is to reduce dynamic
-    // memory allocation: the same scratch vectors can be re-used
-    // again and again for different Multibodies, instead of each
-    // btMultiBody allocating (and then deallocating) their own
-    // individual scratch buffers. This gives a considerable speed
-    // improvement, at least on Windows (where dynamic memory
-    // allocation appears to be fairly slow).
-    //
-   
- 
-	void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
-                        btAlignedObjectArray<btScalar> &scratch_r,
-                        btAlignedObjectArray<btVector3> &scratch_v,
-                        btAlignedObjectArray<btMatrix3x3> &scratch_m,
-			bool isConstraintPass=false
-		);
-
-///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead
-        void stepVelocitiesMultiDof(btScalar dt,
-                        btAlignedObjectArray<btScalar> &scratch_r,
-                        btAlignedObjectArray<btVector3> &scratch_v,
-                        btAlignedObjectArray<btMatrix3x3> &scratch_m,
-                        bool isConstraintPass=false)
-	{
-		computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt,scratch_r,scratch_v,scratch_m,isConstraintPass);
-        }
+	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).
+	//
 
-    // calcAccelerationDeltasMultiDof
-    // input: force vector (in same format as jacobian, i.e.:
-    //                      3 torque values, 3 force values, num_links joint torque values)
-    // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
-    // (existing contents of output array are replaced)
-    // calcAccelerationDeltasMultiDof must have been called first.
+	void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
+															  btAlignedObjectArray<btScalar> & scratch_r,
+															  btAlignedObjectArray<btVector3> & scratch_v,
+															  btAlignedObjectArray<btMatrix3x3> & scratch_m,
+															  bool isConstraintPass,
+                                                              bool jointFeedbackInWorldSpace,
+                                                              bool jointFeedbackInJointFrame
+                                                              );
+
+	///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead
+	//void stepVelocitiesMultiDof(btScalar dt,
+	//							btAlignedObjectArray<btScalar> & scratch_r,
+	//							btAlignedObjectArray<btVector3> & scratch_v,
+	//							btAlignedObjectArray<btMatrix3x3> & scratch_m,
+	//							bool isConstraintPass = false)
+	//{
+	//	computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false);
+	//}
+
+	// calcAccelerationDeltasMultiDof
+	// input: force vector (in same format as jacobian, i.e.:
+	//                      3 torque values, 3 force values, num_links joint torque values)
+	// output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
+	// (existing contents of output array are replaced)
+	// calcAccelerationDeltasMultiDof must have been called first.
 	void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
-                                btAlignedObjectArray<btScalar> &scratch_r,
-                                btAlignedObjectArray<btVector3> &scratch_v) const;
-	
-  
-	void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier)
+										btAlignedObjectArray<btScalar> &scratch_r,
+										btAlignedObjectArray<btVector3> &scratch_v) const;
+
+	void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
 	{
 		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
-                {
-                        m_deltaV[dof] += delta_vee[dof] * multiplier;
-                }
+		{
+			m_deltaV[dof] += delta_vee[dof] * multiplier;
+		}
 	}
+    void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
+    {
+        for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+        {
+            m_splitV[dof] += delta_vee[dof] * multiplier;
+        }
+    }
+    void addSplitV()
+    {
+        applyDeltaVeeMultiDof(&m_splitV[0], 1);
+    }
+    void substractSplitV()
+    {
+        applyDeltaVeeMultiDof(&m_splitV[0], -1);
+        
+        for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+        {
+            m_splitV[dof] = 0.f;
+        }
+    }
 	void processDeltaVeeMultiDof2()
 	{
-		applyDeltaVeeMultiDof(&m_deltaV[0],1);
+		applyDeltaVeeMultiDof(&m_deltaV[0], 1);
 
 		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
-                {
+		{
 			m_deltaV[dof] = 0.f;
 		}
 	}
 
-	void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier) 
+	void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
 	{
 		//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
 		//	printf("%.4f ", delta_vee[dof]*multiplier);
@@ -405,65 +453,81 @@ void addJointTorque(int i, btScalar Q);
 		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
 		{
 			m_realBuf[dof] += delta_vee[dof] * multiplier;
-			btClamp(m_realBuf[dof],-m_maxCoordinateVelocity,m_maxCoordinateVelocity);
+			btClamp(m_realBuf[dof], -m_maxCoordinateVelocity, m_maxCoordinateVelocity);
 		}
-    }
+	}
 
-	
-	
-    // timestep the positions (given current velocities).
+	// timestep the positions (given current velocities).
 	void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
+    
+    // predict the positions
+    void predictPositionsMultiDof(btScalar dt);
 
+	//
+	// contacts
+	//
 
-    //
-    // 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.
 
-    // This routine fills out a contact constraint jacobian for this body.
-    // the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
-    // 'normal' & 'contact_point' are both given in world coordinates.
-	
 	void fillContactJacobianMultiDof(int link,
-                             const btVector3 &contact_point,
-                             const btVector3 &normal,
-                             btScalar *jac,
-                             btAlignedObjectArray<btScalar> &scratch_r,
-                             btAlignedObjectArray<btVector3> &scratch_v,
-							 btAlignedObjectArray<btMatrix3x3> &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
+									 const btVector3 &contact_point,
+									 const btVector3 &normal,
+									 btScalar *jac,
+									 btAlignedObjectArray<btScalar> &scratch_r,
+									 btAlignedObjectArray<btVector3> &scratch_v,
+									 btAlignedObjectArray<btMatrix3x3> &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
 
 	//a more general version of fillContactJacobianMultiDof which does not assume..
 	//.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
 	void fillConstraintJacobianMultiDof(int link,
-                             const btVector3 &contact_point,
-							 const btVector3 &normal_ang,
-                             const btVector3 &normal_lin,
-                             btScalar *jac,
-                             btAlignedObjectArray<btScalar> &scratch_r,
-                             btAlignedObjectArray<btVector3> &scratch_v,
-                             btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
-
+										const btVector3 &contact_point,
+										const btVector3 &normal_ang,
+										const btVector3 &normal_lin,
+										btScalar *jac,
+										btAlignedObjectArray<btScalar> &scratch_r,
+										btAlignedObjectArray<btVector3> &scratch_v,
+										btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
+
+	//
+	// sleeping
+	//
+	void setCanSleep(bool canSleep)
+	{
+		if (m_canWakeup)
+		{
+			m_canSleep = canSleep;
+		}
+	}
 
-    //
-    // sleeping
-    //
-	void	setCanSleep(bool canSleep)
+	bool getCanSleep() const
 	{
-		m_canSleep = canSleep;
+		return m_canSleep;
 	}
 
-	bool getCanSleep()const
+	bool getCanWakeup() const
 	{
-		return m_canSleep;
+		return m_canWakeup;
 	}
+	
+	void setCanWakeup(bool canWakeup) 
+	{
+		m_canWakeup = canWakeup;
+	}
+	bool isAwake() const { return m_awake; }
+	void wakeUp();
+	void goToSleep();
+	void checkMotionAndSleepIfRequired(btScalar timestep);
 
-    bool isAwake() const { return m_awake; }
-    void wakeUp();
-    void goToSleep();
-    void checkMotionAndSleepIfRequired(btScalar timestep);
-    
 	bool hasFixedBase() const
 	{
-		    return m_fixedBase;
+		return m_fixedBase;
+	}
+
+	void setFixedBase(bool fixedBase)
+	{
+		m_fixedBase = fixedBase;
 	}
 
 	int getCompanionId() const
@@ -476,16 +540,16 @@ void addJointTorque(int i, btScalar Q);
 		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
+	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;
+		return m_linearDamping;
 	}
-	void setLinearDamping( btScalar damp)
+	void setLinearDamping(btScalar damp)
 	{
 		m_linearDamping = damp;
 	}
@@ -493,11 +557,11 @@ void addJointTorque(int i, btScalar Q);
 	{
 		return m_angularDamping;
 	}
-	void setAngularDamping( btScalar damp)
+	void setAngularDamping(btScalar damp)
 	{
 		m_angularDamping = damp;
 	}
-		
+
 	bool getUseGyroTerm() const
 	{
 		return m_useGyroTerm;
@@ -506,24 +570,24 @@ void addJointTorque(int i, btScalar Q);
 	{
 		m_useGyroTerm = useGyro;
 	}
-	btScalar	getMaxCoordinateVelocity() const
+	btScalar getMaxCoordinateVelocity() const
 	{
-		return m_maxCoordinateVelocity ;
+		return m_maxCoordinateVelocity;
 	}
-	void	setMaxCoordinateVelocity(btScalar maxVel)
+	void setMaxCoordinateVelocity(btScalar maxVel)
 	{
 		m_maxCoordinateVelocity = maxVel;
 	}
 
-	btScalar	getMaxAppliedImpulse() const
+	btScalar getMaxAppliedImpulse() const
 	{
 		return m_maxAppliedImpulse;
 	}
-	void	setMaxAppliedImpulse(btScalar maxImp)
+	void setMaxAppliedImpulse(btScalar maxImp)
 	{
 		m_maxAppliedImpulse = maxImp;
 	}
-	void	setHasSelfCollision(bool hasSelfCollision)
+	void setHasSelfCollision(bool hasSelfCollision)
 	{
 		m_hasSelfCollision = hasSelfCollision;
 	}
@@ -532,7 +596,6 @@ void addJointTorque(int i, btScalar Q);
 		return m_hasSelfCollision;
 	}
 
-	
 	void finalizeMultiDof();
 
 	void useRK4Integration(bool use) { m_useRK4 = use; }
@@ -548,213 +611,278 @@ void addJointTorque(int i, btScalar Q);
 	{
 		__posUpdated = updated;
 	}
-	
+
 	//internalNeedsJointFeedback is for internal use only
 	bool internalNeedsJointFeedback() const
 	{
 		return m_internalNeedsJointFeedback;
 	}
-	void	forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
+	void forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local, btAlignedObjectArray<btVector3> & local_origin);
 
-	void	updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
-	
-	virtual	int	calculateSerializeBufferSize()	const;
+	void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const;
+
+	void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
+    void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
+
+	virtual int calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer,  class btSerializer* serializer) const;
+	virtual const char *serialize(void *dataBuffer, class btSerializer *serializer) const;
 
-	const char*				getBaseName() const
+	const char *getBaseName() const
 	{
 		return m_baseName;
 	}
 	///memory of setBaseName needs to be manager by user
-	void	setBaseName(const char* name)
+	void setBaseName(const char *name)
 	{
 		m_baseName = name;
 	}
 
-private:
-    btMultiBody(const btMultiBody &);  // not implemented
-    void operator=(const btMultiBody &);  // not implemented
+	///users can point to their objects, userPointer is not used by Bullet
+	void *getUserPointer() const
+	{
+		return m_userObjectPointer;
+	}
 
-    void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
+	int getUserIndex() const
+	{
+		return m_userIndex;
+	}
 
-	void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
-	void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
-	
-	void updateLinksDofOffsets()
+	int getUserIndex2() const
 	{
-		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;
-		}
+		return m_userIndex2;
+	}
+	///users can point to their objects, userPointer is not used by Bullet
+	void setUserPointer(void *userPointer)
+	{
+		m_userObjectPointer = userPointer;
 	}
 
-	void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
-	
-	
-private:
+	///users can point to their objects, userPointer is not used by Bullet
+	void setUserIndex(int index)
+	{
+		m_userIndex = index;
+	}
 
-	btMultiBodyLinkCollider* m_baseCollider;//can be NULL
-	const char*				m_baseName;//memory needs to be manager by user!
+	void setUserIndex2(int index)
+	{
+		m_userIndex2 = index;
+	}
 
-    btVector3 m_basePos;       // position of COM of base (world frame)
-    btQuaternion m_baseQuat;   // rotates world points into base frame
+	static void spatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
+		const btVector3 &displacement,     // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
+		const btVector3 &top_in,       // top part of input vector
+		const btVector3 &bottom_in,    // bottom part of input vector
+		btVector3 &top_out,         // top part of output vector
+		btVector3 &bottom_out);      // bottom part of output vector
 
-    btScalar m_baseMass;         // mass of the base
-    btVector3 m_baseInertia;   // inertia of the base (in local frame; diagonal)
 
-    btVector3 m_baseForce;     // external force applied to base. World frame.
-    btVector3 m_baseTorque;    // external torque applied to base. World frame.
-   
-    btVector3 m_baseConstraintForce;     // external force applied to base. World frame.
-    btVector3 m_baseConstraintTorque;    // external torque applied to base. World frame.
- 
-    btAlignedObjectArray<btMultibodyLink> m_links;    // array of m_links, excluding the base. index from 0 to num_links-1.
-	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_deltaV; 
-    btAlignedObjectArray<btScalar> m_realBuf;
-    btAlignedObjectArray<btVector3> m_vectorBuf;
-    btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
+private:
+	btMultiBody(const btMultiBody &);     // not implemented
+	void operator=(const btMultiBody &);  // not implemented
 
+	void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const;
+	void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
+
+	void updateLinksDofOffsets()
+	{
+		int dofOffset = 0, cfgOffset = 0;
+		for (int bidx = 0; bidx < m_links.size(); ++bidx)
+		{
+			m_links[bidx].m_dofOffset = dofOffset;
+			m_links[bidx].m_cfgOffset = cfgOffset;
+			dofOffset += m_links[bidx].m_dofCount;
+			cfgOffset += m_links[bidx].m_posVarCount;
+		}
+	}
+
+	void mulMatrix(btScalar * pA, btScalar * pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
+
+private:
+	btMultiBodyLinkCollider *m_baseCollider;  //can be NULL
+	const char *m_baseName;                   //memory needs to be manager by user!
+
+	btVector3 m_basePos;      // position of COM of base (world frame)
+    btVector3 m_basePos_interpolate;      // position of interpolated COM of base (world frame)
+	btQuaternion m_baseQuat;  // rotates world points into base frame
+    btQuaternion m_baseQuat_interpolate;  
+
+	btScalar m_baseMass;      // mass of the base
+	btVector3 m_baseInertia;  // inertia of the base (in local frame; diagonal)
+
+	btVector3 m_baseForce;   // external force applied to base. World frame.
+	btVector3 m_baseTorque;  // external torque applied to base. World frame.
+
+	btVector3 m_baseConstraintForce;   // external force applied to base. World frame.
+	btVector3 m_baseConstraintTorque;  // external torque applied to base. World frame.
+
+	btAlignedObjectArray<btMultibodyLink> m_links;  // array of m_links, excluding the base. index from 0 to num_links-1.
+
+	//
+	// realBuf:
+	//  offset         size            array
+	//   0              6 + num_links   v (base_omega; base_vel; joint_vels)					MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized]
+	//   6+num_links    num_links       D
+	//
+	// vectorBuf:
+	//  offset         size         array
+	//   0              num_links    h_top
+	//   num_links      num_links    h_bottom
+	//
+	// matrixBuf:
+	//  offset         size         array
+	//   0              num_links+1  rot_from_parent
+	//
+    btAlignedObjectArray<btScalar> m_splitV;
+	btAlignedObjectArray<btScalar> m_deltaV;
+	btAlignedObjectArray<btScalar> m_realBuf;
+	btAlignedObjectArray<btVector3> m_vectorBuf;
+	btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
 
 	btMatrix3x3 m_cachedInertiaTopLeft;
 	btMatrix3x3 m_cachedInertiaTopRight;
 	btMatrix3x3 m_cachedInertiaLowerLeft;
 	btMatrix3x3 m_cachedInertiaLowerRight;
+	bool m_cachedInertiaValid;
 
-    bool m_fixedBase;
+	bool m_fixedBase;
 
-    // Sleep parameters.
-    bool m_awake;
-    bool m_canSleep;
-    btScalar m_sleepTimer;
+	// Sleep parameters.
+	bool m_awake;
+	bool m_canSleep;
+	bool m_canWakeup;
+	btScalar m_sleepTimer;
+
+	void *m_userObjectPointer;
+	int m_userIndex2;
+	int m_userIndex;
+
+	int m_companionId;
+	btScalar m_linearDamping;
+	btScalar m_angularDamping;
+	bool m_useGyroTerm;
+	btScalar m_maxAppliedImpulse;
+	btScalar m_maxCoordinateVelocity;
+	bool m_hasSelfCollision;
+
+	bool __posUpdated;
+	int m_dofCount, m_posVarCnt;
 
-	int	m_companionId;
-	btScalar	m_linearDamping;
-	btScalar	m_angularDamping;
-	bool	m_useGyroTerm;
-	btScalar	m_maxAppliedImpulse;
-	btScalar	m_maxCoordinateVelocity;
-	bool		m_hasSelfCollision;
-	
-		bool __posUpdated;
-		int m_dofCount, m_posVarCnt;
 	bool m_useRK4, m_useGlobalVelocities;
-	
+	//for global velocities, see 8.3.2B Proposed resolution in Jakub Stepien PhD Thesis
+	//https://drive.google.com/file/d/0Bz3vEa19XOYGNWdZWGpMdUdqVmZ5ZVBOaEh4ZnpNaUxxZFNV/view?usp=sharing
+
 	///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
 	bool m_internalNeedsJointFeedback;
 };
 
 struct btMultiBodyLinkDoubleData
 {
-	btQuaternionDoubleData	m_zeroRotParentToThis;
-	btVector3DoubleData		m_parentComToThisComOffset;
-	btVector3DoubleData		m_thisPivotToThisComOffset;
-	btVector3DoubleData		m_jointAxisTop[6];
-	btVector3DoubleData		m_jointAxisBottom[6];
-
-
-	char					*m_linkName;
-	char					*m_jointName;
-	btCollisionObjectDoubleData	*m_linkCollider;
-	
-	btVector3DoubleData		m_linkInertia;   // inertia of the base (in local frame; diagonal)
-	double					m_linkMass;
-	int						m_parentIndex;
-	int						m_jointType;
-	
-
-	
-
-	int						m_dofCount;
-	int						m_posVarCount;
-	double					m_jointPos[7];
-	double					m_jointVel[6];
-	double					m_jointTorque[6];
-	
-	
-	
+	btQuaternionDoubleData m_zeroRotParentToThis;
+	btVector3DoubleData m_parentComToThisPivotOffset;
+	btVector3DoubleData m_thisPivotToThisComOffset;
+	btVector3DoubleData m_jointAxisTop[6];
+	btVector3DoubleData m_jointAxisBottom[6];
+
+	btVector3DoubleData m_linkInertia;  // inertia of the base (in local frame; diagonal)
+	btVector3DoubleData m_absFrameTotVelocityTop;
+	btVector3DoubleData m_absFrameTotVelocityBottom;
+	btVector3DoubleData m_absFrameLocVelocityTop;
+	btVector3DoubleData m_absFrameLocVelocityBottom;
+
+	double m_linkMass;
+	int m_parentIndex;
+	int m_jointType;
+
+	int m_dofCount;
+	int m_posVarCount;
+	double m_jointPos[7];
+	double m_jointVel[6];
+	double m_jointTorque[6];
+
+	double m_jointDamping;
+	double m_jointFriction;
+	double m_jointLowerLimit;
+	double m_jointUpperLimit;
+	double m_jointMaxForce;
+	double m_jointMaxVelocity;
+
+	char *m_linkName;
+	char *m_jointName;
+	btCollisionObjectDoubleData *m_linkCollider;
+	char *m_paddingPtr;
 };
 
 struct btMultiBodyLinkFloatData
 {
-	btQuaternionFloatData	m_zeroRotParentToThis;
-	btVector3FloatData		m_parentComToThisComOffset;
-	btVector3FloatData		m_thisPivotToThisComOffset;
-	btVector3FloatData		m_jointAxisTop[6];
-	btVector3FloatData		m_jointAxisBottom[6];
-	
-
-	char				*m_linkName;
-	char				*m_jointName;
-	btCollisionObjectFloatData	*m_linkCollider;
-	
-	btVector3FloatData	m_linkInertia;   // inertia of the base (in local frame; diagonal)
-	int						m_dofCount;
-	float				m_linkMass;
-	int					m_parentIndex;
-	int					m_jointType;
-	
-
-		
-	float					m_jointPos[7];
-	float					m_jointVel[6];
-	float					m_jointTorque[6];
-	int						m_posVarCount;
-	
-
+	btQuaternionFloatData m_zeroRotParentToThis;
+	btVector3FloatData m_parentComToThisPivotOffset;
+	btVector3FloatData m_thisPivotToThisComOffset;
+	btVector3FloatData m_jointAxisTop[6];
+	btVector3FloatData m_jointAxisBottom[6];
+	btVector3FloatData m_linkInertia;  // inertia of the base (in local frame; diagonal)
+	btVector3FloatData m_absFrameTotVelocityTop;
+	btVector3FloatData m_absFrameTotVelocityBottom;
+	btVector3FloatData m_absFrameLocVelocityTop;
+	btVector3FloatData m_absFrameLocVelocityBottom;
+
+	int m_dofCount;
+	float m_linkMass;
+	int m_parentIndex;
+	int m_jointType;
+
+	float m_jointPos[7];
+	float m_jointVel[6];
+	float m_jointTorque[6];
+	int m_posVarCount;
+	float m_jointDamping;
+	float m_jointFriction;
+	float m_jointLowerLimit;
+	float m_jointUpperLimit;
+	float m_jointMaxForce;
+	float m_jointMaxVelocity;
+
+	char *m_linkName;
+	char *m_jointName;
+	btCollisionObjectFloatData *m_linkCollider;
+	char *m_paddingPtr;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btMultiBodyDoubleData
+struct btMultiBodyDoubleData
 {
-	char	*m_baseName;
-	btMultiBodyLinkDoubleData	*m_links;
-	btCollisionObjectDoubleData	*m_baseCollider;
-
-	btTransformDoubleData m_baseWorldTransform;
-	btVector3DoubleData m_baseInertia;   // inertia of the base (in local frame; diagonal)
-	
-	int		m_numLinks;
-	double	m_baseMass;
-
+	btVector3DoubleData m_baseWorldPosition;
+	btQuaternionDoubleData m_baseWorldOrientation;
+	btVector3DoubleData m_baseLinearVelocity;
+	btVector3DoubleData m_baseAngularVelocity;
+	btVector3DoubleData m_baseInertia;  // inertia of the base (in local frame; diagonal)
+	double m_baseMass;
+	int m_numLinks;
 	char m_padding[4];
-	
+
+	char *m_baseName;
+	btMultiBodyLinkDoubleData *m_links;
+	btCollisionObjectDoubleData *m_baseCollider;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btMultiBodyFloatData
+struct btMultiBodyFloatData
 {
-	char	*m_baseName;
-	btMultiBodyLinkFloatData	*m_links;
-	btCollisionObjectFloatData	*m_baseCollider;
-	btTransformFloatData m_baseWorldTransform;
-	btVector3FloatData m_baseInertia;   // inertia of the base (in local frame; diagonal)
-	
-	float	m_baseMass;
-	int		m_numLinks;
+	btVector3FloatData m_baseWorldPosition;
+	btQuaternionFloatData m_baseWorldOrientation;
+	btVector3FloatData m_baseLinearVelocity;
+	btVector3FloatData m_baseAngularVelocity;
+
+	btVector3FloatData m_baseInertia;  // inertia of the base (in local frame; diagonal)
+	float m_baseMass;
+	int m_numLinks;
+
+	char *m_baseName;
+	btMultiBodyLinkFloatData *m_links;
+	btCollisionObjectFloatData *m_baseCollider;
 };
 
-
-
 #endif
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
index 12997d2e374..1ba5861145b 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
@@ -1,32 +1,30 @@
 #include "btMultiBodyConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
-#include "btMultiBodyPoint2Point.h"				//for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
-
-
-
-btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
-	:m_bodyA(bodyA),
-	m_bodyB(bodyB),
-	m_linkA(linkA),
-	m_linkB(linkB),
-	m_numRows(numRows),
-	m_jacSizeA(0),
-	m_jacSizeBoth(0),
-	m_isUnilateral(isUnilateral),
-	m_numDofsFinalized(-1),
-	m_maxAppliedImpulse(100)
+#include "btMultiBodyPoint2Point.h"  //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
+
+btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type)
+	: m_bodyA(bodyA),
+	  m_bodyB(bodyB),
+	  m_linkA(linkA),
+	  m_linkB(linkB),
+	  m_type(type),
+	  m_numRows(numRows),
+	  m_jacSizeA(0),
+	  m_jacSizeBoth(0),
+	  m_isUnilateral(isUnilateral),
+	  m_numDofsFinalized(-1),
+	  m_maxAppliedImpulse(100)
 {
-
 }
 
 void btMultiBodyConstraint::updateJacobianSizes()
 {
-    if(m_bodyA)
+	if (m_bodyA)
 	{
 		m_jacSizeA = (6 + m_bodyA->getNumDofs());
 	}
 
-	if(m_bodyB)
+	if (m_bodyB)
 	{
 		m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
 	}
@@ -38,7 +36,7 @@ void btMultiBodyConstraint::allocateJacobiansMultiDof()
 {
 	updateJacobianSizes();
 
-	m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
+	m_posOffset = ((1 + m_jacSizeBoth) * m_numRows);
 	m_data.resize((2 + m_jacSizeBoth) * m_numRows);
 }
 
@@ -46,25 +44,25 @@ btMultiBodyConstraint::~btMultiBodyConstraint()
 {
 }
 
-void	btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
+void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
 {
 	for (int i = 0; i < ndof; ++i)
-		data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
+		data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse;
 }
 
-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)
+btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
+														btMultiBodyJacobianData& data,
+														btScalar* jacOrgA, btScalar* jacOrgB,
+														const btVector3& constraintNormalAng,
+														const btVector3& constraintNormalLin,
+														const btVector3& posAworld, const btVector3& posBworld,
+														btScalar posError,
+														const btContactSolverInfo& infoGlobal,
+														btScalar lowerLimit, btScalar upperLimit,
+														bool angConstraint,
+														btScalar relaxation,
+														bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 {
-
-
 	solverConstraint.m_multiBodyA = m_bodyA;
 	solverConstraint.m_multiBodyB = m_bodyB;
 	solverConstraint.m_linkA = m_linkA;
@@ -79,7 +77,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 	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
+	btVector3 rel_pos1, rel_pos2;  //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
 	if (bodyA)
 		rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
 	if (bodyB)
@@ -87,121 +85,156 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 
 	if (multiBodyA)
 	{
-		if (solverConstraint.m_linkA<0)
+		if (solverConstraint.m_linkA < 0)
 		{
 			rel_pos1 = posAworld - multiBodyA->getBasePos();
-		} else
+		}
+		else
 		{
 			rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
 		}
 
-		const int ndofA  = multiBodyA->getNumDofs() + 6;
+		const int ndofA = multiBodyA->getNumDofs() + 6;
 
 		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
 
-		if (solverConstraint.m_deltaVelAindex <0)
+		if (solverConstraint.m_deltaVelAindex < 0)
 		{
 			solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
 			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
-			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
-		} else
+			data.m_deltaVelocities.resize(data.m_deltaVelocities.size() + ndofA);
+		}
+		else
 		{
-			btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
+			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_jacobians.resize(data.m_jacobians.size() + ndofA);
 		//copy/determine
-		if(jacOrgA)
+		if (jacOrgA)
 		{
-			for (int i=0;i<ndofA;i++)
-				data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
+			for (int i = 0; i < ndofA; i++)
+				data.m_jacobians[solverConstraint.m_jacAindex + i] = jacOrgA[i];
 		}
 		else
 		{
-			btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
-			multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+			btScalar* jac1 = &data.m_jacobians[solverConstraint.m_jacAindex];
+			//multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+			multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
 		}
 
 		//determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
 		//resize..
-		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);		//=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size() + ndofA);  //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
 		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
 		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 		//determine..
-		multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+		multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex], delta, data.scratch_r, data.scratch_v);
 
-		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
+		btVector3 torqueAxis0;
+		if (angConstraint)
+		{
+			torqueAxis0 = constraintNormalAng;
+		}
+		else
+		{
+			torqueAxis0 = rel_pos1.cross(constraintNormalLin);
+		}
 		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-		solverConstraint.m_contactNormal1 = contactNormalOnB;
+		solverConstraint.m_contactNormal1 = constraintNormalLin;
 	}
-	else //if(rb0)
+	else  //if(rb0)
 	{
-		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
-		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+		btVector3 torqueAxis0;
+		if (angConstraint)
+		{
+			torqueAxis0 = constraintNormalAng;
+		}
+		else
+		{
+			torqueAxis0 = rel_pos1.cross(constraintNormalLin);
+		}
+		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
 		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-		solverConstraint.m_contactNormal1 = contactNormalOnB;
+		solverConstraint.m_contactNormal1 = constraintNormalLin;
 	}
 
 	if (multiBodyB)
 	{
-		if (solverConstraint.m_linkB<0)
+		if (solverConstraint.m_linkB < 0)
 		{
 			rel_pos2 = posBworld - multiBodyB->getBasePos();
-		} else
+		}
+		else
 		{
 			rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
 		}
 
-		const int ndofB  = multiBodyB->getNumDofs() + 6;
+		const int ndofB = multiBodyB->getNumDofs() + 6;
 
 		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
-		if (solverConstraint.m_deltaVelBindex <0)
+		if (solverConstraint.m_deltaVelBindex < 0)
 		{
 			solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
 			multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
-			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
+			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);
+		data.m_jacobians.resize(data.m_jacobians.size() + ndofB);
 		//copy/determine..
-		if(jacOrgB)
+		if (jacOrgB)
 		{
-			for (int i=0;i<ndofB;i++)
-				data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
+			for (int i = 0; i < ndofB; i++)
+				data.m_jacobians[solverConstraint.m_jacBindex + i] = jacOrgB[i];
 		}
 		else
 		{
-			multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+			//multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+			multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
 		}
 
 		//determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
 		//resize..
-		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size() + ndofB);
 		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
 		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 		//determine..
-		multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
+		multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex], delta, data.scratch_r, data.scratch_v);
 
-		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
+		btVector3 torqueAxis1;
+		if (angConstraint)
+		{
+			torqueAxis1 = constraintNormalAng;
+		}
+		else
+		{
+			torqueAxis1 = rel_pos2.cross(constraintNormalLin);
+		}
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
-		solverConstraint.m_contactNormal2 = -contactNormalOnB;
-
+		solverConstraint.m_contactNormal2 = -constraintNormalLin;
 	}
-	else //if(rb1)
+	else  //if(rb1)
 	{
-		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
-		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+		btVector3 torqueAxis1;
+		if (angConstraint)
+		{
+			torqueAxis1 = constraintNormalAng;
+		}
+		else
+		{
+			torqueAxis1 = rel_pos2.cross(constraintNormalLin);
+		}
+		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
-		solverConstraint.m_contactNormal2 = -contactNormalOnB;
+		solverConstraint.m_contactNormal2 = -constraintNormalLin;
 	}
 	{
-
 		btVector3 vec;
 		btScalar denom0 = 0.f;
 		btScalar denom1 = 0.f;
@@ -209,7 +242,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 		btScalar* jacA = 0;
 		btScalar* deltaVelA = 0;
 		btScalar* deltaVelB = 0;
-		int ndofA  = 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)
 		{
@@ -218,15 +251,22 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 			deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA; ++i)
 			{
-				btScalar j = jacA[i] ;
+				btScalar j = jacA[i];
 				btScalar l = deltaVelA[i];
-				denom0 += j*l;
+				denom0 += j * l;
 			}
 		}
-		else if(rb0)
+		else if (rb0)
 		{
-			vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
-			denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
+			vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
+			if (angConstraint)
+			{
+				denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA);
+			}
+			else
+			{
+				denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
+			}
 		}
 		//
 		if (multiBodyB)
@@ -236,134 +276,108 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 			deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB; ++i)
 			{
-				btScalar j = jacB[i] ;
+				btScalar j = jacB[i];
 				btScalar l = deltaVelB[i];
-				denom1 += j*l;
+				denom1 += j * l;
 			}
-
 		}
-		else if(rb1)
+		else if (rb1)
 		{
-			vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
-			denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
+			vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
+			if (angConstraint)
+			{
+				denom1 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB);
+			}
+			else
+			{
+				denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
+			}
 		}
 
 		//
-		btScalar d = denom0+denom1;
-		if (d>SIMD_EPSILON)
+		btScalar d = denom0 + denom1;
+		if (d > SIMD_EPSILON)
 		{
-			solverConstraint.m_jacDiagABInv = relaxation/(d);
+			solverConstraint.m_jacDiagABInv = relaxation / (d);
 		}
 		else
 		{
-		//disable the constraint row to handle singularity/redundant constraint
-			solverConstraint.m_jacDiagABInv  = 0.f;
+			//disable the constraint row to handle singularity/redundant constraint
+			solverConstraint.m_jacDiagABInv = 0.f;
 		}
 	}
 
-
 	//compute rhs and remaining solverConstraint fields
-	btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
+	btScalar penetration = isFriction ? 0 : posError;
 
 	btScalar rel_vel = 0.f;
-	int ndofA  = 0;
-	int ndofB  = 0;
+	int ndofA = 0;
+	int ndofB = 0;
 	{
-		btVector3 vel1,vel2;
+		btVector3 vel1, vel2;
 		if (multiBodyA)
 		{
 			ndofA = multiBodyA->getNumDofs() + 6;
 			btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
-			for (int i = 0; i < ndofA ; ++i)
+			for (int i = 0; i < ndofA; ++i)
 				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
 		}
-		else if(rb0)
+		else if (rb0)
 		{
-			rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+			rel_vel += rb0->getLinearVelocity().dot(solverConstraint.m_contactNormal1);
+			rel_vel += rb0->getAngularVelocity().dot(solverConstraint.m_relpos1CrossNormal);
 		}
 		if (multiBodyB)
 		{
 			ndofB = multiBodyB->getNumDofs() + 6;
 			btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
-			for (int i = 0; i < ndofB ; ++i)
+			for (int i = 0; i < ndofB; ++i)
 				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-
 		}
-		else if(rb1)
+		else if (rb1)
 		{
-			rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+			rel_vel += rb1->getLinearVelocity().dot(solverConstraint.m_contactNormal2);
+			rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal);
 		}
 
-		solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
+		solverConstraint.m_friction = 0.f;  //cp.m_combinedFriction;
 	}
 
-
-	///warm starting (or zero if disabled)
-	/*
-	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-	{
-		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-
-		if (solverConstraint.m_appliedImpulse)
-		{
-			if (multiBodyA)
-			{
-				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-				multiBodyA->applyDeltaVee(deltaV,impulse);
-				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
-			} else
-			{
-				if (rb0)
-					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-			}
-			if (multiBodyB)
-			{
-				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-				multiBodyB->applyDeltaVee(deltaV,impulse);
-				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
-			} else
-			{
-				if (rb1)
-					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
-			}
-		}
-	} else
-	*/
-
 	solverConstraint.m_appliedImpulse = 0.f;
 	solverConstraint.m_appliedPushImpulse = 0.f;
 
 	{
-
 		btScalar positionalError = 0.f;
-		btScalar	velocityError = desiredVelocity - rel_vel;// * damping;
-
+		btScalar velocityError = desiredVelocity - rel_vel;  // * damping;
 
 		btScalar erp = infoGlobal.m_erp2;
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+
+		//split impulse is not implemented yet for btMultiBody*
+		//if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 		{
 			erp = infoGlobal.m_erp;
 		}
 
-		positionalError = -penetration * erp/infoGlobal.m_timeStep;
+		positionalError = -penetration * erp / infoGlobal.m_timeStep;
 
-		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
-		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+		btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
+		btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
 
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		//split impulse is not implemented yet for btMultiBody*
+
+		//  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 		{
 			//combine position and velocity into rhs
-			solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+			solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
 			solverConstraint.m_rhsPenetration = 0.f;
-
-		} else
-		{
-			//split position and velocity into rhs and m_rhsPenetration
-			solverConstraint.m_rhs = velocityImpulse;
-			solverConstraint.m_rhsPenetration = penetrationImpulse;
 		}
+		/*else
+        {
+            //split position and velocity into rhs and m_rhsPenetration
+            solverConstraint.m_rhs = velocityImpulse;
+            solverConstraint.m_rhsPenetration = penetrationImpulse;
+        }
+        */
 
 		solverConstraint.m_cfm = 0.f;
 		solverConstraint.m_lowerLimit = lowerLimit;
@@ -371,5 +385,4 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 	}
 
 	return rel_vel;
-
 }
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
index 3b32b46e911..4a6007ee3ed 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
@@ -20,6 +20,21 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 #include "btMultiBody.h"
 
+
+//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
+enum btTypedMultiBodyConstraintType
+{
+	MULTIBODY_CONSTRAINT_LIMIT=3,
+	MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR,
+	MULTIBODY_CONSTRAINT_GEAR,
+	MULTIBODY_CONSTRAINT_POINT_TO_POINT,
+	MULTIBODY_CONSTRAINT_SLIDER,
+	MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR,
+	MULTIBODY_CONSTRAINT_FIXED,
+	
+	MAX_MULTIBODY_CONSTRAINT_TYPE,
+};
+
 class btMultiBody;
 struct btSolverInfo;
 
@@ -27,152 +42,174 @@ struct btSolverInfo;
 
 struct btMultiBodyJacobianData
 {
-	btAlignedObjectArray<btScalar>		m_jacobians;
-	btAlignedObjectArray<btScalar>		m_deltaVelocitiesUnitImpulse;	//holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
-	btAlignedObjectArray<btScalar>		m_deltaVelocities;				//holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
-	btAlignedObjectArray<btScalar>		scratch_r;
-	btAlignedObjectArray<btVector3>		scratch_v;
-	btAlignedObjectArray<btMatrix3x3>	scratch_m;
-	btAlignedObjectArray<btSolverBody>*	m_solverBodyPool;
-	int									m_fixedBodyId;
-
+	btAlignedObjectArray<btScalar> m_jacobians;
+	btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse;  //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
+	btAlignedObjectArray<btScalar> m_deltaVelocities;             //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
+	btAlignedObjectArray<btScalar> scratch_r;
+	btAlignedObjectArray<btVector3> scratch_v;
+	btAlignedObjectArray<btMatrix3x3> scratch_m;
+	btAlignedObjectArray<btSolverBody>* m_solverBodyPool;
+	int m_fixedBodyId;
 };
 
-
-class btMultiBodyConstraint
+ATTRIBUTE_ALIGNED16(class)
+btMultiBodyConstraint
 {
 protected:
-
-	btMultiBody*	m_bodyA;
-    btMultiBody*	m_bodyB;
-    int				m_linkA;
-    int				m_linkB;
-
-    int				m_numRows;
-    int				m_jacSizeA;
-    int				m_jacSizeBoth;
-    int				m_posOffset;
-
-	bool			m_isUnilateral;
-	int				m_numDofsFinalized;
-	btScalar		m_maxAppliedImpulse;
-
-
-    // warning: the data block lay out is not consistent for all constraints
-    // data block laid out as follows:
-    // cached impulses. (one per row.)
-    // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
-    // positions. (one per row.)
-    btAlignedObjectArray<btScalar> m_data;
-
-	void	applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
-
-	btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
-																btMultiBodyJacobianData& data,
-																btScalar* jacOrgA, btScalar* jacOrgB,
-																const btVector3& 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);
+	btMultiBody* m_bodyA;
+	btMultiBody* m_bodyB;
+	int m_linkA;
+	int m_linkB;
+
+	int m_type; //btTypedMultiBodyConstraintType
+
+	int m_numRows;
+	int m_jacSizeA;
+	int m_jacSizeBoth;
+	int m_posOffset;
+
+	bool m_isUnilateral;
+	int m_numDofsFinalized;
+	btScalar m_maxAppliedImpulse;
+
+	// warning: the data block lay out is not consistent for all constraints
+	// data block laid out as follows:
+	// cached impulses. (one per row.)
+	// jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
+	// positions. (one per row.)
+	btAlignedObjectArray<btScalar> m_data;
+
+	void applyDeltaVee(btMultiBodyJacobianData & data, btScalar * delta_vee, btScalar impulse, int velocityIndex, int ndof);
+
+	btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint & solverConstraint,
+									 btMultiBodyJacobianData & data,
+									 btScalar * jacOrgA, btScalar * jacOrgB,
+									 const btVector3& constraintNormalAng,
+
+									 const btVector3& constraintNormalLin,
+									 const btVector3& posAworld, const btVector3& posBworld,
+									 btScalar posError,
+									 const btContactSolverInfo& infoGlobal,
+									 btScalar lowerLimit, btScalar upperLimit,
+									 bool angConstraint = false,
+
+									 btScalar relaxation = 1.f,
+									 bool isFriction = false, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
 
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
+	btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type);
 	virtual ~btMultiBodyConstraint();
 
 	void updateJacobianSizes();
 	void allocateJacobiansMultiDof();
 
-	virtual void finalizeMultiDof()=0;
+	int getConstraintType() const
+	{
+		return m_type;
+	}
+	//many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later.
+	virtual void setFrameInB(const btMatrix3x3& frameInB) {}
+	virtual void setPivotInB(const btVector3& pivotInB) {}
 
-	virtual int getIslandIdA() const =0;
-	virtual int getIslandIdB() const =0;
+	virtual void finalizeMultiDof() = 0;
 
-	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal)=0;
+	virtual int getIslandIdA() const = 0;
+	virtual int getIslandIdB() const = 0;
 
-	int	getNumRows() const
+	virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows,
+									  btMultiBodyJacobianData & data,
+									  const btContactSolverInfo& infoGlobal) = 0;
+
+	int getNumRows() const
 	{
 		return m_numRows;
 	}
 
-	btMultiBody*	getMultiBodyA()
+	btMultiBody* getMultiBodyA()
 	{
 		return m_bodyA;
 	}
-    btMultiBody*	getMultiBodyB()
+	btMultiBody* getMultiBodyB()
 	{
 		return m_bodyB;
 	}
 
-	void	internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
+	int getLinkA() const
+	{
+		return m_linkA;
+	}
+	int getLinkB() const
+	{
+		return m_linkB;
+	}
+	void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
 	{
-		btAssert(dof>=0);
+		btAssert(dof >= 0);
 		btAssert(dof < getNumRows());
 		m_data[dof] = appliedImpulse;
 	}
-	
-	btScalar	getAppliedImpulse(int dof)
+
+	btScalar getAppliedImpulse(int dof)
 	{
-		btAssert(dof>=0);
+		btAssert(dof >= 0);
 		btAssert(dof < getNumRows());
 		return m_data[dof];
 	}
 	// current constraint position
-    // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
-    // NOTE: ignored position for friction rows.
-    btScalar getPosition(int row) const
+	// constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
+	// NOTE: ignored position for friction rows.
+	btScalar getPosition(int row) const
 	{
 		return m_data[m_posOffset + row];
 	}
 
-    void setPosition(int row, btScalar pos)
+	void setPosition(int row, btScalar pos)
 	{
 		m_data[m_posOffset + row] = pos;
 	}
 
-
 	bool isUnilateral() const
 	{
 		return m_isUnilateral;
 	}
 
 	// jacobian blocks.
-    // each of size 6 + num_links. (jacobian2 is null if no body2.)
-    // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
-    btScalar* jacobianA(int row)
+	// each of size 6 + num_links. (jacobian2 is null if no body2.)
+	// format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
+	btScalar* jacobianA(int row)
 	{
 		return &m_data[m_numRows + row * m_jacSizeBoth];
 	}
-    const btScalar* jacobianA(int row) const
+	const btScalar* jacobianA(int row) const
 	{
 		return &m_data[m_numRows + (row * m_jacSizeBoth)];
 	}
-    btScalar* jacobianB(int row)
+	btScalar* jacobianB(int row)
 	{
 		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
 	}
-    const btScalar* jacobianB(int row) const
+	const btScalar* jacobianB(int row) const
 	{
 		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
 	}
 
-	btScalar	getMaxAppliedImpulse() const
+	btScalar getMaxAppliedImpulse() const
 	{
 		return m_maxAppliedImpulse;
 	}
-	void	setMaxAppliedImpulse(btScalar maxImp)
+	void setMaxAppliedImpulse(btScalar maxImp)
 	{
 		m_maxAppliedImpulse = maxImp;
 	}
 
-	virtual void debugDraw(class btIDebugDraw* drawer)=0;
+	virtual void debugDraw(class btIDebugDraw * drawer) = 0;
 
+	virtual void setGearRatio(btScalar ratio) {}
+	virtual void setGearAuxLink(int gearAuxLink) {}
+	virtual void setRelativePositionTarget(btScalar relPosTarget) {}
+	virtual void setErp(btScalar erp) {}
 };
 
-#endif //BT_MULTIBODY_CONSTRAINT_H
-
+#endif  //BT_MULTIBODY_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
index 4f66b20b2c1..2788367431e 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btMultiBodyConstraintSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 #include "btMultiBodyLinkCollider.h"
@@ -23,73 +22,197 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 
 #include "LinearMath/btQuickprof.h"
+#include "BulletDynamics/Featherstone/btMultiBodySolverConstraint.h"
+#include "LinearMath/btScalar.h"
 
-btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
-	btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
-	
-	//solve featherstone non-contact constraints
+	btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
+	//solve featherstone non-contact constraints
+	btScalar nonContactResidual = 0;
 	//printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
-	for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
+	for (int i = 0; i < infoGlobal.m_numNonContactInnerIterations; ++i)
 	{
-		btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
-		
-		resolveSingleConstraintRowGeneric(constraint);
-		if(constraint.m_multiBodyA) 
-			constraint.m_multiBodyA->setPosUpdated(false);
-		if(constraint.m_multiBodyB) 
-			constraint.m_multiBodyB->setPosUpdated(false);
+		// reset the nonContactResdual to 0 at start of each inner iteration
+		nonContactResidual = 0;
+		for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
+		{
+			int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
+
+			btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
+
+			btScalar residual = resolveSingleConstraintRowGeneric(constraint);
+			nonContactResidual = btMax(nonContactResidual, residual * residual);
+
+			if (constraint.m_multiBodyA)
+				constraint.m_multiBodyA->setPosUpdated(false);
+			if (constraint.m_multiBodyB)
+				constraint.m_multiBodyB->setPosUpdated(false);
+		}
 	}
+	leastSquaredResidual = btMax(leastSquaredResidual, nonContactResidual);
 
 	//solve featherstone normal contact
-	for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
+	for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++)
 	{
-		btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
+		int index = j0;  //iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
+
+		btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[index];
+		btScalar residual = 0.f;
+
 		if (iteration < infoGlobal.m_numIterations)
-			resolveSingleConstraintRowGeneric(constraint);
+		{
+			residual = resolveSingleConstraintRowGeneric(constraint);
+		}
+
+		leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
 
-		if(constraint.m_multiBodyA) 
+		if (constraint.m_multiBodyA)
 			constraint.m_multiBodyA->setPosUpdated(false);
-		if(constraint.m_multiBodyB) 
+		if (constraint.m_multiBodyB)
 			constraint.m_multiBodyB->setPosUpdated(false);
 	}
-	
+
 	//solve featherstone frictional contact
+	if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0))
+	{
+		for (int j1 = 0; j1 < this->m_multiBodySpinningFrictionContactConstraints.size(); j1++)
+		{
+			if (iteration < infoGlobal.m_numIterations)
+			{
+				int index = j1;
+
+				btMultiBodySolverConstraint& frictionConstraint = m_multiBodySpinningFrictionContactConstraints[index];
+				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				//adjust friction limits here
+				if (totalImpulse > btScalar(0))
+				{
+					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
+					frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
+					btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
+					leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
+
+					if (frictionConstraint.m_multiBodyA)
+						frictionConstraint.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraint.m_multiBodyB)
+						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				}
+			}
+		}
+
+		for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++)
+		{
+			if (iteration < infoGlobal.m_numIterations)
+			{
+				int index = j1;  //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1;
+
+				btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index];
+				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				j1++;
+				int index2 = j1;
+				btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyTorsionalFrictionContactConstraints[index2];
+				//adjust friction limits here
+				if (totalImpulse > btScalar(0) && frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
+				{
+					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
+					frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
+					frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
+					frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
+
+					btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
+					leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
+
+					if (frictionConstraint.m_multiBodyA)
+						frictionConstraint.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraint.m_multiBodyB)
+						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+
+					if (frictionConstraintB.m_multiBodyA)
+						frictionConstraintB.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraintB.m_multiBodyB)
+						frictionConstraintB.m_multiBodyB->setPosUpdated(false);
+				}
+			}
+		}
+
+		for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
+		{
+			if (iteration < infoGlobal.m_numIterations)
+			{
+				int index = j1;  //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
+				btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
 
-	for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++)
+				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				j1++;
+				int index2 = j1;  //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
+				btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyFrictionContactConstraints[index2];
+				btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex);
+
+				if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
+				{
+					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
+					frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
+					frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
+					frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
+					btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
+					leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
+
+					if (frictionConstraintB.m_multiBodyA)
+						frictionConstraintB.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraintB.m_multiBodyB)
+						frictionConstraintB.m_multiBodyB->setPosUpdated(false);
+
+					if (frictionConstraint.m_multiBodyA)
+						frictionConstraint.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraint.m_multiBodyB)
+						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				}
+			}
+		}
+	}
+	else
 	{
-		if (iteration < infoGlobal.m_numIterations)
+		for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
 		{
-			btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j];
-			btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
-			//adjust friction limits here
-			if (totalImpulse>btScalar(0))
+			if (iteration < infoGlobal.m_numIterations)
 			{
-				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);
+				int index = j1;  //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
+
+				btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
+				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				//adjust friction limits here
+				if (totalImpulse > btScalar(0))
+				{
+					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
+					frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
+					btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
+					leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
+
+					if (frictionConstraint.m_multiBodyA)
+						frictionConstraint.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraint.m_multiBodyB)
+						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				}
 			}
 		}
 	}
-	return val;
+	return leastSquaredResidual;
 }
 
-btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	m_multiBodyNonContactConstraints.resize(0);
 	m_multiBodyNormalContactConstraints.resize(0);
 	m_multiBodyFrictionContactConstraints.resize(0);
+	m_multiBodyTorsionalFrictionContactConstraints.resize(0);
+	m_multiBodySpinningFrictionContactConstraints.resize(0);
+
 	m_data.m_jacobians.resize(0);
 	m_data.m_deltaVelocitiesUnitImpulse.resize(0);
 	m_data.m_deltaVelocities.resize(0);
 
-	for (int i=0;i<numBodies;i++)
+	for (int i = 0; i < numBodies; i++)
 	{
 		const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]);
 		if (fcA)
@@ -98,63 +221,63 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb
 		}
 	}
 
-	btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
+	btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
 	return val;
 }
 
-void	btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
+void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
 {
-    for (int i = 0; i < ndof; ++i) 
-		m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
+	for (int i = 0; i < ndof; ++i)
+		m_data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse;
 }
 
-void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
+btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
 {
-
-	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
-	btScalar deltaVelADotn=0;
-	btScalar deltaVelBDotn=0;
+	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
+	btScalar deltaVelADotn = 0;
+	btScalar deltaVelBDotn = 0;
 	btSolverBody* bodyA = 0;
 	btSolverBody* bodyB = 0;
-	int ndofA=0;
-	int ndofB=0;
+	int ndofA = 0;
+	int ndofB = 0;
 
 	if (c.m_multiBodyA)
 	{
-		ndofA  = c.m_multiBodyA->getNumDofs() + 6;
-		for (int i = 0; i < ndofA; ++i) 
-			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
-	} else if(c.m_solverBodyIdA >= 0)
+		ndofA = c.m_multiBodyA->getNumDofs() + 6;
+		for (int i = 0; i < ndofA; ++i)
+			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex + i] * m_data.m_deltaVelocities[c.m_deltaVelAindex + i];
+	}
+	else if (c.m_solverBodyIdA >= 0)
 	{
 		bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
-		deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
+		deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
 	}
 
 	if (c.m_multiBodyB)
 	{
-		ndofB  = c.m_multiBodyB->getNumDofs() + 6;
-		for (int i = 0; i < ndofB; ++i) 
-			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
-	} else if(c.m_solverBodyIdB >= 0)
+		ndofB = c.m_multiBodyB->getNumDofs() + 6;
+		for (int i = 0; i < ndofB; ++i)
+			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex + i] * m_data.m_deltaVelocities[c.m_deltaVelBindex + i];
+	}
+	else if (c.m_solverBodyIdB >= 0)
 	{
 		bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
-		deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity())  + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
+		deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
 	}
 
-	
-	deltaImpulse	-=	deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
-	deltaImpulse	-=	deltaVelBDotn*c.m_jacDiagABInv;
+	deltaImpulse -= deltaVelADotn * c.m_jacDiagABInv;  //m_jacDiagABInv = 1./denom
+	deltaImpulse -= deltaVelBDotn * c.m_jacDiagABInv;
 	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
-	
+
 	if (sum < c.m_lowerLimit)
 	{
-		deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+		deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
 		c.m_appliedImpulse = c.m_lowerLimit;
 	}
-	else if (sum > c.m_upperLimit) 
+	else if (sum > c.m_upperLimit)
 	{
-		deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
+		deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
 		c.m_appliedImpulse = c.m_upperLimit;
 	}
 	else
@@ -164,42 +287,219 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 
 	if (c.m_multiBodyA)
 	{
-		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
-		c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
-#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
-	} else if(c.m_solverBodyIdA >= 0)
+		c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
+#endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	}
+	else if (c.m_solverBodyIdA >= 0)
 	{
-		bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
-
+		bodyA->internalApplyImpulse(c.m_contactNormal1 * bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
 	}
 	if (c.m_multiBodyB)
 	{
-		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
-		c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
-#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
-	} else if(c.m_solverBodyIdB >= 0)
+		c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
+#endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	}
+	else if (c.m_solverBodyIdB >= 0)
 	{
-		bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+		bodyB->internalApplyImpulse(c.m_contactNormal2 * bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 	}
-
+	btScalar deltaVel = deltaImpulse / c.m_jacDiagABInv;
+	return deltaVel;
 }
 
+btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB)
+{
+	int ndofA = 0;
+	int ndofB = 0;
+	btSolverBody* bodyA = 0;
+	btSolverBody* bodyB = 0;
+	btScalar deltaImpulseB = 0.f;
+	btScalar sumB = 0.f;
+	{
+		deltaImpulseB = cB.m_rhs - btScalar(cB.m_appliedImpulse) * cB.m_cfm;
+		btScalar deltaVelADotn = 0;
+		btScalar deltaVelBDotn = 0;
+		if (cB.m_multiBodyA)
+		{
+			ndofA = cB.m_multiBodyA->getNumDofs() + 6;
+			for (int i = 0; i < ndofA; ++i)
+				deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex + i];
+		}
+		else if (cB.m_solverBodyIdA >= 0)
+		{
+			bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA];
+			deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
+		}
 
+		if (cB.m_multiBodyB)
+		{
+			ndofB = cB.m_multiBodyB->getNumDofs() + 6;
+			for (int i = 0; i < ndofB; ++i)
+				deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex + i];
+		}
+		else if (cB.m_solverBodyIdB >= 0)
+		{
+			bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB];
+			deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
+		}
+
+		deltaImpulseB -= deltaVelADotn * cB.m_jacDiagABInv;  //m_jacDiagABInv = 1./denom
+		deltaImpulseB -= deltaVelBDotn * cB.m_jacDiagABInv;
+		sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB;
+	}
+
+	btScalar deltaImpulseA = 0.f;
+	btScalar sumA = 0.f;
+	const btMultiBodySolverConstraint& cA = cA1;
+	{
+		{
+			deltaImpulseA = cA.m_rhs - btScalar(cA.m_appliedImpulse) * cA.m_cfm;
+			btScalar deltaVelADotn = 0;
+			btScalar deltaVelBDotn = 0;
+			if (cA.m_multiBodyA)
+			{
+				ndofA = cA.m_multiBodyA->getNumDofs() + 6;
+				for (int i = 0; i < ndofA; ++i)
+					deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex + i];
+			}
+			else if (cA.m_solverBodyIdA >= 0)
+			{
+				bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA];
+				deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
+			}
+
+			if (cA.m_multiBodyB)
+			{
+				ndofB = cA.m_multiBodyB->getNumDofs() + 6;
+				for (int i = 0; i < ndofB; ++i)
+					deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex + i];
+			}
+			else if (cA.m_solverBodyIdB >= 0)
+			{
+				bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB];
+				deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
+			}
+
+			deltaImpulseA -= deltaVelADotn * cA.m_jacDiagABInv;  //m_jacDiagABInv = 1./denom
+			deltaImpulseA -= deltaVelBDotn * cA.m_jacDiagABInv;
+			sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA;
+		}
+	}
+
+	if (sumA * sumA + sumB * sumB >= cA.m_lowerLimit * cB.m_lowerLimit)
+	{
+		btScalar angle = btAtan2(sumA, sumB);
+		btScalar sumAclipped = btFabs(cA.m_lowerLimit * btSin(angle));
+		btScalar sumBclipped = btFabs(cB.m_lowerLimit * btCos(angle));
+
+		if (sumA < -sumAclipped)
+		{
+			deltaImpulseA = -sumAclipped - cA.m_appliedImpulse;
+			cA.m_appliedImpulse = -sumAclipped;
+		}
+		else if (sumA > sumAclipped)
+		{
+			deltaImpulseA = sumAclipped - cA.m_appliedImpulse;
+			cA.m_appliedImpulse = sumAclipped;
+		}
+		else
+		{
+			cA.m_appliedImpulse = sumA;
+		}
+
+		if (sumB < -sumBclipped)
+		{
+			deltaImpulseB = -sumBclipped - cB.m_appliedImpulse;
+			cB.m_appliedImpulse = -sumBclipped;
+		}
+		else if (sumB > sumBclipped)
+		{
+			deltaImpulseB = sumBclipped - cB.m_appliedImpulse;
+			cB.m_appliedImpulse = sumBclipped;
+		}
+		else
+		{
+			cB.m_appliedImpulse = sumB;
+		}
+		//deltaImpulseA = sumAclipped-cA.m_appliedImpulse;
+		//cA.m_appliedImpulse = sumAclipped;
+		//deltaImpulseB = sumBclipped-cB.m_appliedImpulse;
+		//cB.m_appliedImpulse = sumBclipped;
+	}
+	else
+	{
+		cA.m_appliedImpulse = sumA;
+		cB.m_appliedImpulse = sumB;
+	}
+
+	if (cA.m_multiBodyA)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA, cA.m_deltaVelAindex, ndofA);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA);
+#endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	}
+	else if (cA.m_solverBodyIdA >= 0)
+	{
+		bodyA->internalApplyImpulse(cA.m_contactNormal1 * bodyA->internalGetInvMass(), cA.m_angularComponentA, deltaImpulseA);
+	}
+	if (cA.m_multiBodyB)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA, cA.m_deltaVelBindex, ndofB);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA);
+#endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	}
+	else if (cA.m_solverBodyIdB >= 0)
+	{
+		bodyB->internalApplyImpulse(cA.m_contactNormal2 * bodyB->internalGetInvMass(), cA.m_angularComponentB, deltaImpulseA);
+	}
+
+	if (cB.m_multiBodyA)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB, cB.m_deltaVelAindex, ndofA);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB);
+#endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	}
+	else if (cB.m_solverBodyIdA >= 0)
+	{
+		bodyA->internalApplyImpulse(cB.m_contactNormal1 * bodyA->internalGetInvMass(), cB.m_angularComponentA, deltaImpulseB);
+	}
+	if (cB.m_multiBodyB)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB, cB.m_deltaVelBindex, ndofB);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB);
+#endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	}
+	else if (cB.m_solverBodyIdB >= 0)
+	{
+		bodyB->internalApplyImpulse(cB.m_contactNormal2 * bodyB->internalGetInvMass(), cB.m_angularComponentB, deltaImpulseB);
+	}
 
+	btScalar deltaVel = deltaImpulseA / cA.m_jacDiagABInv + deltaImpulseB / cB.m_jacDiagABInv;
+	return deltaVel;
+}
 
-void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, 
-																 const btVector3& contactNormal,
-																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
-																 btScalar& relaxation,
-																 bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, const btScalar& appliedImpulse, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 {
-			
 	BT_PROFILE("setupMultiBodyContactConstraint");
 	btVector3 rel_pos1;
 	btVector3 rel_pos2;
@@ -217,214 +517,266 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
 
 	if (bodyA)
-		rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
+		rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
 	if (bodyB)
 		rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
 
-	relaxation = 1.f;
+	relaxation = infoGlobal.m_sor;
 
-	
+	btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep;
+
+	//cfm = 1 /       ( dt * kp + kd )
+	//erp = dt * kp / ( dt * kp + kd )
 
+	btScalar cfm;
+	btScalar erp;
+	if (isFriction)
+	{
+		cfm = infoGlobal.m_frictionCFM;
+		erp = infoGlobal.m_frictionERP;
+	}
+	else
+	{
+		cfm = infoGlobal.m_globalCfm;
+		erp = infoGlobal.m_erp2;
+
+		if ((cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP))
+		{
+			if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM)
+				cfm = cp.m_contactCFM;
+			if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP)
+				erp = cp.m_contactERP;
+		}
+		else
+		{
+			if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
+			{
+				btScalar denom = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1);
+				if (denom < SIMD_EPSILON)
+				{
+					denom = SIMD_EPSILON;
+				}
+				cfm = btScalar(1) / denom;
+				erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
+			}
+		}
+	}
+
+	cfm *= invTimeStep;
 
 	if (multiBodyA)
 	{
-		if (solverConstraint.m_linkA<0)
+		if (solverConstraint.m_linkA < 0)
 		{
 			rel_pos1 = pos1 - multiBodyA->getBasePos();
-		} else
+		}
+		else
 		{
 			rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
 		}
-		const int ndofA  = multiBodyA->getNumDofs() + 6;
+		const int ndofA = multiBodyA->getNumDofs() + 6;
 
 		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
 
-		if (solverConstraint.m_deltaVelAindex <0)
+		if (solverConstraint.m_deltaVelAindex < 0)
 		{
 			solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
 			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
-			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
-		} else
+			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA);
+		}
+		else
 		{
-			btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
+			btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
 		}
 
 		solverConstraint.m_jacAindex = m_data.m_jacobians.size();
-		m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA);
-		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
+		m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA);
+		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA);
 		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 
-		btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
+		btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 		multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
 		btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-		multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
+		multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v);
 
 		btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
 		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
 		solverConstraint.m_contactNormal1 = contactNormal;
-	} else
+	}
+	else
 	{
 		btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
 		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
 		solverConstraint.m_contactNormal1 = contactNormal;
-		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
 	}
 
-	
-
 	if (multiBodyB)
 	{
-		if (solverConstraint.m_linkB<0)
+		if (solverConstraint.m_linkB < 0)
 		{
 			rel_pos2 = pos2 - multiBodyB->getBasePos();
-		} else
+		}
+		else
 		{
 			rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
 		}
 
-		const int ndofB  = multiBodyB->getNumDofs() + 6;
+		const int ndofB = multiBodyB->getNumDofs() + 6;
 
 		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
-		if (solverConstraint.m_deltaVelBindex <0)
+		if (solverConstraint.m_deltaVelBindex < 0)
 		{
 			solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
 			multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
-			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB);
+			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB);
 		}
 
 		solverConstraint.m_jacBindex = m_data.m_jacobians.size();
 
-		m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB);
-		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
+		m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB);
+		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB);
 		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 
 		multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
-		multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
-		
-		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);		
+		multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v);
+
+		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
 		solverConstraint.m_contactNormal2 = -contactNormal;
-	
-	} else
+	}
+	else
 	{
-		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);		
+		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
 		solverConstraint.m_contactNormal2 = -contactNormal;
-	
-		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+
+		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
 	}
 
 	{
-						
 		btVector3 vec;
 		btScalar denom0 = 0.f;
 		btScalar denom1 = 0.f;
 		btScalar* jacB = 0;
 		btScalar* jacA = 0;
-		btScalar* lambdaA =0;
-		btScalar* lambdaB =0;
-		int ndofA  = 0;
+		btScalar* lambdaA = 0;
+		btScalar* lambdaB = 0;
+		int ndofA = 0;
 		if (multiBodyA)
 		{
-			ndofA  = multiBodyA->getNumDofs() + 6;
+			ndofA = multiBodyA->getNumDofs() + 6;
 			jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 			lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA; ++i)
 			{
-				btScalar j = jacA[i] ;
-				btScalar l =lambdaA[i];
-				denom0 += j*l;
+				btScalar j = jacA[i];
+				btScalar l = lambdaA[i];
+				denom0 += j * l;
 			}
-		} else
+		}
+		else
 		{
 			if (rb0)
 			{
-				vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+				vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
 				denom0 = rb0->getInvMass() + contactNormal.dot(vec);
 			}
 		}
 		if (multiBodyB)
 		{
-			const int ndofB  = multiBodyB->getNumDofs() + 6;
+			const int ndofB = multiBodyB->getNumDofs() + 6;
 			jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
 			lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB; ++i)
 			{
-				btScalar j = jacB[i] ;
-				btScalar l =lambdaB[i];
-				denom1 += j*l;
+				btScalar j = jacB[i];
+				btScalar l = lambdaB[i];
+				denom1 += j * l;
 			}
-
-		} else
+		}
+		else
 		{
 			if (rb1)
 			{
-				vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+				vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
 				denom1 = rb1->getInvMass() + contactNormal.dot(vec);
 			}
 		}
 
-		 
-
-		 btScalar d = denom0+denom1;
-		 if (d>SIMD_EPSILON)
-		 {
-			solverConstraint.m_jacDiagABInv = relaxation/(d);
-		 } else
-		 {
+		btScalar d = denom0 + denom1 + cfm;
+		if (d > SIMD_EPSILON)
+		{
+			solverConstraint.m_jacDiagABInv = relaxation / (d);
+		}
+		else
+		{
 			//disable the constraint row to handle singularity/redundant constraint
-			solverConstraint.m_jacDiagABInv  = 0.f;
-		 }
-		
+			solverConstraint.m_jacDiagABInv = 0.f;
+		}
 	}
 
-	
 	//compute rhs and remaining solverConstraint fields
 
-	
-
 	btScalar restitution = 0.f;
-	btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
+	btScalar distance = 0;
+	if (!isFriction)
+	{
+		distance = cp.getDistance() + infoGlobal.m_linearSlop;
+	}
+	else
+	{
+		if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
+		{
+			distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal);
+		}
+	}
 
 	btScalar rel_vel = 0.f;
-	int ndofA  = 0;
-	int ndofB  = 0;
+	int ndofA = 0;
+	int ndofB = 0;
 	{
-
-		btVector3 vel1,vel2;
+		btVector3 vel1, vel2;
 		if (multiBodyA)
 		{
-			ndofA  = multiBodyA->getNumDofs() + 6;
+			ndofA = multiBodyA->getNumDofs() + 6;
 			btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
-			for (int i = 0; i < ndofA ; ++i) 
+			for (int i = 0; i < ndofA; ++i)
 				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
-		} else
+		}
+		else
 		{
 			if (rb0)
 			{
-				rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+				rel_vel += (rb0->getVelocityInLocalPoint(rel_pos1) +
+							(rb0->getTotalTorque() * rb0->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos1) +
+							rb0->getTotalForce() * rb0->getInvMass() * infoGlobal.m_timeStep)
+							   .dot(solverConstraint.m_contactNormal1);
 			}
 		}
 		if (multiBodyB)
 		{
-			ndofB  = multiBodyB->getNumDofs() + 6;
+			ndofB = multiBodyB->getNumDofs() + 6;
 			btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
-			for (int i = 0; i < ndofB ; ++i) 
+			for (int i = 0; i < ndofB; ++i)
 				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-
-		} else
+		}
+		else
 		{
 			if (rb1)
 			{
-				rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+				rel_vel += (rb1->getVelocityInLocalPoint(rel_pos2) +
+							(rb1->getTotalTorque() * rb1->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos2) +
+							rb1->getTotalForce() * rb1->getInvMass() * infoGlobal.m_timeStep)
+							   .dot(solverConstraint.m_contactNormal2);
 			}
 		}
 
 		solverConstraint.m_friction = cp.m_combinedFriction;
 
-		if(!isFriction)
+		if (!isFriction)
 		{
-			restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);	
+			restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
 			if (restitution <= btScalar(0.))
 			{
 				restitution = 0.f;
@@ -432,12 +784,70 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 	}
 
+	{
+		btScalar positionalError = 0.f;
+		btScalar velocityError = restitution - rel_vel;  // * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
+		if (isFriction)
+		{
+			positionalError = -distance * erp / infoGlobal.m_timeStep;
+		}
+		else
+		{
+			if (distance > 0)
+			{
+				positionalError = 0;
+				velocityError -= distance / infoGlobal.m_timeStep;
+			}
+			else
+			{
+				positionalError = -distance * erp / infoGlobal.m_timeStep;
+			}
+		}
+
+		btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
+		btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
+
+		if (!isFriction)
+		{
+			//	if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+			{
+				//combine position and velocity into rhs
+				solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
+				solverConstraint.m_rhsPenetration = 0.f;
+			}
+			/*else
+			{
+				//split position and velocity into rhs and m_rhsPenetration
+				solverConstraint.m_rhs = velocityImpulse;
+				solverConstraint.m_rhsPenetration = penetrationImpulse;
+			}
+			*/
+			solverConstraint.m_lowerLimit = 0;
+			solverConstraint.m_upperLimit = 1e10f;
+		}
+		else
+		{
+			solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
+			solverConstraint.m_rhsPenetration = 0.f;
+			solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
+			solverConstraint.m_upperLimit = solverConstraint.m_friction;
+		}
 
-	///warm starting (or zero if disabled)
-	//disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
-	if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+		solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
+	}
+        
+	if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
 	{
-		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+		if (btFabs(cp.m_prevRHS) > 1e-5 && cp.m_prevRHS < 2* solverConstraint.m_rhs && solverConstraint.m_rhs < 2*cp.m_prevRHS)
+		{
+			solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_articulatedWarmstartingFactor;
+			if (solverConstraint.m_appliedImpulse < 0)
+				solverConstraint.m_appliedImpulse = 0;
+		}
+		else
+		{
+			solverConstraint.m_appliedImpulse = 0.f;
+		}
 
 		if (solverConstraint.m_appliedImpulse)
 		{
@@ -445,109 +855,348 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			{
 				btScalar impulse = solverConstraint.m_appliedImpulse;
 				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-				multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
-				
-				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
-			} else
+				multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
+
+				applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
+			}
+			else
 			{
 				if (rb0)
-					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
 			}
 			if (multiBodyB)
 			{
 				btScalar impulse = solverConstraint.m_appliedImpulse;
 				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-				multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
-				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
-			} else
+				multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
+				applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
+			}
+			else
 			{
 				if (rb1)
-					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
+					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
 			}
 		}
-	} else
+	}
+	else
 	{
 		solverConstraint.m_appliedImpulse = 0.f;
+   	solverConstraint.m_appliedPushImpulse = 0.f;
 	}
+}
 
-	solverConstraint.m_appliedPushImpulse = 0.f;
+void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
+																			const btVector3& constraintNormal,
+																			btManifoldPoint& cp,
+																			btScalar combinedTorsionalFriction,
+																			const btContactSolverInfo& infoGlobal,
+																			btScalar& relaxation,
+																			bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+{
+	BT_PROFILE("setupMultiBodyRollingFrictionConstraint");
+	btVector3 rel_pos1;
+	btVector3 rel_pos2;
+
+	btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
+	btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
+
+	const btVector3& pos1 = cp.getPositionWorldOnA();
+	const btVector3& pos2 = cp.getPositionWorldOnB();
+
+	btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
+	btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
+
+	btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
+	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
 
+	if (bodyA)
+		rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
+	if (bodyB)
+		rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
+
+	relaxation = infoGlobal.m_sor;
+
+	// btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
+
+	if (multiBodyA)
 	{
+		if (solverConstraint.m_linkA < 0)
+		{
+			rel_pos1 = pos1 - multiBodyA->getBasePos();
+		}
+		else
+		{
+			rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
+		}
+		const int ndofA = multiBodyA->getNumDofs() + 6;
 
-		btScalar positionalError = 0.f;
-		btScalar velocityError = restitution - rel_vel;// * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
+		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
 
-		btScalar erp = infoGlobal.m_erp2;
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		if (solverConstraint.m_deltaVelAindex < 0)
+		{
+			solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
+			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
+			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA);
+		}
+		else
 		{
-			erp = infoGlobal.m_erp;
+			btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
 		}
 
-		if (penetration>0)
+		solverConstraint.m_jacAindex = m_data.m_jacobians.size();
+		m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA);
+		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA);
+		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
+
+		btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex];
+		multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0, 0, 0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+		btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+		multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v);
+
+		btVector3 torqueAxis0 = constraintNormal;
+		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+		solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
+	}
+	else
+	{
+		btVector3 torqueAxis0 = constraintNormal;
+		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+		solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
+		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
+	}
+
+	if (multiBodyB)
+	{
+		if (solverConstraint.m_linkB < 0)
 		{
-			positionalError = 0;
-			velocityError -= penetration / infoGlobal.m_timeStep;
+			rel_pos2 = pos2 - multiBodyB->getBasePos();
+		}
+		else
+		{
+			rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
+		}
+
+		const int ndofB = multiBodyB->getNumDofs() + 6;
 
-		} else
+		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
+		if (solverConstraint.m_deltaVelBindex < 0)
 		{
-			positionalError = -penetration * erp/infoGlobal.m_timeStep;
+			solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
+			multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
+			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB);
 		}
 
-		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
-		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+		solverConstraint.m_jacBindex = m_data.m_jacobians.size();
+
+		m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB);
+		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB);
+		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
+
+		multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0, 0, 0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+		multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v);
+
+		btVector3 torqueAxis1 = -constraintNormal;
+		solverConstraint.m_relpos2CrossNormal = torqueAxis1;
+		solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
+	}
+	else
+	{
+		btVector3 torqueAxis1 = -constraintNormal;
+		solverConstraint.m_relpos2CrossNormal = torqueAxis1;
+		solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
+
+		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
+	}
 
-		if(!isFriction)
+	{
+		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)
 		{
-			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+			ndofA = multiBodyA->getNumDofs() + 6;
+			jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
+			lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+			for (int i = 0; i < ndofA; ++i)
 			{
-				//combine position and velocity into rhs
-				solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
-				solverConstraint.m_rhsPenetration = 0.f;
-
-			} else
+				btScalar j = jacA[i];
+				btScalar l = lambdaA[i];
+				denom0 += j * l;
+			}
+		}
+		else
+		{
+			if (rb0)
 			{
-				//split position and velocity into rhs and m_rhsPenetration
-				solverConstraint.m_rhs = velocityImpulse;
-				solverConstraint.m_rhsPenetration = penetrationImpulse;
+				btVector3 iMJaA = rb0 ? rb0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0);
+				denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
+			}
+		}
+		if (multiBodyB)
+		{
+			const int ndofB = multiBodyB->getNumDofs() + 6;
+			jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
+			lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+			for (int i = 0; i < ndofB; ++i)
+			{
+				btScalar j = jacB[i];
+				btScalar l = lambdaB[i];
+				denom1 += j * l;
 			}
+		}
+		else
+		{
+			if (rb1)
+			{
+				btVector3 iMJaB = rb1 ? rb1->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0);
+				denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
+			}
+		}
 
-			solverConstraint.m_lowerLimit = 0;
-			solverConstraint.m_upperLimit = 1e10f;
+		btScalar d = denom0 + denom1 + infoGlobal.m_globalCfm;
+		if (d > SIMD_EPSILON)
+		{
+			solverConstraint.m_jacDiagABInv = relaxation / (d);
 		}
 		else
 		{
-			solverConstraint.m_rhs = velocityImpulse;
-			solverConstraint.m_rhsPenetration = 0.f;
-			solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
-			solverConstraint.m_upperLimit = solverConstraint.m_friction;
+			//disable the constraint row to handle singularity/redundant constraint
+			solverConstraint.m_jacDiagABInv = 0.f;
+		}
+	}
+
+	//compute rhs and remaining solverConstraint fields
+
+	btScalar restitution = 0.f;
+	btScalar penetration = isFriction ? 0 : cp.getDistance();
+
+	btScalar rel_vel = 0.f;
+	int ndofA = 0;
+	int ndofB = 0;
+	{
+		btVector3 vel1, vel2;
+		if (multiBodyA)
+		{
+			ndofA = multiBodyA->getNumDofs() + 6;
+			btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
+			for (int i = 0; i < ndofA; ++i)
+				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
+		}
+		else
+		{
+			if (rb0)
+			{
+				btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
+				rel_vel += solverConstraint.m_contactNormal1.dot(rb0 ? solverBodyA->m_linearVelocity + solverBodyA->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? solverBodyA->m_angularVelocity : btVector3(0, 0, 0));
+			}
+		}
+		if (multiBodyB)
+		{
+			ndofB = multiBodyB->getNumDofs() + 6;
+			btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
+			for (int i = 0; i < ndofB; ++i)
+				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
+		}
+		else
+		{
+			if (rb1)
+			{
+				btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
+				rel_vel += solverConstraint.m_contactNormal2.dot(rb1 ? solverBodyB->m_linearVelocity + solverBodyB->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? solverBodyB->m_angularVelocity : btVector3(0, 0, 0));
+			}
 		}
 
-		solverConstraint.m_cfm = 0.f;			//why not use cfmSlip?
+		solverConstraint.m_friction = combinedTorsionalFriction;
+
+		if (!isFriction)
+		{
+			restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
+			if (restitution <= btScalar(0.))
+			{
+				restitution = 0.f;
+			}
+		}
 	}
 
-}
+	solverConstraint.m_appliedImpulse = 0.f;
+	solverConstraint.m_appliedPushImpulse = 0.f;
 
+	{
+		btScalar velocityError = 0 - rel_vel;  // * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
 
+		btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
 
+		solverConstraint.m_rhs = velocityImpulse;
+		solverConstraint.m_rhsPenetration = 0.f;
+		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
+		solverConstraint.m_upperLimit = solverConstraint.m_friction;
 
-btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
+		solverConstraint.m_cfm = infoGlobal.m_globalCfm * solverConstraint.m_jacDiagABInv;
+	}
+}
+
+btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
 {
 	BT_PROFILE("addMultiBodyFrictionConstraint");
 	btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
-    solverConstraint.m_orgConstraint = 0;
-    solverConstraint.m_orgDofIndex = -1;
-    
+	solverConstraint.m_orgConstraint = 0;
+	solverConstraint.m_orgDofIndex = -1;
+
 	solverConstraint.m_frictionIndex = frictionIndex;
 	bool isFriction = true;
 
 	const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
 	const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
-	
-	btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
-	btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
 
-	int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
-	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
+	btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
+	btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
+
+	int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
+	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
+
+	solverConstraint.m_solverBodyIdA = solverBodyIdA;
+	solverConstraint.m_solverBodyIdB = solverBodyIdB;
+	solverConstraint.m_multiBodyA = mbA;
+	if (mbA)
+		solverConstraint.m_linkA = fcA->m_link;
+
+	solverConstraint.m_multiBodyB = mbB;
+	if (mbB)
+		solverConstraint.m_linkB = fcB->m_link;
+
+	solverConstraint.m_originalContactPoint = &cp;
+
+	setupMultiBodyContactConstraint(solverConstraint, normalAxis, 0, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
+	return solverConstraint;
+}
+
+btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
+																								  btScalar combinedTorsionalFriction,
+																								  btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
+{
+	BT_PROFILE("addMultiBodyRollingFrictionConstraint");
+
+	bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0));
+
+	btMultiBodySolverConstraint& solverConstraint = useTorsionalAndConeFriction ? m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing() : m_multiBodyFrictionContactConstraints.expandNonInitializing();
+	solverConstraint.m_orgConstraint = 0;
+	solverConstraint.m_orgDofIndex = -1;
+
+	solverConstraint.m_frictionIndex = frictionIndex;
+	bool isFriction = true;
+
+	const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
+	const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
+
+	btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
+	btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
+
+	int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
+	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
 
 	solverConstraint.m_solverBodyIdA = solverBodyIdA;
 	solverConstraint.m_solverBodyIdB = solverBodyIdB;
@@ -561,54 +1210,89 @@ btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyFrictionCo
 
 	solverConstraint.m_originalContactPoint = &cp;
 
-	setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
+	setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
 
-void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
+btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
+	btScalar combinedTorsionalFriction,
+	btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
+{
+	BT_PROFILE("addMultiBodyRollingFrictionConstraint");
+
+	btMultiBodySolverConstraint& solverConstraint = m_multiBodySpinningFrictionContactConstraints.expandNonInitializing();
+	solverConstraint.m_orgConstraint = 0;
+	solverConstraint.m_orgDofIndex = -1;
+
+	solverConstraint.m_frictionIndex = frictionIndex;
+	bool isFriction = true;
+
+	const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
+	const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
+
+	btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
+	btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
+
+	int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
+	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
+
+	solverConstraint.m_solverBodyIdA = solverBodyIdA;
+	solverConstraint.m_solverBodyIdB = solverBodyIdB;
+	solverConstraint.m_multiBodyA = mbA;
+	if (mbA)
+		solverConstraint.m_linkA = fcA->m_link;
+
+	solverConstraint.m_multiBodyB = mbB;
+	if (mbB)
+		solverConstraint.m_linkB = fcB->m_link;
+
+	solverConstraint.m_originalContactPoint = &cp;
+
+	setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
+	return solverConstraint;
+}
+void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal)
 {
 	const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
 	const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
 	
-	btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
-	btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
+	btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
+	btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
 
-	btCollisionObject* colObj0=0,*colObj1=0;
+	btCollisionObject *colObj0 = 0, *colObj1 = 0;
 
 	colObj0 = (btCollisionObject*)manifold->getBody0();
 	colObj1 = (btCollisionObject*)manifold->getBody1();
 
-	int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
-	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
-
-//	btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
-//	btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
+	int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
+	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
 
+	//	btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
+	//	btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
 
 	///avoid collision response between two static objects
-//	if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
+	//	if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
 	//	return;
 
+	//only a single rollingFriction per manifold
+	int rollingFriction = 4;
 
-
-	for (int j=0;j<manifold->getNumContacts();j++)
+	for (int j = 0; j < manifold->getNumContacts(); j++)
 	{
-
 		btManifoldPoint& cp = manifold->getContactPoint(j);
 
 		if (cp.getDistance() <= manifold->getContactProcessingThreshold())
 		{
-		
 			btScalar relaxation;
 
 			int frictionIndex = m_multiBodyNormalContactConstraints.size();
 
 			btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
 
-	//		btRigidBody* rb0 = btRigidBody::upcast(colObj0);
-	//		btRigidBody* rb1 = btRigidBody::upcast(colObj1);
-            solverConstraint.m_orgConstraint = 0;
-            solverConstraint.m_orgDofIndex = -1;
+			//		btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+			//		btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+			solverConstraint.m_orgConstraint = 0;
+			solverConstraint.m_orgDofIndex = -1;
 			solverConstraint.m_solverBodyIdA = solverBodyIdA;
 			solverConstraint.m_solverBodyIdB = solverBodyIdB;
 			solverConstraint.m_multiBodyA = mbA;
@@ -622,71 +1306,56 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 			solverConstraint.m_originalContactPoint = &cp;
 
 			bool isFriction = false;
-			setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction);
+			setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp.m_appliedImpulse, cp, infoGlobal, relaxation, isFriction);
 
-//			const btVector3& pos1 = cp.getPositionWorldOnA();
-//			const btVector3& pos2 = cp.getPositionWorldOnB();
+			//			const btVector3& pos1 = cp.getPositionWorldOnA();
+			//			const btVector3& pos2 = cp.getPositionWorldOnB();
 
 			/////setup the friction constraints
 #define ENABLE_FRICTION
 #ifdef ENABLE_FRICTION
-			solverConstraint.m_frictionIndex = frictionIndex;
-#if ROLLING_FRICTION
-	int rollingFriction=1;
-			btVector3 angVelA(0,0,0),angVelB(0,0,0);
-			if (rb0)
-				angVelA = rb0->getAngularVelocity();
-			if (rb1)
-				angVelB = rb1->getAngularVelocity();
-			btVector3 relAngVel = angVelB-angVelA;
-
-			if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
-			{
-				//only a single rollingFriction per manifold
-				rollingFriction--;
-				if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
-				{
-					relAngVel.normalize();
-					applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					if (relAngVel.length()>0.001)
-						addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
-				} else
-				{
-					addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-					btVector3 axis0,axis1;
-					btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
-					applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					if (axis0.length()>0.001)
-						addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-					if (axis1.length()>0.001)
-						addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-		
-				}
-			}
-#endif //ROLLING_FRICTION
+			solverConstraint.m_frictionIndex = m_multiBodyFrictionContactConstraints.size();
 
 			///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 every frame
 			///based on the relative linear velocity.
-			///If the relative velocity it zero, it will automatically compute a friction direction.
-			
+			///If the relative velocity is zero, it will automatically compute a friction direction.
+
 			///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
 			///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
 			///
 			///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
 			///
-			///The user can manually override the friction directions for certain contacts using a contact callback, 
+			///The user can manually override the friction directions for certain contacts using a contact callback,
 			///and set the cp.m_lateralFrictionInitialized to true
 			///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
 			///this will give a conveyor belt effect
 			///
-			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
-			{/*
+
+			btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
+			cp.m_lateralFrictionDir1.normalize();
+			cp.m_lateralFrictionDir2.normalize();
+
+			if (rollingFriction > 0)
+			{
+				if (cp.m_combinedSpinningFriction > 0)
+				{
+					addMultiBodySpinningFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal);
+				}
+				if (cp.m_combinedRollingFriction > 0)
+				{
+					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+
+					addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
+					addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
+				}
+				rollingFriction--;
+			}
+			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
+			{ /*
 				cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
 				btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
 				if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
@@ -709,83 +1378,123 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 				} else
 				*/
 				{
-					btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
-
-					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,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, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
-						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
+						applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+						applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, 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;
+						cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
 					}
 				}
-
-			} else
+			}
+			else
 			{
-				addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
+				addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
 
 				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2);
-
-				//setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
-				//todo:
+					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
 				solverConstraint.m_appliedImpulse = 0.f;
 				solverConstraint.m_appliedPushImpulse = 0.f;
-			}
-		
-
-#endif //ENABLE_FRICTION
+      }
 
+#endif  //ENABLE_FRICTION
+		}
+		else
+		{
+			// Reset quantities related to warmstart as 0.
+			cp.m_appliedImpulse = 0;
+			cp.m_prevRHS = 0;
 		}
 	}
 }
 
-void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
+void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
 {
-	//btPersistentManifold* manifold = 0;
-
-	for (int i=0;i<numManifolds;i++)
+	for (int i = 0; i < numManifolds; i++)
 	{
-		btPersistentManifold* manifold= manifoldPtr[i];
+		btPersistentManifold* manifold = manifoldPtr[i];
 		const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
 		const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
 		if (!fcA && !fcB)
 		{
 			//the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
-			convertContact(manifold,infoGlobal);
-		} else
+			convertContact(manifold, infoGlobal);
+		}
+		else
 		{
-			convertMultiBodyContact(manifold,infoGlobal);
+			convertMultiBodyContact(manifold, infoGlobal);
 		}
 	}
 
 	//also convert the multibody constraints, if any
 
-	
-	for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
+	for (int i = 0; i < m_tmpNumMultiBodyConstraints; i++)
 	{
 		btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
 		m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
 		m_data.m_fixedBodyId = m_fixedBodyId;
-		
-		c->createConstraintRows(m_multiBodyNonContactConstraints,m_data,	infoGlobal);
+
+		c->createConstraintRows(m_multiBodyNonContactConstraints, m_data, infoGlobal);
 	}
 
+	// Warmstart for noncontact constraints
+	if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
+	{
+		for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
+		{
+			btMultiBodySolverConstraint& solverConstraint =
+				m_multiBodyNonContactConstraints[i];
+			solverConstraint.m_appliedImpulse =
+				solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) *
+				infoGlobal.m_articulatedWarmstartingFactor;
+
+			btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
+			btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
+			if (solverConstraint.m_appliedImpulse)
+			{
+				if (multiBodyA)
+				{
+					int ndofA = multiBodyA->getNumDofs() + 6;
+					btScalar* deltaV =
+						&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+					btScalar impulse = solverConstraint.m_appliedImpulse;
+					multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
+					applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
+				}
+				if (multiBodyB)
+				{
+					int ndofB = multiBodyB->getNumDofs() + 6;
+					btScalar* deltaV =
+						&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+					btScalar impulse = solverConstraint.m_appliedImpulse;
+					multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
+					applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
+				}
+			}
+		}
+	}
+	else
+	{
+		for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
+		{
+			btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
+			solverConstraint.m_appliedImpulse = 0;
+		}
+	}
 }
 
-
-
-btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
+btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
 {
-	return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
+	//printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints);
+	return btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
 }
 
 #if 0
@@ -811,53 +1520,52 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS
 
 void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
 {
-#if 1 
-	
+#if 1
+
 	//bod->addBaseForce(m_gravity * bod->getBaseMass());
 	//bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
 
 	if (c.m_orgConstraint)
 	{
-		c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse);
+		c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex, c.m_appliedImpulse);
 	}
-	
 
 	if (c.m_multiBodyA)
 	{
-		
 		c.m_multiBodyA->setCompanionId(-1);
-		btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
-		btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
-		if (c.m_linkA<0)
+		btVector3 force = c.m_contactNormal1 * (c.m_appliedImpulse / deltaTime);
+		btVector3 torque = c.m_relpos1CrossNormal * (c.m_appliedImpulse / deltaTime);
+		if (c.m_linkA < 0)
 		{
 			c.m_multiBodyA->addBaseConstraintForce(force);
 			c.m_multiBodyA->addBaseConstraintTorque(torque);
-		} else
+		}
+		else
 		{
-			c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force);
-				//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
-			c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque);
+			c.m_multiBodyA->addLinkConstraintForce(c.m_linkA, force);
+			//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
+			c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA, torque);
 		}
 	}
-	
+
 	if (c.m_multiBodyB)
 	{
 		{
 			c.m_multiBodyB->setCompanionId(-1);
-			btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
-			btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
-			if (c.m_linkB<0)
+			btVector3 force = c.m_contactNormal2 * (c.m_appliedImpulse / deltaTime);
+			btVector3 torque = c.m_relpos2CrossNormal * (c.m_appliedImpulse / deltaTime);
+			if (c.m_linkB < 0)
 			{
 				c.m_multiBodyB->addBaseConstraintForce(force);
 				c.m_multiBodyB->addBaseConstraintTorque(torque);
-			} else
+			}
+			else
 			{
 				{
-					c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force);
+					c.m_multiBodyB->addLinkConstraintForce(c.m_linkB, force);
 					//b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
-					c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque);
+					c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB, torque);
 				}
-					
 			}
 		}
 	}
@@ -867,82 +1575,65 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
 
 	if (c.m_multiBodyA)
 	{
-		
-		if(c.m_multiBodyA->isMultiDof())
-		{
-			c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
-		}
-		else
-		{
-			c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
-		}
+		c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], c.m_appliedImpulse);
 	}
-	
+
 	if (c.m_multiBodyB)
 	{
-		if(c.m_multiBodyB->isMultiDof())
-		{
-			c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
-		}
-		else
-		{
-			c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
-		}
+		c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], c.m_appliedImpulse);
 	}
 #endif
-
-
-
 }
 
-btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
+btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
 {
 	BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
 	int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
-	
 
-	//write back the delta v to the multi bodies, either as applied impulse (direct velocity change) 
+	//write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
 	//or as applied force, so we can measure the joint reaction forces easier
-	for (int i=0;i<numPoolConstraints;i++)
+	for (int i = 0; i < numPoolConstraints; i++)
 	{
 		btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
-		writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
+		writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
 
-		writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep);
+		writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex], infoGlobal.m_timeStep);
 
 		if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 		{
-			writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep);
+			writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1], infoGlobal.m_timeStep);
 		}
 	}
 
-
-	for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
+	for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
 	{
 		btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
-		writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
+		writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
 	}
 
-	
-	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+
 	{
 		BT_PROFILE("warm starting write back");
-		for (int j=0;j<numPoolConstraints;j++)
+		for (int j = 0; j < numPoolConstraints; j++)
 		{
 			const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
-			btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
+			btManifoldPoint* pt = (btManifoldPoint*)solverConstraint.m_originalContactPoint;
 			btAssert(pt);
 			pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
+ 		  pt->m_prevRHS = solverConstraint.m_rhs;
 			pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
-			
+
 			//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
 			if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 			{
-				pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
+				pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1].m_appliedImpulse;
+			} else
+			{
+				pt->m_appliedImpulseLateral2 = 0;
 			}
-			//do a callback here?
 		}
 	}
+
 #if 0
 	//multibody joint feedback
 	{
@@ -1040,23 +1731,22 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 		}
 
 	}
-#endif 
+#endif
 #endif
 
-	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
+	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)
+void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
 {
+	//printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints);
+
 	//printf("solveMultiBodyGroup start\n");
 	m_tmpMultiBodyConstraints = multiBodyConstraints;
 	m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
-	
-	btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
+
+	btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
 
 	m_tmpMultiBodyConstraints = 0;
 	m_tmpNumMultiBodyConstraints = 0;
-	
-
 }
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
index 321ee4231a3..f584360e2b6 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
@@ -25,63 +25,77 @@ class btMultiBody;
 
 #include "btMultiBodyConstraint.h"
 
-
-
-ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
+ATTRIBUTE_ALIGNED16(class)
+btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
 {
-
 protected:
+	btMultiBodyConstraintArray m_multiBodyNonContactConstraints;
 
-	btMultiBodyConstraintArray			m_multiBodyNonContactConstraints;
+	btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
+	btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
+	btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints;
+	btMultiBodyConstraintArray m_multiBodySpinningFrictionContactConstraints;
 
-	btMultiBodyConstraintArray			m_multiBodyNormalContactConstraints;
-	btMultiBodyConstraintArray			m_multiBodyFrictionContactConstraints;
+	btMultiBodyJacobianData m_data;
 
-	btMultiBodyJacobianData				m_data;
-	
 	//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
-	btMultiBodyConstraint**					m_tmpMultiBodyConstraints;
-	int										m_tmpNumMultiBodyConstraints;
-
-	void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
-	
-
-	void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
-	btMultiBodySolverConstraint&	addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
-
-
-	void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, 
-																 btScalar* jacA,btScalar* jacB,
-																 btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
-																 const btContactSolverInfo& infoGlobal);
+	btMultiBodyConstraint** m_tmpMultiBodyConstraints;
+	int m_tmpNumMultiBodyConstraints;
+
+	btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
+
+	//solve 2 friction directions and clamp against the implicit friction cone
+	btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB);
+
+	void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
+
+	btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
+	btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
+																		 btScalar combinedTorsionalFriction,
+																		 btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
+	btMultiBodySolverConstraint& addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
+		btScalar combinedTorsionalFriction,
+		btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
+	void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint & constraintRow,
+											btScalar * jacA, btScalar * jacB,
+											btScalar penetration, btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
+											const btContactSolverInfo& infoGlobal);
+
+	void setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint,
+										 const btVector3& contactNormal,
+                     const btScalar& appliedImpulse,
+										 btManifoldPoint& cp,
+                     const btContactSolverInfo& infoGlobal,
+										 btScalar& relaxation,
+										 bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
+	//either rolling or spinning friction
+	void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint & solverConstraint,
+												   const btVector3& contactNormal,
+												   btManifoldPoint& cp,
+												   btScalar combinedTorsionalFriction,
+												   const btContactSolverInfo& infoGlobal,
+												   btScalar& relaxation,
+												   bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
+	void convertMultiBodyContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+	//	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+	void applyDeltaVee(btScalar * deltaV, btScalar impulse, int velocityIndex, int ndof);
+	void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & constraint, btScalar deltaTime);
 
-	void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, 
-																 const btVector3& contactNormal,
-																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
-																 btScalar& relaxation,
-																 bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
-
-	void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
-	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-//	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-
-	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-	void	applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
-	void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& constraint, btScalar deltaTime);
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints)
-	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
-	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
-	
-	virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
-};
+	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);
 
-	
-	
-
-
-#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H
+	virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
+};
 
+#endif  //BT_MULTIBODY_CONSTRAINT_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index a9c0b33b3a3..f599c9ccb6f 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -23,45 +23,49 @@ subject to the following restrictions:
 #include "LinearMath/btIDebugDraw.h"
 #include "LinearMath/btSerializer.h"
 
-
-void	btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
+void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, int group, int mask)
 {
 	m_multiBodies.push_back(body);
-
 }
 
-void	btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
+void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
 {
 	m_multiBodies.remove(body);
 }
 
-void	btMultiBodyDynamicsWorld::calculateSimulationIslands()
+void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+    btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
+    predictMultiBodyTransforms(timeStep);
+    
+}
+void btMultiBodyDynamicsWorld::calculateSimulationIslands()
 {
 	BT_PROFILE("calculateSimulationIslands");
 
-	getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
+	getSimulationIslandManager()->updateActivationState(getCollisionWorld(), getCollisionWorld()->getDispatcher());
+
+	{
+		//merge islands based on speculative contact manifolds too
+		for (int i = 0; i < this->m_predictiveManifolds.size(); i++)
+		{
+			btPersistentManifold* manifold = m_predictiveManifolds[i];
+
+			const btCollisionObject* colObj0 = manifold->getBody0();
+			const btCollisionObject* colObj1 = manifold->getBody1();
+
+			if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
+				((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
+			{
+				getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
+			}
+		}
+	}
 
-    {
-        //merge islands based on speculative contact manifolds too
-        for (int i=0;i<this->m_predictiveManifolds.size();i++)
-        {
-            btPersistentManifold* manifold = m_predictiveManifolds[i];
-            
-            const btCollisionObject* colObj0 = manifold->getBody0();
-            const btCollisionObject* colObj1 = manifold->getBody1();
-            
-            if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
-                ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
-            {
-				getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
-            }
-        }
-    }
-    
 	{
 		int i;
 		int numConstraints = int(m_constraints.size());
-		for (i=0;i< numConstraints ; i++ )
+		for (i = 0; i < numConstraints; i++)
 		{
 			btTypedConstraint* constraint = m_constraints[i];
 			if (constraint->isEnabled())
@@ -72,23 +76,23 @@ void	btMultiBodyDynamicsWorld::calculateSimulationIslands()
 				if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
 					((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
 				{
-					getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
+					getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
 				}
 			}
 		}
 	}
 
 	//merge islands linked by Featherstone link colliders
-	for (int i=0;i<m_multiBodies.size();i++)
+	for (int i = 0; i < m_multiBodies.size(); i++)
 	{
 		btMultiBody* body = m_multiBodies[i];
 		{
 			btMultiBodyLinkCollider* prev = body->getBaseCollider();
 
-			for (int b=0;b<body->getNumLinks();b++)
+			for (int b = 0; b < body->getNumLinks(); b++)
 			{
 				btMultiBodyLinkCollider* cur = body->getLink(b).m_collider;
-				
+
 				if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
 					((prev) && (!(prev)->isStaticOrKinematicObject())))
 				{
@@ -98,36 +102,31 @@ void	btMultiBodyDynamicsWorld::calculateSimulationIslands()
 				}
 				if (cur && !cur->isStaticOrKinematicObject())
 					prev = cur;
-				
 			}
 		}
 	}
 
 	//merge islands linked by multibody constraints
 	{
-		for (int i=0;i<this->m_multiBodyConstraints.size();i++)
+		for (int i = 0; i < this->m_multiBodyConstraints.size(); i++)
 		{
 			btMultiBodyConstraint* c = m_multiBodyConstraints[i];
 			int tagA = c->getIslandIdA();
 			int tagB = c->getIslandIdB();
-			if (tagA>=0 && tagB>=0)
+			if (tagA >= 0 && tagB >= 0)
 				getSimulationIslandManager()->getUnionFind().unite(tagA, tagB);
 		}
 	}
 
 	//Store the island id in each body
 	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
-
 }
 
-
-void	btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
+void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
 {
 	BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState");
 
-	
-	
-	for ( int i=0;i<m_multiBodies.size();i++)
+	for (int i = 0; i < m_multiBodies.size(); i++)
 	{
 		btMultiBody* body = m_multiBodies[i];
 		if (body)
@@ -138,659 +137,529 @@ void	btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
 				btMultiBodyLinkCollider* col = body->getBaseCollider();
 				if (col && col->getActivationState() == ACTIVE_TAG)
 				{
-					col->setActivationState( WANTS_DEACTIVATION);
+					col->setActivationState(WANTS_DEACTIVATION);
 					col->setDeactivationTime(0.f);
 				}
-				for (int b=0;b<body->getNumLinks();b++)
+				for (int b = 0; b < body->getNumLinks(); b++)
 				{
 					btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
 					if (col && col->getActivationState() == ACTIVE_TAG)
 					{
-						col->setActivationState( WANTS_DEACTIVATION);
+						col->setActivationState(WANTS_DEACTIVATION);
 						col->setDeactivationTime(0.f);
 					}
 				}
-			} else
+			}
+			else
 			{
 				btMultiBodyLinkCollider* col = body->getBaseCollider();
 				if (col && col->getActivationState() != DISABLE_DEACTIVATION)
-					col->setActivationState( ACTIVE_TAG );
+					col->setActivationState(ACTIVE_TAG);
 
-				for (int b=0;b<body->getNumLinks();b++)
+				for (int b = 0; b < body->getNumLinks(); b++)
 				{
 					btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
 					if (col && col->getActivationState() != DISABLE_DEACTIVATION)
-						col->setActivationState( ACTIVE_TAG );
+						col->setActivationState(ACTIVE_TAG);
 				}
 			}
-
 		}
 	}
 
 	btDiscreteDynamicsWorld::updateActivationState(timeStep);
 }
 
-
-SIMD_FORCE_INLINE	int	btGetConstraintIslandId2(const btTypedConstraint* lhs)
+void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData>& islandAnalyticsData) const
 {
-	int islandId;
-	
-	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
-	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
-	islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
-	return islandId;
-
+	islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData;
 }
 
-
-class btSortConstraintOnIslandPredicate2
+btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+	: btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
+	  m_multiBodyConstraintSolver(constraintSolver)
 {
-	public:
-
-		bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const
-		{
-			int rIslandId0,lIslandId0;
-			rIslandId0 = btGetConstraintIslandId2(rhs);
-			lIslandId0 = btGetConstraintIslandId2(lhs);
-			return lIslandId0 < rIslandId0;
-		}
-};
-
-
+	//split impulse is not yet supported for Featherstone hierarchies
+	//	getSolverInfo().m_splitImpulse = false;
+	getSolverInfo().m_solverMode |= SOLVER_USE_2_FRICTION_DIRECTIONS;
+	m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
+}
 
-SIMD_FORCE_INLINE	int	btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
+btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld()
 {
-	int islandId;
-	
-	int islandTagA = lhs->getIslandIdA();
-	int islandTagB = lhs->getIslandIdB();
-	islandId= islandTagA>=0?islandTagA:islandTagB;
-	return islandId;
-
+	delete m_solverMultiBodyIslandCallback;
 }
 
-
-class btSortMultiBodyConstraintOnIslandPredicate
+void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
 {
-	public:
-
-		bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const
-		{
-			int rIslandId0,lIslandId0;
-			rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
-			lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
-			return lIslandId0 < rIslandId0;
-		}
-};
+	m_multiBodyConstraintSolver = solver;
+	m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver);
+	btDiscreteDynamicsWorld::setConstraintSolver(solver);
+}
 
-struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
+void btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 {
-	btContactSolverInfo*	m_solverInfo;
-	btMultiBodyConstraintSolver*		m_solver;
-	btMultiBodyConstraint**		m_multiBodySortedConstraints;
-	int							m_numMultiBodyConstraints;
-	
-	btTypedConstraint**		m_sortedConstraints;
-	int						m_numConstraints;
-	btIDebugDraw*			m_debugDrawer;
-	btDispatcher*			m_dispatcher;
-	
-	btAlignedObjectArray<btCollisionObject*> m_bodies;
-	btAlignedObjectArray<btPersistentManifold*> m_manifolds;
-	btAlignedObjectArray<btTypedConstraint*> m_constraints;
-	btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
-
-
-	MultiBodyInplaceSolverIslandCallback(	btMultiBodyConstraintSolver*	solver,
-									btDispatcher* dispatcher)
-		:m_solverInfo(NULL),
-		m_solver(solver),
-		m_multiBodySortedConstraints(NULL),
-		m_numConstraints(0),
-		m_debugDrawer(NULL),
-		m_dispatcher(dispatcher)
+	if (solver->getSolverType() == BT_MULTIBODY_SOLVER)
 	{
-
-	}
-
-	MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other)
-	{
-		btAssert(0);
-		(void)other;
-		return *this;
-	}
-
-	SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints,	int	numMultiBodyConstraints,	btIDebugDraw* debugDrawer)
-	{
-		btAssert(solverInfo);
-		m_solverInfo = solverInfo;
-
-		m_multiBodySortedConstraints = sortedMultiBodyConstraints;
-		m_numMultiBodyConstraints = numMultiBodyConstraints;
-		m_sortedConstraints = sortedConstraints;
-		m_numConstraints = numConstraints;
-
-		m_debugDrawer = debugDrawer;
-		m_bodies.resize (0);
-		m_manifolds.resize (0);
-		m_constraints.resize (0);
-		m_multiBodyConstraints.resize(0);
+		m_multiBodyConstraintSolver = (btMultiBodyConstraintSolver*)solver;
 	}
-
-	
-	virtual	void	processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
-	{
-		if (islandId<0)
-		{
-			///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
-			m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
-		} else
-		{
-				//also add all non-contact constraints/joints for this island
-			btTypedConstraint** startConstraint = 0;
-			btMultiBodyConstraint** startMultiBodyConstraint = 0;
-
-			int numCurConstraints = 0;
-			int numCurMultiBodyConstraints = 0;
-
-			int i;
-			
-			//find the first constraint for this island
-
-			for (i=0;i<m_numConstraints;i++)
-			{
-				if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
-				{
-					startConstraint = &m_sortedConstraints[i];
-					break;
-				}
-			}
-			//count the number of constraints in this island
-			for (;i<m_numConstraints;i++)
-			{
-				if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
-				{
-					numCurConstraints++;
-				}
-			}
-
-			for (i=0;i<m_numMultiBodyConstraints;i++)
-			{
-				if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
-				{
-					
-					startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
-					break;
-				}
-			}
-			//count the number of multi body constraints in this island
-			for (;i<m_numMultiBodyConstraints;i++)
-			{
-				if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
-				{
-					numCurMultiBodyConstraints++;
-				}
-			}
-
-			if (m_solverInfo->m_minimumSolverBatchSize<=1)
-			{
-				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
-			} else
-			{
-				
-				for (i=0;i<numBodies;i++)
-					m_bodies.push_back(bodies[i]);
-				for (i=0;i<numManifolds;i++)
-					m_manifolds.push_back(manifolds[i]);
-				for (i=0;i<numCurConstraints;i++)
-					m_constraints.push_back(startConstraint[i]);
-				
-				for (i=0;i<numCurMultiBodyConstraints;i++)
-					m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
-				
-				if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
-				{
-					processConstraints();
-				} else
-				{
-					//printf("deferred\n");
-				}
-			}
-		}
-	}
-	void	processConstraints()
-	{
-
-		btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
-		btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
-		btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
-		btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;			
-
-		//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
-	
-		m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
-		m_bodies.resize(0);
-		m_manifolds.resize(0);
-		m_constraints.resize(0);
-		m_multiBodyConstraints.resize(0);
-	}
-
-};
-
-
-
-btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
-	:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
-	m_multiBodyConstraintSolver(constraintSolver)
-{
-	//split impulse is not yet supported for Featherstone hierarchies
-	getSolverInfo().m_splitImpulse = false;
-	getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS;
-	m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher);
-}
-
-btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
-{
-	delete m_solverMultiBodyIslandCallback;
+	btDiscreteDynamicsWorld::setConstraintSolver(solver);
 }
 
-void	btMultiBodyDynamicsWorld::forwardKinematics()
+void btMultiBodyDynamicsWorld::forwardKinematics()
 {
-	btAlignedObjectArray<btQuaternion> world_to_local;
-	btAlignedObjectArray<btVector3> local_origin;
-
-	for (int b=0;b<m_multiBodies.size();b++)
+	for (int b = 0; b < m_multiBodies.size(); b++)
 	{
 		btMultiBody* bod = m_multiBodies[b];
-		bod->forwardKinematics(world_to_local,local_origin);
+		bod->forwardKinematics(m_scratch_world_to_local, m_scratch_local_origin);
 	}
 }
-void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
+void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
-	forwardKinematics();
-
-	btAlignedObjectArray<btScalar> scratch_r;
-	btAlignedObjectArray<btVector3> scratch_v;
-	btAlignedObjectArray<btMatrix3x3> scratch_m;
-
-
-	BT_PROFILE("solveConstraints");
-	
-	m_sortedConstraints.resize( m_constraints.size());
-	int i; 
-	for (i=0;i<getNumConstraints();i++)
-	{
-		m_sortedConstraints[i] = m_constraints[i];
-	}
-	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
-	btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
-
-	m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
-	for (i=0;i<m_multiBodyConstraints.size();i++)
-	{
-		m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
-	}
-	m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
+    solveExternalForces(solverInfo);
+    buildIslands();
+    solveInternalConstraints(solverInfo);
+}
 
-	btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ?  &m_sortedMultiBodyConstraints[0] : 0;
-	
+void btMultiBodyDynamicsWorld::buildIslands()
+{
+    m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
+}
 
-	m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
-	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
-	
+void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo)
+{
 	/// solve all the constraints for this island
-	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
+	m_solverMultiBodyIslandCallback->processConstraints();
+	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
+    {
+        BT_PROFILE("btMultiBody stepVelocities");
+        for (int i = 0; i < this->m_multiBodies.size(); i++)
+        {
+            btMultiBody* bod = m_multiBodies[i];
+            
+            bool isSleeping = false;
+            
+            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            {
+                isSleeping = true;
+            }
+            for (int b = 0; b < bod->getNumLinks(); b++)
+            {
+                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                    isSleeping = true;
+            }
+            
+            if (!isSleeping)
+            {
+                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+                m_scratch_v.resize(bod->getNumLinks() + 1);
+                m_scratch_m.resize(bod->getNumLinks() + 1);
+                
+                if (bod->internalNeedsJointFeedback())
+                {
+                    if (!bod->isUsingRK4Integration())
+                    {
+                        if (bod->internalNeedsJointFeedback())
+                        {
+                            bool isConstraintPass = true;
+                            bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
+                                                                                      getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                      getSolverInfo().m_jointFeedbackInJointFrame);
+                        }
+                    }
+                }
+            }
+        }
+    }
+    for (int i = 0; i < this->m_multiBodies.size(); i++)
+    {
+        btMultiBody* bod = m_multiBodies[i];
+        bod->processDeltaVeeMultiDof2();
+    }
+}
 
+void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo)
+{
+    forwardKinematics();
+    
+    BT_PROFILE("solveConstraints");
+    
+    clearMultiBodyConstraintForces();
+    
+    m_sortedConstraints.resize(m_constraints.size());
+    int i;
+    for (i = 0; i < getNumConstraints(); i++)
+    {
+        m_sortedConstraints[i] = m_constraints[i];
+    }
+    m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
+    btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
+    
+    m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
+    for (i = 0; i < m_multiBodyConstraints.size(); i++)
+    {
+        m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
+    }
+    m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
+    
+    btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
+    
+    m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
+    m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
+    
 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-	{
-		BT_PROFILE("btMultiBody addForce");
-		for (int i=0;i<this->m_multiBodies.size();i++)
-		{
-			btMultiBody* bod = m_multiBodies[i];
-
-			bool isSleeping = false;
-			
-			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
-			{
-				isSleeping = true;
-			} 
-			for (int b=0;b<bod->getNumLinks();b++)
-			{
-				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
-					isSleeping = true;
-			} 
-
-			if (!isSleeping)
-			{
-				//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
-				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->addBaseForce(m_gravity * bod->getBaseMass());
-
-				for (int j = 0; j < bod->getNumLinks(); ++j) 
-				{
-					bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
-				}
-			}//if (!isSleeping)
-		}
-	}
-#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-	
-
-	{
-		BT_PROFILE("btMultiBody stepVelocities");
-		for (int i=0;i<this->m_multiBodies.size();i++)
-		{
-			btMultiBody* bod = m_multiBodies[i];
-
-			bool isSleeping = false;
-			
-			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
-			{
-				isSleeping = true;
-			} 
-			for (int b=0;b<bod->getNumLinks();b++)
-			{
-				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
-					isSleeping = true;
-			} 
-
-			if (!isSleeping)
-			{
-				//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
-				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);
-				bool doNotUpdatePos = false;
-
-				{
-					if(!bod->isUsingRK4Integration())
-					{
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(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;
+    {
+        BT_PROFILE("btMultiBody addForce");
+        for (int i = 0; i < this->m_multiBodies.size(); i++)
+        {
+            btMultiBody* bod = m_multiBodies[i];
+            
+            bool isSleeping = false;
+            
+            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            {
+                isSleeping = true;
+            }
+            for (int b = 0; b < bod->getNumLinks(); b++)
+            {
+                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                    isSleeping = true;
+            }
+            
+            if (!isSleeping)
+            {
+                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+                m_scratch_v.resize(bod->getNumLinks() + 1);
+                m_scratch_m.resize(bod->getNumLinks() + 1);
+                
+                bod->addBaseForce(m_gravity * bod->getBaseMass());
+                
+                for (int j = 0; j < bod->getNumLinks(); ++j)
+                {
+                    bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
+                }
+            }  //if (!isSleeping)
+        }
+    }
+#endif  //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+    
+    {
+        BT_PROFILE("btMultiBody stepVelocities");
+        for (int i = 0; i < this->m_multiBodies.size(); i++)
+        {
+            btMultiBody* bod = m_multiBodies[i];
+            
+            bool isSleeping = false;
+            
+            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            {
+                isSleeping = true;
+            }
+            for (int b = 0; b < bod->getNumLinks(); b++)
+            {
+                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                    isSleeping = true;
+            }
+            
+            if (!isSleeping)
+            {
+                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+                m_scratch_v.resize(bod->getNumLinks() + 1);
+                m_scratch_m.resize(bod->getNumLinks() + 1);
+                bool doNotUpdatePos = false;
+                bool isConstraintPass = false;
+                {
+                    if (!bod->isUsingRK4Integration())
+                    {
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
+                                                                                  m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
+                                                                                  getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                    }
+                    else
+                    {
+                        //
+                        int numDofs = bod->getNumDofs() + 6;
+                        int numPosVars = bod->getNumPosVars() + 7;
+                        btAlignedObjectArray<btScalar> scratch_r2;
+                        scratch_r2.resize(2 * numPosVars + 8 * numDofs);
+                        //convenience
+                        btScalar* pMem = &scratch_r2[0];
+                        btScalar* scratch_q0 = pMem;
+                        pMem += numPosVars;
+                        btScalar* scratch_qx = pMem;
+                        pMem += numPosVars;
+                        btScalar* scratch_qd0 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qd1 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qd2 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qd3 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd0 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd1 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd2 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd3 = pMem;
+                        pMem += numDofs;
+                        btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]);
+                        
+                        /////
+                        //copy q0 to scratch_q0 and qd0 to scratch_qd0
+                        scratch_q0[0] = bod->getWorldToBaseRot().x();
+                        scratch_q0[1] = bod->getWorldToBaseRot().y();
+                        scratch_q0[2] = bod->getWorldToBaseRot().z();
+                        scratch_q0[3] = bod->getWorldToBaseRot().w();
+                        scratch_q0[4] = bod->getBasePos().x();
+                        scratch_q0[5] = bod->getBasePos().y();
+                        scratch_q0[6] = bod->getBasePos().z();
+                        //
+                        for (int link = 0; link < bod->getNumLinks(); ++link)
+                        {
+                            for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
+                                scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
+                        }
+                        //
+                        for (int dof = 0; dof < numDofs; ++dof)
+                            scratch_qd0[dof] = bod->getVelocityVector()[dof];
+                        ////
+                        struct
+                        {
+                            btMultiBody* bod;
                             btScalar *scratch_qx, *scratch_q0;
-
-						    void operator()()
-						    {
-						        for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
+                            
+                            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)
+                            }
+                        } 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
+                            }
+                            
+                        } pEulerIntegrate;
+                        //
+                        struct
                         {
-                            void operator()(btMultiBody *pBody, const btScalar *pData)
+                            void operator()(btMultiBody* pBody, const btScalar* pData)
                             {
-                                btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
-
-                                for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
+                                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)
+                        {
+                            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->computeAccelerationsArticulatedBodyAlgorithmMultiDof(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->computeAccelerationsArticulatedBodyAlgorithmMultiDof(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->computeAccelerationsArticulatedBodyAlgorithmMultiDof(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->computeAccelerationsArticulatedBodyAlgorithmMultiDof(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->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, scratch_r, scratch_v, scratch_m);
-						}
-						
-					}
-				}
-				
-#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-				bod->clearForcesAndTorques();
-#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-			}//if (!isSleeping)
-		}
-	}
-
-	clearMultiBodyConstraintForces();
-
-	m_solverMultiBodyIslandCallback->processConstraints();
-	
-	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
-
-	{
-                BT_PROFILE("btMultiBody stepVelocities");
-                for (int i=0;i<this->m_multiBodies.size();i++)
-                {
-                        btMultiBody* bod = m_multiBodies[i];
-
-                        bool isSleeping = false;
-
-                        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+                            }
+                        } pCopy;
+                        //
+                        
+                        btScalar h = solverInfo.m_timeStep;
+#define output &m_scratch_r[bod->getNumDofs()]
+                        //calc qdd0 from: q0 & qd0
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd0, 0, numDofs);
+                        //calc q1 = q0 + h/2 * qd0
+                        pResetQx();
+                        bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0);
+                        //calc qd1 = qd0 + h/2 * qdd0
+                        pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
+                        //
+                        //calc qdd1 from: q1 & qd1
+                        pCopyToVelocityVector(bod, scratch_qd1);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd1, 0, numDofs);
+                        //calc q2 = q0 + h/2 * qd1
+                        pResetQx();
+                        bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1);
+                        //calc qd2 = qd0 + h/2 * qdd1
+                        pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
+                        //
+                        //calc qdd2 from: q2 & qd2
+                        pCopyToVelocityVector(bod, scratch_qd2);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd2, 0, numDofs);
+                        //calc q3 = q0 + h * qd2
+                        pResetQx();
+                        bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
+                        //calc qd3 = qd0 + h * qdd2
+                        pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
+                        //
+                        //calc qdd3 from: q3 & qd3
+                        pCopyToVelocityVector(bod, scratch_qd3);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd3, 0, numDofs);
+                        
+                        //
+                        //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
+                        //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
+                        btAlignedObjectArray<btScalar> delta_q;
+                        delta_q.resize(numDofs);
+                        btAlignedObjectArray<btScalar> delta_qd;
+                        delta_qd.resize(numDofs);
+                        for (int i = 0; i < numDofs; ++i)
                         {
-                                isSleeping = true;
+                            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];
                         }
-                        for (int b=0;b<bod->getNumLinks();b++)
+                        //
+                        pCopyToVelocityVector(bod, scratch_qd0);
+                        bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
+                        //
+                        if (!doNotUpdatePos)
                         {
-                                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
-                                        isSleeping = true;
+                            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);
                         }
-
-                        if (!isSleeping)
+                        
+                        //ugly hack which resets the cached data to t0 (needed for constraint solver)
                         {
-                                //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);
-
-                                
-                            {
-                                if(!bod->isUsingRK4Integration())
-                                {
-									bool isConstraintPass = true;
-                                    bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass);
-                                }
-				}
-			}
-		}
-	}
-
-	for (int i=0;i<this->m_multiBodies.size();i++)
-	{
-		btMultiBody* bod = m_multiBodies[i];
-		bod->processDeltaVeeMultiDof2();
-	}
-
+                            for (int link = 0; link < bod->getNumLinks(); ++link)
+                                bod->getLink(link).updateCacheMultiDof();
+                            bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                      isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                      getSolverInfo().m_jointFeedbackInJointFrame);
+                        }
+                    }
+                }
+                
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+                bod->clearForcesAndTorques();
+#endif         //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+            }  //if (!isSleeping)
+        }
+    }
 }
 
-void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
+
+void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
 	btDiscreteDynamicsWorld::integrateTransforms(timeStep);
+    integrateMultiBodyTransforms(timeStep);
+}
 
-	{
+void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
+{
 		BT_PROFILE("btMultiBody stepPositions");
 		//integrate and update the Featherstone hierarchies
-		btAlignedObjectArray<btQuaternion> world_to_local;
-		btAlignedObjectArray<btVector3> local_origin;
 
-		for (int b=0;b<m_multiBodies.size();b++)
+		for (int b = 0; b < m_multiBodies.size(); b++)
 		{
 			btMultiBody* bod = m_multiBodies[b];
 			bool isSleeping = false;
 			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
 			{
 				isSleeping = true;
-			} 
-			for (int b=0;b<bod->getNumLinks();b++)
+			}
+			for (int b = 0; b < bod->getNumLinks(); b++)
 			{
-				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
+				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
 					isSleeping = true;
 			}
 
-
 			if (!isSleeping)
 			{
+				bod->addSplitV();
 				int nLinks = bod->getNumLinks();
 
 				///base + num m_links
-			
-				
-				{
-					if(!bod->isPosUpdated())
-						bod->stepPositionsMultiDof(timeStep);
-					else
-					{
-						btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
-						pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
+                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);
+                }
 
-						bod->stepPositionsMultiDof(1, 0, pRealBuf);
-						bod->setPosUpdated(false);
-					}
-				}
-				
-				world_to_local.resize(nLinks+1);
-				local_origin.resize(nLinks+1);
 
-				bod->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
-				
-			} else
+				m_scratch_world_to_local.resize(nLinks + 1);
+				m_scratch_local_origin.resize(nLinks + 1);
+                bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
+				bod->substractSplitV();
+			}
+			else
 			{
 				bod->clearVelocities();
 			}
 		}
-	}
 }
 
+void btMultiBodyDynamicsWorld::predictMultiBodyTransforms(btScalar timeStep)
+{
+    BT_PROFILE("btMultiBody stepPositions");
+    //integrate and update the Featherstone hierarchies
+    
+    for (int b = 0; b < m_multiBodies.size(); b++)
+    {
+        btMultiBody* bod = m_multiBodies[b];
+        bool isSleeping = false;
+        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+        {
+            isSleeping = true;
+        }
+        for (int b = 0; b < bod->getNumLinks(); b++)
+        {
+            if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                isSleeping = true;
+        }
+        
+        if (!isSleeping)
+        {
+            int nLinks = bod->getNumLinks();
+            bod->predictPositionsMultiDof(timeStep);
+            m_scratch_world_to_local.resize(nLinks + 1);
+            m_scratch_local_origin.resize(nLinks + 1);
+            bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
+        }
+        else
+        {
+            bod->clearVelocities();
+        }
+    }
+}
 
-
-void	btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
+void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint)
 {
 	m_multiBodyConstraints.push_back(constraint);
 }
 
-void	btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
+void btMultiBodyDynamicsWorld::removeMultiBodyConstraint(btMultiBodyConstraint* constraint)
 {
 	m_multiBodyConstraints.remove(constraint);
 }
@@ -800,16 +669,17 @@ void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstrain
 	constraint->debugDraw(getDebugDrawer());
 }
 
-
-void	btMultiBodyDynamicsWorld::debugDrawWorld()
+void btMultiBodyDynamicsWorld::debugDrawWorld()
 {
 	BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
 
+	btDiscreteDynamicsWorld::debugDrawWorld();
+
 	bool drawConstraints = false;
 	if (getDebugDrawer())
 	{
 		int mode = getDebugDrawer()->getDebugMode();
-		if (mode  & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
+		if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
 		{
 			drawConstraints = true;
 		}
@@ -817,159 +687,148 @@ void	btMultiBodyDynamicsWorld::debugDrawWorld()
 		if (drawConstraints)
 		{
 			BT_PROFILE("btMultiBody debugDrawWorld");
-			
-			btAlignedObjectArray<btQuaternion> world_to_local1;
-			btAlignedObjectArray<btVector3> local_origin1;
 
-			for (int c=0;c<m_multiBodyConstraints.size();c++)
+			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++)
+			for (int b = 0; b < m_multiBodies.size(); b++)
 			{
 				btMultiBody* bod = m_multiBodies[b];
-				bod->forwardKinematics(world_to_local1,local_origin1);
-				
-				getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
+				bod->forwardKinematics(m_scratch_world_to_local1, m_scratch_local_origin1);
 
+				if (mode & btIDebugDraw::DBG_DrawFrames)
+				{
+					getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
+				}
 
-				for (int m = 0; m<bod->getNumLinks(); m++)
+				for (int m = 0; m < bod->getNumLinks(); m++)
 				{
-					
 					const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
-
-					getDebugDrawer()->drawTransform(tr, 0.1);
-
-						//draw the joint axis
-					if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
+					if (mode & btIDebugDraw::DBG_DrawFrames)
 					{
-						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
-					
-						btVector4 color(0,0,0,1);//1,1,1);
-						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
-						btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
-						getDebugDrawer()->drawLine(from,to,color);
+						getDebugDrawer()->drawTransform(tr, 0.1);
 					}
-					if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
+					//draw the joint axis
+					if (bod->getLink(m).m_jointType == btMultibodyLink::eRevolute)
 					{
-						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
-					
-						btVector4 color(0,0,0,1);//1,1,1);
-						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
-						btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
-						getDebugDrawer()->drawLine(from,to,color);
+						btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_topVec) * 0.1;
+
+						btVector4 color(0, 0, 0, 1);  //1,1,1);
+						btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						getDebugDrawer()->drawLine(from, to, color);
 					}
-					if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
+					if (bod->getLink(m).m_jointType == btMultibodyLink::eFixed)
 					{
-						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
-					
-						btVector4 color(0,0,0,1);//1,1,1);
-						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
-						btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
-						getDebugDrawer()->drawLine(from,to,color);
+						btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1;
+
+						btVector4 color(0, 0, 0, 1);  //1,1,1);
+						btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						getDebugDrawer()->drawLine(from, to, color);
+					}
+					if (bod->getLink(m).m_jointType == btMultibodyLink::ePrismatic)
+					{
+						btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1;
+
+						btVector4 color(0, 0, 0, 1);  //1,1,1);
+						btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						getDebugDrawer()->drawLine(from, to, color);
 					}
-					
 				}
 			}
 		}
 	}
-
-	btDiscreteDynamicsWorld::debugDrawWorld();
 }
 
-
-
 void btMultiBodyDynamicsWorld::applyGravity()
 {
-        btDiscreteDynamicsWorld::applyGravity();
+	btDiscreteDynamicsWorld::applyGravity();
 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-        BT_PROFILE("btMultiBody addGravity");
-        for (int i=0;i<this->m_multiBodies.size();i++)
-        {
-                btMultiBody* bod = m_multiBodies[i];
+	BT_PROFILE("btMultiBody addGravity");
+	for (int i = 0; i < this->m_multiBodies.size(); i++)
+	{
+		btMultiBody* bod = m_multiBodies[i];
 
-                bool isSleeping = false;
+		bool isSleeping = false;
 
-                if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
-                {
-                        isSleeping = true;
-                }
-                for (int b=0;b<bod->getNumLinks();b++)
-                {
-                        if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
-                                isSleeping = true;
-                }
+		if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+		{
+			isSleeping = true;
+		}
+		for (int b = 0; b < bod->getNumLinks(); b++)
+		{
+			if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+				isSleeping = true;
+		}
 
-                if (!isSleeping)
-                {
-                        bod->addBaseForce(m_gravity * bod->getBaseMass());
+		if (!isSleeping)
+		{
+			bod->addBaseForce(m_gravity * bod->getBaseMass());
 
-                        for (int j = 0; j < bod->getNumLinks(); ++j)
-                        {
-                                bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
-                        }
-                }//if (!isSleeping)
-        }
-#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+			for (int j = 0; j < bod->getNumLinks(); ++j)
+			{
+				bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
+			}
+		}  //if (!isSleeping)
+	}
+#endif  //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 }
 
 void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
-{ 
-  for (int i=0;i<this->m_multiBodies.size();i++)
-                {       
-                        btMultiBody* bod = m_multiBodies[i];
-			bod->clearConstraintForces();
-                  } 
+{
+	for (int i = 0; i < this->m_multiBodies.size(); i++)
+	{
+		btMultiBody* bod = m_multiBodies[i];
+		bod->clearConstraintForces();
+	}
 }
 void btMultiBodyDynamicsWorld::clearMultiBodyForces()
 {
-              {
-                BT_PROFILE("clearMultiBodyForces");
-                for (int i=0;i<this->m_multiBodies.size();i++)
-                {
-                        btMultiBody* bod = m_multiBodies[i];
+	{
+		// BT_PROFILE("clearMultiBodyForces");
+		for (int i = 0; i < this->m_multiBodies.size(); i++)
+		{
+			btMultiBody* bod = m_multiBodies[i];
 
-                        bool isSleeping = false;
+			bool isSleeping = false;
 
-                        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
-                        {       
-                                isSleeping = true;
-                        }
-                        for (int b=0;b<bod->getNumLinks();b++)
-                        {       
-                                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)     
-                                        isSleeping = true;
-                        }
+			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+			{
+				isSleeping = true;
+			}
+			for (int b = 0; b < bod->getNumLinks(); b++)
+			{
+				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+					isSleeping = true;
+			}
 
-                        if (!isSleeping)
-                        {
-                        btMultiBody* bod = m_multiBodies[i];
-                        bod->clearForcesAndTorques();
-                	}
+			if (!isSleeping)
+			{
+				btMultiBody* bod = m_multiBodies[i];
+				bod->clearForcesAndTorques();
+			}
 		}
 	}
-
 }
 void btMultiBodyDynamicsWorld::clearForces()
 {
-        btDiscreteDynamicsWorld::clearForces();
+	btDiscreteDynamicsWorld::clearForces();
 
 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 	clearMultiBodyForces();
 #endif
 }
 
-
-
-
-void	btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
+void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
 {
-
 	serializer->startSerialization();
 
-	serializeDynamicsWorldInfo( serializer);
+	serializeDynamicsWorldInfo(serializer);
 
 	serializeMultiBodies(serializer);
 
@@ -977,22 +836,41 @@ void	btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializeCollisionObjects(serializer);
 
+	serializeContactManifolds(serializer);
+
 	serializer->finishSerialization();
 }
 
-void	btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
+void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
 {
 	int i;
 	//serialize all collision objects
-	for (i=0;i<m_multiBodies.size();i++)
+	for (i = 0; i < m_multiBodies.size(); i++)
 	{
 		btMultiBody* mb = m_multiBodies[i];
 		{
 			int len = mb->calculateSerializeBufferSize();
-			btChunk* chunk = serializer->allocate(len,1);
+			btChunk* chunk = serializer->allocate(len, 1);
 			const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
-			serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
+			serializer->finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb);
 		}
 	}
 
-}
\ No newline at end of file
+	//serialize all multibody links (collision objects)
+	for (i = 0; i < m_collisionObjects.size(); i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		if (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+		{
+			int len = colObj->calculateSerializeBufferSize();
+			btChunk* chunk = serializer->allocate(len, 1);
+			const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk, structType, BT_MB_LINKCOLLIDER_CODE, colObj);
+		}
+	}
+}
+//
+//void btMultiBodyDynamicsWorld::setSplitIslands(bool split)
+//{
+//    m_islandManager->setSplitIslands(split);
+//}
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
index 03ef3335c22..9ac46f4b64f 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
@@ -17,6 +17,7 @@ subject to the following restrictions:
 #define BT_MULTIBODY_DYNAMICS_WORLD_H
 
 #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h"
 
 #define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 
@@ -33,67 +34,91 @@ protected:
 	btAlignedObjectArray<btMultiBody*> m_multiBodies;
 	btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
 	btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
-	btMultiBodyConstraintSolver*	m_multiBodyConstraintSolver;
-	MultiBodyInplaceSolverIslandCallback*	m_solverMultiBodyIslandCallback;
-
-	virtual void	calculateSimulationIslands();
-	virtual void	updateActivationState(btScalar timeStep);
-	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
+	btMultiBodyConstraintSolver* m_multiBodyConstraintSolver;
+	MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback;
+
+	//cached data to avoid memory allocations
+	btAlignedObjectArray<btQuaternion> m_scratch_world_to_local;
+	btAlignedObjectArray<btVector3> m_scratch_local_origin;
+	btAlignedObjectArray<btQuaternion> m_scratch_world_to_local1;
+	btAlignedObjectArray<btVector3> m_scratch_local_origin1;
+	btAlignedObjectArray<btScalar> m_scratch_r;
+	btAlignedObjectArray<btVector3> m_scratch_v;
+	btAlignedObjectArray<btMatrix3x3> m_scratch_m;
+
+	virtual void calculateSimulationIslands();
+	virtual void updateActivationState(btScalar timeStep);
 	
-	virtual void	serializeMultiBodies(btSerializer* serializer);
-
-public:
 
-	btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+	virtual void serializeMultiBodies(btSerializer* serializer);
 
-	virtual ~btMultiBodyDynamicsWorld ();
+public:
+	btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration);
 
-	virtual void	addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter);
+	virtual ~btMultiBodyDynamicsWorld();
+    
+    virtual void solveConstraints(btContactSolverInfo& solverInfo);
+    
+	virtual void addMultiBody(btMultiBody* body, int group = btBroadphaseProxy::DefaultFilter, int mask = btBroadphaseProxy::AllFilter);
 
-	virtual void	removeMultiBody(btMultiBody* body);
+	virtual void removeMultiBody(btMultiBody* body);
 
-	virtual int		getNumMultibodies() const
+	virtual int getNumMultibodies() const
 	{
 		return m_multiBodies.size();
 	}
 
-	btMultiBody*	getMultiBody(int mbIndex)
+	btMultiBody* getMultiBody(int mbIndex)
+	{
+		return m_multiBodies[mbIndex];
+	}
+
+	const btMultiBody* getMultiBody(int mbIndex) const
 	{
 		return m_multiBodies[mbIndex];
 	}
 
-	virtual void	addMultiBodyConstraint( btMultiBodyConstraint* constraint);
+	virtual void addMultiBodyConstraint(btMultiBodyConstraint* constraint);
 
-	virtual int     getNumMultiBodyConstraints() const
+	virtual int getNumMultiBodyConstraints() const
 	{
-        return m_multiBodyConstraints.size();
+		return m_multiBodyConstraints.size();
 	}
 
-	virtual btMultiBodyConstraint*	getMultiBodyConstraint( int constraintIndex)
+	virtual btMultiBodyConstraint* getMultiBodyConstraint(int constraintIndex)
 	{
-        return m_multiBodyConstraints[constraintIndex];
+		return m_multiBodyConstraints[constraintIndex];
 	}
 
-	virtual const btMultiBodyConstraint*	getMultiBodyConstraint( int constraintIndex) const
+	virtual const btMultiBodyConstraint* getMultiBodyConstraint(int constraintIndex) const
 	{
-        return m_multiBodyConstraints[constraintIndex];
+		return m_multiBodyConstraints[constraintIndex];
 	}
 
-	virtual void	removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
+	virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint);
 
-	virtual void	integrateTransforms(btScalar timeStep);
+	virtual void integrateTransforms(btScalar timeStep);
+    void integrateMultiBodyTransforms(btScalar timeStep);
+    void predictMultiBodyTransforms(btScalar timeStep);
+    
+    virtual void predictUnconstraintMotion(btScalar timeStep);
+	virtual void debugDrawWorld();
 
-	virtual void	debugDrawWorld();
+	virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
 
-	virtual void	debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
-	
-	void	forwardKinematics();
+	void forwardKinematics();
 	virtual void clearForces();
 	virtual void clearMultiBodyConstraintForces();
 	virtual void clearMultiBodyForces();
 	virtual void applyGravity();
-	
-	virtual	void	serialize(btSerializer* serializer);
 
+	virtual void serialize(btSerializer* serializer);
+	virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver);
+	virtual void setConstraintSolver(btConstraintSolver* solver);
+	virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
+    
+    virtual void solveExternalForces(btContactSolverInfo& solverInfo);
+    virtual void solveInternalConstraints(btContactSolverInfo& solverInfo);
+    void buildIslands();
 };
-#endif //BT_MULTIBODY_DYNAMICS_WORLD_H
+#endif  //BT_MULTIBODY_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
new file mode 100644
index 00000000000..df2abbe97a5
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
@@ -0,0 +1,215 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodyFixedConstraint.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#define BTMBFIXEDCONSTRAINT_DIM 6
+
+btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+	: btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(bodyB),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB)
+{
+	m_data.resize(BTMBFIXEDCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(0),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB)
+{
+	m_data.resize(BTMBFIXEDCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+void btMultiBodyFixedConstraint::finalizeMultiDof()
+{
+	//not implemented yet
+	btAssert(0);
+}
+
+btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
+{
+}
+
+int btMultiBodyFixedConstraint::getIslandIdA() const
+{
+	if (m_rigidBodyA)
+		return m_rigidBodyA->getIslandTag();
+
+	if (m_bodyA)
+	{
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodyFixedConstraint::getIslandIdB() const
+{
+	if (m_rigidBodyB)
+		return m_rigidBodyB->getIslandTag();
+	if (m_bodyB)
+	{
+		if (m_linkB < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
+{
+	int numDim = BTMBFIXEDCONSTRAINT_DIM;
+	for (int i = 0; i < numDim; i++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = i;
+		constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal1.setValue(0, 0, 0);
+		constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal2.setValue(0, 0, 0);
+		constraintRow.m_angularComponentA.setValue(0, 0, 0);
+		constraintRow.m_angularComponentB.setValue(0, 0, 0);
+
+		constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
+		constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
+
+		// Convert local points back to world
+		btVector3 pivotAworld = m_pivotInA;
+		btMatrix3x3 frameAworld = m_frameInA;
+		if (m_rigidBodyA)
+		{
+			constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
+			pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+			frameAworld = frameAworld.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
+		}
+		else
+		{
+			if (m_bodyA)
+			{
+				pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+				frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
+			}
+		}
+		btVector3 pivotBworld = m_pivotInB;
+		btMatrix3x3 frameBworld = m_frameInB;
+		if (m_rigidBodyB)
+		{
+			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
+			pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+			frameBworld = frameBworld.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
+		}
+		else
+		{
+			if (m_bodyB)
+			{
+				pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+				frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);
+			}
+		}
+
+		btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
+		btVector3 angleDiff;
+		btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
+
+		btVector3 constraintNormalLin(0, 0, 0);
+		btVector3 constraintNormalAng(0, 0, 0);
+		btScalar posError = 0.0;
+		if (i < 3)
+		{
+			constraintNormalLin[i] = 1;
+			posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse);
+		}
+		else
+		{  //i>=3
+			constraintNormalAng = frameAworld.getColumn(i % 3);
+			posError = angleDiff[i % 3];
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
+		}
+	}
+}
+
+void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer)
+{
+	btTransform tr;
+	tr.setIdentity();
+
+	if (m_rigidBodyA)
+	{
+		btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyA)
+	{
+		btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+		tr.setOrigin(pivotAworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_rigidBodyB)
+	{
+		// that ideally should draw the same frame
+		btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyB)
+	{
+		btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+		tr.setOrigin(pivotBworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+}
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h
new file mode 100644
index 00000000000..adb1cb47dad
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h
@@ -0,0 +1,91 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_FIXED_CONSTRAINT_H
+#define BT_MULTIBODY_FIXED_CONSTRAINT_H
+
+#include "btMultiBodyConstraint.h"
+
+class btMultiBodyFixedConstraint : public btMultiBodyConstraint
+{
+protected:
+	btRigidBody* m_rigidBodyA;
+	btRigidBody* m_rigidBodyB;
+	btVector3 m_pivotInA;
+	btVector3 m_pivotInB;
+	btMatrix3x3 m_frameInA;
+	btMatrix3x3 m_frameInB;
+
+public:
+	btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+	btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+
+	virtual ~btMultiBodyFixedConstraint();
+
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	const btVector3& getPivotInA() const
+	{
+		return m_pivotInA;
+	}
+
+	void setPivotInA(const btVector3& pivotInA)
+	{
+		m_pivotInA = pivotInA;
+	}
+
+	const btVector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+	virtual void setPivotInB(const btVector3& pivotInB)
+	{
+		m_pivotInB = pivotInB;
+	}
+
+	const btMatrix3x3& getFrameInA() const
+	{
+		return m_frameInA;
+	}
+
+	void setFrameInA(const btMatrix3x3& frameInA)
+	{
+		m_frameInA = frameInA;
+	}
+
+	const btMatrix3x3& getFrameInB() const
+	{
+		return m_frameInB;
+	}
+
+	virtual void setFrameInB(const btMatrix3x3& frameInB)
+	{
+		m_frameInB = frameInB;
+	}
+
+	virtual void debugDraw(class btIDebugDraw* drawer);
+};
+
+#endif  //BT_MULTIBODY_FIXED_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
new file mode 100644
index 00000000000..ee02cf9b07a
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
@@ -0,0 +1,181 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodyGearConstraint.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false, MULTIBODY_CONSTRAINT_GEAR),
+	  m_gearRatio(1),
+	  m_gearAuxLink(-1),
+	  m_erp(0),
+	  m_relativePositionTarget(0)
+{
+}
+
+void btMultiBodyGearConstraint::finalizeMultiDof()
+{
+	allocateJacobiansMultiDof();
+
+	m_numDofsFinalized = m_jacSizeBoth;
+}
+
+btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
+{
+}
+
+int btMultiBodyGearConstraint::getIslandIdA() const
+{
+	if (m_bodyA)
+	{
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodyGearConstraint::getIslandIdB() const
+{
+	if (m_bodyB)
+	{
+		if (m_linkB < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+													 btMultiBodyJacobianData& data,
+													 const btContactSolverInfo& infoGlobal)
+{
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
+
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+		finalizeMultiDof();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
+		return;
+
+	if (m_maxAppliedImpulse == 0.f)
+		return;
+
+	// note: we rely on the fact that data.m_jacobians are
+	// always initialized to zero by the Constraint ctor
+	int linkDoF = 0;
+	unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
+	unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF);
+
+	// row 0: the lower bound
+	jacobianA(0)[offsetA] = 1;
+	jacobianB(0)[offsetB] = m_gearRatio;
+
+	btScalar posError = 0;
+	const btVector3 dummy(0, 0, 0);
+
+	btScalar kp = 1;
+	btScalar kd = 1;
+	int numRows = getNumRows();
+
+	for (int row = 0; row < numRows; row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+
+		int dof = 0;
+		btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+		btScalar auxVel = 0;
+
+		if (m_gearAuxLink >= 0)
+		{
+			auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
+		}
+		currentVelocity += auxVel;
+		if (m_erp != 0)
+		{
+			btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+			if (m_gearAuxLink >= 0)
+			{
+				currentPositionA -= m_bodyA->getJointPosMultiDof(m_gearAuxLink)[dof];
+			}
+			btScalar currentPositionB = m_gearRatio * m_bodyA->getJointPosMultiDof(m_linkB)[dof];
+			btScalar diff = currentPositionB + currentPositionA;
+			btScalar desiredPositionDiff = this->m_relativePositionTarget;
+			posError = -m_erp * (desiredPositionDiff - diff);
+		}
+
+		btScalar desiredRelativeVelocity = auxVel;
+
+		fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, desiredRelativeVelocity);
+
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = row;
+		{
+			//expect either prismatic or revolute joint type for now
+			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eRevolute:
+				{
+					constraintRow.m_contactNormal1.setZero();
+					constraintRow.m_contactNormal2.setZero();
+					btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+					constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
+					constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
+
+					break;
+				}
+				case btMultibodyLink::ePrismatic:
+				{
+					btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+					constraintRow.m_contactNormal1 = prismaticAxisInWorld;
+					constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
+					constraintRow.m_relpos1CrossNormal.setZero();
+					constraintRow.m_relpos2CrossNormal.setZero();
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+		}
+	}
+}
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h
new file mode 100644
index 00000000000..31888fbc682
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h
@@ -0,0 +1,115 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_GEAR_CONSTRAINT_H
+#define BT_MULTIBODY_GEAR_CONSTRAINT_H
+
+#include "btMultiBodyConstraint.h"
+
+class btMultiBodyGearConstraint : public btMultiBodyConstraint
+{
+protected:
+	btRigidBody* m_rigidBodyA;
+	btRigidBody* m_rigidBodyB;
+	btVector3 m_pivotInA;
+	btVector3 m_pivotInB;
+	btMatrix3x3 m_frameInA;
+	btMatrix3x3 m_frameInB;
+	btScalar m_gearRatio;
+	int m_gearAuxLink;
+	btScalar m_erp;
+	btScalar m_relativePositionTarget;
+
+public:
+	//btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+	btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+
+	virtual ~btMultiBodyGearConstraint();
+
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	const btVector3& getPivotInA() const
+	{
+		return m_pivotInA;
+	}
+
+	void setPivotInA(const btVector3& pivotInA)
+	{
+		m_pivotInA = pivotInA;
+	}
+
+	const btVector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+	virtual void setPivotInB(const btVector3& pivotInB)
+	{
+		m_pivotInB = pivotInB;
+	}
+
+	const btMatrix3x3& getFrameInA() const
+	{
+		return m_frameInA;
+	}
+
+	void setFrameInA(const btMatrix3x3& frameInA)
+	{
+		m_frameInA = frameInA;
+	}
+
+	const btMatrix3x3& getFrameInB() const
+	{
+		return m_frameInB;
+	}
+
+	virtual void setFrameInB(const btMatrix3x3& frameInB)
+	{
+		m_frameInB = frameInB;
+	}
+
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
+
+	virtual void setGearRatio(btScalar gearRatio)
+	{
+		m_gearRatio = gearRatio;
+	}
+	virtual void setGearAuxLink(int gearAuxLink)
+	{
+		m_gearAuxLink = gearAuxLink;
+	}
+	virtual void setRelativePositionTarget(btScalar relPosTarget)
+	{
+		m_relativePositionTarget = relPosTarget;
+	}
+	virtual void setErp(btScalar erp)
+	{
+		m_erp = erp;
+	}
+};
+
+#endif  //BT_MULTIBODY_GEAR_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h
new file mode 100644
index 00000000000..3169b86e616
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h
@@ -0,0 +1,247 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H
+#define BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H
+
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "btMultiBodyConstraintSolver.h"
+
+SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs)
+{
+    int islandId;
+    
+    const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
+    const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
+    islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
+    return islandId;
+}
+class btSortConstraintOnIslandPredicate2
+{
+public:
+    bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const
+    {
+        int rIslandId0, lIslandId0;
+        rIslandId0 = btGetConstraintIslandId2(rhs);
+        lIslandId0 = btGetConstraintIslandId2(lhs);
+        return lIslandId0 < rIslandId0;
+    }
+};
+
+SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
+{
+    int islandId;
+    
+    int islandTagA = lhs->getIslandIdA();
+    int islandTagB = lhs->getIslandIdB();
+    islandId = islandTagA >= 0 ? islandTagA : islandTagB;
+    return islandId;
+}
+
+class btSortMultiBodyConstraintOnIslandPredicate
+{
+public:
+    bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const
+    {
+        int rIslandId0, lIslandId0;
+        rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
+        lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
+        return lIslandId0 < rIslandId0;
+    }
+};
+
+struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
+{
+
+    btContactSolverInfo* m_solverInfo;
+    btMultiBodyConstraintSolver* m_solver;
+    btMultiBodyConstraint** m_multiBodySortedConstraints;
+    int m_numMultiBodyConstraints;
+    
+    btTypedConstraint** m_sortedConstraints;
+    int m_numConstraints;
+    btIDebugDraw* m_debugDrawer;
+    btDispatcher* m_dispatcher;
+    
+    btAlignedObjectArray<btCollisionObject*> m_bodies;
+	btAlignedObjectArray<btCollisionObject*> m_softBodies;
+    btAlignedObjectArray<btPersistentManifold*> m_manifolds;
+    btAlignedObjectArray<btTypedConstraint*> m_constraints;
+    btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
+    
+    btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData;
+    
+    MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver,
+                                         btDispatcher* dispatcher)
+    : m_solverInfo(NULL),
+    m_solver(solver),
+    m_multiBodySortedConstraints(NULL),
+    m_numConstraints(0),
+    m_debugDrawer(NULL),
+    m_dispatcher(dispatcher)
+    {
+    }
+    
+    MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other)
+    {
+        btAssert(0);
+        (void)other;
+        return *this;
+    }
+    
+    SIMD_FORCE_INLINE virtual void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
+    {
+        m_islandAnalyticsData.clear();
+        btAssert(solverInfo);
+        m_solverInfo = solverInfo;
+        
+        m_multiBodySortedConstraints = sortedMultiBodyConstraints;
+        m_numMultiBodyConstraints = numMultiBodyConstraints;
+        m_sortedConstraints = sortedConstraints;
+        m_numConstraints = numConstraints;
+        
+        m_debugDrawer = debugDrawer;
+        m_bodies.resize(0);
+        m_manifolds.resize(0);
+        m_constraints.resize(0);
+        m_multiBodyConstraints.resize(0);
+    }
+    
+    void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
+    {
+        m_solver = solver;
+    }
+    
+    virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId)
+    {
+        if (islandId < 0)
+        {
+            ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
+            m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
+            if (m_solverInfo->m_reportSolverAnalytics&1)
+            {
+                m_solver->m_analyticsData.m_islandId = islandId;
+                m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+            }
+        }
+        else
+        {
+            //also add all non-contact constraints/joints for this island
+            btTypedConstraint** startConstraint = 0;
+            btMultiBodyConstraint** startMultiBodyConstraint = 0;
+            
+            int numCurConstraints = 0;
+            int numCurMultiBodyConstraints = 0;
+            
+            int i;
+            
+            //find the first constraint for this island
+            
+            for (i = 0; i < m_numConstraints; i++)
+            {
+                if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
+                {
+                    startConstraint = &m_sortedConstraints[i];
+                    break;
+                }
+            }
+            //count the number of constraints in this island
+            for (; i < m_numConstraints; i++)
+            {
+                if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
+                {
+                    numCurConstraints++;
+                }
+            }
+            
+            for (i = 0; i < m_numMultiBodyConstraints; i++)
+            {
+                if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
+                {
+                    startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
+                    break;
+                }
+            }
+            //count the number of multi body constraints in this island
+            for (; i < m_numMultiBodyConstraints; i++)
+            {
+                if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
+                {
+                    numCurMultiBodyConstraints++;
+                }
+            }
+            
+            //if (m_solverInfo->m_minimumSolverBatchSize<=1)
+            //{
+            //    m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
+            //} else
+            {
+                for (i = 0; i < numBodies; i++)
+				{
+					bool isSoftBodyType = (bodies[i]->getInternalType() & btCollisionObject::CO_SOFT_BODY);
+					if (!isSoftBodyType)
+					{
+						m_bodies.push_back(bodies[i]);
+					}
+					else
+					{
+						m_softBodies.push_back(bodies[i]);
+					}
+				}
+                for (i = 0; i < numManifolds; i++)
+                    m_manifolds.push_back(manifolds[i]);
+                for (i = 0; i < numCurConstraints; i++)
+                    m_constraints.push_back(startConstraint[i]);
+                
+                for (i = 0; i < numCurMultiBodyConstraints; i++)
+                    m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
+                
+                if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize)
+                {
+                    processConstraints(islandId);
+                }
+                else
+                {
+                    //printf("deferred\n");
+                }
+            }
+        }
+    }
+    
+    virtual void processConstraints(int islandId=-1)
+    {
+        btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
+        btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
+        btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
+        btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
+        
+        //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
+        
+        m_solver->solveMultiBodyGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
+        if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1))
+        {
+            m_solver->m_analyticsData.m_islandId = islandId;
+            m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+        }
+        m_bodies.resize(0);
+		m_softBodies.resize(0);
+        m_manifolds.resize(0);
+        m_constraints.resize(0);
+        m_multiBodyConstraints.resize(0);
+    }
+};
+
+
+#endif /*BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H */
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
index 5c2fa8ed5b9..d943019e711 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
@@ -12,8 +12,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef BT_MULTIBODY_JOINT_FEEDBACK_H
 #define BT_MULTIBODY_JOINT_FEEDBACK_H
 
@@ -21,7 +19,7 @@ subject to the following restrictions:
 
 struct btMultiBodyJointFeedback
 {
-	btSpatialForceVector	m_reactionForces;
+	btSpatialForceVector m_reactionForces;
 };
 
-#endif //BT_MULTIBODY_JOINT_FEEDBACK_H
+#endif  //BT_MULTIBODY_JOINT_FEEDBACK_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
index 3f05aa4d5fa..94b36ac108c 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
@@ -20,21 +20,18 @@ subject to the following restrictions:
 #include "btMultiBodyLinkCollider.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
-
-
 btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
 	//:btMultiBodyConstraint(body,0,link,-1,2,true),
-	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,2,true),
-	m_lowerBound(lower),
-	m_upperBound(upper)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true, MULTIBODY_CONSTRAINT_LIMIT),
+	  m_lowerBound(lower),
+	  m_upperBound(upper)
 {
-
 }
 
 void btMultiBodyJointLimitConstraint::finalizeMultiDof()
 {
 	// the data.m_jacobians never change, so may as well
-    // initialize them here
+	// initialize them here
 
 	allocateJacobiansMultiDof();
 
@@ -55,15 +52,18 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
 
 int btMultiBodyJointLimitConstraint::getIslandIdA() const
 {
-	if(m_bodyA)
+	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -71,82 +71,86 @@ int btMultiBodyJointLimitConstraint::getIslandIdA() const
 
 int btMultiBodyJointLimitConstraint::getIslandIdB() const
 {
-	if(m_bodyB)
+	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
 
-
 void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal)
+														   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.
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
 
 	if (m_numDofsFinalized != m_jacSizeBoth)
 	{
-        finalizeMultiDof();
+		finalizeMultiDof();
 	}
 
+	// row 0: the lower bound
+	setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);  //multidof: this is joint-type dependent
 
-    // row 0: the lower bound
-    setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);			//multidof: this is joint-type dependent
+	// row 1: the upper bound
+	setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
 
-    // row 1: the upper bound
-    setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
-	
-	for (int row=0;row<getNumRows();row++)
+	for (int row = 0; row < getNumRows(); row++)
 	{
-		
-		btScalar direction = row? -1 : 1;
+		btScalar penetration = getPosition(row);
+
+		//todo: consider adding some safety threshold here
+		if (penetration > 0)
+		{
+			continue;
+		}
+		btScalar direction = row ? -1 : 1;
 
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
-        constraintRow.m_orgConstraint = this;
-        constraintRow.m_orgDofIndex = row;
-        
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = row;
+
 		constraintRow.m_multiBodyA = m_bodyA;
 		constraintRow.m_multiBodyB = m_bodyB;
-		const btScalar posError = 0;						//why assume it's zero?
+		const btScalar posError = 0;  //why assume it's zero?
 		const btVector3 dummy(0, 0, 0);
 
-		btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
+		btScalar rel_vel = fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, 0, m_maxAppliedImpulse);
 
 		{
 			//expect either prismatic or revolute joint type for now
-			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
 			switch (m_bodyA->getLink(m_linkA).m_jointType)
 			{
 				case btMultibodyLink::eRevolute:
 				{
 					constraintRow.m_contactNormal1.setZero();
 					constraintRow.m_contactNormal2.setZero();
-					btVector3 revoluteAxisInWorld = direction*quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
-					constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
-					constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
-					
+					btVector3 revoluteAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+					constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
+					constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
+
 					break;
 				}
 				case btMultibodyLink::ePrismatic:
 				{
-					btVector3 prismaticAxisInWorld = direction* quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
-					constraintRow.m_contactNormal1=prismaticAxisInWorld;
-					constraintRow.m_contactNormal2=-prismaticAxisInWorld;
+					btVector3 prismaticAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+					constraintRow.m_contactNormal1 = prismaticAxisInWorld;
+					constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
 					constraintRow.m_relpos1CrossNormal.setZero();
 					constraintRow.m_relpos2CrossNormal.setZero();
-					
+
 					break;
 				}
 				default:
@@ -154,36 +158,35 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 					btAssert(0);
 				}
 			};
-			
 		}
 
 		{
-			btScalar penetration = getPosition(row);
 			btScalar positionalError = 0.f;
-			btScalar	velocityError =  - rel_vel;// * damping;
+			btScalar velocityError = -rel_vel;  // * damping;
 			btScalar erp = infoGlobal.m_erp2;
 			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 			{
 				erp = infoGlobal.m_erp;
 			}
-			if (penetration>0)
+			if (penetration > 0)
 			{
 				positionalError = 0;
 				velocityError = -penetration / infoGlobal.m_timeStep;
-			} else
+			}
+			else
 			{
-				positionalError = -penetration * erp/infoGlobal.m_timeStep;
+				positionalError = -penetration * erp / infoGlobal.m_timeStep;
 			}
 
-			btScalar  penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
-			btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
+			btScalar penetrationImpulse = positionalError * constraintRow.m_jacDiagABInv;
+			btScalar velocityImpulse = velocityError * constraintRow.m_jacDiagABInv;
 			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 			{
 				//combine position and velocity into rhs
-				constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
+				constraintRow.m_rhs = penetrationImpulse + velocityImpulse;
 				constraintRow.m_rhsPenetration = 0.f;
-
-			} else
+			}
+			else
 			{
 				//split position and velocity into rhs and m_rhsPenetration
 				constraintRow.m_rhs = velocityImpulse;
@@ -191,9 +194,4 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 			}
 		}
 	}
-
 }
-
-
-
-
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h
index 55b8d122b97..b810692b4ce 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h
@@ -22,11 +22,10 @@ struct btSolverInfo;
 class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
 {
 protected:
+	btScalar m_lowerBound;
+	btScalar m_upperBound;
 
-	btScalar	m_lowerBound;
-	btScalar	m_upperBound;
 public:
-
 	btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
 	virtual ~btMultiBodyJointLimitConstraint();
 
@@ -36,15 +35,29 @@ public:
 	virtual int getIslandIdB() const;
 
 	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal);
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
 
 	virtual void debugDraw(class btIDebugDraw* drawer)
 	{
 		//todo(erwincoumans)
 	}
-
+	btScalar getLowerBound() const
+	{
+		return m_lowerBound;
+	}
+	btScalar getUpperBound() const
+	{
+		return m_upperBound;
+	}
+	void setLowerBound(btScalar lower)
+	{
+		m_lowerBound = lower;
+	}
+	void setUpperBound(btScalar upper)
+	{
+		m_upperBound = upper;
+	}
 };
 
-#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
-
+#endif  //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
index 062d19accaa..fec9b03213a 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
@@ -20,17 +20,18 @@ subject to the following restrictions:
 #include "btMultiBodyLinkCollider.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
-
 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
-	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
-	m_desiredVelocity(desiredVelocity)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
+	  m_desiredVelocity(desiredVelocity),
+	  m_desiredPosition(0),
+	  m_kd(1.),
+	  m_kp(0),
+	  m_erp(1),
+	  m_rhsClamp(SIMD_INFINITY)
 {
-
 	m_maxAppliedImpulse = maxMotorImpulse;
 	// the data.m_jacobians never change, so may as well
-    // initialize them here
-
-
+	// initialize them here
 }
 
 void btMultiBodyJointMotor::finalizeMultiDof()
@@ -50,13 +51,17 @@ void btMultiBodyJointMotor::finalizeMultiDof()
 
 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
 	//:btMultiBodyConstraint(body,0,link,-1,1,true),
-	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
-	m_desiredVelocity(desiredVelocity)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
+	  m_desiredVelocity(desiredVelocity),
+	  m_desiredPosition(0),
+	  m_kd(1.),
+	  m_kp(0),
+	  m_erp(1),
+	  m_rhsClamp(SIMD_INFINITY)
 {
 	btAssert(linkDoF < body->getLink(link).m_dofCount);
 
 	m_maxAppliedImpulse = maxMotorImpulse;
-
 }
 btMultiBodyJointMotor::~btMultiBodyJointMotor()
 {
@@ -64,83 +69,108 @@ 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 (this->m_linkA < 0)
 	{
-		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();
+	}
+	else
+	{
+		if (m_bodyA->getLink(m_linkA).m_collider)
+		{
+			return m_bodyA->getLink(m_linkA).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++)
+	if (m_linkB < 0)
 	{
-		col = m_bodyB->getLink(i).m_collider;
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 		if (col)
 			return col->getIslandTag();
 	}
+	else
+	{
+		if (m_bodyB->getLink(m_linkB).m_collider)
+		{
+			return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
 	return -1;
 }
 
-
 void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal)
+												 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.
-	
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
+
 	if (m_numDofsFinalized != m_jacSizeBoth)
 	{
-        finalizeMultiDof();
+		finalizeMultiDof();
 	}
 
 	//don't crash
 	if (m_numDofsFinalized != m_jacSizeBoth)
 		return;
 
+	if (m_maxAppliedImpulse == 0.f)
+		return;
+
 	const btScalar posError = 0;
 	const btVector3 dummy(0, 0, 0);
 
-	for (int row=0;row<getNumRows();row++)
+	for (int row = 0; row < getNumRows(); row++)
 	{
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
 
+		int dof = 0;
+		btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+		btScalar positionStabiliationTerm = m_erp * (m_desiredPosition - currentPosition) / infoGlobal.m_timeStep;
 
-		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
+		btScalar velocityError = (m_desiredVelocity - currentVelocity);
+		btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity + m_kd * velocityError;
+		if (rhs > m_rhsClamp)
+		{
+			rhs = m_rhsClamp;
+		}
+		if (rhs < -m_rhsClamp)
+		{
+			rhs = -m_rhsClamp;
+		}
+
+		fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, rhs);
 		constraintRow.m_orgConstraint = this;
 		constraintRow.m_orgDofIndex = row;
 		{
 			//expect either prismatic or revolute joint type for now
-			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
 			switch (m_bodyA->getLink(m_linkA).m_jointType)
 			{
 				case btMultibodyLink::eRevolute:
 				{
 					constraintRow.m_contactNormal1.setZero();
 					constraintRow.m_contactNormal2.setZero();
-					btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
-					constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
-					constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
-					
+					btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+					constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
+					constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
+
 					break;
 				}
 				case btMultibodyLink::ePrismatic:
 				{
-					btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
-					constraintRow.m_contactNormal1=prismaticAxisInWorld;
-					constraintRow.m_contactNormal2=-prismaticAxisInWorld;
+					btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+					constraintRow.m_contactNormal1 = prismaticAxisInWorld;
+					constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
 					constraintRow.m_relpos1CrossNormal.setZero();
 					constraintRow.m_relpos2CrossNormal.setZero();
-					
+
 					break;
 				}
 				default:
@@ -148,10 +178,6 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
 					btAssert(0);
 				}
 			};
-			
 		}
-
 	}
-
 }
-
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
index 011aadcfa4b..1aca36352e7 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
@@ -24,34 +24,54 @@ struct btSolverInfo;
 class btMultiBodyJointMotor : public btMultiBodyConstraint
 {
 protected:
-
-
-	btScalar	m_desiredVelocity;
+	btScalar m_desiredVelocity;
+	btScalar m_desiredPosition;
+	btScalar m_kd;
+	btScalar m_kp;
+	btScalar m_erp;
+	btScalar m_rhsClamp;  //maximum error
 
 public:
-
 	btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
 	btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
 	virtual ~btMultiBodyJointMotor();
-    virtual void finalizeMultiDof();
+	virtual void finalizeMultiDof();
 
 	virtual int getIslandIdA() const;
 	virtual int getIslandIdB() const;
 
 	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal);
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)
+	{
+		m_desiredVelocity = velTarget;
+		m_kd = kd;
+	}
 
-    virtual void setVelocityTarget(btScalar velTarget)
-    {
-        m_desiredVelocity = velTarget;
-    }
+	virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f)
+	{
+		m_desiredPosition = posTarget;
+		m_kp = kp;
+	}
 
+	virtual void setErp(btScalar erp)
+	{
+		m_erp = erp;
+	}
+	virtual btScalar getErp() const
+	{
+		return m_erp;
+	}
+	virtual void setRhsClamp(btScalar rhsClamp)
+	{
+		m_rhsClamp = rhsClamp;
+	}
 	virtual void debugDraw(class btIDebugDraw* drawer)
 	{
 		//todo(erwincoumans)
 	}
 };
 
-#endif //BT_MULTIBODY_JOINT_MOTOR_H
-
+#endif  //BT_MULTIBODY_JOINT_MOTOR_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h
index 668e4443904..01d5583c2fd 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h
@@ -20,9 +20,10 @@ subject to the following restrictions:
 #include "LinearMath/btVector3.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
-enum	btMultiBodyLinkFlags
+enum btMultiBodyLinkFlags
 {
-	BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
+	BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1,
+	BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2,
 };
 
 //both defines are now permanently enabled
@@ -35,7 +36,6 @@ enum	btMultiBodyLinkFlags
 
 //namespace {
 
-
 #include "LinearMath/btSpatialAlgebra.h"
 
 //}
@@ -44,27 +44,26 @@ enum	btMultiBodyLinkFlags
 // Link struct
 //
 
-struct btMultibodyLink 
+struct btMultibodyLink
 {
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-    btScalar m_mass;         // mass of link
-    btVector3 m_inertiaLocal;   // inertia of link (local frame; diagonal)
+	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.
 
-    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.
 
-    btQuaternion m_zeroRotParentToThis;    // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
+	btVector3 m_dVector;  // vector from the inboard joint pos to this link's COM. (local frame.) constant.
+						  //this is set to zero for planar joint (see also m_eVector comment)
 
-    btVector3 m_dVector;   // vector from the inboard joint pos to this link's COM. (local frame.) constant.
-	//this is set to zero for planar joint (see also m_eVector comment)
-	
-    // m_eVector is constant, but depends on the joint type:
-    // revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
+	// m_eVector is constant, but depends on the joint type:
+	// revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
 	// planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
 	// todo: fix the planar so it is consistent with the other joints
-	
-    btVector3 m_eVector;
+
+	btVector3 m_eVector;
 
 	btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
 
@@ -78,13 +77,11 @@ struct btMultibodyLink
 		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 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)
@@ -92,117 +89,149 @@ struct btMultibodyLink
 	// 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		
+	//			   m_axesBottom[1][2] = unit vectors along the translational axes on that plane
 	btSpatialMotionVector m_axes[6];
 	void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
-	void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; }
-	void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_topVec.setValue(x, y, z); }
-	void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); }
-	const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
-	const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
+	void setAxisBottom(int dof, const btVector3 &axis)
+	{
+		m_axes[dof].m_bottomVec = axis;
+	}
+	void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
+	{
+		m_axes[dof].m_topVec.setValue(x, y, z);
+	}
+	void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
+	{
+		m_axes[dof].m_bottomVec.setValue(x, y, z);
+	}
+	const btVector3 &getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
+	const btVector3 &getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
 
 	int m_dofOffset, m_cfgOffset;
 
-    btQuaternion m_cachedRotParentToThis;   // rotates vectors in parent frame to vectors in local frame
-    btVector3 m_cachedRVector;                // vector from COM of parent to COM of this link, in local frame.
+	btQuaternion m_cachedRotParentToThis;  // rotates vectors in parent frame to vectors in local frame
+	btVector3 m_cachedRVector;             // vector from COM of parent to COM of this link, in local frame.
+    
+    // predicted verstion
+    btQuaternion m_cachedRotParentToThis_interpolate;  // rotates vectors in parent frame to vectors in local frame
+    btVector3 m_cachedRVector_interpolate;             // vector from COM of parent to COM of this link, in local frame.
 
-    btVector3 m_appliedForce;    // In WORLD frame
-    btVector3 m_appliedTorque;   // In WORLD frame
+	btVector3 m_appliedForce;   // In WORLD frame
+	btVector3 m_appliedTorque;  // In WORLD frame
 
-btVector3 m_appliedConstraintForce;    // In WORLD frame
-    btVector3 m_appliedConstraintTorque;   // In WORLD frame
+	btVector3 m_appliedConstraintForce;   // In WORLD frame
+	btVector3 m_appliedConstraintTorque;  // In WORLD frame
 
 	btScalar m_jointPos[7];
-    
-    //m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
-    //It gets set to zero after each internal stepSimulation call
+    btScalar m_jointPos_interpolate[7];
+
+	//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
+	//It gets set to zero after each internal stepSimulation call
 	btScalar m_jointTorque[6];
-    
-	class btMultiBodyLinkCollider* m_collider;
+
+	class btMultiBodyLinkCollider *m_collider;
 	int m_flags;
-	
-	
-	int m_dofCount, m_posVarCount;				//redundant but handy
-	
+
+	int m_dofCount, m_posVarCount;  //redundant but handy
+
 	eFeatherstoneJointType m_jointType;
-	
-	struct btMultiBodyJointFeedback*	m_jointFeedback;
 
-	btTransform	m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics
+	struct btMultiBodyJointFeedback *m_jointFeedback;
 
-	const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
-	const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
+	btTransform m_cachedWorldTransform;  //this cache is updated when calling btMultiBody::forwardKinematics
 
-    // ctor: set some sensible defaults
+	const char *m_linkName;   //m_linkName memory needs to be managed by the developer/user!
+	const char *m_jointName;  //m_jointName memory needs to be managed by the developer/user!
+	const void *m_userPtr;    //m_userPtr ptr needs to be managed by the developer/user!
+
+	btScalar m_jointDamping;      //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
+	btScalar m_jointFriction;     //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
+	btScalar m_jointLowerLimit;   //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+	btScalar m_jointUpperLimit;   //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+	btScalar m_jointMaxForce;     //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+	btScalar m_jointMaxVelocity;  //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+
+	// ctor: set some sensible defaults
 	btMultibodyLink()
-		: 	m_mass(1),
-			m_parent(-1),
-			m_zeroRotParentToThis(0, 0, 0, 1),
-			m_cachedRotParentToThis(0, 0, 0, 1),
-			m_collider(0),
-			m_flags(0),
-			m_dofCount(0),
-			m_posVarCount(0),
-			m_jointType(btMultibodyLink::eInvalid),
-			m_jointFeedback(0),
-			m_linkName(0),
-			m_jointName(0)
+		: m_mass(1),
+		  m_parent(-1),
+		  m_zeroRotParentToThis(0, 0, 0, 1),
+		  m_cachedRotParentToThis(0, 0, 0, 1),
+          m_cachedRotParentToThis_interpolate(0, 0, 0, 1),
+		  m_collider(0),
+		  m_flags(0),
+		  m_dofCount(0),
+		  m_posVarCount(0),
+		  m_jointType(btMultibodyLink::eInvalid),
+		  m_jointFeedback(0),
+		  m_linkName(0),
+		  m_jointName(0),
+		  m_userPtr(0),
+		  m_jointDamping(0),
+		  m_jointFriction(0),
+		  m_jointLowerLimit(0),
+		  m_jointUpperLimit(0),
+		  m_jointMaxForce(0),
+		  m_jointMaxVelocity(0)
 	{
-		
 		m_inertiaLocal.setValue(1, 1, 1);
 		setAxisTop(0, 0., 0., 0.);
 		setAxisBottom(0, 1., 0., 0.);
 		m_dVector.setValue(0, 0, 0);
 		m_eVector.setValue(0, 0, 0);
 		m_cachedRVector.setValue(0, 0, 0);
-		m_appliedForce.setValue( 0, 0, 0);
+        m_cachedRVector_interpolate.setValue(0, 0, 0);
+		m_appliedForce.setValue(0, 0, 0);
 		m_appliedTorque.setValue(0, 0, 0);
-		//		
+		m_appliedConstraintForce.setValue(0, 0, 0);
+		m_appliedConstraintTorque.setValue(0, 0, 0);
+		//
 		m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
-		m_jointPos[3] = 1.f;			//"quat.w"
+		m_jointPos[3] = 1.f;  //"quat.w"
 		m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
 		m_cachedWorldTransform.setIdentity();
 	}
 
-    // routine to update m_cachedRotParentToThis and m_cachedRVector
+	// routine to update m_cachedRotParentToThis and m_cachedRVector
 	void updateCacheMultiDof(btScalar *pq = 0)
 	{
-		btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
-
-		switch(m_jointType)
+        btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
+        btQuaternion& cachedRot = m_cachedRotParentToThis;
+        btVector3& cachedVector = m_cachedRVector;
+		switch (m_jointType)
 		{
 			case eRevolute:
 			{
-				m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
+				cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
 
 				break;
 			}
 			case ePrismatic:
 			{
 				// m_cachedRotParentToThis never changes, so no need to update
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
+				cachedVector = 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);
+				cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
 
 				break;
 			}
 			case ePlanar:
 			{
-				m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;				
-				m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector);				
+				cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+				cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
 
 				break;
 			}
 			case eFixed:
 			{
-				m_cachedRotParentToThis = m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
+				cachedRot = m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
 
 				break;
 			}
@@ -212,8 +241,60 @@ btVector3 m_appliedConstraintForce;    // In WORLD frame
 				btAssert(0);
 			}
 		}
+        m_cachedRotParentToThis_interpolate = m_cachedRotParentToThis;
+        m_cachedRVector_interpolate = m_cachedRVector;
 	}
+    
+    void updateInterpolationCacheMultiDof()
+    {
+        btScalar *pJointPos = &m_jointPos_interpolate[0];
+        
+        btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate;
+        btVector3& cachedVector = m_cachedRVector_interpolate;
+        switch (m_jointType)
+        {
+            case eRevolute:
+            {
+                cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+                cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
+                
+                break;
+            }
+            case ePrismatic:
+            {
+                // m_cachedRotParentToThis never changes, so no need to update
+                cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
+                
+                break;
+            }
+            case eSpherical:
+            {
+                cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
+                cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
+                
+                break;
+            }
+            case ePlanar:
+            {
+                cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+                cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
+                
+                break;
+            }
+            case eFixed:
+            {
+                cachedRot = m_zeroRotParentToThis;
+                cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
+                
+                break;
+            }
+            default:
+            {
+                //invalid type
+                btAssert(0);
+            }
+        }
+    }
 };
 
-
-#endif //BT_MULTIBODY_LINK_H
+#endif  //BT_MULTIBODY_LINK_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
index 5080ea87454..bc909990c21 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
@@ -4,8 +4,8 @@ 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, 
+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.
@@ -19,21 +19,32 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
 #include "btMultiBody.h"
+#include "LinearMath/btSerializer.h"
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btMultiBodyLinkColliderData btMultiBodyLinkColliderDoubleData
+#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderDoubleData"
+#else
+#define btMultiBodyLinkColliderData btMultiBodyLinkColliderFloatData
+#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderFloatData"
+#endif
 
 class btMultiBodyLinkCollider : public btCollisionObject
 {
-//protected:
+	//protected:
 public:
-
 	btMultiBody* m_multiBody;
 	int m_link;
 
+	virtual ~btMultiBodyLinkCollider()
+	{
 
-	btMultiBodyLinkCollider (btMultiBody* multiBody,int link)
-		:m_multiBody(multiBody),
-		m_link(link)
+	}
+	btMultiBodyLinkCollider(btMultiBody* multiBody, int link)
+		: m_multiBody(multiBody),
+		  m_link(link)
 	{
-		m_checkCollideWith =  true;
+		m_checkCollideWith = true;
 		//we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands
 		//this means that some constraints might point to bodies that are not in the islands, causing crashes
 		//if (link>=0 || (multiBody && !multiBody->hasFixedBase()))
@@ -49,18 +60,18 @@ public:
 	}
 	static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
 	{
-		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
+		if (colObj->getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK)
 			return (btMultiBodyLinkCollider*)colObj;
 		return 0;
 	}
 	static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj)
 	{
-		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
+		if (colObj->getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK)
 			return (btMultiBodyLinkCollider*)colObj;
 		return 0;
 	}
 
-	virtual bool checkCollideWithOverride(const  btCollisionObject* co) const
+	virtual bool checkCollideWithOverride(const btCollisionObject* co) const
 	{
 		const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co);
 		if (!other)
@@ -71,22 +82,97 @@ public:
 			return false;
 
 		//check if 'link' has collision disabled
-		if (m_link>=0)
+		if (m_link >= 0)
 		{
 			const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
-			if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link)
-				return false;
+			if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
+			{
+				int parent_of_this = m_link;
+				while (1)
+				{
+					if (parent_of_this == -1)
+						break;
+					parent_of_this = m_multiBody->getLink(parent_of_this).m_parent;
+					if (parent_of_this == other->m_link)
+					{
+						return false;
+					}
+				}
+			}
+			else if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
+			{
+				if (link.m_parent == other->m_link)
+					return false;
+			}
 		}
-		
-		if (other->m_link>=0)
+
+		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.m_parent == this->m_link)
-				return false;
+			if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
+			{
+				int parent_of_other = other->m_link;
+				while (1)
+				{
+					if (parent_of_other == -1)
+						break;
+					parent_of_other = m_multiBody->getLink(parent_of_other).m_parent;
+					if (parent_of_other == this->m_link)
+						return false;
+				}
+			}
+			else if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
+			{
+				if (otherLink.m_parent == this->m_link)
+					return false;
+			}
 		}
 		return true;
 	}
+
+	virtual int calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
+};
+
+// clang-format off
+
+struct	btMultiBodyLinkColliderFloatData
+{
+	btCollisionObjectFloatData m_colObjData;
+	btMultiBodyFloatData	*m_multiBody;
+	int			m_link;
+	char		m_padding[4];
 };
 
-#endif //BT_FEATHERSTONE_LINK_COLLIDER_H
+struct	btMultiBodyLinkColliderDoubleData
+{
+	btCollisionObjectDoubleData m_colObjData;
+	btMultiBodyDoubleData		*m_multiBody;
+	int			m_link;
+	char		m_padding[4];
+};
+
+// clang-format on
+
+SIMD_FORCE_INLINE int btMultiBodyLinkCollider::calculateSerializeBufferSize() const
+{
+	return sizeof(btMultiBodyLinkColliderData);
+}
+
+SIMD_FORCE_INLINE const char* btMultiBodyLinkCollider::serialize(void* dataBuffer, class btSerializer* serializer) const
+{
+	btMultiBodyLinkColliderData* dataOut = (btMultiBodyLinkColliderData*)dataBuffer;
+	btCollisionObject::serialize(&dataOut->m_colObjData, serializer);
+
+	dataOut->m_link = this->m_link;
+	dataOut->m_multiBody = (btMultiBodyData*)serializer->getUniquePointer(m_multiBody);
+
+	// Fill padding with zeros to appease msan.
+	memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding));
+
+	return btMultiBodyLinkColliderDataName;
+}
 
+#endif  //BT_FEATHERSTONE_LINK_COLLIDER_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp
new file mode 100644
index 00000000000..f2186a93e9d
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp
@@ -0,0 +1,966 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
+
+#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+
+static bool interleaveContactAndFriction1 = false;
+
+struct btJointNode1
+{
+	int jointIndex;          // pointer to enclosing dxJoint object
+	int otherBodyIndex;      // *other* body this joint is connected to
+	int nextJointNodeIndex;  //-1 for null
+	int constraintRowIndex;
+};
+
+// Helper function to compute a delta velocity in the constraint space.
+static btScalar computeDeltaVelocityInConstraintSpace(
+	const btVector3& angularDeltaVelocity,
+	const btVector3& contactNormal,
+	btScalar invMass,
+	const btVector3& angularJacobian,
+	const btVector3& linearJacobian)
+{
+	return angularDeltaVelocity.dot(angularJacobian) + contactNormal.dot(linearJacobian) * invMass;
+}
+
+// Faster version of computeDeltaVelocityInConstraintSpace that can be used when contactNormal and linearJacobian are
+// identical.
+static btScalar computeDeltaVelocityInConstraintSpace(
+	const btVector3& angularDeltaVelocity,
+	btScalar invMass,
+	const btVector3& angularJacobian)
+{
+	return angularDeltaVelocity.dot(angularJacobian) + invMass;
+}
+
+// Helper function to compute a delta velocity in the constraint space.
+static btScalar computeDeltaVelocityInConstraintSpace(const btScalar* deltaVelocity, const btScalar* jacobian, int size)
+{
+	btScalar result = 0;
+	for (int i = 0; i < size; ++i)
+		result += deltaVelocity[i] * jacobian[i];
+
+	return result;
+}
+
+static btScalar computeConstraintMatrixDiagElementMultiBody(
+	const btAlignedObjectArray<btSolverBody>& solverBodyPool,
+	const btMultiBodyJacobianData& data,
+	const btMultiBodySolverConstraint& constraint)
+{
+	btScalar ret = 0;
+
+	const btMultiBody* multiBodyA = constraint.m_multiBodyA;
+	const btMultiBody* multiBodyB = constraint.m_multiBodyB;
+
+	if (multiBodyA)
+	{
+		const btScalar* jacA = &data.m_jacobians[constraint.m_jacAindex];
+		const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
+		const int ndofA = multiBodyA->getNumDofs() + 6;
+		ret += computeDeltaVelocityInConstraintSpace(deltaA, jacA, ndofA);
+	}
+	else
+	{
+		const int solverBodyIdA = constraint.m_solverBodyIdA;
+		btAssert(solverBodyIdA != -1);
+		const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
+		const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
+		ret += computeDeltaVelocityInConstraintSpace(
+			constraint.m_relpos1CrossNormal,
+			invMassA,
+			constraint.m_angularComponentA);
+	}
+
+	if (multiBodyB)
+	{
+		const btScalar* jacB = &data.m_jacobians[constraint.m_jacBindex];
+		const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
+		const int ndofB = multiBodyB->getNumDofs() + 6;
+		ret += computeDeltaVelocityInConstraintSpace(deltaB, jacB, ndofB);
+	}
+	else
+	{
+		const int solverBodyIdB = constraint.m_solverBodyIdB;
+		btAssert(solverBodyIdB != -1);
+		const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
+		const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
+		ret += computeDeltaVelocityInConstraintSpace(
+			constraint.m_relpos2CrossNormal,
+			invMassB,
+			constraint.m_angularComponentB);
+	}
+
+	return ret;
+}
+
+static btScalar computeConstraintMatrixOffDiagElementMultiBody(
+	const btAlignedObjectArray<btSolverBody>& solverBodyPool,
+	const btMultiBodyJacobianData& data,
+	const btMultiBodySolverConstraint& constraint,
+	const btMultiBodySolverConstraint& offDiagConstraint)
+{
+	btScalar offDiagA = btScalar(0);
+
+	const btMultiBody* multiBodyA = constraint.m_multiBodyA;
+	const btMultiBody* multiBodyB = constraint.m_multiBodyB;
+	const btMultiBody* offDiagMultiBodyA = offDiagConstraint.m_multiBodyA;
+	const btMultiBody* offDiagMultiBodyB = offDiagConstraint.m_multiBodyB;
+
+	// Assumed at least one system is multibody
+	btAssert(multiBodyA || multiBodyB);
+	btAssert(offDiagMultiBodyA || offDiagMultiBodyB);
+
+	if (offDiagMultiBodyA)
+	{
+		const btScalar* offDiagJacA = &data.m_jacobians[offDiagConstraint.m_jacAindex];
+
+		if (offDiagMultiBodyA == multiBodyA)
+		{
+			const int ndofA = multiBodyA->getNumDofs() + 6;
+			const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacA, ndofA);
+		}
+		else if (offDiagMultiBodyA == multiBodyB)
+		{
+			const int ndofB = multiBodyB->getNumDofs() + 6;
+			const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacA, ndofB);
+		}
+	}
+	else
+	{
+		const int solverBodyIdA = constraint.m_solverBodyIdA;
+		const int solverBodyIdB = constraint.m_solverBodyIdB;
+
+		const int offDiagSolverBodyIdA = offDiagConstraint.m_solverBodyIdA;
+		btAssert(offDiagSolverBodyIdA != -1);
+
+		if (offDiagSolverBodyIdA == solverBodyIdA)
+		{
+			btAssert(solverBodyIdA != -1);
+			const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
+			const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos1CrossNormal,
+				offDiagConstraint.m_contactNormal1,
+				invMassA, constraint.m_angularComponentA,
+				constraint.m_contactNormal1);
+		}
+		else if (offDiagSolverBodyIdA == solverBodyIdB)
+		{
+			btAssert(solverBodyIdB != -1);
+			const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
+			const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos1CrossNormal,
+				offDiagConstraint.m_contactNormal1,
+				invMassB,
+				constraint.m_angularComponentB,
+				constraint.m_contactNormal2);
+		}
+	}
+
+	if (offDiagMultiBodyB)
+	{
+		const btScalar* offDiagJacB = &data.m_jacobians[offDiagConstraint.m_jacBindex];
+
+		if (offDiagMultiBodyB == multiBodyA)
+		{
+			const int ndofA = multiBodyA->getNumDofs() + 6;
+			const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacB, ndofA);
+		}
+		else if (offDiagMultiBodyB == multiBodyB)
+		{
+			const int ndofB = multiBodyB->getNumDofs() + 6;
+			const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacB, ndofB);
+		}
+	}
+	else
+	{
+		const int solverBodyIdA = constraint.m_solverBodyIdA;
+		const int solverBodyIdB = constraint.m_solverBodyIdB;
+
+		const int offDiagSolverBodyIdB = offDiagConstraint.m_solverBodyIdB;
+		btAssert(offDiagSolverBodyIdB != -1);
+
+		if (offDiagSolverBodyIdB == solverBodyIdA)
+		{
+			btAssert(solverBodyIdA != -1);
+			const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
+			const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos2CrossNormal,
+				offDiagConstraint.m_contactNormal2,
+				invMassA, constraint.m_angularComponentA,
+				constraint.m_contactNormal1);
+		}
+		else if (offDiagSolverBodyIdB == solverBodyIdB)
+		{
+			btAssert(solverBodyIdB != -1);
+			const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
+			const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos2CrossNormal,
+				offDiagConstraint.m_contactNormal2,
+				invMassB, constraint.m_angularComponentB,
+				constraint.m_contactNormal2);
+		}
+	}
+
+	return offDiagA;
+}
+
+void btMultiBodyMLCPConstraintSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
+{
+	createMLCPFastRigidBody(infoGlobal);
+	createMLCPFastMultiBody(infoGlobal);
+}
+
+void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal)
+{
+	int numContactRows = interleaveContactAndFriction1 ? 3 : 1;
+
+	int numConstraintRows = m_allConstraintPtrArray.size();
+
+	if (numConstraintRows == 0)
+		return;
+
+	int n = numConstraintRows;
+	{
+		BT_PROFILE("init b (rhs)");
+		m_b.resize(numConstraintRows);
+		m_bSplit.resize(numConstraintRows);
+		m_b.setZero();
+		m_bSplit.setZero();
+		for (int i = 0; i < numConstraintRows; i++)
+		{
+			btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
+			if (!btFuzzyZero(jacDiag))
+			{
+				btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
+				btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
+				m_b[i] = rhs / jacDiag;
+				m_bSplit[i] = rhsPenetration / jacDiag;
+			}
+		}
+	}
+
+	//	btScalar* w = 0;
+	//	int nub = 0;
+
+	m_lo.resize(numConstraintRows);
+	m_hi.resize(numConstraintRows);
+
+	{
+		BT_PROFILE("init lo/ho");
+
+		for (int i = 0; i < numConstraintRows; i++)
+		{
+			if (0)  //m_limitDependencies[i]>=0)
+			{
+				m_lo[i] = -BT_INFINITY;
+				m_hi[i] = BT_INFINITY;
+			}
+			else
+			{
+				m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
+				m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
+			}
+		}
+	}
+
+	//
+	int m = m_allConstraintPtrArray.size();
+
+	int numBodies = m_tmpSolverBodyPool.size();
+	btAlignedObjectArray<int> bodyJointNodeArray;
+	{
+		BT_PROFILE("bodyJointNodeArray.resize");
+		bodyJointNodeArray.resize(numBodies, -1);
+	}
+	btAlignedObjectArray<btJointNode1> jointNodeArray;
+	{
+		BT_PROFILE("jointNodeArray.reserve");
+		jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
+	}
+
+	btMatrixXu& J3 = m_scratchJ3;
+	{
+		BT_PROFILE("J3.resize");
+		J3.resize(2 * m, 8);
+	}
+	btMatrixXu& JinvM3 = m_scratchJInvM3;
+	{
+		BT_PROFILE("JinvM3.resize/setZero");
+
+		JinvM3.resize(2 * m, 8);
+		JinvM3.setZero();
+		J3.setZero();
+	}
+	int cur = 0;
+	int rowOffset = 0;
+	btAlignedObjectArray<int>& ofs = m_scratchOfs;
+	{
+		BT_PROFILE("ofs resize");
+		ofs.resize(0);
+		ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
+	}
+	{
+		BT_PROFILE("Compute J and JinvM");
+		int c = 0;
+
+		int numRows = 0;
+
+		for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
+		{
+			ofs[c] = rowOffset;
+			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
+			btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+			btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+			numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
+			if (orgBodyA)
+			{
+				{
+					int slotA = -1;
+					//find free jointNode slot for sbA
+					slotA = jointNodeArray.size();
+					jointNodeArray.expand();  //NonInitializing();
+					int prevSlot = bodyJointNodeArray[sbA];
+					bodyJointNodeArray[sbA] = slotA;
+					jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
+					jointNodeArray[slotA].jointIndex = c;
+					jointNodeArray[slotA].constraintRowIndex = i;
+					jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
+				}
+				for (int row = 0; row < numRows; row++, cur++)
+				{
+					btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass();
+					btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
+
+					for (int r = 0; r < 3; r++)
+					{
+						J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]);
+						J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]);
+						JinvM3.setElem(cur, r, normalInvMass[r]);
+						JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
+					}
+					J3.setElem(cur, 3, 0);
+					JinvM3.setElem(cur, 3, 0);
+					J3.setElem(cur, 7, 0);
+					JinvM3.setElem(cur, 7, 0);
+				}
+			}
+			else
+			{
+				cur += numRows;
+			}
+			if (orgBodyB)
+			{
+				{
+					int slotB = -1;
+					//find free jointNode slot for sbA
+					slotB = jointNodeArray.size();
+					jointNodeArray.expand();  //NonInitializing();
+					int prevSlot = bodyJointNodeArray[sbB];
+					bodyJointNodeArray[sbB] = slotB;
+					jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
+					jointNodeArray[slotB].jointIndex = c;
+					jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
+					jointNodeArray[slotB].constraintRowIndex = i;
+				}
+
+				for (int row = 0; row < numRows; row++, cur++)
+				{
+					btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass();
+					btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
+
+					for (int r = 0; r < 3; r++)
+					{
+						J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]);
+						J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]);
+						JinvM3.setElem(cur, r, normalInvMassB[r]);
+						JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
+					}
+					J3.setElem(cur, 3, 0);
+					JinvM3.setElem(cur, 3, 0);
+					J3.setElem(cur, 7, 0);
+					JinvM3.setElem(cur, 7, 0);
+				}
+			}
+			else
+			{
+				cur += numRows;
+			}
+			rowOffset += numRows;
+		}
+	}
+
+	//compute JinvM = J*invM.
+	const btScalar* JinvM = JinvM3.getBufferPointer();
+
+	const btScalar* Jptr = J3.getBufferPointer();
+	{
+		BT_PROFILE("m_A.resize");
+		m_A.resize(n, n);
+	}
+
+	{
+		BT_PROFILE("m_A.setZero");
+		m_A.setZero();
+	}
+	int c = 0;
+	{
+		int numRows = 0;
+		BT_PROFILE("Compute A");
+		for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
+		{
+			int row__ = ofs[c];
+			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
+			//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+			//	btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+			numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
+
+			const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
+
+			{
+				int startJointNodeA = bodyJointNodeArray[sbA];
+				while (startJointNodeA >= 0)
+				{
+					int j0 = jointNodeArray[startJointNodeA].jointIndex;
+					int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
+					if (j0 < c)
+					{
+						int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
+						size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0;
+						//printf("%d joint i %d and j0: %d: ",count++,i,j0);
+						m_A.multiplyAdd2_p8r(JinvMrow,
+											 Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
+					}
+					startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
+				}
+			}
+
+			{
+				int startJointNodeB = bodyJointNodeArray[sbB];
+				while (startJointNodeB >= 0)
+				{
+					int j1 = jointNodeArray[startJointNodeB].jointIndex;
+					int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
+
+					if (j1 < c)
+					{
+						int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
+						size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0;
+						m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows,
+											 Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
+					}
+					startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
+				}
+			}
+		}
+
+		{
+			BT_PROFILE("compute diagonal");
+			// compute diagonal blocks of m_A
+
+			int row__ = 0;
+			int numJointRows = m_allConstraintPtrArray.size();
+
+			int jj = 0;
+			for (; row__ < numJointRows;)
+			{
+				//int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
+				int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
+				//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+				btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+				const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
+
+				const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
+				const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__;
+				m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__);
+				if (orgBodyB)
+				{
+					m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
+				}
+				row__ += infom;
+				jj++;
+			}
+		}
+	}
+
+	if (1)
+	{
+		// add cfm to the diagonal of m_A
+		for (int i = 0; i < m_A.rows(); ++i)
+		{
+			m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
+		}
+	}
+
+	///fill the upper triangle of the matrix, to make it symmetric
+	{
+		BT_PROFILE("fill the upper triangle ");
+		m_A.copyLowerToUpperTriangle();
+	}
+
+	{
+		BT_PROFILE("resize/init x");
+		m_x.resize(numConstraintRows);
+		m_xSplit.resize(numConstraintRows);
+
+		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+		{
+			for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
+			{
+				const btSolverConstraint& c = *m_allConstraintPtrArray[i];
+				m_x[i] = c.m_appliedImpulse;
+				m_xSplit[i] = c.m_appliedPushImpulse;
+			}
+		}
+		else
+		{
+			m_x.setZero();
+			m_xSplit.setZero();
+		}
+	}
+}
+
+void btMultiBodyMLCPConstraintSolver::createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal)
+{
+	const int multiBodyNumConstraints = m_multiBodyAllConstraintPtrArray.size();
+
+	if (multiBodyNumConstraints == 0)
+		return;
+
+	// 1. Compute b
+	{
+		BT_PROFILE("init b (rhs)");
+
+		m_multiBodyB.resize(multiBodyNumConstraints);
+		m_multiBodyB.setZero();
+
+		for (int i = 0; i < multiBodyNumConstraints; ++i)
+		{
+			const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+			const btScalar jacDiag = constraint.m_jacDiagABInv;
+
+			if (!btFuzzyZero(jacDiag))
+			{
+				// Note that rhsPenetration is currently always zero because the split impulse hasn't been implemented for multibody yet.
+				const btScalar rhs = constraint.m_rhs;
+				m_multiBodyB[i] = rhs / jacDiag;
+			}
+		}
+	}
+
+	// 2. Compute lo and hi
+	{
+		BT_PROFILE("init lo/ho");
+
+		m_multiBodyLo.resize(multiBodyNumConstraints);
+		m_multiBodyHi.resize(multiBodyNumConstraints);
+
+		for (int i = 0; i < multiBodyNumConstraints; ++i)
+		{
+			const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+			m_multiBodyLo[i] = constraint.m_lowerLimit;
+			m_multiBodyHi[i] = constraint.m_upperLimit;
+		}
+	}
+
+	// 3. Construct A matrix by using the impulse testing
+	{
+		BT_PROFILE("Compute A");
+
+		{
+			BT_PROFILE("m_A.resize");
+			m_multiBodyA.resize(multiBodyNumConstraints, multiBodyNumConstraints);
+		}
+
+		for (int i = 0; i < multiBodyNumConstraints; ++i)
+		{
+			// Compute the diagonal of A, which is A(i, i)
+			const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+			const btScalar diagA = computeConstraintMatrixDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint);
+			m_multiBodyA.setElem(i, i, diagA);
+
+			// Computes the off-diagonals of A:
+			//   a. The rest of i-th row of A, from A(i, i+1) to A(i, n)
+			//   b. The rest of i-th column of A, from A(i+1, i) to A(n, i)
+			for (int j = i + 1; j < multiBodyNumConstraints; ++j)
+			{
+				const btMultiBodySolverConstraint& offDiagConstraint = *m_multiBodyAllConstraintPtrArray[j];
+				const btScalar offDiagA = computeConstraintMatrixOffDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint, offDiagConstraint);
+
+				// Set the off-diagonal values of A. Note that A is symmetric.
+				m_multiBodyA.setElem(i, j, offDiagA);
+				m_multiBodyA.setElem(j, i, offDiagA);
+			}
+		}
+	}
+
+	// Add CFM to the diagonal of m_A
+	for (int i = 0; i < m_multiBodyA.rows(); ++i)
+	{
+		m_multiBodyA.setElem(i, i, m_multiBodyA(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
+	}
+
+	// 4. Initialize x
+	{
+		BT_PROFILE("resize/init x");
+
+		m_multiBodyX.resize(multiBodyNumConstraints);
+
+		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+		{
+			for (int i = 0; i < multiBodyNumConstraints; ++i)
+			{
+				const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+				m_multiBodyX[i] = constraint.m_appliedImpulse;
+			}
+		}
+		else
+		{
+			m_multiBodyX.setZero();
+		}
+	}
+}
+
+bool btMultiBodyMLCPConstraintSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
+{
+	bool result = true;
+
+	if (m_A.rows() != 0)
+	{
+		// If using split impulse, we solve 2 separate (M)LCPs
+		if (infoGlobal.m_splitImpulse)
+		{
+			const btMatrixXu Acopy = m_A;
+			const btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
+			// TODO(JS): Do we really need these copies when solveMLCP takes them as const?
+
+			result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
+			if (result)
+				result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations);
+		}
+		else
+		{
+			result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
+		}
+	}
+
+	if (!result)
+		return false;
+
+	if (m_multiBodyA.rows() != 0)
+	{
+		result = m_solver->solveMLCP(m_multiBodyA, m_multiBodyB, m_multiBodyX, m_multiBodyLo, m_multiBodyHi, m_multiBodyLimitDependencies, infoGlobal.m_numIterations);
+	}
+
+	return result;
+}
+
+btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup(
+	btCollisionObject** bodies,
+	int numBodies,
+	btPersistentManifold** manifoldPtr,
+	int numManifolds,
+	btTypedConstraint** constraints,
+	int numConstraints,
+	const btContactSolverInfo& infoGlobal,
+	btIDebugDraw* debugDrawer)
+{
+	// 1. Setup for rigid-bodies
+	btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(
+		bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+
+	// 2. Setup for multi-bodies
+	//   a. Collect all different kinds of constraint as pointers into one array, m_allConstraintPtrArray
+	//   b. Set the index array for frictional contact constraints, m_limitDependencies
+	{
+		BT_PROFILE("gather constraint data");
+
+		int dindex = 0;
+
+		const int numRigidBodyConstraints = m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size();
+		const int numMultiBodyConstraints = m_multiBodyNonContactConstraints.size() + m_multiBodyNormalContactConstraints.size() + m_multiBodyFrictionContactConstraints.size();
+
+		m_allConstraintPtrArray.resize(0);
+		m_multiBodyAllConstraintPtrArray.resize(0);
+
+		// i. Setup for rigid bodies
+
+		m_limitDependencies.resize(numRigidBodyConstraints);
+
+		for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); ++i)
+		{
+			m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
+			m_limitDependencies[dindex++] = -1;
+		}
+
+		int firstContactConstraintOffset = dindex;
+
+		// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
+		if (interleaveContactAndFriction1)
+		{
+			for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
+			{
+				const int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
+
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]);
+				int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact));
+				m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
+				if (numFrictionPerContact == 2)
+				{
+					m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]);
+					m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
+				}
+			}
+		}
+		else
+		{
+			for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
+			{
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+			}
+			for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++)
+			{
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
+				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
+			}
+		}
+
+		if (!m_allConstraintPtrArray.size())
+		{
+			m_A.resize(0, 0);
+			m_b.resize(0);
+			m_x.resize(0);
+			m_lo.resize(0);
+			m_hi.resize(0);
+		}
+
+		// ii. Setup for multibodies
+
+		dindex = 0;
+
+		m_multiBodyLimitDependencies.resize(numMultiBodyConstraints);
+
+		for (int i = 0; i < m_multiBodyNonContactConstraints.size(); ++i)
+		{
+			m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNonContactConstraints[i]);
+			m_multiBodyLimitDependencies[dindex++] = -1;
+		}
+
+		firstContactConstraintOffset = dindex;
+
+		// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
+		if (interleaveContactAndFriction1)
+		{
+			for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
+			{
+				const int numtiBodyNumFrictionPerContact = m_multiBodyNormalContactConstraints.size() == m_multiBodyFrictionContactConstraints.size() ? 1 : 2;
+
+				m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
+				m_multiBodyLimitDependencies[dindex++] = -1;
+
+				btMultiBodySolverConstraint& frictionContactConstraint1 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact];
+				m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint1);
+
+				const int findex = (frictionContactConstraint1.m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset;
+
+				m_multiBodyLimitDependencies[dindex++] = findex;
+
+				if (numtiBodyNumFrictionPerContact == 2)
+				{
+					btMultiBodySolverConstraint& frictionContactConstraint2 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact + 1];
+					m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint2);
+
+					m_multiBodyLimitDependencies[dindex++] = findex;
+				}
+			}
+		}
+		else
+		{
+			for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
+			{
+				m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
+				m_multiBodyLimitDependencies[dindex++] = -1;
+			}
+			for (int i = 0; i < m_multiBodyFrictionContactConstraints.size(); ++i)
+			{
+				m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyFrictionContactConstraints[i]);
+				m_multiBodyLimitDependencies[dindex++] = m_multiBodyFrictionContactConstraints[i].m_frictionIndex + firstContactConstraintOffset;
+			}
+		}
+
+		if (!m_multiBodyAllConstraintPtrArray.size())
+		{
+			m_multiBodyA.resize(0, 0);
+			m_multiBodyB.resize(0);
+			m_multiBodyX.resize(0);
+			m_multiBodyLo.resize(0);
+			m_multiBodyHi.resize(0);
+		}
+	}
+
+	// Construct MLCP terms
+	{
+		BT_PROFILE("createMLCPFast");
+		createMLCPFast(infoGlobal);
+	}
+
+	return btScalar(0);
+}
+
+btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
+	bool result = true;
+	{
+		BT_PROFILE("solveMLCP");
+		result = solveMLCP(infoGlobal);
+	}
+
+	// Fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations if the solution isn't valid.
+	if (!result)
+	{
+		m_fallback++;
+		return btMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+	}
+
+	{
+		BT_PROFILE("process MLCP results");
+
+		for (int i = 0; i < m_allConstraintPtrArray.size(); ++i)
+		{
+			const btSolverConstraint& c = *m_allConstraintPtrArray[i];
+
+			const btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse;
+			c.m_appliedImpulse = m_x[i];
+
+			int sbA = c.m_solverBodyIdA;
+			int sbB = c.m_solverBodyIdB;
+
+			btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
+			btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
+
+			solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+			solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+
+			if (infoGlobal.m_splitImpulse)
+			{
+				const btScalar deltaPushImpulse = m_xSplit[i] - c.m_appliedPushImpulse;
+				solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaPushImpulse);
+				solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaPushImpulse);
+				c.m_appliedPushImpulse = m_xSplit[i];
+			}
+		}
+
+		for (int i = 0; i < m_multiBodyAllConstraintPtrArray.size(); ++i)
+		{
+			btMultiBodySolverConstraint& c = *m_multiBodyAllConstraintPtrArray[i];
+
+			const btScalar deltaImpulse = m_multiBodyX[i] - c.m_appliedImpulse;
+			c.m_appliedImpulse = m_multiBodyX[i];
+
+			btMultiBody* multiBodyA = c.m_multiBodyA;
+			if (multiBodyA)
+			{
+				const int ndofA = multiBodyA->getNumDofs() + 6;
+				applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+				//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+				//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+				multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
+#endif  // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+			}
+			else
+			{
+				const int sbA = c.m_solverBodyIdA;
+				btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
+				solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+			}
+
+			btMultiBody* multiBodyB = c.m_multiBodyB;
+			if (multiBodyB)
+			{
+				const int ndofB = multiBodyB->getNumDofs() + 6;
+				applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+				//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+				//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+				multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
+#endif  // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+			}
+			else
+			{
+				const int sbB = c.m_solverBodyIdB;
+				btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
+				solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+			}
+		}
+	}
+
+	return btScalar(0);
+}
+
+btMultiBodyMLCPConstraintSolver::btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver)
+	: m_solver(solver), m_fallback(0)
+{
+	// Do nothing
+}
+
+btMultiBodyMLCPConstraintSolver::~btMultiBodyMLCPConstraintSolver()
+{
+	// Do nothing
+}
+
+void btMultiBodyMLCPConstraintSolver::setMLCPSolver(btMLCPSolverInterface* solver)
+{
+	m_solver = solver;
+}
+
+int btMultiBodyMLCPConstraintSolver::getNumFallbacks() const
+{
+	return m_fallback;
+}
+
+void btMultiBodyMLCPConstraintSolver::setNumFallbacks(int num)
+{
+	m_fallback = num;
+}
+
+btConstraintSolverType btMultiBodyMLCPConstraintSolver::getSolverType() const
+{
+	return BT_MLCP_SOLVER;
+}
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h
new file mode 100644
index 00000000000..77fdb86bb95
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h
@@ -0,0 +1,187 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
+#define BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
+
+#include "LinearMath/btMatrixX.h"
+#include "LinearMath/btThreads.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+
+class btMLCPSolverInterface;
+class btMultiBody;
+
+class btMultiBodyMLCPConstraintSolver : public btMultiBodyConstraintSolver
+{
+protected:
+	/// \name MLCP Formulation for Rigid Bodies
+	/// \{
+
+	/// A matrix in the MLCP formulation
+	btMatrixXu m_A;
+
+	/// b vector in the MLCP formulation.
+	btVectorXu m_b;
+
+	/// Constraint impulse, which is an output of MLCP solving.
+	btVectorXu m_x;
+
+	/// Lower bound of constraint impulse, \c m_x.
+	btVectorXu m_lo;
+
+	/// Upper bound of constraint impulse, \c m_x.
+	btVectorXu m_hi;
+
+	/// \}
+
+	/// \name Cache Variables for Split Impulse for Rigid Bodies
+	/// When using 'split impulse' we solve two separate (M)LCPs
+	/// \{
+
+	/// Split impulse Cache vector corresponding to \c m_b.
+	btVectorXu m_bSplit;
+
+	/// Split impulse cache vector corresponding to \c m_x.
+	btVectorXu m_xSplit;
+
+	/// \}
+
+	/// \name MLCP Formulation for Multibodies
+	/// \{
+
+	/// A matrix in the MLCP formulation
+	btMatrixXu m_multiBodyA;
+
+	/// b vector in the MLCP formulation.
+	btVectorXu m_multiBodyB;
+
+	/// Constraint impulse, which is an output of MLCP solving.
+	btVectorXu m_multiBodyX;
+
+	/// Lower bound of constraint impulse, \c m_x.
+	btVectorXu m_multiBodyLo;
+
+	/// Upper bound of constraint impulse, \c m_x.
+	btVectorXu m_multiBodyHi;
+
+	/// \}
+
+	/// Indices of normal contact constraint associated with frictional contact constraint for rigid bodies.
+	///
+	/// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate
+	/// normal contact impulse. For example, i-th element represents the index of a normal constraint that is
+	/// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint.
+	/// Otherwise, -1.
+	btAlignedObjectArray<int> m_limitDependencies;
+
+	/// Indices of normal contact constraint associated with frictional contact constraint for multibodies.
+	///
+	/// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate
+	/// normal contact impulse. For example, i-th element represents the index of a normal constraint that is
+	/// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint.
+	/// Otherwise, -1.
+	btAlignedObjectArray<int> m_multiBodyLimitDependencies;
+
+	/// Array of all the rigid body constraints
+	btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray;
+
+	/// Array of all the multibody constraints
+	btAlignedObjectArray<btMultiBodySolverConstraint*> m_multiBodyAllConstraintPtrArray;
+
+	/// MLCP solver
+	btMLCPSolverInterface* m_solver;
+
+	/// Count of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP solver fails.
+	int m_fallback;
+
+	/// \name MLCP Scratch Variables
+	/// The following scratch variables are not stateful -- contents are cleared prior to each use.
+	/// They are only cached here to avoid extra memory allocations and deallocations and to ensure
+	/// that multiple instances of the solver can be run in parallel.
+	///
+	/// \{
+
+	/// Cache variable for constraint Jacobian matrix.
+	btMatrixXu m_scratchJ3;
+
+	/// Cache variable for constraint Jacobian times inverse mass matrix.
+	btMatrixXu m_scratchJInvM3;
+
+	/// Cache variable for offsets.
+	btAlignedObjectArray<int> m_scratchOfs;
+
+	/// \}
+
+	/// Constructs MLCP terms, which are \c m_A, \c m_b, \c m_lo, and \c m_hi.
+	virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
+
+	/// Constructs MLCP terms for constraints of two rigid bodies
+	void createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal);
+
+	/// Constructs MLCP terms for constraints of two multi-bodies or one rigid body and one multibody
+	void createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal);
+
+	/// Solves MLCP and returns the success
+	virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
+
+	// Documentation inherited
+	btScalar solveGroupCacheFriendlySetup(
+		btCollisionObject** bodies,
+		int numBodies,
+		btPersistentManifold** manifoldPtr,
+		int numManifolds,
+		btTypedConstraint** constraints,
+		int numConstraints,
+		const btContactSolverInfo& infoGlobal,
+		btIDebugDraw* debugDrawer) BT_OVERRIDE;
+
+	// Documentation inherited
+	btScalar solveGroupCacheFriendlyIterations(
+		btCollisionObject** bodies,
+		int numBodies,
+		btPersistentManifold** manifoldPtr,
+		int numManifolds,
+		btTypedConstraint** constraints,
+		int numConstraints,
+		const btContactSolverInfo& infoGlobal,
+		btIDebugDraw* debugDrawer) ;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR()
+
+	/// Constructor
+	///
+	/// \param[in] solver MLCP solver. Assumed it's not null.
+	/// \param[in] maxLCPSize Maximum size of LCP to solve using MLCP solver. If the MLCP size exceeds this number, sequaltial impulse method will be used.
+	explicit btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver);
+
+	/// Destructor
+	virtual ~btMultiBodyMLCPConstraintSolver();
+
+	/// Sets MLCP solver. Assumed it's not null.
+	void setMLCPSolver(btMLCPSolverInterface* solver);
+
+	/// Returns the number of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP
+	/// solver fails.
+	int getNumFallbacks() const;
+
+	/// Sets the number of fallbacks. This function may be used to reset the number to zero.
+	void setNumFallbacks(int num);
+
+	/// Returns the constraint solver type.
+	virtual btConstraintSolverType getSolverType() const;
+};
+
+#endif  // BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
index 12b21f74603..f51e69deb1c 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
@@ -21,29 +21,29 @@ subject to the following restrictions:
 #include "LinearMath/btIDebugDraw.h"
 
 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
-	#define BTMBP2PCONSTRAINT_DIM 3
+#define BTMBP2PCONSTRAINT_DIM 3
 #else
-	#define BTMBP2PCONSTRAINT_DIM 6
+#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)
+	: btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(bodyB),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB)
 {
-    m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
+	m_data.resize(BTMBP2PCONSTRAINT_DIM);  //at least store the applied impulses
 }
 
 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
-	:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false),
-	m_rigidBodyA(0),
-	m_rigidBodyB(0),
-	m_pivotInA(pivotInA),
-	m_pivotInB(pivotInB)
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(0),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB)
 {
-    m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
+	m_data.resize(BTMBP2PCONSTRAINT_DIM);  //at least store the applied impulses
 }
 
 void btMultiBodyPoint2Point::finalizeMultiDof()
@@ -56,7 +56,6 @@ btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
 {
 }
 
-
 int btMultiBodyPoint2Point::getIslandIdA() const
 {
 	if (m_rigidBodyA)
@@ -64,13 +63,16 @@ int btMultiBodyPoint2Point::getIslandIdA() const
 
 	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -82,62 +84,58 @@ int btMultiBodyPoint2Point::getIslandIdB() const
 		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++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
 
-
-
 void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal)
+												  btMultiBodyJacobianData& data,
+												  const btContactSolverInfo& infoGlobal)
 {
-
-//	int i=1;
-int numDim = BTMBP2PCONSTRAINT_DIM;
-	for (int i=0;i<numDim;i++)
+	//	int i=1;
+	int numDim = BTMBP2PCONSTRAINT_DIM;
+	for (int i = 0; i < numDim; i++)
 	{
-
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
-        //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
-	constraintRow.m_orgConstraint = this;
-	constraintRow.m_orgDofIndex = i;
-        constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
-        constraintRow.m_contactNormal1.setValue(0,0,0);
-        constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
-        constraintRow.m_contactNormal2.setValue(0,0,0);
-        constraintRow.m_angularComponentA.setValue(0,0,0);
-        constraintRow.m_angularComponentB.setValue(0,0,0);
+		//memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = i;
+		constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal1.setValue(0, 0, 0);
+		constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal2.setValue(0, 0, 0);
+		constraintRow.m_angularComponentA.setValue(0, 0, 0);
+		constraintRow.m_angularComponentB.setValue(0, 0, 0);
 
 		constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
 		constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
 
-		btVector3 contactNormalOnB(0,0,0);
+		btVector3 contactNormalOnB(0, 0, 0);
 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
 		contactNormalOnB[i] = -1;
 #else
-		contactNormalOnB[i%3] = -1;
+		contactNormalOnB[i % 3] = -1;
 #endif
 
-
-		 // Convert local points back to world
+		// 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
+			pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		}
+		else
 		{
 			if (m_bodyA)
 				pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
@@ -146,44 +144,41 @@ int numDim = BTMBP2PCONSTRAINT_DIM;
 		if (m_rigidBodyB)
 		{
 			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
-			pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
-		} else
+			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;
+		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
+		fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0, 0, 0),
+								contactNormalOnB, pivotAworld, pivotBworld,  //sucks but let it be this way "for the time being"
+								posError,
+								infoGlobal,
+								-m_maxAppliedImpulse, m_maxAppliedImpulse);
+		//@todo: support the case of btMultiBody versus btRigidBody,
+		//see btPoint2PointConstraint::getInfo2NonVirtual
 #else
 		const btVector3 dummy(0, 0, 0);
 
 		btAssert(m_bodyA->isMultiDof());
 
 		btScalar* jac1 = jacobianA(i);
-		const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy;
-		const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy;
+		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
-													);
+								dummy, dummy, dummy,  //sucks but let it be this way "for the time being"
+								posError,
+								infoGlobal,
+								-m_maxAppliedImpulse, m_maxAppliedImpulse);
 #endif
 	}
 }
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h
index b2e219ac159..ef03a557ec5 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h
@@ -22,20 +22,20 @@ subject to the following restrictions:
 
 //#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
 
-class btMultiBodyPoint2Point : public btMultiBodyConstraint
+ATTRIBUTE_ALIGNED16(class)
+btMultiBodyPoint2Point : public btMultiBodyConstraint
 {
 protected:
-
-	btRigidBody*	m_rigidBodyA;
-	btRigidBody*	m_rigidBodyB;
-	btVector3		m_pivotInA;
-	btVector3		m_pivotInB;
-
+	btRigidBody* m_rigidBodyA;
+	btRigidBody* m_rigidBodyB;
+	btVector3 m_pivotInA;
+	btVector3 m_pivotInB;
 
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB);
-	btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB);
+	btMultiBodyPoint2Point(btMultiBody * body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB);
+	btMultiBodyPoint2Point(btMultiBody * bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB);
 
 	virtual ~btMultiBodyPoint2Point();
 
@@ -44,22 +44,21 @@ public:
 	virtual int getIslandIdA() const;
 	virtual int getIslandIdB() const;
 
-	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal);
+	virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows,
+									  btMultiBodyJacobianData & data,
+									  const btContactSolverInfo& infoGlobal);
 
 	const btVector3& getPivotInB() const
 	{
 		return m_pivotInB;
 	}
 
-	void setPivotInB(const btVector3& pivotInB)
+	virtual void setPivotInB(const btVector3& pivotInB)
 	{
 		m_pivotInB = pivotInB;
 	}
 
-	virtual void debugDraw(class btIDebugDraw* drawer);
-
+	virtual void debugDraw(class btIDebugDraw * drawer);
 };
 
-#endif //BT_MULTIBODY_POINT2POINT_H
+#endif  //BT_MULTIBODY_POINT2POINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
new file mode 100644
index 00000000000..48ec1d5af2b
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
@@ -0,0 +1,234 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodySliderConstraint.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#define BTMBSLIDERCONSTRAINT_DIM 5
+#define EPSILON 0.000001
+
+btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
+	: btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(bodyB),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB),
+	  m_jointAxis(jointAxis)
+{
+	m_data.resize(BTMBSLIDERCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(0),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB),
+	  m_jointAxis(jointAxis)
+{
+	m_data.resize(BTMBSLIDERCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+void btMultiBodySliderConstraint::finalizeMultiDof()
+{
+	//not implemented yet
+	btAssert(0);
+}
+
+btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
+{
+}
+
+int btMultiBodySliderConstraint::getIslandIdA() const
+{
+	if (m_rigidBodyA)
+		return m_rigidBodyA->getIslandTag();
+
+	if (m_bodyA)
+	{
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodySliderConstraint::getIslandIdB() const
+{
+	if (m_rigidBodyB)
+		return m_rigidBodyB->getIslandTag();
+	if (m_bodyB)
+	{
+		if (m_linkB < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
+{
+	// Convert local points back to world
+	btVector3 pivotAworld = m_pivotInA;
+	btMatrix3x3 frameAworld = m_frameInA;
+	btVector3 jointAxis = m_jointAxis;
+	if (m_rigidBodyA)
+	{
+		pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		frameAworld = m_frameInA.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
+		jointAxis = quatRotate(m_rigidBodyA->getOrientation(), m_jointAxis);
+	}
+	else if (m_bodyA)
+	{
+		pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+		frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
+		jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);
+	}
+	btVector3 pivotBworld = m_pivotInB;
+	btMatrix3x3 frameBworld = m_frameInB;
+	if (m_rigidBodyB)
+	{
+		pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		frameBworld = m_frameInB.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
+	}
+	else if (m_bodyB)
+	{
+		pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+		frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
+	}
+
+	btVector3 constraintAxis[2];
+	for (int i = 0; i < 3; ++i)
+	{
+		constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
+		if (constraintAxis[0].safeNorm() > EPSILON)
+		{
+			constraintAxis[0] = constraintAxis[0].normalized();
+			constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
+			constraintAxis[1] = constraintAxis[1].normalized();
+			break;
+		}
+	}
+
+	btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
+	btVector3 angleDiff;
+	btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
+
+	int numDim = BTMBSLIDERCONSTRAINT_DIM;
+	for (int i = 0; i < numDim; i++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = i;
+		constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal1.setValue(0, 0, 0);
+		constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal2.setValue(0, 0, 0);
+		constraintRow.m_angularComponentA.setValue(0, 0, 0);
+		constraintRow.m_angularComponentB.setValue(0, 0, 0);
+
+		constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
+		constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
+
+		if (m_rigidBodyA)
+		{
+			constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
+		}
+		if (m_rigidBodyB)
+		{
+			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
+		}
+
+		btVector3 constraintNormalLin(0, 0, 0);
+		btVector3 constraintNormalAng(0, 0, 0);
+		btScalar posError = 0.0;
+		if (i < 2)
+		{
+			constraintNormalLin = constraintAxis[i];
+			posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse);
+		}
+		else
+		{  //i>=2
+			constraintNormalAng = frameAworld.getColumn(i % 3);
+			posError = angleDiff[i % 3];
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
+		}
+	}
+}
+
+void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer)
+{
+	btTransform tr;
+	tr.setIdentity();
+
+	if (m_rigidBodyA)
+	{
+		btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyA)
+	{
+		btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+		tr.setOrigin(pivotAworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_rigidBodyB)
+	{
+		// that ideally should draw the same frame
+		btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyB)
+	{
+		btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+		tr.setOrigin(pivotBworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+}
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h
new file mode 100644
index 00000000000..b192b6f8f37
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h
@@ -0,0 +1,102 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_SLIDER_CONSTRAINT_H
+#define BT_MULTIBODY_SLIDER_CONSTRAINT_H
+
+#include "btMultiBodyConstraint.h"
+
+class btMultiBodySliderConstraint : public btMultiBodyConstraint
+{
+protected:
+	btRigidBody* m_rigidBodyA;
+	btRigidBody* m_rigidBodyB;
+	btVector3 m_pivotInA;
+	btVector3 m_pivotInB;
+	btMatrix3x3 m_frameInA;
+	btMatrix3x3 m_frameInB;
+	btVector3 m_jointAxis;
+
+public:
+	btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);
+	btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);
+
+	virtual ~btMultiBodySliderConstraint();
+
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	const btVector3& getPivotInA() const
+	{
+		return m_pivotInA;
+	}
+
+	void setPivotInA(const btVector3& pivotInA)
+	{
+		m_pivotInA = pivotInA;
+	}
+
+	const btVector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+	virtual void setPivotInB(const btVector3& pivotInB)
+	{
+		m_pivotInB = pivotInB;
+	}
+
+	const btMatrix3x3& getFrameInA() const
+	{
+		return m_frameInA;
+	}
+
+	void setFrameInA(const btMatrix3x3& frameInA)
+	{
+		m_frameInA = frameInA;
+	}
+
+	const btMatrix3x3& getFrameInB() const
+	{
+		return m_frameInB;
+	}
+
+	virtual void setFrameInB(const btMatrix3x3& frameInB)
+	{
+		m_frameInB = frameInB;
+	}
+
+	const btVector3& getJointAxis() const
+	{
+		return m_jointAxis;
+	}
+
+	void setJointAxis(const btVector3& jointAxis)
+	{
+		m_jointAxis = jointAxis;
+	}
+
+	virtual void debugDraw(class btIDebugDraw* drawer);
+};
+
+#endif  //BT_MULTIBODY_SLIDER_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h
index 6fa1550e9e6..deed3e2a123 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h
@@ -25,66 +25,66 @@ class btMultiBodyConstraint;
 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 
 ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
-ATTRIBUTE_ALIGNED16 (struct)	btMultiBodySolverConstraint
+ATTRIBUTE_ALIGNED16(struct)
+btMultiBodySolverConstraint
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_orgConstraint(0), m_orgDofIndex(-1)
-	{}
-
-	int				m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
-	int				m_jacAindex;
-	int				m_deltaVelBindex;
-	int				m_jacBindex;
-
-	btVector3		m_relpos1CrossNormal;
-	btVector3		m_contactNormal1;	
-	btVector3		m_relpos2CrossNormal;
-	btVector3		m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
-	
-
-	btVector3		m_angularComponentA;
-	btVector3		m_angularComponentB;
-	
-	mutable btSimdScalar	m_appliedPushImpulse;
-	mutable btSimdScalar	m_appliedImpulse;
-
-	btScalar	m_friction;
-	btScalar	m_jacDiagABInv;
-	btScalar		m_rhs;
-	btScalar		m_cfm;
-	
-    btScalar		m_lowerLimit;
-	btScalar		m_upperLimit;
-	btScalar		m_rhsPenetration;
-    union
+	btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1), m_orgConstraint(0), m_orgDofIndex(-1)
 	{
-		void*		m_originalContactPoint;
-		btScalar	m_unusedPadding4;
+	}
+
+	int m_deltaVelAindex;  //more generic version of m_relpos1CrossNormal/m_contactNormal1
+	int m_jacAindex;
+	int m_deltaVelBindex;
+	int m_jacBindex;
+
+	btVector3 m_relpos1CrossNormal;
+	btVector3 m_contactNormal1;
+	btVector3 m_relpos2CrossNormal;
+	btVector3 m_contactNormal2;  //usually m_contactNormal2 == -m_contactNormal1, but not always
+
+	btVector3 m_angularComponentA;
+	btVector3 m_angularComponentB;
+
+	mutable btSimdScalar m_appliedPushImpulse;
+	mutable btSimdScalar m_appliedImpulse;
+
+	btScalar m_friction;
+	btScalar m_jacDiagABInv;
+	btScalar m_rhs;
+	btScalar m_cfm;
+
+	btScalar m_lowerLimit;
+	btScalar m_upperLimit;
+	btScalar m_rhsPenetration;
+	union {
+		void* m_originalContactPoint;
+		btScalar m_unusedPadding4;
 	};
 
-	int	m_overrideNumSolverIterations;
-    int			m_frictionIndex;
+	int m_overrideNumSolverIterations;
+	int m_frictionIndex;
 
 	int m_solverBodyIdA;
 	btMultiBody* m_multiBodyA;
-	int			m_linkA;
-	
+	int m_linkA;
+
 	int m_solverBodyIdB;
 	btMultiBody* m_multiBodyB;
-	int			m_linkB;
+	int m_linkB;
 
 	//for writing back applied impulses
-	btMultiBodyConstraint*	m_orgConstraint;
+	btMultiBodyConstraint* m_orgConstraint;
 	int m_orgDofIndex;
 
-	enum		btSolverConstraintType
+	enum btSolverConstraintType
 	{
 		BT_SOLVER_CONTACT_1D = 0,
 		BT_SOLVER_FRICTION_1D
 	};
 };
 
-typedef btAlignedObjectArray<btMultiBodySolverConstraint>	btMultiBodyConstraintArray;
+typedef btAlignedObjectArray<btMultiBodySolverConstraint> btMultiBodyConstraintArray;
 
-#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H
+#endif  //BT_MULTIBODY_SOLVER_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp
new file mode 100644
index 00000000000..25ddd539bf9
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp
@@ -0,0 +1,172 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodySphericalJointMotor.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "LinearMath/btTransformUtil.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+
+btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR),
+	m_desiredVelocity(0, 0, 0),
+	m_desiredPosition(0,0,0,1),
+	m_kd(1.),
+	m_kp(0.2),
+	m_erp(1),
+	m_rhsClamp(SIMD_INFINITY)
+{
+
+	m_maxAppliedImpulse = maxMotorImpulse;
+}
+
+
+void btMultiBodySphericalJointMotor::finalizeMultiDof()
+{
+	allocateJacobiansMultiDof();
+	// note: we rely on the fact that data.m_jacobians are
+	// always initialized to zero by the Constraint ctor
+	int linkDoF = 0;
+	unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
+
+	// row 0: the lower bound
+	// row 0: the lower bound
+	jacobianA(0)[offset] = 1;
+
+	m_numDofsFinalized = m_jacSizeBoth;
+}
+
+
+btMultiBodySphericalJointMotor::~btMultiBodySphericalJointMotor()
+{
+}
+
+int btMultiBodySphericalJointMotor::getIslandIdA() const
+{
+	if (this->m_linkA < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyA->getLink(m_linkA).m_collider)
+		{
+			return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodySphericalJointMotor::getIslandIdB() const
+{
+	if (m_linkB < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyB->getLink(m_linkB).m_collider)
+		{
+			return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodySphericalJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+												 btMultiBodyJacobianData& data,
+												 const btContactSolverInfo& infoGlobal)
+{
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
+
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+		finalizeMultiDof();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
+		return;
+	
+
+	if (m_maxAppliedImpulse == 0.f)
+		return;
+
+	const btScalar posError = 0;
+	const btVector3 dummy(0, 0, 0);
+
+	
+	btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
+	
+	btQuaternion desiredQuat = m_desiredPosition;
+	btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0],
+		m_bodyA->getJointPosMultiDof(m_linkA)[1],
+		m_bodyA->getJointPosMultiDof(m_linkA)[2],
+		m_bodyA->getJointPosMultiDof(m_linkA)[3]);
+
+btQuaternion relRot = currentQuat.inverse() * desiredQuat;
+	btVector3 angleDiff;
+	btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff);
+
+
+
+	for (int row = 0; row < getNumRows(); row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+
+		int dof = row;
+		
+		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+		btScalar desiredVelocity = this->m_desiredVelocity[row];
+		
+		btScalar velocityError = desiredVelocity - currentVelocity;
+
+		btMatrix3x3 frameAworld;
+		frameAworld.setIdentity();
+		frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
+		btScalar posError = 0;
+		{
+			btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical);
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eSpherical:
+				{
+					btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
+					posError = m_kp*angleDiff[row % 3];
+					fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+						btVector3(0,0,0), dummy, dummy,
+						posError,
+						infoGlobal,
+						-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
+					constraintRow.m_orgConstraint = this;
+					constraintRow.m_orgDofIndex = row;
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+		}
+	}
+}
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h
new file mode 100644
index 00000000000..621beab5a42
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h
@@ -0,0 +1,77 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H
+#define BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H
+
+#include "btMultiBodyConstraint.h"
+struct btSolverInfo;
+
+class btMultiBodySphericalJointMotor : public btMultiBodyConstraint
+{
+protected:
+	btVector3 m_desiredVelocity;
+	btQuaternion m_desiredPosition;
+	btScalar m_kd;
+	btScalar m_kp;
+	btScalar m_erp;
+	btScalar m_rhsClamp;  //maximum error
+
+public:
+	btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse);
+	
+	virtual ~btMultiBodySphericalJointMotor();
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.f)
+	{
+		m_desiredVelocity = velTarget;
+		m_kd = kd;
+	}
+
+	virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp = 1.f)
+	{
+		m_desiredPosition = posTarget;
+		m_kp = kp;
+	}
+
+	virtual void setErp(btScalar erp)
+	{
+		m_erp = erp;
+	}
+	virtual btScalar getErp() const
+	{
+		return m_erp;
+	}
+	virtual void setRhsClamp(btScalar rhsClamp)
+	{
+		m_rhsClamp = rhsClamp;
+	}
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
+};
+
+#endif  //BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp
index 986f2148701..98ecdc07947 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp
@@ -108,18 +108,16 @@ rows/columns and manipulate C.
 
 */
 
-
 #include "btDantzigLCP.h"
 
-#include <string.h>//memcpy
+#include <string.h>  //memcpy
 
 bool s_error = false;
 
 //***************************************************************************
 // code generation parameters
 
-
-#define btLCP_FAST		// use fast btLCP object
+#define btLCP_FAST  // use fast btLCP object
 
 // option 1 : matrix row pointers (less data copying)
 #define BTROWPTRS
@@ -133,8 +131,6 @@ bool s_error = false;
 
 #define BTNUB_OPTIMIZATIONS
 
-
-
 /* solve L*X=B, with B containing 1 right hand sides.
  * L is an n*n lower triangular matrix with ones on the diagonal.
  * L is stored by rows and its leading dimension is lskip.
@@ -145,66 +141,69 @@ bool s_error = false;
  * if this is in the factorizer source file, n must be a multiple of 2.
  */
 
-static void btSolveL1_1 (const btScalar *L, btScalar *B, int n, int lskip1)
-{  
-  /* declare variables - Z matrix, p and q vectors, etc */
-  btScalar Z11,m11,Z21,m21,p1,q1,p2,*ex;
-  const btScalar *ell;
-  int i,j;
-  /* compute all 2 x 1 blocks of X */
-  for (i=0; i < n; i+=2) {
-    /* compute all 2 x 1 block of X, from rows i..i+2-1 */
-    /* set the Z matrix to 0 */
-    Z11=0;
-    Z21=0;
-    ell = L + i*lskip1;
-    ex = B;
-    /* the inner loop that computes outer products and adds them to Z */
-    for (j=i-2; j >= 0; j -= 2) {
-      /* compute outer product and add it to the Z matrix */
-      p1=ell[0];
-      q1=ex[0];
-      m11 = p1 * q1;
-      p2=ell[lskip1];
-      m21 = p2 * q1;
-      Z11 += m11;
-      Z21 += m21;
-      /* compute outer product and add it to the Z matrix */
-      p1=ell[1];
-      q1=ex[1];
-      m11 = p1 * q1;
-      p2=ell[1+lskip1];
-      m21 = p2 * q1;
-      /* advance pointers */
-      ell += 2;
-      ex += 2;
-      Z11 += m11;
-      Z21 += m21;
-      /* end of inner loop */
-    }
-    /* compute left-over iterations */
-    j += 2;
-    for (; j > 0; j--) {
-      /* compute outer product and add it to the Z matrix */
-      p1=ell[0];
-      q1=ex[0];
-      m11 = p1 * q1;
-      p2=ell[lskip1];
-      m21 = p2 * q1;
-      /* advance pointers */
-      ell += 1;
-      ex += 1;
-      Z11 += m11;
-      Z21 += m21;
-    }
-    /* finish computing the X(i) block */
-    Z11 = ex[0] - Z11;
-    ex[0] = Z11;
-    p1 = ell[lskip1];
-    Z21 = ex[1] - Z21 - p1*Z11;
-    ex[1] = Z21;
-    /* end of outer loop */
-  }
+static void btSolveL1_1(const btScalar *L, btScalar *B, int n, int lskip1)
+{
+	/* declare variables - Z matrix, p and q vectors, etc */
+	btScalar Z11, m11, Z21, m21, p1, q1, p2, *ex;
+	const btScalar *ell;
+	int i, j;
+	/* compute all 2 x 1 blocks of X */
+	for (i = 0; i < n; i += 2)
+	{
+		/* compute all 2 x 1 block of X, from rows i..i+2-1 */
+		/* set the Z matrix to 0 */
+		Z11 = 0;
+		Z21 = 0;
+		ell = L + i * lskip1;
+		ex = B;
+		/* the inner loop that computes outer products and adds them to Z */
+		for (j = i - 2; j >= 0; j -= 2)
+		{
+			/* compute outer product and add it to the Z matrix */
+			p1 = ell[0];
+			q1 = ex[0];
+			m11 = p1 * q1;
+			p2 = ell[lskip1];
+			m21 = p2 * q1;
+			Z11 += m11;
+			Z21 += m21;
+			/* compute outer product and add it to the Z matrix */
+			p1 = ell[1];
+			q1 = ex[1];
+			m11 = p1 * q1;
+			p2 = ell[1 + lskip1];
+			m21 = p2 * q1;
+			/* advance pointers */
+			ell += 2;
+			ex += 2;
+			Z11 += m11;
+			Z21 += m21;
+			/* end of inner loop */
+		}
+		/* compute left-over iterations */
+		j += 2;
+		for (; j > 0; j--)
+		{
+			/* compute outer product and add it to the Z matrix */
+			p1 = ell[0];
+			q1 = ex[0];
+			m11 = p1 * q1;
+			p2 = ell[lskip1];
+			m21 = p2 * q1;
+			/* advance pointers */
+			ell += 1;
+			ex += 1;
+			Z11 += m11;
+			Z21 += m21;
+		}
+		/* finish computing the X(i) block */
+		Z11 = ex[0] - Z11;
+		ex[0] = Z11;
+		p1 = ell[lskip1];
+		Z21 = ex[1] - Z21 - p1 * Z11;
+		ex[1] = Z21;
+		/* end of outer loop */
+	}
 }
 
 /* solve L*X=B, with B containing 2 right hand sides.
@@ -217,300 +216,308 @@ static void btSolveL1_1 (const btScalar *L, btScalar *B, int n, int lskip1)
  * if this is in the factorizer source file, n must be a multiple of 2.
  */
 
-static void btSolveL1_2 (const btScalar *L, btScalar *B, int n, int lskip1)
-{  
-  /* declare variables - Z matrix, p and q vectors, etc */
-  btScalar Z11,m11,Z12,m12,Z21,m21,Z22,m22,p1,q1,p2,q2,*ex;
-  const btScalar *ell;
-  int i,j;
-  /* compute all 2 x 2 blocks of X */
-  for (i=0; i < n; i+=2) {
-    /* compute all 2 x 2 block of X, from rows i..i+2-1 */
-    /* set the Z matrix to 0 */
-    Z11=0;
-    Z12=0;
-    Z21=0;
-    Z22=0;
-    ell = L + i*lskip1;
-    ex = B;
-    /* the inner loop that computes outer products and adds them to Z */
-    for (j=i-2; j >= 0; j -= 2) {
-      /* compute outer product and add it to the Z matrix */
-      p1=ell[0];
-      q1=ex[0];
-      m11 = p1 * q1;
-      q2=ex[lskip1];
-      m12 = p1 * q2;
-      p2=ell[lskip1];
-      m21 = p2 * q1;
-      m22 = p2 * q2;
-      Z11 += m11;
-      Z12 += m12;
-      Z21 += m21;
-      Z22 += m22;
-      /* compute outer product and add it to the Z matrix */
-      p1=ell[1];
-      q1=ex[1];
-      m11 = p1 * q1;
-      q2=ex[1+lskip1];
-      m12 = p1 * q2;
-      p2=ell[1+lskip1];
-      m21 = p2 * q1;
-      m22 = p2 * q2;
-      /* advance pointers */
-      ell += 2;
-      ex += 2;
-      Z11 += m11;
-      Z12 += m12;
-      Z21 += m21;
-      Z22 += m22;
-      /* end of inner loop */
-    }
-    /* compute left-over iterations */
-    j += 2;
-    for (; j > 0; j--) {
-      /* compute outer product and add it to the Z matrix */
-      p1=ell[0];
-      q1=ex[0];
-      m11 = p1 * q1;
-      q2=ex[lskip1];
-      m12 = p1 * q2;
-      p2=ell[lskip1];
-      m21 = p2 * q1;
-      m22 = p2 * q2;
-      /* advance pointers */
-      ell += 1;
-      ex += 1;
-      Z11 += m11;
-      Z12 += m12;
-      Z21 += m21;
-      Z22 += m22;
-    }
-    /* finish computing the X(i) block */
-    Z11 = ex[0] - Z11;
-    ex[0] = Z11;
-    Z12 = ex[lskip1] - Z12;
-    ex[lskip1] = Z12;
-    p1 = ell[lskip1];
-    Z21 = ex[1] - Z21 - p1*Z11;
-    ex[1] = Z21;
-    Z22 = ex[1+lskip1] - Z22 - p1*Z12;
-    ex[1+lskip1] = Z22;
-    /* end of outer loop */
-  }
+static void btSolveL1_2(const btScalar *L, btScalar *B, int n, int lskip1)
+{
+	/* declare variables - Z matrix, p and q vectors, etc */
+	btScalar Z11, m11, Z12, m12, Z21, m21, Z22, m22, p1, q1, p2, q2, *ex;
+	const btScalar *ell;
+	int i, j;
+	/* compute all 2 x 2 blocks of X */
+	for (i = 0; i < n; i += 2)
+	{
+		/* compute all 2 x 2 block of X, from rows i..i+2-1 */
+		/* set the Z matrix to 0 */
+		Z11 = 0;
+		Z12 = 0;
+		Z21 = 0;
+		Z22 = 0;
+		ell = L + i * lskip1;
+		ex = B;
+		/* the inner loop that computes outer products and adds them to Z */
+		for (j = i - 2; j >= 0; j -= 2)
+		{
+			/* compute outer product and add it to the Z matrix */
+			p1 = ell[0];
+			q1 = ex[0];
+			m11 = p1 * q1;
+			q2 = ex[lskip1];
+			m12 = p1 * q2;
+			p2 = ell[lskip1];
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z12 += m12;
+			Z21 += m21;
+			Z22 += m22;
+			/* compute outer product and add it to the Z matrix */
+			p1 = ell[1];
+			q1 = ex[1];
+			m11 = p1 * q1;
+			q2 = ex[1 + lskip1];
+			m12 = p1 * q2;
+			p2 = ell[1 + lskip1];
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			/* advance pointers */
+			ell += 2;
+			ex += 2;
+			Z11 += m11;
+			Z12 += m12;
+			Z21 += m21;
+			Z22 += m22;
+			/* end of inner loop */
+		}
+		/* compute left-over iterations */
+		j += 2;
+		for (; j > 0; j--)
+		{
+			/* compute outer product and add it to the Z matrix */
+			p1 = ell[0];
+			q1 = ex[0];
+			m11 = p1 * q1;
+			q2 = ex[lskip1];
+			m12 = p1 * q2;
+			p2 = ell[lskip1];
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			/* advance pointers */
+			ell += 1;
+			ex += 1;
+			Z11 += m11;
+			Z12 += m12;
+			Z21 += m21;
+			Z22 += m22;
+		}
+		/* finish computing the X(i) block */
+		Z11 = ex[0] - Z11;
+		ex[0] = Z11;
+		Z12 = ex[lskip1] - Z12;
+		ex[lskip1] = Z12;
+		p1 = ell[lskip1];
+		Z21 = ex[1] - Z21 - p1 * Z11;
+		ex[1] = Z21;
+		Z22 = ex[1 + lskip1] - Z22 - p1 * Z12;
+		ex[1 + lskip1] = Z22;
+		/* end of outer loop */
+	}
 }
 
+void btFactorLDLT(btScalar *A, btScalar *d, int n, int nskip1)
+{
+	int i, j;
+	btScalar sum, *ell, *dee, dd, p1, p2, q1, q2, Z11, m11, Z21, m21, Z22, m22;
+	if (n < 1) return;
 
-void btFactorLDLT (btScalar *A, btScalar *d, int n, int nskip1)
-{  
-  int i,j;
-  btScalar sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22;
-  if (n < 1) return;
-  
-  for (i=0; i<=n-2; i += 2) {
-    /* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */
-    btSolveL1_2 (A,A+i*nskip1,i,nskip1);
-    /* scale the elements in a 2 x i block at A(i,0), and also */
-    /* compute Z = the outer product matrix that we'll need. */
-    Z11 = 0;
-    Z21 = 0;
-    Z22 = 0;
-    ell = A+i*nskip1;
-    dee = d;
-    for (j=i-6; j >= 0; j -= 6) {
-      p1 = ell[0];
-      p2 = ell[nskip1];
-      dd = dee[0];
-      q1 = p1*dd;
-      q2 = p2*dd;
-      ell[0] = q1;
-      ell[nskip1] = q2;
-      m11 = p1*q1;
-      m21 = p2*q1;
-      m22 = p2*q2;
-      Z11 += m11;
-      Z21 += m21;
-      Z22 += m22;
-      p1 = ell[1];
-      p2 = ell[1+nskip1];
-      dd = dee[1];
-      q1 = p1*dd;
-      q2 = p2*dd;
-      ell[1] = q1;
-      ell[1+nskip1] = q2;
-      m11 = p1*q1;
-      m21 = p2*q1;
-      m22 = p2*q2;
-      Z11 += m11;
-      Z21 += m21;
-      Z22 += m22;
-      p1 = ell[2];
-      p2 = ell[2+nskip1];
-      dd = dee[2];
-      q1 = p1*dd;
-      q2 = p2*dd;
-      ell[2] = q1;
-      ell[2+nskip1] = q2;
-      m11 = p1*q1;
-      m21 = p2*q1;
-      m22 = p2*q2;
-      Z11 += m11;
-      Z21 += m21;
-      Z22 += m22;
-      p1 = ell[3];
-      p2 = ell[3+nskip1];
-      dd = dee[3];
-      q1 = p1*dd;
-      q2 = p2*dd;
-      ell[3] = q1;
-      ell[3+nskip1] = q2;
-      m11 = p1*q1;
-      m21 = p2*q1;
-      m22 = p2*q2;
-      Z11 += m11;
-      Z21 += m21;
-      Z22 += m22;
-      p1 = ell[4];
-      p2 = ell[4+nskip1];
-      dd = dee[4];
-      q1 = p1*dd;
-      q2 = p2*dd;
-      ell[4] = q1;
-      ell[4+nskip1] = q2;
-      m11 = p1*q1;
-      m21 = p2*q1;
-      m22 = p2*q2;
-      Z11 += m11;
-      Z21 += m21;
-      Z22 += m22;
-      p1 = ell[5];
-      p2 = ell[5+nskip1];
-      dd = dee[5];
-      q1 = p1*dd;
-      q2 = p2*dd;
-      ell[5] = q1;
-      ell[5+nskip1] = q2;
-      m11 = p1*q1;
-      m21 = p2*q1;
-      m22 = p2*q2;
-      Z11 += m11;
-      Z21 += m21;
-      Z22 += m22;
-      ell += 6;
-      dee += 6;
-    }
-    /* compute left-over iterations */
-    j += 6;
-    for (; j > 0; j--) {
-      p1 = ell[0];
-      p2 = ell[nskip1];
-      dd = dee[0];
-      q1 = p1*dd;
-      q2 = p2*dd;
-      ell[0] = q1;
-      ell[nskip1] = q2;
-      m11 = p1*q1;
-      m21 = p2*q1;
-      m22 = p2*q2;
-      Z11 += m11;
-      Z21 += m21;
-      Z22 += m22;
-      ell++;
-      dee++;
-    }
-    /* solve for diagonal 2 x 2 block at A(i,i) */
-    Z11 = ell[0] - Z11;
-    Z21 = ell[nskip1] - Z21;
-    Z22 = ell[1+nskip1] - Z22;
-    dee = d + i;
-    /* factorize 2 x 2 block Z,dee */
-    /* factorize row 1 */
-    dee[0] = btRecip(Z11);
-    /* factorize row 2 */
-    sum = 0;
-    q1 = Z21;
-    q2 = q1 * dee[0];
-    Z21 = q2;
-    sum += q1*q2;
-    dee[1] = btRecip(Z22 - sum);
-    /* done factorizing 2 x 2 block */
-    ell[nskip1] = Z21;
-  }
-  /* compute the (less than 2) rows at the bottom */
-  switch (n-i) {
-    case 0:
-    break;
-    
-    case 1:
-    btSolveL1_1 (A,A+i*nskip1,i,nskip1);
-    /* scale the elements in a 1 x i block at A(i,0), and also */
-    /* compute Z = the outer product matrix that we'll need. */
-    Z11 = 0;
-    ell = A+i*nskip1;
-    dee = d;
-    for (j=i-6; j >= 0; j -= 6) {
-      p1 = ell[0];
-      dd = dee[0];
-      q1 = p1*dd;
-      ell[0] = q1;
-      m11 = p1*q1;
-      Z11 += m11;
-      p1 = ell[1];
-      dd = dee[1];
-      q1 = p1*dd;
-      ell[1] = q1;
-      m11 = p1*q1;
-      Z11 += m11;
-      p1 = ell[2];
-      dd = dee[2];
-      q1 = p1*dd;
-      ell[2] = q1;
-      m11 = p1*q1;
-      Z11 += m11;
-      p1 = ell[3];
-      dd = dee[3];
-      q1 = p1*dd;
-      ell[3] = q1;
-      m11 = p1*q1;
-      Z11 += m11;
-      p1 = ell[4];
-      dd = dee[4];
-      q1 = p1*dd;
-      ell[4] = q1;
-      m11 = p1*q1;
-      Z11 += m11;
-      p1 = ell[5];
-      dd = dee[5];
-      q1 = p1*dd;
-      ell[5] = q1;
-      m11 = p1*q1;
-      Z11 += m11;
-      ell += 6;
-      dee += 6;
-    }
-    /* compute left-over iterations */
-    j += 6;
-    for (; j > 0; j--) {
-      p1 = ell[0];
-      dd = dee[0];
-      q1 = p1*dd;
-      ell[0] = q1;
-      m11 = p1*q1;
-      Z11 += m11;
-      ell++;
-      dee++;
-    }
-    /* solve for diagonal 1 x 1 block at A(i,i) */
-    Z11 = ell[0] - Z11;
-    dee = d + i;
-    /* factorize 1 x 1 block Z,dee */
-    /* factorize row 1 */
-    dee[0] = btRecip(Z11);
-    /* done factorizing 1 x 1 block */
-    break;
-    
-    //default: *((char*)0)=0;  /* this should never happen! */
-  }
+	for (i = 0; i <= n - 2; i += 2)
+	{
+		/* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */
+		btSolveL1_2(A, A + i * nskip1, i, nskip1);
+		/* scale the elements in a 2 x i block at A(i,0), and also */
+		/* compute Z = the outer product matrix that we'll need. */
+		Z11 = 0;
+		Z21 = 0;
+		Z22 = 0;
+		ell = A + i * nskip1;
+		dee = d;
+		for (j = i - 6; j >= 0; j -= 6)
+		{
+			p1 = ell[0];
+			p2 = ell[nskip1];
+			dd = dee[0];
+			q1 = p1 * dd;
+			q2 = p2 * dd;
+			ell[0] = q1;
+			ell[nskip1] = q2;
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z21 += m21;
+			Z22 += m22;
+			p1 = ell[1];
+			p2 = ell[1 + nskip1];
+			dd = dee[1];
+			q1 = p1 * dd;
+			q2 = p2 * dd;
+			ell[1] = q1;
+			ell[1 + nskip1] = q2;
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z21 += m21;
+			Z22 += m22;
+			p1 = ell[2];
+			p2 = ell[2 + nskip1];
+			dd = dee[2];
+			q1 = p1 * dd;
+			q2 = p2 * dd;
+			ell[2] = q1;
+			ell[2 + nskip1] = q2;
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z21 += m21;
+			Z22 += m22;
+			p1 = ell[3];
+			p2 = ell[3 + nskip1];
+			dd = dee[3];
+			q1 = p1 * dd;
+			q2 = p2 * dd;
+			ell[3] = q1;
+			ell[3 + nskip1] = q2;
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z21 += m21;
+			Z22 += m22;
+			p1 = ell[4];
+			p2 = ell[4 + nskip1];
+			dd = dee[4];
+			q1 = p1 * dd;
+			q2 = p2 * dd;
+			ell[4] = q1;
+			ell[4 + nskip1] = q2;
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z21 += m21;
+			Z22 += m22;
+			p1 = ell[5];
+			p2 = ell[5 + nskip1];
+			dd = dee[5];
+			q1 = p1 * dd;
+			q2 = p2 * dd;
+			ell[5] = q1;
+			ell[5 + nskip1] = q2;
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z21 += m21;
+			Z22 += m22;
+			ell += 6;
+			dee += 6;
+		}
+		/* compute left-over iterations */
+		j += 6;
+		for (; j > 0; j--)
+		{
+			p1 = ell[0];
+			p2 = ell[nskip1];
+			dd = dee[0];
+			q1 = p1 * dd;
+			q2 = p2 * dd;
+			ell[0] = q1;
+			ell[nskip1] = q2;
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z21 += m21;
+			Z22 += m22;
+			ell++;
+			dee++;
+		}
+		/* solve for diagonal 2 x 2 block at A(i,i) */
+		Z11 = ell[0] - Z11;
+		Z21 = ell[nskip1] - Z21;
+		Z22 = ell[1 + nskip1] - Z22;
+		dee = d + i;
+		/* factorize 2 x 2 block Z,dee */
+		/* factorize row 1 */
+		dee[0] = btRecip(Z11);
+		/* factorize row 2 */
+		sum = 0;
+		q1 = Z21;
+		q2 = q1 * dee[0];
+		Z21 = q2;
+		sum += q1 * q2;
+		dee[1] = btRecip(Z22 - sum);
+		/* done factorizing 2 x 2 block */
+		ell[nskip1] = Z21;
+	}
+	/* compute the (less than 2) rows at the bottom */
+	switch (n - i)
+	{
+		case 0:
+			break;
+
+		case 1:
+			btSolveL1_1(A, A + i * nskip1, i, nskip1);
+			/* scale the elements in a 1 x i block at A(i,0), and also */
+			/* compute Z = the outer product matrix that we'll need. */
+			Z11 = 0;
+			ell = A + i * nskip1;
+			dee = d;
+			for (j = i - 6; j >= 0; j -= 6)
+			{
+				p1 = ell[0];
+				dd = dee[0];
+				q1 = p1 * dd;
+				ell[0] = q1;
+				m11 = p1 * q1;
+				Z11 += m11;
+				p1 = ell[1];
+				dd = dee[1];
+				q1 = p1 * dd;
+				ell[1] = q1;
+				m11 = p1 * q1;
+				Z11 += m11;
+				p1 = ell[2];
+				dd = dee[2];
+				q1 = p1 * dd;
+				ell[2] = q1;
+				m11 = p1 * q1;
+				Z11 += m11;
+				p1 = ell[3];
+				dd = dee[3];
+				q1 = p1 * dd;
+				ell[3] = q1;
+				m11 = p1 * q1;
+				Z11 += m11;
+				p1 = ell[4];
+				dd = dee[4];
+				q1 = p1 * dd;
+				ell[4] = q1;
+				m11 = p1 * q1;
+				Z11 += m11;
+				p1 = ell[5];
+				dd = dee[5];
+				q1 = p1 * dd;
+				ell[5] = q1;
+				m11 = p1 * q1;
+				Z11 += m11;
+				ell += 6;
+				dee += 6;
+			}
+			/* compute left-over iterations */
+			j += 6;
+			for (; j > 0; j--)
+			{
+				p1 = ell[0];
+				dd = dee[0];
+				q1 = p1 * dd;
+				ell[0] = q1;
+				m11 = p1 * q1;
+				Z11 += m11;
+				ell++;
+				dee++;
+			}
+			/* solve for diagonal 1 x 1 block at A(i,i) */
+			Z11 = ell[0] - Z11;
+			dee = d + i;
+			/* factorize 1 x 1 block Z,dee */
+			/* factorize row 1 */
+			dee[0] = btRecip(Z11);
+			/* done factorizing 1 x 1 block */
+			break;
+
+			//default: *((char*)0)=0;  /* this should never happen! */
+	}
 }
 
 /* solve L*X=B, with B containing 1 right hand sides.
@@ -523,289 +530,295 @@ void btFactorLDLT (btScalar *A, btScalar *d, int n, int nskip1)
  * if this is in the factorizer source file, n must be a multiple of 4.
  */
 
-void btSolveL1 (const btScalar *L, btScalar *B, int n, int lskip1)
-{  
-  /* declare variables - Z matrix, p and q vectors, etc */
-  btScalar Z11,Z21,Z31,Z41,p1,q1,p2,p3,p4,*ex;
-  const btScalar *ell;
-  int lskip2,lskip3,i,j;
-  /* compute lskip values */
-  lskip2 = 2*lskip1;
-  lskip3 = 3*lskip1;
-  /* compute all 4 x 1 blocks of X */
-  for (i=0; i <= n-4; i+=4) {
-    /* compute all 4 x 1 block of X, from rows i..i+4-1 */
-    /* set the Z matrix to 0 */
-    Z11=0;
-    Z21=0;
-    Z31=0;
-    Z41=0;
-    ell = L + i*lskip1;
-    ex = B;
-    /* the inner loop that computes outer products and adds them to Z */
-    for (j=i-12; j >= 0; j -= 12) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      p2=ell[lskip1];
-      p3=ell[lskip2];
-      p4=ell[lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[1];
-      q1=ex[1];
-      p2=ell[1+lskip1];
-      p3=ell[1+lskip2];
-      p4=ell[1+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[2];
-      q1=ex[2];
-      p2=ell[2+lskip1];
-      p3=ell[2+lskip2];
-      p4=ell[2+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[3];
-      q1=ex[3];
-      p2=ell[3+lskip1];
-      p3=ell[3+lskip2];
-      p4=ell[3+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[4];
-      q1=ex[4];
-      p2=ell[4+lskip1];
-      p3=ell[4+lskip2];
-      p4=ell[4+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[5];
-      q1=ex[5];
-      p2=ell[5+lskip1];
-      p3=ell[5+lskip2];
-      p4=ell[5+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[6];
-      q1=ex[6];
-      p2=ell[6+lskip1];
-      p3=ell[6+lskip2];
-      p4=ell[6+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[7];
-      q1=ex[7];
-      p2=ell[7+lskip1];
-      p3=ell[7+lskip2];
-      p4=ell[7+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[8];
-      q1=ex[8];
-      p2=ell[8+lskip1];
-      p3=ell[8+lskip2];
-      p4=ell[8+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[9];
-      q1=ex[9];
-      p2=ell[9+lskip1];
-      p3=ell[9+lskip2];
-      p4=ell[9+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[10];
-      q1=ex[10];
-      p2=ell[10+lskip1];
-      p3=ell[10+lskip2];
-      p4=ell[10+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[11];
-      q1=ex[11];
-      p2=ell[11+lskip1];
-      p3=ell[11+lskip2];
-      p4=ell[11+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* advance pointers */
-      ell += 12;
-      ex += 12;
-      /* end of inner loop */
-    }
-    /* compute left-over iterations */
-    j += 12;
-    for (; j > 0; j--) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      p2=ell[lskip1];
-      p3=ell[lskip2];
-      p4=ell[lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* advance pointers */
-      ell += 1;
-      ex += 1;
-    }
-    /* finish computing the X(i) block */
-    Z11 = ex[0] - Z11;
-    ex[0] = Z11;
-    p1 = ell[lskip1];
-    Z21 = ex[1] - Z21 - p1*Z11;
-    ex[1] = Z21;
-    p1 = ell[lskip2];
-    p2 = ell[1+lskip2];
-    Z31 = ex[2] - Z31 - p1*Z11 - p2*Z21;
-    ex[2] = Z31;
-    p1 = ell[lskip3];
-    p2 = ell[1+lskip3];
-    p3 = ell[2+lskip3];
-    Z41 = ex[3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
-    ex[3] = Z41;
-    /* end of outer loop */
-  }
-  /* compute rows at end that are not a multiple of block size */
-  for (; i < n; i++) {
-    /* compute all 1 x 1 block of X, from rows i..i+1-1 */
-    /* set the Z matrix to 0 */
-    Z11=0;
-    ell = L + i*lskip1;
-    ex = B;
-    /* the inner loop that computes outer products and adds them to Z */
-    for (j=i-12; j >= 0; j -= 12) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[1];
-      q1=ex[1];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[2];
-      q1=ex[2];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[3];
-      q1=ex[3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[4];
-      q1=ex[4];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[5];
-      q1=ex[5];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[6];
-      q1=ex[6];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[7];
-      q1=ex[7];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[8];
-      q1=ex[8];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[9];
-      q1=ex[9];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[10];
-      q1=ex[10];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[11];
-      q1=ex[11];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* advance pointers */
-      ell += 12;
-      ex += 12;
-      /* end of inner loop */
-    }
-    /* compute left-over iterations */
-    j += 12;
-    for (; j > 0; j--) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* advance pointers */
-      ell += 1;
-      ex += 1;
-    }
-    /* finish computing the X(i) block */
-    Z11 = ex[0] - Z11;
-    ex[0] = Z11;
-  }
+void btSolveL1(const btScalar *L, btScalar *B, int n, int lskip1)
+{
+	/* declare variables - Z matrix, p and q vectors, etc */
+	btScalar Z11, Z21, Z31, Z41, p1, q1, p2, p3, p4, *ex;
+	const btScalar *ell;
+	int lskip2, lskip3, i, j;
+	/* compute lskip values */
+	lskip2 = 2 * lskip1;
+	lskip3 = 3 * lskip1;
+	/* compute all 4 x 1 blocks of X */
+	for (i = 0; i <= n - 4; i += 4)
+	{
+		/* compute all 4 x 1 block of X, from rows i..i+4-1 */
+		/* set the Z matrix to 0 */
+		Z11 = 0;
+		Z21 = 0;
+		Z31 = 0;
+		Z41 = 0;
+		ell = L + i * lskip1;
+		ex = B;
+		/* the inner loop that computes outer products and adds them to Z */
+		for (j = i - 12; j >= 0; j -= 12)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			p2 = ell[lskip1];
+			p3 = ell[lskip2];
+			p4 = ell[lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[1];
+			q1 = ex[1];
+			p2 = ell[1 + lskip1];
+			p3 = ell[1 + lskip2];
+			p4 = ell[1 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[2];
+			q1 = ex[2];
+			p2 = ell[2 + lskip1];
+			p3 = ell[2 + lskip2];
+			p4 = ell[2 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[3];
+			q1 = ex[3];
+			p2 = ell[3 + lskip1];
+			p3 = ell[3 + lskip2];
+			p4 = ell[3 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[4];
+			q1 = ex[4];
+			p2 = ell[4 + lskip1];
+			p3 = ell[4 + lskip2];
+			p4 = ell[4 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[5];
+			q1 = ex[5];
+			p2 = ell[5 + lskip1];
+			p3 = ell[5 + lskip2];
+			p4 = ell[5 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[6];
+			q1 = ex[6];
+			p2 = ell[6 + lskip1];
+			p3 = ell[6 + lskip2];
+			p4 = ell[6 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[7];
+			q1 = ex[7];
+			p2 = ell[7 + lskip1];
+			p3 = ell[7 + lskip2];
+			p4 = ell[7 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[8];
+			q1 = ex[8];
+			p2 = ell[8 + lskip1];
+			p3 = ell[8 + lskip2];
+			p4 = ell[8 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[9];
+			q1 = ex[9];
+			p2 = ell[9 + lskip1];
+			p3 = ell[9 + lskip2];
+			p4 = ell[9 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[10];
+			q1 = ex[10];
+			p2 = ell[10 + lskip1];
+			p3 = ell[10 + lskip2];
+			p4 = ell[10 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[11];
+			q1 = ex[11];
+			p2 = ell[11 + lskip1];
+			p3 = ell[11 + lskip2];
+			p4 = ell[11 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* advance pointers */
+			ell += 12;
+			ex += 12;
+			/* end of inner loop */
+		}
+		/* compute left-over iterations */
+		j += 12;
+		for (; j > 0; j--)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			p2 = ell[lskip1];
+			p3 = ell[lskip2];
+			p4 = ell[lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* advance pointers */
+			ell += 1;
+			ex += 1;
+		}
+		/* finish computing the X(i) block */
+		Z11 = ex[0] - Z11;
+		ex[0] = Z11;
+		p1 = ell[lskip1];
+		Z21 = ex[1] - Z21 - p1 * Z11;
+		ex[1] = Z21;
+		p1 = ell[lskip2];
+		p2 = ell[1 + lskip2];
+		Z31 = ex[2] - Z31 - p1 * Z11 - p2 * Z21;
+		ex[2] = Z31;
+		p1 = ell[lskip3];
+		p2 = ell[1 + lskip3];
+		p3 = ell[2 + lskip3];
+		Z41 = ex[3] - Z41 - p1 * Z11 - p2 * Z21 - p3 * Z31;
+		ex[3] = Z41;
+		/* end of outer loop */
+	}
+	/* compute rows at end that are not a multiple of block size */
+	for (; i < n; i++)
+	{
+		/* compute all 1 x 1 block of X, from rows i..i+1-1 */
+		/* set the Z matrix to 0 */
+		Z11 = 0;
+		ell = L + i * lskip1;
+		ex = B;
+		/* the inner loop that computes outer products and adds them to Z */
+		for (j = i - 12; j >= 0; j -= 12)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[1];
+			q1 = ex[1];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[2];
+			q1 = ex[2];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[3];
+			q1 = ex[3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[4];
+			q1 = ex[4];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[5];
+			q1 = ex[5];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[6];
+			q1 = ex[6];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[7];
+			q1 = ex[7];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[8];
+			q1 = ex[8];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[9];
+			q1 = ex[9];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[10];
+			q1 = ex[10];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[11];
+			q1 = ex[11];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* advance pointers */
+			ell += 12;
+			ex += 12;
+			/* end of inner loop */
+		}
+		/* compute left-over iterations */
+		j += 12;
+		for (; j > 0; j--)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* advance pointers */
+			ell += 1;
+			ex += 1;
+		}
+		/* finish computing the X(i) block */
+		Z11 = ex[0] - Z11;
+		ex[0] = Z11;
+	}
 }
 
 /* solve L^T * x=b, with b containing 1 right hand side.
@@ -816,215 +829,218 @@ void btSolveL1 (const btScalar *L, btScalar *B, int n, int lskip1)
  * this processes blocks of 4.
  */
 
-void btSolveL1T (const btScalar *L, btScalar *B, int n, int lskip1)
-{  
-  /* declare variables - Z matrix, p and q vectors, etc */
-  btScalar Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex;
-  const btScalar *ell;
-  int lskip2,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;
-  /* compute all 4 x 1 blocks of X */
-  for (i=0; i <= n-4; i+=4) {
-    /* compute all 4 x 1 block of X, from rows i..i+4-1 */
-    /* set the Z matrix to 0 */
-    Z11=0;
-    Z21=0;
-    Z31=0;
-    Z41=0;
-    ell = L - i;
-    ex = B;
-    /* the inner loop that computes outer products and adds them to Z */
-    for (j=i-4; j >= 0; j -= 4) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      p2=ell[-1];
-      p3=ell[-2];
-      p4=ell[-3];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      m21 = p2 * q1;
-      m31 = p3 * q1;
-      m41 = p4 * q1;
-      ell += lskip1;
-      Z11 += m11;
-      Z21 += m21;
-      Z31 += m31;
-      Z41 += m41;
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[-1];
-      p2=ell[-1];
-      p3=ell[-2];
-      p4=ell[-3];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      m21 = p2 * q1;
-      m31 = p3 * q1;
-      m41 = p4 * q1;
-      ell += lskip1;
-      Z11 += m11;
-      Z21 += m21;
-      Z31 += m31;
-      Z41 += m41;
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[-2];
-      p2=ell[-1];
-      p3=ell[-2];
-      p4=ell[-3];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      m21 = p2 * q1;
-      m31 = p3 * q1;
-      m41 = p4 * q1;
-      ell += lskip1;
-      Z11 += m11;
-      Z21 += m21;
-      Z31 += m31;
-      Z41 += m41;
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[-3];
-      p2=ell[-1];
-      p3=ell[-2];
-      p4=ell[-3];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      m21 = p2 * q1;
-      m31 = p3 * q1;
-      m41 = p4 * q1;
-      ell += lskip1;
-      ex -= 4;
-      Z11 += m11;
-      Z21 += m21;
-      Z31 += m31;
-      Z41 += m41;
-      /* end of inner loop */
-    }
-    /* compute left-over iterations */
-    j += 4;
-    for (; j > 0; j--) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      p2=ell[-1];
-      p3=ell[-2];
-      p4=ell[-3];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      m21 = p2 * q1;
-      m31 = p3 * q1;
-      m41 = p4 * q1;
-      ell += lskip1;
-      ex -= 1;
-      Z11 += m11;
-      Z21 += m21;
-      Z31 += m31;
-      Z41 += m41;
-    }
-    /* finish computing the X(i) block */
-    Z11 = ex[0] - Z11;
-    ex[0] = Z11;
-    p1 = ell[-1];
-    Z21 = ex[-1] - Z21 - p1*Z11;
-    ex[-1] = Z21;
-    p1 = ell[-2];
-    p2 = ell[-2+lskip1];
-    Z31 = ex[-2] - Z31 - p1*Z11 - p2*Z21;
-    ex[-2] = Z31;
-    p1 = ell[-3];
-    p2 = ell[-3+lskip1];
-    p3 = ell[-3+lskip2];
-    Z41 = ex[-3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
-    ex[-3] = Z41;
-    /* end of outer loop */
-  }
-  /* compute rows at end that are not a multiple of block size */
-  for (; i < n; i++) {
-    /* compute all 1 x 1 block of X, from rows i..i+1-1 */
-    /* set the Z matrix to 0 */
-    Z11=0;
-    ell = L - i;
-    ex = B;
-    /* the inner loop that computes outer products and adds them to Z */
-    for (j=i-4; j >= 0; j -= 4) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      ell += lskip1;
-      Z11 += m11;
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[-1];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      ell += lskip1;
-      Z11 += m11;
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[-2];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      ell += lskip1;
-      Z11 += m11;
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[-3];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      ell += lskip1;
-      ex -= 4;
-      Z11 += m11;
-      /* end of inner loop */
-    }
-    /* compute left-over iterations */
-    j += 4;
-    for (; j > 0; j--) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      ell += lskip1;
-      ex -= 1;
-      Z11 += m11;
-    }
-    /* finish computing the X(i) block */
-    Z11 = ex[0] - Z11;
-    ex[0] = Z11;
-  }
+void 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, 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;
+	/* compute all 4 x 1 blocks of X */
+	for (i = 0; i <= n - 4; i += 4)
+	{
+		/* compute all 4 x 1 block of X, from rows i..i+4-1 */
+		/* set the Z matrix to 0 */
+		Z11 = 0;
+		Z21 = 0;
+		Z31 = 0;
+		Z41 = 0;
+		ell = L - i;
+		ex = B;
+		/* the inner loop that computes outer products and adds them to Z */
+		for (j = i - 4; j >= 0; j -= 4)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			p2 = ell[-1];
+			p3 = ell[-2];
+			p4 = ell[-3];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m31 = p3 * q1;
+			m41 = p4 * q1;
+			ell += lskip1;
+			Z11 += m11;
+			Z21 += m21;
+			Z31 += m31;
+			Z41 += m41;
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[-1];
+			p2 = ell[-1];
+			p3 = ell[-2];
+			p4 = ell[-3];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m31 = p3 * q1;
+			m41 = p4 * q1;
+			ell += lskip1;
+			Z11 += m11;
+			Z21 += m21;
+			Z31 += m31;
+			Z41 += m41;
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[-2];
+			p2 = ell[-1];
+			p3 = ell[-2];
+			p4 = ell[-3];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m31 = p3 * q1;
+			m41 = p4 * q1;
+			ell += lskip1;
+			Z11 += m11;
+			Z21 += m21;
+			Z31 += m31;
+			Z41 += m41;
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[-3];
+			p2 = ell[-1];
+			p3 = ell[-2];
+			p4 = ell[-3];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m31 = p3 * q1;
+			m41 = p4 * q1;
+			ell += lskip1;
+			ex -= 4;
+			Z11 += m11;
+			Z21 += m21;
+			Z31 += m31;
+			Z41 += m41;
+			/* end of inner loop */
+		}
+		/* compute left-over iterations */
+		j += 4;
+		for (; j > 0; j--)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			p2 = ell[-1];
+			p3 = ell[-2];
+			p4 = ell[-3];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m31 = p3 * q1;
+			m41 = p4 * q1;
+			ell += lskip1;
+			ex -= 1;
+			Z11 += m11;
+			Z21 += m21;
+			Z31 += m31;
+			Z41 += m41;
+		}
+		/* finish computing the X(i) block */
+		Z11 = ex[0] - Z11;
+		ex[0] = Z11;
+		p1 = ell[-1];
+		Z21 = ex[-1] - Z21 - p1 * Z11;
+		ex[-1] = Z21;
+		p1 = ell[-2];
+		p2 = ell[-2 + lskip1];
+		Z31 = ex[-2] - Z31 - p1 * Z11 - p2 * Z21;
+		ex[-2] = Z31;
+		p1 = ell[-3];
+		p2 = ell[-3 + lskip1];
+		p3 = ell[-3 + lskip2];
+		Z41 = ex[-3] - Z41 - p1 * Z11 - p2 * Z21 - p3 * Z31;
+		ex[-3] = Z41;
+		/* end of outer loop */
+	}
+	/* compute rows at end that are not a multiple of block size */
+	for (; i < n; i++)
+	{
+		/* compute all 1 x 1 block of X, from rows i..i+1-1 */
+		/* set the Z matrix to 0 */
+		Z11 = 0;
+		ell = L - i;
+		ex = B;
+		/* the inner loop that computes outer products and adds them to Z */
+		for (j = i - 4; j >= 0; j -= 4)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			ell += lskip1;
+			Z11 += m11;
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[-1];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			ell += lskip1;
+			Z11 += m11;
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[-2];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			ell += lskip1;
+			Z11 += m11;
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[-3];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			ell += lskip1;
+			ex -= 4;
+			Z11 += m11;
+			/* end of inner loop */
+		}
+		/* compute left-over iterations */
+		j += 4;
+		for (; j > 0; j--)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			ell += lskip1;
+			ex -= 1;
+			Z11 += m11;
+		}
+		/* finish computing the X(i) block */
+		Z11 = ex[0] - Z11;
+		ex[0] = Z11;
+	}
 }
 
-
-
-void btVectorScale (btScalar *a, const btScalar *d, int n)
+void btVectorScale(btScalar *a, const btScalar *d, int n)
 {
-  btAssert (a && d && n >= 0);
-  for (int i=0; i<n; i++) {
-    a[i] *= d[i];
-  }
+	btAssert(a && d && n >= 0);
+	for (int i = 0; i < n; i++)
+	{
+		a[i] *= d[i];
+	}
 }
 
-void btSolveLDLT (const btScalar *L, const btScalar *d, btScalar *b, int n, int nskip)
+void btSolveLDLT(const btScalar *L, const btScalar *d, btScalar *b, int n, int nskip)
 {
-  btAssert (L && d && b && n > 0 && nskip >= n);
-  btSolveL1 (L,b,n,nskip);
-  btVectorScale (b,d,n);
-  btSolveL1T (L,b,n,nskip);
+	btAssert(L && d && b && n > 0 && nskip >= n);
+	btSolveL1(L, b, n, nskip);
+	btVectorScale(b, d, n);
+	btSolveL1T(L, b, n, nskip);
 }
 
-
-
 //***************************************************************************
 
 // swap row/column i1 with i2 in the n*n matrix A. the leading dimension of
@@ -1033,124 +1049,129 @@ void btSolveLDLT (const btScalar *L, const btScalar *d, btScalar *b, int n, int
 // rows will be swapped by exchanging row pointers. otherwise the data will
 // be copied.
 
-static void btSwapRowsAndCols (BTATYPE A, int n, int i1, int i2, int nskip, 
-  int do_fast_row_swaps)
+static void btSwapRowsAndCols(BTATYPE A, int n, int i1, int i2, int nskip,
+							  int do_fast_row_swaps)
 {
-  btAssert (A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n &&
-    nskip >= n && i1 < i2);
-
-# ifdef BTROWPTRS
-  btScalar *A_i1 = A[i1];
-  btScalar *A_i2 = A[i2];
-  for (int i=i1+1; i<i2; ++i) {
-    btScalar *A_i_i1 = A[i] + i1;
-    A_i1[i] = *A_i_i1;
-    *A_i_i1 = A_i2[i];
-  }
-  A_i1[i2] = A_i1[i1];
-  A_i1[i1] = A_i2[i1];
-  A_i2[i1] = A_i2[i2];
-  // swap rows, by swapping row pointers
-  if (do_fast_row_swaps) {
-    A[i1] = A_i2;
-    A[i2] = A_i1;
-  }
-  else {
-    // Only swap till i2 column to match A plain storage variant.
-    for (int k = 0; k <= i2; ++k) {
-      btScalar tmp = A_i1[k];
-      A_i1[k] = A_i2[k];
-      A_i2[k] = tmp;
-    }
-  }
-  // swap columns the hard way
-  for (int j=i2+1; j<n; ++j) {
-    btScalar *A_j = A[j];
-    btScalar tmp = A_j[i1];
-    A_j[i1] = A_j[i2];
-    A_j[i2] = tmp;
-  }
-# else
-  btScalar *A_i1 = A+i1*nskip;
-  btScalar *A_i2 = A+i2*nskip;
-  for (int k = 0; k < i1; ++k) {
-    btScalar tmp = A_i1[k];
-    A_i1[k] = A_i2[k];
-    A_i2[k] = tmp;
-  }
-  btScalar *A_i = A_i1 + nskip;
-  for (int i=i1+1; i<i2; A_i+=nskip, ++i) {
-    btScalar tmp = A_i2[i];
-    A_i2[i] = A_i[i1];
-    A_i[i1] = tmp;
-  }
-  {
-    btScalar tmp = A_i1[i1];
-    A_i1[i1] = A_i2[i2];
-    A_i2[i2] = tmp;
-  }
-  btScalar *A_j = A_i2 + nskip;
-  for (int j=i2+1; j<n; A_j+=nskip, ++j) {
-    btScalar tmp = A_j[i1];
-    A_j[i1] = A_j[i2];
-    A_j[i2] = tmp;
-  }
-# endif
-}
+	btAssert(A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n &&
+			 nskip >= n && i1 < i2);
 
+#ifdef BTROWPTRS
+	btScalar *A_i1 = A[i1];
+	btScalar *A_i2 = A[i2];
+	for (int i = i1 + 1; i < i2; ++i)
+	{
+		btScalar *A_i_i1 = A[i] + i1;
+		A_i1[i] = *A_i_i1;
+		*A_i_i1 = A_i2[i];
+	}
+	A_i1[i2] = A_i1[i1];
+	A_i1[i1] = A_i2[i1];
+	A_i2[i1] = A_i2[i2];
+	// swap rows, by swapping row pointers
+	if (do_fast_row_swaps)
+	{
+		A[i1] = A_i2;
+		A[i2] = A_i1;
+	}
+	else
+	{
+		// Only swap till i2 column to match A plain storage variant.
+		for (int k = 0; k <= i2; ++k)
+		{
+			btScalar tmp = A_i1[k];
+			A_i1[k] = A_i2[k];
+			A_i2[k] = tmp;
+		}
+	}
+	// swap columns the hard way
+	for (int j = i2 + 1; j < n; ++j)
+	{
+		btScalar *A_j = A[j];
+		btScalar tmp = A_j[i1];
+		A_j[i1] = A_j[i2];
+		A_j[i2] = tmp;
+	}
+#else
+	btScalar *A_i1 = A + i1 * nskip;
+	btScalar *A_i2 = A + i2 * nskip;
+	for (int k = 0; k < i1; ++k)
+	{
+		btScalar tmp = A_i1[k];
+		A_i1[k] = A_i2[k];
+		A_i2[k] = tmp;
+	}
+	btScalar *A_i = A_i1 + nskip;
+	for (int i = i1 + 1; i < i2; A_i += nskip, ++i)
+	{
+		btScalar tmp = A_i2[i];
+		A_i2[i] = A_i[i1];
+		A_i[i1] = tmp;
+	}
+	{
+		btScalar tmp = A_i1[i1];
+		A_i1[i1] = A_i2[i2];
+		A_i2[i2] = tmp;
+	}
+	btScalar *A_j = A_i2 + nskip;
+	for (int j = i2 + 1; j < n; A_j += nskip, ++j)
+	{
+		btScalar tmp = A_j[i1];
+		A_j[i1] = A_j[i2];
+		A_j[i2] = tmp;
+	}
+#endif
+}
 
 // swap two indexes in the n*n LCP problem. i1 must be <= i2.
 
-static void btSwapProblem (BTATYPE A, btScalar *x, btScalar *b, btScalar *w, btScalar *lo,
-                         btScalar *hi, int *p, bool *state, int *findex,
-                         int n, int i1, int i2, int nskip,
-                         int do_fast_row_swaps)
+static void btSwapProblem(BTATYPE A, btScalar *x, btScalar *b, btScalar *w, btScalar *lo,
+						  btScalar *hi, int *p, bool *state, int *findex,
+						  int n, int i1, int i2, int nskip,
+						  int do_fast_row_swaps)
 {
-  btScalar tmpr;
-  int tmpi;
-  bool tmpb;
-  btAssert (n>0 && i1 >=0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && i1 <= i2);
-  if (i1==i2) return;
-  
-  btSwapRowsAndCols (A,n,i1,i2,nskip,do_fast_row_swaps);
-  
-  tmpr = x[i1];
-  x[i1] = x[i2];
-  x[i2] = tmpr;
-  
-  tmpr = b[i1];
-  b[i1] = b[i2];
-  b[i2] = tmpr;
-  
-  tmpr = w[i1];
-  w[i1] = w[i2];
-  w[i2] = tmpr;
-  
-  tmpr = lo[i1];
-  lo[i1] = lo[i2];
-  lo[i2] = tmpr;
-
-  tmpr = hi[i1];
-  hi[i1] = hi[i2];
-  hi[i2] = tmpr;
-
-  tmpi = p[i1];
-  p[i1] = p[i2];
-  p[i2] = tmpi;
-
-  tmpb = state[i1];
-  state[i1] = state[i2];
-  state[i2] = tmpb;
-
-  if (findex) {
-    tmpi = findex[i1];
-    findex[i1] = findex[i2];
-    findex[i2] = tmpi;
-  }
-}
+	btScalar tmpr;
+	int tmpi;
+	bool tmpb;
+	btAssert(n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && i1 <= i2);
+	if (i1 == i2) return;
+
+	btSwapRowsAndCols(A, n, i1, i2, nskip, do_fast_row_swaps);
+
+	tmpr = x[i1];
+	x[i1] = x[i2];
+	x[i2] = tmpr;
 
+	tmpr = b[i1];
+	b[i1] = b[i2];
+	b[i2] = tmpr;
 
+	tmpr = w[i1];
+	w[i1] = w[i2];
+	w[i2] = tmpr;
 
+	tmpr = lo[i1];
+	lo[i1] = lo[i2];
+	lo[i2] = tmpr;
+
+	tmpr = hi[i1];
+	hi[i1] = hi[i2];
+	hi[i2] = tmpr;
+
+	tmpi = p[i1];
+	p[i1] = p[i2];
+	p[i2] = tmpi;
+
+	tmpb = state[i1];
+	state[i1] = state[i2];
+	state[i2] = tmpb;
+
+	if (findex)
+	{
+		tmpi = findex[i1];
+		findex[i1] = findex[i2];
+		findex[i2] = tmpi;
+	}
+}
 
 //***************************************************************************
 // btLCP manipulator object. this represents an n*n LCP problem.
@@ -1186,79 +1207,88 @@ static void btSwapProblem (BTATYPE A, btScalar *x, btScalar *b, btScalar *w, btS
 
 #ifdef btLCP_FAST
 
-struct btLCP 
+struct btLCP
 {
 	const int m_n;
 	const int m_nskip;
 	int m_nub;
-	int m_nC, m_nN;				// size of each index set
-	BTATYPE const m_A;				// A rows
-	btScalar *const m_x, * const m_b, *const m_w, *const m_lo,* const m_hi;	// permuted LCP problem data
-	btScalar *const m_L, *const m_d;				// L*D*L' factorization of set C
+	int m_nC, m_nN;                                                         // size of each index set
+	BTATYPE const m_A;                                                      // A rows
+	btScalar *const m_x, *const m_b, *const m_w, *const m_lo, *const m_hi;  // permuted LCP problem data
+	btScalar *const m_L, *const m_d;                                        // L*D*L' factorization of set C
 	btScalar *const m_Dell, *const m_ell, *const m_tmp;
 	bool *const m_state;
 	int *const m_findex, *const m_p, *const m_C;
 
-	btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
-		btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
-		btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
-		bool *_state, int *_findex, int *p, int *c, btScalar **Arows);
+	btLCP(int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
+		  btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
+		  btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
+		  bool *_state, int *_findex, int *p, int *c, btScalar **Arows);
 	int getNub() const { return m_nub; }
-	void transfer_i_to_C (int i);
-	void transfer_i_to_N (int i) { m_nN++; }			// because we can assume C and N span 1:i-1
-	void transfer_i_from_N_to_C (int i);
-	void transfer_i_from_C_to_N (int i, btAlignedObjectArray<btScalar>& scratch);
+	void transfer_i_to_C(int i);
+	void transfer_i_to_N(int i) { m_nN++; }  // because we can assume C and N span 1:i-1
+	void transfer_i_from_N_to_C(int i);
+	void transfer_i_from_C_to_N(int i, btAlignedObjectArray<btScalar> &scratch);
 	int numC() const { return m_nC; }
 	int numN() const { return m_nN; }
-	int indexC (int i) const { return i; }
-	int indexN (int i) const { return i+m_nC; }
-	btScalar Aii (int i) const  { return BTAROW(i)[i]; }
-	btScalar AiC_times_qC (int i, btScalar *q) const { return btLargeDot (BTAROW(i), q, m_nC); }
-	btScalar AiN_times_qN (int i, btScalar *q) const { return btLargeDot (BTAROW(i)+m_nC, q+m_nC, m_nN); }
-	void pN_equals_ANC_times_qC (btScalar *p, btScalar *q);
-	void pN_plusequals_ANi (btScalar *p, int i, int sign=1);
-	void pC_plusequals_s_times_qC (btScalar *p, btScalar s, btScalar *q);
-	void pN_plusequals_s_times_qN (btScalar *p, btScalar s, btScalar *q);
-	void solve1 (btScalar *a, int i, int dir=1, int only_transfer=0);
+	int indexC(int i) const { return i; }
+	int indexN(int i) const { return i + m_nC; }
+	btScalar Aii(int i) const { return BTAROW(i)[i]; }
+	btScalar AiC_times_qC(int i, btScalar *q) const { return btLargeDot(BTAROW(i), q, m_nC); }
+	btScalar AiN_times_qN(int i, btScalar *q) const { return btLargeDot(BTAROW(i) + m_nC, q + m_nC, m_nN); }
+	void pN_equals_ANC_times_qC(btScalar *p, btScalar *q);
+	void pN_plusequals_ANi(btScalar *p, int i, int sign = 1);
+	void pC_plusequals_s_times_qC(btScalar *p, btScalar s, btScalar *q);
+	void pN_plusequals_s_times_qN(btScalar *p, btScalar s, btScalar *q);
+	void solve1(btScalar *a, int i, int dir = 1, int only_transfer = 0);
 	void unpermute();
 };
 
-
-btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
-            btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
-            btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
-            bool *_state, int *_findex, int *p, int *c, btScalar **Arows):
-  m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0),
-# ifdef BTROWPTRS
-  m_A(Arows),
+btLCP::btLCP(int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
+			 btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
+			 btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
+			 bool *_state, int *_findex, int *p, int *c, btScalar **Arows) : m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0),
+#ifdef BTROWPTRS
+																			 m_A(Arows),
 #else
-  m_A(_Adata),
+																			 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_x(_x),
+																			 m_b(_b),
+																			 m_w(_w),
+																			 m_lo(_lo),
+																			 m_hi(_hi),
+																			 m_L(l),
+																			 m_d(_d),
+																			 m_Dell(_Dell),
+																			 m_ell(_ell),
+																			 m_tmp(_tmp),
+																			 m_state(_state),
+																			 m_findex(_findex),
+																			 m_p(p),
+																			 m_C(c)
 {
-  {
-    btSetZero (m_x,m_n);
-  }
+	{
+		btSetZero(m_x, m_n);
+	}
 
-  {
-# ifdef BTROWPTRS
-    // make matrix row pointers
-    btScalar *aptr = _Adata;
-    BTATYPE A = m_A;
-    const int n = m_n, nskip = m_nskip;
-    for (int k=0; k<n; aptr+=nskip, ++k) A[k] = aptr;
-# endif
-  }
+	{
+#ifdef BTROWPTRS
+		// make matrix row pointers
+		btScalar *aptr = _Adata;
+		BTATYPE A = m_A;
+		const int n = m_n, nskip = m_nskip;
+		for (int k = 0; k < n; aptr += nskip, ++k) A[k] = aptr;
+#endif
+	}
 
-  {
-    int *p = m_p;
-    const int n = m_n;
-    for (int k=0; k<n; ++k) p[k]=k;		// initially unpermuted
-  }
+	{
+		int *p = m_p;
+		const int n = m_n;
+		for (int k = 0; k < n; ++k) p[k] = k;  // initially unpermuted
+	}
 
-  /*
+	/*
   // for testing, we can do some random swaps in the area i > nub
   {
     const int n = m_n;
@@ -1277,63 +1307,69 @@ btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btSc
   }
   */
 
-  // permute the problem so that *all* the unbounded variables are at the
-  // start, i.e. look for unbounded variables not included in `nub'. we can
-  // potentially push up `nub' this way and get a bigger initial factorization.
-  // note that when we swap rows/cols here we must not just swap row pointers,
-  // as the initial factorization relies on the data being all in one chunk.
-  // variables that have findex >= 0 are *not* considered to be unbounded even
-  // if lo=-inf and hi=inf - this is because these limits may change during the
-  // solution process.
-
-  {
-    int *findex = m_findex;
-    btScalar *lo = m_lo, *hi = m_hi;
-    const int n = m_n;
-    for (int k = m_nub; k<n; ++k) {
-      if (findex && findex[k] >= 0) continue;
-      if (lo[k]==-BT_INFINITY && hi[k]==BT_INFINITY) {
-        btSwapProblem (m_A,m_x,m_b,m_w,lo,hi,m_p,m_state,findex,n,m_nub,k,m_nskip,0);
-        m_nub++;
-      }
-    }
-  }
-
-  // if there are unbounded variables at the start, factorize A up to that
-  // point and solve for x. this puts all indexes 0..nub-1 into C.
-  if (m_nub > 0) {
-    const int nub = m_nub;
-    {
-      btScalar *Lrow = m_L;
-      const int nskip = m_nskip;
-      for (int j=0; j<nub; Lrow+=nskip, ++j) memcpy(Lrow,BTAROW(j),(j+1)*sizeof(btScalar));
-    }
-    btFactorLDLT (m_L,m_d,nub,m_nskip);
-    memcpy (m_x,m_b,nub*sizeof(btScalar));
-    btSolveLDLT (m_L,m_d,m_x,nub,m_nskip);
-    btSetZero (m_w,nub);
-    {
-      int *C = m_C;
-      for (int k=0; k<nub; ++k) C[k] = k;
-    }
-    m_nC = nub;
-  }
-
-  // permute the indexes > nub such that all findex variables are at the end
-  if (m_findex) {
-    const int nub = m_nub;
-    int *findex = m_findex;
-    int num_at_end = 0;
-    for (int k=m_n-1; k >= nub; k--) {
-      if (findex[k] >= 0) {
-        btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,findex,m_n,k,m_n-1-num_at_end,m_nskip,1);
-        num_at_end++;
-      }
-    }
-  }
+	// permute the problem so that *all* the unbounded variables are at the
+	// start, i.e. look for unbounded variables not included in `nub'. we can
+	// potentially push up `nub' this way and get a bigger initial factorization.
+	// note that when we swap rows/cols here we must not just swap row pointers,
+	// as the initial factorization relies on the data being all in one chunk.
+	// variables that have findex >= 0 are *not* considered to be unbounded even
+	// if lo=-inf and hi=inf - this is because these limits may change during the
+	// solution process.
 
-  // print info about indexes
-  /*
+	{
+		int *findex = m_findex;
+		btScalar *lo = m_lo, *hi = m_hi;
+		const int n = m_n;
+		for (int k = m_nub; k < n; ++k)
+		{
+			if (findex && findex[k] >= 0) continue;
+			if (lo[k] == -BT_INFINITY && hi[k] == BT_INFINITY)
+			{
+				btSwapProblem(m_A, m_x, m_b, m_w, lo, hi, m_p, m_state, findex, n, m_nub, k, m_nskip, 0);
+				m_nub++;
+			}
+		}
+	}
+
+	// if there are unbounded variables at the start, factorize A up to that
+	// point and solve for x. this puts all indexes 0..nub-1 into C.
+	if (m_nub > 0)
+	{
+		const int nub = m_nub;
+		{
+			btScalar *Lrow = m_L;
+			const int nskip = m_nskip;
+			for (int j = 0; j < nub; Lrow += nskip, ++j) memcpy(Lrow, BTAROW(j), (j + 1) * sizeof(btScalar));
+		}
+		btFactorLDLT(m_L, m_d, nub, m_nskip);
+		memcpy(m_x, m_b, nub * sizeof(btScalar));
+		btSolveLDLT(m_L, m_d, m_x, nub, m_nskip);
+		btSetZero(m_w, nub);
+		{
+			int *C = m_C;
+			for (int k = 0; k < nub; ++k) C[k] = k;
+		}
+		m_nC = nub;
+	}
+
+	// permute the indexes > nub such that all findex variables are at the end
+	if (m_findex)
+	{
+		const int nub = m_nub;
+		int *findex = m_findex;
+		int num_at_end = 0;
+		for (int k = m_n - 1; k >= nub; k--)
+		{
+			if (findex[k] >= 0)
+			{
+				btSwapProblem(m_A, m_x, m_b, m_w, m_lo, m_hi, m_p, m_state, findex, m_n, k, m_n - 1 - num_at_end, m_nskip, 1);
+				num_at_end++;
+			}
+		}
+	}
+
+	// print info about indexes
+	/*
   {
     const int n = m_n;
     const int nub = m_nub;
@@ -1347,734 +1383,776 @@ btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btSc
   */
 }
 
-
-void btLCP::transfer_i_to_C (int i)
+void btLCP::transfer_i_to_C(int i)
 {
-  {
-    if (m_nC > 0) {
-      // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))
-      {
-        const int nC = m_nC;
-        btScalar *const Ltgt = m_L + nC*m_nskip, *ell = m_ell;
-        for (int j=0; j<nC; ++j) Ltgt[j] = ell[j];
-      }
-      const int nC = m_nC;
-      m_d[nC] = btRecip (BTAROW(i)[i] - btLargeDot(m_ell,m_Dell,nC));
-    }
-    else {
-      m_d[0] = btRecip (BTAROW(i)[i]);
-    }
-
-    btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1);
+	{
+		if (m_nC > 0)
+		{
+			// ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))
+			{
+				const int nC = m_nC;
+				btScalar *const Ltgt = m_L + nC * m_nskip, *ell = m_ell;
+				for (int j = 0; j < nC; ++j) Ltgt[j] = ell[j];
+			}
+			const int nC = m_nC;
+			m_d[nC] = btRecip(BTAROW(i)[i] - btLargeDot(m_ell, m_Dell, nC));
+		}
+		else
+		{
+			m_d[0] = btRecip(BTAROW(i)[i]);
+		}
 
-    const int nC = m_nC;
-    m_C[nC] = nC;
-    m_nC = nC + 1; // nC value is outdated after this line
-  }
+		btSwapProblem(m_A, m_x, m_b, m_w, m_lo, m_hi, m_p, m_state, m_findex, m_n, m_nC, i, m_nskip, 1);
 
+		const int nC = m_nC;
+		m_C[nC] = nC;
+		m_nC = nC + 1;  // nC value is outdated after this line
+	}
 }
 
-
-void btLCP::transfer_i_from_N_to_C (int i)
+void btLCP::transfer_i_from_N_to_C(int i)
 {
-  {
-    if (m_nC > 0) {
-      {
-        btScalar *const aptr = BTAROW(i);
-        btScalar *Dell = m_Dell;
-        const int *C = m_C;
-#   ifdef BTNUB_OPTIMIZATIONS
-        // if nub>0, initial part of aptr unpermuted
-        const int nub = m_nub;
-        int j=0;
-        for ( ; j<nub; ++j) Dell[j] = aptr[j];
-        const int nC = m_nC;
-        for ( ; j<nC; ++j) Dell[j] = aptr[C[j]];
-#   else
-        const int nC = m_nC;
-        for (int j=0; j<nC; ++j) Dell[j] = aptr[C[j]];
-#   endif
-      }
-      btSolveL1 (m_L,m_Dell,m_nC,m_nskip);
-      {
-        const int nC = m_nC;
-        btScalar *const Ltgt = m_L + nC*m_nskip;
-        btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d;
-        for (int j=0; j<nC; ++j) Ltgt[j] = ell[j] = Dell[j] * d[j];
-      }
-      const int nC = m_nC;
-      m_d[nC] = btRecip (BTAROW(i)[i] - btLargeDot(m_ell,m_Dell,nC));
-    }
-    else {
-      m_d[0] = btRecip (BTAROW(i)[i]);
-    }
-
-    btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1);
-
-    const int nC = m_nC;
-    m_C[nC] = nC;
-    m_nN--;
-    m_nC = nC + 1; // nC value is outdated after this line
-  }
-
-  // @@@ TO DO LATER
-  // if we just finish here then we'll go back and re-solve for
-  // delta_x. but actually we can be more efficient and incrementally
-  // update delta_x here. but if we do this, we wont have ell and Dell
-  // to use in updating the factorization later.
-
+	{
+		if (m_nC > 0)
+		{
+			{
+				btScalar *const aptr = BTAROW(i);
+				btScalar *Dell = m_Dell;
+				const int *C = m_C;
+#ifdef BTNUB_OPTIMIZATIONS
+				// if nub>0, initial part of aptr unpermuted
+				const int nub = m_nub;
+				int j = 0;
+				for (; j < nub; ++j) Dell[j] = aptr[j];
+				const int nC = m_nC;
+				for (; j < nC; ++j) Dell[j] = aptr[C[j]];
+#else
+				const int nC = m_nC;
+				for (int j = 0; j < nC; ++j) Dell[j] = aptr[C[j]];
+#endif
+			}
+			btSolveL1(m_L, m_Dell, m_nC, m_nskip);
+			{
+				const int nC = m_nC;
+				btScalar *const Ltgt = m_L + nC * m_nskip;
+				btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d;
+				for (int j = 0; j < nC; ++j) Ltgt[j] = ell[j] = Dell[j] * d[j];
+			}
+			const int nC = m_nC;
+			m_d[nC] = btRecip(BTAROW(i)[i] - btLargeDot(m_ell, m_Dell, nC));
+		}
+		else
+		{
+			m_d[0] = btRecip(BTAROW(i)[i]);
+		}
+
+		btSwapProblem(m_A, m_x, m_b, m_w, m_lo, m_hi, m_p, m_state, m_findex, m_n, m_nC, i, m_nskip, 1);
+
+		const int nC = m_nC;
+		m_C[nC] = nC;
+		m_nN--;
+		m_nC = nC + 1;  // nC value is outdated after this line
+	}
+
+	// @@@ TO DO LATER
+	// if we just finish here then we'll go back and re-solve for
+	// delta_x. but actually we can be more efficient and incrementally
+	// update delta_x here. but if we do this, we wont have ell and Dell
+	// to use in updating the factorization later.
 }
 
-void btRemoveRowCol (btScalar *A, int n, int nskip, int r)
+void btRemoveRowCol(btScalar *A, int n, int nskip, int r)
 {
-  btAssert(A && n > 0 && nskip >= n && r >= 0 && r < n);
-  if (r >= n-1) return;
-  if (r > 0) {
-    {
-      const size_t move_size = (n-r-1)*sizeof(btScalar);
-      btScalar *Adst = A + r;
-      for (int i=0; i<r; Adst+=nskip,++i) {
-        btScalar *Asrc = Adst + 1;
-        memmove (Adst,Asrc,move_size);
-      }
-    }
-    {
-      const size_t cpy_size = r*sizeof(btScalar);
-      btScalar *Adst = A + r * nskip;
-      for (int i=r; i<(n-1); ++i) {
-        btScalar *Asrc = Adst + nskip;
-        memcpy (Adst,Asrc,cpy_size);
-        Adst = Asrc;
-      }
-    }
-  }
-  {
-    const size_t cpy_size = (n-r-1)*sizeof(btScalar);
-    btScalar *Adst = A + r * (nskip + 1);
-    for (int i=r; i<(n-1); ++i) {
-      btScalar *Asrc = Adst + (nskip + 1);
-      memcpy (Adst,Asrc,cpy_size);
-      Adst = Asrc - 1;
-    }
-  }
+	btAssert(A && n > 0 && nskip >= n && r >= 0 && r < n);
+	if (r >= n - 1) return;
+	if (r > 0)
+	{
+		{
+			const size_t move_size = (n - r - 1) * sizeof(btScalar);
+			btScalar *Adst = A + r;
+			for (int i = 0; i < r; Adst += nskip, ++i)
+			{
+				btScalar *Asrc = Adst + 1;
+				memmove(Adst, Asrc, move_size);
+			}
+		}
+		{
+			const size_t cpy_size = r * sizeof(btScalar);
+			btScalar *Adst = A + r * nskip;
+			for (int i = r; i < (n - 1); ++i)
+			{
+				btScalar *Asrc = Adst + nskip;
+				memcpy(Adst, Asrc, cpy_size);
+				Adst = Asrc;
+			}
+		}
+	}
+	{
+		const size_t cpy_size = (n - r - 1) * sizeof(btScalar);
+		btScalar *Adst = A + r * (nskip + 1);
+		for (int i = r; i < (n - 1); ++i)
+		{
+			btScalar *Asrc = Adst + (nskip + 1);
+			memcpy(Adst, Asrc, cpy_size);
+			Adst = Asrc - 1;
+		}
+	}
 }
 
+void btLDLTAddTL(btScalar *L, btScalar *d, const btScalar *a, int n, int nskip, btAlignedObjectArray<btScalar> &scratch)
+{
+	btAssert(L && d && a && n > 0 && nskip >= n);
 
+	if (n < 2) return;
+	scratch.resize(2 * nskip);
+	btScalar *W1 = &scratch[0];
 
+	btScalar *W2 = W1 + nskip;
 
-void btLDLTAddTL (btScalar *L, btScalar *d, const btScalar *a, int n, int nskip, btAlignedObjectArray<btScalar>& scratch)
-{
-  btAssert (L && d && a && n > 0 && nskip >= n);
-
-  if (n < 2) return;
-  scratch.resize(2*nskip);
-  btScalar *W1 = &scratch[0];
-  
-  btScalar *W2 = W1 + nskip;
-
-  W1[0] = btScalar(0.0);
-  W2[0] = btScalar(0.0);
-  for (int j=1; j<n; ++j) {
-    W1[j] = W2[j] = (btScalar) (a[j] * SIMDSQRT12);
-  }
-  btScalar W11 = (btScalar) ((btScalar(0.5)*a[0]+1)*SIMDSQRT12);
-  btScalar W21 = (btScalar) ((btScalar(0.5)*a[0]-1)*SIMDSQRT12);
-
-  btScalar alpha1 = btScalar(1.0);
-  btScalar alpha2 = btScalar(1.0);
+	W1[0] = btScalar(0.0);
+	W2[0] = btScalar(0.0);
+	for (int j = 1; j < n; ++j)
+	{
+		W1[j] = W2[j] = (btScalar)(a[j] * SIMDSQRT12);
+	}
+	btScalar W11 = (btScalar)((btScalar(0.5) * a[0] + 1) * SIMDSQRT12);
+	btScalar W21 = (btScalar)((btScalar(0.5) * a[0] - 1) * SIMDSQRT12);
 
-  {
-    btScalar dee = d[0];
-    btScalar alphanew = alpha1 + (W11*W11)*dee;
-    btAssert(alphanew != btScalar(0.0));
-    dee /= alphanew;
-    btScalar gamma1 = W11 * dee;
-    dee *= alpha1;
-    alpha1 = alphanew;
-    alphanew = alpha2 - (W21*W21)*dee;
-    dee /= alphanew;
-    //btScalar gamma2 = W21 * dee;
-    alpha2 = alphanew;
-    btScalar k1 = btScalar(1.0) - W21*gamma1;
-    btScalar k2 = W21*gamma1*W11 - W21;
-    btScalar *ll = L + nskip;
-    for (int p=1; p<n; ll+=nskip, ++p) {
-      btScalar Wp = W1[p];
-      btScalar ell = *ll;
-      W1[p] =    Wp - W11*ell;
-      W2[p] = k1*Wp +  k2*ell;
-    }
-  }
+	btScalar alpha1 = btScalar(1.0);
+	btScalar alpha2 = btScalar(1.0);
 
-  btScalar *ll = L + (nskip + 1);
-  for (int j=1; j<n; ll+=nskip+1, ++j) {
-    btScalar k1 = W1[j];
-    btScalar k2 = W2[j];
-
-    btScalar dee = d[j];
-    btScalar alphanew = alpha1 + (k1*k1)*dee;
-    btAssert(alphanew != btScalar(0.0));
-    dee /= alphanew;
-    btScalar gamma1 = k1 * dee;
-    dee *= alpha1;
-    alpha1 = alphanew;
-    alphanew = alpha2 - (k2*k2)*dee;
-    dee /= alphanew;
-    btScalar gamma2 = k2 * dee;
-    dee *= alpha2;
-    d[j] = dee;
-    alpha2 = alphanew;
-
-    btScalar *l = ll + nskip;
-    for (int p=j+1; p<n; l+=nskip, ++p) {
-      btScalar ell = *l;
-      btScalar Wp = W1[p] - k1 * ell;
-      ell += gamma1 * Wp;
-      W1[p] = Wp;
-      Wp = W2[p] - k2 * ell;
-      ell -= gamma2 * Wp;
-      W2[p] = Wp;
-      *l = ell;
-    }
-  }
+	{
+		btScalar dee = d[0];
+		btScalar alphanew = alpha1 + (W11 * W11) * dee;
+		btAssert(alphanew != btScalar(0.0));
+		dee /= alphanew;
+		btScalar gamma1 = W11 * dee;
+		dee *= alpha1;
+		alpha1 = alphanew;
+		alphanew = alpha2 - (W21 * W21) * dee;
+		dee /= alphanew;
+		//btScalar gamma2 = W21 * dee;
+		alpha2 = alphanew;
+		btScalar k1 = btScalar(1.0) - W21 * gamma1;
+		btScalar k2 = W21 * gamma1 * W11 - W21;
+		btScalar *ll = L + nskip;
+		for (int p = 1; p < n; ll += nskip, ++p)
+		{
+			btScalar Wp = W1[p];
+			btScalar ell = *ll;
+			W1[p] = Wp - W11 * ell;
+			W2[p] = k1 * Wp + k2 * ell;
+		}
+	}
+
+	btScalar *ll = L + (nskip + 1);
+	for (int j = 1; j < n; ll += nskip + 1, ++j)
+	{
+		btScalar k1 = W1[j];
+		btScalar k2 = W2[j];
+
+		btScalar dee = d[j];
+		btScalar alphanew = alpha1 + (k1 * k1) * dee;
+		btAssert(alphanew != btScalar(0.0));
+		dee /= alphanew;
+		btScalar gamma1 = k1 * dee;
+		dee *= alpha1;
+		alpha1 = alphanew;
+		alphanew = alpha2 - (k2 * k2) * dee;
+		dee /= alphanew;
+		btScalar gamma2 = k2 * dee;
+		dee *= alpha2;
+		d[j] = dee;
+		alpha2 = alphanew;
+
+		btScalar *l = ll + nskip;
+		for (int p = j + 1; p < n; l += nskip, ++p)
+		{
+			btScalar ell = *l;
+			btScalar Wp = W1[p] - k1 * ell;
+			ell += gamma1 * Wp;
+			W1[p] = Wp;
+			Wp = W2[p] - k2 * ell;
+			ell -= gamma2 * Wp;
+			W2[p] = Wp;
+			*l = ell;
+		}
+	}
 }
 
-
-#define _BTGETA(i,j) (A[i][j])
+#define _BTGETA(i, j) (A[i][j])
 //#define _GETA(i,j) (A[(i)*nskip+(j)])
-#define BTGETA(i,j) ((i > j) ? _BTGETA(i,j) : _BTGETA(j,i))
+#define BTGETA(i, j) ((i > j) ? _BTGETA(i, j) : _BTGETA(j, i))
 
 inline size_t btEstimateLDLTAddTLTmpbufSize(int nskip)
 {
-  return nskip * 2 * sizeof(btScalar);
+	return nskip * 2 * sizeof(btScalar);
 }
 
-
-void btLDLTRemove (btScalar **A, const int *p, btScalar *L, btScalar *d,
-    int n1, int n2, int r, int nskip, btAlignedObjectArray<btScalar>& scratch)
+void btLDLTRemove(btScalar **A, const int *p, btScalar *L, btScalar *d,
+				  int n1, int n2, int r, int nskip, btAlignedObjectArray<btScalar> &scratch)
 {
-  btAssert(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 &&
-	   n1 >= n2 && nskip >= n1);
-  #ifdef BT_DEBUG
-	for (int i=0; i<n2; ++i) 
+	btAssert(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 &&
+			 n1 >= n2 && nskip >= n1);
+#ifdef BT_DEBUG
+	for (int i = 0; i < n2; ++i)
 		btAssert(p[i] >= 0 && p[i] < n1);
-  #endif
-
-  if (r==n2-1) {
-    return;		// deleting last row/col is easy
-  }
-  else {
-    size_t LDLTAddTL_size = btEstimateLDLTAddTLTmpbufSize(nskip);
-    btAssert(LDLTAddTL_size % sizeof(btScalar) == 0);
-	scratch.resize(nskip * 2+n2);
-    btScalar *tmp = &scratch[0];
-    if (r==0) {
-      btScalar *a = (btScalar *)((char *)tmp + LDLTAddTL_size);
-      const int p_0 = p[0];
-      for (int i=0; i<n2; ++i) {
-        a[i] = -BTGETA(p[i],p_0);
-      }
-      a[0] += btScalar(1.0);
-      btLDLTAddTL (L,d,a,n2,nskip,scratch);
-    }
-    else {
-      btScalar *t = (btScalar *)((char *)tmp + LDLTAddTL_size);
-      {
-        btScalar *Lcurr = L + r*nskip;
-        for (int i=0; i<r; ++Lcurr, ++i) {
-          btAssert(d[i] != btScalar(0.0));
-          t[i] = *Lcurr / d[i];
-        }
-      }
-      btScalar *a = t + r;
-      {
-        btScalar *Lcurr = L + r*nskip;
-        const int *pp_r = p + r, p_r = *pp_r;
-        const int n2_minus_r = n2-r;
-        for (int i=0; i<n2_minus_r; Lcurr+=nskip,++i) {
-          a[i] = btLargeDot(Lcurr,t,r) - BTGETA(pp_r[i],p_r);
-        }
-      }
-      a[0] += btScalar(1.0);
-      btLDLTAddTL (L + r*nskip+r, d+r, a, n2-r, nskip, scratch);
-    }
-  }
+#endif
 
-  // snip out row/column r from L and d
-  btRemoveRowCol (L,n2,nskip,r);
-  if (r < (n2-1)) memmove (d+r,d+r+1,(n2-r-1)*sizeof(btScalar));
+	if (r == n2 - 1)
+	{
+		return;  // deleting last row/col is easy
+	}
+	else
+	{
+		size_t LDLTAddTL_size = btEstimateLDLTAddTLTmpbufSize(nskip);
+		btAssert(LDLTAddTL_size % sizeof(btScalar) == 0);
+		scratch.resize(nskip * 2 + n2);
+		btScalar *tmp = &scratch[0];
+		if (r == 0)
+		{
+			btScalar *a = (btScalar *)((char *)tmp + LDLTAddTL_size);
+			const int p_0 = p[0];
+			for (int i = 0; i < n2; ++i)
+			{
+				a[i] = -BTGETA(p[i], p_0);
+			}
+			a[0] += btScalar(1.0);
+			btLDLTAddTL(L, d, a, n2, nskip, scratch);
+		}
+		else
+		{
+			btScalar *t = (btScalar *)((char *)tmp + LDLTAddTL_size);
+			{
+				btScalar *Lcurr = L + r * nskip;
+				for (int i = 0; i < r; ++Lcurr, ++i)
+				{
+					btAssert(d[i] != btScalar(0.0));
+					t[i] = *Lcurr / d[i];
+				}
+			}
+			btScalar *a = t + r;
+			{
+				btScalar *Lcurr = L + r * nskip;
+				const int *pp_r = p + r, p_r = *pp_r;
+				const int n2_minus_r = n2 - r;
+				for (int i = 0; i < n2_minus_r; Lcurr += nskip, ++i)
+				{
+					a[i] = btLargeDot(Lcurr, t, r) - BTGETA(pp_r[i], p_r);
+				}
+			}
+			a[0] += btScalar(1.0);
+			btLDLTAddTL(L + r * nskip + r, d + r, a, n2 - r, nskip, scratch);
+		}
+	}
+
+	// snip out row/column r from L and d
+	btRemoveRowCol(L, n2, nskip, r);
+	if (r < (n2 - 1)) memmove(d + r, d + r + 1, (n2 - r - 1) * sizeof(btScalar));
 }
 
-
-void btLCP::transfer_i_from_C_to_N (int i, btAlignedObjectArray<btScalar>& scratch)
+void btLCP::transfer_i_from_C_to_N(int i, btAlignedObjectArray<btScalar> &scratch)
 {
-  {
-    int *C = m_C;
-    // remove a row/column from the factorization, and adjust the
-    // indexes (black magic!)
-    int last_idx = -1;
-    const int nC = m_nC;
-    int j = 0;
-    for ( ; j<nC; ++j) {
-      if (C[j]==nC-1) {
-        last_idx = j;
-      }
-      if (C[j]==i) {
-        btLDLTRemove (m_A,C,m_L,m_d,m_n,nC,j,m_nskip,scratch);
-        int k;
-        if (last_idx == -1) {
-          for (k=j+1 ; k<nC; ++k) {
-            if (C[k]==nC-1) {
-              break;
-            }
-          }
-          btAssert (k < nC);
-        }
-        else {
-          k = last_idx;
-        }
-        C[k] = C[j];
-        if (j < (nC-1)) memmove (C+j,C+j+1,(nC-j-1)*sizeof(int));
-        break;
-      }
-    }
-    btAssert (j < nC);
-
-    btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,i,nC-1,m_nskip,1);
-
-    m_nN++;
-    m_nC = nC - 1; // nC value is outdated after this line
-  }
-
+	{
+		int *C = m_C;
+		// remove a row/column from the factorization, and adjust the
+		// indexes (black magic!)
+		int last_idx = -1;
+		const int nC = m_nC;
+		int j = 0;
+		for (; j < nC; ++j)
+		{
+			if (C[j] == nC - 1)
+			{
+				last_idx = j;
+			}
+			if (C[j] == i)
+			{
+				btLDLTRemove(m_A, C, m_L, m_d, m_n, nC, j, m_nskip, scratch);
+				int k;
+				if (last_idx == -1)
+				{
+					for (k = j + 1; k < nC; ++k)
+					{
+						if (C[k] == nC - 1)
+						{
+							break;
+						}
+					}
+					btAssert(k < nC);
+				}
+				else
+				{
+					k = last_idx;
+				}
+				C[k] = C[j];
+				if (j < (nC - 1)) memmove(C + j, C + j + 1, (nC - j - 1) * sizeof(int));
+				break;
+			}
+		}
+		btAssert(j < nC);
+
+		btSwapProblem(m_A, m_x, m_b, m_w, m_lo, m_hi, m_p, m_state, m_findex, m_n, i, nC - 1, m_nskip, 1);
+
+		m_nN++;
+		m_nC = nC - 1;  // nC value is outdated after this line
+	}
 }
 
-
-void btLCP::pN_equals_ANC_times_qC (btScalar *p, btScalar *q)
+void btLCP::pN_equals_ANC_times_qC(btScalar *p, btScalar *q)
 {
-  // we could try to make this matrix-vector multiplication faster using
-  // outer product matrix tricks, e.g. with the dMultidotX() functions.
-  // but i tried it and it actually made things slower on random 100x100
-  // problems because of the overhead involved. so we'll stick with the
-  // simple method for now.
-  const int nC = m_nC;
-  btScalar *ptgt = p + nC;
-  const int nN = m_nN;
-  for (int i=0; i<nN; ++i) {
-    ptgt[i] = btLargeDot (BTAROW(i+nC),q,nC);
-  }
+	// we could try to make this matrix-vector multiplication faster using
+	// outer product matrix tricks, e.g. with the dMultidotX() functions.
+	// but i tried it and it actually made things slower on random 100x100
+	// problems because of the overhead involved. so we'll stick with the
+	// simple method for now.
+	const int nC = m_nC;
+	btScalar *ptgt = p + nC;
+	const int nN = m_nN;
+	for (int i = 0; i < nN; ++i)
+	{
+		ptgt[i] = btLargeDot(BTAROW(i + nC), q, nC);
+	}
 }
 
-
-void btLCP::pN_plusequals_ANi (btScalar *p, int i, int sign)
+void btLCP::pN_plusequals_ANi(btScalar *p, int i, int sign)
 {
-  const int nC = m_nC;
-  btScalar *aptr = BTAROW(i) + nC;
-  btScalar *ptgt = p + nC;
-  if (sign > 0) {
-    const int nN = m_nN;
-    for (int j=0; j<nN; ++j) ptgt[j] += aptr[j];
-  }
-  else {
-    const int nN = m_nN;
-    for (int j=0; j<nN; ++j) ptgt[j] -= aptr[j];
-  }
+	const int nC = m_nC;
+	btScalar *aptr = BTAROW(i) + nC;
+	btScalar *ptgt = p + nC;
+	if (sign > 0)
+	{
+		const int nN = m_nN;
+		for (int j = 0; j < nN; ++j) ptgt[j] += aptr[j];
+	}
+	else
+	{
+		const int nN = m_nN;
+		for (int j = 0; j < nN; ++j) ptgt[j] -= aptr[j];
+	}
 }
 
-void btLCP::pC_plusequals_s_times_qC (btScalar *p, btScalar s, btScalar *q)
+void btLCP::pC_plusequals_s_times_qC(btScalar *p, btScalar s, btScalar *q)
 {
-  const int nC = m_nC;
-  for (int i=0; i<nC; ++i) {
-    p[i] += s*q[i];
-  }
+	const int nC = m_nC;
+	for (int i = 0; i < nC; ++i)
+	{
+		p[i] += s * q[i];
+	}
 }
 
-void btLCP::pN_plusequals_s_times_qN (btScalar *p, btScalar s, btScalar *q)
+void btLCP::pN_plusequals_s_times_qN(btScalar *p, btScalar s, btScalar *q)
 {
-  const int nC = m_nC;
-  btScalar *ptgt = p + nC, *qsrc = q + nC;
-  const int nN = m_nN;
-  for (int i=0; i<nN; ++i) {
-    ptgt[i] += s*qsrc[i];
-  }
+	const int nC = m_nC;
+	btScalar *ptgt = p + nC, *qsrc = q + nC;
+	const int nN = m_nN;
+	for (int i = 0; i < nN; ++i)
+	{
+		ptgt[i] += s * qsrc[i];
+	}
 }
 
-void btLCP::solve1 (btScalar *a, int i, int dir, int only_transfer)
+void btLCP::solve1(btScalar *a, int i, int dir, int only_transfer)
 {
-  // the `Dell' and `ell' that are computed here are saved. if index i is
-  // later added to the factorization then they can be reused.
-  //
-  // @@@ question: do we need to solve for entire delta_x??? yes, but
-  //     only if an x goes below 0 during the step.
-
-  if (m_nC > 0) {
-    {
-      btScalar *Dell = m_Dell;
-      int *C = m_C;
-      btScalar *aptr = BTAROW(i);
-#   ifdef BTNUB_OPTIMIZATIONS
-      // if nub>0, initial part of aptr[] is guaranteed unpermuted
-      const int nub = m_nub;
-      int j=0;
-      for ( ; j<nub; ++j) Dell[j] = aptr[j];
-      const int nC = m_nC;
-      for ( ; j<nC; ++j) Dell[j] = aptr[C[j]];
-#   else
-      const int nC = m_nC;
-      for (int j=0; j<nC; ++j) Dell[j] = aptr[C[j]];
-#   endif
-    }
-    btSolveL1 (m_L,m_Dell,m_nC,m_nskip);
-    {
-      btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d;
-      const int nC = m_nC;
-      for (int j=0; j<nC; ++j) ell[j] = Dell[j] * d[j];
-    }
+	// the `Dell' and `ell' that are computed here are saved. if index i is
+	// later added to the factorization then they can be reused.
+	//
+	// @@@ question: do we need to solve for entire delta_x??? yes, but
+	//     only if an x goes below 0 during the step.
 
-    if (!only_transfer) {
-      btScalar *tmp = m_tmp, *ell = m_ell;
-      {
-        const int nC = m_nC;
-        for (int j=0; j<nC; ++j) tmp[j] = ell[j];
-      }
-      btSolveL1T (m_L,tmp,m_nC,m_nskip);
-      if (dir > 0) {
-        int *C = m_C;
-        btScalar *tmp = m_tmp;
-        const int nC = m_nC;
-        for (int j=0; j<nC; ++j) a[C[j]] = -tmp[j];
-      } else {
-        int *C = m_C;
-        btScalar *tmp = m_tmp;
-        const int nC = m_nC;
-        for (int j=0; j<nC; ++j) a[C[j]] = tmp[j];
-      }
-    }
-  }
-}
+	if (m_nC > 0)
+	{
+		{
+			btScalar *Dell = m_Dell;
+			int *C = m_C;
+			btScalar *aptr = BTAROW(i);
+#ifdef BTNUB_OPTIMIZATIONS
+			// if nub>0, initial part of aptr[] is guaranteed unpermuted
+			const int nub = m_nub;
+			int j = 0;
+			for (; j < nub; ++j) Dell[j] = aptr[j];
+			const int nC = m_nC;
+			for (; j < nC; ++j) Dell[j] = aptr[C[j]];
+#else
+			const int nC = m_nC;
+			for (int j = 0; j < nC; ++j) Dell[j] = aptr[C[j]];
+#endif
+		}
+		btSolveL1(m_L, m_Dell, m_nC, m_nskip);
+		{
+			btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d;
+			const int nC = m_nC;
+			for (int j = 0; j < nC; ++j) ell[j] = Dell[j] * d[j];
+		}
 
+		if (!only_transfer)
+		{
+			btScalar *tmp = m_tmp, *ell = m_ell;
+			{
+				const int nC = m_nC;
+				for (int j = 0; j < nC; ++j) tmp[j] = ell[j];
+			}
+			btSolveL1T(m_L, tmp, m_nC, m_nskip);
+			if (dir > 0)
+			{
+				int *C = m_C;
+				btScalar *tmp = m_tmp;
+				const int nC = m_nC;
+				for (int j = 0; j < nC; ++j) a[C[j]] = -tmp[j];
+			}
+			else
+			{
+				int *C = m_C;
+				btScalar *tmp = m_tmp;
+				const int nC = m_nC;
+				for (int j = 0; j < nC; ++j) a[C[j]] = tmp[j];
+			}
+		}
+	}
+}
 
 void btLCP::unpermute()
 {
-  // now we have to un-permute x and w
-  {
-    memcpy (m_tmp,m_x,m_n*sizeof(btScalar));
-    btScalar *x = m_x, *tmp = m_tmp;
-    const int *p = m_p;
-    const int n = m_n;
-    for (int j=0; j<n; ++j) x[p[j]] = tmp[j];
-  }
-  {
-    memcpy (m_tmp,m_w,m_n*sizeof(btScalar));
-    btScalar *w = m_w, *tmp = m_tmp;
-    const int *p = m_p;
-    const int n = m_n;
-    for (int j=0; j<n; ++j) w[p[j]] = tmp[j];
-  }
+	// now we have to un-permute x and w
+	{
+		memcpy(m_tmp, m_x, m_n * sizeof(btScalar));
+		btScalar *x = m_x, *tmp = m_tmp;
+		const int *p = m_p;
+		const int n = m_n;
+		for (int j = 0; j < n; ++j) x[p[j]] = tmp[j];
+	}
+	{
+		memcpy(m_tmp, m_w, m_n * sizeof(btScalar));
+		btScalar *w = m_w, *tmp = m_tmp;
+		const int *p = m_p;
+		const int n = m_n;
+		for (int j = 0; j < n; ++j) w[p[j]] = tmp[j];
+	}
 }
 
-#endif // btLCP_FAST
-
+#endif  // btLCP_FAST
 
 //***************************************************************************
 // an optimized Dantzig LCP driver routine for the lo-hi LCP problem.
 
-bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b,
-                btScalar* outer_w, int nub, btScalar *lo, btScalar *hi, int *findex, btDantzigScratchMemory& scratchMem)
+bool btSolveDantzigLCP(int n, btScalar *A, btScalar *x, btScalar *b,
+					   btScalar *outer_w, int nub, btScalar *lo, btScalar *hi, int *findex, btDantzigScratchMemory &scratchMem)
 {
 	s_error = false;
 
-//	printf("btSolveDantzigLCP n=%d\n",n);
-  btAssert (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n);
-  btAssert(outer_w);
+	//	printf("btSolveDantzigLCP n=%d\n",n);
+	btAssert(n > 0 && A && x && b && lo && hi && nub >= 0 && nub <= n);
+	btAssert(outer_w);
 
 #ifdef BT_DEBUG
-  {
-    // check restrictions on lo and hi
-    for (int k=0; k<n; ++k) 
-		btAssert (lo[k] <= 0 && hi[k] >= 0);
-  }
-# endif
-
-
-  // if all the variables are unbounded then we can just factor, solve,
-  // and return
-  if (nub >= n) 
-  {
-   
-
-    int nskip = (n);
-    btFactorLDLT (A, outer_w, n, nskip);
-    btSolveLDLT (A, outer_w, b, n, nskip);
-    memcpy (x, b, n*sizeof(btScalar));
-
-    return !s_error;
-  }
-
-  const int nskip = (n);
-  scratchMem.L.resize(n*nskip);
-
-  scratchMem.d.resize(n);
-
-  btScalar *w = outer_w;
-  scratchMem.delta_w.resize(n);
-  scratchMem.delta_x.resize(n);
-  scratchMem.Dell.resize(n);
-  scratchMem.ell.resize(n);
-  scratchMem.Arows.resize(n);
-  scratchMem.p.resize(n);
-  scratchMem.C.resize(n);
-
-  // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
-  scratchMem.state.resize(n);
-
-
-  // create LCP object. note that tmp is set to delta_w to save space, this
-  // optimization relies on knowledge of how tmp is used, so be careful!
-  btLCP lcp(n,nskip,nub,A,x,b,w,lo,hi,&scratchMem.L[0],&scratchMem.d[0],&scratchMem.Dell[0],&scratchMem.ell[0],&scratchMem.delta_w[0],&scratchMem.state[0],findex,&scratchMem.p[0],&scratchMem.C[0],&scratchMem.Arows[0]);
-  int adj_nub = lcp.getNub();
-
-  // loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the
-  // LCP conditions then i is added to the appropriate index set. otherwise
-  // x(i),w(i) is driven either +ve or -ve to force it to the valid region.
-  // as we drive x(i), x(C) is also adjusted to keep w(C) at zero.
-  // while driving x(i) we maintain the LCP conditions on the other variables
-  // 0..i-1. we do this by watching out for other x(i),w(i) values going
-  // outside the valid region, and then switching them between index sets
-  // when that happens.
-
-  bool hit_first_friction_index = false;
-  for (int i=adj_nub; i<n; ++i) 
-  {
-    s_error = false;
-    // the index i is the driving index and indexes i+1..n-1 are "dont care",
-    // i.e. when we make changes to the system those x's will be zero and we
-    // don't care what happens to those w's. in other words, we only consider
-    // an (i+1)*(i+1) sub-problem of A*x=b+w.
-
-    // if we've hit the first friction index, we have to compute the lo and
-    // hi values based on the values of x already computed. we have been
-    // permuting the indexes, so the values stored in the findex vector are
-    // no longer valid. thus we have to temporarily unpermute the x vector. 
-    // for the purposes of this computation, 0*infinity = 0 ... so if the
-    // contact constraint's normal force is 0, there should be no tangential
-    // force applied.
-
-    if (!hit_first_friction_index && findex && findex[i] >= 0) {
-      // un-permute x into delta_w, which is not being used at the moment
-      for (int j=0; j<n; ++j) scratchMem.delta_w[scratchMem.p[j]] = x[j];
-
-      // set lo and hi values
-      for (int k=i; k<n; ++k) {
-        btScalar wfk = scratchMem.delta_w[findex[k]];
-        if (wfk == 0) {
-          hi[k] = 0;
-          lo[k] = 0;
-        }
-        else {
-          hi[k] = btFabs (hi[k] * wfk);
-          lo[k] = -hi[k];
-        }
-      }
-      hit_first_friction_index = true;
-    }
-
-    // thus far we have not even been computing the w values for indexes
-    // greater than i, so compute w[i] now.
-    w[i] = lcp.AiC_times_qC (i,x) + lcp.AiN_times_qN (i,x) - b[i];
-
-    // if lo=hi=0 (which can happen for tangential friction when normals are
-    // 0) then the index will be assigned to set N with some state. however,
-    // set C's line has zero size, so the index will always remain in set N.
-    // with the "normal" switching logic, if w changed sign then the index
-    // would have to switch to set C and then back to set N with an inverted
-    // state. this is pointless, and also computationally expensive. to
-    // prevent this from happening, we use the rule that indexes with lo=hi=0
-    // will never be checked for set changes. this means that the state for
-    // these indexes may be incorrect, but that doesn't matter.
-
-    // see if x(i),w(i) is in a valid region
-    if (lo[i]==0 && w[i] >= 0) {
-      lcp.transfer_i_to_N (i);
-      scratchMem.state[i] = false;
-    }
-    else if (hi[i]==0 && w[i] <= 0) {
-      lcp.transfer_i_to_N (i);
-      scratchMem.state[i] = true;
-    }
-    else if (w[i]==0) {
-      // this is a degenerate case. by the time we get to this test we know
-      // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve,
-      // and similarly that hi > 0. this means that the line segment
-      // corresponding to set C is at least finite in extent, and we are on it.
-      // NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C()
-      lcp.solve1 (&scratchMem.delta_x[0],i,0,1);
-
-      lcp.transfer_i_to_C (i);
-    }
-    else {
-      // we must push x(i) and w(i)
-      for (;;) {
-        int dir;
-        btScalar dirf;
-        // find direction to push on x(i)
-        if (w[i] <= 0) {
-          dir = 1;
-          dirf = btScalar(1.0);
-        }
-        else {
-          dir = -1;
-          dirf = btScalar(-1.0);
-        }
-
-        // compute: delta_x(C) = -dir*A(C,C)\A(C,i)
-        lcp.solve1 (&scratchMem.delta_x[0],i,dir);
-
-        // note that delta_x[i] = dirf, but we wont bother to set it
-
-        // compute: delta_w = A*delta_x ... note we only care about
-        // delta_w(N) and delta_w(i), the rest is ignored
-        lcp.pN_equals_ANC_times_qC (&scratchMem.delta_w[0],&scratchMem.delta_x[0]);
-        lcp.pN_plusequals_ANi (&scratchMem.delta_w[0],i,dir);
-        scratchMem.delta_w[i] = lcp.AiC_times_qC (i,&scratchMem.delta_x[0]) + lcp.Aii(i)*dirf;
-
-        // find largest step we can take (size=s), either to drive x(i),w(i)
-        // to the valid LCP region or to drive an already-valid variable
-        // outside the valid region.
-
-        int cmd = 1;		// index switching command
-        int si = 0;		// si = index to switch if cmd>3
-        btScalar s = -w[i]/scratchMem.delta_w[i];
-        if (dir > 0) {
-          if (hi[i] < BT_INFINITY) {
-            btScalar s2 = (hi[i]-x[i])*dirf;	// was (hi[i]-x[i])/dirf	// step to x(i)=hi(i)
-            if (s2 < s) {
-              s = s2;
-              cmd = 3;
-            }
-          }
-        }
-        else {
-          if (lo[i] > -BT_INFINITY) {
-            btScalar s2 = (lo[i]-x[i])*dirf;	// was (lo[i]-x[i])/dirf	// step to x(i)=lo(i)
-            if (s2 < s) {
-              s = s2;
-              cmd = 2;
-            }
-          }
-        }
-
-        {
-          const int numN = lcp.numN();
-          for (int k=0; k < numN; ++k) {
-            const int indexN_k = lcp.indexN(k);
-            if (!scratchMem.state[indexN_k] ? scratchMem.delta_w[indexN_k] < 0 : scratchMem.delta_w[indexN_k] > 0) {
-                // don't bother checking if lo=hi=0
-                if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue;
-                btScalar s2 = -w[indexN_k] / scratchMem.delta_w[indexN_k];
-                if (s2 < s) {
-                  s = s2;
-                  cmd = 4;
-                  si = indexN_k;
-                }
-            }
-          }
-        }
-
-        {
-          const int numC = lcp.numC();
-          for (int k=adj_nub; k < numC; ++k) {
-            const int indexC_k = lcp.indexC(k);
-            if (scratchMem.delta_x[indexC_k] < 0 && lo[indexC_k] > -BT_INFINITY) {
-              btScalar s2 = (lo[indexC_k]-x[indexC_k]) / scratchMem.delta_x[indexC_k];
-              if (s2 < s) {
-                s = s2;
-                cmd = 5;
-                si = indexC_k;
-              }
-            }
-            if (scratchMem.delta_x[indexC_k] > 0 && hi[indexC_k] < BT_INFINITY) {
-              btScalar s2 = (hi[indexC_k]-x[indexC_k]) / scratchMem.delta_x[indexC_k];
-              if (s2 < s) {
-                s = s2;
-                cmd = 6;
-                si = indexC_k;
-              }
-            }
-          }
-        }
-
-        //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C",
-        //			     "C->NL","C->NH"};
-        //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i);
-
-        // if s <= 0 then we've got a problem. if we just keep going then
-        // we're going to get stuck in an infinite loop. instead, just cross
-        // our fingers and exit with the current solution.
-        if (s <= btScalar(0.0)) 
-		{
-//          printf("LCP internal error, s <= 0 (s=%.4e)",(double)s);
-          if (i < n) {
-            btSetZero (x+i,n-i);
-            btSetZero (w+i,n-i);
-          }
-          s_error = true;
-          break;
-        }
-
-        // apply x = x + s * delta_x
-        lcp.pC_plusequals_s_times_qC (x, s, &scratchMem.delta_x[0]);
-        x[i] += s * dirf;
-
-        // apply w = w + s * delta_w
-        lcp.pN_plusequals_s_times_qN (w, s, &scratchMem.delta_w[0]);
-        w[i] += s * scratchMem.delta_w[i];
-
-//        void *tmpbuf;
-        // switch indexes between sets if necessary
-        switch (cmd) {
-        case 1:		// done
-          w[i] = 0;
-          lcp.transfer_i_to_C (i);
-          break;
-        case 2:		// done
-          x[i] = lo[i];
-          scratchMem.state[i] = false;
-          lcp.transfer_i_to_N (i);
-          break;
-        case 3:		// done
-          x[i] = hi[i];
-          scratchMem.state[i] = true;
-          lcp.transfer_i_to_N (i);
-          break;
-        case 4:		// keep going
-          w[si] = 0;
-          lcp.transfer_i_from_N_to_C (si);
-          break;
-        case 5:		// keep going
-          x[si] = lo[si];
-          scratchMem.state[si] = false;
-		  lcp.transfer_i_from_C_to_N (si, scratchMem.m_scratch);
-          break;
-        case 6:		// keep going
-          x[si] = hi[si];
-          scratchMem.state[si] = true;
-          lcp.transfer_i_from_C_to_N (si, scratchMem.m_scratch);
-          break;
-        }
-
-        if (cmd <= 3) break;
-      } // for (;;)
-    } // else
-
-    if (s_error) 
 	{
-      break;
-    }
-  } // for (int i=adj_nub; i<n; ++i)
+		// check restrictions on lo and hi
+		for (int k = 0; k < n; ++k)
+			btAssert(lo[k] <= 0 && hi[k] >= 0);
+	}
+#endif
 
-  lcp.unpermute();
+	// if all the variables are unbounded then we can just factor, solve,
+	// and return
+	if (nub >= n)
+	{
+		int nskip = (n);
+		btFactorLDLT(A, outer_w, n, nskip);
+		btSolveLDLT(A, outer_w, b, n, nskip);
+		memcpy(x, b, n * sizeof(btScalar));
+
+		return !s_error;
+	}
+
+	const int nskip = (n);
+	scratchMem.L.resize(n * nskip);
+
+	scratchMem.d.resize(n);
+
+	btScalar *w = outer_w;
+	scratchMem.delta_w.resize(n);
+	scratchMem.delta_x.resize(n);
+	scratchMem.Dell.resize(n);
+	scratchMem.ell.resize(n);
+	scratchMem.Arows.resize(n);
+	scratchMem.p.resize(n);
+	scratchMem.C.resize(n);
+
+	// for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
+	scratchMem.state.resize(n);
+
+	// create LCP object. note that tmp is set to delta_w to save space, this
+	// optimization relies on knowledge of how tmp is used, so be careful!
+	btLCP lcp(n, nskip, nub, A, x, b, w, lo, hi, &scratchMem.L[0], &scratchMem.d[0], &scratchMem.Dell[0], &scratchMem.ell[0], &scratchMem.delta_w[0], &scratchMem.state[0], findex, &scratchMem.p[0], &scratchMem.C[0], &scratchMem.Arows[0]);
+	int adj_nub = lcp.getNub();
+
+	// loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the
+	// LCP conditions then i is added to the appropriate index set. otherwise
+	// x(i),w(i) is driven either +ve or -ve to force it to the valid region.
+	// as we drive x(i), x(C) is also adjusted to keep w(C) at zero.
+	// while driving x(i) we maintain the LCP conditions on the other variables
+	// 0..i-1. we do this by watching out for other x(i),w(i) values going
+	// outside the valid region, and then switching them between index sets
+	// when that happens.
+
+	bool hit_first_friction_index = false;
+	for (int i = adj_nub; i < n; ++i)
+	{
+		s_error = false;
+		// the index i is the driving index and indexes i+1..n-1 are "dont care",
+		// i.e. when we make changes to the system those x's will be zero and we
+		// don't care what happens to those w's. in other words, we only consider
+		// an (i+1)*(i+1) sub-problem of A*x=b+w.
+
+		// if we've hit the first friction index, we have to compute the lo and
+		// hi values based on the values of x already computed. we have been
+		// permuting the indexes, so the values stored in the findex vector are
+		// no longer valid. thus we have to temporarily unpermute the x vector.
+		// for the purposes of this computation, 0*infinity = 0 ... so if the
+		// contact constraint's normal force is 0, there should be no tangential
+		// force applied.
+
+		if (!hit_first_friction_index && findex && findex[i] >= 0)
+		{
+			// un-permute x into delta_w, which is not being used at the moment
+			for (int j = 0; j < n; ++j) scratchMem.delta_w[scratchMem.p[j]] = x[j];
+
+			// set lo and hi values
+			for (int k = i; k < n; ++k)
+			{
+				btScalar wfk = scratchMem.delta_w[findex[k]];
+				if (wfk == 0)
+				{
+					hi[k] = 0;
+					lo[k] = 0;
+				}
+				else
+				{
+					hi[k] = btFabs(hi[k] * wfk);
+					lo[k] = -hi[k];
+				}
+			}
+			hit_first_friction_index = true;
+		}
+
+		// thus far we have not even been computing the w values for indexes
+		// greater than i, so compute w[i] now.
+		w[i] = lcp.AiC_times_qC(i, x) + lcp.AiN_times_qN(i, x) - b[i];
+
+		// if lo=hi=0 (which can happen for tangential friction when normals are
+		// 0) then the index will be assigned to set N with some state. however,
+		// set C's line has zero size, so the index will always remain in set N.
+		// with the "normal" switching logic, if w changed sign then the index
+		// would have to switch to set C and then back to set N with an inverted
+		// state. this is pointless, and also computationally expensive. to
+		// prevent this from happening, we use the rule that indexes with lo=hi=0
+		// will never be checked for set changes. this means that the state for
+		// these indexes may be incorrect, but that doesn't matter.
+
+		// see if x(i),w(i) is in a valid region
+		if (lo[i] == 0 && w[i] >= 0)
+		{
+			lcp.transfer_i_to_N(i);
+			scratchMem.state[i] = false;
+		}
+		else if (hi[i] == 0 && w[i] <= 0)
+		{
+			lcp.transfer_i_to_N(i);
+			scratchMem.state[i] = true;
+		}
+		else if (w[i] == 0)
+		{
+			// this is a degenerate case. by the time we get to this test we know
+			// that lo != 0, which means that lo < 0 as lo is not allowed to be +ve,
+			// and similarly that hi > 0. this means that the line segment
+			// corresponding to set C is at least finite in extent, and we are on it.
+			// NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C()
+			lcp.solve1(&scratchMem.delta_x[0], i, 0, 1);
+
+			lcp.transfer_i_to_C(i);
+		}
+		else
+		{
+			// we must push x(i) and w(i)
+			for (;;)
+			{
+				int dir;
+				btScalar dirf;
+				// find direction to push on x(i)
+				if (w[i] <= 0)
+				{
+					dir = 1;
+					dirf = btScalar(1.0);
+				}
+				else
+				{
+					dir = -1;
+					dirf = btScalar(-1.0);
+				}
+
+				// compute: delta_x(C) = -dir*A(C,C)\A(C,i)
+				lcp.solve1(&scratchMem.delta_x[0], i, dir);
+
+				// note that delta_x[i] = dirf, but we wont bother to set it
+
+				// compute: delta_w = A*delta_x ... note we only care about
+				// delta_w(N) and delta_w(i), the rest is ignored
+				lcp.pN_equals_ANC_times_qC(&scratchMem.delta_w[0], &scratchMem.delta_x[0]);
+				lcp.pN_plusequals_ANi(&scratchMem.delta_w[0], i, dir);
+				scratchMem.delta_w[i] = lcp.AiC_times_qC(i, &scratchMem.delta_x[0]) + lcp.Aii(i) * dirf;
+
+				// find largest step we can take (size=s), either to drive x(i),w(i)
+				// to the valid LCP region or to drive an already-valid variable
+				// outside the valid region.
+
+				int cmd = 1;  // index switching command
+				int si = 0;   // si = index to switch if cmd>3
+				btScalar s = -w[i] / scratchMem.delta_w[i];
+				if (dir > 0)
+				{
+					if (hi[i] < BT_INFINITY)
+					{
+						btScalar s2 = (hi[i] - x[i]) * dirf;  // was (hi[i]-x[i])/dirf	// step to x(i)=hi(i)
+						if (s2 < s)
+						{
+							s = s2;
+							cmd = 3;
+						}
+					}
+				}
+				else
+				{
+					if (lo[i] > -BT_INFINITY)
+					{
+						btScalar s2 = (lo[i] - x[i]) * dirf;  // was (lo[i]-x[i])/dirf	// step to x(i)=lo(i)
+						if (s2 < s)
+						{
+							s = s2;
+							cmd = 2;
+						}
+					}
+				}
+
+				{
+					const int numN = lcp.numN();
+					for (int k = 0; k < numN; ++k)
+					{
+						const int indexN_k = lcp.indexN(k);
+						if (!scratchMem.state[indexN_k] ? scratchMem.delta_w[indexN_k] < 0 : scratchMem.delta_w[indexN_k] > 0)
+						{
+							// don't bother checking if lo=hi=0
+							if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue;
+							btScalar s2 = -w[indexN_k] / scratchMem.delta_w[indexN_k];
+							if (s2 < s)
+							{
+								s = s2;
+								cmd = 4;
+								si = indexN_k;
+							}
+						}
+					}
+				}
+
+				{
+					const int numC = lcp.numC();
+					for (int k = adj_nub; k < numC; ++k)
+					{
+						const int indexC_k = lcp.indexC(k);
+						if (scratchMem.delta_x[indexC_k] < 0 && lo[indexC_k] > -BT_INFINITY)
+						{
+							btScalar s2 = (lo[indexC_k] - x[indexC_k]) / scratchMem.delta_x[indexC_k];
+							if (s2 < s)
+							{
+								s = s2;
+								cmd = 5;
+								si = indexC_k;
+							}
+						}
+						if (scratchMem.delta_x[indexC_k] > 0 && hi[indexC_k] < BT_INFINITY)
+						{
+							btScalar s2 = (hi[indexC_k] - x[indexC_k]) / scratchMem.delta_x[indexC_k];
+							if (s2 < s)
+							{
+								s = s2;
+								cmd = 6;
+								si = indexC_k;
+							}
+						}
+					}
+				}
+
+				//static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C",
+				//			     "C->NL","C->NH"};
+				//printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i);
+
+				// if s <= 0 then we've got a problem. if we just keep going then
+				// we're going to get stuck in an infinite loop. instead, just cross
+				// our fingers and exit with the current solution.
+				if (s <= btScalar(0.0))
+				{
+					//          printf("LCP internal error, s <= 0 (s=%.4e)",(double)s);
+					if (i < n)
+					{
+						btSetZero(x + i, n - i);
+						btSetZero(w + i, n - i);
+					}
+					s_error = true;
+					break;
+				}
+
+				// apply x = x + s * delta_x
+				lcp.pC_plusequals_s_times_qC(x, s, &scratchMem.delta_x[0]);
+				x[i] += s * dirf;
+
+				// apply w = w + s * delta_w
+				lcp.pN_plusequals_s_times_qN(w, s, &scratchMem.delta_w[0]);
+				w[i] += s * scratchMem.delta_w[i];
+
+				//        void *tmpbuf;
+				// switch indexes between sets if necessary
+				switch (cmd)
+				{
+					case 1:  // done
+						w[i] = 0;
+						lcp.transfer_i_to_C(i);
+						break;
+					case 2:  // done
+						x[i] = lo[i];
+						scratchMem.state[i] = false;
+						lcp.transfer_i_to_N(i);
+						break;
+					case 3:  // done
+						x[i] = hi[i];
+						scratchMem.state[i] = true;
+						lcp.transfer_i_to_N(i);
+						break;
+					case 4:  // keep going
+						w[si] = 0;
+						lcp.transfer_i_from_N_to_C(si);
+						break;
+					case 5:  // keep going
+						x[si] = lo[si];
+						scratchMem.state[si] = false;
+						lcp.transfer_i_from_C_to_N(si, scratchMem.m_scratch);
+						break;
+					case 6:  // keep going
+						x[si] = hi[si];
+						scratchMem.state[si] = true;
+						lcp.transfer_i_from_C_to_N(si, scratchMem.m_scratch);
+						break;
+				}
+
+				if (cmd <= 3) break;
+			}  // for (;;)
+		}      // else
+
+		if (s_error)
+		{
+			break;
+		}
+	}  // for (int i=adj_nub; i<n; ++i)
 
+	lcp.unpermute();
 
-  return !s_error;
+	return !s_error;
 }
-
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h
index 903832770ae..8d9b2a13e99 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h
@@ -41,7 +41,6 @@ to be implemented. the first `nub' variables are assumed to have findex < 0.
 
 */
 
-
 #ifndef _BT_LCP_H_
 #define _BT_LCP_H_
 
@@ -49,7 +48,6 @@ to be implemented. the first `nub' variables are assumed to have findex < 0.
 #include <stdio.h>
 #include <assert.h>
 
-
 #include "LinearMath/btScalar.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
@@ -62,16 +60,14 @@ struct btDantzigScratchMemory
 	btAlignedObjectArray<btScalar> delta_x;
 	btAlignedObjectArray<btScalar> Dell;
 	btAlignedObjectArray<btScalar> ell;
-	btAlignedObjectArray<btScalar*> Arows;
+	btAlignedObjectArray<btScalar *> Arows;
 	btAlignedObjectArray<int> p;
 	btAlignedObjectArray<int> C;
 	btAlignedObjectArray<bool> state;
 };
 
 //return false if solving failed
-bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b, btScalar *w,
-	int nub, btScalar *lo, btScalar *hi, int *findex,btDantzigScratchMemory& scratch);
-
-
+bool btSolveDantzigLCP(int n, btScalar *A, btScalar *x, btScalar *b, btScalar *w,
+					   int nub, btScalar *lo, btScalar *hi, int *findex, btDantzigScratchMemory &scratch);
 
-#endif //_BT_LCP_H_
+#endif  //_BT_LCP_H_
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h
index 2a2f2d3d32d..1f669751ce7 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h
@@ -20,30 +20,28 @@ subject to the following restrictions:
 #include "btMLCPSolverInterface.h"
 #include "btDantzigLCP.h"
 
-
 class btDantzigSolver : public btMLCPSolverInterface
 {
 protected:
-
 	btScalar m_acceptableUpperLimitSolution;
 
-	btAlignedObjectArray<char>	m_tempBuffer;
+	btAlignedObjectArray<char> m_tempBuffer;
 
 	btAlignedObjectArray<btScalar> m_A;
 	btAlignedObjectArray<btScalar> m_b;
 	btAlignedObjectArray<btScalar> m_x;
 	btAlignedObjectArray<btScalar> m_lo;
 	btAlignedObjectArray<btScalar> m_hi;
-	btAlignedObjectArray<int>	m_dependencies;
+	btAlignedObjectArray<int> m_dependencies;
 	btDantzigScratchMemory m_scratchMemory;
-public:
 
+public:
 	btDantzigSolver()
-		:m_acceptableUpperLimitSolution(btScalar(1000))
+		: m_acceptableUpperLimitSolution(btScalar(1000))
 	{
 	}
 
-	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	virtual bool solveMLCP(const btMatrixXu& A, const btVectorXu& b, btVectorXu& x, const btVectorXu& lo, const btVectorXu& hi, const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
 	{
 		bool result = true;
 		int n = b.rows();
@@ -52,14 +50,12 @@ public:
 			int nub = 0;
 			btAlignedObjectArray<btScalar> ww;
 			ww.resize(n);
-	
 
 			const btScalar* Aptr = A.getBufferPointer();
-			m_A.resize(n*n);
-			for (int i=0;i<n*n;i++)
+			m_A.resize(n * n);
+			for (int i = 0; i < n * n; i++)
 			{
 				m_A[i] = Aptr[i];
-
 			}
 
 			m_b.resize(n);
@@ -67,7 +63,7 @@ public:
 			m_lo.resize(n);
 			m_hi.resize(n);
 			m_dependencies.resize(n);
-			for (int i=0;i<n;i++)
+			for (int i = 0; i < n; i++)
 			{
 				m_lo[i] = lo[i];
 				m_hi[i] = hi[i];
@@ -76,13 +72,12 @@ public:
 				m_dependencies[i] = limitDependency[i];
 			}
 
-
-			result = btSolveDantzigLCP (n,&m_A[0],&m_x[0],&m_b[0],&ww[0],nub,&m_lo[0],&m_hi[0],&m_dependencies[0],m_scratchMemory);
+			result = btSolveDantzigLCP(n, &m_A[0], &m_x[0], &m_b[0], &ww[0], nub, &m_lo[0], &m_hi[0], &m_dependencies[0], m_scratchMemory);
 			if (!result)
 				return result;
 
-//			printf("numAllocas = %d\n",numAllocas);
-			for (int i=0;i<n;i++)
+			//			printf("numAllocas = %d\n",numAllocas);
+			for (int i = 0; i < n; i++)
 			{
 				volatile btScalar xx = m_x[i];
 				if (xx != m_x[i])
@@ -98,15 +93,14 @@ public:
 				}
 			}
 
-			for (int i=0;i<n;i++)
+			for (int i = 0; i < n; i++)
 			{
 				x[i] = m_x[i];
 			}
-			
 		}
 
 		return result;
 	}
 };
 
-#endif //BT_DANTZIG_SOLVER_H
+#endif  //BT_DANTZIG_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
index 1f4015c7c72..954ffaed759 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
@@ -19,64 +19,60 @@ subject to the following restrictions:
 //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
+#endif  //BT_DEBUG_OSTREAM
 
 btScalar btMachEps()
 {
-	static bool calculated=false;
+	static bool calculated = false;
 	static btScalar machEps = btScalar(1.);
 	if (!calculated)
 	{
-		do {
+		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;
+		} while ((btScalar)(1.0 + (machEps / btScalar(2.0))) != btScalar(1.0));
+		//		printf( "\nCalculated Machine epsilon: %G\n", machEps );
+		calculated = true;
 	}
 	return machEps;
 }
 
-btScalar btEpsRoot() {
-	
+btScalar btEpsRoot()
+{
 	static btScalar epsroot = 0.;
 	static bool alreadyCalculated = false;
-	
-	if (!alreadyCalculated) {
+
+	if (!alreadyCalculated)
+	{
 		epsroot = btSqrt(btMachEps());
 		alreadyCalculated = true;
 	}
 	return epsroot;
 }
 
-	 
-
-  btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
+btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
 {
-  
-    
-    steps = 0;
+	steps = 0;
 
-    int dim = m_q.size();
+	int dim = m_q.size();
 #ifdef BT_DEBUG_OSTREAM
-    if(DEBUGLEVEL >= 1) {
-      cout << "Dimension = " << dim << endl;
-    }
-#endif //BT_DEBUG_OSTREAM
+	if (DEBUGLEVEL >= 1)
+	{
+		cout << "Dimension = " << dim << endl;
+	}
+#endif  //BT_DEBUG_OSTREAM
 
 	btVectorXu solutionVector(2 * dim);
 	solutionVector.setZero();
-	  
-	  //, INIT, 0.);
+
+	//, INIT, 0.);
 
 	btMatrixXu ident(dim, dim);
 	ident.setIdentity();
@@ -85,287 +81,289 @@ btScalar btEpsRoot() {
 #endif
 
 	btMatrixXu mNeg = m_M.negative();
-	  
-    btMatrixXu A(dim, 2 * dim + 2);
+
+	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, 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);
+	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
+#endif  //BT_DEBUG_OSTREAM
 
+	//   btVectorXu q_;
+	//   q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1);
 
- //   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);
+	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++)
+	for (int i = 0; i < dim; i++)
 	{
-		btScalar v =A(i,2*dim+1);
-		if (v<minValue)
+		btScalar v = A(i, 2 * dim + 1);
+		if (v < minValue)
 		{
-			minValue=v;
+			minValue = v;
 			pivotRowIndex = i;
 		}
-		if (v<0)
+		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
+	//  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)
+	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
+		//  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...
+		}
 
-      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;
-      }
-
-      }
+		/*start looping*/
+		for (steps = 0; steps < maxloops; steps++)
+		{
+			GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
 #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;
+			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)
-          cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
-#endif //BT_DEBUG_OSTREAM
+		if (DEBUGLEVEL >= 1)
+		{
+			cout << "Number of loops: " << steps << endl;
+			cout << "Number of maximal loops: " << maxloops << endl;
+		}
+#endif  //BT_DEBUG_OSTREAM
 
-        return solutionVector;
-      }
+		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++)
+	if (DEBUGLEVEL >= 2)
 	{
-      solutionVector[basis[i]] = A(i,2*dim+1);//q_[i];
+		// cout << "A: " << A << endl;
+		cout << "pivotRowIndex " << pivotRowIndex << endl;
+		cout << "pivotColIndex " << pivotColIndex << endl;
 	}
+#endif  //BT_DEBUG_OSTREAM
 
-    info = 0;
+	for (int i = 0; i < basis.size(); i++)
+	{
+		solutionVector[basis[i]] = A(i, 2 * dim + 1);  //q_[i];
+	}
 
-    return solutionVector;
-  }
+	info = 0;
 
-  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++) 
-	  {
+	return solutionVector;
+}
 
-		  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;
+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;
+				//		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;
+#endif
+		}
+	}
 
-    while(i < v.size()-1 && fabs(v[i]) < btMachEps())
-      i++;
-    if (v[i] > 0)
-      return true;
+	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 false;
-  }
+	return RowIndex;
+}
 
-void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis) 
+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++)
+	for (int i = 0; i < A.rows(); i++)
 	{
-      if (i != pivotRowIndex)
-	  {
-        for (int j = 0; j < A.cols(); j++)
+		if (i != pivotRowIndex)
 		{
-          if (j != pivotColumnIndex)
-		  {
-			  btScalar v = A(i, j);
-			  v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
-            A.setElem(i, j, v);
-		  }
+			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++) 
+#endif  //BT_DEBUG_OSTREAM
+	for (int i = 0; i < A.cols(); i++)
 	{
-      A.mulElem(pivotRowIndex, i,-a);
-    }
+		A.mulElem(pivotRowIndex, i, -a);
+	}
 #ifdef BT_DEBUG_OSTREAM
 	cout << A << std::endl;
-#endif //#ifdef BT_DEBUG_OSTREAM
+#endif  //#ifdef BT_DEBUG_OSTREAM
 
-    for (int i = 0; i < A.rows(); i++)
+	for (int i = 0; i < A.rows(); i++)
 	{
-      if (i != pivotRowIndex)
-	  {
-        A.setElem(i, pivotColumnIndex,0);
-	  }
+		if (i != pivotRowIndex)
+		{
+			A.setElem(i, pivotColumnIndex, 0);
+		}
 	}
 #ifdef BT_DEBUG_OSTREAM
 	cout << A << std::endl;
-#endif //#ifdef BT_DEBUG_OSTREAM
-  }
+#endif  //#ifdef BT_DEBUG_OSTREAM
+}
 
-  bool btLemkeAlgorithm::greaterZero(const btVectorXu & vector)
+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;
-  }
+	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;
+}
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
index 7555cd9d207..3c6bf72a233 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
@@ -19,90 +19,84 @@ subject to the following restrictions:
 //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
+#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_);
-  }
+	btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int& DEBUGLEVEL_ = 0) : DEBUGLEVEL(DEBUGLEVEL_)
+	{
+		setSystem(M_, q_);
+	}
 
-  /* GETTER / SETTER */
-  /**
+	/* GETTER / SETTER */
+	/**
    * \brief return info of solution process
    */
-  int getInfo() {
-	return info;
-  }
+	int getInfo()
+	{
+		return info;
+	}
 
-  /**
+	/**
    * \brief get the number of steps until the solution was found
    */
-  int getSteps(void) {
-	return steps;
-  }
-
-
+	int getSteps(void)
+	{
+		return steps;
+	}
 
-  /**
+	/**
    * \brief set system with Matrix M and vector q
    */
-  void setSystem(const btMatrixXu & M_, const btVectorXu & 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);
+	btVectorXu solve(unsigned int maxloops = 0);
 
-  virtual ~btLemkeAlgorithm() {
-  }
+	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);
+	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;
+	btMatrixXu m_M;
+	btVectorXu m_q;
 
-  /**
+	/**
    * \brief number of steps until the Lemke algorithm found a solution
    */
-  unsigned int steps;
+	unsigned int steps;
 
-  /**
+	/**
    * \brief define level of debug output
    */
-  int DEBUGLEVEL;
+	int DEBUGLEVEL;
 
-  /**
+	/**
    * \brief did the algorithm find a solution
    *
    * -1 : not successful
    *  0 : successful
    */
-  int info;
+	int info;
 };
 
-
 #endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
index 98484c37964..f18c4ea41b0 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
@@ -17,334 +17,322 @@ subject to the following restrictions:
 #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) "
+///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;
-	
-	
+	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)
+		: 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)
+	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;
 
-		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];
-		}
+			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;
+			//		cout << "A" << endl;
+			//		cout << A << endl;
 
 			/////////////////////////////////////
 
 			//slow matrix inversion, replace with LU decomposition
 			btMatrixXu A1;
-			btMatrixXu B(n,n);
+			btMatrixXu B(n, n);
 			{
-				BT_PROFILE("inverse(slow)");
-				A1.resize(A.rows(),A.cols());
-				for (int row=0;row<A.rows();row++)
+				//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++)
+					for (int col = 0; col < A.cols(); col++)
 					{
-						A1.setElem(row,col,A(row,col));
+						A1.setElem(row, col, A(row, col));
 					}
 				}
 
 				btMatrixXu matrix;
-				matrix.resize(n,2*n);
-				for (int row=0;row<n;row++)
+				matrix.resize(n, 2 * n);
+				for (int row = 0; row < n; row++)
 				{
-					for (int col=0;col<n;col++)
+					for (int col = 0; col < n; col++)
 					{
-						matrix.setElem(row,col,A1(row,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);
+				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)
+				for (i = 0; i < n; i++)
+				{
+					for (j = 0; j < n; j++)
 					{
-						btScalar v = matrix(i,i);
-						if (btFuzzyZero(v))
+						if (i != j)
 						{
-							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));
+							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))
+				for (i = 0; i < n; i++)
 				{
-					a = 0.000001f;
-				}
-				btScalar invA = 1.f/a;
-				for(j = 0; j < 2*n; j++){
-					matrix.mulElem(i,j,invA);
+					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 row = 0; row < n; row++)
 				{
-					for (int col=0;col<n;col++)
+					for (int col = 0; col < n; col++)
 					{
-						B.setElem(row,col,matrix(row,n+col));
+						B.setElem(row, col, matrix(row, n + col));
 					}
 				}
 			}
 
-		btMatrixXu b1(n,1);
+			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++)
+			btMatrixXu M(n * 2, n * 2);
+			for (int row = 0; row < n; row++)
 			{
-				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);
-
+				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)' ]'
+			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 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;
+			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 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;
+			btMatrixXu x1;
 
-		x1 = B*(y1_b1);
-			
-		for (int row=0;row<n;row++)
-		{
-			solution[row] = x1(row,0);//n];
-		}
+			x1 = B * (y1_b1);
 
-		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)
+			for (int row = 0; row < n; row++)
 			{
-				//printf("Lemke result is #NAN\n");
-				x.setZero();
-				return false;
+				solution[row] = x1(row, 0);  //n];
 			}
-			
-			//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)
+
+			int errorIndexMax = -1;
+			int errorIndexMin = -1;
+			float errorValueMax = -1e30;
+			float errorValueMin = 1e30;
+
+			for (int i = 0; i < n; i++)
 			{
-				if (x[i]> errorValueMax)
+				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)
 				{
-					fail = true;
-					errorIndexMax = i;
-					errorValueMax = x[i];
+					if (x[i] < errorValueMin)
+					{
+						errorIndexMin = i;
+						errorValueMin = x[i];
+						fail = true;
+						//printf("x[i] = %f,",x[i]);
+					}
 				}
-				////printf("x[i] = %f,",x[i]);
 			}
-			if (x[i]<-m_maxValue)
+			if (fail)
 			{
-				if (x[i]<errorValueMin)
+				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++)
 				{
-					errorIndexMin = i;
-					errorValueMin = x[i];
-					fail = true;
-					//printf("x[i] = %f,",x[i]);
+					x[i] = 0.f;
 				}
 			}
+			return !fail;
 		}
-		if (fail)
+		else
+
 		{
-			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)
+			if (0 == dimension)
+				return true;
+
+			//		printf("================ solving using Lemke/Newton/Fixpoint\n");
+
+			btVectorXu q;
+			q.resize(dimension);
+			for (int row = 0; row < dimension; row++)
 			{
-				x.setZero();
-				return false;
+				q[row] = -b[row];
 			}
-			
-			//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)
+
+			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++)
 			{
-				if (x[i]> errorValueMax)
+				x[i] = solution[i + dimension];
+				volatile btScalar check = x[i];
+				if (x[i] != check)
 				{
-					fail = true;
-					errorIndexMax = i;
-					errorValueMax = x[i];
+					x.setZero();
+					return false;
 				}
-				////printf("x[i] = %f,",x[i]);
-			}
-			if (x[i]<-m_maxValue)
-			{
-				if (x[i]<errorValueMin)
+
+				//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)
 				{
-					errorIndexMin = i;
-					errorValueMin = x[i];
-					fail = true;
-					//printf("x[i] = %f,",x[i]);
+					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++)
+			if (fail)
 			{
-				x[i]=0.f;
+				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;
 
+			return !fail;
+		}
+		return true;
 	}
-
 };
 
-#endif //BT_LEMKE_SOLVER_H
+#endif  //BT_LEMKE_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
index 6688694a928..5d864f27578 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
@@ -19,11 +19,9 @@ subject to the following restrictions:
 #include "LinearMath/btQuickprof.h"
 #include "btSolveProjectedGaussSeidel.h"
 
-
-btMLCPSolver::btMLCPSolver(	 btMLCPSolverInterface* solver)
-:m_solver(solver),
-m_fallback(0),
-m_cfm(0.000001)//0.0000001
+btMLCPSolver::btMLCPSolver(btMLCPSolverInterface* solver)
+	: m_solver(solver),
+	  m_fallback(0)
 {
 }
 
@@ -34,67 +32,65 @@ btMLCPSolver::~btMLCPSolver()
 bool gUseMatrixMultiply = false;
 bool interleaveContactAndFriction = false;
 
-btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
-	btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies, numBodiesUnUsed, manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+	btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodiesUnUsed, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
 	{
 		BT_PROFILE("gather constraint data");
 
-		int numFrictionPerContact = m_tmpSolverContactConstraintPool.size()==m_tmpSolverContactFrictionConstraintPool.size()? 1 : 2;
-
+		int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
 
-	//	int numBodies = m_tmpSolverBodyPool.size();
+		//	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());
+		m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size());
+		btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size());
+		//	printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size());
 
 		int dindex = 0;
-		for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++)
+		for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); i++)
 		{
 			m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
 			m_limitDependencies[dindex++] = -1;
 		}
- 
+
 		///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
-		
-		int firstContactConstraintOffset=dindex;
+
+		int firstContactConstraintOffset = dindex;
 
 		if (interleaveContactAndFriction)
 		{
-			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
+			for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
 			{
 				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
 				m_limitDependencies[dindex++] = -1;
-				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]);
-				int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact));
-				m_limitDependencies[dindex++] = findex +firstContactConstraintOffset;
-				if (numFrictionPerContact==2)
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]);
+				int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact));
+				m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
+				if (numFrictionPerContact == 2)
 				{
-					m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
-					m_limitDependencies[dindex++] = findex+firstContactConstraintOffset;
+					m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]);
+					m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
 				}
 			}
-		} else
+		}
+		else
 		{
-			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
+			for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
 			{
 				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
 				m_limitDependencies[dindex++] = -1;
 			}
-			for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++)
+			for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++)
 			{
 				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
-				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset;
+				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
 			}
-			
 		}
 
-
 		if (!m_allConstraintPtrArray.size())
 		{
-			m_A.resize(0,0);
+			m_A.resize(0, 0);
 			m_b.resize(0);
 			m_x.resize(0);
 			m_lo.resize(0);
@@ -103,7 +99,6 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
 		}
 	}
 
-	
 	if (gUseMatrixMultiply)
 	{
 		BT_PROFILE("createMLCP");
@@ -122,7 +117,7 @@ bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
 {
 	bool result = true;
 
-	if (m_A.rows()==0)
+	if (m_A.rows() == 0)
 		return true;
 
 	//if using split impulse, we solve 2 separate (M)LCPs
@@ -130,28 +125,26 @@ bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
 	{
 		btMatrixXu Acopy = m_A;
 		btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
-//		printf("solve first LCP\n");
-		result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
+		//		printf("solve first LCP\n");
+		result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
 		if (result)
-			result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations );
-
-	} else
+			result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations);
+	}
+	else
 	{
-		result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
+		result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
 	}
 	return result;
 }
 
 struct btJointNode
 {
-	int jointIndex;     // pointer to enclosing dxJoint object
-	int otherBodyIndex;       // *other* body this joint is connected to
-	int nextJointNodeIndex;//-1 for null
+	int jointIndex;          // pointer to enclosing dxJoint object
+	int otherBodyIndex;      // *other* body this joint is connected to
+	int nextJointNodeIndex;  //-1 for null
 	int constraintRowIndex;
 };
 
-
-
 void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 {
 	int numContactRows = interleaveContactAndFriction ? 3 : 1;
@@ -164,36 +157,36 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 		m_bSplit.resize(numConstraintRows);
 		m_b.setZero();
 		m_bSplit.setZero();
-		for (int i=0;i<numConstraintRows ;i++)
+		for (int i = 0; i < numConstraintRows; i++)
 		{
 			btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
 			if (!btFuzzyZero(jacDiag))
 			{
 				btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
 				btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
-				m_b[i]=rhs/jacDiag;
-				m_bSplit[i] = rhsPenetration/jacDiag;
+				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);
- 
+
 	{
 		BT_PROFILE("init lo/ho");
 
-		for (int i=0;i<numConstraintRows;i++)
+		for (int i = 0; i < numConstraintRows; i++)
 		{
-			if (0)//m_limitDependencies[i]>=0)
+			if (0)  //m_limitDependencies[i]>=0)
 			{
 				m_lo[i] = -BT_INFINITY;
 				m_hi[i] = BT_INFINITY;
-			} else
+			}
+			else
 			{
 				m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
 				m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
@@ -202,48 +195,48 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 	}
 
 	//
-	int m=m_allConstraintPtrArray.size();
+	int m = m_allConstraintPtrArray.size();
 
 	int numBodies = m_tmpSolverBodyPool.size();
 	btAlignedObjectArray<int> bodyJointNodeArray;
 	{
 		BT_PROFILE("bodyJointNodeArray.resize");
-		bodyJointNodeArray.resize(numBodies,-1);
+		bodyJointNodeArray.resize(numBodies, -1);
 	}
 	btAlignedObjectArray<btJointNode> jointNodeArray;
 	{
 		BT_PROFILE("jointNodeArray.reserve");
-		jointNodeArray.reserve(2*m_allConstraintPtrArray.size());
+		jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
 	}
 
-	static btMatrixXu J3;
+	btMatrixXu& J3 = m_scratchJ3;
 	{
 		BT_PROFILE("J3.resize");
-		J3.resize(2*m,8);
+		J3.resize(2 * m, 8);
 	}
-	static btMatrixXu JinvM3;
+	btMatrixXu& JinvM3 = m_scratchJInvM3;
 	{
 		BT_PROFILE("JinvM3.resize/setZero");
 
-		JinvM3.resize(2*m,8);
+		JinvM3.resize(2 * m, 8);
 		JinvM3.setZero();
 		J3.setZero();
 	}
-	int cur=0;
+	int cur = 0;
 	int rowOffset = 0;
-	static btAlignedObjectArray<int> ofs;
+	btAlignedObjectArray<int>& ofs = m_scratchOfs;
 	{
 		BT_PROFILE("ofs resize");
 		ofs.resize(0);
 		ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
-	}				
+	}
 	{
 		BT_PROFILE("Compute J and JinvM");
-		int c=0;
+		int c = 0;
 
 		int numRows = 0;
 
-		for (int i=0;i<m_allConstraintPtrArray.size();i+=numRows,c++)
+		for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
 		{
 			ofs[c] = rowOffset;
 			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
@@ -251,14 +244,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 			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 ;
+			numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
 			if (orgBodyA)
 			{
 				{
-					int slotA=-1;
+					int slotA = -1;
 					//find free jointNode slot for sbA
-					slotA =jointNodeArray.size();
-					jointNodeArray.expand();//NonInitializing();
+					slotA = jointNodeArray.size();
+					jointNodeArray.expand();  //NonInitializing();
 					int prevSlot = bodyJointNodeArray[sbA];
 					bodyJointNodeArray[sbA] = slotA;
 					jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
@@ -266,35 +259,35 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 					jointNodeArray[slotA].constraintRowIndex = i;
 					jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
 				}
-				for (int row=0;row<numRows;row++,cur++)
+				for (int row = 0; row < numRows; row++, cur++)
 				{
-					btVector3 normalInvMass =				m_allConstraintPtrArray[i+row]->m_contactNormal1 *		orgBodyA->getInvMass();
-					btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal *	orgBodyA->getInvInertiaTensorWorld();
+					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++)
+					for (int r = 0; r < 3; r++)
 					{
-						J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]);
-						J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]);
-						JinvM3.setElem(cur,r,normalInvMass[r]);
-						JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]);
+						J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]);
+						J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]);
+						JinvM3.setElem(cur, r, normalInvMass[r]);
+						JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
 					}
-					J3.setElem(cur,3,0);
-					JinvM3.setElem(cur,3,0);
-					J3.setElem(cur,7,0);
-					JinvM3.setElem(cur,7,0);
+					J3.setElem(cur, 3, 0);
+					JinvM3.setElem(cur, 3, 0);
+					J3.setElem(cur, 7, 0);
+					JinvM3.setElem(cur, 7, 0);
 				}
-			} else
+			}
+			else
 			{
 				cur += numRows;
 			}
 			if (orgBodyB)
 			{
-
 				{
-					int slotB=-1;
+					int slotB = -1;
 					//find free jointNode slot for sbA
-					slotB =jointNodeArray.size();
-					jointNodeArray.expand();//NonInitializing();
+					slotB = jointNodeArray.size();
+					jointNodeArray.expand();  //NonInitializing();
 					int prevSlot = bodyJointNodeArray[sbB];
 					bodyJointNodeArray[sbB] = slotB;
 					jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
@@ -303,78 +296,74 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 					jointNodeArray[slotB].constraintRowIndex = i;
 				}
 
-				for (int row=0;row<numRows;row++,cur++)
+				for (int row = 0; row < numRows; row++, cur++)
 				{
-					btVector3 normalInvMassB = m_allConstraintPtrArray[i+row]->m_contactNormal2*orgBodyB->getInvMass();
-					btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
+					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++)
+					for (int r = 0; r < 3; r++)
 					{
-						J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]);
-						J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]);
-						JinvM3.setElem(cur,r,normalInvMassB[r]);
-						JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]);
+						J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]);
+						J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]);
+						JinvM3.setElem(cur, r, normalInvMassB[r]);
+						JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
 					}
-					J3.setElem(cur,3,0);
-					JinvM3.setElem(cur,3,0);
-					J3.setElem(cur,7,0);
-					JinvM3.setElem(cur,7,0);
+					J3.setElem(cur, 3, 0);
+					JinvM3.setElem(cur, 3, 0);
+					J3.setElem(cur, 7, 0);
+					JinvM3.setElem(cur, 7, 0);
 				}
 			}
 			else
 			{
 				cur += numRows;
 			}
-			rowOffset+=numRows;
-
+			rowOffset += numRows;
 		}
-		
 	}
 
-
 	//compute JinvM = J*invM.
 	const btScalar* JinvM = JinvM3.getBufferPointer();
 
 	const btScalar* Jptr = J3.getBufferPointer();
 	{
 		BT_PROFILE("m_A.resize");
-		m_A.resize(n,n);
+		m_A.resize(n, n);
 	}
-	
+
 	{
 		BT_PROFILE("m_A.setZero");
 		m_A.setZero();
 	}
-	int c=0;
+	int c = 0;
 	{
 		int numRows = 0;
 		BT_PROFILE("Compute A");
-		for (int i=0;i<m_allConstraintPtrArray.size();i+= numRows,c++)
+		for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
 		{
 			int row__ = ofs[c];
 			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
 			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
-		//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
-		//	btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+			//	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;
 
-			numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ;
-					
-			const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
+			const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
 
 			{
 				int startJointNodeA = bodyJointNodeArray[sbA];
-				while (startJointNodeA>=0)
+				while (startJointNodeA >= 0)
 				{
 					int j0 = jointNodeArray[startJointNodeA].jointIndex;
 					int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
-					if (j0<c)
+					if (j0 < c)
 					{
-								 
 						int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
-						size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8*numRowsOther  : 0;
+						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]);
+						m_A.multiplyAdd2_p8r(JinvMrow,
+											 Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
 					}
 					startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
 				}
@@ -382,17 +371,17 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 
 			{
 				int startJointNodeB = bodyJointNodeArray[sbB];
-				while (startJointNodeB>=0)
+				while (startJointNodeB >= 0)
 				{
 					int j1 = jointNodeArray[startJointNodeB].jointIndex;
 					int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
 
-					if (j1<c)
+					if (j1 < c)
 					{
-						int numRowsOther =  cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
-						size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8*numRowsOther  : 0;
-						m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, 
-						Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]);
+						int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
+						size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0;
+						m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows,
+											 Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
 					}
 					startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
 				}
@@ -403,27 +392,25 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 			BT_PROFILE("compute diagonal");
 			// compute diagonal blocks of m_A
 
-			int  row__ = 0;
+			int row__ = 0;
 			int numJointRows = m_allConstraintPtrArray.size();
 
-			int jj=0;
-			for (;row__<numJointRows;)
+			int jj = 0;
+			for (; row__ < numJointRows;)
 			{
-
 				//int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
 				int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
-			//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+				//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
 				btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 
+				const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
 
-				const unsigned int infom =  row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
-				
-				const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
-				const btScalar *Jrow = Jptr + 2*8*(size_t)row__;
-				m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__);
-				if (orgBodyB) 
+				const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
+				const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__;
+				m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__);
+				if (orgBodyB)
 				{
-					m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom,  row__,row__);
+					m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
 				}
 				row__ += infom;
 				jj++;
@@ -434,12 +421,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 	if (1)
 	{
 		// add cfm to the diagonal of m_A
-		for ( int i=0; i<m_A.rows(); ++i) 
+		for (int i = 0; i < m_A.rows(); ++i)
 		{
-			m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
+			m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
 		}
 	}
-				   
+
 	///fill the upper triangle of the matrix, to make it symmetric
 	{
 		BT_PROFILE("fill the upper triangle ");
@@ -451,21 +438,21 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 		m_x.resize(numConstraintRows);
 		m_xSplit.resize(numConstraintRows);
 
-		if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
+		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 		{
-			for (int i=0;i<m_allConstraintPtrArray.size();i++)
+			for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
 			{
 				const btSolverConstraint& c = *m_allConstraintPtrArray[i];
-				m_x[i]=c.m_appliedImpulse;
+				m_x[i] = c.m_appliedImpulse;
 				m_xSplit[i] = c.m_appliedPushImpulse;
 			}
-		} else
+		}
+		else
 		{
 			m_x.setZero();
 			m_xSplit.setZero();
 		}
 	}
-
 }
 
 void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
@@ -476,120 +463,116 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 	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++)
+	for (int i = 0; i < numConstraintRows; i++)
 	{
 		if (m_allConstraintPtrArray[i]->m_jacDiagABInv)
 		{
-			m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv;
+			m_b[i] = m_allConstraintPtrArray[i]->m_rhs / m_allConstraintPtrArray[i]->m_jacDiagABInv;
 			if (infoGlobal.m_splitImpulse)
-				m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv;
+				m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration / m_allConstraintPtrArray[i]->m_jacDiagABInv;
 		}
 	}
- 
-	static btMatrixXu Minv;
-	Minv.resize(6*numBodies,6*numBodies);
+
+	btMatrixXu& Minv = m_scratchMInv;
+	Minv.resize(6 * numBodies, 6 * numBodies);
 	Minv.setZero();
-	for (int i=0;i<numBodies;i++)
+	for (int i = 0; i < numBodies; i++)
 	{
 		const btSolverBody& rb = m_tmpSolverBodyPool[i];
 		const btVector3& invMass = rb.m_invMass;
-		setElem(Minv,i*6+0,i*6+0,invMass[0]);
-		setElem(Minv,i*6+1,i*6+1,invMass[1]);
-		setElem(Minv,i*6+2,i*6+2,invMass[2]);
+		setElem(Minv, i * 6 + 0, i * 6 + 0, invMass[0]);
+		setElem(Minv, i * 6 + 1, i * 6 + 1, invMass[1]);
+		setElem(Minv, i * 6 + 2, i * 6 + 2, invMass[2]);
 		btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody;
- 
-		for (int r=0;r<3;r++)
-			for (int c=0;c<3;c++)
-				setElem(Minv,i*6+3+r,i*6+3+c,orgBody? orgBody->getInvInertiaTensorWorld()[r][c] : 0);
+
+		for (int r = 0; r < 3; r++)
+			for (int c = 0; c < 3; c++)
+				setElem(Minv, i * 6 + 3 + r, i * 6 + 3 + c, orgBody ? orgBody->getInvInertiaTensorWorld()[r][c] : 0);
 	}
- 
-	static btMatrixXu J;
-	J.resize(numConstraintRows,6*numBodies);
+
+	btMatrixXu& J = m_scratchJ;
+	J.resize(numConstraintRows, 6 * numBodies);
 	J.setZero();
- 
+
 	m_lo.resize(numConstraintRows);
 	m_hi.resize(numConstraintRows);
- 
-	for (int i=0;i<numConstraintRows;i++)
-	{
 
+	for (int i = 0; i < numConstraintRows; i++)
+	{
 		m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
 		m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
- 
+
 		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_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]);
+			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_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]);
+			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]);
 		}
 	}
- 
-	static btMatrixXu J_transpose;
-	J_transpose= J.transpose();
 
-	static btMatrixXu tmp;
+	btMatrixXu& J_transpose = m_scratchJTranspose;
+	J_transpose = J.transpose();
+
+	btMatrixXu& tmp = m_scratchTmp;
 
 	{
 		{
 			BT_PROFILE("J*Minv");
-			tmp = J*Minv;
-
+			tmp = J * Minv;
 		}
 		{
 			BT_PROFILE("J*tmp");
-			m_A = tmp*J_transpose;
+			m_A = tmp * J_transpose;
 		}
 	}
 
 	if (1)
 	{
 		// add cfm to the diagonal of m_A
-		for ( int i=0; i<m_A.rows(); ++i) 
+		for (int i = 0; i < m_A.rows(); ++i)
 		{
-			m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
+			m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
 		}
 	}
 
 	m_x.resize(numConstraintRows);
 	if (infoGlobal.m_splitImpulse)
 		m_xSplit.resize(numConstraintRows);
-//	m_x.setZero();
+	//	m_x.setZero();
 
-	for (int i=0;i<m_allConstraintPtrArray.size();i++)
+	for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
 	{
 		const btSolverConstraint& c = *m_allConstraintPtrArray[i];
-		m_x[i]=c.m_appliedImpulse;
+		m_x[i] = c.m_appliedImpulse;
 		if (infoGlobal.m_splitImpulse)
 			m_xSplit[i] = c.m_appliedPushImpulse;
 	}
-
 }
 
-
-btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	bool result = true;
 	{
 		BT_PROFILE("solveMLCP");
-//		printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
+		//		printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
 		result = solveMLCP(infoGlobal);
 	}
 
@@ -597,44 +580,41 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
 	if (result)
 	{
 		BT_PROFILE("process MLCP results");
-		for (int i=0;i<m_allConstraintPtrArray.size();i++)
+		for (int i = 0; i < m_allConstraintPtrArray.size(); 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* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 
 				btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
 				btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
- 
+
 				{
-					btScalar deltaImpulse = m_x[i]-c.m_appliedImpulse;
+					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);
+					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)
 				{
 					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);
+					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];
 				}
-				
 			}
 		}
 	}
 	else
 	{
-	//	printf("m_fallback = %d\n",m_fallback);
+		//	printf("m_fallback = %d\n",m_fallback);
 		m_fallback++;
-		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 	}
 
 	return 0.f;
 }
-
-
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
index 43e85445bad..510ae59e58b 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
@@ -23,15 +23,13 @@ subject to the following restrictions:
 
 class btMLCPSolver : public btSequentialImpulseConstraintSolver
 {
-
 protected:
-	
 	btMatrixXu m_A;
 	btVectorXu m_b;
 	btVectorXu m_x;
 	btVectorXu m_lo;
 	btVectorXu m_hi;
-	
+
 	///when using 'split impulse' we solve two separate (M)LCPs
 	btVectorXu m_bSplit;
 	btVectorXu m_xSplit;
@@ -39,14 +37,23 @@ protected:
 	btVectorXu m_xSplit2;
 
 	btAlignedObjectArray<int> m_limitDependencies;
-	btAlignedObjectArray<btSolverConstraint*>	m_allConstraintPtrArray;
+	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);
+	/// The following scratch variables are not stateful -- contents are cleared prior to each use.
+	/// They are only cached here to avoid extra memory allocations and deallocations and to ensure
+	/// that multiple instances of the solver can be run in parallel.
+	btMatrixXu m_scratchJ3;
+	btMatrixXu m_scratchJInvM3;
+	btAlignedObjectArray<int> m_scratchOfs;
+	btMatrixXu m_scratchMInv;
+	btMatrixXu m_scratchJ;
+	btMatrixXu m_scratchJTranspose;
+	btMatrixXu m_scratchTmp;
 
+	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);
@@ -55,8 +62,7 @@ protected:
 	virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
 
 public:
-
-	btMLCPSolver(	 btMLCPSolverInterface* solver);
+	btMLCPSolver(btMLCPSolverInterface* solver);
 	virtual ~btMLCPSolver();
 
 	void setMLCPSolver(btMLCPSolverInterface* solver)
@@ -73,21 +79,10 @@ public:
 		m_fallback = num;
 	}
 
-	btScalar	getCfm() const
-	{
-		return m_cfm;
-	}
-	void setCfm(btScalar cfm)
-	{
-		m_cfm = cfm;
-	}
-
-	virtual btConstraintSolverType	getSolverType() const
+	virtual btConstraintSolverType getSolverType() const
 	{
 		return BT_MLCP_SOLVER;
 	}
-
 };
 
-
-#endif //BT_MLCP_SOLVER_H
+#endif  //BT_MLCP_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h
index 25bb3f6d327..6b0465b88d7 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h
@@ -27,7 +27,7 @@ public:
 	}
 
 	//return true is it solves the problem successfully
-	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)=0;
+	virtual bool solveMLCP(const btMatrixXu& A, const btVectorXu& b, btVectorXu& x, const btVectorXu& lo, const btVectorXu& hi, const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true) = 0;
 };
 
-#endif //BT_MLCP_SOLVER_INTERFACE_H
+#endif  //BT_MLCP_SOLVER_INTERFACE_H
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h
index 9ec31a6d4e4..7f8eec3f6e3 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h
@@ -14,38 +14,35 @@ subject to the following restrictions:
 */
 ///original version written by Erwin Coumans, October 2013
 
-
 #ifndef BT_PATH_SOLVER_H
 #define BT_PATH_SOLVER_H
 
 //#define BT_USE_PATH
 #ifdef BT_USE_PATH
 
-extern "C" {
+extern "C"
+{
 #include "PATH/SimpleLCP.h"
 #include "PATH/License.h"
 #include "PATH/Error_Interface.h"
 };
-  void __stdcall MyError(Void *data, Char *msg)
+void __stdcall MyError(Void *data, Char *msg)
 {
-	printf("Path Error: %s\n",msg);
+	printf("Path Error: %s\n", msg);
 }
-  void __stdcall MyWarning(Void *data, Char *msg)
+void __stdcall MyWarning(Void *data, Char *msg)
 {
-	printf("Path Warning: %s\n",msg);
+	printf("Path Warning: %s\n", msg);
 }
 
 Error_Interface e;
 
-
-
 #include "btMLCPSolverInterface.h"
 #include "Dantzig/lcp.h"
 
 class btPathSolver : public btMLCPSolverInterface
 {
 public:
-
 	btPathSolver()
 	{
 		License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0");
@@ -55,17 +52,15 @@ public:
 		Error_SetInterface(&e);
 	}
 
-
-	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	virtual bool solveMLCP(const btMatrixXu &A, const btVectorXu &b, btVectorXu &x, const btVectorXu &lo, const btVectorXu &hi, const btAlignedObjectArray<int> &limitDependency, int numIterations, bool useSparsity = true)
 	{
 		MCP_Termination status;
-		
 
 		int numVariables = b.rows();
-		if (0==numVariables)
+		if (0 == numVariables)
 			return true;
 
-			/*	 - variables - the number of variables in the problem
+		/*	 - variables - the number of variables in the problem
 			- m_nnz - the number of nonzeros in the M matrix
 			- m_i - a vector of size m_nnz containing the row indices for M
 			- m_j - a vector of size m_nnz containing the column indices for M
@@ -78,16 +73,16 @@ public:
 		btAlignedObjectArray<int> rowIndices;
 		btAlignedObjectArray<int> colIndices;
 
-		for (int i=0;i<A.rows();i++)
+		for (int i = 0; i < A.rows(); i++)
 		{
-			for (int j=0;j<A.cols();j++)
+			for (int j = 0; j < A.cols(); j++)
 			{
-				if (A(i,j)!=0.f)
+				if (A(i, j) != 0.f)
 				{
 					//add 1, because Path starts at 1, instead of 0
-					rowIndices.push_back(i+1);
-					colIndices.push_back(j+1);
-					values.push_back(A(i,j));
+					rowIndices.push_back(i + 1);
+					colIndices.push_back(j + 1);
+					values.push_back(A(i, j));
 				}
 			}
 		}
@@ -97,19 +92,18 @@ public:
 		btAlignedObjectArray<double> rhs;
 		btAlignedObjectArray<double> upperBounds;
 		btAlignedObjectArray<double> lowerBounds;
-		for (int i=0;i<numVariables;i++)
+		for (int i = 0; i < numVariables; i++)
 		{
 			upperBounds.push_back(hi[i]);
 			lowerBounds.push_back(lo[i]);
 			rhs.push_back(-b[i]);
 		}
 
-
-		SimpleLCP(numVariables,numNonZero,&rowIndices[0],&colIndices[0],&values[0],&rhs[0],&lowerBounds[0],&upperBounds[0], &status, &zResult[0]);
+		SimpleLCP(numVariables, numNonZero, &rowIndices[0], &colIndices[0], &values[0], &rhs[0], &lowerBounds[0], &upperBounds[0], &status, &zResult[0]);
 
 		if (status != MCP_Solved)
 		{
-			static const char* gReturnMsgs[] = {
+			static const char *gReturnMsgs[] = {
 				"Invalid return",
 				"MCP_Solved: The problem was solved",
 				"MCP_NoProgress: A stationary point was found",
@@ -122,16 +116,16 @@ public:
 				"MCP_Infeasible: Problem has no solution",
 				"MCP_Error: An error occurred within the code",
 				"MCP_LicenseError: License could not be found",
-				"MCP_OK"
-			};
+				"MCP_OK"};
 
-			printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl;
+			printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);  // << std::endl;
 			printf("using Projected Gauss Seidel fallback\n");
-			
+
 			return false;
-		} else
+		}
+		else
 		{
-			for (int i=0;i<numVariables;i++)
+			for (int i = 0; i < numVariables; i++)
 			{
 				x[i] = zResult[i];
 				//check for #NAN
@@ -139,13 +133,10 @@ public:
 					return false;
 			}
 			return true;
-
 		}
-
 	}
 };
 
-#endif //BT_USE_PATH
-
+#endif  //BT_USE_PATH
 
-#endif //BT_PATH_SOLVER_H
+#endif  //BT_PATH_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
index 77cc57c6e0e..c3f4ec39974 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
@@ -17,14 +17,22 @@ subject to the following restrictions:
 #ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
 #define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
 
-
 #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)
+	btScalar m_leastSquaresResidualThreshold;
+	btScalar m_leastSquaresResidual;
+
+	btSolveProjectedGaussSeidel()
+		: m_leastSquaresResidualThreshold(0),
+		  m_leastSquaresResidual(0)
+	{
+	}
+
+	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;
@@ -35,52 +43,65 @@ public:
 		btAssert(A.rows() == b.rows());
 
 		int i, j, numRows = A.rows();
-	
-		float delta;
 
-		for (int k = 0; k <numIterations; k++)
+		btScalar delta;
+
+		for (int k = 0; k < numIterations; k++)
 		{
-			for (i = 0; i <numRows; i++)
+			m_leastSquaresResidual = 0.f;
+			for (i = 0; i < numRows; i++)
 			{
 				delta = 0.0f;
 				if (useSparsity)
 				{
-					for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
+					for (int h = 0; h < A.m_rowNonZeroElements1[i].size(); h++)
 					{
-						int j = A.m_rowNonZeroElements1[i][h];
-						if (j != i)//skip main diagonal
+						j = A.m_rowNonZeroElements1[i][h];
+						if (j != i)  //skip main diagonal
 						{
-							delta += A(i,j) * x[j];
+							delta += A(i, j) * x[j];
 						}
 					}
-				} else
+				}
+				else
 				{
-					for (j = 0; j <i; j++) 
-						delta += A(i,j) * x[j];
-					for (j = i+1; j<numRows; j++) 
-						delta += A(i,j) * x[j];
+					for (j = 0; j < i; j++)
+						delta += A(i, j) * x[j];
+					for (j = i + 1; j < numRows; j++)
+						delta += A(i, j) * x[j];
 				}
 
-				float aDiag = A(i,i);
-				x [i] = (b [i] - delta) / aDiag;
-				float s = 1.f;
+				btScalar aDiag = A(i, i);
+				btScalar xOld = x[i];
+				x[i] = (b[i] - delta) / aDiag;
+				btScalar s = 1.f;
 
-				if (limitDependency[i]>=0)
+				if (limitDependency[i] >= 0)
 				{
 					s = x[limitDependency[i]];
-					if (s<0)
-						s=1;
+					if (s < 0)
+						s = 1;
 				}
-			
-				if (x[i]<lo[i]*s)
-					x[i]=lo[i]*s;
-				if (x[i]>hi[i]*s)
-					x[i]=hi[i]*s;
+
+				if (x[i] < lo[i] * s)
+					x[i] = lo[i] * s;
+				if (x[i] > hi[i] * s)
+					x[i] = hi[i] * s;
+				btScalar diff = x[i] - xOld;
+				m_leastSquaresResidual += diff * diff;
+			}
+
+			btScalar eps = m_leastSquaresResidualThreshold;
+			if ((m_leastSquaresResidual < eps) || (k >= (numIterations - 1)))
+			{
+#ifdef VERBOSE_PRINTF_RESIDUAL
+				printf("totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual, k);
+#endif
+				break;
 			}
 		}
 		return true;
 	}
-
 };
 
-#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
+#endif  //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
index a7b1688469f..fc70d8e637d 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
@@ -24,17 +24,16 @@
 
 #define ROLLING_INFLUENCE_FIX
 
-
 btRigidBody& btActionInterface::getFixedBody()
 {
-	static btRigidBody s_fixed(0, 0,0);
-	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+	static btRigidBody s_fixed(0, 0, 0);
+	s_fixed.setMassProps(btScalar(0.), btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
 	return s_fixed;
 }
 
-btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,	btVehicleRaycaster* raycaster )
-:m_vehicleRaycaster(raycaster),
-m_pitchControl(btScalar(0.))
+btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning, btRigidBody* chassis, btVehicleRaycaster* raycaster)
+	: m_vehicleRaycaster(raycaster),
+	  m_pitchControl(btScalar(0.))
 {
 	m_chassisBody = chassis;
 	m_indexRightAxis = 0;
@@ -43,28 +42,22 @@ m_pitchControl(btScalar(0.))
 	defaultInit(tuning);
 }
 
-
 void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning)
 {
 	(void)tuning;
 	m_currentVehicleSpeedKmHour = btScalar(0.);
 	m_steeringValue = btScalar(0.);
-	
 }
 
-	
-
 btRaycastVehicle::~btRaycastVehicle()
 {
 }
 
-
 //
 // basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed
 //
-btWheelInfo&	btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel)
+btWheelInfo& btRaycastVehicle::addWheel(const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0, const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius, const btVehicleTuning& tuning, bool isFrontWheel)
 {
-
 	btWheelInfoConstructionInfo ci;
 
 	ci.m_chassisConnectionCS = connectionPointCS;
@@ -80,76 +73,76 @@ btWheelInfo&	btRaycastVehicle::addWheel( const btVector3& connectionPointCS, con
 	ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
 	ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce;
 
-	m_wheelInfo.push_back( btWheelInfo(ci));
-	
-	btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1];
-	
-	updateWheelTransformsWS( wheel , false );
-	updateWheelTransform(getNumWheels()-1,false);
-	return wheel;
-}
-
+	m_wheelInfo.push_back(btWheelInfo(ci));
 
+	btWheelInfo& wheel = m_wheelInfo[getNumWheels() - 1];
 
+	updateWheelTransformsWS(wheel, false);
+	updateWheelTransform(getNumWheels() - 1, false);
+	return wheel;
+}
 
-const btTransform&	btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
+const btTransform& btRaycastVehicle::getWheelTransformWS(int wheelIndex) const
 {
 	btAssert(wheelIndex < getNumWheels());
 	const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
 	return wheel.m_worldTransform;
-
 }
 
-void	btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform)
+void btRaycastVehicle::updateWheelTransform(int wheelIndex, bool interpolatedTransform)
 {
-	
-	btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
-	updateWheelTransformsWS(wheel,interpolatedTransform);
+	btWheelInfo& wheel = m_wheelInfo[wheelIndex];
+	updateWheelTransformsWS(wheel, interpolatedTransform);
 	btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
 	const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
 	btVector3 fwd = up.cross(right);
 	fwd = fwd.normalize();
-//	up = right.cross(fwd);
-//	up.normalize();
+	//	up = right.cross(fwd);
+	//	up.normalize();
 
 	//rotate around steering over de wheelAxleWS
 	btScalar steering = wheel.m_steering;
-	
-	btQuaternion steeringOrn(up,steering);//wheel.m_steering);
+
+	btQuaternion steeringOrn(up, steering);  //wheel.m_steering);
 	btMatrix3x3 steeringMat(steeringOrn);
 
-	btQuaternion rotatingOrn(right,-wheel.m_rotation);
+	btQuaternion rotatingOrn(right, -wheel.m_rotation);
 	btMatrix3x3 rotatingMat(rotatingOrn);
 
-	btMatrix3x3 basis2(
-		right[0],fwd[0],up[0],
-		right[1],fwd[1],up[1],
-		right[2],fwd[2],up[2]
-	);
-	
+	btMatrix3x3 basis2;
+	basis2[0][m_indexRightAxis] = -right[0];
+	basis2[1][m_indexRightAxis] = -right[1];
+	basis2[2][m_indexRightAxis] = -right[2];
+
+	basis2[0][m_indexUpAxis] = up[0];
+	basis2[1][m_indexUpAxis] = up[1];
+	basis2[2][m_indexUpAxis] = up[2];
+
+	basis2[0][m_indexForwardAxis] = fwd[0];
+	basis2[1][m_indexForwardAxis] = fwd[1];
+	basis2[2][m_indexForwardAxis] = fwd[2];
+
 	wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
 	wheel.m_worldTransform.setOrigin(
-		wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
-	);
+		wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength);
 }
 
 void btRaycastVehicle::resetSuspension()
 {
-
 	int i;
-	for (i=0;i<m_wheelInfo.size();	i++)
+	for (i = 0; i < m_wheelInfo.size(); i++)
 	{
-			btWheelInfo& wheel = m_wheelInfo[i];
-			wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
-			wheel.m_suspensionRelativeVelocity = btScalar(0.0);
-			
-			wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
-			//wheel_info.setContactFriction(btScalar(0.0));
-			wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
+		btWheelInfo& wheel = m_wheelInfo[i];
+		wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
+		wheel.m_suspensionRelativeVelocity = btScalar(0.0);
+
+		wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
+		//wheel_info.setContactFriction(btScalar(0.0));
+		wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
 	}
 }
 
-void	btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform)
+void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel, bool interpolatedTransform)
 {
 	wheel.m_raycastInfo.m_isInContact = false;
 
@@ -159,19 +152,18 @@ void	btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpo
 		getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
 	}
 
-	wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS );
-	wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() *  wheel.m_wheelDirectionCS ;
+	wheel.m_raycastInfo.m_hardPointWS = chassisTrans(wheel.m_chassisConnectionPointCS);
+	wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS;
 	wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
 }
 
 btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 {
-	updateWheelTransformsWS( wheel,false);
+	updateWheelTransformsWS(wheel, false);
 
-	
 	btScalar depth = -1;
-	
-	btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius;
+
+	btScalar raylen = wheel.getSuspensionRestLength() + wheel.m_wheelsRadius;
 
 	btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
 	const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
@@ -179,12 +171,12 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 	const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;
 
 	btScalar param = btScalar(0.);
-	
-	btVehicleRaycaster::btVehicleRaycasterResult	rayResults;
+
+	btVehicleRaycaster::btVehicleRaycasterResult rayResults;
 
 	btAssert(m_vehicleRaycaster);
 
-	void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
+	void* object = m_vehicleRaycaster->castRay(source, target, rayResults);
 
 	wheel.m_raycastInfo.m_groundObject = 0;
 
@@ -192,19 +184,18 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 	{
 		param = rayResults.m_distFraction;
 		depth = raylen * rayResults.m_distFraction;
-		wheel.m_raycastInfo.m_contactNormalWS  = rayResults.m_hitNormalInWorld;
+		wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
 		wheel.m_raycastInfo.m_isInContact = true;
-		
-		wheel.m_raycastInfo.m_groundObject = &getFixedBody();///@todo for driving on dynamic/movable objects!;
-		//wheel.m_raycastInfo.m_groundObject = object;
 
+		wheel.m_raycastInfo.m_groundObject = &getFixedBody();  ///@todo for driving on dynamic/movable objects!;
+		//wheel.m_raycastInfo.m_groundObject = object;
 
-		btScalar hitDistance = param*raylen;
+		btScalar hitDistance = param * raylen;
 		wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
 		//clamp on max suspension travel
 
-		btScalar  minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01);
-		btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01);
+		btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm * btScalar(0.01);
+		btScalar maxSuspensionLength = wheel.getSuspensionRestLength() + wheel.m_maxSuspensionTravelCm * btScalar(0.01);
 		if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
 		{
 			wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
@@ -216,16 +207,16 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 
 		wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;
 
-		btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS );
+		btScalar denominator = wheel.m_raycastInfo.m_contactNormalWS.dot(wheel.m_raycastInfo.m_wheelDirectionWS);
 
 		btVector3 chassis_velocity_at_contactPoint;
-		btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition();
+		btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
 
 		chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos);
 
-		btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
+		btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint);
 
-		if ( denominator >= btScalar(-0.1))
+		if (denominator >= btScalar(-0.1))
 		{
 			wheel.m_suspensionRelativeVelocity = btScalar(0.0);
 			wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
@@ -236,20 +227,19 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 			wheel.m_suspensionRelativeVelocity = projVel * inv;
 			wheel.m_clippedInvContactDotSuspension = inv;
 		}
-			
-	} else
+	}
+	else
 	{
 		//put wheel info as in rest position
 		wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
 		wheel.m_suspensionRelativeVelocity = btScalar(0.0);
-		wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
+		wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
 		wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
 	}
 
 	return depth;
 }
 
-
 const btTransform& btRaycastVehicle::getChassisWorldTransform() const
 {
 	/*if (getRigidBody()->getMotionState())
@@ -260,26 +250,23 @@ const btTransform& btRaycastVehicle::getChassisWorldTransform() const
 	}
 	*/
 
-	
 	return getRigidBody()->getCenterOfMassTransform();
 }
 
-
-void btRaycastVehicle::updateVehicle( btScalar step )
+void btRaycastVehicle::updateVehicle(btScalar step)
 {
 	{
-		for (int i=0;i<getNumWheels();i++)
+		for (int i = 0; i < getNumWheels(); i++)
 		{
-			updateWheelTransform(i,false);
+			updateWheelTransform(i, false);
 		}
 	}
 
-
 	m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length();
-	
+
 	const btTransform& chassisTrans = getChassisWorldTransform();
 
-	btVector3 forwardW (
+	btVector3 forwardW(
 		chassisTrans.getBasis()[0][m_indexForwardAxis],
 		chassisTrans.getBasis()[1][m_indexForwardAxis],
 		chassisTrans.getBasis()[2][m_indexForwardAxis]);
@@ -292,52 +279,47 @@ void btRaycastVehicle::updateVehicle( btScalar step )
 	//
 	// simulate suspension
 	//
-	
-	int i=0;
-	for (i=0;i<m_wheelInfo.size();i++)
+
+	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);
 
-	
-	for (i=0;i<m_wheelInfo.size();i++)
+	for (i = 0; i < m_wheelInfo.size(); i++)
 	{
 		//apply suspension force
 		btWheelInfo& wheel = m_wheelInfo[i];
-		
+
 		btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
-		
+
 		if (suspensionForce > wheel.m_maxSuspensionForce)
 		{
 			suspensionForce = wheel.m_maxSuspensionForce;
 		}
 		btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
 		btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
-		
+
 		getRigidBody()->applyImpulse(impulse, relpos);
-	
 	}
-	
 
-	
-	updateFriction( step);
+	updateFriction(step);
 
-	
-	for (i=0;i<m_wheelInfo.size();i++)
+	for (i = 0; i < m_wheelInfo.size(); i++)
 	{
 		btWheelInfo& wheel = m_wheelInfo[i];
 		btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
-		btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos );
+		btVector3 vel = getRigidBody()->getVelocityInLocalPoint(relpos);
 
 		if (wheel.m_raycastInfo.m_isInContact)
 		{
-			const btTransform&	chassisWorldTransform = getChassisWorldTransform();
+			const btTransform& chassisWorldTransform = getChassisWorldTransform();
 
-			btVector3 fwd (
+			btVector3 fwd(
 				chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
 				chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
 				chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
@@ -346,99 +328,88 @@ void btRaycastVehicle::updateVehicle( btScalar step )
 			fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
 
 			btScalar proj2 = fwd.dot(vel);
-			
+
 			wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
 			wheel.m_rotation += wheel.m_deltaRotation;
-
-		} else
+		}
+		else
 		{
 			wheel.m_rotation += wheel.m_deltaRotation;
 		}
-		
-		wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact
 
+		wheel.m_deltaRotation *= btScalar(0.99);  //damping of rotation when not in contact
 	}
-
-
-
 }
 
-
-void	btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
+void btRaycastVehicle::setSteeringValue(btScalar steering, int wheel)
 {
-	btAssert(wheel>=0 && wheel < getNumWheels());
+	btAssert(wheel >= 0 && wheel < getNumWheels());
 
 	btWheelInfo& wheelInfo = getWheelInfo(wheel);
 	wheelInfo.m_steering = steering;
 }
 
-
-
-btScalar	btRaycastVehicle::getSteeringValue(int wheel) const
+btScalar btRaycastVehicle::getSteeringValue(int wheel) const
 {
 	return getWheelInfo(wheel).m_steering;
 }
 
-
-void	btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
+void btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
 {
-	btAssert(wheel>=0 && wheel < getNumWheels());
+	btAssert(wheel >= 0 && wheel < getNumWheels());
 	btWheelInfo& wheelInfo = getWheelInfo(wheel);
 	wheelInfo.m_engineForce = force;
 }
 
-
-const btWheelInfo&	btRaycastVehicle::getWheelInfo(int index) const
+const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const
 {
-	btAssert((index >= 0) && (index < 	getNumWheels()));
-	
+	btAssert((index >= 0) && (index < getNumWheels()));
+
 	return m_wheelInfo[index];
 }
 
-btWheelInfo&	btRaycastVehicle::getWheelInfo(int index) 
+btWheelInfo& btRaycastVehicle::getWheelInfo(int index)
 {
-	btAssert((index >= 0) && (index < 	getNumWheels()));
-	
+	btAssert((index >= 0) && (index < getNumWheels()));
+
 	return m_wheelInfo[index];
 }
 
-void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex)
+void btRaycastVehicle::setBrake(btScalar brake, int wheelIndex)
 {
-	btAssert((wheelIndex >= 0) && (wheelIndex < 	getNumWheels()));
+	btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels()));
 	getWheelInfo(wheelIndex).m_brake = brake;
 }
 
-
-void	btRaycastVehicle::updateSuspension(btScalar deltaTime)
+void btRaycastVehicle::updateSuspension(btScalar deltaTime)
 {
 	(void)deltaTime;
 
 	btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();
-	
-	for (int w_it=0; w_it<getNumWheels(); w_it++)
+
+	for (int w_it = 0; w_it < getNumWheels(); w_it++)
 	{
-		btWheelInfo &wheel_info = m_wheelInfo[w_it];
-		
-		if ( wheel_info.m_raycastInfo.m_isInContact )
+		btWheelInfo& wheel_info = m_wheelInfo[w_it];
+
+		if (wheel_info.m_raycastInfo.m_isInContact)
 		{
 			btScalar force;
 			//	Spring
 			{
-				btScalar	susp_length			= wheel_info.getSuspensionRestLength();
-				btScalar	current_length = wheel_info.m_raycastInfo.m_suspensionLength;
+				btScalar susp_length = wheel_info.getSuspensionRestLength();
+				btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
 
 				btScalar length_diff = (susp_length - current_length);
 
-				force = wheel_info.m_suspensionStiffness
-					* length_diff * wheel_info.m_clippedInvContactDotSuspension;
+				force = wheel_info.m_suspensionStiffness * length_diff * wheel_info.m_clippedInvContactDotSuspension;
 			}
-		
+
 			// Damper
 			{
 				btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
 				{
-					btScalar	susp_damping;
-					if ( projected_rel_vel < btScalar(0.0) )
+					btScalar susp_damping;
+					if (projected_rel_vel < btScalar(0.0))
 					{
 						susp_damping = wheel_info.m_wheelsDampingCompression;
 					}
@@ -462,50 +433,43 @@ void	btRaycastVehicle::updateSuspension(btScalar deltaTime)
 			wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
 		}
 	}
-
 }
 
-
 struct btWheelContactPoint
 {
 	btRigidBody* m_body0;
 	btRigidBody* m_body1;
-	btVector3	m_frictionPositionWorld;
-	btVector3	m_frictionDirectionWorld;
-	btScalar	m_jacDiagABInv;
-	btScalar	m_maxImpulse;
-
-
-	btWheelContactPoint(btRigidBody* body0,btRigidBody* body1,const btVector3& frictionPosWorld,const btVector3& frictionDirectionWorld, btScalar maxImpulse)
-		:m_body0(body0),
-		m_body1(body1),
-		m_frictionPositionWorld(frictionPosWorld),
-		m_frictionDirectionWorld(frictionDirectionWorld),
-		m_maxImpulse(maxImpulse)
+	btVector3 m_frictionPositionWorld;
+	btVector3 m_frictionDirectionWorld;
+	btScalar m_jacDiagABInv;
+	btScalar m_maxImpulse;
+
+	btWheelContactPoint(btRigidBody* body0, btRigidBody* body1, const btVector3& frictionPosWorld, const btVector3& frictionDirectionWorld, btScalar maxImpulse)
+		: m_body0(body0),
+		  m_body1(body1),
+		  m_frictionPositionWorld(frictionPosWorld),
+		  m_frictionDirectionWorld(frictionDirectionWorld),
+		  m_maxImpulse(maxImpulse)
 	{
-		btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
-		btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
-		btScalar	relaxation = 1.f;
-		m_jacDiagABInv = relaxation/(denom0+denom1);
+		btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld);
+		btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld);
+		btScalar relaxation = 1.f;
+		m_jacDiagABInv = relaxation / (denom0 + denom1);
 	}
-
-
-
 };
 
-btScalar calcRollingFriction(btWheelContactPoint& contactPoint);
-btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
+btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround);
+btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround)
 {
-
-	btScalar j1=0.f;
+	btScalar j1 = 0.f;
 
 	const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld;
 
-	btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); 
+	btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition();
 	btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition();
-	
-	btScalar maxImpulse  = contactPoint.m_maxImpulse;
-	
+
+	btScalar maxImpulse = contactPoint.m_maxImpulse;
+
 	btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1);
 	btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2);
 	btVector3 vel = vel1 - vel2;
@@ -513,252 +477,225 @@ btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
 	btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
 
 	// calculate j that moves us to zero relative velocity
-	j1 = -vrel * contactPoint.m_jacDiagABInv;
+	j1 = -vrel * contactPoint.m_jacDiagABInv / btScalar(numWheelsOnGround);
 	btSetMin(j1, maxImpulse);
 	btSetMax(j1, -maxImpulse);
 
 	return j1;
 }
 
+btScalar sideFrictionStiffness2 = btScalar(1.0);
+void btRaycastVehicle::updateFriction(btScalar timeStep)
+{
+	//calculate the impulse, so that the wheels don't move sidewards
+	int numWheel = getNumWheels();
+	if (!numWheel)
+		return;
 
+	m_forwardWS.resize(numWheel);
+	m_axle.resize(numWheel);
+	m_forwardImpulse.resize(numWheel);
+	m_sideImpulse.resize(numWheel);
 
+	int numWheelsOnGround = 0;
 
-btScalar sideFrictionStiffness2 = btScalar(1.0);
-void	btRaycastVehicle::updateFriction(btScalar	timeStep)
-{
+	//collapse all those loops into one!
+	for (int i = 0; i < getNumWheels(); i++)
+	{
+		btWheelInfo& wheelInfo = m_wheelInfo[i];
+		class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
+		if (groundObject)
+			numWheelsOnGround++;
+		m_sideImpulse[i] = btScalar(0.);
+		m_forwardImpulse[i] = btScalar(0.);
+	}
 
-		//calculate the impulse, so that the wheels don't move sidewards
-		int numWheel = getNumWheels();
-		if (!numWheel)
-			return;
-
-		m_forwardWS.resize(numWheel);
-		m_axle.resize(numWheel);
-		m_forwardImpulse.resize(numWheel);
-		m_sideImpulse.resize(numWheel);
-		
-		int numWheelsOnGround = 0;
-	
-
-		//collapse all those loops into one!
-		for (int i=0;i<getNumWheels();i++)
+	{
+		for (int i = 0; i < getNumWheels(); i++)
 		{
 			btWheelInfo& wheelInfo = m_wheelInfo[i];
-			class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
-			if (groundObject)
-				numWheelsOnGround++;
-			m_sideImpulse[i] = btScalar(0.);
-			m_forwardImpulse[i] = btScalar(0.);
 
-		}
-	
-		{
-	
-			for (int i=0;i<getNumWheels();i++)
+			class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
+
+			if (groundObject)
 			{
+				const btTransform& wheelTrans = getWheelTransformWS(i);
 
-				btWheelInfo& wheelInfo = m_wheelInfo[i];
-					
-				class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
+				btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
+				m_axle[i] = -btVector3(
+					wheelBasis0[0][m_indexRightAxis],
+					wheelBasis0[1][m_indexRightAxis],
+					wheelBasis0[2][m_indexRightAxis]);
 
-				if (groundObject)
-				{
+				const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
+				btScalar proj = m_axle[i].dot(surfNormalWS);
+				m_axle[i] -= surfNormalWS * proj;
+				m_axle[i] = m_axle[i].normalize();
 
-					const btTransform& wheelTrans = getWheelTransformWS( i );
-
-					btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
-					m_axle[i] = btVector3(	
-						wheelBasis0[0][m_indexRightAxis],
-						wheelBasis0[1][m_indexRightAxis],
-						wheelBasis0[2][m_indexRightAxis]);
-					
-					const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
-					btScalar proj = m_axle[i].dot(surfNormalWS);
-					m_axle[i] -= surfNormalWS * proj;
-					m_axle[i] = m_axle[i].normalize();
-					
-					m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
-					m_forwardWS[i].normalize();
-
-				
-					resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
-							  *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
-							  btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);
-
-					m_sideImpulse[i] *= sideFrictionStiffness2;
-						
-				}
-				
+				m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
+				m_forwardWS[i].normalize();
+
+				resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
+									   *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
+									   btScalar(0.), m_axle[i], m_sideImpulse[i], timeStep);
 
+				m_sideImpulse[i] *= sideFrictionStiffness2;
 			}
 		}
+	}
 
 	btScalar sideFactor = btScalar(1.);
 	btScalar fwdFactor = 0.5;
 
 	bool sliding = false;
 	{
-		for (int wheel =0;wheel <getNumWheels();wheel++)
+		for (int wheel = 0; wheel < getNumWheels(); wheel++)
 		{
 			btWheelInfo& wheelInfo = m_wheelInfo[wheel];
-			class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
+			class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
 
-			btScalar	rollingFriction = 0.f;
+			btScalar rollingFriction = 0.f;
 
 			if (groundObject)
 			{
 				if (wheelInfo.m_engineForce != 0.f)
 				{
-					rollingFriction = wheelInfo.m_engineForce* timeStep;
-				} else
+					rollingFriction = wheelInfo.m_engineForce * timeStep;
+				}
+				else
 				{
 					btScalar defaultRollingFrictionImpulse = 0.f;
 					btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
-					btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
-					rollingFriction = calcRollingFriction(contactPt);
+					btWheelContactPoint contactPt(m_chassisBody, groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, m_forwardWS[wheel], maxImpulse);
+					btAssert(numWheelsOnGround > 0);
+					rollingFriction = calcRollingFriction(contactPt, numWheelsOnGround);
 				}
 			}
 
 			//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
-			
-
-
 
 			m_forwardImpulse[wheel] = btScalar(0.);
-			m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
+			m_wheelInfo[wheel].m_skidInfo = btScalar(1.);
 
 			if (groundObject)
 			{
-				m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
-				
+				m_wheelInfo[wheel].m_skidInfo = btScalar(1.);
+
 				btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
 				btScalar maximpSide = maximp;
 
 				btScalar maximpSquared = maximp * maximpSide;
-			
 
-				m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
+				m_forwardImpulse[wheel] = rollingFriction;  //wheelInfo.m_engineForce* timeStep;
 
-				btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
-				btScalar y = (m_sideImpulse[wheel] ) * sideFactor;
-				
-				btScalar impulseSquared = (x*x + y*y);
+				btScalar x = (m_forwardImpulse[wheel]) * fwdFactor;
+				btScalar y = (m_sideImpulse[wheel]) * sideFactor;
+
+				btScalar impulseSquared = (x * x + y * y);
 
 				if (impulseSquared > maximpSquared)
 				{
 					sliding = true;
-					
+
 					btScalar factor = maximp / btSqrt(impulseSquared);
-					
+
 					m_wheelInfo[wheel].m_skidInfo *= factor;
 				}
-			} 
-
+			}
 		}
 	}
 
-	
-
-
-		if (sliding)
+	if (sliding)
+	{
+		for (int wheel = 0; wheel < getNumWheels(); wheel++)
 		{
-			for (int wheel = 0;wheel < getNumWheels(); wheel++)
+			if (m_sideImpulse[wheel] != btScalar(0.))
 			{
-				if (m_sideImpulse[wheel] != btScalar(0.))
+				if (m_wheelInfo[wheel].m_skidInfo < btScalar(1.))
 				{
-					if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
-					{
-						m_forwardImpulse[wheel] *=	m_wheelInfo[wheel].m_skidInfo;
-						m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
-					}
+					m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
+					m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
 				}
 			}
 		}
+	}
 
-		// apply the impulses
+	// apply the impulses
+	{
+		for (int wheel = 0; wheel < getNumWheels(); wheel++)
 		{
-			for (int wheel = 0;wheel<getNumWheels() ; wheel++)
-			{
-				btWheelInfo& wheelInfo = m_wheelInfo[wheel];
+			btWheelInfo& wheelInfo = m_wheelInfo[wheel];
 
-				btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - 
-						m_chassisBody->getCenterOfMassPosition();
+			btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
+								m_chassisBody->getCenterOfMassPosition();
 
-				if (m_forwardImpulse[wheel] != btScalar(0.))
-				{
-					m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
-				}
-				if (m_sideImpulse[wheel] != btScalar(0.))
-				{
-					class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
+			if (m_forwardImpulse[wheel] != btScalar(0.))
+			{
+				m_chassisBody->applyImpulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos);
+			}
+			if (m_sideImpulse[wheel] != btScalar(0.))
+			{
+				class btRigidBody* groundObject = (class btRigidBody*)m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
 
-					btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - 
-						groundObject->getCenterOfMassPosition();
+				btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
+									 groundObject->getCenterOfMassPosition();
 
-					
-					btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
+				btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
 
-#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
-					btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
-					rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence));
+#if defined ROLLING_INFLUENCE_FIX  // fix. It only worked if car's up was along Y - VT.
+				btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
+				rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f - wheelInfo.m_rollInfluence));
 #else
-					rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
+				rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
 #endif
-					m_chassisBody->applyImpulse(sideImp,rel_pos);
+				m_chassisBody->applyImpulse(sideImp, rel_pos);
 
-					//apply friction impulse on the ground
-					groundObject->applyImpulse(-sideImp,rel_pos2);
-				}
+				//apply friction impulse on the ground
+				groundObject->applyImpulse(-sideImp, rel_pos2);
 			}
 		}
-
-	
+	}
 }
 
-
-
-void	btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
+void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
 {
-
-	for (int v=0;v<this->getNumWheels();v++)
+	for (int v = 0; v < this->getNumWheels(); v++)
 	{
-		btVector3 wheelColor(0,1,1);
+		btVector3 wheelColor(0, 1, 1);
 		if (getWheelInfo(v).m_raycastInfo.m_isInContact)
 		{
-			wheelColor.setValue(0,0,1);
-		} else
+			wheelColor.setValue(0, 0, 1);
+		}
+		else
 		{
-			wheelColor.setValue(1,0,1);
+			wheelColor.setValue(1, 0, 1);
 		}
 
 		btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin();
 
-		btVector3 axle = btVector3(	
+		btVector3 axle = btVector3(
 			getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()],
 			getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()],
 			getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]);
 
 		//debug wheels (cylinders)
-		debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
-		debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
-
+		debugDrawer->drawLine(wheelPosWS, wheelPosWS + axle, wheelColor);
+		debugDrawer->drawLine(wheelPosWS, getWheelInfo(v).m_raycastInfo.m_contactPointWS, wheelColor);
 	}
 }
 
-
-void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
+void* btDefaultVehicleRaycaster::castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result)
 {
-//	RayResultCallback& resultCallback;
+	//	RayResultCallback& resultCallback;
 
-	btCollisionWorld::ClosestRayResultCallback rayCallback(from,to);
+	btCollisionWorld::ClosestRayResultCallback rayCallback(from, to);
 
 	m_dynamicsWorld->rayTest(from, to, rayCallback);
 
 	if (rayCallback.hasHit())
 	{
-		
 		const btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
-        if (body && body->hasContactResponse())
+		if (body && body->hasContactResponse())
 		{
 			result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
 			result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
@@ -769,4 +706,3 @@ void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3&
 	}
 	return 0;
 }
-
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
index 82d44c73e05..99d6894e56e 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
@@ -19,127 +19,116 @@ class btDynamicsWorld;
 #include "btWheelInfo.h"
 #include "BulletDynamics/Dynamics/btActionInterface.h"
 
-class btVehicleTuning;
+//class btVehicleTuning;
 
 ///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
 class btRaycastVehicle : public btActionInterface
 {
+	btAlignedObjectArray<btVector3> m_forwardWS;
+	btAlignedObjectArray<btVector3> m_axle;
+	btAlignedObjectArray<btScalar> m_forwardImpulse;
+	btAlignedObjectArray<btScalar> m_sideImpulse;
 
-		btAlignedObjectArray<btVector3>	m_forwardWS;
-		btAlignedObjectArray<btVector3>	m_axle;
-		btAlignedObjectArray<btScalar>	m_forwardImpulse;
-		btAlignedObjectArray<btScalar>	m_sideImpulse;
-	
-		///backwards compatibility
-		int	m_userConstraintType;
-		int	m_userConstraintId;
+	///backwards compatibility
+	int m_userConstraintType;
+	int m_userConstraintId;
 
 public:
 	class btVehicleTuning
+	{
+	public:
+		btVehicleTuning()
+			: m_suspensionStiffness(btScalar(5.88)),
+			  m_suspensionCompression(btScalar(0.83)),
+			  m_suspensionDamping(btScalar(0.88)),
+			  m_maxSuspensionTravelCm(btScalar(500.)),
+			  m_frictionSlip(btScalar(10.5)),
+			  m_maxSuspensionForce(btScalar(6000.))
 		{
-			public:
-
-			btVehicleTuning()
-				:m_suspensionStiffness(btScalar(5.88)),
-				m_suspensionCompression(btScalar(0.83)),
-				m_suspensionDamping(btScalar(0.88)),
-				m_maxSuspensionTravelCm(btScalar(500.)),
-				m_frictionSlip(btScalar(10.5)),
-				m_maxSuspensionForce(btScalar(6000.))
-			{
-			}
-			btScalar	m_suspensionStiffness;
-			btScalar	m_suspensionCompression;
-			btScalar	m_suspensionDamping;
-			btScalar	m_maxSuspensionTravelCm;
-			btScalar	m_frictionSlip;
-			btScalar	m_maxSuspensionForce;
-
-		};
-private:
+		}
+		btScalar m_suspensionStiffness;
+		btScalar m_suspensionCompression;
+		btScalar m_suspensionDamping;
+		btScalar m_maxSuspensionTravelCm;
+		btScalar m_frictionSlip;
+		btScalar m_maxSuspensionForce;
+	};
 
-	btVehicleRaycaster*	m_vehicleRaycaster;
-	btScalar		m_pitchControl;
-	btScalar	m_steeringValue; 
+private:
+	btVehicleRaycaster* m_vehicleRaycaster;
+	btScalar m_pitchControl;
+	btScalar m_steeringValue;
 	btScalar m_currentVehicleSpeedKmHour;
 
 	btRigidBody* m_chassisBody;
 
 	int m_indexRightAxis;
 	int m_indexUpAxis;
-	int	m_indexForwardAxis;
+	int m_indexForwardAxis;
 
 	void defaultInit(const btVehicleTuning& tuning);
 
 public:
-
 	//constructor to create a car from an existing rigidbody
-	btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,	btVehicleRaycaster* raycaster );
-
-	virtual ~btRaycastVehicle() ;
+	btRaycastVehicle(const btVehicleTuning& tuning, btRigidBody* chassis, btVehicleRaycaster* raycaster);
 
+	virtual ~btRaycastVehicle();
 
 	///btActionInterface interface
-	virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step)
+	virtual void updateAction(btCollisionWorld* collisionWorld, btScalar step)
 	{
-        (void) collisionWorld;
+		(void)collisionWorld;
 		updateVehicle(step);
 	}
-	
 
 	///btActionInterface interface
-	void	debugDraw(btIDebugDraw* debugDrawer);
-			
+	void debugDraw(btIDebugDraw* debugDrawer);
+
 	const btTransform& getChassisWorldTransform() const;
-	
+
 	btScalar rayCast(btWheelInfo& wheel);
 
 	virtual void updateVehicle(btScalar step);
-	
-	
+
 	void resetSuspension();
 
-	btScalar	getSteeringValue(int wheel) const;
+	btScalar getSteeringValue(int wheel) const;
 
-	void	setSteeringValue(btScalar steering,int wheel);
+	void setSteeringValue(btScalar steering, int wheel);
 
+	void applyEngineForce(btScalar force, int wheel);
 
-	void	applyEngineForce(btScalar force, int wheel);
+	const btTransform& getWheelTransformWS(int wheelIndex) const;
 
-	const btTransform&	getWheelTransformWS( int wheelIndex ) const;
+	void updateWheelTransform(int wheelIndex, bool interpolatedTransform = true);
 
-	void	updateWheelTransform( int wheelIndex, bool interpolatedTransform = true );
-	
-//	void	setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
+	//	void	setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
 
-	btWheelInfo&	addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel);
+	btWheelInfo& addWheel(const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0, const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius, const btVehicleTuning& tuning, bool isFrontWheel);
 
-	inline int		getNumWheels() const {
-		return int (m_wheelInfo.size());
+	inline int getNumWheels() const
+	{
+		return int(m_wheelInfo.size());
 	}
-	
-	btAlignedObjectArray<btWheelInfo>	m_wheelInfo;
 
+	btAlignedObjectArray<btWheelInfo> m_wheelInfo;
 
-	const btWheelInfo&	getWheelInfo(int index) const;
+	const btWheelInfo& getWheelInfo(int index) const;
 
-	btWheelInfo&	getWheelInfo(int index);
+	btWheelInfo& getWheelInfo(int index);
 
-	void	updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true);
+	void updateWheelTransformsWS(btWheelInfo& wheel, bool interpolatedTransform = true);
 
-	
-	void setBrake(btScalar brake,int wheelIndex);
+	void setBrake(btScalar brake, int wheelIndex);
 
-	void	setPitchControl(btScalar pitch)
+	void setPitchControl(btScalar pitch)
 	{
 		m_pitchControl = pitch;
 	}
-	
-	void	updateSuspension(btScalar deltaTime);
-
-	virtual void	updateFriction(btScalar	timeStep);
 
+	void updateSuspension(btScalar deltaTime);
 
+	virtual void updateFriction(btScalar timeStep);
 
 	inline btRigidBody* getRigidBody()
 	{
@@ -151,7 +140,7 @@ public:
 		return m_chassisBody;
 	}
 
-	inline int	getRightAxis() const
+	inline int getRightAxis() const
 	{
 		return m_indexRightAxis;
 	}
@@ -165,46 +154,44 @@ public:
 		return m_indexForwardAxis;
 	}
 
-	
 	///Worldspace forward vector
 	btVector3 getForwardVector() const
 	{
-		const btTransform& chassisTrans = getChassisWorldTransform(); 
+		const btTransform& chassisTrans = getChassisWorldTransform();
 
-		btVector3 forwardW ( 
-			  chassisTrans.getBasis()[0][m_indexForwardAxis], 
-			  chassisTrans.getBasis()[1][m_indexForwardAxis], 
-			  chassisTrans.getBasis()[2][m_indexForwardAxis]); 
+		btVector3 forwardW(
+			chassisTrans.getBasis()[0][m_indexForwardAxis],
+			chassisTrans.getBasis()[1][m_indexForwardAxis],
+			chassisTrans.getBasis()[2][m_indexForwardAxis]);
 
 		return forwardW;
 	}
 
 	///Velocity of vehicle (positive if velocity vector has same direction as foward vector)
-	btScalar	getCurrentSpeedKmHour() const
+	btScalar getCurrentSpeedKmHour() const
 	{
 		return m_currentVehicleSpeedKmHour;
 	}
 
-	virtual void	setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex)
+	virtual void setCoordinateSystem(int rightIndex, int upIndex, int forwardIndex)
 	{
 		m_indexRightAxis = rightIndex;
 		m_indexUpAxis = upIndex;
 		m_indexForwardAxis = forwardIndex;
 	}
 
-
 	///backwards compatibility
 	int getUserConstraintType() const
 	{
-		return m_userConstraintType ;
+		return m_userConstraintType;
 	}
 
-	void	setUserConstraintType(int userConstraintType)
+	void setUserConstraintType(int userConstraintType)
 	{
 		m_userConstraintType = userConstraintType;
 	};
 
-	void	setUserConstraintId(int uid)
+	void setUserConstraintId(int uid)
 	{
 		m_userConstraintId = uid;
 	}
@@ -213,22 +200,19 @@ public:
 	{
 		return m_userConstraintId;
 	}
-
 };
 
 class btDefaultVehicleRaycaster : public btVehicleRaycaster
 {
-	btDynamicsWorld*	m_dynamicsWorld;
+	btDynamicsWorld* m_dynamicsWorld;
+
 public:
 	btDefaultVehicleRaycaster(btDynamicsWorld* world)
-		:m_dynamicsWorld(world)
+		: m_dynamicsWorld(world)
 	{
 	}
 
-	virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result);
-
+	virtual void* castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result);
 };
 
-
-#endif //BT_RAYCASTVEHICLE_H
-
+#endif  //BT_RAYCASTVEHICLE_H
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
index 3cc909c6530..2c44ce546c7 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
@@ -16,20 +16,18 @@
 /// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting
 struct btVehicleRaycaster
 {
-virtual ~btVehicleRaycaster()
-{
-}
+	virtual ~btVehicleRaycaster()
+	{
+	}
 	struct btVehicleRaycasterResult
 	{
-		btVehicleRaycasterResult() :m_distFraction(btScalar(-1.)){};
-		btVector3	m_hitPointInWorld;
-		btVector3	m_hitNormalInWorld;
-		btScalar	m_distFraction;
+		btVehicleRaycasterResult() : m_distFraction(btScalar(-1.)){};
+		btVector3 m_hitPointInWorld;
+		btVector3 m_hitNormalInWorld;
+		btScalar m_distFraction;
 	};
 
-	virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) = 0;
-
+	virtual void* castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result) = 0;
 };
 
-#endif //BT_VEHICLE_RAYCASTER_H
-
+#endif  //BT_VEHICLE_RAYCASTER_H
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp
index ef93c16fffc..d5c12f223b5 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp
@@ -9,30 +9,26 @@
  * It is provided "as is" without express or implied warranty.
 */
 #include "btWheelInfo.h"
-#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity
-
+#include "BulletDynamics/Dynamics/btRigidBody.h"  // for pointvelocity
 
 btScalar btWheelInfo::getSuspensionRestLength() const
 {
-
 	return m_suspensionRestLength1;
-
 }
 
-void	btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo)
+void btWheelInfo::updateWheel(const btRigidBody& chassis, RaycastInfo& raycastInfo)
 {
 	(void)raycastInfo;
 
-	
 	if (m_raycastInfo.m_isInContact)
 
 	{
-		btScalar	project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS );
-		btVector3	 chassis_velocity_at_contactPoint;
+		btScalar project = m_raycastInfo.m_contactNormalWS.dot(m_raycastInfo.m_wheelDirectionWS);
+		btVector3 chassis_velocity_at_contactPoint;
 		btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition();
-		chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos );
-		btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
-		if ( project >= btScalar(-0.1))
+		chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint(relpos);
+		btScalar projVel = m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint);
+		if (project >= btScalar(-0.1))
 		{
 			m_suspensionRelativeVelocity = btScalar(0.0);
 			m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
@@ -43,10 +39,9 @@ void	btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInf
 			m_suspensionRelativeVelocity = projVel * inv;
 			m_clippedInvContactDotSuspension = inv;
 		}
-		
 	}
 
-	else	// Not in contact : position wheel in a nice (rest length) position
+	else  // Not in contact : position wheel in a nice (rest length) position
 	{
 		m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength();
 		m_suspensionRelativeVelocity = btScalar(0.0);
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h
index f916053ecac..af88b8ff837 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h
@@ -18,20 +18,19 @@ class btRigidBody;
 
 struct btWheelInfoConstructionInfo
 {
-	btVector3	m_chassisConnectionCS;
-	btVector3	m_wheelDirectionCS;
-	btVector3	m_wheelAxleCS;
-	btScalar	m_suspensionRestLength;
-	btScalar	m_maxSuspensionTravelCm;
-	btScalar	m_wheelRadius;
-	
-	btScalar		m_suspensionStiffness;
-	btScalar		m_wheelsDampingCompression;
-	btScalar		m_wheelsDampingRelaxation;
-	btScalar		m_frictionSlip;
-	btScalar		m_maxSuspensionForce;
+	btVector3 m_chassisConnectionCS;
+	btVector3 m_wheelDirectionCS;
+	btVector3 m_wheelAxleCS;
+	btScalar m_suspensionRestLength;
+	btScalar m_maxSuspensionTravelCm;
+	btScalar m_wheelRadius;
+
+	btScalar m_suspensionStiffness;
+	btScalar m_wheelsDampingCompression;
+	btScalar m_wheelsDampingRelaxation;
+	btScalar m_frictionSlip;
+	btScalar m_maxSuspensionForce;
 	bool m_bIsFrontWheel;
-	
 };
 
 /// btWheelInfo contains information per wheel about friction and suspension.
@@ -40,49 +39,50 @@ struct btWheelInfo
 	struct RaycastInfo
 	{
 		//set by raycaster
-		btVector3	m_contactNormalWS;//contactnormal
-		btVector3	m_contactPointWS;//raycast hitpoint
-		btScalar	m_suspensionLength;
-		btVector3	m_hardPointWS;//raycast starting point
-		btVector3	m_wheelDirectionWS; //direction in worldspace
-		btVector3	m_wheelAxleWS; // axle in worldspace
-		bool		m_isInContact;
-		void*		m_groundObject; //could be general void* ptr
+		btVector3 m_contactNormalWS;  //contactnormal
+		btVector3 m_contactPointWS;   //raycast hitpoint
+		btScalar m_suspensionLength;
+		btVector3 m_hardPointWS;       //raycast starting point
+		btVector3 m_wheelDirectionWS;  //direction in worldspace
+		btVector3 m_wheelAxleWS;       // axle in worldspace
+		bool m_isInContact;
+		void* m_groundObject;  //could be general void* ptr
 	};
 
-	RaycastInfo	m_raycastInfo;
+	RaycastInfo m_raycastInfo;
 
-	btTransform	m_worldTransform;
-	
-	btVector3	m_chassisConnectionPointCS; //const
-	btVector3	m_wheelDirectionCS;//const
-	btVector3	m_wheelAxleCS; // const or modified by steering
-	btScalar	m_suspensionRestLength1;//const
-	btScalar	m_maxSuspensionTravelCm;
+	btTransform m_worldTransform;
+
+	btVector3 m_chassisConnectionPointCS;  //const
+	btVector3 m_wheelDirectionCS;          //const
+	btVector3 m_wheelAxleCS;               // const or modified by steering
+	btScalar m_suspensionRestLength1;      //const
+	btScalar m_maxSuspensionTravelCm;
 	btScalar getSuspensionRestLength() const;
-	btScalar	m_wheelsRadius;//const
-	btScalar	m_suspensionStiffness;//const
-	btScalar	m_wheelsDampingCompression;//const
-	btScalar	m_wheelsDampingRelaxation;//const
-	btScalar	m_frictionSlip;
-	btScalar	m_steering;
-	btScalar	m_rotation;
-	btScalar	m_deltaRotation;
-	btScalar	m_rollInfluence;
-	btScalar	m_maxSuspensionForce;
-
-	btScalar	m_engineForce;
-
-	btScalar	m_brake;
-	
+	btScalar m_wheelsRadius;              //const
+	btScalar m_suspensionStiffness;       //const
+	btScalar m_wheelsDampingCompression;  //const
+	btScalar m_wheelsDampingRelaxation;   //const
+	btScalar m_frictionSlip;
+	btScalar m_steering;
+	btScalar m_rotation;
+	btScalar m_deltaRotation;
+	btScalar m_rollInfluence;
+	btScalar m_maxSuspensionForce;
+
+	btScalar m_engineForce;
+
+	btScalar m_brake;
+
 	bool m_bIsFrontWheel;
-	
-	void*		m_clientInfo;//can be used to store pointer to sync transforms...
+
+	void* m_clientInfo;  //can be used to store pointer to sync transforms...
+
+	btWheelInfo() {}
 
 	btWheelInfo(btWheelInfoConstructionInfo& ci)
 
 	{
-
 		m_suspensionRestLength1 = ci.m_suspensionRestLength;
 		m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm;
 
@@ -102,18 +102,15 @@ struct btWheelInfo
 		m_rollInfluence = btScalar(0.1);
 		m_bIsFrontWheel = ci.m_bIsFrontWheel;
 		m_maxSuspensionForce = ci.m_maxSuspensionForce;
-
 	}
 
-	void	updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo);
+	void updateWheel(const btRigidBody& chassis, RaycastInfo& raycastInfo);
 
-	btScalar	m_clippedInvContactDotSuspension;
-	btScalar	m_suspensionRelativeVelocity;
+	btScalar m_clippedInvContactDotSuspension;
+	btScalar m_suspensionRelativeVelocity;
 	//calculated by suspension
-	btScalar	m_wheelsSuspensionForce;
-	btScalar	m_skidInfo;
-
+	btScalar m_wheelsSuspensionForce;
+	btScalar m_skidInfo;
 };
 
-#endif //BT_WHEEL_INFO_H
-
+#endif  //BT_WHEEL_INFO_H
diff --git a/extern/bullet2/src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h b/extern/bullet2/src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h
new file mode 100644
index 00000000000..01c7e93a1b6
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h
@@ -0,0 +1,45 @@
+//
+//  DeformableBodyInplaceSolverIslandCallback.h
+//  BulletSoftBody
+//
+//  Created by Xuchen Han on 12/16/19.
+//
+
+#ifndef DeformableBodyInplaceSolverIslandCallback_h
+#define DeformableBodyInplaceSolverIslandCallback_h
+
+struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolverIslandCallback
+{
+	btDeformableMultiBodyConstraintSolver* m_deformableSolver;
+
+	DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver,
+											  btDispatcher* dispatcher)
+		: MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver)
+	{
+	}
+
+	virtual void processConstraints(int islandId = -1)
+	{
+		btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
+		btCollisionObject** softBodies = m_softBodies.size() ? &m_softBodies[0] : 0;
+		btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
+		btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
+		btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
+
+		//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
+
+		m_deformableSolver->solveDeformableBodyGroup(bodies, m_bodies.size(), softBodies, m_softBodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
+		if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics & 1))
+		{
+			m_deformableSolver->m_analyticsData.m_islandId = islandId;
+			m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+		}
+		m_bodies.resize(0);
+		m_softBodies.resize(0);
+		m_manifolds.resize(0);
+		m_constraints.resize(0);
+		m_multiBodyConstraints.resize(0);
+	}
+};
+
+#endif /* DeformableBodyInplaceSolverIslandCallback_h */
diff --git a/extern/bullet2/src/BulletSoftBody/btCGProjection.h b/extern/bullet2/src/BulletSoftBody/btCGProjection.h
new file mode 100644
index 00000000000..e05970664c2
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btCGProjection.h
@@ -0,0 +1,104 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_CG_PROJECTION_H
+#define BT_CG_PROJECTION_H
+
+#include "btSoftBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+
+struct DeformableContactConstraint
+{
+	const btSoftBody::Node* m_node;
+	btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
+	btAlignedObjectArray<btVector3> m_total_normal_dv;
+	btAlignedObjectArray<btVector3> m_total_tangent_dv;
+	btAlignedObjectArray<bool> m_static;
+	btAlignedObjectArray<bool> m_can_be_dynamic;
+
+	DeformableContactConstraint(const btSoftBody::RContact& rcontact) : m_node(rcontact.m_node)
+	{
+		append(rcontact);
+	}
+
+	DeformableContactConstraint() : m_node(NULL)
+	{
+		m_contact.push_back(NULL);
+	}
+
+	void append(const btSoftBody::RContact& rcontact)
+	{
+		m_contact.push_back(&rcontact);
+		m_total_normal_dv.push_back(btVector3(0, 0, 0));
+		m_total_tangent_dv.push_back(btVector3(0, 0, 0));
+		m_static.push_back(false);
+		m_can_be_dynamic.push_back(true);
+	}
+
+	void replace(const btSoftBody::RContact& rcontact)
+	{
+		m_contact.clear();
+		m_total_normal_dv.clear();
+		m_total_tangent_dv.clear();
+		m_static.clear();
+		m_can_be_dynamic.clear();
+		append(rcontact);
+	}
+
+	~DeformableContactConstraint()
+	{
+	}
+};
+
+class btCGProjection
+{
+public:
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
+	typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
+	btAlignedObjectArray<btSoftBody*>& m_softBodies;
+	const btScalar& m_dt;
+	// map from node indices to node pointers
+	const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
+
+	btCGProjection(btAlignedObjectArray<btSoftBody*>& softBodies, const btScalar& dt)
+		: m_softBodies(softBodies), m_dt(dt)
+	{
+	}
+
+	virtual ~btCGProjection()
+	{
+	}
+
+	// apply the constraints
+	virtual void project(TVStack& x) = 0;
+
+	virtual void setConstraints() = 0;
+
+	// update the constraints
+	virtual btScalar update() = 0;
+
+	virtual void reinitialize(bool nodeUpdated)
+	{
+	}
+
+	virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
+	{
+		m_nodes = nodes;
+	}
+};
+
+#endif /* btCGProjection_h */
diff --git a/extern/bullet2/src/BulletSoftBody/btConjugateGradient.h b/extern/bullet2/src/BulletSoftBody/btConjugateGradient.h
new file mode 100644
index 00000000000..bcd5e6b519d
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btConjugateGradient.h
@@ -0,0 +1,117 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_CONJUGATE_GRADIENT_H
+#define BT_CONJUGATE_GRADIENT_H
+#include "btKrylovSolver.h"
+template <class MatrixX>
+class btConjugateGradient : public btKrylovSolver<MatrixX>
+{
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	typedef btKrylovSolver<MatrixX> Base;
+	TVStack r, p, z, temp;
+
+public:
+	btConjugateGradient(const int max_it_in)
+		: btKrylovSolver<MatrixX>(max_it_in, SIMD_EPSILON)
+	{
+	}
+
+	virtual ~btConjugateGradient() {}
+
+	// return the number of iterations taken
+	int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
+	{
+		BT_PROFILE("CGSolve");
+		btAssert(x.size() == b.size());
+		reinitialize(b);
+		temp = b;
+		A.project(temp);
+		p = temp;
+		A.precondition(p, z);
+		btScalar d0 = this->dot(z, temp);
+		d0 = btMin(btScalar(1), d0);
+		// r = b - A * x --with assigned dof zeroed out
+		A.multiply(x, temp);
+		r = this->sub(b, temp);
+		A.project(r);
+		// z = M^(-1) * r
+		A.precondition(r, z);
+		A.project(z);
+		btScalar r_dot_z = this->dot(z, r);
+		if (r_dot_z <= Base::m_tolerance * d0)
+		{
+			if (verbose)
+			{
+				std::cout << "Iteration = 0" << std::endl;
+				std::cout << "Two norm of the residual = " << r_dot_z << std::endl;
+			}
+			return 0;
+		}
+		p = z;
+		btScalar r_dot_z_new = r_dot_z;
+		for (int k = 1; k <= Base::m_maxIterations; k++)
+		{
+			// temp = A*p
+			A.multiply(p, temp);
+			A.project(temp);
+			if (this->dot(p, temp) < 0)
+			{
+				if (verbose)
+					std::cout << "Encountered negative direction in CG!" << std::endl;
+				if (k == 1)
+				{
+					x = b;
+				}
+				return k;
+			}
+			// alpha = r^T * z / (p^T * A * p)
+			btScalar alpha = r_dot_z_new / this->dot(p, temp);
+			//  x += alpha * p;
+			this->multAndAddTo(alpha, p, x);
+			//  r -= alpha * temp;
+			this->multAndAddTo(-alpha, temp, r);
+			// z = M^(-1) * r
+			A.precondition(r, z);
+			r_dot_z = r_dot_z_new;
+			r_dot_z_new = this->dot(r, z);
+			if (r_dot_z_new < Base::m_tolerance * d0)
+			{
+				if (verbose)
+				{
+					std::cout << "ConjugateGradient iterations " << k << " residual = " << r_dot_z_new << std::endl;
+				}
+				return k;
+			}
+
+			btScalar beta = r_dot_z_new / r_dot_z;
+			p = this->multAndAdd(beta, p, z);
+		}
+		if (verbose)
+		{
+			std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << " error = " << r_dot_z_new << std::endl;
+		}
+		return Base::m_maxIterations;
+	}
+
+	void reinitialize(const TVStack& b)
+	{
+		r.resize(b.size());
+		p.resize(b.size());
+		z.resize(b.size());
+		temp.resize(b.size());
+	}
+};
+#endif /* btConjugateGradient_h */
diff --git a/extern/bullet2/src/BulletSoftBody/btConjugateResidual.h b/extern/bullet2/src/BulletSoftBody/btConjugateResidual.h
new file mode 100644
index 00000000000..61461203653
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btConjugateResidual.h
@@ -0,0 +1,112 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_CONJUGATE_RESIDUAL_H
+#define BT_CONJUGATE_RESIDUAL_H
+#include "btKrylovSolver.h"
+
+template <class MatrixX>
+class btConjugateResidual : public btKrylovSolver<MatrixX>
+{
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	typedef btKrylovSolver<MatrixX> Base;
+	TVStack r, p, z, temp_p, temp_r, best_x;
+	// temp_r = A*r
+	// temp_p = A*p
+	// z = M^(-1) * temp_p = M^(-1) * A * p
+	btScalar best_r;
+
+public:
+	btConjugateResidual(const int max_it_in)
+		: Base(max_it_in, 1e-8)
+	{
+	}
+
+	virtual ~btConjugateResidual() {}
+
+	// return the number of iterations taken
+	int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
+	{
+		BT_PROFILE("CRSolve");
+		btAssert(x.size() == b.size());
+		reinitialize(b);
+		// r = b - A * x --with assigned dof zeroed out
+		A.multiply(x, temp_r);  // borrow temp_r here to store A*x
+		r = this->sub(b, temp_r);
+		// z = M^(-1) * r
+		A.precondition(r, z);  // borrow z to store preconditioned r
+		r = z;
+		btScalar residual_norm = this->norm(r);
+		if (residual_norm <= Base::m_tolerance)
+		{
+			return 0;
+		}
+		p = r;
+		btScalar r_dot_Ar, r_dot_Ar_new;
+		// temp_p = A*p
+		A.multiply(p, temp_p);
+		// temp_r = A*r
+		temp_r = temp_p;
+		r_dot_Ar = this->dot(r, temp_r);
+		for (int k = 1; k <= Base::m_maxIterations; k++)
+		{
+			// z = M^(-1) * Ap
+			A.precondition(temp_p, z);
+			// alpha = r^T * A * r / (Ap)^T * M^-1 * Ap)
+			btScalar alpha = r_dot_Ar / this->dot(temp_p, z);
+			//  x += alpha * p;
+			this->multAndAddTo(alpha, p, x);
+			//  r -= alpha * z;
+			this->multAndAddTo(-alpha, z, r);
+			btScalar norm_r = this->norm(r);
+			if (norm_r < best_r)
+			{
+				best_x = x;
+				best_r = norm_r;
+				if (norm_r < Base::m_tolerance)
+				{
+					return k;
+				}
+			}
+			// temp_r = A * r;
+			A.multiply(r, temp_r);
+			r_dot_Ar_new = this->dot(r, temp_r);
+			btScalar beta = r_dot_Ar_new / r_dot_Ar;
+			r_dot_Ar = r_dot_Ar_new;
+			// p = beta*p + r;
+			p = this->multAndAdd(beta, p, r);
+			// temp_p = beta*temp_p + temp_r;
+			temp_p = this->multAndAdd(beta, temp_p, temp_r);
+		}
+		if (verbose)
+		{
+			std::cout << "ConjugateResidual max iterations reached, residual = " << best_r << std::endl;
+		}
+		x = best_x;
+		return Base::m_maxIterations;
+	}
+
+	void reinitialize(const TVStack& b)
+	{
+		r.resize(b.size());
+		p.resize(b.size());
+		z.resize(b.size());
+		temp_p.resize(b.size());
+		temp_r.resize(b.size());
+		best_x.resize(b.size());
+		best_r = SIMD_INFINITY;
+	}
+};
+#endif /* btConjugateResidual_h */
diff --git a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp
index e90d24e6edf..5a79ef86e28 100644
--- a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp
+++ b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp
@@ -21,11 +21,10 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
 #include "BulletSoftBody/btSoftBody.h"
 
-
 btDefaultSoftBodySolver::btDefaultSoftBodySolver()
 {
 	// Initial we will clearly need to update solver constants
-	// For now this is global for the cloths linked with this solver - we should probably make this body specific 
+	// For now this is global for the cloths linked with this solver - we should probably make this body specific
 	// for performance in future once we understand more clearly when constants need to be updated
 	m_updateSolverConstants = true;
 }
@@ -37,115 +36,111 @@ btDefaultSoftBodySolver::~btDefaultSoftBodySolver()
 // In this case the data is already in the soft bodies so there is no need for us to do anything
 void btDefaultSoftBodySolver::copyBackToSoftBodies(bool bMove)
 {
-
 }
 
-void btDefaultSoftBodySolver::optimize( btAlignedObjectArray< btSoftBody * > &softBodies , bool forceUpdate)
+void btDefaultSoftBodySolver::optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate)
 {
-	m_softBodySet.copyFromArray( softBodies );
+	m_softBodySet.copyFromArray(softBodies);
 }
 
-void btDefaultSoftBodySolver::updateSoftBodies( )
+void btDefaultSoftBodySolver::updateSoftBodies()
 {
-	for ( int i=0; i < m_softBodySet.size(); i++)
+	for (int i = 0; i < m_softBodySet.size(); i++)
 	{
-		btSoftBody*	psb=(btSoftBody*)m_softBodySet[i];
+		btSoftBody *psb = (btSoftBody *)m_softBodySet[i];
 		if (psb->isActive())
 		{
-			psb->integrateMotion();	
+			psb->integrateMotion();
 		}
 	}
-} // updateSoftBodies
+}  // updateSoftBodies
 
 bool btDefaultSoftBodySolver::checkInitialized()
 {
 	return true;
 }
 
-void btDefaultSoftBodySolver::solveConstraints( float solverdt )
+void btDefaultSoftBodySolver::solveConstraints(btScalar solverdt)
 {
 	// Solve constraints for non-solver softbodies
-	for(int i=0; i < m_softBodySet.size(); ++i)
+	for (int i = 0; i < m_softBodySet.size(); ++i)
 	{
-		btSoftBody*	psb = static_cast<btSoftBody*>(m_softBodySet[i]);
+		btSoftBody *psb = static_cast<btSoftBody *>(m_softBodySet[i]);
 		if (psb->isActive())
 		{
 			psb->solveConstraints();
 		}
-	}	
-} // btDefaultSoftBodySolver::solveConstraints
-
+	}
+}  // btDefaultSoftBodySolver::solveConstraints
 
-void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer( const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer )
+void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer)
 {
 	// Currently only support CPU output buffers
 	// TODO: check for DX11 buffers. Take all offsets into the same DX11 buffer
 	// and use them together on a single kernel call if possible by setting up a
 	// per-cloth target buffer array for the copy kernel.
 
-	if( vertexBuffer->getBufferType() == btVertexBufferDescriptor::CPU_BUFFER )
+	if (vertexBuffer->getBufferType() == btVertexBufferDescriptor::CPU_BUFFER)
 	{
-		const btAlignedObjectArray<btSoftBody::Node> &clothVertices( softBody->m_nodes );
+		const btAlignedObjectArray<btSoftBody::Node> &clothVertices(softBody->m_nodes);
 		int numVertices = clothVertices.size();
 
-		const btCPUVertexBufferDescriptor *cpuVertexBuffer = static_cast< btCPUVertexBufferDescriptor* >(vertexBuffer);						
-		float *basePointer = cpuVertexBuffer->getBasePointer();						
+		const btCPUVertexBufferDescriptor *cpuVertexBuffer = static_cast<btCPUVertexBufferDescriptor *>(vertexBuffer);
+		float *basePointer = cpuVertexBuffer->getBasePointer();
 
-		if( vertexBuffer->hasVertexPositions() )
+		if (vertexBuffer->hasVertexPositions())
 		{
 			const int vertexOffset = cpuVertexBuffer->getVertexOffset();
 			const int vertexStride = cpuVertexBuffer->getVertexStride();
 			float *vertexPointer = basePointer + vertexOffset;
 
-			for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex )
+			for (int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex)
 			{
 				btVector3 position = clothVertices[vertexIndex].m_x;
-				*(vertexPointer + 0) = position.getX();
-				*(vertexPointer + 1) = position.getY();
-				*(vertexPointer + 2) = position.getZ();
+				*(vertexPointer + 0) = (float)position.getX();
+				*(vertexPointer + 1) = (float)position.getY();
+				*(vertexPointer + 2) = (float)position.getZ();
 				vertexPointer += vertexStride;
 			}
 		}
-		if( vertexBuffer->hasNormals() )
+		if (vertexBuffer->hasNormals())
 		{
 			const int normalOffset = cpuVertexBuffer->getNormalOffset();
 			const int normalStride = cpuVertexBuffer->getNormalStride();
 			float *normalPointer = basePointer + normalOffset;
 
-			for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex )
+			for (int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex)
 			{
 				btVector3 normal = clothVertices[vertexIndex].m_n;
-				*(normalPointer + 0) = normal.getX();
-				*(normalPointer + 1) = normal.getY();
-				*(normalPointer + 2) = normal.getZ();
+				*(normalPointer + 0) = (float)normal.getX();
+				*(normalPointer + 1) = (float)normal.getY();
+				*(normalPointer + 2) = (float)normal.getZ();
 				normalPointer += normalStride;
 			}
 		}
 	}
-} // btDefaultSoftBodySolver::copySoftBodyToVertexBuffer
+}  // btDefaultSoftBodySolver::copySoftBodyToVertexBuffer
 
-void btDefaultSoftBodySolver::processCollision( btSoftBody* softBody, btSoftBody* otherSoftBody)
+void btDefaultSoftBodySolver::processCollision(btSoftBody *softBody, btSoftBody *otherSoftBody)
 {
-	softBody->defaultCollisionHandler( otherSoftBody);
+	softBody->defaultCollisionHandler(otherSoftBody);
 }
 
 // For the default solver just leave the soft body to do its collision processing
-void btDefaultSoftBodySolver::processCollision( btSoftBody *softBody, const btCollisionObjectWrapper* collisionObjectWrap )
+void btDefaultSoftBodySolver::processCollision(btSoftBody *softBody, const btCollisionObjectWrapper *collisionObjectWrap)
 {
-	softBody->defaultCollisionHandler( collisionObjectWrap );
-} // btDefaultSoftBodySolver::processCollision
-
+	softBody->defaultCollisionHandler(collisionObjectWrap);
+}  // btDefaultSoftBodySolver::processCollision
 
-void btDefaultSoftBodySolver::predictMotion( float timeStep )
+void btDefaultSoftBodySolver::predictMotion(btScalar timeStep)
 {
-	for ( int i=0; i < m_softBodySet.size(); ++i)
+	for (int i = 0; i < m_softBodySet.size(); ++i)
 	{
-		btSoftBody*	psb = m_softBodySet[i];
+		btSoftBody *psb = m_softBodySet[i];
 
 		if (psb->isActive())
 		{
-			psb->predictMotion(timeStep);		
+			psb->predictMotion(timeStep);
 		}
 	}
 }
-
diff --git a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h
index 1c17ffcbb2b..3965b07c58e 100644
--- a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h
+++ b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h
@@ -16,25 +16,23 @@ subject to the following restrictions:
 #ifndef BT_SOFT_BODY_DEFAULT_SOLVER_H
 #define BT_SOFT_BODY_DEFAULT_SOLVER_H
 
-
 #include "BulletSoftBody/btSoftBodySolvers.h"
 #include "btSoftBodySolverVertexBuffer.h"
 struct btCollisionObjectWrapper;
 
 class btDefaultSoftBodySolver : public btSoftBodySolver
 {
-protected:		
+protected:
 	/** Variable to define whether we need to update solver constants on the next iteration */
 	bool m_updateSolverConstants;
 
-	btAlignedObjectArray< btSoftBody * > m_softBodySet;
-
+	btAlignedObjectArray<btSoftBody *> m_softBodySet;
 
 public:
 	btDefaultSoftBodySolver();
-	
+
 	virtual ~btDefaultSoftBodySolver();
-	
+
 	virtual SolverTypes getSolverType() const
 	{
 		return DEFAULT_SOLVER;
@@ -42,22 +40,21 @@ public:
 
 	virtual bool checkInitialized();
 
-	virtual void updateSoftBodies( );
+	virtual void updateSoftBodies();
 
-	virtual void optimize( btAlignedObjectArray< btSoftBody * > &softBodies,bool forceUpdate=false );
+	virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false);
 
 	virtual void copyBackToSoftBodies(bool bMove = true);
 
-	virtual void solveConstraints( float solverdt );
-
-	virtual void predictMotion( float solverdt );
+	virtual void solveConstraints(btScalar solverdt);
 
-	virtual void copySoftBodyToVertexBuffer( const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer );
+	virtual void predictMotion(btScalar solverdt);
 
-	virtual void processCollision( btSoftBody *, const btCollisionObjectWrapper* );
+	virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer);
 
-	virtual void processCollision( btSoftBody*, btSoftBody* );
+	virtual void processCollision(btSoftBody *, const btCollisionObjectWrapper *);
 
+	virtual void processCollision(btSoftBody *, btSoftBody *);
 };
 
-#endif // #ifndef BT_ACCELERATED_SOFT_BODY_CPU_SOLVER_H
+#endif  // #ifndef BT_ACCELERATED_SOFT_BODY_CPU_SOLVER_H
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/extern/bullet2/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
new file mode 100644
index 00000000000..0c3e0b5eb82
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
@@ -0,0 +1,296 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "btDeformableBackwardEulerObjective.h"
+#include "btPreconditioner.h"
+#include "LinearMath/btQuickprof.h"
+
+btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v)
+	: m_softBodies(softBodies), m_projection(softBodies), m_backupVelocity(backup_v), m_implicit(false)
+{
+	m_massPreconditioner = new MassPreconditioner(m_softBodies);
+	m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit);
+	m_preconditioner = m_KKTPreconditioner;
+}
+
+btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective()
+{
+	delete m_KKTPreconditioner;
+	delete m_massPreconditioner;
+}
+
+void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt)
+{
+	BT_PROFILE("reinitialize");
+	if (dt > 0)
+	{
+		setDt(dt);
+	}
+	if (nodeUpdated)
+	{
+		updateId();
+	}
+	for (int i = 0; i < m_lf.size(); ++i)
+	{
+		m_lf[i]->reinitialize(nodeUpdated);
+	}
+	btMatrix3x3 I;
+	I.setIdentity();
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			if (psb->m_nodes[j].m_im > 0)
+				psb->m_nodes[j].m_effectiveMass = I * (1.0 / psb->m_nodes[j].m_im);
+		}
+	}
+	m_projection.reinitialize(nodeUpdated);
+	//    m_preconditioner->reinitialize(nodeUpdated);
+}
+
+void btDeformableBackwardEulerObjective::setDt(btScalar dt)
+{
+	m_dt = dt;
+}
+
+void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
+{
+	BT_PROFILE("multiply");
+	// add in the mass term
+	size_t counter = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			const btSoftBody::Node& node = psb->m_nodes[j];
+			b[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : x[counter] / node.m_im;
+			++counter;
+		}
+	}
+
+	for (int i = 0; i < m_lf.size(); ++i)
+	{
+		// add damping matrix
+		m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
+        // Always integrate picking force implicitly for stability.
+        if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE)
+		{
+			m_lf[i]->addScaledElasticForceDifferential(-m_dt * m_dt, x, b);
+		}
+	}
+	int offset = m_nodes.size();
+	for (int i = offset; i < b.size(); ++i)
+	{
+		b[i].setZero();
+	}
+	// add in the lagrange multiplier terms
+
+	for (int c = 0; c < m_projection.m_lagrangeMultipliers.size(); ++c)
+	{
+		// C^T * lambda
+		const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[c];
+		for (int i = 0; i < lm.m_num_nodes; ++i)
+		{
+			for (int j = 0; j < lm.m_num_constraints; ++j)
+			{
+				b[lm.m_indices[i]] += x[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
+			}
+		}
+		// C * x
+		for (int d = 0; d < lm.m_num_constraints; ++d)
+		{
+			for (int i = 0; i < lm.m_num_nodes; ++i)
+			{
+				b[offset + c][d] += lm.m_weights[i] * x[lm.m_indices[i]].dot(lm.m_dirs[d]);
+			}
+		}
+	}
+}
+
+void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
+{
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			btSoftBody::Node& node = psb->m_nodes[j];
+			node.m_v = m_backupVelocity[node.index] + dv[node.index];
+		}
+	}
+}
+
+void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero)
+{
+	size_t counter = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			counter += psb->m_nodes.size();
+			continue;
+		}
+		if (m_implicit)
+		{
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				if (psb->m_nodes[j].m_im != 0)
+				{
+					psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++];
+				}
+			}
+		}
+		else
+		{
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im;
+				psb->m_nodes[j].m_v += one_over_mass * force[counter++];
+			}
+		}
+	}
+	if (setZero)
+	{
+		for (int i = 0; i < force.size(); ++i)
+			force[i].setZero();
+	}
+}
+
+void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack& residual)
+{
+	BT_PROFILE("computeResidual");
+	// add implicit force
+	for (int i = 0; i < m_lf.size(); ++i)
+	{
+        // Always integrate picking force implicitly for stability.
+		if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE)
+		{
+			m_lf[i]->addScaledForces(dt, residual);
+		}
+		else
+		{
+			m_lf[i]->addScaledDampingForce(dt, residual);
+		}
+	}
+	//    m_projection.project(residual);
+}
+
+btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const
+{
+	btScalar mag = 0;
+	for (int i = 0; i < residual.size(); ++i)
+	{
+		mag += residual[i].length2();
+	}
+	return std::sqrt(mag);
+}
+
+btScalar btDeformableBackwardEulerObjective::totalEnergy(btScalar dt)
+{
+	btScalar e = 0;
+	for (int i = 0; i < m_lf.size(); ++i)
+	{
+		e += m_lf[i]->totalEnergy(dt);
+	}
+	return e;
+}
+
+void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)
+{
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		m_softBodies[i]->advanceDeformation();
+	}
+	if (m_implicit)
+	{
+		// apply forces except gravity force
+		btVector3 gravity;
+		for (int i = 0; i < m_lf.size(); ++i)
+		{
+			if (m_lf[i]->getForceType() == BT_GRAVITY_FORCE)
+			{
+				gravity = static_cast<btDeformableGravityForce*>(m_lf[i])->m_gravity;
+			}
+			else
+			{
+				m_lf[i]->addScaledForces(m_dt, force);
+			}
+		}
+		for (int i = 0; i < m_lf.size(); ++i)
+		{
+			m_lf[i]->addScaledHessian(m_dt);
+		}
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					// add gravity explicitly
+					psb->m_nodes[j].m_v += m_dt * psb->m_gravityFactor * gravity;
+				}
+			}
+		}
+	}
+	else
+	{
+		for (int i = 0; i < m_lf.size(); ++i)
+		{
+			m_lf[i]->addScaledExplicitForce(m_dt, force);
+		}
+	}
+	// calculate inverse mass matrix for all nodes
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (psb->isActive())
+		{
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				psb->m_nodes[j].m_effectiveMass_inv = psb->m_nodes[j].m_effectiveMass.inverse();
+			}
+		}
+	}
+	applyForce(force, true);
+}
+
+void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual)
+{
+	size_t counter = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			dv[counter] = psb->m_nodes[j].m_im * residual[counter];
+			++counter;
+		}
+	}
+}
+
+//set constraints as projections
+void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal)
+{
+	m_projection.setConstraints(infoGlobal);
+}
+
+void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r)
+{
+	m_projection.applyDynamicFriction(r);
+}
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/extern/bullet2/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
new file mode 100644
index 00000000000..eb05b9f010c
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
@@ -0,0 +1,198 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_BACKWARD_EULER_OBJECTIVE_H
+#define BT_BACKWARD_EULER_OBJECTIVE_H
+//#include "btConjugateGradient.h"
+#include "btDeformableLagrangianForce.h"
+#include "btDeformableMassSpringForce.h"
+#include "btDeformableGravityForce.h"
+#include "btDeformableCorotatedForce.h"
+#include "btDeformableMousePickingForce.h"
+#include "btDeformableLinearElasticityForce.h"
+#include "btDeformableNeoHookeanForce.h"
+#include "btDeformableContactProjection.h"
+#include "btPreconditioner.h"
+#include "btDeformableMultiBodyDynamicsWorld.h"
+#include "LinearMath/btQuickprof.h"
+
+class btDeformableBackwardEulerObjective
+{
+public:
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btScalar m_dt;
+	btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
+	btAlignedObjectArray<btSoftBody*>& m_softBodies;
+	Preconditioner* m_preconditioner;
+	btDeformableContactProjection m_projection;
+	const TVStack& m_backupVelocity;
+	btAlignedObjectArray<btSoftBody::Node*> m_nodes;
+	bool m_implicit;
+	MassPreconditioner* m_massPreconditioner;
+	KKTPreconditioner* m_KKTPreconditioner;
+
+	btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v);
+
+	virtual ~btDeformableBackwardEulerObjective();
+
+	void initialize() {}
+
+	// compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
+	void computeResidual(btScalar dt, TVStack& residual);
+
+	// add explicit force to the velocity
+	void applyExplicitForce(TVStack& force);
+
+	// apply force to velocity and optionally reset the force to zero
+	void applyForce(TVStack& force, bool setZero);
+
+	// compute the norm of the residual
+	btScalar computeNorm(const TVStack& residual) const;
+
+	// compute one step of the solve (there is only one solve if the system is linear)
+	void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
+
+	// perform A*x = b
+	void multiply(const TVStack& x, TVStack& b) const;
+
+	// set initial guess for CG solve
+	void initialGuess(TVStack& dv, const TVStack& residual);
+
+	// reset data structure and reset dt
+	void reinitialize(bool nodeUpdated, btScalar dt);
+
+	void setDt(btScalar dt);
+
+	// add friction force to residual
+	void applyDynamicFriction(TVStack& r);
+
+	// add dv to velocity
+	void updateVelocity(const TVStack& dv);
+
+	//set constraints as projections
+	void setConstraints(const btContactSolverInfo& infoGlobal);
+
+	// update the projections and project the residual
+	void project(TVStack& r)
+	{
+		BT_PROFILE("project");
+		m_projection.project(r);
+	}
+
+	// perform precondition M^(-1) x = b
+	void precondition(const TVStack& x, TVStack& b)
+	{
+		m_preconditioner->operator()(x, b);
+	}
+
+	// reindex all the vertices
+	virtual void updateId()
+	{
+		size_t node_id = 0;
+		size_t face_id = 0;
+		m_nodes.clear();
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				psb->m_nodes[j].index = node_id;
+				m_nodes.push_back(&psb->m_nodes[j]);
+				++node_id;
+			}
+			for (int j = 0; j < psb->m_faces.size(); ++j)
+			{
+				psb->m_faces[j].m_index = face_id;
+				++face_id;
+			}
+		}
+	}
+
+	const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const
+	{
+		return &m_nodes;
+	}
+
+	void setImplicit(bool implicit)
+	{
+		m_implicit = implicit;
+	}
+
+	// Calculate the total potential energy in the system
+	btScalar totalEnergy(btScalar dt);
+
+	void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec)
+	{
+		extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size());
+		for (int i = 0; i < vec.size(); ++i)
+		{
+			extended_vec[i] = vec[i];
+		}
+		int offset = vec.size();
+		for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
+		{
+			extended_vec[offset + i].setZero();
+		}
+	}
+
+	void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual)
+	{
+		extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size());
+		for (int i = 0; i < residual.size(); ++i)
+		{
+			extended_residual[i] = residual[i];
+		}
+		int offset = residual.size();
+		for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
+		{
+			const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i];
+			extended_residual[offset + i].setZero();
+			for (int d = 0; d < lm.m_num_constraints; ++d)
+			{
+				for (int n = 0; n < lm.m_num_nodes; ++n)
+				{
+					extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]);
+				}
+			}
+		}
+	}
+
+	void calculateContactForce(const TVStack& dv, const TVStack& rhs, TVStack& f)
+	{
+		size_t counter = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				const btSoftBody::Node& node = psb->m_nodes[j];
+				f[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : dv[counter] / node.m_im;
+				++counter;
+			}
+		}
+		for (int i = 0; i < m_lf.size(); ++i)
+		{
+			// add damping matrix
+			m_lf[i]->addScaledDampingForceDifferential(-m_dt, dv, f);
+		}
+		counter = 0;
+		for (; counter < f.size(); ++counter)
+		{
+			f[counter] = rhs[counter] - f[counter];
+		}
+	}
+};
+
+#endif /* btBackwardEulerObjective_h */
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableBodySolver.cpp b/extern/bullet2/src/BulletSoftBody/btDeformableBodySolver.cpp
new file mode 100644
index 00000000000..4b11fccecba
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -0,0 +1,506 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include <stdio.h>
+#include <limits>
+#include "btDeformableBodySolver.h"
+#include "btSoftBodyInternals.h"
+#include "LinearMath/btQuickprof.h"
+static const int kMaxConjugateGradientIterations = 300;
+btDeformableBodySolver::btDeformableBodySolver()
+	: m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false)
+{
+	m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
+}
+
+btDeformableBodySolver::~btDeformableBodySolver()
+{
+	delete m_objective;
+}
+
+void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
+{
+	BT_PROFILE("solveDeformableConstraints");
+	if (!m_implicit)
+	{
+		m_objective->computeResidual(solverdt, m_residual);
+		m_objective->applyDynamicFriction(m_residual);
+		if (m_useProjection)
+		{
+			computeStep(m_dv, m_residual);
+		}
+		else
+		{
+			TVStack rhs, x;
+			m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs);
+			m_objective->addLagrangeMultiplier(m_dv, x);
+			m_objective->m_preconditioner->reinitialize(true);
+			computeStep(x, rhs);
+			for (int i = 0; i < m_dv.size(); ++i)
+			{
+				m_dv[i] = x[i];
+			}
+		}
+		updateVelocity();
+	}
+	else
+	{
+		for (int i = 0; i < m_maxNewtonIterations; ++i)
+		{
+			updateState();
+			// add the inertia term in the residual
+			int counter = 0;
+			for (int k = 0; k < m_softBodies.size(); ++k)
+			{
+				btSoftBody* psb = m_softBodies[k];
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					if (psb->m_nodes[j].m_im > 0)
+					{
+						m_residual[counter] = (-1. / psb->m_nodes[j].m_im) * m_dv[counter];
+					}
+					++counter;
+				}
+			}
+
+			m_objective->computeResidual(solverdt, m_residual);
+			if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0)
+			{
+				break;
+			}
+			// todo xuchenhan@: this really only needs to be calculated once
+			m_objective->applyDynamicFriction(m_residual);
+			if (m_lineSearch)
+			{
+				btScalar inner_product = computeDescentStep(m_ddv, m_residual);
+				btScalar alpha = 0.01, beta = 0.5;  // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8
+				btScalar scale = 2;
+				btScalar f0 = m_objective->totalEnergy(solverdt) + kineticEnergy(), f1, f2;
+				backupDv();
+				do
+				{
+					scale *= beta;
+					if (scale < 1e-8)
+					{
+						return;
+					}
+					updateEnergy(scale);
+					f1 = m_objective->totalEnergy(solverdt) + kineticEnergy();
+					f2 = f0 - alpha * scale * inner_product;
+				} while (!(f1 < f2 + SIMD_EPSILON));  // if anything here is nan then the search continues
+				revertDv();
+				updateDv(scale);
+			}
+			else
+			{
+				computeStep(m_ddv, m_residual);
+				updateDv();
+			}
+			for (int j = 0; j < m_numNodes; ++j)
+			{
+				m_ddv[j].setZero();
+				m_residual[j].setZero();
+			}
+		}
+		updateVelocity();
+	}
+}
+
+btScalar btDeformableBodySolver::kineticEnergy()
+{
+	btScalar ke = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			btSoftBody::Node& node = psb->m_nodes[j];
+			if (node.m_im > 0)
+			{
+				ke += m_dv[node.index].length2() * 0.5 / node.m_im;
+			}
+		}
+	}
+	return ke;
+}
+
+void btDeformableBodySolver::backupDv()
+{
+	m_backup_dv.resize(m_dv.size());
+	for (int i = 0; i < m_backup_dv.size(); ++i)
+	{
+		m_backup_dv[i] = m_dv[i];
+	}
+}
+
+void btDeformableBodySolver::revertDv()
+{
+	for (int i = 0; i < m_backup_dv.size(); ++i)
+	{
+		m_dv[i] = m_backup_dv[i];
+	}
+}
+
+void btDeformableBodySolver::updateEnergy(btScalar scale)
+{
+	for (int i = 0; i < m_dv.size(); ++i)
+	{
+		m_dv[i] = m_backup_dv[i] + scale * m_ddv[i];
+	}
+	updateState();
+}
+
+btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose)
+{
+	m_cg.solve(*m_objective, ddv, residual, false);
+	btScalar inner_product = m_cg.dot(residual, m_ddv);
+	btScalar res_norm = m_objective->computeNorm(residual);
+	btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv);
+	if (inner_product < -tol)
+	{
+		if (verbose)
+		{
+			std::cout << "Looking backwards!" << std::endl;
+		}
+		for (int i = 0; i < m_ddv.size(); ++i)
+		{
+			m_ddv[i] = -m_ddv[i];
+		}
+		inner_product = -inner_product;
+	}
+	else if (std::abs(inner_product) < tol)
+	{
+		if (verbose)
+		{
+			std::cout << "Gradient Descent!" << std::endl;
+		}
+		btScalar scale = m_objective->computeNorm(m_ddv) / res_norm;
+		for (int i = 0; i < m_ddv.size(); ++i)
+		{
+			m_ddv[i] = scale * residual[i];
+		}
+		inner_product = scale * res_norm * res_norm;
+	}
+	return inner_product;
+}
+
+void btDeformableBodySolver::updateState()
+{
+	updateVelocity();
+	updateTempPosition();
+}
+
+void btDeformableBodySolver::updateDv(btScalar scale)
+{
+	for (int i = 0; i < m_numNodes; ++i)
+	{
+		m_dv[i] += scale * m_ddv[i];
+	}
+}
+
+void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
+{
+	if (m_useProjection)
+		m_cg.solve(*m_objective, ddv, residual, false);
+	else
+		m_cr.solve(*m_objective, ddv, residual, false);
+}
+
+void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt)
+{
+	m_softBodies.copyFromArray(softBodies);
+	bool nodeUpdated = updateNodes();
+
+	if (nodeUpdated)
+	{
+		m_dv.resize(m_numNodes, btVector3(0, 0, 0));
+		m_ddv.resize(m_numNodes, btVector3(0, 0, 0));
+		m_residual.resize(m_numNodes, btVector3(0, 0, 0));
+		m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0));
+	}
+
+	// need to setZero here as resize only set value for newly allocated items
+	for (int i = 0; i < m_numNodes; ++i)
+	{
+		m_dv[i].setZero();
+		m_ddv[i].setZero();
+		m_residual[i].setZero();
+	}
+
+	if (dt > 0)
+	{
+		m_dt = dt;
+	}
+	m_objective->reinitialize(nodeUpdated, dt);
+	updateSoftBodies();
+}
+
+void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("setConstraint");
+	m_objective->setConstraints(infoGlobal);
+}
+
+btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("solveContactConstraints");
+	btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies, numDeformableBodies, infoGlobal);
+	return maxSquaredResidual;
+}
+
+void btDeformableBodySolver::updateVelocity()
+{
+	int counter = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		psb->m_maxSpeedSquared = 0;
+		if (!psb->isActive())
+		{
+			counter += psb->m_nodes.size();
+			continue;
+		}
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			// set NaN to zero;
+			if (m_dv[counter] != m_dv[counter])
+			{
+				m_dv[counter].setZero();
+			}
+			if (m_implicit)
+			{
+				psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter];
+			}
+			else
+			{
+				psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv;
+			}
+			psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
+			++counter;
+		}
+	}
+}
+
+void btDeformableBodySolver::updateTempPosition()
+{
+	int counter = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			counter += psb->m_nodes.size();
+			continue;
+		}
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * (psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv);
+			++counter;
+		}
+		psb->updateDeformation();
+	}
+}
+
+void btDeformableBodySolver::backupVelocity()
+{
+	int counter = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
+		}
+	}
+}
+
+void btDeformableBodySolver::setupDeformableSolve(bool implicit)
+{
+	int counter = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			counter += psb->m_nodes.size();
+			continue;
+		}
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			if (implicit)
+			{
+				// setting the initial guess for newton, need m_dv = v_{n+1} - v_n for dofs that are in constraint.
+				if (psb->m_nodes[j].m_v == m_backupVelocity[counter])
+					m_dv[counter].setZero();
+				else
+					m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn;
+				m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
+			}
+			else
+			{
+				m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter];
+			}
+			psb->m_nodes[j].m_v = m_backupVelocity[counter];
+			++counter;
+		}
+	}
+}
+
+void btDeformableBodySolver::revertVelocity()
+{
+	int counter = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			psb->m_nodes[j].m_v = m_backupVelocity[counter++];
+		}
+	}
+}
+
+bool btDeformableBodySolver::updateNodes()
+{
+	int numNodes = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+		numNodes += m_softBodies[i]->m_nodes.size();
+	if (numNodes != m_numNodes)
+	{
+		m_numNodes = numNodes;
+		return true;
+	}
+	return false;
+}
+
+void btDeformableBodySolver::predictMotion(btScalar solverdt)
+{
+	// apply explicit forces to velocity
+	if (m_implicit)
+	{
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + psb->m_nodes[j].m_v * solverdt;
+				}
+			}
+		}
+	}
+	m_objective->applyExplicitForce(m_residual);
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+
+		if (psb->isActive())
+		{
+			// predict motion for collision detection
+			predictDeformableMotion(psb, solverdt);
+		}
+	}
+}
+
+void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt)
+{
+	BT_PROFILE("btDeformableBodySolver::predictDeformableMotion");
+	int i, ni;
+
+	/* Update                */
+	if (psb->m_bUpdateRtCst)
+	{
+		psb->m_bUpdateRtCst = false;
+		psb->updateConstants();
+		psb->m_fdbvt.clear();
+		if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD)
+		{
+			psb->initializeFaceTree();
+		}
+	}
+
+	/* Prepare                */
+	psb->m_sst.sdt = dt * psb->m_cfg.timescale;
+	psb->m_sst.isdt = 1 / psb->m_sst.sdt;
+	psb->m_sst.velmrg = psb->m_sst.sdt * 3;
+	psb->m_sst.radmrg = psb->getCollisionShape()->getMargin();
+	psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25;
+	/* Bounds                */
+	psb->updateBounds();
+
+	/* Integrate            */
+	// do not allow particles to move more than the bounding box size
+	btScalar max_v = (psb->m_bounds[1] - psb->m_bounds[0]).norm() / dt;
+	for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
+	{
+		btSoftBody::Node& n = psb->m_nodes[i];
+		// apply drag
+		n.m_v *= (1 - psb->m_cfg.drag);
+		// scale velocity back
+		if (m_implicit)
+		{
+			n.m_q = n.m_x;
+		}
+		else
+		{
+			if (n.m_v.norm() > max_v)
+			{
+				n.m_v.safeNormalize();
+				n.m_v *= max_v;
+			}
+			n.m_q = n.m_x + n.m_v * dt;
+		}
+		n.m_splitv.setZero();
+		n.m_constrained = false;
+	}
+
+	/* Nodes                */
+	psb->updateNodeTree(true, true);
+	if (!psb->m_fdbvt.empty())
+	{
+		psb->updateFaceTree(true, true);
+	}
+	/* Clear contacts */
+	psb->m_nodeRigidContacts.resize(0);
+	psb->m_faceRigidContacts.resize(0);
+	psb->m_faceNodeContacts.resize(0);
+	/* Optimize dbvt's        */
+	//    psb->m_ndbvt.optimizeIncremental(1);
+	//    psb->m_fdbvt.optimizeIncremental(1);
+}
+
+void btDeformableBodySolver::updateSoftBodies()
+{
+	BT_PROFILE("updateSoftBodies");
+	for (int i = 0; i < m_softBodies.size(); i++)
+	{
+		btSoftBody* psb = (btSoftBody*)m_softBodies[i];
+		if (psb->isActive())
+		{
+			psb->updateNormals();
+		}
+	}
+}
+
+void btDeformableBodySolver::setImplicit(bool implicit)
+{
+	m_implicit = implicit;
+	m_objective->setImplicit(implicit);
+}
+
+void btDeformableBodySolver::setLineSearch(bool lineSearch)
+{
+	m_lineSearch = lineSearch;
+}
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableBodySolver.h b/extern/bullet2/src/BulletSoftBody/btDeformableBodySolver.h
new file mode 100644
index 00000000000..ae674d6e892
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableBodySolver.h
@@ -0,0 +1,160 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_DEFORMABLE_BODY_SOLVERS_H
+#define BT_DEFORMABLE_BODY_SOLVERS_H
+
+#include "btSoftBodySolvers.h"
+#include "btDeformableBackwardEulerObjective.h"
+#include "btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+#include "btConjugateResidual.h"
+#include "btConjugateGradient.h"
+struct btCollisionObjectWrapper;
+class btDeformableBackwardEulerObjective;
+class btDeformableMultiBodyDynamicsWorld;
+
+class btDeformableBodySolver : public btSoftBodySolver
+{
+	typedef btAlignedObjectArray<btVector3> TVStack;
+
+protected:
+	int m_numNodes;                                                // total number of deformable body nodes
+	TVStack m_dv;                                                  // v_{n+1} - v_n
+	TVStack m_backup_dv;                                           // backed up dv
+	TVStack m_ddv;                                                 // incremental dv
+	TVStack m_residual;                                            // rhs of the linear solve
+	btAlignedObjectArray<btSoftBody*> m_softBodies;                // all deformable bodies
+	TVStack m_backupVelocity;                                      // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit
+	btScalar m_dt;                                                 // dt
+	btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;  // CG solver
+	btConjugateResidual<btDeformableBackwardEulerObjective> m_cr;  // CR solver
+	bool m_implicit;                                               // use implicit scheme if true, explicit scheme if false
+	int m_maxNewtonIterations;                                     // max number of newton iterations
+	btScalar m_newtonTolerance;                                    // stop newton iterations if f(x) < m_newtonTolerance
+	bool m_lineSearch;                                             // If true, use newton's method with line search under implicit scheme
+public:
+	// handles data related to objective function
+	btDeformableBackwardEulerObjective* m_objective;
+	bool m_useProjection;
+
+	btDeformableBodySolver();
+
+	virtual ~btDeformableBodySolver();
+
+	virtual SolverTypes getSolverType() const
+	{
+		return DEFORMABLE_SOLVER;
+	}
+
+	// update soft body normals
+	virtual void updateSoftBodies();
+
+	virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
+
+	// solve the momentum equation
+	virtual void solveDeformableConstraints(btScalar solverdt);
+
+	// resize/clear data structures
+	void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt);
+
+	// set up contact constraints
+	void setConstraints(const btContactSolverInfo& infoGlobal);
+
+	// add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
+	virtual void predictMotion(btScalar solverdt);
+
+	// move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^*
+	// x_{n+1}^* is stored in m_q
+	void predictDeformableMotion(btSoftBody* psb, btScalar dt);
+
+	// save the current velocity to m_backupVelocity
+	void backupVelocity();
+
+	// set m_dv and m_backupVelocity to desired value to prepare for momentum solve
+	void setupDeformableSolve(bool implicit);
+
+	// set the current velocity to that backed up in m_backupVelocity
+	void revertVelocity();
+
+	// set velocity to m_dv + m_backupVelocity
+	void updateVelocity();
+
+	// update the node count
+	bool updateNodes();
+
+	// calculate the change in dv resulting from the momentum solve
+	void computeStep(TVStack& ddv, const TVStack& residual);
+
+	// calculate the change in dv resulting from the momentum solve when line search is turned on
+	btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose = false);
+
+	virtual void copySoftBodyToVertexBuffer(const btSoftBody* const softBody, btVertexBufferDescriptor* vertexBuffer) {}
+
+	// process collision between deformable and rigid
+	virtual void processCollision(btSoftBody* softBody, const btCollisionObjectWrapper* collisionObjectWrap)
+	{
+		softBody->defaultCollisionHandler(collisionObjectWrap);
+	}
+
+	// process collision between deformable and deformable
+	virtual void processCollision(btSoftBody* softBody, btSoftBody* otherSoftBody)
+	{
+		softBody->defaultCollisionHandler(otherSoftBody);
+	}
+
+	// If true, implicit time stepping scheme is used.
+	// Otherwise, explicit time stepping scheme is used
+	void setImplicit(bool implicit);
+
+	// If true, newton's method with line search is used when implicit time stepping scheme is turned on
+	void setLineSearch(bool lineSearch);
+
+	// set temporary position x^* = x_n + dt * v
+	// update the deformation gradient at position x^*
+	void updateState();
+
+	// set dv = dv + scale * ddv
+	void updateDv(btScalar scale = 1);
+
+	// set temporary position x^* = x_n + dt * v^*
+	void updateTempPosition();
+
+	// save the current dv to m_backup_dv;
+	void backupDv();
+
+	// set dv to the backed-up value
+	void revertDv();
+
+	// set dv = dv + scale * ddv
+	// set v^* = v_n + dv
+	// set temporary position x^* = x_n + dt * v^*
+	// update the deformation gradient at position x^*
+	void updateEnergy(btScalar scale);
+
+	// calculates the appropriately scaled kinetic energy in the system, which is
+	// 1/2 * dv^T * M * dv
+	// used in line search
+	btScalar kineticEnergy();
+
+	// unused functions
+	virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {}
+	virtual void solveConstraints(btScalar dt) {}
+	virtual bool checkInitialized() { return true; }
+	virtual void copyBackToSoftBodies(bool bMove = true) {}
+};
+
+#endif /* btDeformableBodySolver_h */
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableContactConstraint.cpp b/extern/bullet2/src/BulletSoftBody/btDeformableContactConstraint.cpp
new file mode 100644
index 00000000000..09398d79a5c
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableContactConstraint.cpp
@@ -0,0 +1,720 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "btDeformableContactConstraint.h"
+/* ================   Deformable Node Anchor   =================== */
+btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a, const btContactSolverInfo& infoGlobal)
+	: m_anchor(&a), btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal)
+{
+}
+
+btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other)
+	: m_anchor(other.m_anchor), btDeformableContactConstraint(other)
+{
+}
+
+btVector3 btDeformableNodeAnchorConstraint::getVa() const
+{
+	const btSoftBody::sCti& cti = m_anchor->m_cti;
+	btVector3 va(0, 0, 0);
+	if (cti.m_colObj->hasContactResponse())
+	{
+		btRigidBody* rigidCol = 0;
+		btMultiBodyLinkCollider* multibodyLinkCol = 0;
+
+		// grab the velocity of the rigid body
+		if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+		{
+			rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+			va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0);
+		}
+		else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+		{
+			multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+			if (multibodyLinkCol)
+			{
+				const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+				const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0];
+				const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0];
+				const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0];
+				const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
+				const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
+				// add in the normal component of the va
+				btScalar vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += (local_v[k] + local_dv[k]) * J_n[k];
+				}
+				va = cti.m_normal * vel;
+				// add in the tangential components of the va
+				vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += (local_v[k] + local_dv[k]) * J_t1[k];
+				}
+				va += m_anchor->t1 * vel;
+				vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += (local_v[k] + local_dv[k]) * J_t2[k];
+				}
+				va += m_anchor->t2 * vel;
+			}
+		}
+	}
+	return va;
+}
+
+btScalar btDeformableNodeAnchorConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
+{
+	const btSoftBody::sCti& cti = m_anchor->m_cti;
+	btVector3 va = getVa();
+	btVector3 vb = getVb();
+	btVector3 vr = (vb - va);
+	// + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0
+	const btScalar dn = btDot(vr, vr);
+	// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+	btScalar residualSquare = dn * dn;
+	btVector3 impulse = m_anchor->m_c0 * vr;
+	// apply impulse to deformable nodes involved and change their velocities
+	applyImpulse(impulse);
+
+	// apply impulse to the rigid/multibodies involved and change their velocities
+	if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+	{
+		btRigidBody* rigidCol = 0;
+		rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+		if (rigidCol)
+		{
+			rigidCol->applyImpulse(impulse, m_anchor->m_c1);
+		}
+	}
+	else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+	{
+		btMultiBodyLinkCollider* multibodyLinkCol = 0;
+		multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+		if (multibodyLinkCol)
+		{
+			const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+			// apply normal component of the impulse
+			multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
+			// apply tangential component of the impulse
+			const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+			multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1));
+			const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+			multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2));
+		}
+	}
+	return residualSquare;
+}
+
+btVector3 btDeformableNodeAnchorConstraint::getVb() const
+{
+	return m_anchor->m_node->m_v;
+}
+
+void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
+{
+	btVector3 dv = impulse * m_anchor->m_c2;
+	m_anchor->m_node->m_v -= dv;
+}
+
+/* ================   Deformable vs. Rigid   =================== */
+btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal)
+	: m_contact(&c), btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal)
+{
+	m_total_normal_dv.setZero();
+	m_total_tangent_dv.setZero();
+	// The magnitude of penetration is the depth of penetration.
+	m_penetration = c.m_cti.m_offset;
+	m_total_split_impulse = 0;
+	m_binding = false;
+}
+
+btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
+	: m_contact(other.m_contact), btDeformableContactConstraint(other), m_penetration(other.m_penetration), m_total_split_impulse(other.m_total_split_impulse), m_binding(other.m_binding)
+{
+	m_total_normal_dv = other.m_total_normal_dv;
+	m_total_tangent_dv = other.m_total_tangent_dv;
+}
+
+btVector3 btDeformableRigidContactConstraint::getVa() const
+{
+	const btSoftBody::sCti& cti = m_contact->m_cti;
+	btVector3 va(0, 0, 0);
+	if (cti.m_colObj->hasContactResponse())
+	{
+		btRigidBody* rigidCol = 0;
+		btMultiBodyLinkCollider* multibodyLinkCol = 0;
+
+		// grab the velocity of the rigid body
+		if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+		{
+			rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+			va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0);
+		}
+		else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+		{
+			multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+			if (multibodyLinkCol)
+			{
+				const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+				const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
+				const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
+				const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
+				const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
+				const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
+				// add in the normal component of the va
+				btScalar vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += (local_v[k] + local_dv[k]) * J_n[k];
+				}
+				va = cti.m_normal * vel;
+				// add in the tangential components of the va
+				vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += (local_v[k] + local_dv[k]) * J_t1[k];
+				}
+				va += m_contact->t1 * vel;
+				vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += (local_v[k] + local_dv[k]) * J_t2[k];
+				}
+				va += m_contact->t2 * vel;
+			}
+		}
+	}
+	return va;
+}
+
+btVector3 btDeformableRigidContactConstraint::getSplitVa() const
+{
+	const btSoftBody::sCti& cti = m_contact->m_cti;
+	btVector3 va(0, 0, 0);
+	if (cti.m_colObj->hasContactResponse())
+	{
+		btRigidBody* rigidCol = 0;
+		btMultiBodyLinkCollider* multibodyLinkCol = 0;
+
+		// grab the velocity of the rigid body
+		if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+		{
+			rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+			va = rigidCol ? (rigidCol->getPushVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0);
+		}
+		else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+		{
+			multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+			if (multibodyLinkCol)
+			{
+				const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+				const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
+				const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
+				const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
+				const btScalar* local_split_v = multibodyLinkCol->m_multiBody->getSplitVelocityVector();
+				// add in the normal component of the va
+				btScalar vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += local_split_v[k] * J_n[k];
+				}
+				va = cti.m_normal * vel;
+				// add in the tangential components of the va
+				vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += local_split_v[k] * J_t1[k];
+				}
+				va += m_contact->t1 * vel;
+				vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += local_split_v[k] * J_t2[k];
+				}
+				va += m_contact->t2 * vel;
+			}
+		}
+	}
+	return va;
+}
+
+btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
+{
+	const btSoftBody::sCti& cti = m_contact->m_cti;
+	btVector3 va = getVa();
+	btVector3 vb = getVb();
+	btVector3 vr = vb - va;
+	btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.m_deformable_cfm;
+	if (m_penetration > 0)
+	{
+		dn += m_penetration / infoGlobal.m_timeStep;
+	}
+	if (!infoGlobal.m_splitImpulse)
+	{
+		dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
+	}
+	// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+	btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0)));
+	if (!infoGlobal.m_splitImpulse)
+	{
+		impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal);
+	}
+	btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
+	btVector3 impulse_tangent = impulse - impulse_normal;
+	if (dn > 0)
+	{
+		return 0;
+	}
+	m_binding = true;
+	btScalar residualSquare = dn * dn;
+	btVector3 old_total_tangent_dv = m_total_tangent_dv;
+	// m_c5 is the inverse mass of the deformable node/face
+	m_total_normal_dv -= m_contact->m_c5 * impulse_normal;
+	m_total_tangent_dv -= m_contact->m_c5 * impulse_tangent;
+
+	if (m_total_normal_dv.dot(cti.m_normal) < 0)
+	{
+		// separating in the normal direction
+		m_binding = false;
+		m_static = false;
+		impulse_tangent.setZero();
+	}
+	else
+	{
+		if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm())
+		{
+			// dynamic friction
+			// with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
+			m_static = false;
+			if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
+			{
+				m_total_tangent_dv = btVector3(0, 0, 0);
+			}
+			else
+			{
+				m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3;
+			}
+			//            impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
+			impulse_tangent = m_contact->m_c5.inverse() * (old_total_tangent_dv - m_total_tangent_dv);
+		}
+		else
+		{
+			// static friction
+			m_static = true;
+		}
+	}
+	impulse = impulse_normal + impulse_tangent;
+	// apply impulse to deformable nodes involved and change their velocities
+	applyImpulse(impulse);
+	// apply impulse to the rigid/multibodies involved and change their velocities
+	if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+	{
+		btRigidBody* rigidCol = 0;
+		rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+		if (rigidCol)
+		{
+			rigidCol->applyImpulse(impulse, m_contact->m_c1);
+		}
+	}
+	else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+	{
+		btMultiBodyLinkCollider* multibodyLinkCol = 0;
+		multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+		if (multibodyLinkCol)
+		{
+			const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+			// apply normal component of the impulse
+			multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
+			if (impulse_tangent.norm() > SIMD_EPSILON)
+			{
+				// apply tangential component of the impulse
+				const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+				multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1));
+				const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+				multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2));
+			}
+		}
+	}
+	return residualSquare;
+}
+
+btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
+{
+	btScalar MAX_PENETRATION_CORRECTION = infoGlobal.m_deformable_maxErrorReduction;
+	const btSoftBody::sCti& cti = m_contact->m_cti;
+	btVector3 vb = getSplitVb();
+	btVector3 va = getSplitVa();
+	btScalar p = m_penetration;
+	if (p > 0)
+	{
+		return 0;
+	}
+	btVector3 vr = vb - va;
+	btScalar dn = btDot(vr, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
+	if (dn > 0)
+	{
+		return 0;
+	}
+	if (m_total_split_impulse + dn > MAX_PENETRATION_CORRECTION)
+	{
+		dn = MAX_PENETRATION_CORRECTION - m_total_split_impulse;
+	}
+	if (m_total_split_impulse + dn < -MAX_PENETRATION_CORRECTION)
+	{
+		dn = -MAX_PENETRATION_CORRECTION - m_total_split_impulse;
+	}
+	m_total_split_impulse += dn;
+
+	btScalar residualSquare = dn * dn;
+	const btVector3 impulse = m_contact->m_c0 * (cti.m_normal * dn);
+	applySplitImpulse(impulse);
+
+	// apply split impulse to the rigid/multibodies involved and change their velocities
+	if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+	{
+		btRigidBody* rigidCol = 0;
+		rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+		if (rigidCol)
+		{
+			rigidCol->applyPushImpulse(impulse, m_contact->m_c1);
+		}
+	}
+	else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+	{
+		btMultiBodyLinkCollider* multibodyLinkCol = 0;
+		multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+		if (multibodyLinkCol)
+		{
+			const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+			// apply normal component of the impulse
+			multibodyLinkCol->m_multiBody->applyDeltaSplitVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
+		}
+	}
+	return residualSquare;
+}
+/* ================   Node vs. Rigid   =================== */
+btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal)
+	: m_node(contact.m_node), btDeformableRigidContactConstraint(contact, infoGlobal)
+{
+}
+
+btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other)
+	: m_node(other.m_node), btDeformableRigidContactConstraint(other)
+{
+}
+
+btVector3 btDeformableNodeRigidContactConstraint::getVb() const
+{
+	return m_node->m_v;
+}
+
+btVector3 btDeformableNodeRigidContactConstraint::getSplitVb() const
+{
+	return m_node->m_splitv;
+}
+
+btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
+{
+	return m_total_normal_dv + m_total_tangent_dv;
+}
+
+void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
+{
+	const btSoftBody::DeformableNodeRigidContact* contact = getContact();
+	btVector3 dv = contact->m_c5 * impulse;
+	contact->m_node->m_v -= dv;
+}
+
+void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
+{
+	const btSoftBody::DeformableNodeRigidContact* contact = getContact();
+	btVector3 dv = contact->m_c5 * impulse;
+	contact->m_node->m_splitv -= dv;
+}
+
+/* ================   Face vs. Rigid   =================== */
+btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting)
+	: m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btDeformableRigidContactConstraint(contact, infoGlobal)
+{
+}
+
+btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other)
+	: m_face(other.m_face), m_useStrainLimiting(other.m_useStrainLimiting), btDeformableRigidContactConstraint(other)
+{
+}
+
+btVector3 btDeformableFaceRigidContactConstraint::getVb() const
+{
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
+	return vb;
+}
+
+btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
+{
+	btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	if (m_face->m_n[0] == node)
+	{
+		return face_dv * contact->m_weights[0];
+	}
+	if (m_face->m_n[1] == node)
+	{
+		return face_dv * contact->m_weights[1];
+	}
+	btAssert(node == m_face->m_n[2]);
+	return face_dv * contact->m_weights[2];
+}
+
+void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse)
+{
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	btVector3 dv = impulse * contact->m_c2;
+	btSoftBody::Face* face = contact->m_face;
+
+	btVector3& v0 = face->m_n[0]->m_v;
+	btVector3& v1 = face->m_n[1]->m_v;
+	btVector3& v2 = face->m_n[2]->m_v;
+	const btScalar& im0 = face->m_n[0]->m_im;
+	const btScalar& im1 = face->m_n[1]->m_im;
+	const btScalar& im2 = face->m_n[2]->m_im;
+	if (im0 > 0)
+		v0 -= dv * contact->m_weights[0];
+	if (im1 > 0)
+		v1 -= dv * contact->m_weights[1];
+	if (im2 > 0)
+		v2 -= dv * contact->m_weights[2];
+	if (m_useStrainLimiting)
+	{
+		btScalar relaxation = 1. / btScalar(m_infoGlobal->m_numIterations);
+		btScalar m01 = (relaxation / (im0 + im1));
+		btScalar m02 = (relaxation / (im0 + im2));
+		btScalar m12 = (relaxation / (im1 + im2));
+#ifdef USE_STRAIN_RATE_LIMITING
+		// apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%.
+		btScalar p = 0.01;
+		btVector3& x0 = face->m_n[0]->m_x;
+		btVector3& x1 = face->m_n[1]->m_x;
+		btVector3& x2 = face->m_n[2]->m_x;
+		const btVector3 x_diff[3] = {x1 - x0, x2 - x0, x2 - x1};
+		const btVector3 v_diff[3] = {v1 - v0, v2 - v0, v2 - v1};
+		btVector3 u[3];
+		btScalar x_diff_dot_u, dn[3];
+		btScalar dt = m_infoGlobal->m_timeStep;
+		for (int i = 0; i < 3; ++i)
+		{
+			btScalar x_diff_norm = x_diff[i].safeNorm();
+			btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm();
+			btScalar strainRate = x_diff_norm_new / x_diff_norm;
+			u[i] = v_diff[i];
+			u[i].safeNormalize();
+			if (x_diff_norm == 0 || (1 - p <= strainRate && strainRate <= 1 + p))
+			{
+				dn[i] = 0;
+				continue;
+			}
+			x_diff_dot_u = btDot(x_diff[i], u[i]);
+			btScalar s;
+			if (1 - p > strainRate)
+			{
+				s = 1 / dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p - 2 * p) * x_diff_norm * x_diff_norm));
+			}
+			else
+			{
+				s = 1 / dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p + 2 * p) * x_diff_norm * x_diff_norm));
+			}
+			//		x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm();
+			//		strainRate = x_diff_norm_new/x_diff_norm;
+			dn[i] = s - v_diff[i].safeNorm();
+		}
+		btVector3 dv0 = im0 * (m01 * u[0] * (-dn[0]) + m02 * u[1] * -(dn[1]));
+		btVector3 dv1 = im1 * (m01 * u[0] * (dn[0]) + m12 * u[2] * (-dn[2]));
+		btVector3 dv2 = im2 * (m12 * u[2] * (dn[2]) + m02 * u[1] * (dn[1]));
+#else
+		// apply strain limiting to prevent undamped modes
+		btVector3 dv0 = im0 * (m01 * (v1 - v0) + m02 * (v2 - v0));
+		btVector3 dv1 = im1 * (m01 * (v0 - v1) + m12 * (v2 - v1));
+		btVector3 dv2 = im2 * (m12 * (v1 - v2) + m02 * (v0 - v2));
+#endif
+		v0 += dv0;
+		v1 += dv1;
+		v2 += dv2;
+	}
+}
+
+btVector3 btDeformableFaceRigidContactConstraint::getSplitVb() const
+{
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv) * contact->m_bary[2];
+	return vb;
+}
+
+void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
+{
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	btVector3 dv = impulse * contact->m_c2;
+	btSoftBody::Face* face = contact->m_face;
+	btVector3& v0 = face->m_n[0]->m_splitv;
+	btVector3& v1 = face->m_n[1]->m_splitv;
+	btVector3& v2 = face->m_n[2]->m_splitv;
+	const btScalar& im0 = face->m_n[0]->m_im;
+	const btScalar& im1 = face->m_n[1]->m_im;
+	const btScalar& im2 = face->m_n[2]->m_im;
+	if (im0 > 0)
+	{
+		v0 -= dv * contact->m_weights[0];
+	}
+	if (im1 > 0)
+	{
+		v1 -= dv * contact->m_weights[1];
+	}
+	if (im2 > 0)
+	{
+		v2 -= dv * contact->m_weights[2];
+	}
+}
+
+/* ================   Face vs. Node   =================== */
+btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal)
+	: m_node(contact.m_node), m_face(contact.m_face), m_contact(&contact), btDeformableContactConstraint(contact.m_normal, infoGlobal)
+{
+	m_total_normal_dv.setZero();
+	m_total_tangent_dv.setZero();
+}
+
+btVector3 btDeformableFaceNodeContactConstraint::getVa() const
+{
+	return m_node->m_v;
+}
+
+btVector3 btDeformableFaceNodeContactConstraint::getVb() const
+{
+	const btSoftBody::DeformableFaceNodeContact* contact = getContact();
+	btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
+	return vb;
+}
+
+btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n) const
+{
+	btVector3 dv = m_total_normal_dv + m_total_tangent_dv;
+	if (n == m_node)
+		return dv;
+	const btSoftBody::DeformableFaceNodeContact* contact = getContact();
+	if (m_face->m_n[0] == n)
+	{
+		return dv * contact->m_weights[0];
+	}
+	if (m_face->m_n[1] == n)
+	{
+		return dv * contact->m_weights[1];
+	}
+	btAssert(n == m_face->m_n[2]);
+	return dv * contact->m_weights[2];
+}
+
+btScalar btDeformableFaceNodeContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
+{
+	btVector3 va = getVa();
+	btVector3 vb = getVb();
+	btVector3 vr = vb - va;
+	const btScalar dn = btDot(vr, m_contact->m_normal);
+	// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+	btScalar residualSquare = dn * dn;
+	btVector3 impulse = m_contact->m_c0 * vr;
+	const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn);
+	btVector3 impulse_tangent = impulse - impulse_normal;
+
+	btVector3 old_total_tangent_dv = m_total_tangent_dv;
+	// m_c2 is the inverse mass of the deformable node/face
+	if (m_node->m_im > 0)
+	{
+		m_total_normal_dv -= impulse_normal * m_node->m_im;
+		m_total_tangent_dv -= impulse_tangent * m_node->m_im;
+	}
+	else
+	{
+		m_total_normal_dv -= impulse_normal * m_contact->m_imf;
+		m_total_tangent_dv -= impulse_tangent * m_contact->m_imf;
+	}
+
+	if (m_total_normal_dv.dot(m_contact->m_normal) > 0)
+	{
+		// separating in the normal direction
+		m_static = false;
+		m_total_tangent_dv = btVector3(0, 0, 0);
+		impulse_tangent.setZero();
+	}
+	else
+	{
+		if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm())
+		{
+			// dynamic friction
+			// with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
+			m_static = false;
+			if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
+			{
+				m_total_tangent_dv = btVector3(0, 0, 0);
+			}
+			else
+			{
+				m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction;
+			}
+			impulse_tangent = -btScalar(1) / m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv);
+		}
+		else
+		{
+			// static friction
+			m_static = true;
+		}
+	}
+	impulse = impulse_normal + impulse_tangent;
+	// apply impulse to deformable nodes involved and change their velocities
+	applyImpulse(impulse);
+	return residualSquare;
+}
+
+void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impulse)
+{
+	const btSoftBody::DeformableFaceNodeContact* contact = getContact();
+	btVector3 dva = impulse * contact->m_node->m_im;
+	btVector3 dvb = impulse * contact->m_imf;
+	if (contact->m_node->m_im > 0)
+	{
+		contact->m_node->m_v += dva;
+	}
+
+	btSoftBody::Face* face = contact->m_face;
+	btVector3& v0 = face->m_n[0]->m_v;
+	btVector3& v1 = face->m_n[1]->m_v;
+	btVector3& v2 = face->m_n[2]->m_v;
+	const btScalar& im0 = face->m_n[0]->m_im;
+	const btScalar& im1 = face->m_n[1]->m_im;
+	const btScalar& im2 = face->m_n[2]->m_im;
+	if (im0 > 0)
+	{
+		v0 -= dvb * contact->m_weights[0];
+	}
+	if (im1 > 0)
+	{
+		v1 -= dvb * contact->m_weights[1];
+	}
+	if (im2 > 0)
+	{
+		v2 -= dvb * contact->m_weights[2];
+	}
+}
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableContactConstraint.h b/extern/bullet2/src/BulletSoftBody/btDeformableContactConstraint.h
new file mode 100644
index 00000000000..1e2c9f5bce4
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableContactConstraint.h
@@ -0,0 +1,284 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_DEFORMABLE_CONTACT_CONSTRAINT_H
+#define BT_DEFORMABLE_CONTACT_CONSTRAINT_H
+#include "btSoftBody.h"
+
+// btDeformableContactConstraint is an abstract class specifying the method that each type of contact constraint needs to implement
+class btDeformableContactConstraint
+{
+public:
+	// True if the friction is static
+	// False if the friction is dynamic
+	bool m_static;
+	const btContactSolverInfo* m_infoGlobal;
+
+	// normal of the contact
+	btVector3 m_normal;
+
+	btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal)
+	{
+	}
+
+	btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal)
+	{
+	}
+
+	btDeformableContactConstraint() {}
+
+	btDeformableContactConstraint(const btDeformableContactConstraint& other)
+		: m_static(other.m_static), m_normal(other.m_normal), m_infoGlobal(other.m_infoGlobal)
+	{
+	}
+
+	virtual ~btDeformableContactConstraint() {}
+
+	// solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence
+	// the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact
+	virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0;
+
+	// get the velocity of the object A in the contact
+	virtual btVector3 getVa() const = 0;
+
+	// get the velocity of the object B in the contact
+	virtual btVector3 getVb() const = 0;
+
+	// get the velocity change of the soft body node in the constraint
+	virtual btVector3 getDv(const btSoftBody::Node*) const = 0;
+
+	// apply impulse to the soft body node and/or face involved
+	virtual void applyImpulse(const btVector3& impulse) = 0;
+
+	// scale the penetration depth by erp
+	virtual void setPenetrationScale(btScalar scale) = 0;
+};
+
+//
+// Constraint that a certain node in the deformable objects cannot move
+class btDeformableStaticConstraint : public btDeformableContactConstraint
+{
+public:
+	btSoftBody::Node* m_node;
+
+	btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal) : m_node(node), btDeformableContactConstraint(false, btVector3(0, 0, 0), infoGlobal)
+	{
+	}
+	btDeformableStaticConstraint() {}
+	btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
+		: m_node(other.m_node), btDeformableContactConstraint(other)
+	{
+	}
+
+	virtual ~btDeformableStaticConstraint() {}
+
+	virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal)
+	{
+		return 0;
+	}
+
+	virtual btVector3 getVa() const
+	{
+		return btVector3(0, 0, 0);
+	}
+
+	virtual btVector3 getVb() const
+	{
+		return btVector3(0, 0, 0);
+	}
+
+	virtual btVector3 getDv(const btSoftBody::Node* n) const
+	{
+		return btVector3(0, 0, 0);
+	}
+
+	virtual void applyImpulse(const btVector3& impulse) {}
+	virtual void setPenetrationScale(btScalar scale) {}
+};
+
+//
+// Anchor Constraint between rigid and deformable node
+class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint
+{
+public:
+	const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
+
+	btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal);
+	btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
+	btDeformableNodeAnchorConstraint() {}
+	virtual ~btDeformableNodeAnchorConstraint()
+	{
+	}
+	virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
+
+	// object A is the rigid/multi body, and object B is the deformable node/face
+	virtual btVector3 getVa() const;
+	// get the velocity of the deformable node in contact
+	virtual btVector3 getVb() const;
+	virtual btVector3 getDv(const btSoftBody::Node* n) const
+	{
+		return btVector3(0, 0, 0);
+	}
+	virtual void applyImpulse(const btVector3& impulse);
+
+	virtual void setPenetrationScale(btScalar scale) {}
+};
+
+//
+// Constraint between rigid/multi body and deformable objects
+class btDeformableRigidContactConstraint : public btDeformableContactConstraint
+{
+public:
+	btVector3 m_total_normal_dv;
+	btVector3 m_total_tangent_dv;
+	btScalar m_penetration;
+	btScalar m_total_split_impulse;
+	bool m_binding;
+	const btSoftBody::DeformableRigidContact* m_contact;
+
+	btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal);
+	btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other);
+	btDeformableRigidContactConstraint() {}
+	virtual ~btDeformableRigidContactConstraint()
+	{
+	}
+
+	// object A is the rigid/multi body, and object B is the deformable node/face
+	virtual btVector3 getVa() const;
+
+	// get the split impulse velocity of the deformable face at the contact point
+	virtual btVector3 getSplitVb() const = 0;
+
+	// get the split impulse velocity of the rigid/multibdoy at the contaft
+	virtual btVector3 getSplitVa() const;
+
+	virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
+
+	virtual void setPenetrationScale(btScalar scale)
+	{
+		m_penetration *= scale;
+	}
+
+	btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
+
+	virtual void applySplitImpulse(const btVector3& impulse) = 0;
+};
+
+//
+// Constraint between rigid/multi body and deformable objects nodes
+class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactConstraint
+{
+public:
+	// the deformable node in contact
+	btSoftBody::Node* m_node;
+
+	btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal);
+	btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other);
+	btDeformableNodeRigidContactConstraint() {}
+	virtual ~btDeformableNodeRigidContactConstraint()
+	{
+	}
+
+	// get the velocity of the deformable node in contact
+	virtual btVector3 getVb() const;
+
+	// get the split impulse velocity of the deformable face at the contact point
+	virtual btVector3 getSplitVb() const;
+
+	// get the velocity change of the input soft body node in the constraint
+	virtual btVector3 getDv(const btSoftBody::Node*) const;
+
+	// cast the contact to the desired type
+	const btSoftBody::DeformableNodeRigidContact* getContact() const
+	{
+		return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact);
+	}
+
+	virtual void applyImpulse(const btVector3& impulse);
+
+	virtual void applySplitImpulse(const btVector3& impulse);
+};
+
+//
+// Constraint between rigid/multi body and deformable objects faces
+class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint
+{
+public:
+	btSoftBody::Face* m_face;
+	bool m_useStrainLimiting;
+	btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting);
+	btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
+	btDeformableFaceRigidContactConstraint() : m_useStrainLimiting(false) {}
+	virtual ~btDeformableFaceRigidContactConstraint()
+	{
+	}
+
+	// get the velocity of the deformable face at the contact point
+	virtual btVector3 getVb() const;
+
+	// get the split impulse velocity of the deformable face at the contact point
+	virtual btVector3 getSplitVb() const;
+
+	// get the velocity change of the input soft body node in the constraint
+	virtual btVector3 getDv(const btSoftBody::Node*) const;
+
+	// cast the contact to the desired type
+	const btSoftBody::DeformableFaceRigidContact* getContact() const
+	{
+		return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact);
+	}
+
+	virtual void applyImpulse(const btVector3& impulse);
+
+	virtual void applySplitImpulse(const btVector3& impulse);
+};
+
+//
+// Constraint between  deformable objects faces and deformable objects nodes
+class btDeformableFaceNodeContactConstraint : public btDeformableContactConstraint
+{
+public:
+	btSoftBody::Node* m_node;
+	btSoftBody::Face* m_face;
+	const btSoftBody::DeformableFaceNodeContact* m_contact;
+	btVector3 m_total_normal_dv;
+	btVector3 m_total_tangent_dv;
+
+	btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal);
+	btDeformableFaceNodeContactConstraint() {}
+	virtual ~btDeformableFaceNodeContactConstraint() {}
+
+	virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
+
+	// get the velocity of the object A in the contact
+	virtual btVector3 getVa() const;
+
+	// get the velocity of the object B in the contact
+	virtual btVector3 getVb() const;
+
+	// get the velocity change of the input soft body node in the constraint
+	virtual btVector3 getDv(const btSoftBody::Node*) const;
+
+	// cast the contact to the desired type
+	const btSoftBody::DeformableFaceNodeContact* getContact() const
+	{
+		return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact);
+	}
+
+	virtual void applyImpulse(const btVector3& impulse);
+
+	virtual void setPenetrationScale(btScalar scale) {}
+};
+#endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableContactProjection.cpp b/extern/bullet2/src/BulletSoftBody/btDeformableContactProjection.cpp
new file mode 100644
index 00000000000..7f67260ce6c
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -0,0 +1,639 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "btDeformableContactProjection.h"
+#include "btDeformableMultiBodyDynamicsWorld.h"
+#include <algorithm>
+#include <cmath>
+btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
+{
+	btScalar residualSquare = 0;
+	for (int i = 0; i < numDeformableBodies; ++i)
+	{
+		for (int j = 0; j < m_softBodies.size(); ++j)
+		{
+			btCollisionObject* psb = m_softBodies[j];
+			if (psb != deformableBodies[i])
+			{
+				continue;
+			}
+			for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
+			{
+				btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
+				residualSquare = btMax(residualSquare, localResidualSquare);
+			}
+			for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
+			{
+				btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k];
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
+				residualSquare = btMax(residualSquare, localResidualSquare);
+			}
+			for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
+			{
+				btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
+				residualSquare = btMax(residualSquare, localResidualSquare);
+			}
+			for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
+			{
+				btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k];
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
+				residualSquare = btMax(residualSquare, localResidualSquare);
+			}
+		}
+	}
+	return residualSquare;
+}
+
+btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
+{
+	btScalar residualSquare = 0;
+	for (int i = 0; i < numDeformableBodies; ++i)
+	{
+		for (int j = 0; j < m_softBodies.size(); ++j)
+		{
+			btCollisionObject* psb = m_softBodies[j];
+			if (psb != deformableBodies[i])
+			{
+				continue;
+			}
+			for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
+			{
+				btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
+				btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
+				residualSquare = btMax(residualSquare, localResidualSquare);
+			}
+			for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
+			{
+				btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
+				btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
+				residualSquare = btMax(residualSquare, localResidualSquare);
+			}
+		}
+	}
+	return residualSquare;
+}
+
+void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("setConstraints");
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			continue;
+		}
+
+		// set Dirichlet constraint
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			if (psb->m_nodes[j].m_im == 0)
+			{
+				btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal);
+				m_staticConstraints[i].push_back(static_constraint);
+			}
+		}
+
+		// set up deformable anchors
+		for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
+		{
+			btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j];
+			// skip fixed points
+			if (anchor.m_node->m_im == 0)
+			{
+				continue;
+			}
+			anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
+			btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal);
+			m_nodeAnchorConstraints[i].push_back(constraint);
+		}
+
+		// set Deformable Node vs. Rigid constraint
+		for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
+		{
+			const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
+			// skip fixed points
+			if (contact.m_node->m_im == 0)
+			{
+				continue;
+			}
+			btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
+			m_nodeRigidConstraints[i].push_back(constraint);
+		}
+
+		// set Deformable Face vs. Rigid constraint
+		for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
+		{
+			const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j];
+			// skip fixed faces
+			if (contact.m_c2 == 0)
+			{
+				continue;
+			}
+			btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting);
+			m_faceRigidConstraints[i].push_back(constraint);
+		}
+	}
+}
+
+void btDeformableContactProjection::project(TVStack& x)
+{
+#ifndef USE_MGS
+	const int dim = 3;
+	for (int index = 0; index < m_projectionsDict.size(); ++index)
+	{
+		btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
+		size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
+		if (projectionDirs.size() >= dim)
+		{
+			// static node
+			x[i].setZero();
+			continue;
+		}
+		else if (projectionDirs.size() == 2)
+		{
+			btVector3 dir0 = projectionDirs[0];
+			btVector3 dir1 = projectionDirs[1];
+			btVector3 free_dir = btCross(dir0, dir1);
+			if (free_dir.safeNorm() < SIMD_EPSILON)
+			{
+				x[i] -= x[i].dot(dir0) * dir0;
+			}
+			else
+			{
+				free_dir.normalize();
+				x[i] = x[i].dot(free_dir) * free_dir;
+			}
+		}
+		else
+		{
+			btAssert(projectionDirs.size() == 1);
+			btVector3 dir0 = projectionDirs[0];
+			x[i] -= x[i].dot(dir0) * dir0;
+		}
+	}
+#else
+	btReducedVector p(x.size());
+	for (int i = 0; i < m_projections.size(); ++i)
+	{
+		p += (m_projections[i].dot(x) * m_projections[i]);
+	}
+	for (int i = 0; i < p.m_indices.size(); ++i)
+	{
+		x[p.m_indices[i]] -= p.m_vecs[i];
+	}
+#endif
+}
+
+void btDeformableContactProjection::setProjection()
+{
+#ifndef USE_MGS
+	BT_PROFILE("btDeformableContactProjection::setProjection");
+	btAlignedObjectArray<btVector3> units;
+	units.push_back(btVector3(1, 0, 0));
+	units.push_back(btVector3(0, 1, 0));
+	units.push_back(btVector3(0, 0, 1));
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			continue;
+		}
+		for (int j = 0; j < m_staticConstraints[i].size(); ++j)
+		{
+			int index = m_staticConstraints[i][j].m_node->index;
+			m_staticConstraints[i][j].m_node->m_constrained = true;
+			if (m_projectionsDict.find(index) == NULL)
+			{
+				m_projectionsDict.insert(index, units);
+			}
+			else
+			{
+				btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+				for (int k = 0; k < 3; ++k)
+				{
+					projections.push_back(units[k]);
+				}
+			}
+		}
+		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
+		{
+			int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
+			m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
+			if (m_projectionsDict.find(index) == NULL)
+			{
+				m_projectionsDict.insert(index, units);
+			}
+			else
+			{
+				btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+				for (int k = 0; k < 3; ++k)
+				{
+					projections.push_back(units[k]);
+				}
+			}
+		}
+		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+		{
+			int index = m_nodeRigidConstraints[i][j].m_node->index;
+			m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
+			if (m_nodeRigidConstraints[i][j].m_binding)
+			{
+				if (m_nodeRigidConstraints[i][j].m_static)
+				{
+					if (m_projectionsDict.find(index) == NULL)
+					{
+						m_projectionsDict.insert(index, units);
+					}
+					else
+					{
+						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+						for (int k = 0; k < 3; ++k)
+						{
+							projections.push_back(units[k]);
+						}
+					}
+				}
+				else
+				{
+					if (m_projectionsDict.find(index) == NULL)
+					{
+						btAlignedObjectArray<btVector3> projections;
+						projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
+						m_projectionsDict.insert(index, projections);
+					}
+					else
+					{
+						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+						projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
+					}
+				}
+			}
+		}
+		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
+		{
+			const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
+			if (m_faceRigidConstraints[i][j].m_binding)
+			{
+				for (int k = 0; k < 3; ++k)
+				{
+					face->m_n[k]->m_constrained = true;
+				}
+			}
+			for (int k = 0; k < 3; ++k)
+			{
+				btSoftBody::Node* node = face->m_n[k];
+				int index = node->index;
+				if (m_faceRigidConstraints[i][j].m_static)
+				{
+					if (m_projectionsDict.find(index) == NULL)
+					{
+						m_projectionsDict.insert(index, units);
+					}
+					else
+					{
+						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+						for (int l = 0; l < 3; ++l)
+						{
+							projections.push_back(units[l]);
+						}
+					}
+				}
+				else
+				{
+					if (m_projectionsDict.find(index) == NULL)
+					{
+						btAlignedObjectArray<btVector3> projections;
+						projections.push_back(m_faceRigidConstraints[i][j].m_normal);
+						m_projectionsDict.insert(index, projections);
+					}
+					else
+					{
+						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+						projections.push_back(m_faceRigidConstraints[i][j].m_normal);
+					}
+				}
+			}
+		}
+	}
+#else
+	int dof = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		dof += m_softBodies[i]->m_nodes.size();
+	}
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			continue;
+		}
+		for (int j = 0; j < m_staticConstraints[i].size(); ++j)
+		{
+			int index = m_staticConstraints[i][j].m_node->index;
+			m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
+			btAlignedObjectArray<int> indices;
+			btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
+			indices.push_back(index);
+			vecs1.push_back(btVector3(1, 0, 0));
+			vecs2.push_back(btVector3(0, 1, 0));
+			vecs3.push_back(btVector3(0, 0, 1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs2));
+			m_projections.push_back(btReducedVector(dof, indices, vecs3));
+		}
+
+		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
+		{
+			int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
+			m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
+			btAlignedObjectArray<int> indices;
+			btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
+			indices.push_back(index);
+			vecs1.push_back(btVector3(1, 0, 0));
+			vecs2.push_back(btVector3(0, 1, 0));
+			vecs3.push_back(btVector3(0, 0, 1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs2));
+			m_projections.push_back(btReducedVector(dof, indices, vecs3));
+		}
+		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+		{
+			int index = m_nodeRigidConstraints[i][j].m_node->index;
+			m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
+			btAlignedObjectArray<int> indices;
+			indices.push_back(index);
+			btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
+			if (m_nodeRigidConstraints[i][j].m_static)
+			{
+				vecs1.push_back(btVector3(1, 0, 0));
+				vecs2.push_back(btVector3(0, 1, 0));
+				vecs3.push_back(btVector3(0, 0, 1));
+				m_projections.push_back(btReducedVector(dof, indices, vecs1));
+				m_projections.push_back(btReducedVector(dof, indices, vecs2));
+				m_projections.push_back(btReducedVector(dof, indices, vecs3));
+			}
+			else
+			{
+				vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal);
+				m_projections.push_back(btReducedVector(dof, indices, vecs1));
+			}
+		}
+		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
+		{
+			const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
+			btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
+			btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
+			for (int k = 0; k < 3; ++k)
+			{
+				face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
+			}
+			if (m_faceRigidConstraints[i][j].m_static)
+			{
+				for (int l = 0; l < 3; ++l)
+				{
+					btReducedVector rv(dof);
+					for (int k = 0; k < 3; ++k)
+					{
+						rv.m_indices.push_back(face->m_n[k]->index);
+						btVector3 v(0, 0, 0);
+						v[l] = bary[k];
+						rv.m_vecs.push_back(v);
+						rv.sort();
+					}
+					m_projections.push_back(rv);
+				}
+			}
+			else
+			{
+				btReducedVector rv(dof);
+				for (int k = 0; k < 3; ++k)
+				{
+					rv.m_indices.push_back(face->m_n[k]->index);
+					rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal);
+					rv.sort();
+				}
+				m_projections.push_back(rv);
+			}
+		}
+	}
+	btModifiedGramSchmidt<btReducedVector> mgs(m_projections);
+	mgs.solve();
+	m_projections = mgs.m_out;
+#endif
+}
+
+void btDeformableContactProjection::checkConstraints(const TVStack& x)
+{
+	for (int i = 0; i < m_lagrangeMultipliers.size(); ++i)
+	{
+		btVector3 d(0, 0, 0);
+		const LagrangeMultiplier& lm = m_lagrangeMultipliers[i];
+		for (int j = 0; j < lm.m_num_constraints; ++j)
+		{
+			for (int k = 0; k < lm.m_num_nodes; ++k)
+			{
+				d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]);
+			}
+		}
+		//		printf("d = %f, %f, %f\n", d[0], d[1], d[2]);
+		//        printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]);
+	}
+}
+
+void btDeformableContactProjection::setLagrangeMultiplier()
+{
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			continue;
+		}
+		for (int j = 0; j < m_staticConstraints[i].size(); ++j)
+		{
+			int index = m_staticConstraints[i][j].m_node->index;
+			m_staticConstraints[i][j].m_node->m_constrained = true;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 1;
+			lm.m_indices[0] = index;
+			lm.m_weights[0] = 1.0;
+			lm.m_num_constraints = 3;
+			lm.m_dirs[0] = btVector3(1, 0, 0);
+			lm.m_dirs[1] = btVector3(0, 1, 0);
+			lm.m_dirs[2] = btVector3(0, 0, 1);
+			m_lagrangeMultipliers.push_back(lm);
+		}
+		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
+		{
+			int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
+			m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 1;
+			lm.m_indices[0] = index;
+			lm.m_weights[0] = 1.0;
+			lm.m_num_constraints = 3;
+			lm.m_dirs[0] = btVector3(1, 0, 0);
+			lm.m_dirs[1] = btVector3(0, 1, 0);
+			lm.m_dirs[2] = btVector3(0, 0, 1);
+			m_lagrangeMultipliers.push_back(lm);
+		}
+
+		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+		{
+			if (!m_nodeRigidConstraints[i][j].m_binding)
+			{
+				continue;
+			}
+			int index = m_nodeRigidConstraints[i][j].m_node->index;
+			m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 1;
+			lm.m_indices[0] = index;
+			lm.m_weights[0] = 1.0;
+			if (m_nodeRigidConstraints[i][j].m_static)
+			{
+				lm.m_num_constraints = 3;
+				lm.m_dirs[0] = btVector3(1, 0, 0);
+				lm.m_dirs[1] = btVector3(0, 1, 0);
+				lm.m_dirs[2] = btVector3(0, 0, 1);
+			}
+			else
+			{
+				lm.m_num_constraints = 1;
+				lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal;
+			}
+			m_lagrangeMultipliers.push_back(lm);
+		}
+
+		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
+		{
+			if (!m_faceRigidConstraints[i][j].m_binding)
+			{
+				continue;
+			}
+			btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
+
+			btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 3;
+
+			for (int k = 0; k < 3; ++k)
+			{
+				face->m_n[k]->m_constrained = true;
+				lm.m_indices[k] = face->m_n[k]->index;
+				lm.m_weights[k] = bary[k];
+			}
+			if (m_faceRigidConstraints[i][j].m_static)
+			{
+				face->m_pcontact[3] = 1;
+				lm.m_num_constraints = 3;
+				lm.m_dirs[0] = btVector3(1, 0, 0);
+				lm.m_dirs[1] = btVector3(0, 1, 0);
+				lm.m_dirs[2] = btVector3(0, 0, 1);
+			}
+			else
+			{
+				face->m_pcontact[3] = 0;
+				lm.m_num_constraints = 1;
+				lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
+			}
+			m_lagrangeMultipliers.push_back(lm);
+		}
+	}
+}
+
+//
+void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
+{
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+		{
+			const btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
+			const btSoftBody::Node* node = constraint.m_node;
+			if (node->m_im != 0)
+			{
+				int index = node->index;
+				f[index] += constraint.getDv(node) * (1. / node->m_im);
+			}
+		}
+		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
+		{
+			const btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
+			const btSoftBody::Face* face = constraint.getContact()->m_face;
+			for (int k = 0; k < 3; ++k)
+			{
+				const btSoftBody::Node* node = face->m_n[k];
+				if (node->m_im != 0)
+				{
+					int index = node->index;
+					f[index] += constraint.getDv(node) * (1. / node->m_im);
+				}
+			}
+		}
+		for (int j = 0; j < m_deformableConstraints[i].size(); ++j)
+		{
+			const btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[i][j];
+			const btSoftBody::Face* face = constraint.getContact()->m_face;
+			const btSoftBody::Node* node = constraint.getContact()->m_node;
+			if (node->m_im != 0)
+			{
+				int index = node->index;
+				f[index] += constraint.getDv(node) * (1. / node->m_im);
+			}
+			for (int k = 0; k < 3; ++k)
+			{
+				const btSoftBody::Node* node = face->m_n[k];
+				if (node->m_im != 0)
+				{
+					int index = node->index;
+					f[index] += constraint.getDv(node) * (1. / node->m_im);
+				}
+			}
+		}
+	}
+}
+
+void btDeformableContactProjection::reinitialize(bool nodeUpdated)
+{
+	int N = m_softBodies.size();
+	if (nodeUpdated)
+	{
+		m_staticConstraints.resize(N);
+		m_nodeAnchorConstraints.resize(N);
+		m_nodeRigidConstraints.resize(N);
+		m_faceRigidConstraints.resize(N);
+		m_deformableConstraints.resize(N);
+	}
+	for (int i = 0; i < N; ++i)
+	{
+		m_staticConstraints[i].clear();
+		m_nodeAnchorConstraints[i].clear();
+		m_nodeRigidConstraints[i].clear();
+		m_faceRigidConstraints[i].clear();
+		m_deformableConstraints[i].clear();
+	}
+#ifndef USE_MGS
+	m_projectionsDict.clear();
+#else
+	m_projections.clear();
+#endif
+	m_lagrangeMultipliers.clear();
+}
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableContactProjection.h b/extern/bullet2/src/BulletSoftBody/btDeformableContactProjection.h
new file mode 100644
index 00000000000..4964eaf990a
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableContactProjection.h
@@ -0,0 +1,99 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_CONTACT_PROJECTION_H
+#define BT_CONTACT_PROJECTION_H
+#include "btCGProjection.h"
+#include "btSoftBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+#include "btDeformableContactConstraint.h"
+#include "LinearMath/btHashMap.h"
+#include "LinearMath/btReducedVector.h"
+#include "LinearMath/btModifiedGramSchmidt.h"
+#include <vector>
+
+struct LagrangeMultiplier
+{
+	int m_num_constraints;  // Number of constraints
+	int m_num_nodes;        // Number of nodes in these constraints
+	btScalar m_weights[3];  // weights of the nodes involved, same size as m_num_nodes
+	btVector3 m_dirs[3];    // Constraint directions, same size of m_num_constraints;
+	int m_indices[3];       // indices of the nodes involved, same size as m_num_nodes;
+};
+
+class btDeformableContactProjection
+{
+public:
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btAlignedObjectArray<btSoftBody*>& m_softBodies;
+
+	// all constraints involving face
+	btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
+#ifndef USE_MGS
+	// map from node index to projection directions
+	btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
+#else
+	btAlignedObjectArray<btReducedVector> m_projections;
+#endif
+
+	btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers;
+
+	// map from node index to static constraint
+	btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints;
+	// map from node index to node rigid constraint
+	btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
+	// map from node index to face rigid constraint
+	btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceRigidContactConstraint> > m_faceRigidConstraints;
+	// map from node index to deformable constraint
+	btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceNodeContactConstraint> > m_deformableConstraints;
+	// map from node index to node anchor constraint
+	btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints;
+
+	bool m_useStrainLimiting;
+
+	btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& softBodies)
+		: m_softBodies(softBodies)
+	{
+	}
+
+	virtual ~btDeformableContactProjection()
+	{
+	}
+
+	// apply the constraints to the rhs of the linear solve
+	virtual void project(TVStack& x);
+
+	// add friction force to the rhs of the linear solve
+	virtual void applyDynamicFriction(TVStack& f);
+
+	// update and solve the constraints
+	virtual btScalar update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
+
+	// Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
+	virtual void setConstraints(const btContactSolverInfo& infoGlobal);
+
+	// Set up projections for each vertex by adding the projection direction to
+	virtual void setProjection();
+
+	virtual void reinitialize(bool nodeUpdated);
+
+	btScalar solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
+
+	virtual void setLagrangeMultiplier();
+
+	void checkConstraints(const TVStack& x);
+};
+#endif /* btDeformableContactProjection_h */
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableCorotatedForce.h b/extern/bullet2/src/BulletSoftBody/btDeformableCorotatedForce.h
new file mode 100644
index 00000000000..dfd85523bc2
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableCorotatedForce.h
@@ -0,0 +1,124 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_COROTATED_H
+#define BT_COROTATED_H
+
+#include "btDeformableLagrangianForce.h"
+#include "LinearMath/btPolarDecomposition.h"
+
+static inline int PolarDecomposition(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
+{
+	static const btPolarDecomposition polar;
+	return polar.decompose(m, q, s);
+}
+
+class btDeformableCorotatedForce : public btDeformableLagrangianForce
+{
+public:
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btScalar m_mu, m_lambda;
+	btDeformableCorotatedForce() : m_mu(1), m_lambda(1)
+	{
+	}
+
+	btDeformableCorotatedForce(btScalar mu, btScalar lambda) : m_mu(mu), m_lambda(lambda)
+	{
+	}
+
+	virtual void addScaledForces(btScalar scale, TVStack& force)
+	{
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+	{
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+	{
+	}
+
+	virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= force.size());
+		btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_tetras.size(); ++j)
+			{
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btMatrix3x3 P;
+				firstPiola(tetra.m_F, P);
+				btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col);
+				btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
+
+				btSoftBody::Node* node0 = tetra.m_n[0];
+				btSoftBody::Node* node1 = tetra.m_n[1];
+				btSoftBody::Node* node2 = tetra.m_n[2];
+				btSoftBody::Node* node3 = tetra.m_n[3];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				size_t id3 = node3->index;
+
+				// elastic force
+				// explicit elastic force
+				btScalar scale1 = scale * tetra.m_element_measure;
+				force[id0] -= scale1 * force_on_node0;
+				force[id1] -= scale1 * force_on_node123.getColumn(0);
+				force[id2] -= scale1 * force_on_node123.getColumn(1);
+				force[id3] -= scale1 * force_on_node123.getColumn(2);
+			}
+		}
+	}
+
+	void firstPiola(const btMatrix3x3& F, btMatrix3x3& P)
+	{
+		// btMatrix3x3 JFinvT = F.adjoint();
+		btScalar J = F.determinant();
+		P = F.adjoint().transpose() * (m_lambda * (J - 1));
+		if (m_mu > SIMD_EPSILON)
+		{
+			btMatrix3x3 R, S;
+			if (J < 1024 * SIMD_EPSILON)
+				R.setIdentity();
+			else
+				PolarDecomposition(F, R, S);  // this QR is not robust, consider using implicit shift svd
+			/*https://fuchuyuan.github.io/research/svd/paper.pdf*/
+			P += (F - R) * 2 * m_mu;
+		}
+	}
+
+	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+	{
+	}
+
+	virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+	{
+	}
+
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
+
+	virtual btDeformableLagrangianForceType getForceType()
+	{
+		return BT_COROTATED_FORCE;
+	}
+};
+
+#endif /* btCorotated_h */
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableGravityForce.h b/extern/bullet2/src/BulletSoftBody/btDeformableGravityForce.h
new file mode 100644
index 00000000000..d91867f4578
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableGravityForce.h
@@ -0,0 +1,105 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_DEFORMABLE_GRAVITY_FORCE_H
+#define BT_DEFORMABLE_GRAVITY_FORCE_H
+
+#include "btDeformableLagrangianForce.h"
+
+class btDeformableGravityForce : public btDeformableLagrangianForce
+{
+public:
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btVector3 m_gravity;
+
+	btDeformableGravityForce(const btVector3& g) : m_gravity(g)
+	{
+	}
+
+	virtual void addScaledForces(btScalar scale, TVStack& force)
+	{
+		addScaledGravityForce(scale, force);
+	}
+
+	virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+	{
+		addScaledGravityForce(scale, force);
+	}
+
+	virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+	{
+	}
+
+	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+	{
+	}
+
+	virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+	{
+	}
+
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
+
+	virtual void addScaledGravityForce(btScalar scale, TVStack& force)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= force.size());
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				btSoftBody::Node& n = psb->m_nodes[j];
+				size_t id = n.index;
+				btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im;
+				btVector3 scaled_force = scale * m_gravity * mass * m_softBodies[i]->m_gravityFactor;
+				force[id] += scaled_force;
+			}
+		}
+	}
+
+	virtual btDeformableLagrangianForceType getForceType()
+	{
+		return BT_GRAVITY_FORCE;
+	}
+
+	// the gravitational potential energy
+	virtual double totalEnergy(btScalar dt)
+	{
+		double e = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				const btSoftBody::Node& node = psb->m_nodes[j];
+				if (node.m_im > 0)
+				{
+					e -= m_gravity.dot(node.m_q) / node.m_im;
+				}
+			}
+		}
+		return e;
+	}
+};
+#endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableLagrangianForce.h b/extern/bullet2/src/BulletSoftBody/btDeformableLagrangianForce.h
new file mode 100644
index 00000000000..d58d825d1ce
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableLagrangianForce.h
@@ -0,0 +1,372 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_DEFORMABLE_LAGRANGIAN_FORCE_H
+#define BT_DEFORMABLE_LAGRANGIAN_FORCE_H
+
+#include "btSoftBody.h"
+#include <LinearMath/btHashMap.h>
+#include <iostream>
+
+enum btDeformableLagrangianForceType
+{
+	BT_GRAVITY_FORCE = 1,
+	BT_MASSSPRING_FORCE = 2,
+	BT_COROTATED_FORCE = 3,
+	BT_NEOHOOKEAN_FORCE = 4,
+	BT_LINEAR_ELASTICITY_FORCE = 5,
+	BT_MOUSE_PICKING_FORCE = 6
+};
+
+static inline double randomDouble(double low, double high)
+{
+	return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
+}
+
+class btDeformableLagrangianForce
+{
+public:
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btAlignedObjectArray<btSoftBody*> m_softBodies;
+	const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
+
+	btDeformableLagrangianForce()
+	{
+	}
+
+	virtual ~btDeformableLagrangianForce() {}
+
+	// add all forces
+	virtual void addScaledForces(btScalar scale, TVStack& force) = 0;
+
+	// add damping df
+	virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
+
+	// build diagonal of A matrix
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0;
+
+	// add elastic df
+	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
+
+	// add all forces that are explicit in explicit solve
+	virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
+
+	// add all damping forces
+	virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
+
+	virtual void addScaledHessian(btScalar scale) {}
+
+	virtual btDeformableLagrangianForceType getForceType() = 0;
+
+	virtual void reinitialize(bool nodeUpdated)
+	{
+	}
+
+	// get number of nodes that have the force
+	virtual int getNumNodes()
+	{
+		int numNodes = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			numNodes += m_softBodies[i]->m_nodes.size();
+		}
+		return numNodes;
+	}
+
+	// add a soft body to be affected by the particular lagrangian force
+	virtual void addSoftBody(btSoftBody* psb)
+	{
+		m_softBodies.push_back(psb);
+	}
+
+	virtual void removeSoftBody(btSoftBody* psb)
+	{
+		m_softBodies.remove(psb);
+	}
+
+	virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
+	{
+		m_nodes = nodes;
+	}
+
+	// Calculate the incremental deformable generated from the input dx
+	virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx)
+	{
+		btVector3 c1 = dx[id1] - dx[id0];
+		btVector3 c2 = dx[id2] - dx[id0];
+		btVector3 c3 = dx[id3] - dx[id0];
+		return btMatrix3x3(c1, c2, c3).transpose();
+	}
+
+	// Calculate the incremental deformable generated from the current velocity
+	virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3)
+	{
+		btVector3 c1 = n1->m_v - n0->m_v;
+		btVector3 c2 = n2->m_v - n0->m_v;
+		btVector3 c3 = n3->m_v - n0->m_v;
+		return btMatrix3x3(c1, c2, c3).transpose();
+	}
+
+	// test for addScaledElasticForce function
+	virtual void testDerivative()
+	{
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
+			}
+			psb->updateDeformation();
+		}
+
+		TVStack dx;
+		dx.resize(getNumNodes());
+		TVStack dphi_dx;
+		dphi_dx.resize(dx.size());
+		for (int i = 0; i < dphi_dx.size(); ++i)
+		{
+			dphi_dx[i].setZero();
+		}
+		addScaledForces(-1, dphi_dx);
+
+		// write down the current position
+		TVStack x;
+		x.resize(dx.size());
+		int counter = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				x[counter] = psb->m_nodes[j].m_q;
+				counter++;
+			}
+		}
+		counter = 0;
+
+		// populate dx with random vectors
+		for (int i = 0; i < dx.size(); ++i)
+		{
+			dx[i].setX(randomDouble(-1, 1));
+			dx[i].setY(randomDouble(-1, 1));
+			dx[i].setZ(randomDouble(-1, 1));
+		}
+
+		btAlignedObjectArray<double> errors;
+		for (int it = 0; it < 10; ++it)
+		{
+			for (int i = 0; i < dx.size(); ++i)
+			{
+				dx[i] *= 0.5;
+			}
+
+			// get dphi/dx * dx
+			double dphi = 0;
+			for (int i = 0; i < dx.size(); ++i)
+			{
+				dphi += dphi_dx[i].dot(dx[i]);
+			}
+
+			for (int i = 0; i < m_softBodies.size(); ++i)
+			{
+				btSoftBody* psb = m_softBodies[i];
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = x[counter] + dx[counter];
+					counter++;
+				}
+				psb->updateDeformation();
+			}
+			counter = 0;
+			double f1 = totalElasticEnergy(0);
+
+			for (int i = 0; i < m_softBodies.size(); ++i)
+			{
+				btSoftBody* psb = m_softBodies[i];
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = x[counter] - dx[counter];
+					counter++;
+				}
+				psb->updateDeformation();
+			}
+			counter = 0;
+
+			double f2 = totalElasticEnergy(0);
+
+			//restore m_q
+			for (int i = 0; i < m_softBodies.size(); ++i)
+			{
+				btSoftBody* psb = m_softBodies[i];
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = x[counter];
+					counter++;
+				}
+				psb->updateDeformation();
+			}
+			counter = 0;
+			double error = f1 - f2 - 2 * dphi;
+			errors.push_back(error);
+			std::cout << "Iteration = " << it << ", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl;
+		}
+		for (int i = 1; i < errors.size(); ++i)
+		{
+			std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl;
+		}
+	}
+
+	// test for addScaledElasticForce function
+	virtual void testHessian()
+	{
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
+			}
+			psb->updateDeformation();
+		}
+
+		TVStack dx;
+		dx.resize(getNumNodes());
+		TVStack df;
+		df.resize(dx.size());
+		TVStack f1;
+		f1.resize(dx.size());
+		TVStack f2;
+		f2.resize(dx.size());
+
+		// write down the current position
+		TVStack x;
+		x.resize(dx.size());
+		int counter = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				x[counter] = psb->m_nodes[j].m_q;
+				counter++;
+			}
+		}
+		counter = 0;
+
+		// populate dx with random vectors
+		for (int i = 0; i < dx.size(); ++i)
+		{
+			dx[i].setX(randomDouble(-1, 1));
+			dx[i].setY(randomDouble(-1, 1));
+			dx[i].setZ(randomDouble(-1, 1));
+		}
+
+		btAlignedObjectArray<double> errors;
+		for (int it = 0; it < 10; ++it)
+		{
+			for (int i = 0; i < dx.size(); ++i)
+			{
+				dx[i] *= 0.5;
+			}
+
+			// get df
+			for (int i = 0; i < df.size(); ++i)
+			{
+				df[i].setZero();
+				f1[i].setZero();
+				f2[i].setZero();
+			}
+
+			//set df
+			addScaledElasticForceDifferential(-1, dx, df);
+
+			for (int i = 0; i < m_softBodies.size(); ++i)
+			{
+				btSoftBody* psb = m_softBodies[i];
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = x[counter] + dx[counter];
+					counter++;
+				}
+				psb->updateDeformation();
+			}
+			counter = 0;
+
+			//set f1
+			addScaledForces(-1, f1);
+
+			for (int i = 0; i < m_softBodies.size(); ++i)
+			{
+				btSoftBody* psb = m_softBodies[i];
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = x[counter] - dx[counter];
+					counter++;
+				}
+				psb->updateDeformation();
+			}
+			counter = 0;
+
+			//set f2
+			addScaledForces(-1, f2);
+
+			//restore m_q
+			for (int i = 0; i < m_softBodies.size(); ++i)
+			{
+				btSoftBody* psb = m_softBodies[i];
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = x[counter];
+					counter++;
+				}
+				psb->updateDeformation();
+			}
+			counter = 0;
+			double error = 0;
+			for (int i = 0; i < df.size(); ++i)
+			{
+				btVector3 error_vector = f1[i] - f2[i] - 2 * df[i];
+				error += error_vector.length2();
+			}
+			error = btSqrt(error);
+			errors.push_back(error);
+			std::cout << "Iteration = " << it << ", error = " << error << std::endl;
+		}
+		for (int i = 1; i < errors.size(); ++i)
+		{
+			std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl;
+		}
+	}
+
+	//
+	virtual double totalElasticEnergy(btScalar dt)
+	{
+		return 0;
+	}
+
+	//
+	virtual double totalDampingEnergy(btScalar dt)
+	{
+		return 0;
+	}
+
+	// total Energy takes dt as input because certain energies depend on dt
+	virtual double totalEnergy(btScalar dt)
+	{
+		return totalElasticEnergy(dt) + totalDampingEnergy(dt);
+	}
+};
+#endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableLinearElasticityForce.h b/extern/bullet2/src/BulletSoftBody/btDeformableLinearElasticityForce.h
new file mode 100644
index 00000000000..971192050b4
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableLinearElasticityForce.h
@@ -0,0 +1,462 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_LINEAR_ELASTICITY_H
+#define BT_LINEAR_ELASTICITY_H
+
+#include "btDeformableLagrangianForce.h"
+#include "LinearMath/btQuickprof.h"
+#include "btSoftBodyInternals.h"
+#define TETRA_FLAT_THRESHOLD 0.01
+class btDeformableLinearElasticityForce : public btDeformableLagrangianForce
+{
+public:
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btScalar m_mu, m_lambda;
+	btScalar m_E, m_nu;  // Young's modulus and Poisson ratio
+	btScalar m_damping_alpha, m_damping_beta;
+	btDeformableLinearElasticityForce() : m_mu(1), m_lambda(1), m_damping_alpha(0.01), m_damping_beta(0.01)
+	{
+		updateYoungsModulusAndPoissonRatio();
+	}
+
+	btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping_alpha = 0.01, btScalar damping_beta = 0.01) : m_mu(mu), m_lambda(lambda), m_damping_alpha(damping_alpha), m_damping_beta(damping_beta)
+	{
+		updateYoungsModulusAndPoissonRatio();
+	}
+
+	void updateYoungsModulusAndPoissonRatio()
+	{
+		// conversion from Lame Parameters to Young's modulus and Poisson ratio
+		// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
+		m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu);
+		m_nu = m_lambda * 0.5 / (m_mu + m_lambda);
+	}
+
+	void updateLameParameters()
+	{
+		// conversion from Young's modulus and Poisson ratio to Lame Parameters
+		// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
+		m_mu = m_E * 0.5 / (1 + m_nu);
+		m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu));
+	}
+
+	void setYoungsModulus(btScalar E)
+	{
+		m_E = E;
+		updateLameParameters();
+	}
+
+	void setPoissonRatio(btScalar nu)
+	{
+		m_nu = nu;
+		updateLameParameters();
+	}
+
+	void setDamping(btScalar damping_alpha, btScalar damping_beta)
+	{
+		m_damping_alpha = damping_alpha;
+		m_damping_beta = damping_beta;
+	}
+
+	void setLameParameters(btScalar mu, btScalar lambda)
+	{
+		m_mu = mu;
+		m_lambda = lambda;
+		updateYoungsModulusAndPoissonRatio();
+	}
+
+	virtual void addScaledForces(btScalar scale, TVStack& force)
+	{
+		addScaledDampingForce(scale, force);
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+	{
+		addScaledElasticForce(scale, force);
+	}
+
+	// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
+	virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+	{
+		if (m_damping_alpha == 0 && m_damping_beta == 0)
+			return;
+		btScalar mu_damp = m_damping_beta * m_mu;
+		btScalar lambda_damp = m_damping_beta * m_lambda;
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= force.size());
+		btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_tetras.size(); ++j)
+			{
+				bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD);
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btSoftBody::Node* node0 = tetra.m_n[0];
+				btSoftBody::Node* node1 = tetra.m_n[1];
+				btSoftBody::Node* node2 = tetra.m_n[2];
+				btSoftBody::Node* node3 = tetra.m_n[3];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				size_t id3 = node3->index;
+				btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
+				if (!close_to_flat)
+				{
+					dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF;
+				}
+				btMatrix3x3 I;
+				I.setIdentity();
+				btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp);
+				btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
+				if (!close_to_flat)
+				{
+					df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123;
+				}
+				btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
+				// damping force differential
+				btScalar scale1 = scale * tetra.m_element_measure;
+				force[id0] -= scale1 * df_on_node0;
+				force[id1] -= scale1 * df_on_node123.getColumn(0);
+				force[id2] -= scale1 * df_on_node123.getColumn(1);
+				force[id3] -= scale1 * df_on_node123.getColumn(2);
+			}
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				const btSoftBody::Node& node = psb->m_nodes[j];
+				size_t id = node.index;
+				if (node.m_im > 0)
+				{
+					force[id] -= scale * node.m_v / node.m_im * m_damping_alpha;
+				}
+			}
+		}
+	}
+
+	virtual double totalElasticEnergy(btScalar dt)
+	{
+		double energy = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
+			{
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
+				energy += tetra.m_element_measure * elasticEnergyDensity(s);
+			}
+		}
+		return energy;
+	}
+
+	// The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
+	virtual double totalDampingEnergy(btScalar dt)
+	{
+		double energy = 0;
+		int sz = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				sz = btMax(sz, psb->m_nodes[j].index);
+			}
+		}
+		TVStack dampingForce;
+		dampingForce.resize(sz + 1);
+		for (int i = 0; i < dampingForce.size(); ++i)
+			dampingForce[i].setZero();
+		addScaledDampingForce(0.5, dampingForce);
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				const btSoftBody::Node& node = psb->m_nodes[j];
+				energy -= dampingForce[node.index].dot(node.m_v) / dt;
+			}
+		}
+		return energy;
+	}
+
+	double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
+	{
+		double density = 0;
+		btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
+		btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
+		density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2());
+		density += m_lambda * trace * trace * 0.5;
+		return density;
+	}
+
+	virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= force.size());
+		btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			btScalar max_p = psb->m_cfg.m_maxStress;
+			for (int j = 0; j < psb->m_tetras.size(); ++j)
+			{
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btMatrix3x3 P;
+				firstPiola(psb->m_tetraScratches[j], P);
+#if USE_SVD
+				if (max_p > 0)
+				{
+					// since we want to clamp the principal stress to max_p, we only need to
+					// calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
+					btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
+					if (trPTP > max_p * max_p)
+					{
+						btMatrix3x3 U, V;
+						btVector3 sigma;
+						singularValueDecomposition(P, U, sigma, V);
+						sigma[0] = btMin(sigma[0], max_p);
+						sigma[1] = btMin(sigma[1], max_p);
+						sigma[2] = btMin(sigma[2], max_p);
+						sigma[0] = btMax(sigma[0], -max_p);
+						sigma[1] = btMax(sigma[1], -max_p);
+						sigma[2] = btMax(sigma[2], -max_p);
+						btMatrix3x3 Sigma;
+						Sigma.setIdentity();
+						Sigma[0][0] = sigma[0];
+						Sigma[1][1] = sigma[1];
+						Sigma[2][2] = sigma[2];
+						P = U * Sigma * V.transpose();
+					}
+				}
+#endif
+				//                btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+				btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose();
+				btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
+
+				btSoftBody::Node* node0 = tetra.m_n[0];
+				btSoftBody::Node* node1 = tetra.m_n[1];
+				btSoftBody::Node* node2 = tetra.m_n[2];
+				btSoftBody::Node* node3 = tetra.m_n[3];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				size_t id3 = node3->index;
+
+				// elastic force
+				btScalar scale1 = scale * tetra.m_element_measure;
+				force[id0] -= scale1 * force_on_node0;
+				force[id1] -= scale1 * force_on_node123.getColumn(0);
+				force[id2] -= scale1 * force_on_node123.getColumn(1);
+				force[id3] -= scale1 * force_on_node123.getColumn(2);
+			}
+		}
+	}
+
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
+
+	// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
+	virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+	{
+		if (m_damping_alpha == 0 && m_damping_beta == 0)
+			return;
+		btScalar mu_damp = m_damping_beta * m_mu;
+		btScalar lambda_damp = m_damping_beta * m_lambda;
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= df.size());
+		btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_tetras.size(); ++j)
+			{
+				bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD);
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btSoftBody::Node* node0 = tetra.m_n[0];
+				btSoftBody::Node* node1 = tetra.m_n[1];
+				btSoftBody::Node* node2 = tetra.m_n[2];
+				btSoftBody::Node* node3 = tetra.m_n[3];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				size_t id3 = node3->index;
+				btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
+				if (!close_to_flat)
+				{
+					dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF;
+				}
+				btMatrix3x3 I;
+				I.setIdentity();
+				btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp);
+				btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
+				if (!close_to_flat)
+				{
+					df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123;
+				}
+				btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
+
+				// damping force differential
+				btScalar scale1 = scale * tetra.m_element_measure;
+				df[id0] -= scale1 * df_on_node0;
+				df[id1] -= scale1 * df_on_node123.getColumn(0);
+				df[id2] -= scale1 * df_on_node123.getColumn(1);
+				df[id3] -= scale1 * df_on_node123.getColumn(2);
+			}
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				const btSoftBody::Node& node = psb->m_nodes[j];
+				size_t id = node.index;
+				if (node.m_im > 0)
+				{
+					df[id] -= scale * dv[id] / node.m_im * m_damping_alpha;
+				}
+			}
+		}
+	}
+
+	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= df.size());
+		btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_tetras.size(); ++j)
+			{
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btSoftBody::Node* node0 = tetra.m_n[0];
+				btSoftBody::Node* node1 = tetra.m_n[1];
+				btSoftBody::Node* node2 = tetra.m_n[2];
+				btSoftBody::Node* node3 = tetra.m_n[3];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				size_t id3 = node3->index;
+				btMatrix3x3 dF = psb->m_tetraScratches[j].m_corotation.transpose() * Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
+				btMatrix3x3 dP;
+				firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
+				//                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+				btMatrix3x3 df_on_node123 = psb->m_tetraScratches[j].m_corotation * dP * tetra.m_Dm_inverse.transpose();
+				btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
+
+				// elastic force differential
+				btScalar scale1 = scale * tetra.m_element_measure;
+				df[id0] -= scale1 * df_on_node0;
+				df[id1] -= scale1 * df_on_node123.getColumn(0);
+				df[id2] -= scale1 * df_on_node123.getColumn(1);
+				df[id3] -= scale1 * df_on_node123.getColumn(2);
+			}
+		}
+	}
+
+	void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
+	{
+		btMatrix3x3 corotated_F = s.m_corotation.transpose() * s.m_F;
+
+		btMatrix3x3 epsilon = (corotated_F + corotated_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
+		btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
+		P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
+	}
+
+	// Let P be the first piola stress.
+	// This function calculates the dP = dP/dF * dF
+	void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
+	{
+		btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
+		dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
+	}
+
+	// Let Q be the damping stress.
+	// This function calculates the dP = dQ/dF * dF
+	void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
+	{
+		btScalar mu_damp = m_damping_beta * m_mu;
+		btScalar lambda_damp = m_damping_beta * m_lambda;
+		btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
+		dP = (dF + dF.transpose()) * mu_damp + btMatrix3x3::getIdentity() * lambda_damp * trace;
+	}
+
+	virtual void addScaledHessian(btScalar scale)
+	{
+		btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_tetras.size(); ++j)
+			{
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btMatrix3x3 P;
+				firstPiola(psb->m_tetraScratches[j], P);  // make sure scratch is evaluated at x_n + dt * vn
+				btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose();
+				btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
+				btSoftBody::Node* node0 = tetra.m_n[0];
+				btSoftBody::Node* node1 = tetra.m_n[1];
+				btSoftBody::Node* node2 = tetra.m_n[2];
+				btSoftBody::Node* node3 = tetra.m_n[3];
+				btScalar scale1 = scale * (scale + m_damping_beta) * tetra.m_element_measure;  // stiff and stiffness-damping terms;
+				node0->m_effectiveMass += OuterProduct(force_on_node0, force_on_node0) * scale1;
+				node1->m_effectiveMass += OuterProduct(force_on_node123.getColumn(0), force_on_node123.getColumn(0)) * scale1;
+				node2->m_effectiveMass += OuterProduct(force_on_node123.getColumn(1), force_on_node123.getColumn(1)) * scale1;
+				node3->m_effectiveMass += OuterProduct(force_on_node123.getColumn(2), force_on_node123.getColumn(2)) * scale1;
+			}
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				btSoftBody::Node& node = psb->m_nodes[j];
+				if (node.m_im > 0)
+				{
+					btMatrix3x3 I;
+					I.setIdentity();
+					node.m_effectiveMass += I * (scale * (1.0 / node.m_im) * m_damping_alpha);
+				}
+			}
+		}
+	}
+
+	virtual btDeformableLagrangianForceType getForceType()
+	{
+		return BT_LINEAR_ELASTICITY_FORCE;
+	}
+};
+#endif /* BT_LINEAR_ELASTICITY_H */
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableMassSpringForce.h b/extern/bullet2/src/BulletSoftBody/btDeformableMassSpringForce.h
new file mode 100644
index 00000000000..8c97bd1ba8b
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableMassSpringForce.h
@@ -0,0 +1,301 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_MASS_SPRING_H
+#define BT_MASS_SPRING_H
+
+#include "btDeformableLagrangianForce.h"
+
+class btDeformableMassSpringForce : public btDeformableLagrangianForce
+{
+	// If true, the damping force will be in the direction of the spring
+	// If false, the damping force will be in the direction of the velocity
+	bool m_momentum_conserving;
+	btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness;
+
+public:
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05)
+	{
+	}
+	btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k)
+	{
+		if (m_bendingStiffness < btScalar(0))
+		{
+			m_bendingStiffness = m_elasticStiffness;
+		}
+	}
+
+	virtual void addScaledForces(btScalar scale, TVStack& force)
+	{
+		addScaledDampingForce(scale, force);
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+	{
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= force.size());
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			const btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_links.size(); ++j)
+			{
+				const btSoftBody::Link& link = psb->m_links[j];
+				btSoftBody::Node* node1 = link.m_n[0];
+				btSoftBody::Node* node2 = link.m_n[1];
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+
+				// damping force
+				btVector3 v_diff = (node2->m_v - node1->m_v);
+				btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
+				if (m_momentum_conserving)
+				{
+					if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
+					{
+						btVector3 dir = (node2->m_x - node1->m_x).normalized();
+						scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
+					}
+				}
+				force[id1] += scaled_force;
+				force[id2] -= scaled_force;
+			}
+		}
+	}
+
+	virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= force.size());
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			const btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_links.size(); ++j)
+			{
+				const btSoftBody::Link& link = psb->m_links[j];
+				btSoftBody::Node* node1 = link.m_n[0];
+				btSoftBody::Node* node2 = link.m_n[1];
+				btScalar r = link.m_rl;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+
+				// elastic force
+				btVector3 dir = (node2->m_q - node1->m_q);
+				btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
+				btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
+				btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r);
+				force[id1] += scaled_force;
+				force[id2] -= scaled_force;
+			}
+		}
+	}
+
+	virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+	{
+		// implicit damping force differential
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			btScalar scaled_k_damp = m_dampingStiffness * scale;
+			for (int j = 0; j < psb->m_links.size(); ++j)
+			{
+				const btSoftBody::Link& link = psb->m_links[j];
+				btSoftBody::Node* node1 = link.m_n[0];
+				btSoftBody::Node* node2 = link.m_n[1];
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+
+				btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
+				if (m_momentum_conserving)
+				{
+					if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
+					{
+						btVector3 dir = (node2->m_x - node1->m_x).normalized();
+						local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
+					}
+				}
+				df[id1] += local_scaled_df;
+				df[id2] -= local_scaled_df;
+			}
+		}
+	}
+
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA)
+	{
+		// implicit damping force differential
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			btScalar scaled_k_damp = m_dampingStiffness * scale;
+			for (int j = 0; j < psb->m_links.size(); ++j)
+			{
+				const btSoftBody::Link& link = psb->m_links[j];
+				btSoftBody::Node* node1 = link.m_n[0];
+				btSoftBody::Node* node2 = link.m_n[1];
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				if (m_momentum_conserving)
+				{
+					if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
+					{
+						btVector3 dir = (node2->m_x - node1->m_x).normalized();
+						for (int d = 0; d < 3; ++d)
+						{
+							if (node1->m_im > 0)
+								diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d];
+							if (node2->m_im > 0)
+								diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d];
+						}
+					}
+				}
+				else
+				{
+					for (int d = 0; d < 3; ++d)
+					{
+						if (node1->m_im > 0)
+							diagA[id1][d] -= scaled_k_damp;
+						if (node2->m_im > 0)
+							diagA[id2][d] -= scaled_k_damp;
+					}
+				}
+			}
+		}
+	}
+
+	virtual double totalElasticEnergy(btScalar dt)
+	{
+		double energy = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			const btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_links.size(); ++j)
+			{
+				const btSoftBody::Link& link = psb->m_links[j];
+				btSoftBody::Node* node1 = link.m_n[0];
+				btSoftBody::Node* node2 = link.m_n[1];
+				btScalar r = link.m_rl;
+
+				// elastic force
+				btVector3 dir = (node2->m_q - node1->m_q);
+				energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() - r);
+			}
+		}
+		return energy;
+	}
+
+	virtual double totalDampingEnergy(btScalar dt)
+	{
+		double energy = 0;
+		int sz = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				sz = btMax(sz, psb->m_nodes[j].index);
+			}
+		}
+		TVStack dampingForce;
+		dampingForce.resize(sz + 1);
+		for (int i = 0; i < dampingForce.size(); ++i)
+			dampingForce[i].setZero();
+		addScaledDampingForce(0.5, dampingForce);
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				const btSoftBody::Node& node = psb->m_nodes[j];
+				energy -= dampingForce[node.index].dot(node.m_v) / dt;
+			}
+		}
+		return energy;
+	}
+
+	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+	{
+		// implicit damping force differential
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			const btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_links.size(); ++j)
+			{
+				const btSoftBody::Link& link = psb->m_links[j];
+				btSoftBody::Node* node1 = link.m_n[0];
+				btSoftBody::Node* node2 = link.m_n[1];
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				btScalar r = link.m_rl;
+
+				btVector3 dir = (node1->m_q - node2->m_q);
+				btScalar dir_norm = dir.norm();
+				btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
+				btVector3 dx_diff = dx[id1] - dx[id2];
+				btVector3 scaled_df = btVector3(0, 0, 0);
+				btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
+				if (dir_norm > SIMD_EPSILON)
+				{
+					scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized;
+					scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized;
+					scaled_df -= scaled_k * ((dir_norm - r) / dir_norm) * dx_diff;
+				}
+
+				df[id1] += scaled_df;
+				df[id2] -= scaled_df;
+			}
+		}
+	}
+
+	virtual btDeformableLagrangianForceType getForceType()
+	{
+		return BT_MASSSPRING_FORCE;
+	}
+};
+
+#endif /* btMassSpring_h */
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableMousePickingForce.h b/extern/bullet2/src/BulletSoftBody/btDeformableMousePickingForce.h
new file mode 100644
index 00000000000..d218d962149
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableMousePickingForce.h
@@ -0,0 +1,162 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_MOUSE_PICKING_FORCE_H
+#define BT_MOUSE_PICKING_FORCE_H
+
+#include "btDeformableLagrangianForce.h"
+
+class btDeformableMousePickingForce : public btDeformableLagrangianForce
+{
+	// If true, the damping force will be in the direction of the spring
+	// If false, the damping force will be in the direction of the velocity
+	btScalar m_elasticStiffness, m_dampingStiffness;
+	const btSoftBody::Face& m_face;
+	btVector3 m_mouse_pos;
+	btScalar m_maxForce;
+
+public:
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce)
+	{
+	}
+
+	virtual void addScaledForces(btScalar scale, TVStack& force)
+	{
+		addScaledDampingForce(scale, force);
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+	{
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+	{
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 v_diff = m_face.m_n[i]->m_v;
+			btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
+			if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
+			{
+				btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
+				scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
+			}
+			force[m_face.m_n[i]->index] -= scaled_force;
+		}
+	}
+
+	virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+	{
+		btScalar scaled_stiffness = scale * m_elasticStiffness;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
+			btVector3 scaled_force = scaled_stiffness * dir;
+			if (scaled_force.safeNorm() > m_maxForce)
+			{
+				scaled_force.safeNormalize();
+				scaled_force *= m_maxForce;
+			}
+			force[m_face.m_n[i]->index] -= scaled_force;
+		}
+	}
+
+	virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+	{
+		btScalar scaled_k_damp = m_dampingStiffness * scale;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index];
+			if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
+			{
+				btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
+				local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir;
+			}
+			df[m_face.m_n[i]->index] -= local_scaled_df;
+		}
+	}
+
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
+
+	virtual double totalElasticEnergy(btScalar dt)
+	{
+		double energy = 0;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
+			btVector3 scaled_force = m_elasticStiffness * dir;
+			if (scaled_force.safeNorm() > m_maxForce)
+			{
+				scaled_force.safeNormalize();
+				scaled_force *= m_maxForce;
+			}
+			energy += 0.5 * scaled_force.dot(dir);
+		}
+		return energy;
+	}
+
+	virtual double totalDampingEnergy(btScalar dt)
+	{
+		double energy = 0;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 v_diff = m_face.m_n[i]->m_v;
+			btVector3 scaled_force = m_dampingStiffness * v_diff;
+			if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
+			{
+				btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
+				scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir;
+			}
+			energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt;
+		}
+		return energy;
+	}
+
+	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+	{
+		btScalar scaled_stiffness = scale * m_elasticStiffness;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
+			btScalar dir_norm = dir.norm();
+			btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
+			int id = m_face.m_n[i]->index;
+			btVector3 dx_diff = dx[id];
+			btScalar r = 0;  // rest length is 0 for picking spring
+			btVector3 scaled_df = btVector3(0, 0, 0);
+			if (dir_norm > SIMD_EPSILON)
+			{
+				scaled_df -= scaled_stiffness * dir_normalized.dot(dx_diff) * dir_normalized;
+				scaled_df += scaled_stiffness * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized;
+				scaled_df -= scaled_stiffness * ((dir_norm - r) / dir_norm) * dx_diff;
+			}
+			df[id] += scaled_df;
+		}
+	}
+
+	void setMousePos(const btVector3& p)
+	{
+		m_mouse_pos = p;
+	}
+
+	virtual btDeformableLagrangianForceType getForceType()
+	{
+		return BT_MOUSE_PICKING_FORCE;
+	}
+};
+
+#endif /* btMassSpring_h */
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
new file mode 100644
index 00000000000..631fd5fbed5
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
@@ -0,0 +1,144 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "btDeformableMultiBodyConstraintSolver.h"
+#include <iostream>
+// override the iterations method to include deformable/multibody contact
+btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
+	{
+		///this is a special step to resolve penetrations (just for contacts)
+		solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+
+		int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
+		for (int iteration = 0; iteration < maxIterations; iteration++)
+		{
+			// rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body
+
+			// solve rigid/rigid in solver body
+			m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+			// solver body velocity -> rigid body velocity
+			solverBodyWriteBack(infoGlobal);
+			btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies, numDeformableBodies, infoGlobal);
+			// update rigid body velocity in rigid/deformable contact
+			m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
+			// solver body velocity <- rigid body velocity
+			writeToSolverBody(bodies, numBodies, infoGlobal);
+
+			if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
+			{
+#ifdef VERBOSE_RESIDUAL_PRINTF
+				if (iteration >= (maxIterations - 1))
+					printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
+#endif
+				m_analyticsData.m_numSolverCalls++;
+				m_analyticsData.m_numIterationsUsed = iteration + 1;
+				m_analyticsData.m_islandId = -2;
+				if (numBodies > 0)
+					m_analyticsData.m_islandId = bodies[0]->getCompanionId();
+				m_analyticsData.m_numBodies = numBodies;
+				m_analyticsData.m_numContactManifolds = numManifolds;
+				m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
+				break;
+			}
+		}
+	}
+	return 0.f;
+}
+
+void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+{
+	m_tmpMultiBodyConstraints = multiBodyConstraints;
+	m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
+
+	// inherited from MultiBodyConstraintSolver
+	solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+
+	// overriden
+	solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+
+	// inherited from MultiBodyConstraintSolver
+	solveGroupCacheFriendlyFinish(bodies, numBodies, info);
+
+	m_tmpMultiBodyConstraints = 0;
+	m_tmpNumMultiBodyConstraints = 0;
+}
+
+void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+	for (int i = 0; i < numBodies; i++)
+	{
+		int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
+
+		btRigidBody* body = btRigidBody::upcast(bodies[i]);
+		if (body && body->getInvMass())
+		{
+			btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
+			solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
+			solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
+		}
+	}
+}
+
+void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
+{
+	for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
+	{
+		btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
+		if (body)
+		{
+			m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
+			m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity + m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
+		}
+	}
+}
+
+void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
+	BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
+	int iteration;
+	if (infoGlobal.m_splitImpulse)
+	{
+		{
+			for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
+			{
+				btScalar leastSquaresResidual = 0.f;
+				{
+					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+					int j;
+					for (j = 0; j < numPoolConstraints; j++)
+					{
+						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+
+						btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+						leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+					}
+					// solve the position correction between deformable and rigid/multibody
+					//                    btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
+					btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal);
+					leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+				}
+				if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
+				{
+#ifdef VERBOSE_RESIDUAL_PRINTF
+					if (iteration >= (infoGlobal.m_numIterations - 1))
+						printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
+#endif
+					break;
+				}
+			}
+		}
+	}
+}
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h
new file mode 100644
index 00000000000..94aabce838e
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h
@@ -0,0 +1,61 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
+#define BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
+
+#include "btDeformableBodySolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+
+class btDeformableBodySolver;
+
+// btDeformableMultiBodyConstraintSolver extendsn btMultiBodyConstraintSolver to solve for the contact among rigid/multibody and deformable bodies. Notice that the following constraints
+// 1. rigid/multibody against rigid/multibody
+// 2. rigid/multibody against deforamble
+// 3. deformable against deformable
+// 4. deformable self collision
+// 5. joint constraints
+// are all coupled in this solve.
+ATTRIBUTE_ALIGNED16(class)
+btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver
+{
+	btDeformableBodySolver* m_deformableSolver;
+
+protected:
+	// override the iterations method to include deformable/multibody contact
+	//    virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+
+	// write the velocity of the the solver body to the underlying rigid body
+	void solverBodyWriteBack(const btContactSolverInfo& infoGlobal);
+
+	// write the velocity of the underlying rigid body to the the the solver body
+	void writeToSolverBody(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
+
+	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+
+	virtual btScalar solveDeformableGroupIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	void setDeformableSolver(btDeformableBodySolver * deformableSolver)
+	{
+		m_deformableSolver = deformableSolver;
+	}
+
+	virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
+};
+
+#endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
new file mode 100644
index 00000000000..983e622b5f6
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
@@ -0,0 +1,814 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+/* ====== Overview of the Deformable Algorithm ====== */
+
+/*
+A single step of the deformable body simulation contains the following main components:
+Call internalStepSimulation multiple times, to achieve 240Hz (4 steps of 60Hz).
+1. Deformable maintaintenance of rest lengths and volume preservation. Forces only depend on position: Update velocity to a temporary state v_{n+1}^* = v_n + explicit_force * dt / mass, where explicit forces include gravity and elastic forces.
+2. Detect discrete collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*.
+
+3a. Solve all constraints, including LCP. Contact, position correction due to numerical drift, friction, and anchors for deformable.
+
+3b. 5 Newton steps (multiple step). Conjugent Gradient solves linear system. Deformable Damping: Then velocities of deformable bodies v_{n+1} are solved in
+        M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass,
+   by a conjugate gradient solver, where the damping force is implicit and depends on v_{n+1}.
+   Make sure contact constraints are not violated in step b by performing velocity projections as in the paper by Baraff and Witkin https://www.cs.cmu.edu/~baraff/papers/sig98.pdf. Dynamic frictions are treated as a force and added to the rhs of the CG solve, whereas static frictions are treated as constraints similar to contact.
+4. Position is updated via x_{n+1} = x_n + dt * v_{n+1}.
+
+
+The algorithm also closely resembles the one in http://physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf
+ */
+
+#include <stdio.h>
+#include "btDeformableMultiBodyDynamicsWorld.h"
+#include "DeformableBodyInplaceSolverIslandCallback.h"
+#include "btDeformableBodySolver.h"
+#include "LinearMath/btQuickprof.h"
+#include "btSoftBodyInternals.h"
+btDeformableMultiBodyDynamicsWorld::btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver)
+	: btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
+	  m_deformableBodySolver(deformableBodySolver),
+	  m_solverCallback(0)
+{
+	m_drawFlags = fDrawFlags::Std;
+	m_drawNodeTree = true;
+	m_drawFaceTree = false;
+	m_drawClusterTree = false;
+	m_sbi.m_broadphase = pairCache;
+	m_sbi.m_dispatcher = dispatcher;
+	m_sbi.m_sparsesdf.Initialize();
+	m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005);
+	m_sbi.m_sparsesdf.Reset();
+
+	m_sbi.air_density = (btScalar)1.2;
+	m_sbi.water_density = 0;
+	m_sbi.water_offset = 0;
+	m_sbi.water_normal = btVector3(0, 0, 0);
+	m_sbi.m_gravity.setValue(0, -9.8, 0);
+	m_internalTime = 0.0;
+	m_implicit = false;
+	m_lineSearch = false;
+	m_useProjection = false;
+	m_ccdIterations = 5;
+	m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
+}
+
+btDeformableMultiBodyDynamicsWorld::~btDeformableMultiBodyDynamicsWorld()
+{
+	delete m_solverDeformableBodyIslandCallback;
+}
+
+void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
+{
+	BT_PROFILE("internalSingleStepSimulation");
+	if (0 != m_internalPreTickCallback)
+	{
+		(*m_internalPreTickCallback)(this, timeStep);
+	}
+	reinitialize(timeStep);
+
+	// add gravity to velocity of rigid and multi bodys
+	applyRigidBodyGravity(timeStep);
+
+	///apply gravity and explicit force to velocity, predict motion
+	predictUnconstraintMotion(timeStep);
+
+	///perform collision detection that involves rigid/multi bodies
+	btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
+
+	btMultiBodyDynamicsWorld::calculateSimulationIslands();
+
+	beforeSolverCallbacks(timeStep);
+
+	///solve contact constraints and then deformable bodies momemtum equation
+	solveConstraints(timeStep);
+
+	afterSolverCallbacks(timeStep);
+
+	performDeformableCollisionDetection();
+
+	applyRepulsionForce(timeStep);
+
+	performGeometricCollisions(timeStep);
+
+	integrateTransforms(timeStep);
+
+	///update vehicle simulation
+	btMultiBodyDynamicsWorld::updateActions(timeStep);
+
+	updateActivationState(timeStep);
+	// End solver-wise simulation step
+	// ///////////////////////////////
+}
+
+void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection()
+{
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		m_softBodies[i]->m_softSoftCollision = true;
+	}
+
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		for (int j = i; j < m_softBodies.size(); ++j)
+		{
+			m_softBodies[i]->defaultCollisionHandler(m_softBodies[j]);
+		}
+	}
+
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		m_softBodies[i]->m_softSoftCollision = false;
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
+{
+	for (int i = 0; i < m_softBodies.size(); i++)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		psb->updateDeactivation(timeStep);
+		if (psb->wantsSleeping())
+		{
+			if (psb->getActivationState() == ACTIVE_TAG)
+				psb->setActivationState(WANTS_DEACTIVATION);
+			if (psb->getActivationState() == ISLAND_SLEEPING)
+			{
+				psb->setZeroVelocity();
+			}
+		}
+		else
+		{
+			if (psb->getActivationState() != DISABLE_DEACTIVATION)
+				psb->setActivationState(ACTIVE_TAG);
+		}
+	}
+	btMultiBodyDynamicsWorld::updateActivationState(timeStep);
+}
+
+void btDeformableMultiBodyDynamicsWorld::applyRepulsionForce(btScalar timeStep)
+{
+	BT_PROFILE("btDeformableMultiBodyDynamicsWorld::applyRepulsionForce");
+	for (int i = 0; i < m_softBodies.size(); i++)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (psb->isActive())
+		{
+			psb->applyRepulsionForce(timeStep, true);
+		}
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar timeStep)
+{
+	BT_PROFILE("btDeformableMultiBodyDynamicsWorld::performGeometricCollisions");
+	// refit the BVH tree for CCD
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (psb->isActive())
+		{
+			m_softBodies[i]->updateFaceTree(true, false);
+			m_softBodies[i]->updateNodeTree(true, false);
+			for (int j = 0; j < m_softBodies[i]->m_faces.size(); ++j)
+			{
+				btSoftBody::Face& f = m_softBodies[i]->m_faces[j];
+				f.m_n0 = (f.m_n[1]->m_x - f.m_n[0]->m_x).cross(f.m_n[2]->m_x - f.m_n[0]->m_x);
+			}
+		}
+	}
+
+	// clear contact points & update DBVT
+	for (int r = 0; r < m_ccdIterations; ++r)
+	{
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				// clear contact points in the previous iteration
+				psb->m_faceNodeContacts.clear();
+
+				// update m_q and normals for CCD calculation
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + timeStep * psb->m_nodes[j].m_v;
+				}
+				for (int j = 0; j < psb->m_faces.size(); ++j)
+				{
+					btSoftBody::Face& f = psb->m_faces[j];
+					f.m_n1 = (f.m_n[1]->m_q - f.m_n[0]->m_q).cross(f.m_n[2]->m_q - f.m_n[0]->m_q);
+					f.m_vn = (f.m_n[1]->m_v - f.m_n[0]->m_v).cross(f.m_n[2]->m_v - f.m_n[0]->m_v) * timeStep * timeStep;
+				}
+			}
+		}
+
+		// apply CCD to register new contact points
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			for (int j = i; j < m_softBodies.size(); ++j)
+			{
+				btSoftBody* psb1 = m_softBodies[i];
+				btSoftBody* psb2 = m_softBodies[j];
+				if (psb1->isActive() && psb2->isActive())
+				{
+					m_softBodies[i]->geometricCollisionHandler(m_softBodies[j]);
+				}
+			}
+		}
+
+		int penetration_count = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				penetration_count += psb->m_faceNodeContacts.size();
+			}
+		}
+		if (penetration_count == 0)
+		{
+			break;
+		}
+
+		// apply inelastic impulse
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				psb->applyRepulsionForce(timeStep, false);
+			}
+		}
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision()
+{
+	BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision");
+	for (int i = 0; i < m_softBodies.size(); i++)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (psb->isActive())
+		{
+			psb->defaultCollisionHandler(psb);
+		}
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep)
+{
+	// correct the position of rigid bodies with temporary velocity generated from split impulse
+	btContactSolverInfo infoGlobal;
+	btVector3 zero(0, 0, 0);
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
+	{
+		btRigidBody* rb = m_nonStaticRigidBodies[i];
+		//correct the position/orientation based on push/turn recovery
+		btTransform newTransform;
+		btVector3 pushVelocity = rb->getPushVelocity();
+		btVector3 turnVelocity = rb->getTurnVelocity();
+		if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0)
+		{
+			btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform);
+			rb->setWorldTransform(newTransform);
+			rb->setPushVelocity(zero);
+			rb->setTurnVelocity(zero);
+		}
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
+{
+	BT_PROFILE("integrateTransforms");
+	positionCorrection(timeStep);
+	btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			btSoftBody::Node& node = psb->m_nodes[j];
+			btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement;
+			btScalar clampDeltaV = maxDisplacement / timeStep;
+			for (int c = 0; c < 3; c++)
+			{
+				if (node.m_v[c] > clampDeltaV)
+				{
+					node.m_v[c] = clampDeltaV;
+				}
+				if (node.m_v[c] < -clampDeltaV)
+				{
+					node.m_v[c] = -clampDeltaV;
+				}
+			}
+			node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv);
+			node.m_q = node.m_x;
+			node.m_vn = node.m_v;
+		}
+		// enforce anchor constraints
+		for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
+		{
+			btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
+			btSoftBody::Node* n = a.m_node;
+			n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
+
+			// update multibody anchor info
+			if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+			{
+				btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
+				if (multibodyLinkCol)
+				{
+					btVector3 nrm;
+					const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
+					const btTransform& wtr = multibodyLinkCol->getWorldTransform();
+					psb->m_worldInfo->m_sparsesdf.Evaluate(
+						wtr.invXform(n->m_x),
+						shp,
+						nrm,
+						0);
+					a.m_cti.m_normal = wtr.getBasis() * nrm;
+					btVector3 normal = a.m_cti.m_normal;
+					btVector3 t1 = generateUnitOrthogonalVector(normal);
+					btVector3 t2 = btCross(normal, t1);
+					btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+					findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
+					findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
+					findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2);
+
+					btScalar* J_n = &jacobianData_normal.m_jacobians[0];
+					btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
+					btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
+
+					btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+					btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+					btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+
+					btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
+									t1.getX(), t1.getY(), t1.getZ(),
+									t2.getX(), t2.getY(), t2.getZ());  // world frame to local frame
+					const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+					btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+					a.m_c0 = rot.transpose() * local_impulse_matrix * rot;
+					a.jacobianData_normal = jacobianData_normal;
+					a.jacobianData_t1 = jacobianData_t1;
+					a.jacobianData_t2 = jacobianData_t2;
+					a.t1 = t1;
+					a.t2 = t2;
+				}
+			}
+		}
+		psb->interpolateRenderMesh();
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
+{
+	BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints");
+	// save v_{n+1}^* velocity after explicit forces
+	m_deformableBodySolver->backupVelocity();
+
+	// set up constraints among multibodies and between multibodies and deformable bodies
+	setupConstraints();
+
+	// solve contact constraints
+	solveContactConstraints();
+
+	// set up the directions in which the velocity does not change in the momentum solve
+	if (m_useProjection)
+		m_deformableBodySolver->m_objective->m_projection.setProjection();
+	else
+		m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier();
+
+	// for explicit scheme, m_backupVelocity = v_{n+1}^*
+	// for implicit scheme, m_backupVelocity = v_n
+	// Here, set dv = v_{n+1} - v_n for nodes in contact
+	m_deformableBodySolver->setupDeformableSolve(m_implicit);
+
+	// At this point, dv should be golden for nodes in contact
+	// proceed to solve deformable momentum equation
+	m_deformableBodySolver->solveDeformableConstraints(timeStep);
+}
+
+void btDeformableMultiBodyDynamicsWorld::setupConstraints()
+{
+	// set up constraints between multibody and deformable bodies
+	m_deformableBodySolver->setConstraints(m_solverInfo);
+
+	// set up constraints among multibodies
+	{
+		sortConstraints();
+		// setup the solver callback
+		btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
+		btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
+		m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
+
+		// build islands
+		m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld());
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::sortConstraints()
+{
+	m_sortedConstraints.resize(m_constraints.size());
+	int i;
+	for (i = 0; i < getNumConstraints(); i++)
+	{
+		m_sortedConstraints[i] = m_constraints[i];
+	}
+	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
+
+	m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
+	for (i = 0; i < m_multiBodyConstraints.size(); i++)
+	{
+		m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
+	}
+	m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
+}
+
+void btDeformableMultiBodyDynamicsWorld::solveContactConstraints()
+{
+	// process constraints on each island
+	m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback);
+
+	// process deferred
+	m_solverDeformableBodyIslandCallback->processConstraints();
+	m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer);
+
+	// write joint feedback
+	{
+		for (int i = 0; i < this->m_multiBodies.size(); i++)
+		{
+			btMultiBody* bod = m_multiBodies[i];
+
+			bool isSleeping = false;
+
+			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+			{
+				isSleeping = true;
+			}
+			for (int b = 0; b < bod->getNumLinks(); b++)
+			{
+				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+					isSleeping = true;
+			}
+
+			if (!isSleeping)
+			{
+				//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+				m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+				m_scratch_v.resize(bod->getNumLinks() + 1);
+				m_scratch_m.resize(bod->getNumLinks() + 1);
+
+				if (bod->internalNeedsJointFeedback())
+				{
+					if (!bod->isUsingRK4Integration())
+					{
+						if (bod->internalNeedsJointFeedback())
+						{
+							bool isConstraintPass = true;
+							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
+																					  getSolverInfo().m_jointFeedbackInWorldSpace,
+																					  getSolverInfo().m_jointFeedbackInJointFrame);
+						}
+					}
+				}
+			}
+		}
+	}
+
+	for (int i = 0; i < this->m_multiBodies.size(); i++)
+	{
+		btMultiBody* bod = m_multiBodies[i];
+		bod->processDeltaVeeMultiDof2();
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask)
+{
+	m_softBodies.push_back(body);
+
+	// Set the soft body solver that will deal with this body
+	// to be the world's solver
+	body->setSoftBodySolver(m_deformableBodySolver);
+
+	btCollisionWorld::addCollisionObject(body,
+										 collisionFilterGroup,
+										 collisionFilterMask);
+}
+
+void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+	BT_PROFILE("predictUnconstraintMotion");
+	btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep);
+	m_deformableBodySolver->predictMotion(timeStep);
+}
+
+void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
+{
+	m_internalTime += timeStep;
+	m_deformableBodySolver->setImplicit(m_implicit);
+	m_deformableBodySolver->setLineSearch(m_lineSearch);
+	m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
+	btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
+	dispatchInfo.m_timeStep = timeStep;
+	dispatchInfo.m_stepCount = 0;
+	dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
+	btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
+	if (m_useProjection)
+	{
+		m_deformableBodySolver->m_useProjection = true;
+		m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true;
+		m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner;
+	}
+	else
+	{
+		m_deformableBodySolver->m_useProjection = false;
+		m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false;
+		m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner;
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::debugDrawWorld()
+{
+	btMultiBodyDynamicsWorld::debugDrawWorld();
+
+	for (int i = 0; i < getSoftBodyArray().size(); i++)
+	{
+		btSoftBody* psb = (btSoftBody*)getSoftBodyArray()[i];
+		{
+			btSoftBodyHelpers::DrawFrame(psb, getDebugDrawer());
+			btSoftBodyHelpers::Draw(psb, getDebugDrawer(), getDrawFlags());
+		}
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep)
+{
+	// Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again
+	// so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep
+	// when there are multiple substeps
+	btMultiBodyDynamicsWorld::applyGravity();
+	// integrate rigid body gravity
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
+	{
+		btRigidBody* rb = m_nonStaticRigidBodies[i];
+		rb->integrateVelocities(timeStep);
+	}
+
+	// integrate multibody gravity
+	{
+		forwardKinematics();
+		clearMultiBodyConstraintForces();
+		{
+			for (int i = 0; i < this->m_multiBodies.size(); i++)
+			{
+				btMultiBody* bod = m_multiBodies[i];
+
+				bool isSleeping = false;
+
+				if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+				{
+					isSleeping = true;
+				}
+				for (int b = 0; b < bod->getNumLinks(); b++)
+				{
+					if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+						isSleeping = true;
+				}
+
+				if (!isSleeping)
+				{
+					m_scratch_r.resize(bod->getNumLinks() + 1);
+					m_scratch_v.resize(bod->getNumLinks() + 1);
+					m_scratch_m.resize(bod->getNumLinks() + 1);
+					bool isConstraintPass = false;
+					{
+						if (!bod->isUsingRK4Integration())
+						{
+							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep,
+																					  m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
+																					  getSolverInfo().m_jointFeedbackInWorldSpace,
+																					  getSolverInfo().m_jointFeedbackInJointFrame);
+						}
+						else
+						{
+							btAssert(" RK4Integration is not supported");
+						}
+					}
+				}
+			}
+		}
+	}
+	clearGravity();
+}
+
+void btDeformableMultiBodyDynamicsWorld::clearGravity()
+{
+	BT_PROFILE("btMultiBody clearGravity");
+	// clear rigid body gravity
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
+	{
+		btRigidBody* body = m_nonStaticRigidBodies[i];
+		if (body->isActive())
+		{
+			body->clearGravity();
+		}
+	}
+	// clear multibody gravity
+	for (int i = 0; i < this->m_multiBodies.size(); i++)
+	{
+		btMultiBody* bod = m_multiBodies[i];
+
+		bool isSleeping = false;
+
+		if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+		{
+			isSleeping = true;
+		}
+		for (int b = 0; b < bod->getNumLinks(); b++)
+		{
+			if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+				isSleeping = true;
+		}
+
+		if (!isSleeping)
+		{
+			bod->addBaseForce(-m_gravity * bod->getBaseMass());
+
+			for (int j = 0; j < bod->getNumLinks(); ++j)
+			{
+				bod->addLinkForce(j, -m_gravity * bod->getLinkMass(j));
+			}
+		}
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::beforeSolverCallbacks(btScalar timeStep)
+{
+	if (0 != m_internalTickCallback)
+	{
+		(*m_internalTickCallback)(this, timeStep);
+	}
+
+	if (0 != m_solverCallback)
+	{
+		(*m_solverCallback)(m_internalTime, this);
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
+{
+	if (0 != m_solverCallback)
+	{
+		(*m_solverCallback)(m_internalTime, this);
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force)
+{
+	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+	bool added = false;
+	for (int i = 0; i < forces.size(); ++i)
+	{
+		if (forces[i]->getForceType() == force->getForceType())
+		{
+			forces[i]->addSoftBody(psb);
+			added = true;
+			break;
+		}
+	}
+	if (!added)
+	{
+		force->addSoftBody(psb);
+		force->setIndices(m_deformableBodySolver->m_objective->getIndices());
+		forces.push_back(force);
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force)
+{
+	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+	int removed_index = -1;
+	for (int i = 0; i < forces.size(); ++i)
+	{
+		if (forces[i]->getForceType() == force->getForceType())
+		{
+			forces[i]->removeSoftBody(psb);
+			if (forces[i]->m_softBodies.size() == 0)
+				removed_index = i;
+			break;
+		}
+	}
+	if (removed_index >= 0)
+		forces.removeAtIndex(removed_index);
+}
+
+void btDeformableMultiBodyDynamicsWorld::removeSoftBodyForce(btSoftBody* psb)
+{
+	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+	for (int i = 0; i < forces.size(); ++i)
+	{
+		forces[i]->removeSoftBody(psb);
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
+{
+	removeSoftBodyForce(body);
+	m_softBodies.remove(body);
+	btCollisionWorld::removeCollisionObject(body);
+	// force a reinitialize so that node indices get updated.
+	m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1));
+}
+
+void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+	btSoftBody* body = btSoftBody::upcast(collisionObject);
+	if (body)
+		removeSoftBody(body);
+	else
+		btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
+}
+
+int btDeformableMultiBodyDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
+{
+	startProfiling(timeStep);
+
+	int numSimulationSubSteps = 0;
+
+	if (maxSubSteps)
+	{
+		//fixed timestep with interpolation
+		m_fixedTimeStep = fixedTimeStep;
+		m_localTime += timeStep;
+		if (m_localTime >= fixedTimeStep)
+		{
+			numSimulationSubSteps = int(m_localTime / fixedTimeStep);
+			m_localTime -= numSimulationSubSteps * fixedTimeStep;
+		}
+	}
+	else
+	{
+		//variable timestep
+		fixedTimeStep = timeStep;
+		m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep;
+		m_fixedTimeStep = 0;
+		if (btFuzzyZero(timeStep))
+		{
+			numSimulationSubSteps = 0;
+			maxSubSteps = 0;
+		}
+		else
+		{
+			numSimulationSubSteps = 1;
+			maxSubSteps = 1;
+		}
+	}
+
+	//process some debugging flags
+	if (getDebugDrawer())
+	{
+		btIDebugDraw* debugDrawer = getDebugDrawer();
+		gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
+	}
+	if (numSimulationSubSteps)
+	{
+		//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
+		int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
+
+		saveKinematicState(fixedTimeStep * clampedSimulationSteps);
+
+		for (int i = 0; i < clampedSimulationSteps; i++)
+		{
+			internalSingleStepSimulation(fixedTimeStep);
+			synchronizeMotionStates();
+		}
+	}
+	else
+	{
+		synchronizeMotionStates();
+	}
+
+	clearForces();
+
+#ifndef BT_NO_PROFILE
+	CProfileManager::Increment_Frame_Counter();
+#endif  //BT_NO_PROFILE
+
+	return numSimulationSubSteps;
+}
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
new file mode 100644
index 00000000000..4b7069aac7c
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
@@ -0,0 +1,316 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H
+#define BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H
+
+#include "btSoftMultiBodyDynamicsWorld.h"
+#include "btDeformableLagrangianForce.h"
+#include "btDeformableMassSpringForce.h"
+#include "btDeformableBodySolver.h"
+#include "btDeformableMultiBodyConstraintSolver.h"
+#include "btSoftBodyHelpers.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+#include <functional>
+typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
+
+class btDeformableBodySolver;
+class btDeformableLagrangianForce;
+struct MultiBodyInplaceSolverIslandCallback;
+struct DeformableBodyInplaceSolverIslandCallback;
+class btDeformableMultiBodyConstraintSolver;
+
+typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
+
+class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
+{
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	///Solver classes that encapsulate multiple deformable bodies for solving
+	btDeformableBodySolver* m_deformableBodySolver;
+	btSoftBodyArray m_softBodies;
+	int m_drawFlags;
+	bool m_drawNodeTree;
+	bool m_drawFaceTree;
+	bool m_drawClusterTree;
+	btSoftBodyWorldInfo m_sbi;
+	btScalar m_internalTime;
+	int m_ccdIterations;
+	bool m_implicit;
+	bool m_lineSearch;
+	bool m_useProjection;
+	DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback;
+
+	typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
+	btSolverCallback m_solverCallback;
+
+protected:
+	virtual void internalSingleStepSimulation(btScalar timeStep);
+
+	virtual void integrateTransforms(btScalar timeStep);
+
+	void positionCorrection(btScalar timeStep);
+
+	void solveConstraints(btScalar timeStep);
+
+	void updateActivationState(btScalar timeStep);
+
+	void clearGravity();
+
+public:
+	btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0);
+
+	virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
+
+	virtual void debugDrawWorld();
+
+	void setSolverCallback(btSolverCallback cb)
+	{
+		m_solverCallback = cb;
+	}
+
+	virtual ~btDeformableMultiBodyDynamicsWorld();
+
+	virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld()
+	{
+		return (btMultiBodyDynamicsWorld*)(this);
+	}
+
+	virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const
+	{
+		return (const btMultiBodyDynamicsWorld*)(this);
+	}
+
+	virtual btDynamicsWorldType getWorldType() const
+	{
+		return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD;
+	}
+
+	virtual void predictUnconstraintMotion(btScalar timeStep);
+
+	virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
+
+	btSoftBodyArray& getSoftBodyArray()
+	{
+		return m_softBodies;
+	}
+
+	const btSoftBodyArray& getSoftBodyArray() const
+	{
+		return m_softBodies;
+	}
+
+	btSoftBodyWorldInfo& getWorldInfo()
+	{
+		return m_sbi;
+	}
+
+	const btSoftBodyWorldInfo& getWorldInfo() const
+	{
+		return m_sbi;
+	}
+
+	void reinitialize(btScalar timeStep);
+
+	void applyRigidBodyGravity(btScalar timeStep);
+
+	void beforeSolverCallbacks(btScalar timeStep);
+
+	void afterSolverCallbacks(btScalar timeStep);
+
+	void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
+
+	void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force);
+
+	void removeSoftBodyForce(btSoftBody* psb);
+
+	void removeSoftBody(btSoftBody* body);
+
+	void removeCollisionObject(btCollisionObject* collisionObject);
+
+	int getDrawFlags() const { return (m_drawFlags); }
+	void setDrawFlags(int f) { m_drawFlags = f; }
+
+	void setupConstraints();
+
+	void performDeformableCollisionDetection();
+
+	void solveMultiBodyConstraints();
+
+	void solveContactConstraints();
+
+	void sortConstraints();
+
+	void softBodySelfCollision();
+
+	void setImplicit(bool implicit)
+	{
+		m_implicit = implicit;
+	}
+
+	void setLineSearch(bool lineSearch)
+	{
+		m_lineSearch = lineSearch;
+	}
+
+	void setUseProjection(bool useProjection)
+	{
+		m_useProjection = useProjection;
+	}
+
+	void applyRepulsionForce(btScalar timeStep);
+
+	void performGeometricCollisions(btScalar timeStep);
+
+	struct btDeformableSingleRayCallback : public btBroadphaseRayCallback
+	{
+		btVector3 m_rayFromWorld;
+		btVector3 m_rayToWorld;
+		btTransform m_rayFromTrans;
+		btTransform m_rayToTrans;
+		btVector3 m_hitNormal;
+
+		const btDeformableMultiBodyDynamicsWorld* m_world;
+		btCollisionWorld::RayResultCallback& m_resultCallback;
+
+		btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
+			: m_rayFromWorld(rayFromWorld),
+			  m_rayToWorld(rayToWorld),
+			  m_world(world),
+			  m_resultCallback(resultCallback)
+		{
+			m_rayFromTrans.setIdentity();
+			m_rayFromTrans.setOrigin(m_rayFromWorld);
+			m_rayToTrans.setIdentity();
+			m_rayToTrans.setOrigin(m_rayToWorld);
+
+			btVector3 rayDir = (rayToWorld - rayFromWorld);
+
+			rayDir.normalize();
+			///what about division by zero? --> just set rayDirection[i] to INF/1e30
+			m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
+			m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
+			m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
+			m_signs[0] = m_rayDirectionInverse[0] < 0.0;
+			m_signs[1] = m_rayDirectionInverse[1] < 0.0;
+			m_signs[2] = m_rayDirectionInverse[2] < 0.0;
+
+			m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
+		}
+
+		virtual bool process(const btBroadphaseProxy* proxy)
+		{
+			///terminate further ray tests, once the closestHitFraction reached zero
+			if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
+				return false;
+
+			btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
+
+			//only perform raycast if filterMask matches
+			if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
+			{
+				//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+				//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+#if 0
+#ifdef RECALCULATE_AABB
+                btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+                collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+#else
+                //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
+                const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
+                const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
+#endif
+#endif
+				//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
+				//culling already done by broadphase
+				//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
+				{
+					m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
+										   collisionObject,
+										   collisionObject->getCollisionShape(),
+										   collisionObject->getWorldTransform(),
+										   m_resultCallback);
+				}
+			}
+			return true;
+		}
+	};
+
+	void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
+	{
+		BT_PROFILE("rayTest");
+		/// use the broadphase to accelerate the search for objects, based on their aabb
+		/// and for each object with ray-aabb overlap, perform an exact ray test
+		btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback);
+
+#ifndef USE_BRUTEFORCE_RAYBROADPHASE
+		m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
+#else
+		for (int i = 0; i < this->getNumCollisionObjects(); i++)
+		{
+			rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
+		}
+#endif  //USE_BRUTEFORCE_RAYBROADPHASE
+	}
+
+	void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
+					   btCollisionObject* collisionObject,
+					   const btCollisionShape* collisionShape,
+					   const btTransform& colObjWorldTransform,
+					   RayResultCallback& resultCallback) const
+	{
+		if (collisionShape->isSoftBody())
+		{
+			btSoftBody* softBody = btSoftBody::upcast(collisionObject);
+			if (softBody)
+			{
+				btSoftBody::sRayCast softResult;
+				if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
+				{
+					if (softResult.fraction <= resultCallback.m_closestHitFraction)
+					{
+						btCollisionWorld::LocalShapeInfo shapeInfo;
+						shapeInfo.m_shapePart = 0;
+						shapeInfo.m_triangleIndex = softResult.index;
+						// get the normal
+						btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
+						btVector3 normal = -rayDir;
+						normal.normalize();
+						{
+							normal = softBody->m_faces[softResult.index].m_normal;
+							if (normal.dot(rayDir) > 0)
+							{
+								// normal always point toward origin of the ray
+								normal = -normal;
+							}
+						}
+
+						btCollisionWorld::LocalRayResult rayResult(collisionObject,
+																   &shapeInfo,
+																   normal,
+																   softResult.fraction);
+						bool normalInWorldSpace = true;
+						resultCallback.addSingleResult(rayResult, normalInWorldSpace);
+					}
+				}
+			}
+		}
+		else
+		{
+			btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback);
+		}
+	}
+};
+
+#endif  //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/extern/bullet2/src/BulletSoftBody/btDeformableNeoHookeanForce.h
new file mode 100644
index 00000000000..60798c5bcd3
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btDeformableNeoHookeanForce.h
@@ -0,0 +1,420 @@
+/*
+Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2019 Google Inc. http://bulletphysics.org
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_NEOHOOKEAN_H
+#define BT_NEOHOOKEAN_H
+
+#include "btDeformableLagrangianForce.h"
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btImplicitQRSVD.h"
+// This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf
+class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
+{
+public:
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btScalar m_mu, m_lambda;  // Lame Parameters
+	btScalar m_E, m_nu;       // Young's modulus and Poisson ratio
+	btScalar m_mu_damp, m_lambda_damp;
+	btDeformableNeoHookeanForce() : m_mu(1), m_lambda(1)
+	{
+		btScalar damping = 0.05;
+		m_mu_damp = damping * m_mu;
+		m_lambda_damp = damping * m_lambda;
+		updateYoungsModulusAndPoissonRatio();
+	}
+
+	btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05) : m_mu(mu), m_lambda(lambda)
+	{
+		m_mu_damp = damping * m_mu;
+		m_lambda_damp = damping * m_lambda;
+		updateYoungsModulusAndPoissonRatio();
+	}
+
+	void updateYoungsModulusAndPoissonRatio()
+	{
+		// conversion from Lame Parameters to Young's modulus and Poisson ratio
+		// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
+		m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu);
+		m_nu = m_lambda * 0.5 / (m_mu + m_lambda);
+	}
+
+	void updateLameParameters()
+	{
+		// conversion from Young's modulus and Poisson ratio to Lame Parameters
+		// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
+		m_mu = m_E * 0.5 / (1 + m_nu);
+		m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu));
+	}
+
+	void setYoungsModulus(btScalar E)
+	{
+		m_E = E;
+		updateLameParameters();
+	}
+
+	void setPoissonRatio(btScalar nu)
+	{
+		m_nu = nu;
+		updateLameParameters();
+	}
+
+	void setDamping(btScalar damping)
+	{
+		m_mu_damp = damping * m_mu;
+		m_lambda_damp = damping * m_lambda;
+	}
+
+	void setLameParameters(btScalar mu, btScalar lambda)
+	{
+		m_mu = mu;
+		m_lambda = lambda;
+		updateYoungsModulusAndPoissonRatio();
+	}
+
+	virtual void addScaledForces(btScalar scale, TVStack& force)
+	{
+		addScaledDampingForce(scale, force);
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+	{
+		addScaledElasticForce(scale, force);
+	}
+
+	// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
+	virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+	{
+		if (m_mu_damp == 0 && m_lambda_damp == 0)
+			return;
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= force.size());
+		btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_tetras.size(); ++j)
+			{
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btSoftBody::Node* node0 = tetra.m_n[0];
+				btSoftBody::Node* node1 = tetra.m_n[1];
+				btSoftBody::Node* node2 = tetra.m_n[2];
+				btSoftBody::Node* node3 = tetra.m_n[3];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				size_t id3 = node3->index;
+				btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
+				btMatrix3x3 I;
+				I.setIdentity();
+				btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp;
+				//                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
+				btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col);
+				btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
+
+				// damping force differential
+				btScalar scale1 = scale * tetra.m_element_measure;
+				force[id0] -= scale1 * df_on_node0;
+				force[id1] -= scale1 * df_on_node123.getColumn(0);
+				force[id2] -= scale1 * df_on_node123.getColumn(1);
+				force[id3] -= scale1 * df_on_node123.getColumn(2);
+			}
+		}
+	}
+
+	virtual double totalElasticEnergy(btScalar dt)
+	{
+		double energy = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
+			{
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
+				energy += tetra.m_element_measure * elasticEnergyDensity(s);
+			}
+		}
+		return energy;
+	}
+
+	// The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
+	virtual double totalDampingEnergy(btScalar dt)
+	{
+		double energy = 0;
+		int sz = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				sz = btMax(sz, psb->m_nodes[j].index);
+			}
+		}
+		TVStack dampingForce;
+		dampingForce.resize(sz + 1);
+		for (int i = 0; i < dampingForce.size(); ++i)
+			dampingForce[i].setZero();
+		addScaledDampingForce(0.5, dampingForce);
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				const btSoftBody::Node& node = psb->m_nodes[j];
+				energy -= dampingForce[node.index].dot(node.m_v) / dt;
+			}
+		}
+		return energy;
+	}
+
+	double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
+	{
+		double density = 0;
+		density += m_mu * 0.5 * (s.m_trace - 3.);
+		density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda) * (s.m_J - 1. - 0.75 * m_mu / m_lambda);
+		density -= m_mu * 0.5 * log(s.m_trace + 1);
+		return density;
+	}
+
+	virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= force.size());
+		btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			btScalar max_p = psb->m_cfg.m_maxStress;
+			for (int j = 0; j < psb->m_tetras.size(); ++j)
+			{
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btMatrix3x3 P;
+				firstPiola(psb->m_tetraScratches[j], P);
+#ifdef USE_SVD
+				if (max_p > 0)
+				{
+					// since we want to clamp the principal stress to max_p, we only need to
+					// calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
+					btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
+					if (trPTP > max_p * max_p)
+					{
+						btMatrix3x3 U, V;
+						btVector3 sigma;
+						singularValueDecomposition(P, U, sigma, V);
+						sigma[0] = btMin(sigma[0], max_p);
+						sigma[1] = btMin(sigma[1], max_p);
+						sigma[2] = btMin(sigma[2], max_p);
+						sigma[0] = btMax(sigma[0], -max_p);
+						sigma[1] = btMax(sigma[1], -max_p);
+						sigma[2] = btMax(sigma[2], -max_p);
+						btMatrix3x3 Sigma;
+						Sigma.setIdentity();
+						Sigma[0][0] = sigma[0];
+						Sigma[1][1] = sigma[1];
+						Sigma[2][2] = sigma[2];
+						P = U * Sigma * V.transpose();
+					}
+				}
+#endif
+				//                btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+				btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
+				btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
+
+				btSoftBody::Node* node0 = tetra.m_n[0];
+				btSoftBody::Node* node1 = tetra.m_n[1];
+				btSoftBody::Node* node2 = tetra.m_n[2];
+				btSoftBody::Node* node3 = tetra.m_n[3];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				size_t id3 = node3->index;
+
+				// elastic force
+				btScalar scale1 = scale * tetra.m_element_measure;
+				force[id0] -= scale1 * force_on_node0;
+				force[id1] -= scale1 * force_on_node123.getColumn(0);
+				force[id2] -= scale1 * force_on_node123.getColumn(1);
+				force[id3] -= scale1 * force_on_node123.getColumn(2);
+			}
+		}
+	}
+
+	// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
+	virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+	{
+		if (m_mu_damp == 0 && m_lambda_damp == 0)
+			return;
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= df.size());
+		btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_tetras.size(); ++j)
+			{
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btSoftBody::Node* node0 = tetra.m_n[0];
+				btSoftBody::Node* node1 = tetra.m_n[1];
+				btSoftBody::Node* node2 = tetra.m_n[2];
+				btSoftBody::Node* node3 = tetra.m_n[3];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				size_t id3 = node3->index;
+				btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
+				btMatrix3x3 I;
+				I.setIdentity();
+				btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp;
+				//                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
+				//                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+				btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
+				btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
+
+				// damping force differential
+				btScalar scale1 = scale * tetra.m_element_measure;
+				df[id0] -= scale1 * df_on_node0;
+				df[id1] -= scale1 * df_on_node123.getColumn(0);
+				df[id2] -= scale1 * df_on_node123.getColumn(1);
+				df[id3] -= scale1 * df_on_node123.getColumn(2);
+			}
+		}
+	}
+
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
+
+	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= df.size());
+		btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_tetras.size(); ++j)
+			{
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btSoftBody::Node* node0 = tetra.m_n[0];
+				btSoftBody::Node* node1 = tetra.m_n[1];
+				btSoftBody::Node* node2 = tetra.m_n[2];
+				btSoftBody::Node* node3 = tetra.m_n[3];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				size_t id3 = node3->index;
+				btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
+				btMatrix3x3 dP;
+				firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
+				//                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+				btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
+				btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
+
+				// elastic force differential
+				btScalar scale1 = scale * tetra.m_element_measure;
+				df[id0] -= scale1 * df_on_node0;
+				df[id1] -= scale1 * df_on_node123.getColumn(0);
+				df[id2] -= scale1 * df_on_node123.getColumn(1);
+				df[id3] -= scale1 * df_on_node123.getColumn(2);
+			}
+		}
+	}
+
+	void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
+	{
+		btScalar c1 = (m_mu * (1. - 1. / (s.m_trace + 1.)));
+		btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
+		P = s.m_F * c1 + s.m_cofF * c2;
+	}
+
+	// Let P be the first piola stress.
+	// This function calculates the dP = dP/dF * dF
+	void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
+	{
+		btScalar c1 = m_mu * (1. - 1. / (s.m_trace + 1.));
+		btScalar c2 = (2. * m_mu) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace)));
+		btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF));
+		dP = dF * c1 + s.m_F * c2;
+		addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda * (s.m_J - 1.) - 0.75 * m_mu, dP);
+		dP += s.m_cofF * c3;
+	}
+
+	// Let Q be the damping stress.
+	// This function calculates the dP = dQ/dF * dF
+	void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
+	{
+		btScalar c1 = (m_mu_damp * (1. - 1. / (s.m_trace + 1.)));
+		btScalar c2 = ((2. * m_mu_damp) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace))));
+		btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF));
+		dP = dF * c1 + s.m_F * c2;
+		addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp * (s.m_J - 1.) - 0.75 * m_mu_damp, dP);
+		dP += s.m_cofF * c3;
+	}
+
+	btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)
+	{
+		btScalar ans = 0;
+		for (int i = 0; i < 3; ++i)
+		{
+			ans += A[i].dot(B[i]);
+		}
+		return ans;
+	}
+
+	// Let C(A) be the cofactor of the matrix A
+	// Let H = the derivative of C(A) with respect to A evaluated at F = A
+	// This function calculates H*dF
+	void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M)
+	{
+		M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);
+		M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]);
+		M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]);
+		M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]);
+		M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]);
+		M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]);
+		M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]);
+		M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
+		M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]);
+	}
+
+	virtual btDeformableLagrangianForceType getForceType()
+	{
+		return BT_NEOHOOKEAN_FORCE;
+	}
+};
+#endif /* BT_NEOHOOKEAN_H */
diff --git a/extern/bullet2/src/BulletSoftBody/btKrylovSolver.h b/extern/bullet2/src/BulletSoftBody/btKrylovSolver.h
new file mode 100644
index 00000000000..59126b47ae2
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btKrylovSolver.h
@@ -0,0 +1,107 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_KRYLOV_SOLVER_H
+#define BT_KRYLOV_SOLVER_H
+#include <iostream>
+#include <cmath>
+#include <limits>
+#include <LinearMath/btAlignedObjectArray.h>
+#include <LinearMath/btVector3.h>
+#include <LinearMath/btScalar.h>
+#include "LinearMath/btQuickprof.h"
+
+template <class MatrixX>
+class btKrylovSolver
+{
+	typedef btAlignedObjectArray<btVector3> TVStack;
+
+public:
+	int m_maxIterations;
+	btScalar m_tolerance;
+	btKrylovSolver(int maxIterations, btScalar tolerance)
+		: m_maxIterations(maxIterations), m_tolerance(tolerance)
+	{
+	}
+
+	virtual ~btKrylovSolver() {}
+
+	virtual int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) = 0;
+
+	virtual void reinitialize(const TVStack& b) = 0;
+
+	virtual SIMD_FORCE_INLINE TVStack sub(const TVStack& a, const TVStack& b)
+	{
+		// c = a-b
+		btAssert(a.size() == b.size());
+		TVStack c;
+		c.resize(a.size());
+		for (int i = 0; i < a.size(); ++i)
+		{
+			c[i] = a[i] - b[i];
+		}
+		return c;
+	}
+
+	virtual SIMD_FORCE_INLINE btScalar squaredNorm(const TVStack& a)
+	{
+		return dot(a, a);
+	}
+
+	virtual SIMD_FORCE_INLINE btScalar norm(const TVStack& a)
+	{
+		btScalar ret = 0;
+		for (int i = 0; i < a.size(); ++i)
+		{
+			for (int d = 0; d < 3; ++d)
+			{
+				ret = btMax(ret, btFabs(a[i][d]));
+			}
+		}
+		return ret;
+	}
+
+	virtual SIMD_FORCE_INLINE btScalar dot(const TVStack& a, const TVStack& b)
+	{
+		btScalar ans(0);
+		for (int i = 0; i < a.size(); ++i)
+			ans += a[i].dot(b[i]);
+		return ans;
+	}
+
+	virtual SIMD_FORCE_INLINE void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
+	{
+		//        result += s*a
+		btAssert(a.size() == result.size());
+		for (int i = 0; i < a.size(); ++i)
+			result[i] += s * a[i];
+	}
+
+	virtual SIMD_FORCE_INLINE TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b)
+	{
+		// result = a*s + b
+		TVStack result;
+		result.resize(a.size());
+		for (int i = 0; i < a.size(); ++i)
+			result[i] = s * a[i] + b[i];
+		return result;
+	}
+
+	virtual SIMD_FORCE_INLINE void setTolerance(btScalar tolerance)
+	{
+		m_tolerance = tolerance;
+	}
+};
+#endif /* BT_KRYLOV_SOLVER_H */
diff --git a/extern/bullet2/src/BulletSoftBody/btPreconditioner.h b/extern/bullet2/src/BulletSoftBody/btPreconditioner.h
new file mode 100644
index 00000000000..21c1106a426
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btPreconditioner.h
@@ -0,0 +1,285 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_PRECONDITIONER_H
+#define BT_PRECONDITIONER_H
+
+class Preconditioner
+{
+public:
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	virtual void operator()(const TVStack& x, TVStack& b) = 0;
+	virtual void reinitialize(bool nodeUpdated) = 0;
+	virtual ~Preconditioner() {}
+};
+
+class DefaultPreconditioner : public Preconditioner
+{
+public:
+	virtual void operator()(const TVStack& x, TVStack& b)
+	{
+		btAssert(b.size() == x.size());
+		for (int i = 0; i < b.size(); ++i)
+			b[i] = x[i];
+	}
+	virtual void reinitialize(bool nodeUpdated)
+	{
+	}
+
+	virtual ~DefaultPreconditioner() {}
+};
+
+class MassPreconditioner : public Preconditioner
+{
+	btAlignedObjectArray<btScalar> m_inv_mass;
+	const btAlignedObjectArray<btSoftBody*>& m_softBodies;
+
+public:
+	MassPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies)
+		: m_softBodies(softBodies)
+	{
+	}
+
+	virtual void reinitialize(bool nodeUpdated)
+	{
+		if (nodeUpdated)
+		{
+			m_inv_mass.clear();
+			for (int i = 0; i < m_softBodies.size(); ++i)
+			{
+				btSoftBody* psb = m_softBodies[i];
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+					m_inv_mass.push_back(psb->m_nodes[j].m_im);
+			}
+		}
+	}
+
+	virtual void operator()(const TVStack& x, TVStack& b)
+	{
+		btAssert(b.size() == x.size());
+		btAssert(m_inv_mass.size() <= x.size());
+		for (int i = 0; i < m_inv_mass.size(); ++i)
+		{
+			b[i] = x[i] * m_inv_mass[i];
+		}
+		for (int i = m_inv_mass.size(); i < b.size(); ++i)
+		{
+			b[i] = x[i];
+		}
+	}
+};
+
+class KKTPreconditioner : public Preconditioner
+{
+	const btAlignedObjectArray<btSoftBody*>& m_softBodies;
+	const btDeformableContactProjection& m_projections;
+	const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf;
+	TVStack m_inv_A, m_inv_S;
+	const btScalar& m_dt;
+	const bool& m_implicit;
+
+public:
+	KKTPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit)
+		: m_softBodies(softBodies), m_projections(projections), m_lf(lf), m_dt(dt), m_implicit(implicit)
+	{
+	}
+
+	virtual void reinitialize(bool nodeUpdated)
+	{
+		if (nodeUpdated)
+		{
+			int num_nodes = 0;
+			for (int i = 0; i < m_softBodies.size(); ++i)
+			{
+				btSoftBody* psb = m_softBodies[i];
+				num_nodes += psb->m_nodes.size();
+			}
+			m_inv_A.resize(num_nodes);
+		}
+		buildDiagonalA(m_inv_A);
+		for (int i = 0; i < m_inv_A.size(); ++i)
+		{
+			//            printf("A[%d] = %f, %f, %f \n", i, m_inv_A[i][0], m_inv_A[i][1], m_inv_A[i][2]);
+			for (int d = 0; d < 3; ++d)
+			{
+				m_inv_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0 / m_inv_A[i][d];
+			}
+		}
+		m_inv_S.resize(m_projections.m_lagrangeMultipliers.size());
+		//        printf("S.size() = %d \n", m_inv_S.size());
+		buildDiagonalS(m_inv_A, m_inv_S);
+		for (int i = 0; i < m_inv_S.size(); ++i)
+		{
+			//            printf("S[%d] = %f, %f, %f \n", i, m_inv_S[i][0], m_inv_S[i][1], m_inv_S[i][2]);
+			for (int d = 0; d < 3; ++d)
+			{
+				m_inv_S[i][d] = (m_inv_S[i][d] == 0) ? 0.0 : 1.0 / m_inv_S[i][d];
+			}
+		}
+	}
+
+	void buildDiagonalA(TVStack& diagA) const
+	{
+		size_t counter = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				const btSoftBody::Node& node = psb->m_nodes[j];
+				diagA[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : btVector3(1.0 / node.m_im, 1.0 / node.m_im, 1.0 / node.m_im);
+				++counter;
+			}
+		}
+		if (m_implicit)
+		{
+			printf("implicit not implemented\n");
+			btAssert(false);
+		}
+		for (int i = 0; i < m_lf.size(); ++i)
+		{
+			// add damping matrix
+			m_lf[i]->buildDampingForceDifferentialDiagonal(-m_dt, diagA);
+		}
+	}
+
+	void buildDiagonalS(const TVStack& inv_A, TVStack& diagS)
+	{
+		for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
+		{
+			// S[k,k] = e_k^T * C A_d^-1 C^T * e_k
+			const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
+			btVector3& t = diagS[c];
+			t.setZero();
+			for (int j = 0; j < lm.m_num_constraints; ++j)
+			{
+				for (int i = 0; i < lm.m_num_nodes; ++i)
+				{
+					for (int d = 0; d < 3; ++d)
+					{
+						t[j] += inv_A[lm.m_indices[i]][d] * lm.m_dirs[j][d] * lm.m_dirs[j][d] * lm.m_weights[i] * lm.m_weights[i];
+					}
+				}
+			}
+		}
+	}
+//#define USE_FULL_PRECONDITIONER
+#ifndef USE_FULL_PRECONDITIONER
+	virtual void operator()(const TVStack& x, TVStack& b)
+	{
+		btAssert(b.size() == x.size());
+		for (int i = 0; i < m_inv_A.size(); ++i)
+		{
+			b[i] = x[i] * m_inv_A[i];
+		}
+		int offset = m_inv_A.size();
+		for (int i = 0; i < m_inv_S.size(); ++i)
+		{
+			b[i + offset] = x[i + offset] * m_inv_S[i];
+		}
+	}
+#else
+	virtual void operator()(const TVStack& x, TVStack& b)
+	{
+		btAssert(b.size() == x.size());
+		int offset = m_inv_A.size();
+
+		for (int i = 0; i < m_inv_A.size(); ++i)
+		{
+			b[i] = x[i] * m_inv_A[i];
+		}
+
+		for (int i = 0; i < m_inv_S.size(); ++i)
+		{
+			b[i + offset].setZero();
+		}
+
+		for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
+		{
+			const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
+			// C * x
+			for (int d = 0; d < lm.m_num_constraints; ++d)
+			{
+				for (int i = 0; i < lm.m_num_nodes; ++i)
+				{
+					b[offset + c][d] += lm.m_weights[i] * b[lm.m_indices[i]].dot(lm.m_dirs[d]);
+				}
+			}
+		}
+
+		for (int i = 0; i < m_inv_S.size(); ++i)
+		{
+			b[i + offset] = b[i + offset] * m_inv_S[i];
+		}
+
+		for (int i = 0; i < m_inv_A.size(); ++i)
+		{
+			b[i].setZero();
+		}
+
+		for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
+		{
+			// C^T * lambda
+			const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
+			for (int i = 0; i < lm.m_num_nodes; ++i)
+			{
+				for (int j = 0; j < lm.m_num_constraints; ++j)
+				{
+					b[lm.m_indices[i]] += b[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
+				}
+			}
+		}
+
+		for (int i = 0; i < m_inv_A.size(); ++i)
+		{
+			b[i] = (x[i] - b[i]) * m_inv_A[i];
+		}
+
+		TVStack t;
+		t.resize(b.size());
+		for (int i = 0; i < m_inv_S.size(); ++i)
+		{
+			t[i + offset] = x[i + offset] * m_inv_S[i];
+		}
+		for (int i = 0; i < m_inv_A.size(); ++i)
+		{
+			t[i].setZero();
+		}
+		for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
+		{
+			// C^T * lambda
+			const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
+			for (int i = 0; i < lm.m_num_nodes; ++i)
+			{
+				for (int j = 0; j < lm.m_num_constraints; ++j)
+				{
+					t[lm.m_indices[i]] += t[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
+				}
+			}
+		}
+		for (int i = 0; i < m_inv_A.size(); ++i)
+		{
+			b[i] += t[i] * m_inv_A[i];
+		}
+
+		for (int i = 0; i < m_inv_S.size(); ++i)
+		{
+			b[i + offset] -= x[i + offset] * m_inv_S[i];
+		}
+	}
+#endif
+};
+
+#endif /* BT_PRECONDITIONER_H */
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp
index 51f4b33d034..0597a8016cd 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp
+++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp
@@ -18,442 +18,690 @@ subject to the following restrictions:
 #include "BulletSoftBody/btSoftBodySolvers.h"
 #include "btSoftBodyData.h"
 #include "LinearMath/btSerializer.h"
-
+#include "LinearMath/btImplicitQRSVD.h"
+#include "LinearMath/btAlignedAllocator.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include <iostream>
+//
+static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& leafNodes, btAlignedObjectArray<btAlignedObjectArray<int> >& adj)
+{
+	int N = leafNodes.size();
+	if (N == 0)
+	{
+		return NULL;
+	}
+	while (N > 1)
+	{
+		btAlignedObjectArray<bool> marked;
+		btAlignedObjectArray<btDbvtNode*> newLeafNodes;
+		btAlignedObjectArray<std::pair<int, int> > childIds;
+		btAlignedObjectArray<btAlignedObjectArray<int> > newAdj;
+		marked.resize(N);
+		for (int i = 0; i < N; ++i)
+			marked[i] = false;
+
+		// pair adjacent nodes into new(parent) node
+		for (int i = 0; i < N; ++i)
+		{
+			if (marked[i])
+				continue;
+			bool merged = false;
+			for (int j = 0; j < adj[i].size(); ++j)
+			{
+				int n = adj[i][j];
+				if (!marked[adj[i][j]])
+				{
+					btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
+					node->parent = NULL;
+					node->childs[0] = leafNodes[i];
+					node->childs[1] = leafNodes[n];
+					leafNodes[i]->parent = node;
+					leafNodes[n]->parent = node;
+					newLeafNodes.push_back(node);
+					childIds.push_back(std::make_pair(i, n));
+					merged = true;
+					marked[n] = true;
+					break;
+				}
+			}
+			if (!merged)
+			{
+				newLeafNodes.push_back(leafNodes[i]);
+				childIds.push_back(std::make_pair(i, -1));
+			}
+			marked[i] = true;
+		}
+		// update adjacency matrix
+		newAdj.resize(newLeafNodes.size());
+		for (int i = 0; i < newLeafNodes.size(); ++i)
+		{
+			for (int j = i + 1; j < newLeafNodes.size(); ++j)
+			{
+				bool neighbor = false;
+				const btAlignedObjectArray<int>& leftChildNeighbors = adj[childIds[i].first];
+				for (int k = 0; k < leftChildNeighbors.size(); ++k)
+				{
+					if (leftChildNeighbors[k] == childIds[j].first || leftChildNeighbors[k] == childIds[j].second)
+					{
+						neighbor = true;
+						break;
+					}
+				}
+				if (!neighbor && childIds[i].second != -1)
+				{
+					const btAlignedObjectArray<int>& rightChildNeighbors = adj[childIds[i].second];
+					for (int k = 0; k < rightChildNeighbors.size(); ++k)
+					{
+						if (rightChildNeighbors[k] == childIds[j].first || rightChildNeighbors[k] == childIds[j].second)
+						{
+							neighbor = true;
+							break;
+						}
+					}
+				}
+				if (neighbor)
+				{
+					newAdj[i].push_back(j);
+					newAdj[j].push_back(i);
+				}
+			}
+		}
+		leafNodes = newLeafNodes;
+		//this assignment leaks memory, the assignment doesn't do a deep copy, for now a manual copy
+		//adj = newAdj;
+		adj.clear();
+		adj.resize(newAdj.size());
+		for (int i = 0; i < newAdj.size(); i++)
+		{
+			for (int j = 0; j < newAdj[i].size(); j++)
+			{
+				adj[i].push_back(newAdj[i][j]);
+			}
+		}
+		N = leafNodes.size();
+	}
+	return leafNodes[0];
+}
 
 //
-btSoftBody::btSoftBody(btSoftBodyWorldInfo*	worldInfo,int node_count,  const btVector3* x,  const btScalar* m)
-:m_softBodySolver(0),m_worldInfo(worldInfo)
-{	
-	/* Init		*/ 
+btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
+	: m_softBodySolver(0), m_worldInfo(worldInfo)
+{
+	/* Init		*/
 	initDefaults();
 
-	/* Default material	*/ 
-	Material*	pm=appendMaterial();
-	pm->m_kLST	=	1;
-	pm->m_kAST	=	1;
-	pm->m_kVST	=	1;
-	pm->m_flags	=	fMaterial::Default;
+	/* Default material	*/
+	Material* pm = appendMaterial();
+	pm->m_kLST = 1;
+	pm->m_kAST = 1;
+	pm->m_kVST = 1;
+	pm->m_flags = fMaterial::Default;
 
-	/* Nodes			*/ 
-	const btScalar		margin=getCollisionShape()->getMargin();
+	/* Nodes			*/
+	const btScalar margin = getCollisionShape()->getMargin();
 	m_nodes.resize(node_count);
-	for(int i=0,ni=node_count;i<ni;++i)
-	{	
-		Node&	n=m_nodes[i];
+	m_X.resize(node_count);
+	for (int i = 0, ni = node_count; i < ni; ++i)
+	{
+		Node& n = m_nodes[i];
 		ZeroInitialize(n);
-		n.m_x		=	x?*x++:btVector3(0,0,0);
-		n.m_q		=	n.m_x;
-		n.m_im		=	m?*m++:1;
-		n.m_im		=	n.m_im>0?1/n.m_im:0;
-		n.m_leaf	=	m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
-		n.m_material=	pm;
+		n.m_x = x ? *x++ : btVector3(0, 0, 0);
+		n.m_q = n.m_x;
+		n.m_im = m ? *m++ : 1;
+		n.m_im = n.m_im > 0 ? 1 / n.m_im : 0;
+		n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n);
+		n.m_material = pm;
+		m_X[i] = n.m_x;
 	}
-	updateBounds();	
-
+	updateBounds();
+	setCollisionQuadrature(3);
+	m_fdbvnt = 0;
 }
 
-btSoftBody::btSoftBody(btSoftBodyWorldInfo*	worldInfo)
-:m_worldInfo(worldInfo)
+btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo)
+	: m_worldInfo(worldInfo)
 {
 	initDefaults();
 }
 
-
-void	btSoftBody::initDefaults()
-{
-	m_internalType		=	CO_SOFT_BODY;
-	m_cfg.aeromodel		=	eAeroModel::V_Point;
-	m_cfg.kVCF			=	1;
-	m_cfg.kDG			=	0;
-	m_cfg.kLF			=	0;
-	m_cfg.kDP			=	0;
-	m_cfg.kPR			=	0;
-	m_cfg.kVC			=	0;
-	m_cfg.kDF			=	(btScalar)0.2;
-	m_cfg.kMT			=	0;
-	m_cfg.kCHR			=	(btScalar)1.0;
-	m_cfg.kKHR			=	(btScalar)0.1;
-	m_cfg.kSHR			=	(btScalar)1.0;
-	m_cfg.kAHR			=	(btScalar)0.7;
-	m_cfg.kSRHR_CL		=	(btScalar)0.1;
-	m_cfg.kSKHR_CL		=	(btScalar)1;
-	m_cfg.kSSHR_CL		=	(btScalar)0.5;
-	m_cfg.kSR_SPLT_CL	=	(btScalar)0.5;
-	m_cfg.kSK_SPLT_CL	=	(btScalar)0.5;
-	m_cfg.kSS_SPLT_CL	=	(btScalar)0.5;
-	m_cfg.maxvolume		=	(btScalar)1;
-	m_cfg.timescale		=	1;
-	m_cfg.viterations	=	0;
-	m_cfg.piterations	=	1;	
-	m_cfg.diterations	=	0;
-	m_cfg.citerations	=	4;
-	m_cfg.collisions	=	fCollision::Default;
-	m_pose.m_bvolume	=	false;
-	m_pose.m_bframe		=	false;
-	m_pose.m_volume		=	0;
-	m_pose.m_com		=	btVector3(0,0,0);
+void btSoftBody::initDefaults()
+{
+	m_internalType = CO_SOFT_BODY;
+	m_cfg.aeromodel = eAeroModel::V_Point;
+	m_cfg.kVCF = 1;
+	m_cfg.kDG = 0;
+	m_cfg.kLF = 0;
+	m_cfg.kDP = 0;
+	m_cfg.kPR = 0;
+	m_cfg.kVC = 0;
+	m_cfg.kDF = (btScalar)0.2;
+	m_cfg.kMT = 0;
+	m_cfg.kCHR = (btScalar)1.0;
+	m_cfg.kKHR = (btScalar)0.1;
+	m_cfg.kSHR = (btScalar)1.0;
+	m_cfg.kAHR = (btScalar)0.7;
+	m_cfg.kSRHR_CL = (btScalar)0.1;
+	m_cfg.kSKHR_CL = (btScalar)1;
+	m_cfg.kSSHR_CL = (btScalar)0.5;
+	m_cfg.kSR_SPLT_CL = (btScalar)0.5;
+	m_cfg.kSK_SPLT_CL = (btScalar)0.5;
+	m_cfg.kSS_SPLT_CL = (btScalar)0.5;
+	m_cfg.maxvolume = (btScalar)1;
+	m_cfg.timescale = 1;
+	m_cfg.viterations = 0;
+	m_cfg.piterations = 1;
+	m_cfg.diterations = 0;
+	m_cfg.citerations = 4;
+	m_cfg.drag = 0;
+	m_cfg.m_maxStress = 0;
+	m_cfg.collisions = fCollision::Default;
+	m_pose.m_bvolume = false;
+	m_pose.m_bframe = false;
+	m_pose.m_volume = 0;
+	m_pose.m_com = btVector3(0, 0, 0);
 	m_pose.m_rot.setIdentity();
 	m_pose.m_scl.setIdentity();
-	m_tag				=	0;
-	m_timeacc			=	0;
-	m_bUpdateRtCst		=	true;
-	m_bounds[0]			=	btVector3(0,0,0);
-	m_bounds[1]			=	btVector3(0,0,0);
+	m_tag = 0;
+	m_timeacc = 0;
+	m_bUpdateRtCst = true;
+	m_bounds[0] = btVector3(0, 0, 0);
+	m_bounds[1] = btVector3(0, 0, 0);
 	m_worldTransform.setIdentity();
 	setSolver(eSolverPresets::Positions);
-	
-	/* Collision shape	*/ 
+
+	/* Collision shape	*/
 	///for now, create a collision shape internally
 	m_collisionShape = new btSoftBodyCollisionShape(this);
 	m_collisionShape->setMargin(0.25f);
-	
-	m_initialWorldTransform.setIdentity();
 
-	m_windVelocity = btVector3(0,0,0);
+	m_worldTransform.setIdentity();
+
+	m_windVelocity = btVector3(0, 0, 0);
 	m_restLengthScale = btScalar(1.0);
+	m_dampingCoefficient = 1.0;
+	m_sleepingThreshold = .04;
+	m_useSelfCollision = false;
+	m_collisionFlags = 0;
+	m_softSoftCollision = false;
+	m_maxSpeedSquared = 0;
+	m_repulsionStiffness = 0.5;
+	m_gravityFactor = 1;
+	m_fdbvnt = 0;
 }
 
 //
 btSoftBody::~btSoftBody()
 {
 	//for now, delete the internal shape
-	delete m_collisionShape;	
+	delete m_collisionShape;
 	int i;
 
 	releaseClusters();
-	for(i=0;i<m_materials.size();++i) 
+	for (i = 0; i < m_materials.size(); ++i)
 		btAlignedFree(m_materials[i]);
-	for(i=0;i<m_joints.size();++i) 
+	for (i = 0; i < m_joints.size(); ++i)
 		btAlignedFree(m_joints[i]);
+	if (m_fdbvnt)
+		delete m_fdbvnt;
 }
 
 //
-bool			btSoftBody::checkLink(int node0,int node1) const
+bool btSoftBody::checkLink(int node0, int node1) const
 {
-	return(checkLink(&m_nodes[node0],&m_nodes[node1]));
+	return (checkLink(&m_nodes[node0], &m_nodes[node1]));
 }
 
 //
-bool			btSoftBody::checkLink(const Node* node0,const Node* node1) const
+bool btSoftBody::checkLink(const Node* node0, const Node* node1) const
 {
-	const Node*	n[]={node0,node1};
-	for(int i=0,ni=m_links.size();i<ni;++i)
+	const Node* n[] = {node0, node1};
+	for (int i = 0, ni = m_links.size(); i < ni; ++i)
 	{
-		const Link&	l=m_links[i];
-		if(	(l.m_n[0]==n[0]&&l.m_n[1]==n[1])||
-			(l.m_n[0]==n[1]&&l.m_n[1]==n[0]))
+		const Link& l = m_links[i];
+		if ((l.m_n[0] == n[0] && l.m_n[1] == n[1]) ||
+			(l.m_n[0] == n[1] && l.m_n[1] == n[0]))
 		{
-			return(true);
+			return (true);
 		}
 	}
-	return(false);
+	return (false);
 }
 
 //
-bool			btSoftBody::checkFace(int node0,int node1,int node2) const
+bool btSoftBody::checkFace(int node0, int node1, int node2) const
 {
-	const Node*	n[]={	&m_nodes[node0],
-		&m_nodes[node1],
-		&m_nodes[node2]};
-	for(int i=0,ni=m_faces.size();i<ni;++i)
+	const Node* n[] = {&m_nodes[node0],
+					   &m_nodes[node1],
+					   &m_nodes[node2]};
+	for (int i = 0, ni = m_faces.size(); i < ni; ++i)
 	{
-		const Face&	f=m_faces[i];
-		int			c=0;
-		for(int j=0;j<3;++j)
+		const Face& f = m_faces[i];
+		int c = 0;
+		for (int j = 0; j < 3; ++j)
 		{
-			if(	(f.m_n[j]==n[0])||
-				(f.m_n[j]==n[1])||
-				(f.m_n[j]==n[2])) c|=1<<j; else break;
+			if ((f.m_n[j] == n[0]) ||
+				(f.m_n[j] == n[1]) ||
+				(f.m_n[j] == n[2]))
+				c |= 1 << j;
+			else
+				break;
 		}
-		if(c==7) return(true);
+		if (c == 7) return (true);
 	}
-	return(false);
+	return (false);
 }
 
 //
-btSoftBody::Material*		btSoftBody::appendMaterial()
+btSoftBody::Material* btSoftBody::appendMaterial()
 {
-	Material*	pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
-	if(m_materials.size()>0)
-		*pm=*m_materials[0];
+	Material* pm = new (btAlignedAlloc(sizeof(Material), 16)) Material();
+	if (m_materials.size() > 0)
+		*pm = *m_materials[0];
 	else
 		ZeroInitialize(*pm);
 	m_materials.push_back(pm);
-	return(pm);
+	return (pm);
 }
 
 //
-void			btSoftBody::appendNote(	const char* text,
-									   const btVector3& o,
-									   const btVector4& c,
-									   Node* n0,
-									   Node* n1,
-									   Node* n2,
-									   Node* n3)
+void btSoftBody::appendNote(const char* text,
+							const btVector3& o,
+							const btVector4& c,
+							Node* n0,
+							Node* n1,
+							Node* n2,
+							Node* n3)
 {
-	Note	n;
+	Note n;
 	ZeroInitialize(n);
-	n.m_rank		=	0;
-	n.m_text		=	text;
-	n.m_offset		=	o;
-	n.m_coords[0]	=	c.x();
-	n.m_coords[1]	=	c.y();
-	n.m_coords[2]	=	c.z();
-	n.m_coords[3]	=	c.w();
-	n.m_nodes[0]	=	n0;n.m_rank+=n0?1:0;
-	n.m_nodes[1]	=	n1;n.m_rank+=n1?1:0;
-	n.m_nodes[2]	=	n2;n.m_rank+=n2?1:0;
-	n.m_nodes[3]	=	n3;n.m_rank+=n3?1:0;
+	n.m_rank = 0;
+	n.m_text = text;
+	n.m_offset = o;
+	n.m_coords[0] = c.x();
+	n.m_coords[1] = c.y();
+	n.m_coords[2] = c.z();
+	n.m_coords[3] = c.w();
+	n.m_nodes[0] = n0;
+	n.m_rank += n0 ? 1 : 0;
+	n.m_nodes[1] = n1;
+	n.m_rank += n1 ? 1 : 0;
+	n.m_nodes[2] = n2;
+	n.m_rank += n2 ? 1 : 0;
+	n.m_nodes[3] = n3;
+	n.m_rank += n3 ? 1 : 0;
 	m_notes.push_back(n);
 }
 
 //
-void			btSoftBody::appendNote(	const char* text,
-									   const btVector3& o,
-									   Node* feature)
+void btSoftBody::appendNote(const char* text,
+							const btVector3& o,
+							Node* feature)
 {
-	appendNote(text,o,btVector4(1,0,0,0),feature);
+	appendNote(text, o, btVector4(1, 0, 0, 0), feature);
 }
 
 //
-void			btSoftBody::appendNote(	const char* text,
-									   const btVector3& o,
-									   Link* feature)
+void btSoftBody::appendNote(const char* text,
+							const btVector3& o,
+							Link* feature)
 {
-	static const btScalar	w=1/(btScalar)2;
-	appendNote(text,o,btVector4(w,w,0,0),	feature->m_n[0],
-		feature->m_n[1]);
+	static const btScalar w = 1 / (btScalar)2;
+	appendNote(text, o, btVector4(w, w, 0, 0), feature->m_n[0],
+			   feature->m_n[1]);
 }
 
 //
-void			btSoftBody::appendNote(	const char* text,
-									   const btVector3& o,
-									   Face* feature)
+void btSoftBody::appendNote(const char* text,
+							const btVector3& o,
+							Face* feature)
 {
-	static const btScalar	w=1/(btScalar)3;
-	appendNote(text,o,btVector4(w,w,w,0),	feature->m_n[0],
-		feature->m_n[1],
-		feature->m_n[2]);
+	static const btScalar w = 1 / (btScalar)3;
+	appendNote(text, o, btVector4(w, w, w, 0), feature->m_n[0],
+			   feature->m_n[1],
+			   feature->m_n[2]);
 }
 
 //
-void			btSoftBody::appendNode(	const btVector3& x,btScalar m)
+void btSoftBody::appendNode(const btVector3& x, btScalar m)
 {
-	if(m_nodes.capacity()==m_nodes.size())
+	if (m_nodes.capacity() == m_nodes.size())
 	{
 		pointersToIndices();
-		m_nodes.reserve(m_nodes.size()*2+1);
+		m_nodes.reserve(m_nodes.size() * 2 + 1);
 		indicesToPointers();
 	}
-	const btScalar	margin=getCollisionShape()->getMargin();
+	const btScalar margin = getCollisionShape()->getMargin();
 	m_nodes.push_back(Node());
-	Node&			n=m_nodes[m_nodes.size()-1];
+	Node& n = m_nodes[m_nodes.size() - 1];
 	ZeroInitialize(n);
-	n.m_x			=	x;
-	n.m_q			=	n.m_x;
-	n.m_im			=	m>0?1/m:0;
-	n.m_material	=	m_materials[0];
-	n.m_leaf		=	m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
+	n.m_x = x;
+	n.m_q = n.m_x;
+	n.m_im = m > 0 ? 1 / m : 0;
+	n.m_material = m_materials[0];
+	n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n);
 }
 
 //
-void			btSoftBody::appendLink(int model,Material* mat)
+void btSoftBody::appendLink(int model, Material* mat)
 {
-	Link	l;
-	if(model>=0)
-		l=m_links[model];
+	Link l;
+	if (model >= 0)
+		l = m_links[model];
 	else
-	{ ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
+	{
+		ZeroInitialize(l);
+		l.m_material = mat ? mat : m_materials[0];
+	}
 	m_links.push_back(l);
 }
 
 //
-void			btSoftBody::appendLink(	int node0,
-									   int node1,
-									   Material* mat,
-									   bool bcheckexist)
+void btSoftBody::appendLink(int node0,
+							int node1,
+							Material* mat,
+							bool bcheckexist)
 {
-	appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist);
+	appendLink(&m_nodes[node0], &m_nodes[node1], mat, bcheckexist);
 }
 
 //
-void			btSoftBody::appendLink(	Node* node0,
-									   Node* node1,
-									   Material* mat,
-									   bool bcheckexist)
+void btSoftBody::appendLink(Node* node0,
+							Node* node1,
+							Material* mat,
+							bool bcheckexist)
 {
-	if((!bcheckexist)||(!checkLink(node0,node1)))
+	if ((!bcheckexist) || (!checkLink(node0, node1)))
 	{
-		appendLink(-1,mat);
-		Link&	l=m_links[m_links.size()-1];
-		l.m_n[0]		=	node0;
-		l.m_n[1]		=	node1;
-		l.m_rl			=	(l.m_n[0]->m_x-l.m_n[1]->m_x).length();
-		m_bUpdateRtCst=true;
+		appendLink(-1, mat);
+		Link& l = m_links[m_links.size() - 1];
+		l.m_n[0] = node0;
+		l.m_n[1] = node1;
+		l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length();
+		m_bUpdateRtCst = true;
 	}
 }
 
 //
-void			btSoftBody::appendFace(int model,Material* mat)
+void btSoftBody::appendFace(int model, Material* mat)
 {
-	Face	f;
-	if(model>=0)
-	{ f=m_faces[model]; }
+	Face f;
+	if (model >= 0)
+	{
+		f = m_faces[model];
+	}
 	else
-	{ ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
+	{
+		ZeroInitialize(f);
+		f.m_material = mat ? mat : m_materials[0];
+	}
 	m_faces.push_back(f);
 }
 
 //
-void			btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
+void btSoftBody::appendFace(int node0, int node1, int node2, Material* mat)
 {
-	if (node0==node1)
+	if (node0 == node1)
 		return;
-	if (node1==node2)
+	if (node1 == node2)
 		return;
-	if (node2==node0)
+	if (node2 == node0)
 		return;
 
-	appendFace(-1,mat);
-	Face&	f=m_faces[m_faces.size()-1];
-	btAssert(node0!=node1);
-	btAssert(node1!=node2);
-	btAssert(node2!=node0);
-	f.m_n[0]	=	&m_nodes[node0];
-	f.m_n[1]	=	&m_nodes[node1];
-	f.m_n[2]	=	&m_nodes[node2];
-	f.m_ra		=	AreaOf(	f.m_n[0]->m_x,
-		f.m_n[1]->m_x,
-		f.m_n[2]->m_x);	
-	m_bUpdateRtCst=true;
+	appendFace(-1, mat);
+	Face& f = m_faces[m_faces.size() - 1];
+	btAssert(node0 != node1);
+	btAssert(node1 != node2);
+	btAssert(node2 != node0);
+	f.m_n[0] = &m_nodes[node0];
+	f.m_n[1] = &m_nodes[node1];
+	f.m_n[2] = &m_nodes[node2];
+	f.m_ra = AreaOf(f.m_n[0]->m_x,
+					f.m_n[1]->m_x,
+					f.m_n[2]->m_x);
+	m_bUpdateRtCst = true;
 }
 
 //
-void			btSoftBody::appendTetra(int model,Material* mat)
+void btSoftBody::appendTetra(int model, Material* mat)
 {
-Tetra	t;
-if(model>=0)
-	t=m_tetras[model];
+	Tetra t;
+	if (model >= 0)
+		t = m_tetras[model];
 	else
-	{ ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; }
-m_tetras.push_back(t);
+	{
+		ZeroInitialize(t);
+		t.m_material = mat ? mat : m_materials[0];
+	}
+	m_tetras.push_back(t);
 }
 
 //
-void			btSoftBody::appendTetra(int node0,
-										int node1,
-										int node2,
-										int node3,
-										Material* mat)
+void btSoftBody::appendTetra(int node0,
+							 int node1,
+							 int node2,
+							 int node3,
+							 Material* mat)
 {
-	appendTetra(-1,mat);
-	Tetra&	t=m_tetras[m_tetras.size()-1];
-	t.m_n[0]	=	&m_nodes[node0];
-	t.m_n[1]	=	&m_nodes[node1];
-	t.m_n[2]	=	&m_nodes[node2];
-	t.m_n[3]	=	&m_nodes[node3];
-	t.m_rv		=	VolumeOf(t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x);
-	m_bUpdateRtCst=true;
+	appendTetra(-1, mat);
+	Tetra& t = m_tetras[m_tetras.size() - 1];
+	t.m_n[0] = &m_nodes[node0];
+	t.m_n[1] = &m_nodes[node1];
+	t.m_n[2] = &m_nodes[node2];
+	t.m_n[3] = &m_nodes[node3];
+	t.m_rv = VolumeOf(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x);
+	m_bUpdateRtCst = true;
 }
 
 //
 
-void			btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies,btScalar influence)
+void btSoftBody::appendAnchor(int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies, btScalar influence)
 {
-	btVector3 local = body->getWorldTransform().inverse()*m_nodes[node].m_x;
-	appendAnchor(node,body,local,disableCollisionBetweenLinkedBodies,influence);
+	btVector3 local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
+	appendAnchor(node, body, local, disableCollisionBetweenLinkedBodies, influence);
 }
 
 //
-void			btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies,btScalar influence)
+void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies, btScalar influence)
 {
 	if (disableCollisionBetweenLinkedBodies)
 	{
-		if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size())
+		if (m_collisionDisabledObjects.findLinearSearch(body) == m_collisionDisabledObjects.size())
 		{
 			m_collisionDisabledObjects.push_back(body);
 		}
 	}
 
-	Anchor	a;
-	a.m_node			=	&m_nodes[node];
-	a.m_body			=	body;
-	a.m_local			=	localPivot;
-	a.m_node->m_battach	=	1;
+	Anchor a;
+	a.m_node = &m_nodes[node];
+	a.m_body = body;
+	a.m_local = localPivot;
+	a.m_node->m_battach = 1;
 	a.m_influence = influence;
 	m_anchors.push_back(a);
 }
 
 //
-void			btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
+void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body)
+{
+	DeformableNodeRigidAnchor c;
+	btSoftBody::Node& n = m_nodes[node];
+	const btScalar ima = n.m_im;
+	const btScalar imb = body->getInvMass();
+	btVector3 nrm;
+	const btCollisionShape* shp = body->getCollisionShape();
+	const btTransform& wtr = body->getWorldTransform();
+	btScalar dst =
+		m_worldInfo->m_sparsesdf.Evaluate(
+			wtr.invXform(m_nodes[node].m_x),
+			shp,
+			nrm,
+			0);
+
+	c.m_cti.m_colObj = body;
+	c.m_cti.m_normal = wtr.getBasis() * nrm;
+	c.m_cti.m_offset = dst;
+	c.m_node = &m_nodes[node];
+	const btScalar fc = m_cfg.kDF * body->getFriction();
+	c.m_c2 = ima;
+	c.m_c3 = fc;
+	c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
+	static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	const btMatrix3x3& iwi = body->getInvInertiaTensorWorld();
+	const btVector3 ra = n.m_x - wtr.getOrigin();
+
+	c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
+	c.m_c1 = ra;
+	c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
+	c.m_node->m_battach = 1;
+	m_deformableAnchors.push_back(c);
+}
+
+void btSoftBody::removeAnchor(int node)
+{
+	const btSoftBody::Node& n = m_nodes[node];
+	for (int i = 0; i < m_deformableAnchors.size();)
+	{
+		const DeformableNodeRigidAnchor& c = m_deformableAnchors[i];
+		if (c.m_node == &n)
+		{
+			m_deformableAnchors.removeAtIndex(i);
+		}
+		else
+		{
+			i++;
+		}
+	}
+}
+
+//
+void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link)
+{
+	DeformableNodeRigidAnchor c;
+	btSoftBody::Node& n = m_nodes[node];
+	const btScalar ima = n.m_im;
+	btVector3 nrm;
+	const btCollisionShape* shp = link->getCollisionShape();
+	const btTransform& wtr = link->getWorldTransform();
+	btScalar dst =
+		m_worldInfo->m_sparsesdf.Evaluate(
+			wtr.invXform(m_nodes[node].m_x),
+			shp,
+			nrm,
+			0);
+	c.m_cti.m_colObj = link;
+	c.m_cti.m_normal = wtr.getBasis() * nrm;
+	c.m_cti.m_offset = dst;
+	c.m_node = &m_nodes[node];
+	const btScalar fc = m_cfg.kDF * link->getFriction();
+	c.m_c2 = ima;
+	c.m_c3 = fc;
+	c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
+	btVector3 normal = c.m_cti.m_normal;
+	btVector3 t1 = generateUnitOrthogonalVector(normal);
+	btVector3 t2 = btCross(normal, t1);
+	btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+	findJacobian(link, jacobianData_normal, c.m_node->m_x, normal);
+	findJacobian(link, jacobianData_t1, c.m_node->m_x, t1);
+	findJacobian(link, jacobianData_t2, c.m_node->m_x, t2);
+
+	btScalar* J_n = &jacobianData_normal.m_jacobians[0];
+	btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
+	btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
+
+	btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+	btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+	btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+
+	btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
+					t1.getX(), t1.getY(), t1.getZ(),
+					t2.getX(), t2.getY(), t2.getZ());  // world frame to local frame
+	const int ndof = link->m_multiBody->getNumDofs() + 6;
+	btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+	c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
+	c.jacobianData_normal = jacobianData_normal;
+	c.jacobianData_t1 = jacobianData_t1;
+	c.jacobianData_t2 = jacobianData_t2;
+	c.t1 = t1;
+	c.t2 = t2;
+	const btVector3 ra = n.m_x - wtr.getOrigin();
+	c.m_c1 = ra;
+	c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x;
+	c.m_node->m_battach = 1;
+	m_deformableAnchors.push_back(c);
+}
+//
+void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1)
 {
-	LJoint*		pj	=	new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
-	pj->m_bodies[0]	=	body0;
-	pj->m_bodies[1]	=	body1;
-	pj->m_refs[0]	=	pj->m_bodies[0].xform().inverse()*specs.position;
-	pj->m_refs[1]	=	pj->m_bodies[1].xform().inverse()*specs.position;
-	pj->m_cfm		=	specs.cfm;
-	pj->m_erp		=	specs.erp;
-	pj->m_split		=	specs.split;
+	LJoint* pj = new (btAlignedAlloc(sizeof(LJoint), 16)) LJoint();
+	pj->m_bodies[0] = body0;
+	pj->m_bodies[1] = body1;
+	pj->m_refs[0] = pj->m_bodies[0].xform().inverse() * specs.position;
+	pj->m_refs[1] = pj->m_bodies[1].xform().inverse() * specs.position;
+	pj->m_cfm = specs.cfm;
+	pj->m_erp = specs.erp;
+	pj->m_split = specs.split;
 	m_joints.push_back(pj);
 }
 
 //
-void			btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
+void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Body body)
 {
-	appendLinearJoint(specs,m_clusters[0],body);
+	appendLinearJoint(specs, m_clusters[0], body);
 }
 
 //
-void			btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
+void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, btSoftBody* body)
 {
-	appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
+	appendLinearJoint(specs, m_clusters[0], body->m_clusters[0]);
 }
 
 //
-void			btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
+void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, Cluster* body0, Body body1)
 {
-	AJoint*		pj	=	new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
-	pj->m_bodies[0]	=	body0;
-	pj->m_bodies[1]	=	body1;
-	pj->m_refs[0]	=	pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
-	pj->m_refs[1]	=	pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
-	pj->m_cfm		=	specs.cfm;
-	pj->m_erp		=	specs.erp;
-	pj->m_split		=	specs.split;
-	pj->m_icontrol	=	specs.icontrol;
+	AJoint* pj = new (btAlignedAlloc(sizeof(AJoint), 16)) AJoint();
+	pj->m_bodies[0] = body0;
+	pj->m_bodies[1] = body1;
+	pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis() * specs.axis;
+	pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis() * specs.axis;
+	pj->m_cfm = specs.cfm;
+	pj->m_erp = specs.erp;
+	pj->m_split = specs.split;
+	pj->m_icontrol = specs.icontrol;
 	m_joints.push_back(pj);
 }
 
 //
-void			btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
+void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, Body body)
 {
-	appendAngularJoint(specs,m_clusters[0],body);
+	appendAngularJoint(specs, m_clusters[0], body);
 }
 
 //
-void			btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
+void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, btSoftBody* body)
 {
-	appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
+	appendAngularJoint(specs, m_clusters[0], body->m_clusters[0]);
 }
 
 //
-void			btSoftBody::addForce(const btVector3& force)
+void btSoftBody::addForce(const btVector3& force)
 {
-	for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i);
+	for (int i = 0, ni = m_nodes.size(); i < ni; ++i) addForce(force, i);
 }
 
 //
-void			btSoftBody::addForce(const btVector3& force,int node)
+void btSoftBody::addForce(const btVector3& force, int node)
 {
-	Node&	n=m_nodes[node];
-	if(n.m_im>0)
+	Node& n = m_nodes[node];
+	if (n.m_im > 0)
 	{
-		n.m_f	+=	force;
+		n.m_f += force;
 	}
 }
 
-void			btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeIndex)
+void btSoftBody::addAeroForceToNode(const btVector3& windVelocity, int nodeIndex)
 {
 	btAssert(nodeIndex >= 0 && nodeIndex < m_nodes.size());
 
@@ -462,51 +710,51 @@ void			btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeInde
 	const btScalar kDG = m_cfg.kDG;
 	//const btScalar kPR = m_cfg.kPR;
 	//const btScalar kVC = m_cfg.kVC;
-	const bool as_lift = kLF>0;
-	const bool as_drag = kDG>0;
+	const bool as_lift = kLF > 0;
+	const bool as_drag = kDG > 0;
 	const bool as_aero = as_lift || as_drag;
 	const bool as_vaero = as_aero && (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
 
 	Node& n = m_nodes[nodeIndex];
 
-	if( n.m_im>0 )
+	if (n.m_im > 0)
 	{
-		btSoftBody::sMedium	medium;
+		btSoftBody::sMedium medium;
 
 		EvaluateMedium(m_worldInfo, n.m_x, medium);
 		medium.m_velocity = windVelocity;
 		medium.m_density = m_worldInfo->air_density;
 
-		/* Aerodynamics			*/ 
-		if(as_vaero)
-		{				
-			const btVector3	rel_v = n.m_v - medium.m_velocity;					
+		/* Aerodynamics			*/
+		if (as_vaero)
+		{
+			const btVector3 rel_v = n.m_v - medium.m_velocity;
 			const btScalar rel_v_len = rel_v.length();
-			const btScalar	rel_v2 = rel_v.length2();
+			const btScalar rel_v2 = rel_v.length2();
 
-			if(rel_v2>SIMD_EPSILON)
+			if (rel_v2 > SIMD_EPSILON)
 			{
 				const btVector3 rel_v_nrm = rel_v.normalized();
-				btVector3	nrm = n.m_n;						
+				btVector3 nrm = n.m_n;
 
 				if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSidedLiftDrag)
 				{
-					nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
+					nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
 					btVector3 fDrag(0, 0, 0);
 					btVector3 fLift(0, 0, 0);
 
 					btScalar n_dot_v = nrm.dot(rel_v_nrm);
 					btScalar tri_area = 0.5f * n.m_area;
-							
+
 					fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
-							
+
 					// Check angle of attack
 					// cos(10º) = 0.98480
-					if ( 0 < n_dot_v && n_dot_v < 0.98480f)
-						fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f-n_dot_v*n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
+					if (0 < n_dot_v && n_dot_v < 0.98480f)
+						fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
 
 					// Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
-					btVector3 del_v_by_fDrag = fDrag*n.m_im*m_sst.sdt;										
+					btVector3 del_v_by_fDrag = fDrag * n.m_im * m_sst.sdt;
 					btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
 					btScalar v_len2 = n.m_v.length2();
 
@@ -514,7 +762,7 @@ void			btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeInde
 					{
 						btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
 						btScalar v_len = n.m_v.length();
-						fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len);
+						fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len);
 					}
 
 					n.m_f += fDrag;
@@ -522,84 +770,84 @@ void			btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeInde
 				}
 				else if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_Point || m_cfg.aeromodel == btSoftBody::eAeroModel::V_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided)
 				{
-					if (btSoftBody::eAeroModel::V_TwoSided)
-						nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
+					if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided)
+						nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
 
-					const btScalar dvn = btDot(rel_v,nrm);
-					/* Compute forces	*/ 
-					if(dvn>0)
+					const btScalar dvn = btDot(rel_v, nrm);
+					/* Compute forces	*/
+					if (dvn > 0)
 					{
-						btVector3		force(0,0,0);
-						const btScalar	c0	=	n.m_area * dvn * rel_v2/2;
-						const btScalar	c1	=	c0 * medium.m_density;
-						force	+=	nrm*(-c1*kLF);
-						force	+=	rel_v.normalized() * (-c1 * kDG);
+						btVector3 force(0, 0, 0);
+						const btScalar c0 = n.m_area * dvn * rel_v2 / 2;
+						const btScalar c1 = c0 * medium.m_density;
+						force += nrm * (-c1 * kLF);
+						force += rel_v.normalized() * (-c1 * kDG);
 						ApplyClampedForce(n, force, dt);
 					}
-				}	
+				}
 			}
 		}
 	}
 }
 
-void			btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceIndex)
+void btSoftBody::addAeroForceToFace(const btVector3& windVelocity, int faceIndex)
 {
 	const btScalar dt = m_sst.sdt;
 	const btScalar kLF = m_cfg.kLF;
 	const btScalar kDG = m_cfg.kDG;
-//	const btScalar kPR = m_cfg.kPR;
-//	const btScalar kVC = m_cfg.kVC;
-	const bool as_lift = kLF>0;
-	const bool as_drag = kDG>0;
+	//	const btScalar kPR = m_cfg.kPR;
+	//	const btScalar kVC = m_cfg.kVC;
+	const bool as_lift = kLF > 0;
+	const bool as_drag = kDG > 0;
 	const bool as_aero = as_lift || as_drag;
 	const bool as_faero = as_aero && (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
 
-	if(as_faero)
+	if (as_faero)
 	{
-		btSoftBody::Face&	f=m_faces[faceIndex];
+		btSoftBody::Face& f = m_faces[faceIndex];
 
-		btSoftBody::sMedium	medium;
-		
-		const btVector3	v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3;
-		const btVector3	x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3;
-		EvaluateMedium(m_worldInfo,x,medium);
+		btSoftBody::sMedium medium;
+
+		const btVector3 v = (f.m_n[0]->m_v + f.m_n[1]->m_v + f.m_n[2]->m_v) / 3;
+		const btVector3 x = (f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3;
+		EvaluateMedium(m_worldInfo, x, medium);
 		medium.m_velocity = windVelocity;
 		medium.m_density = m_worldInfo->air_density;
-		const btVector3	rel_v=v-medium.m_velocity;
+		const btVector3 rel_v = v - medium.m_velocity;
 		const btScalar rel_v_len = rel_v.length();
-		const btScalar	rel_v2=rel_v.length2();
+		const btScalar rel_v2 = rel_v.length2();
 
-		if(rel_v2>SIMD_EPSILON)
+		if (rel_v2 > SIMD_EPSILON)
 		{
 			const btVector3 rel_v_nrm = rel_v.normalized();
-			btVector3	nrm = f.m_normal;
+			btVector3 nrm = f.m_normal;
 
 			if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSidedLiftDrag)
 			{
-				nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
+				nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
 
 				btVector3 fDrag(0, 0, 0);
 				btVector3 fLift(0, 0, 0);
 
 				btScalar n_dot_v = nrm.dot(rel_v_nrm);
 				btScalar tri_area = 0.5f * f.m_ra;
-					
+
 				fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
 
 				// Check angle of attack
 				// cos(10º) = 0.98480
-				if ( 0 < n_dot_v && n_dot_v < 0.98480f)
-					fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f-n_dot_v*n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
+				if (0 < n_dot_v && n_dot_v < 0.98480f)
+					fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
 
 				fDrag /= 3;
 				fLift /= 3;
 
-				for(int j=0;j<3;++j) 
+				for (int j = 0; j < 3; ++j)
 				{
-					if (f.m_n[j]->m_im>0)
+					if (f.m_n[j]->m_im > 0)
 					{
 						// Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
-						btVector3 del_v_by_fDrag = fDrag*f.m_n[j]->m_im*m_sst.sdt;										
+						btVector3 del_v_by_fDrag = fDrag * f.m_n[j]->m_im * m_sst.sdt;
 						btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
 						btScalar v_len2 = f.m_n[j]->m_v.length2();
 
@@ -607,237 +855,305 @@ void			btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceInde
 						{
 							btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
 							btScalar v_len = f.m_n[j]->m_v.length();
-							fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len);
+							fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len);
 						}
 
-						f.m_n[j]->m_f += fDrag; 
+						f.m_n[j]->m_f += fDrag;
 						f.m_n[j]->m_f += fLift;
 					}
 				}
 			}
 			else if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided)
 			{
-				if (btSoftBody::eAeroModel::F_TwoSided)
-					nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
+				if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided)
+					nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
 
-				const btScalar	dvn=btDot(rel_v,nrm);
-				/* Compute forces	*/ 
-				if(dvn>0)
+				const btScalar dvn = btDot(rel_v, nrm);
+				/* Compute forces	*/
+				if (dvn > 0)
 				{
-					btVector3		force(0,0,0);
-					const btScalar	c0	=	f.m_ra*dvn*rel_v2;
-					const btScalar	c1	=	c0*medium.m_density;
-					force	+=	nrm*(-c1*kLF);
-					force	+=	rel_v.normalized()*(-c1*kDG);
-					force	/=	3;
-					for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt);
+					btVector3 force(0, 0, 0);
+					const btScalar c0 = f.m_ra * dvn * rel_v2;
+					const btScalar c1 = c0 * medium.m_density;
+					force += nrm * (-c1 * kLF);
+					force += rel_v.normalized() * (-c1 * kDG);
+					force /= 3;
+					for (int j = 0; j < 3; ++j) ApplyClampedForce(*f.m_n[j], force, dt);
 				}
 			}
 		}
 	}
-
 }
 
 //
-void			btSoftBody::addVelocity(const btVector3& velocity)
+void btSoftBody::addVelocity(const btVector3& velocity)
 {
-	for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i);
+	for (int i = 0, ni = m_nodes.size(); i < ni; ++i) addVelocity(velocity, i);
 }
 
-/* Set velocity for the entire body										*/ 
-void				btSoftBody::setVelocity(	const btVector3& velocity)
+/* Set velocity for the entire body										*/
+void btSoftBody::setVelocity(const btVector3& velocity)
 {
-	for(int i=0,ni=m_nodes.size();i<ni;++i) 
+	for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
 	{
-		Node&	n=m_nodes[i];
-		if(n.m_im>0)
+		Node& n = m_nodes[i];
+		if (n.m_im > 0)
 		{
-			n.m_v	=	velocity;
+			n.m_v = velocity;
+			n.m_vn = velocity;
 		}
 	}
 }
 
-
 //
-void			btSoftBody::addVelocity(const btVector3& velocity,int node)
+void btSoftBody::addVelocity(const btVector3& velocity, int node)
 {
-	Node&	n=m_nodes[node];
-	if(n.m_im>0)
+	Node& n = m_nodes[node];
+	if (n.m_im > 0)
 	{
-		n.m_v	+=	velocity;
+		n.m_v += velocity;
 	}
 }
 
 //
-void			btSoftBody::setMass(int node,btScalar mass)
+void btSoftBody::setMass(int node, btScalar mass)
 {
-	m_nodes[node].m_im=mass>0?1/mass:0;
-	m_bUpdateRtCst=true;
+	m_nodes[node].m_im = mass > 0 ? 1 / mass : 0;
+	m_bUpdateRtCst = true;
 }
 
 //
-btScalar		btSoftBody::getMass(int node) const
+btScalar btSoftBody::getMass(int node) const
 {
-	return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0);
+	return (m_nodes[node].m_im > 0 ? 1 / m_nodes[node].m_im : 0);
 }
 
 //
-btScalar		btSoftBody::getTotalMass() const
+btScalar btSoftBody::getTotalMass() const
 {
-	btScalar	mass=0;
-	for(int i=0;i<m_nodes.size();++i)
+	btScalar mass = 0;
+	for (int i = 0; i < m_nodes.size(); ++i)
 	{
-		mass+=getMass(i);
+		mass += getMass(i);
 	}
-	return(mass);
+	return (mass);
 }
 
 //
-void			btSoftBody::setTotalMass(btScalar mass,bool fromfaces)
+void btSoftBody::setTotalMass(btScalar mass, bool fromfaces)
 {
 	int i;
 
-	if(fromfaces)
+	if (fromfaces)
 	{
-
-		for(i=0;i<m_nodes.size();++i)
+		for (i = 0; i < m_nodes.size(); ++i)
 		{
-			m_nodes[i].m_im=0;
+			m_nodes[i].m_im = 0;
 		}
-		for(i=0;i<m_faces.size();++i)
+		for (i = 0; i < m_faces.size(); ++i)
 		{
-			const Face&		f=m_faces[i];
-			const btScalar	twicearea=AreaOf(	f.m_n[0]->m_x,
-				f.m_n[1]->m_x,
-				f.m_n[2]->m_x);
-			for(int j=0;j<3;++j)
+			const Face& f = m_faces[i];
+			const btScalar twicearea = AreaOf(f.m_n[0]->m_x,
+											  f.m_n[1]->m_x,
+											  f.m_n[2]->m_x);
+			for (int j = 0; j < 3; ++j)
 			{
-				f.m_n[j]->m_im+=twicearea;
+				f.m_n[j]->m_im += twicearea;
 			}
 		}
-		for( i=0;i<m_nodes.size();++i)
+		for (i = 0; i < m_nodes.size(); ++i)
 		{
-			m_nodes[i].m_im=1/m_nodes[i].m_im;
+			m_nodes[i].m_im = 1 / m_nodes[i].m_im;
 		}
 	}
-	const btScalar	tm=getTotalMass();
-	const btScalar	itm=1/tm;
-	for( i=0;i<m_nodes.size();++i)
+	const btScalar tm = getTotalMass();
+	const btScalar itm = 1 / tm;
+	for (i = 0; i < m_nodes.size(); ++i)
 	{
-		m_nodes[i].m_im/=itm*mass;
+		m_nodes[i].m_im /= itm * mass;
 	}
-	m_bUpdateRtCst=true;
+	m_bUpdateRtCst = true;
 }
 
 //
-void			btSoftBody::setTotalDensity(btScalar density)
+void btSoftBody::setTotalDensity(btScalar density)
 {
-	setTotalMass(getVolume()*density,true);
+	setTotalMass(getVolume() * density, true);
 }
 
 //
-void			btSoftBody::setVolumeMass(btScalar mass)
+void btSoftBody::setVolumeMass(btScalar mass)
 {
-btAlignedObjectArray<btScalar>	ranks;
-ranks.resize(m_nodes.size(),0);
-int i;
+	btAlignedObjectArray<btScalar> ranks;
+	ranks.resize(m_nodes.size(), 0);
+	int i;
 
-for(i=0;i<m_nodes.size();++i)
+	for (i = 0; i < m_nodes.size(); ++i)
 	{
-	m_nodes[i].m_im=0;
+		m_nodes[i].m_im = 0;
 	}
-for(i=0;i<m_tetras.size();++i)
+	for (i = 0; i < m_tetras.size(); ++i)
 	{
-	const Tetra& t=m_tetras[i];
-	for(int j=0;j<4;++j)
+		const Tetra& t = m_tetras[i];
+		for (int j = 0; j < 4; ++j)
 		{
-		t.m_n[j]->m_im+=btFabs(t.m_rv);
-		ranks[int(t.m_n[j]-&m_nodes[0])]+=1;
+			t.m_n[j]->m_im += btFabs(t.m_rv);
+			ranks[int(t.m_n[j] - &m_nodes[0])] += 1;
 		}
 	}
-for( i=0;i<m_nodes.size();++i)
+	for (i = 0; i < m_nodes.size(); ++i)
 	{
-	if(m_nodes[i].m_im>0)
+		if (m_nodes[i].m_im > 0)
 		{
-		m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im;
+			m_nodes[i].m_im = ranks[i] / m_nodes[i].m_im;
 		}
 	}
-setTotalMass(mass,false);
+	setTotalMass(mass, false);
 }
 
 //
-void			btSoftBody::setVolumeDensity(btScalar density)
+void btSoftBody::setVolumeDensity(btScalar density)
 {
-btScalar	volume=0;
-for(int i=0;i<m_tetras.size();++i)
+	btScalar volume = 0;
+	for (int i = 0; i < m_tetras.size(); ++i)
 	{
-	const Tetra& t=m_tetras[i];
-	for(int j=0;j<4;++j)
+		const Tetra& t = m_tetras[i];
+		for (int j = 0; j < 4; ++j)
 		{
-		volume+=btFabs(t.m_rv);
+			volume += btFabs(t.m_rv);
 		}
 	}
-setVolumeMass(volume*density/6);
+	setVolumeMass(volume * density / 6);
+}
+
+//
+btVector3 btSoftBody::getLinearVelocity()
+{
+	btVector3 total_momentum = btVector3(0, 0, 0);
+	for (int i = 0; i < m_nodes.size(); ++i)
+	{
+		btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0 / m_nodes[i].m_im;
+		total_momentum += mass * m_nodes[i].m_v;
+	}
+	btScalar total_mass = getTotalMass();
+	return total_mass == 0 ? total_momentum : total_momentum / total_mass;
+}
+
+//
+void btSoftBody::setLinearVelocity(const btVector3& linVel)
+{
+	btVector3 old_vel = getLinearVelocity();
+	btVector3 diff = linVel - old_vel;
+	for (int i = 0; i < m_nodes.size(); ++i)
+		m_nodes[i].m_v += diff;
+}
+
+//
+void btSoftBody::setAngularVelocity(const btVector3& angVel)
+{
+	btVector3 old_vel = getLinearVelocity();
+	btVector3 com = getCenterOfMass();
+	for (int i = 0; i < m_nodes.size(); ++i)
+	{
+		m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel;
+	}
+}
+
+//
+btTransform btSoftBody::getRigidTransform()
+{
+	btVector3 t = getCenterOfMass();
+	btMatrix3x3 S;
+	S.setZero();
+	// Get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i ||
+	// It's important to make sure that S has the correct signs.
+	// SVD is only unique up to the ordering of singular values.
+	// SVD will manipulate U and V to ensure the ordering of singular values. If all three singular
+	// vaues are negative, SVD will permute colums of U to make two of them positive.
+	for (int i = 0; i < m_nodes.size(); ++i)
+	{
+		S -= OuterProduct(m_X[i], t - m_nodes[i].m_x);
+	}
+	btVector3 sigma;
+	btMatrix3x3 U, V;
+	singularValueDecomposition(S, U, sigma, V);
+	btMatrix3x3 R = V * U.transpose();
+	btTransform trs;
+	trs.setIdentity();
+	trs.setOrigin(t);
+	trs.setBasis(R);
+	return trs;
+}
+
+//
+void btSoftBody::transformTo(const btTransform& trs)
+{
+	// get the current best rigid fit
+	btTransform current_transform = getRigidTransform();
+	// apply transform in material space
+	btTransform new_transform = trs * current_transform.inverse();
+	transform(new_transform);
 }
 
 //
-void			btSoftBody::transform(const btTransform& trs)
+void btSoftBody::transform(const btTransform& trs)
 {
-	const btScalar	margin=getCollisionShape()->getMargin();
-	ATTRIBUTE_ALIGNED16(btDbvtVolume)	vol;
-	
-	for(int i=0,ni=m_nodes.size();i<ni;++i)
+	const btScalar margin = getCollisionShape()->getMargin();
+	ATTRIBUTE_ALIGNED16(btDbvtVolume)
+	vol;
+
+	for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
 	{
-		Node&	n=m_nodes[i];
-		n.m_x=trs*n.m_x;
-		n.m_q=trs*n.m_q;
-		n.m_n=trs.getBasis()*n.m_n;
-		vol = btDbvtVolume::FromCR(n.m_x,margin);
-		
-		m_ndbvt.update(n.m_leaf,vol);
+		Node& n = m_nodes[i];
+		n.m_x = trs * n.m_x;
+		n.m_q = trs * n.m_q;
+		n.m_n = trs.getBasis() * n.m_n;
+		vol = btDbvtVolume::FromCR(n.m_x, margin);
+
+		m_ndbvt.update(n.m_leaf, vol);
 	}
 	updateNormals();
 	updateBounds();
 	updateConstants();
-	m_initialWorldTransform = trs;
 }
 
 //
-void			btSoftBody::translate(const btVector3& trs)
+void btSoftBody::translate(const btVector3& trs)
 {
-	btTransform	t;
+	btTransform t;
 	t.setIdentity();
 	t.setOrigin(trs);
 	transform(t);
 }
 
 //
-void			btSoftBody::rotate(	const btQuaternion& rot)
+void btSoftBody::rotate(const btQuaternion& rot)
 {
-	btTransform	t;
+	btTransform t;
 	t.setIdentity();
 	t.setRotation(rot);
 	transform(t);
 }
 
 //
-void			btSoftBody::scale(const btVector3& scl)
+void btSoftBody::scale(const btVector3& scl)
 {
+	const btScalar margin = getCollisionShape()->getMargin();
+	ATTRIBUTE_ALIGNED16(btDbvtVolume)
+	vol;
 
-	const btScalar	margin=getCollisionShape()->getMargin();
-	ATTRIBUTE_ALIGNED16(btDbvtVolume)	vol;
-	
-	for(int i=0,ni=m_nodes.size();i<ni;++i)
+	for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
 	{
-		Node&	n=m_nodes[i];
-		n.m_x*=scl;
-		n.m_q*=scl;
-		vol = btDbvtVolume::FromCR(n.m_x,margin);
-		m_ndbvt.update(n.m_leaf,vol);
+		Node& n = m_nodes[i];
+		n.m_x *= scl;
+		n.m_q *= scl;
+		vol = btDbvtVolume::FromCR(n.m_x, margin);
+		m_ndbvt.update(n.m_leaf, vol);
 	}
 	updateNormals();
 	updateBounds();
 	updateConstants();
+	initializeDmInverse();
 }
 
 //
@@ -849,437 +1165,432 @@ btScalar btSoftBody::getRestLengthScale()
 //
 void btSoftBody::setRestLengthScale(btScalar restLengthScale)
 {
-	for(int i=0, ni=m_links.size(); i<ni; ++i)
+	for (int i = 0, ni = m_links.size(); i < ni; ++i)
 	{
-		Link&		l=m_links[i];
-		l.m_rl	=	l.m_rl / m_restLengthScale * restLengthScale;
-		l.m_c1	=	l.m_rl*l.m_rl;
+		Link& l = m_links[i];
+		l.m_rl = l.m_rl / m_restLengthScale * restLengthScale;
+		l.m_c1 = l.m_rl * l.m_rl;
 	}
 	m_restLengthScale = restLengthScale;
-	
+
 	if (getActivationState() == ISLAND_SLEEPING)
 		activate();
 }
 
 //
-void			btSoftBody::setPose(bool bvolume,bool bframe)
+void btSoftBody::setPose(bool bvolume, bool bframe)
 {
-	m_pose.m_bvolume	=	bvolume;
-	m_pose.m_bframe		=	bframe;
-	int i,ni;
-
-	/* Weights		*/ 
-	const btScalar	omass=getTotalMass();
-	const btScalar	kmass=omass*m_nodes.size()*1000;
-	btScalar		tmass=omass;
+	m_pose.m_bvolume = bvolume;
+	m_pose.m_bframe = bframe;
+	int i, ni;
+
+	/* Weights		*/
+	const btScalar omass = getTotalMass();
+	const btScalar kmass = omass * m_nodes.size() * 1000;
+	btScalar tmass = omass;
 	m_pose.m_wgh.resize(m_nodes.size());
-	for(i=0,ni=m_nodes.size();i<ni;++i)
+	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 	{
-		if(m_nodes[i].m_im<=0) tmass+=kmass;
+		if (m_nodes[i].m_im <= 0) tmass += kmass;
 	}
-	for( i=0,ni=m_nodes.size();i<ni;++i)
+	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 	{
-		Node&	n=m_nodes[i];
-		m_pose.m_wgh[i]=	n.m_im>0					?
-			1/(m_nodes[i].m_im*tmass)	:
-		kmass/tmass;
+		Node& n = m_nodes[i];
+		m_pose.m_wgh[i] = n.m_im > 0 ? 1 / (m_nodes[i].m_im * tmass) : kmass / tmass;
 	}
-	/* Pos		*/ 
-	const btVector3	com=evaluateCom();
+	/* Pos		*/
+	const btVector3 com = evaluateCom();
 	m_pose.m_pos.resize(m_nodes.size());
-	for( i=0,ni=m_nodes.size();i<ni;++i)
+	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 	{
-		m_pose.m_pos[i]=m_nodes[i].m_x-com;
+		m_pose.m_pos[i] = m_nodes[i].m_x - com;
 	}
-	m_pose.m_volume	=	bvolume?getVolume():0;
-	m_pose.m_com	=	com;
+	m_pose.m_volume = bvolume ? getVolume() : 0;
+	m_pose.m_com = com;
 	m_pose.m_rot.setIdentity();
 	m_pose.m_scl.setIdentity();
-	/* Aqq		*/ 
-	m_pose.m_aqq[0]	=
-		m_pose.m_aqq[1]	=
-		m_pose.m_aqq[2]	=	btVector3(0,0,0);
-	for( i=0,ni=m_nodes.size();i<ni;++i)
-	{
-		const btVector3&	q=m_pose.m_pos[i];
-		const btVector3		mq=m_pose.m_wgh[i]*q;
-		m_pose.m_aqq[0]+=mq.x()*q;
-		m_pose.m_aqq[1]+=mq.y()*q;
-		m_pose.m_aqq[2]+=mq.z()*q;
-	}
-	m_pose.m_aqq=m_pose.m_aqq.inverse();
-	
+	/* Aqq		*/
+	m_pose.m_aqq[0] =
+		m_pose.m_aqq[1] =
+			m_pose.m_aqq[2] = btVector3(0, 0, 0);
+	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
+	{
+		const btVector3& q = m_pose.m_pos[i];
+		const btVector3 mq = m_pose.m_wgh[i] * q;
+		m_pose.m_aqq[0] += mq.x() * q;
+		m_pose.m_aqq[1] += mq.y() * q;
+		m_pose.m_aqq[2] += mq.z() * q;
+	}
+	m_pose.m_aqq = m_pose.m_aqq.inverse();
+
 	updateConstants();
 }
 
-void				btSoftBody::resetLinkRestLengths()
+void btSoftBody::resetLinkRestLengths()
 {
-	for(int i=0, ni=m_links.size();i<ni;++i)
+	for (int i = 0, ni = m_links.size(); i < ni; ++i)
 	{
-		Link& l =	m_links[i];
-		l.m_rl	=	(l.m_n[0]->m_x-l.m_n[1]->m_x).length();
-		l.m_c1	=	l.m_rl*l.m_rl;
+		Link& l = m_links[i];
+		l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length();
+		l.m_c1 = l.m_rl * l.m_rl;
 	}
 }
 
 //
-btScalar		btSoftBody::getVolume() const
+btScalar btSoftBody::getVolume() const
 {
-	btScalar	vol=0;
-	if(m_nodes.size()>0)
+	btScalar vol = 0;
+	if (m_nodes.size() > 0)
 	{
-		int i,ni;
+		int i, ni;
 
-		const btVector3	org=m_nodes[0].m_x;
-		for(i=0,ni=m_faces.size();i<ni;++i)
+		const btVector3 org = m_nodes[0].m_x;
+		for (i = 0, ni = m_faces.size(); i < ni; ++i)
 		{
-			const Face&	f=m_faces[i];
-			vol+=btDot(f.m_n[0]->m_x-org,btCross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org));
+			const Face& f = m_faces[i];
+			vol += btDot(f.m_n[0]->m_x - org, btCross(f.m_n[1]->m_x - org, f.m_n[2]->m_x - org));
 		}
-		vol/=(btScalar)6;
+		vol /= (btScalar)6;
 	}
-	return(vol);
+	return (vol);
 }
 
 //
-int				btSoftBody::clusterCount() const
+int btSoftBody::clusterCount() const
 {
-	return(m_clusters.size());
+	return (m_clusters.size());
 }
 
 //
-btVector3		btSoftBody::clusterCom(const Cluster* cluster)
+btVector3 btSoftBody::clusterCom(const Cluster* cluster)
 {
-	btVector3		com(0,0,0);
-	for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
+	btVector3 com(0, 0, 0);
+	for (int i = 0, ni = cluster->m_nodes.size(); i < ni; ++i)
 	{
-		com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
+		com += cluster->m_nodes[i]->m_x * cluster->m_masses[i];
 	}
-	return(com*cluster->m_imass);
+	return (com * cluster->m_imass);
 }
 
 //
-btVector3		btSoftBody::clusterCom(int cluster) const
+btVector3 btSoftBody::clusterCom(int cluster) const
 {
-	return(clusterCom(m_clusters[cluster]));
+	return (clusterCom(m_clusters[cluster]));
 }
 
 //
-btVector3		btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
+btVector3 btSoftBody::clusterVelocity(const Cluster* cluster, const btVector3& rpos)
 {
-	return(cluster->m_lv+btCross(cluster->m_av,rpos));
+	return (cluster->m_lv + btCross(cluster->m_av, rpos));
 }
 
 //
-void			btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
+void btSoftBody::clusterVImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse)
 {
-	const btVector3	li=cluster->m_imass*impulse;
-	const btVector3	ai=cluster->m_invwi*btCross(rpos,impulse);
-	cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
-	cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
+	const btVector3 li = cluster->m_imass * impulse;
+	const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse);
+	cluster->m_vimpulses[0] += li;
+	cluster->m_lv += li;
+	cluster->m_vimpulses[1] += ai;
+	cluster->m_av += ai;
 	cluster->m_nvimpulses++;
 }
 
 //
-void			btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
+void btSoftBody::clusterDImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse)
 {
-	const btVector3	li=cluster->m_imass*impulse;
-	const btVector3	ai=cluster->m_invwi*btCross(rpos,impulse);
-	cluster->m_dimpulses[0]+=li;
-	cluster->m_dimpulses[1]+=ai;
+	const btVector3 li = cluster->m_imass * impulse;
+	const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse);
+	cluster->m_dimpulses[0] += li;
+	cluster->m_dimpulses[1] += ai;
 	cluster->m_ndimpulses++;
 }
 
 //
-void			btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
+void btSoftBody::clusterImpulse(Cluster* cluster, const btVector3& rpos, const Impulse& impulse)
 {
-	if(impulse.m_asVelocity)	clusterVImpulse(cluster,rpos,impulse.m_velocity);
-	if(impulse.m_asDrift)		clusterDImpulse(cluster,rpos,impulse.m_drift);
+	if (impulse.m_asVelocity) clusterVImpulse(cluster, rpos, impulse.m_velocity);
+	if (impulse.m_asDrift) clusterDImpulse(cluster, rpos, impulse.m_drift);
 }
 
 //
-void			btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
+void btSoftBody::clusterVAImpulse(Cluster* cluster, const btVector3& impulse)
 {
-	const btVector3	ai=cluster->m_invwi*impulse;
-	cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
+	const btVector3 ai = cluster->m_invwi * impulse;
+	cluster->m_vimpulses[1] += ai;
+	cluster->m_av += ai;
 	cluster->m_nvimpulses++;
 }
 
 //
-void			btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
+void btSoftBody::clusterDAImpulse(Cluster* cluster, const btVector3& impulse)
 {
-	const btVector3	ai=cluster->m_invwi*impulse;
-	cluster->m_dimpulses[1]+=ai;
+	const btVector3 ai = cluster->m_invwi * impulse;
+	cluster->m_dimpulses[1] += ai;
 	cluster->m_ndimpulses++;
 }
 
 //
-void			btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
+void btSoftBody::clusterAImpulse(Cluster* cluster, const Impulse& impulse)
 {
-	if(impulse.m_asVelocity)	clusterVAImpulse(cluster,impulse.m_velocity);
-	if(impulse.m_asDrift)		clusterDAImpulse(cluster,impulse.m_drift);
+	if (impulse.m_asVelocity) clusterVAImpulse(cluster, impulse.m_velocity);
+	if (impulse.m_asDrift) clusterDAImpulse(cluster, impulse.m_drift);
 }
 
 //
-void			btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
+void btSoftBody::clusterDCImpulse(Cluster* cluster, const btVector3& impulse)
 {
-	cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
+	cluster->m_dimpulses[0] += impulse * cluster->m_imass;
 	cluster->m_ndimpulses++;
 }
 
 struct NodeLinks
 {
-    btAlignedObjectArray<int> m_links;
+	btAlignedObjectArray<int> m_links;
 };
 
-
-
 //
-int				btSoftBody::generateBendingConstraints(int distance,Material* mat)
+int btSoftBody::generateBendingConstraints(int distance, Material* mat)
 {
-	int i,j;
+	int i, j;
 
-	if(distance>1)
+	if (distance > 1)
 	{
-		/* Build graph	*/ 
-		const int		n=m_nodes.size();
-		const unsigned	inf=(~(unsigned)0)>>1;
-		unsigned*		adj=new unsigned[n*n];
-		
+		/* Build graph	*/
+		const int n = m_nodes.size();
+		const unsigned inf = (~(unsigned)0) >> 1;
+		unsigned* adj = new unsigned[n * n];
 
-#define IDX(_x_,_y_)	((_y_)*n+(_x_))
-		for(j=0;j<n;++j)
+#define IDX(_x_, _y_) ((_y_)*n + (_x_))
+		for (j = 0; j < n; ++j)
 		{
-			for(i=0;i<n;++i)
+			for (i = 0; i < n; ++i)
 			{
-				if(i!=j)
+				if (i != j)
 				{
-					adj[IDX(i,j)]=adj[IDX(j,i)]=inf;
+					adj[IDX(i, j)] = adj[IDX(j, i)] = inf;
 				}
 				else
 				{
-					adj[IDX(i,j)]=adj[IDX(j,i)]=0;
+					adj[IDX(i, j)] = adj[IDX(j, i)] = 0;
 				}
 			}
 		}
-		for( i=0;i<m_links.size();++i)
+		for (i = 0; i < m_links.size(); ++i)
 		{
-			const int	ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
-			const int	ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
-			adj[IDX(ia,ib)]=1;
-			adj[IDX(ib,ia)]=1;
+			const int ia = (int)(m_links[i].m_n[0] - &m_nodes[0]);
+			const int ib = (int)(m_links[i].m_n[1] - &m_nodes[0]);
+			adj[IDX(ia, ib)] = 1;
+			adj[IDX(ib, ia)] = 1;
 		}
 
-
 		//special optimized case for distance == 2
 		if (distance == 2)
 		{
-
 			btAlignedObjectArray<NodeLinks> nodeLinks;
 
-
 			/* Build node links */
 			nodeLinks.resize(m_nodes.size());
 
-			for( i=0;i<m_links.size();++i)
+			for (i = 0; i < m_links.size(); ++i)
 			{
-				const int	ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
-				const int	ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
-				if (nodeLinks[ia].m_links.findLinearSearch(ib)==nodeLinks[ia].m_links.size())
+				const int ia = (int)(m_links[i].m_n[0] - &m_nodes[0]);
+				const int ib = (int)(m_links[i].m_n[1] - &m_nodes[0]);
+				if (nodeLinks[ia].m_links.findLinearSearch(ib) == nodeLinks[ia].m_links.size())
 					nodeLinks[ia].m_links.push_back(ib);
 
-				if (nodeLinks[ib].m_links.findLinearSearch(ia)==nodeLinks[ib].m_links.size())
+				if (nodeLinks[ib].m_links.findLinearSearch(ia) == nodeLinks[ib].m_links.size())
 					nodeLinks[ib].m_links.push_back(ia);
 			}
-			for (int ii=0;ii<nodeLinks.size();ii++)
+			for (int ii = 0; ii < nodeLinks.size(); ii++)
 			{
-				int i=ii;
+				int i = ii;
 
-				for (int jj=0;jj<nodeLinks[ii].m_links.size();jj++)
+				for (int jj = 0; jj < nodeLinks[ii].m_links.size(); jj++)
 				{
 					int k = nodeLinks[ii].m_links[jj];
-					for (int kk=0;kk<nodeLinks[k].m_links.size();kk++)
+					for (int kk = 0; kk < nodeLinks[k].m_links.size(); kk++)
 					{
 						int j = nodeLinks[k].m_links[kk];
-						if (i!=j)
+						if (i != j)
 						{
-							const unsigned	sum=adj[IDX(i,k)]+adj[IDX(k,j)];
-							btAssert(sum==2);
-							if(adj[IDX(i,j)]>sum)
+							const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)];
+							btAssert(sum == 2);
+							if (adj[IDX(i, j)] > sum)
 							{
-								adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
+								adj[IDX(i, j)] = adj[IDX(j, i)] = sum;
 							}
 						}
-
 					}
 				}
 			}
-		} else
+		}
+		else
 		{
 			///generic Floyd's algorithm
-			for(int k=0;k<n;++k)
+			for (int k = 0; k < n; ++k)
 			{
-				for(j=0;j<n;++j)
+				for (j = 0; j < n; ++j)
 				{
-					for(i=j+1;i<n;++i)
+					for (i = j + 1; i < n; ++i)
 					{
-						const unsigned	sum=adj[IDX(i,k)]+adj[IDX(k,j)];
-						if(adj[IDX(i,j)]>sum)
+						const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)];
+						if (adj[IDX(i, j)] > sum)
 						{
-							adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
+							adj[IDX(i, j)] = adj[IDX(j, i)] = sum;
 						}
 					}
 				}
 			}
 		}
 
-
-		/* Build links	*/ 
-		int	nlinks=0;
-		for(j=0;j<n;++j)
+		/* Build links	*/
+		int nlinks = 0;
+		for (j = 0; j < n; ++j)
 		{
-			for(i=j+1;i<n;++i)
+			for (i = j + 1; i < n; ++i)
 			{
-				if(adj[IDX(i,j)]==(unsigned)distance)
+				if (adj[IDX(i, j)] == (unsigned)distance)
 				{
-					appendLink(i,j,mat);
-					m_links[m_links.size()-1].m_bbending=1;
+					appendLink(i, j, mat);
+					m_links[m_links.size() - 1].m_bbending = 1;
 					++nlinks;
 				}
 			}
 		}
-		delete[] adj;		
-		return(nlinks);
+		delete[] adj;
+		return (nlinks);
 	}
-	return(0);
+	return (0);
 }
 
 //
-void			btSoftBody::randomizeConstraints()
+void btSoftBody::randomizeConstraints()
 {
-	unsigned long	seed=243703;
-#define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
-	int i,ni;
+	unsigned long seed = 243703;
+#define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff)
+	int i, ni;
 
-	for(i=0,ni=m_links.size();i<ni;++i)
+	for (i = 0, ni = m_links.size(); i < ni; ++i)
 	{
-		btSwap(m_links[i],m_links[NEXTRAND%ni]);
+		btSwap(m_links[i], m_links[NEXTRAND % ni]);
 	}
-	for(i=0,ni=m_faces.size();i<ni;++i)
+	for (i = 0, ni = m_faces.size(); i < ni; ++i)
 	{
-		btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
+		btSwap(m_faces[i], m_faces[NEXTRAND % ni]);
 	}
 #undef NEXTRAND
 }
 
 //
-void			btSoftBody::releaseCluster(int index)
+void btSoftBody::releaseCluster(int index)
 {
-	Cluster*	c=m_clusters[index];
-	if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
+	Cluster* c = m_clusters[index];
+	if (c->m_leaf) m_cdbvt.remove(c->m_leaf);
 	c->~Cluster();
 	btAlignedFree(c);
 	m_clusters.remove(c);
 }
 
 //
-void			btSoftBody::releaseClusters()
+void btSoftBody::releaseClusters()
 {
-	while(m_clusters.size()>0) releaseCluster(0);
+	while (m_clusters.size() > 0) releaseCluster(0);
 }
 
 //
-int				btSoftBody::generateClusters(int k,int maxiterations)
+int btSoftBody::generateClusters(int k, int maxiterations)
 {
 	int i;
 	releaseClusters();
-	m_clusters.resize(btMin(k,m_nodes.size()));
-	for(i=0;i<m_clusters.size();++i)
-	{
-		m_clusters[i]			=	new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
-		m_clusters[i]->m_collide=	true;
-	}
-	k=m_clusters.size();
-	if(k>0)
-	{
-		/* Initialize		*/ 
-		btAlignedObjectArray<btVector3>	centers;
-		btVector3						cog(0,0,0);
-		int								i;
-		for(i=0;i<m_nodes.size();++i)
-		{
-			cog+=m_nodes[i].m_x;
-			m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
-		}
-		cog/=(btScalar)m_nodes.size();
-		centers.resize(k,cog);
-		/* Iterate			*/ 
-		const btScalar	slope=16;
-		bool			changed;
-		int				iterations=0;
-		do	{
-			const btScalar	w=2-btMin<btScalar>(1,iterations/slope);
-			changed=false;
-			iterations++;	
+	m_clusters.resize(btMin(k, m_nodes.size()));
+	for (i = 0; i < m_clusters.size(); ++i)
+	{
+		m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
+		m_clusters[i]->m_collide = true;
+	}
+	k = m_clusters.size();
+	if (k > 0)
+	{
+		/* Initialize		*/
+		btAlignedObjectArray<btVector3> centers;
+		btVector3 cog(0, 0, 0);
+		int i;
+		for (i = 0; i < m_nodes.size(); ++i)
+		{
+			cog += m_nodes[i].m_x;
+			m_clusters[(i * 29873) % m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
+		}
+		cog /= (btScalar)m_nodes.size();
+		centers.resize(k, cog);
+		/* Iterate			*/
+		const btScalar slope = 16;
+		bool changed;
+		int iterations = 0;
+		do
+		{
+			const btScalar w = 2 - btMin<btScalar>(1, iterations / slope);
+			changed = false;
+			iterations++;
 			int i;
 
-			for(i=0;i<k;++i)
+			for (i = 0; i < k; ++i)
 			{
-				btVector3	c(0,0,0);
-				for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
+				btVector3 c(0, 0, 0);
+				for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j)
 				{
-					c+=m_clusters[i]->m_nodes[j]->m_x;
+					c += m_clusters[i]->m_nodes[j]->m_x;
 				}
-				if(m_clusters[i]->m_nodes.size())
+				if (m_clusters[i]->m_nodes.size())
 				{
-					c			/=	(btScalar)m_clusters[i]->m_nodes.size();
-					c			=	centers[i]+(c-centers[i])*w;
-					changed		|=	((c-centers[i]).length2()>SIMD_EPSILON);
-					centers[i]	=	c;
+					c /= (btScalar)m_clusters[i]->m_nodes.size();
+					c = centers[i] + (c - centers[i]) * w;
+					changed |= ((c - centers[i]).length2() > SIMD_EPSILON);
+					centers[i] = c;
 					m_clusters[i]->m_nodes.resize(0);
-				}			
+				}
 			}
-			for(i=0;i<m_nodes.size();++i)
+			for (i = 0; i < m_nodes.size(); ++i)
 			{
-				const btVector3	nx=m_nodes[i].m_x;
-				int				kbest=0;
-				btScalar		kdist=ClusterMetric(centers[0],nx);
-				for(int j=1;j<k;++j)
+				const btVector3 nx = m_nodes[i].m_x;
+				int kbest = 0;
+				btScalar kdist = ClusterMetric(centers[0], nx);
+				for (int j = 1; j < k; ++j)
 				{
-					const btScalar	d=ClusterMetric(centers[j],nx);
-					if(d<kdist)
+					const btScalar d = ClusterMetric(centers[j], nx);
+					if (d < kdist)
 					{
-						kbest=j;
-						kdist=d;
+						kbest = j;
+						kdist = d;
 					}
 				}
 				m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
-			}		
-		} while(changed&&(iterations<maxiterations));
-		/* Merge		*/ 
-		btAlignedObjectArray<int>	cids;
-		cids.resize(m_nodes.size(),-1);
-		for(i=0;i<m_clusters.size();++i)
+			}
+		} while (changed && (iterations < maxiterations));
+		/* Merge		*/
+		btAlignedObjectArray<int> cids;
+		cids.resize(m_nodes.size(), -1);
+		for (i = 0; i < m_clusters.size(); ++i)
 		{
-			for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
+			for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j)
 			{
-				cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
+				cids[int(m_clusters[i]->m_nodes[j] - &m_nodes[0])] = i;
 			}
 		}
-		for(i=0;i<m_faces.size();++i)
+		for (i = 0; i < m_faces.size(); ++i)
 		{
-			const int idx[]={	int(m_faces[i].m_n[0]-&m_nodes[0]),
-				int(m_faces[i].m_n[1]-&m_nodes[0]),
-				int(m_faces[i].m_n[2]-&m_nodes[0])};
-			for(int j=0;j<3;++j)
+			const int idx[] = {int(m_faces[i].m_n[0] - &m_nodes[0]),
+							   int(m_faces[i].m_n[1] - &m_nodes[0]),
+							   int(m_faces[i].m_n[2] - &m_nodes[0])};
+			for (int j = 0; j < 3; ++j)
 			{
-				const int cid=cids[idx[j]];
-				for(int q=1;q<3;++q)
+				const int cid = cids[idx[j]];
+				for (int q = 1; q < 3; ++q)
 				{
-					const int kid=idx[(j+q)%3];
-					if(cids[kid]!=cid)
+					const int kid = idx[(j + q) % 3];
+					if (cids[kid] != cid)
 					{
-						if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
+						if (m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid]) == m_clusters[cid]->m_nodes.size())
 						{
 							m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
 						}
@@ -1287,55 +1598,56 @@ int				btSoftBody::generateClusters(int k,int maxiterations)
 				}
 			}
 		}
-		/* Master		*/ 
-		if(m_clusters.size()>1)
+		/* Master		*/
+		if (m_clusters.size() > 1)
 		{
-			Cluster*	pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
-			pmaster->m_collide	=	false;
+			Cluster* pmaster = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
+			pmaster->m_collide = false;
 			pmaster->m_nodes.reserve(m_nodes.size());
-			for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
+			for (int i = 0; i < m_nodes.size(); ++i) pmaster->m_nodes.push_back(&m_nodes[i]);
 			m_clusters.push_back(pmaster);
-			btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
+			btSwap(m_clusters[0], m_clusters[m_clusters.size() - 1]);
 		}
-		/* Terminate	*/ 
-		for(i=0;i<m_clusters.size();++i)
+		/* Terminate	*/
+		for (i = 0; i < m_clusters.size(); ++i)
 		{
-			if(m_clusters[i]->m_nodes.size()==0)
+			if (m_clusters[i]->m_nodes.size() == 0)
 			{
 				releaseCluster(i--);
 			}
 		}
-	} else
+	}
+	else
 	{
 		//create a cluster for each tetrahedron (if tetrahedra exist) or each face
 		if (m_tetras.size())
 		{
 			m_clusters.resize(m_tetras.size());
-			for(i=0;i<m_clusters.size();++i)
+			for (i = 0; i < m_clusters.size(); ++i)
 			{
-				m_clusters[i]			=	new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
-				m_clusters[i]->m_collide=	true;
+				m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
+				m_clusters[i]->m_collide = true;
 			}
-			for (i=0;i<m_tetras.size();i++)
+			for (i = 0; i < m_tetras.size(); i++)
 			{
-				for (int j=0;j<4;j++)
+				for (int j = 0; j < 4; j++)
 				{
 					m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]);
 				}
 			}
-
-		} else
+		}
+		else
 		{
 			m_clusters.resize(m_faces.size());
-			for(i=0;i<m_clusters.size();++i)
+			for (i = 0; i < m_clusters.size(); ++i)
 			{
-				m_clusters[i]			=	new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
-				m_clusters[i]->m_collide=	true;
+				m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
+				m_clusters[i]->m_collide = true;
 			}
 
-			for(i=0;i<m_faces.size();++i)
+			for (i = 0; i < m_faces.size(); ++i)
 			{
-				for(int j=0;j<3;++j)
+				for (int j = 0; j < 3; ++j)
 				{
 					m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]);
 				}
@@ -1348,261 +1660,272 @@ int				btSoftBody::generateClusters(int k,int maxiterations)
 		initializeClusters();
 		updateClusters();
 
-
 		//for self-collision
-		m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size());
+		m_clusterConnectivity.resize(m_clusters.size() * m_clusters.size());
 		{
-			for (int c0=0;c0<m_clusters.size();c0++)
+			for (int c0 = 0; c0 < m_clusters.size(); c0++)
 			{
-				m_clusters[c0]->m_clusterIndex=c0;
-				for (int c1=0;c1<m_clusters.size();c1++)
+				m_clusters[c0]->m_clusterIndex = c0;
+				for (int c1 = 0; c1 < m_clusters.size(); c1++)
 				{
-					
-					bool connected=false;
+					bool connected = false;
 					Cluster* cla = m_clusters[c0];
 					Cluster* clb = m_clusters[c1];
-					for (int i=0;!connected&&i<cla->m_nodes.size();i++)
+					for (int i = 0; !connected && i < cla->m_nodes.size(); i++)
 					{
-						for (int j=0;j<clb->m_nodes.size();j++)
+						for (int j = 0; j < clb->m_nodes.size(); j++)
 						{
 							if (cla->m_nodes[i] == clb->m_nodes[j])
 							{
-								connected=true;
+								connected = true;
 								break;
 							}
 						}
 					}
-					m_clusterConnectivity[c0+c1*m_clusters.size()]=connected;
+					m_clusterConnectivity[c0 + c1 * m_clusters.size()] = connected;
 				}
 			}
 		}
 	}
 
-	return(m_clusters.size());
+	return (m_clusters.size());
 }
 
 //
-void			btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
+void btSoftBody::refine(ImplicitFn* ifn, btScalar accurary, bool cut)
 {
-	const Node*			nbase = &m_nodes[0];
-	int					ncount = m_nodes.size();
-	btSymMatrix<int>	edges(ncount,-2);
-	int					newnodes=0;
-	int i,j,k,ni;
-
-	/* Filter out		*/ 
-	for(i=0;i<m_links.size();++i)
+	const Node* nbase = &m_nodes[0];
+	int ncount = m_nodes.size();
+	btSymMatrix<int> edges(ncount, -2);
+	int newnodes = 0;
+	int i, j, k, ni;
+
+	/* Filter out		*/
+	for (i = 0; i < m_links.size(); ++i)
 	{
-		Link&	l=m_links[i];
-		if(l.m_bbending)
+		Link& l = m_links[i];
+		if (l.m_bbending)
 		{
-			if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
+			if (!SameSign(ifn->Eval(l.m_n[0]->m_x), ifn->Eval(l.m_n[1]->m_x)))
 			{
-				btSwap(m_links[i],m_links[m_links.size()-1]);
-				m_links.pop_back();--i;
+				btSwap(m_links[i], m_links[m_links.size() - 1]);
+				m_links.pop_back();
+				--i;
 			}
-		}	
+		}
 	}
-	/* Fill edges		*/ 
-	for(i=0;i<m_links.size();++i)
+	/* Fill edges		*/
+	for (i = 0; i < m_links.size(); ++i)
 	{
-		Link&	l=m_links[i];
-		edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
+		Link& l = m_links[i];
+		edges(int(l.m_n[0] - nbase), int(l.m_n[1] - nbase)) = -1;
 	}
-	for(i=0;i<m_faces.size();++i)
-	{	
-		Face&	f=m_faces[i];
-		edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
-		edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
-		edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
+	for (i = 0; i < m_faces.size(); ++i)
+	{
+		Face& f = m_faces[i];
+		edges(int(f.m_n[0] - nbase), int(f.m_n[1] - nbase)) = -1;
+		edges(int(f.m_n[1] - nbase), int(f.m_n[2] - nbase)) = -1;
+		edges(int(f.m_n[2] - nbase), int(f.m_n[0] - nbase)) = -1;
 	}
-	/* Intersect		*/ 
-	for(i=0;i<ncount;++i)
+	/* Intersect		*/
+	for (i = 0; i < ncount; ++i)
 	{
-		for(j=i+1;j<ncount;++j)
+		for (j = i + 1; j < ncount; ++j)
 		{
-			if(edges(i,j)==-1)
+			if (edges(i, j) == -1)
 			{
-				Node&			a=m_nodes[i];
-				Node&			b=m_nodes[j];
-				const btScalar	t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
-				if(t>0)
+				Node& a = m_nodes[i];
+				Node& b = m_nodes[j];
+				const btScalar t = ImplicitSolve(ifn, a.m_x, b.m_x, accurary);
+				if (t > 0)
 				{
-					const btVector3	x=Lerp(a.m_x,b.m_x,t);
-					const btVector3	v=Lerp(a.m_v,b.m_v,t);
-					btScalar		m=0;
-					if(a.m_im>0)
+					const btVector3 x = Lerp(a.m_x, b.m_x, t);
+					const btVector3 v = Lerp(a.m_v, b.m_v, t);
+					btScalar m = 0;
+					if (a.m_im > 0)
 					{
-						if(b.m_im>0)
+						if (b.m_im > 0)
 						{
-							const btScalar	ma=1/a.m_im;
-							const btScalar	mb=1/b.m_im;
-							const btScalar	mc=Lerp(ma,mb,t);
-							const btScalar	f=(ma+mb)/(ma+mb+mc);
-							a.m_im=1/(ma*f);
-							b.m_im=1/(mb*f);
-							m=mc*f;
+							const btScalar ma = 1 / a.m_im;
+							const btScalar mb = 1 / b.m_im;
+							const btScalar mc = Lerp(ma, mb, t);
+							const btScalar f = (ma + mb) / (ma + mb + mc);
+							a.m_im = 1 / (ma * f);
+							b.m_im = 1 / (mb * f);
+							m = mc * f;
 						}
 						else
-						{ a.m_im/=0.5f;m=1/a.m_im; }
+						{
+							a.m_im /= 0.5f;
+							m = 1 / a.m_im;
+						}
 					}
 					else
 					{
-						if(b.m_im>0)
-						{ b.m_im/=0.5f;m=1/b.m_im; }
+						if (b.m_im > 0)
+						{
+							b.m_im /= 0.5f;
+							m = 1 / b.m_im;
+						}
 						else
-							m=0;
+							m = 0;
 					}
-					appendNode(x,m);
-					edges(i,j)=m_nodes.size()-1;
-					m_nodes[edges(i,j)].m_v=v;
+					appendNode(x, m);
+					edges(i, j) = m_nodes.size() - 1;
+					m_nodes[edges(i, j)].m_v = v;
 					++newnodes;
 				}
 			}
 		}
 	}
-	nbase=&m_nodes[0];
-	/* Refine links		*/ 
-	for(i=0,ni=m_links.size();i<ni;++i)
+	nbase = &m_nodes[0];
+	/* Refine links		*/
+	for (i = 0, ni = m_links.size(); i < ni; ++i)
 	{
-		Link&		feat=m_links[i];
-		const int	idx[]={	int(feat.m_n[0]-nbase),
-			int(feat.m_n[1]-nbase)};
-		if((idx[0]<ncount)&&(idx[1]<ncount))
+		Link& feat = m_links[i];
+		const int idx[] = {int(feat.m_n[0] - nbase),
+						   int(feat.m_n[1] - nbase)};
+		if ((idx[0] < ncount) && (idx[1] < ncount))
 		{
-			const int ni=edges(idx[0],idx[1]);
-			if(ni>0)
+			const int ni = edges(idx[0], idx[1]);
+			if (ni > 0)
 			{
 				appendLink(i);
-				Link*		pft[]={	&m_links[i],
-					&m_links[m_links.size()-1]};			
-				pft[0]->m_n[0]=&m_nodes[idx[0]];
-				pft[0]->m_n[1]=&m_nodes[ni];
-				pft[1]->m_n[0]=&m_nodes[ni];
-				pft[1]->m_n[1]=&m_nodes[idx[1]];
+				Link* pft[] = {&m_links[i],
+							   &m_links[m_links.size() - 1]};
+				pft[0]->m_n[0] = &m_nodes[idx[0]];
+				pft[0]->m_n[1] = &m_nodes[ni];
+				pft[1]->m_n[0] = &m_nodes[ni];
+				pft[1]->m_n[1] = &m_nodes[idx[1]];
 			}
 		}
 	}
-	/* Refine faces		*/ 
-	for(i=0;i<m_faces.size();++i)
+	/* Refine faces		*/
+	for (i = 0; i < m_faces.size(); ++i)
 	{
-		const Face&	feat=m_faces[i];
-		const int	idx[]={	int(feat.m_n[0]-nbase),
-			int(feat.m_n[1]-nbase),
-			int(feat.m_n[2]-nbase)};
-		for(j=2,k=0;k<3;j=k++)
+		const Face& feat = m_faces[i];
+		const int idx[] = {int(feat.m_n[0] - nbase),
+						   int(feat.m_n[1] - nbase),
+						   int(feat.m_n[2] - nbase)};
+		for (j = 2, k = 0; k < 3; j = k++)
 		{
-			if((idx[j]<ncount)&&(idx[k]<ncount))
+			if ((idx[j] < ncount) && (idx[k] < ncount))
 			{
-				const int ni=edges(idx[j],idx[k]);
-				if(ni>0)
+				const int ni = edges(idx[j], idx[k]);
+				if (ni > 0)
 				{
 					appendFace(i);
-					const int	l=(k+1)%3;
-					Face*		pft[]={	&m_faces[i],
-						&m_faces[m_faces.size()-1]};
-					pft[0]->m_n[0]=&m_nodes[idx[l]];
-					pft[0]->m_n[1]=&m_nodes[idx[j]];
-					pft[0]->m_n[2]=&m_nodes[ni];
-					pft[1]->m_n[0]=&m_nodes[ni];
-					pft[1]->m_n[1]=&m_nodes[idx[k]];
-					pft[1]->m_n[2]=&m_nodes[idx[l]];
-					appendLink(ni,idx[l],pft[0]->m_material);
-					--i;break;
+					const int l = (k + 1) % 3;
+					Face* pft[] = {&m_faces[i],
+								   &m_faces[m_faces.size() - 1]};
+					pft[0]->m_n[0] = &m_nodes[idx[l]];
+					pft[0]->m_n[1] = &m_nodes[idx[j]];
+					pft[0]->m_n[2] = &m_nodes[ni];
+					pft[1]->m_n[0] = &m_nodes[ni];
+					pft[1]->m_n[1] = &m_nodes[idx[k]];
+					pft[1]->m_n[2] = &m_nodes[idx[l]];
+					appendLink(ni, idx[l], pft[0]->m_material);
+					--i;
+					break;
 				}
 			}
 		}
 	}
-	/* Cut				*/ 
-	if(cut)
-	{	
-		btAlignedObjectArray<int>	cnodes;
-		const int					pcount=ncount;
-		int							i;
-		ncount=m_nodes.size();
-		cnodes.resize(ncount,0);
-		/* Nodes		*/ 
-		for(i=0;i<ncount;++i)
+	/* Cut				*/
+	if (cut)
+	{
+		btAlignedObjectArray<int> cnodes;
+		const int pcount = ncount;
+		int i;
+		ncount = m_nodes.size();
+		cnodes.resize(ncount, 0);
+		/* Nodes		*/
+		for (i = 0; i < ncount; ++i)
 		{
-			const btVector3	x=m_nodes[i].m_x;
-			if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
+			const btVector3 x = m_nodes[i].m_x;
+			if ((i >= pcount) || (btFabs(ifn->Eval(x)) < accurary))
 			{
-				const btVector3	v=m_nodes[i].m_v;
-				btScalar		m=getMass(i);
-				if(m>0) { m*=0.5f;m_nodes[i].m_im/=0.5f; }
-				appendNode(x,m);
-				cnodes[i]=m_nodes.size()-1;
-				m_nodes[cnodes[i]].m_v=v;
+				const btVector3 v = m_nodes[i].m_v;
+				btScalar m = getMass(i);
+				if (m > 0)
+				{
+					m *= 0.5f;
+					m_nodes[i].m_im /= 0.5f;
+				}
+				appendNode(x, m);
+				cnodes[i] = m_nodes.size() - 1;
+				m_nodes[cnodes[i]].m_v = v;
 			}
 		}
-		nbase=&m_nodes[0];
-		/* Links		*/ 
-		for(i=0,ni=m_links.size();i<ni;++i)
+		nbase = &m_nodes[0];
+		/* Links		*/
+		for (i = 0, ni = m_links.size(); i < ni; ++i)
 		{
-			const int		id[]={	int(m_links[i].m_n[0]-nbase),
-				int(m_links[i].m_n[1]-nbase)};
-			int				todetach=0;
-			if(cnodes[id[0]]&&cnodes[id[1]])
+			const int id[] = {int(m_links[i].m_n[0] - nbase),
+							  int(m_links[i].m_n[1] - nbase)};
+			int todetach = 0;
+			if (cnodes[id[0]] && cnodes[id[1]])
 			{
 				appendLink(i);
-				todetach=m_links.size()-1;
+				todetach = m_links.size() - 1;
 			}
 			else
 			{
-				if((	(ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
-					(ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
-					todetach=i;
+				if (((ifn->Eval(m_nodes[id[0]].m_x) < accurary) &&
+					 (ifn->Eval(m_nodes[id[1]].m_x) < accurary)))
+					todetach = i;
 			}
-			if(todetach)
+			if (todetach)
 			{
-				Link&	l=m_links[todetach];
-				for(int j=0;j<2;++j)
+				Link& l = m_links[todetach];
+				for (int j = 0; j < 2; ++j)
 				{
-					int cn=cnodes[int(l.m_n[j]-nbase)];
-					if(cn) l.m_n[j]=&m_nodes[cn];
-				}			
+					int cn = cnodes[int(l.m_n[j] - nbase)];
+					if (cn) l.m_n[j] = &m_nodes[cn];
+				}
 			}
 		}
-		/* Faces		*/ 
-		for(i=0,ni=m_faces.size();i<ni;++i)
+		/* Faces		*/
+		for (i = 0, ni = m_faces.size(); i < ni; ++i)
 		{
-			Node**			n=	m_faces[i].m_n;
-			if(	(ifn->Eval(n[0]->m_x)<accurary)&&
-				(ifn->Eval(n[1]->m_x)<accurary)&&
-				(ifn->Eval(n[2]->m_x)<accurary))
+			Node** n = m_faces[i].m_n;
+			if ((ifn->Eval(n[0]->m_x) < accurary) &&
+				(ifn->Eval(n[1]->m_x) < accurary) &&
+				(ifn->Eval(n[2]->m_x) < accurary))
 			{
-				for(int j=0;j<3;++j)
+				for (int j = 0; j < 3; ++j)
 				{
-					int cn=cnodes[int(n[j]-nbase)];
-					if(cn) n[j]=&m_nodes[cn];
+					int cn = cnodes[int(n[j] - nbase)];
+					if (cn) n[j] = &m_nodes[cn];
 				}
 			}
 		}
-		/* Clean orphans	*/ 
-		int							nnodes=m_nodes.size();
-		btAlignedObjectArray<int>	ranks;
-		btAlignedObjectArray<int>	todelete;
-		ranks.resize(nnodes,0);
-		for(i=0,ni=m_links.size();i<ni;++i)
+		/* Clean orphans	*/
+		int nnodes = m_nodes.size();
+		btAlignedObjectArray<int> ranks;
+		btAlignedObjectArray<int> todelete;
+		ranks.resize(nnodes, 0);
+		for (i = 0, ni = m_links.size(); i < ni; ++i)
 		{
-			for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
+			for (int j = 0; j < 2; ++j) ranks[int(m_links[i].m_n[j] - nbase)]++;
 		}
-		for(i=0,ni=m_faces.size();i<ni;++i)
+		for (i = 0, ni = m_faces.size(); i < ni; ++i)
 		{
-			for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
+			for (int j = 0; j < 3; ++j) ranks[int(m_faces[i].m_n[j] - nbase)]++;
 		}
-		for(i=0;i<m_links.size();++i)
+		for (i = 0; i < m_links.size(); ++i)
 		{
-			const int	id[]={	int(m_links[i].m_n[0]-nbase),
-				int(m_links[i].m_n[1]-nbase)};
-			const bool	sg[]={	ranks[id[0]]==1,
-				ranks[id[1]]==1};
-			if(sg[0]||sg[1])
+			const int id[] = {int(m_links[i].m_n[0] - nbase),
+							  int(m_links[i].m_n[1] - nbase)};
+			const bool sg[] = {ranks[id[0]] == 1,
+							   ranks[id[1]] == 1};
+			if (sg[0] || sg[1])
 			{
 				--ranks[id[0]];
 				--ranks[id[1]];
-				btSwap(m_links[i],m_links[m_links.size()-1]);
-				m_links.pop_back();--i;
+				btSwap(m_links[i], m_links[m_links.size() - 1]);
+				m_links.pop_back();
+				--i;
 			}
 		}
 #if 0	
@@ -1629,679 +1952,999 @@ void			btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
 		}
 #endif
 	}
-	m_bUpdateRtCst=true;
+	m_bUpdateRtCst = true;
 }
 
 //
-bool			btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
+bool btSoftBody::cutLink(const Node* node0, const Node* node1, btScalar position)
 {
-	return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
+	return (cutLink(int(node0 - &m_nodes[0]), int(node1 - &m_nodes[0]), position));
 }
 
 //
-bool			btSoftBody::cutLink(int node0,int node1,btScalar position)
+bool btSoftBody::cutLink(int node0, int node1, btScalar position)
 {
-	bool			done=false;
-	int i,ni;
-//	const btVector3	d=m_nodes[node0].m_x-m_nodes[node1].m_x;
-	const btVector3	x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
-	const btVector3	v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
-	const btScalar	m=1;
-	appendNode(x,m);
-	appendNode(x,m);
-	Node*			pa=&m_nodes[node0];
-	Node*			pb=&m_nodes[node1];
-	Node*			pn[2]={	&m_nodes[m_nodes.size()-2],
-		&m_nodes[m_nodes.size()-1]};
-	pn[0]->m_v=v;
-	pn[1]->m_v=v;
-	for(i=0,ni=m_links.size();i<ni;++i)
+	bool done = false;
+	int i, ni;
+	//	const btVector3	d=m_nodes[node0].m_x-m_nodes[node1].m_x;
+	const btVector3 x = Lerp(m_nodes[node0].m_x, m_nodes[node1].m_x, position);
+	const btVector3 v = Lerp(m_nodes[node0].m_v, m_nodes[node1].m_v, position);
+	const btScalar m = 1;
+	appendNode(x, m);
+	appendNode(x, m);
+	Node* pa = &m_nodes[node0];
+	Node* pb = &m_nodes[node1];
+	Node* pn[2] = {&m_nodes[m_nodes.size() - 2],
+				   &m_nodes[m_nodes.size() - 1]};
+	pn[0]->m_v = v;
+	pn[1]->m_v = v;
+	for (i = 0, ni = m_links.size(); i < ni; ++i)
 	{
-		const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
-		if(mtch!=-1)
+		const int mtch = MatchEdge(m_links[i].m_n[0], m_links[i].m_n[1], pa, pb);
+		if (mtch != -1)
 		{
 			appendLink(i);
-			Link*	pft[]={&m_links[i],&m_links[m_links.size()-1]};
-			pft[0]->m_n[1]=pn[mtch];
-			pft[1]->m_n[0]=pn[1-mtch];
-			done=true;
+			Link* pft[] = {&m_links[i], &m_links[m_links.size() - 1]};
+			pft[0]->m_n[1] = pn[mtch];
+			pft[1]->m_n[0] = pn[1 - mtch];
+			done = true;
 		}
 	}
-	for(i=0,ni=m_faces.size();i<ni;++i)
+	for (i = 0, ni = m_faces.size(); i < ni; ++i)
 	{
-		for(int k=2,l=0;l<3;k=l++)
+		for (int k = 2, l = 0; l < 3; k = l++)
 		{
-			const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
-			if(mtch!=-1)
+			const int mtch = MatchEdge(m_faces[i].m_n[k], m_faces[i].m_n[l], pa, pb);
+			if (mtch != -1)
 			{
 				appendFace(i);
-				Face*	pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
-				pft[0]->m_n[l]=pn[mtch];
-				pft[1]->m_n[k]=pn[1-mtch];
-				appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
-				appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
+				Face* pft[] = {&m_faces[i], &m_faces[m_faces.size() - 1]};
+				pft[0]->m_n[l] = pn[mtch];
+				pft[1]->m_n[k] = pn[1 - mtch];
+				appendLink(pn[0], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true);
+				appendLink(pn[1], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true);
 			}
 		}
 	}
-	if(!done)
+	if (!done)
 	{
 		m_ndbvt.remove(pn[0]->m_leaf);
 		m_ndbvt.remove(pn[1]->m_leaf);
 		m_nodes.pop_back();
 		m_nodes.pop_back();
 	}
-	return(done);
+	return (done);
 }
 
 //
-bool			btSoftBody::rayTest(const btVector3& rayFrom,
-									const btVector3& rayTo,
-									sRayCast& results)
+bool btSoftBody::rayTest(const btVector3& rayFrom,
+						 const btVector3& rayTo,
+						 sRayCast& results)
 {
-	if(m_faces.size()&&m_fdbvt.empty()) 
+	if (m_faces.size() && m_fdbvt.empty())
 		initializeFaceTree();
 
-	results.body	=	this;
+	results.body = this;
 	results.fraction = 1.f;
-	results.feature	=	eFeature::None;
-	results.index	=	-1;
+	results.feature = eFeature::None;
+	results.index = -1;
 
-	return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0);
+	return (rayTest(rayFrom, rayTo, results.fraction, results.feature, results.index, false) != 0);
+}
+
+bool btSoftBody::rayFaceTest(const btVector3& rayFrom,
+							 const btVector3& rayTo,
+							 sRayCast& results)
+{
+	if (m_faces.size() == 0)
+		return false;
+	else
+	{
+		if (m_fdbvt.empty())
+			initializeFaceTree();
+	}
+
+	results.body = this;
+	results.fraction = 1.f;
+	results.index = -1;
+
+	return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0);
 }
 
 //
-void			btSoftBody::setSolver(eSolverPresets::_ preset)
+void btSoftBody::setSolver(eSolverPresets::_ preset)
 {
 	m_cfg.m_vsequence.clear();
 	m_cfg.m_psequence.clear();
 	m_cfg.m_dsequence.clear();
-	switch(preset)
-	{
-	case	eSolverPresets::Positions:
-		m_cfg.m_psequence.push_back(ePSolver::Anchors);
-		m_cfg.m_psequence.push_back(ePSolver::RContacts);
-		m_cfg.m_psequence.push_back(ePSolver::SContacts);
-		m_cfg.m_psequence.push_back(ePSolver::Linear);	
-		break;	
-	case	eSolverPresets::Velocities:
-		m_cfg.m_vsequence.push_back(eVSolver::Linear);
-
-		m_cfg.m_psequence.push_back(ePSolver::Anchors);
-		m_cfg.m_psequence.push_back(ePSolver::RContacts);
-		m_cfg.m_psequence.push_back(ePSolver::SContacts);
-
-		m_cfg.m_dsequence.push_back(ePSolver::Linear);
-		break;
+	switch (preset)
+	{
+		case eSolverPresets::Positions:
+			m_cfg.m_psequence.push_back(ePSolver::Anchors);
+			m_cfg.m_psequence.push_back(ePSolver::RContacts);
+			m_cfg.m_psequence.push_back(ePSolver::SContacts);
+			m_cfg.m_psequence.push_back(ePSolver::Linear);
+			break;
+		case eSolverPresets::Velocities:
+			m_cfg.m_vsequence.push_back(eVSolver::Linear);
+
+			m_cfg.m_psequence.push_back(ePSolver::Anchors);
+			m_cfg.m_psequence.push_back(ePSolver::RContacts);
+			m_cfg.m_psequence.push_back(ePSolver::SContacts);
+
+			m_cfg.m_dsequence.push_back(ePSolver::Linear);
+			break;
 	}
 }
 
-//
-void			btSoftBody::predictMotion(btScalar dt)
+void btSoftBody::predictMotion(btScalar dt)
 {
+	int i, ni;
 
-	int i,ni;
-
-	/* Update				*/ 
-	if(m_bUpdateRtCst)
+	/* Update                */
+	if (m_bUpdateRtCst)
 	{
-		m_bUpdateRtCst=false;
+		m_bUpdateRtCst = false;
 		updateConstants();
 		m_fdbvt.clear();
-		if(m_cfg.collisions&fCollision::VF_SS)
+		if (m_cfg.collisions & fCollision::VF_SS)
 		{
-			initializeFaceTree();			
+			initializeFaceTree();
 		}
 	}
 
-	/* Prepare				*/ 
-	m_sst.sdt		=	dt*m_cfg.timescale;
-	m_sst.isdt		=	1/m_sst.sdt;
-	m_sst.velmrg	=	m_sst.sdt*3;
-	m_sst.radmrg	=	getCollisionShape()->getMargin();
-	m_sst.updmrg	=	m_sst.radmrg*(btScalar)0.25;
-	/* Forces				*/ 
-	addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
+	/* Prepare                */
+	m_sst.sdt = dt * m_cfg.timescale;
+	m_sst.isdt = 1 / m_sst.sdt;
+	m_sst.velmrg = m_sst.sdt * 3;
+	m_sst.radmrg = getCollisionShape()->getMargin();
+	m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
+	/* Forces                */
+	addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
 	applyForces();
-	/* Integrate			*/ 
-	for(i=0,ni=m_nodes.size();i<ni;++i)
+	/* Integrate            */
+	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 	{
-		Node&	n=m_nodes[i];
-		n.m_q	=	n.m_x;
-		btVector3 deltaV = n.m_f*n.m_im*m_sst.sdt;
+		Node& n = m_nodes[i];
+		n.m_q = n.m_x;
+		btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt;
 		{
 			btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
-			btScalar clampDeltaV = maxDisplacement/m_sst.sdt;
-			for (int c=0;c<3;c++)
+			btScalar clampDeltaV = maxDisplacement / m_sst.sdt;
+			for (int c = 0; c < 3; c++)
 			{
-				if (deltaV[c]>clampDeltaV)
+				if (deltaV[c] > clampDeltaV)
 				{
 					deltaV[c] = clampDeltaV;
 				}
-				if (deltaV[c]<-clampDeltaV)
+				if (deltaV[c] < -clampDeltaV)
 				{
-					deltaV[c]=-clampDeltaV;
+					deltaV[c] = -clampDeltaV;
 				}
 			}
 		}
-		n.m_v	+=	deltaV;
-		n.m_x	+=	n.m_v*m_sst.sdt;
-		n.m_f	=	btVector3(0,0,0);
+		n.m_v += deltaV;
+		n.m_x += n.m_v * m_sst.sdt;
+		n.m_f = btVector3(0, 0, 0);
 	}
-	/* Clusters				*/ 
+	/* Clusters                */
 	updateClusters();
-	/* Bounds				*/ 
-	updateBounds();	
-	/* Nodes				*/ 
-	ATTRIBUTE_ALIGNED16(btDbvtVolume)	vol;
-	for(i=0,ni=m_nodes.size();i<ni;++i)
-	{
-		Node&	n=m_nodes[i];
-		vol = btDbvtVolume::FromCR(n.m_x,m_sst.radmrg);
-		m_ndbvt.update(	n.m_leaf,
-			vol,
-			n.m_v*m_sst.velmrg,
-			m_sst.updmrg);
-	}
-	/* Faces				*/ 
-	if(!m_fdbvt.empty())
-	{
-		for(int i=0;i<m_faces.size();++i)
-		{
-			Face&			f=m_faces[i];
-			const btVector3	v=(	f.m_n[0]->m_v+
-				f.m_n[1]->m_v+
-				f.m_n[2]->m_v)/3;
-			vol = VolumeOf(f,m_sst.radmrg);
-			m_fdbvt.update(	f.m_leaf,
-				vol,
-				v*m_sst.velmrg,
-				m_sst.updmrg);
-		}
-	}
-	/* Pose					*/ 
+	/* Bounds                */
+	updateBounds();
+	/* Nodes                */
+	ATTRIBUTE_ALIGNED16(btDbvtVolume)
+	vol;
+	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
+	{
+		Node& n = m_nodes[i];
+		vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg);
+		m_ndbvt.update(n.m_leaf,
+					   vol,
+					   n.m_v * m_sst.velmrg,
+					   m_sst.updmrg);
+	}
+	/* Faces                */
+	if (!m_fdbvt.empty())
+	{
+		for (int i = 0; i < m_faces.size(); ++i)
+		{
+			Face& f = m_faces[i];
+			const btVector3 v = (f.m_n[0]->m_v +
+								 f.m_n[1]->m_v +
+								 f.m_n[2]->m_v) /
+								3;
+			vol = VolumeOf(f, m_sst.radmrg);
+			m_fdbvt.update(f.m_leaf,
+						   vol,
+						   v * m_sst.velmrg,
+						   m_sst.updmrg);
+		}
+	}
+	/* Pose                    */
 	updatePose();
-	/* Match				*/ 
-	if(m_pose.m_bframe&&(m_cfg.kMT>0))
+	/* Match                */
+	if (m_pose.m_bframe && (m_cfg.kMT > 0))
 	{
-		const btMatrix3x3	posetrs=m_pose.m_rot;
-		for(int i=0,ni=m_nodes.size();i<ni;++i)
+		const btMatrix3x3 posetrs = m_pose.m_rot;
+		for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
 		{
-			Node&	n=m_nodes[i];
-			if(n.m_im>0)
+			Node& n = m_nodes[i];
+			if (n.m_im > 0)
 			{
-				const btVector3	x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
-				n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
+				const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com;
+				n.m_x = Lerp(n.m_x, x, m_cfg.kMT);
 			}
 		}
 	}
-	/* Clear contacts		*/ 
+	/* Clear contacts        */
 	m_rcontacts.resize(0);
 	m_scontacts.resize(0);
-	/* Optimize dbvt's		*/ 
+	/* Optimize dbvt's        */
 	m_ndbvt.optimizeIncremental(1);
 	m_fdbvt.optimizeIncremental(1);
 	m_cdbvt.optimizeIncremental(1);
 }
 
 //
-void			btSoftBody::solveConstraints()
+void btSoftBody::solveConstraints()
 {
-
-	/* Apply clusters		*/ 
+	/* Apply clusters		*/
 	applyClusters(false);
-	/* Prepare links		*/ 
+	/* Prepare links		*/
 
-	int i,ni;
+	int i, ni;
 
-	for(i=0,ni=m_links.size();i<ni;++i)
+	for (i = 0, ni = m_links.size(); i < ni; ++i)
 	{
-		Link&	l=m_links[i];
-		l.m_c3		=	l.m_n[1]->m_q-l.m_n[0]->m_q;
-		l.m_c2		=	1/(l.m_c3.length2()*l.m_c0);
+		Link& l = m_links[i];
+		l.m_c3 = l.m_n[1]->m_q - l.m_n[0]->m_q;
+		l.m_c2 = 1 / (l.m_c3.length2() * l.m_c0);
 	}
-	/* Prepare anchors		*/ 
-	for(i=0,ni=m_anchors.size();i<ni;++i)
+	/* Prepare anchors		*/
+	for (i = 0, ni = m_anchors.size(); i < ni; ++i)
 	{
-		Anchor&			a=m_anchors[i];
-		const btVector3	ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
-		a.m_c0	=	ImpulseMatrix(	m_sst.sdt,
-			a.m_node->m_im,
-			a.m_body->getInvMass(),
-			a.m_body->getInvInertiaTensorWorld(),
-			ra);
-		a.m_c1	=	ra;
-		a.m_c2	=	m_sst.sdt*a.m_node->m_im;
+		Anchor& a = m_anchors[i];
+		const btVector3 ra = a.m_body->getWorldTransform().getBasis() * a.m_local;
+		a.m_c0 = ImpulseMatrix(m_sst.sdt,
+							   a.m_node->m_im,
+							   a.m_body->getInvMass(),
+							   a.m_body->getInvInertiaTensorWorld(),
+							   ra);
+		a.m_c1 = ra;
+		a.m_c2 = m_sst.sdt * a.m_node->m_im;
 		a.m_body->activate();
 	}
-	/* Solve velocities		*/ 
-	if(m_cfg.viterations>0)
+	/* Solve velocities		*/
+	if (m_cfg.viterations > 0)
 	{
-		/* Solve			*/ 
-		for(int isolve=0;isolve<m_cfg.viterations;++isolve)
+		/* Solve			*/
+		for (int isolve = 0; isolve < m_cfg.viterations; ++isolve)
 		{
-			for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
+			for (int iseq = 0; iseq < m_cfg.m_vsequence.size(); ++iseq)
 			{
-				getSolver(m_cfg.m_vsequence[iseq])(this,1);
+				getSolver(m_cfg.m_vsequence[iseq])(this, 1);
 			}
 		}
-		/* Update			*/ 
-		for(i=0,ni=m_nodes.size();i<ni;++i)
+		/* Update			*/
+		for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 		{
-			Node&	n=m_nodes[i];
-			n.m_x	=	n.m_q+n.m_v*m_sst.sdt;
+			Node& n = m_nodes[i];
+			n.m_x = n.m_q + n.m_v * m_sst.sdt;
 		}
 	}
-	/* Solve positions		*/ 
-	if(m_cfg.piterations>0)
+	/* Solve positions		*/
+	if (m_cfg.piterations > 0)
 	{
-		for(int isolve=0;isolve<m_cfg.piterations;++isolve)
+		for (int isolve = 0; isolve < m_cfg.piterations; ++isolve)
 		{
-			const btScalar ti=isolve/(btScalar)m_cfg.piterations;
-			for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
+			const btScalar ti = isolve / (btScalar)m_cfg.piterations;
+			for (int iseq = 0; iseq < m_cfg.m_psequence.size(); ++iseq)
 			{
-				getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
+				getSolver(m_cfg.m_psequence[iseq])(this, 1, ti);
 			}
 		}
-		const btScalar	vc=m_sst.isdt*(1-m_cfg.kDP);
-		for(i=0,ni=m_nodes.size();i<ni;++i)
+		const btScalar vc = m_sst.isdt * (1 - m_cfg.kDP);
+		for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 		{
-			Node&	n=m_nodes[i];
-			n.m_v	=	(n.m_x-n.m_q)*vc;
-			n.m_f	=	btVector3(0,0,0);		
+			Node& n = m_nodes[i];
+			n.m_v = (n.m_x - n.m_q) * vc;
+			n.m_f = btVector3(0, 0, 0);
 		}
 	}
-	/* Solve drift			*/ 
-	if(m_cfg.diterations>0)
+	/* Solve drift			*/
+	if (m_cfg.diterations > 0)
 	{
-		const btScalar	vcf=m_cfg.kVCF*m_sst.isdt;
-		for(i=0,ni=m_nodes.size();i<ni;++i)
+		const btScalar vcf = m_cfg.kVCF * m_sst.isdt;
+		for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 		{
-			Node&	n=m_nodes[i];
-			n.m_q	=	n.m_x;
+			Node& n = m_nodes[i];
+			n.m_q = n.m_x;
 		}
-		for(int idrift=0;idrift<m_cfg.diterations;++idrift)
+		for (int idrift = 0; idrift < m_cfg.diterations; ++idrift)
 		{
-			for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
+			for (int iseq = 0; iseq < m_cfg.m_dsequence.size(); ++iseq)
 			{
-				getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
+				getSolver(m_cfg.m_dsequence[iseq])(this, 1, 0);
 			}
 		}
-		for(int i=0,ni=m_nodes.size();i<ni;++i)
+		for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
 		{
-			Node&	n=m_nodes[i];
-			n.m_v	+=	(n.m_x-n.m_q)*vcf;
+			Node& n = m_nodes[i];
+			n.m_v += (n.m_x - n.m_q) * vcf;
 		}
 	}
-	/* Apply clusters		*/ 
+	/* Apply clusters		*/
 	dampClusters();
 	applyClusters(true);
 }
 
 //
-void			btSoftBody::staticSolve(int iterations)
+void btSoftBody::staticSolve(int iterations)
 {
-	for(int isolve=0;isolve<iterations;++isolve)
+	for (int isolve = 0; isolve < iterations; ++isolve)
 	{
-		for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
+		for (int iseq = 0; iseq < m_cfg.m_psequence.size(); ++iseq)
 		{
-			getSolver(m_cfg.m_psequence[iseq])(this,1,0);
+			getSolver(m_cfg.m_psequence[iseq])(this, 1, 0);
 		}
 	}
 }
 
 //
-void			btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
+void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/, int /*count*/, int /*iterations*/)
 {
 	/// placeholder
 }
 
 //
-void			btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
+void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
 {
-	const int	nb=bodies.size();
-	int			iterations=0;
+	const int nb = bodies.size();
+	int iterations = 0;
 	int i;
 
-	for(i=0;i<nb;++i)
+	for (i = 0; i < nb; ++i)
 	{
-		iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
+		iterations = btMax(iterations, bodies[i]->m_cfg.citerations);
 	}
-	for(i=0;i<nb;++i)
+	for (i = 0; i < nb; ++i)
 	{
 		bodies[i]->prepareClusters(iterations);
 	}
-	for(i=0;i<iterations;++i)
+	for (i = 0; i < iterations; ++i)
 	{
-		const btScalar sor=1;
-		for(int j=0;j<nb;++j)
+		const btScalar sor = 1;
+		for (int j = 0; j < nb; ++j)
 		{
 			bodies[j]->solveClusters(sor);
 		}
 	}
-	for(i=0;i<nb;++i)
+	for (i = 0; i < nb; ++i)
 	{
 		bodies[i]->cleanupClusters();
 	}
 }
 
 //
-void			btSoftBody::integrateMotion()
+void btSoftBody::integrateMotion()
 {
-	/* Update			*/ 
+	/* Update			*/
 	updateNormals();
 }
 
 //
-btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt)
+btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom, const btVector3& rayTo, btScalar mxt)
 {
 	m_rayFrom = rayFrom;
-	m_rayNormalizedDirection = (rayTo-rayFrom);
+	m_rayNormalizedDirection = (rayTo - rayFrom);
 	m_rayTo = rayTo;
-	m_mint	=	mxt;
-	m_face	=	0;
-	m_tests	=	0;
+	m_mint = mxt;
+	m_face = 0;
+	m_tests = 0;
 }
 
 //
-void				btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
+void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
 {
-	btSoftBody::Face&	f=*(btSoftBody::Face*)leaf->data;
-	const btScalar		t=rayFromToTriangle(	m_rayFrom,m_rayTo,m_rayNormalizedDirection,
-		f.m_n[0]->m_x,
-		f.m_n[1]->m_x,
-		f.m_n[2]->m_x,
-		m_mint);
-	if((t>0)&&(t<m_mint)) 
-	{ 
-		m_mint=t;m_face=&f; 
+	btSoftBody::Face& f = *(btSoftBody::Face*)leaf->data;
+	const btScalar t = rayFromToTriangle(m_rayFrom, m_rayTo, m_rayNormalizedDirection,
+										 f.m_n[0]->m_x,
+										 f.m_n[1]->m_x,
+										 f.m_n[2]->m_x,
+										 m_mint);
+	if ((t > 0) && (t < m_mint))
+	{
+		m_mint = t;
+		m_face = &f;
 	}
 	++m_tests;
 }
 
 //
-btScalar			btSoftBody::RayFromToCaster::rayFromToTriangle(	const btVector3& rayFrom,
-																   const btVector3& rayTo,
-																   const btVector3& rayNormalizedDirection,
-																   const btVector3& a,
-																   const btVector3& b,
-																   const btVector3& c,
-																   btScalar maxt)
+btScalar btSoftBody::RayFromToCaster::rayFromToTriangle(const btVector3& rayFrom,
+														const btVector3& rayTo,
+														const btVector3& rayNormalizedDirection,
+														const btVector3& a,
+														const btVector3& b,
+														const btVector3& c,
+														btScalar maxt)
 {
-	static const btScalar	ceps=-SIMD_EPSILON*10;
-	static const btScalar	teps=SIMD_EPSILON*10;
+	static const btScalar ceps = -SIMD_EPSILON * 10;
+	static const btScalar teps = SIMD_EPSILON * 10;
 
-	const btVector3			n=btCross(b-a,c-a);
-	const btScalar			d=btDot(a,n);
-	const btScalar			den=btDot(rayNormalizedDirection,n);
-	if(!btFuzzyZero(den))
+	const btVector3 n = btCross(b - a, c - a);
+	const btScalar d = btDot(a, n);
+	const btScalar den = btDot(rayNormalizedDirection, n);
+	if (!btFuzzyZero(den))
 	{
-		const btScalar		num=btDot(rayFrom,n)-d;
-		const btScalar		t=-num/den;
-		if((t>teps)&&(t<maxt))
+		const btScalar num = btDot(rayFrom, n) - d;
+		const btScalar t = -num / den;
+		if ((t > teps) && (t < maxt))
 		{
-			const btVector3	hit=rayFrom+rayNormalizedDirection*t;
-			if(	(btDot(n,btCross(a-hit,b-hit))>ceps)	&&			
-				(btDot(n,btCross(b-hit,c-hit))>ceps)	&&
-				(btDot(n,btCross(c-hit,a-hit))>ceps))
+			const btVector3 hit = rayFrom + rayNormalizedDirection * t;
+			if ((btDot(n, btCross(a - hit, b - hit)) > ceps) &&
+				(btDot(n, btCross(b - hit, c - hit)) > ceps) &&
+				(btDot(n, btCross(c - hit, a - hit)) > ceps))
 			{
-				return(t);
+				return (t);
 			}
 		}
 	}
-	return(-1);
+	return (-1);
 }
 
 //
-void				btSoftBody::pointersToIndices()
+void btSoftBody::pointersToIndices()
 {
-#define	PTR2IDX(_p_,_b_)	reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
-	btSoftBody::Node*	base=m_nodes.size() ? &m_nodes[0] : 0;
-	int i,ni;
+#define PTR2IDX(_p_, _b_) reinterpret_cast<btSoftBody::Node*>((_p_) - (_b_))
+	btSoftBody::Node* base = m_nodes.size() ? &m_nodes[0] : 0;
+	int i, ni;
 
-	for(i=0,ni=m_nodes.size();i<ni;++i)
+	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 	{
-		if(m_nodes[i].m_leaf)
+		if (m_nodes[i].m_leaf)
 		{
-			m_nodes[i].m_leaf->data=*(void**)&i;
+			m_nodes[i].m_leaf->data = *(void**)&i;
 		}
 	}
-	for(i=0,ni=m_links.size();i<ni;++i)
+	for (i = 0, ni = m_links.size(); i < ni; ++i)
 	{
-		m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
-		m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
+		m_links[i].m_n[0] = PTR2IDX(m_links[i].m_n[0], base);
+		m_links[i].m_n[1] = PTR2IDX(m_links[i].m_n[1], base);
 	}
-	for(i=0,ni=m_faces.size();i<ni;++i)
+	for (i = 0, ni = m_faces.size(); i < ni; ++i)
 	{
-		m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
-		m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
-		m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
-		if(m_faces[i].m_leaf)
+		m_faces[i].m_n[0] = PTR2IDX(m_faces[i].m_n[0], base);
+		m_faces[i].m_n[1] = PTR2IDX(m_faces[i].m_n[1], base);
+		m_faces[i].m_n[2] = PTR2IDX(m_faces[i].m_n[2], base);
+		if (m_faces[i].m_leaf)
 		{
-			m_faces[i].m_leaf->data=*(void**)&i;
+			m_faces[i].m_leaf->data = *(void**)&i;
 		}
 	}
-	for(i=0,ni=m_anchors.size();i<ni;++i)
+	for (i = 0, ni = m_anchors.size(); i < ni; ++i)
 	{
-		m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
+		m_anchors[i].m_node = PTR2IDX(m_anchors[i].m_node, base);
 	}
-	for(i=0,ni=m_notes.size();i<ni;++i)
+	for (i = 0, ni = m_notes.size(); i < ni; ++i)
 	{
-		for(int j=0;j<m_notes[i].m_rank;++j)
+		for (int j = 0; j < m_notes[i].m_rank; ++j)
 		{
-			m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
+			m_notes[i].m_nodes[j] = PTR2IDX(m_notes[i].m_nodes[j], base);
 		}
 	}
-#undef	PTR2IDX
+#undef PTR2IDX
 }
 
 //
-void				btSoftBody::indicesToPointers(const int* map)
+void btSoftBody::indicesToPointers(const int* map)
 {
-#define	IDX2PTR(_p_,_b_)	map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]):	\
-	(&(_b_)[(((char*)_p_)-(char*)0)])
-	btSoftBody::Node*	base=m_nodes.size() ? &m_nodes[0]:0;
-	int i,ni;
+#define IDX2PTR(_p_, _b_) map ? (&(_b_)[map[(((char*)_p_) - (char*)0)]]) : (&(_b_)[(((char*)_p_) - (char*)0)])
+	btSoftBody::Node* base = m_nodes.size() ? &m_nodes[0] : 0;
+	int i, ni;
 
-	for(i=0,ni=m_nodes.size();i<ni;++i)
+	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 	{
-		if(m_nodes[i].m_leaf)
+		if (m_nodes[i].m_leaf)
 		{
-			m_nodes[i].m_leaf->data=&m_nodes[i];
+			m_nodes[i].m_leaf->data = &m_nodes[i];
 		}
 	}
-	for(i=0,ni=m_links.size();i<ni;++i)
+	for (i = 0, ni = m_links.size(); i < ni; ++i)
 	{
-		m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
-		m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
+		m_links[i].m_n[0] = IDX2PTR(m_links[i].m_n[0], base);
+		m_links[i].m_n[1] = IDX2PTR(m_links[i].m_n[1], base);
 	}
-	for(i=0,ni=m_faces.size();i<ni;++i)
+	for (i = 0, ni = m_faces.size(); i < ni; ++i)
 	{
-		m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
-		m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
-		m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
-		if(m_faces[i].m_leaf)
+		m_faces[i].m_n[0] = IDX2PTR(m_faces[i].m_n[0], base);
+		m_faces[i].m_n[1] = IDX2PTR(m_faces[i].m_n[1], base);
+		m_faces[i].m_n[2] = IDX2PTR(m_faces[i].m_n[2], base);
+		if (m_faces[i].m_leaf)
 		{
-			m_faces[i].m_leaf->data=&m_faces[i];
+			m_faces[i].m_leaf->data = &m_faces[i];
 		}
 	}
-	for(i=0,ni=m_anchors.size();i<ni;++i)
+	for (i = 0, ni = m_anchors.size(); i < ni; ++i)
 	{
-		m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
+		m_anchors[i].m_node = IDX2PTR(m_anchors[i].m_node, base);
 	}
-	for(i=0,ni=m_notes.size();i<ni;++i)
+	for (i = 0, ni = m_notes.size(); i < ni; ++i)
 	{
-		for(int j=0;j<m_notes[i].m_rank;++j)
+		for (int j = 0; j < m_notes[i].m_rank; ++j)
 		{
-			m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
+			m_notes[i].m_nodes[j] = IDX2PTR(m_notes[i].m_nodes[j], base);
 		}
 	}
-#undef	IDX2PTR
+#undef IDX2PTR
 }
 
 //
-int					btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
-										btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
+int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo,
+						btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const
 {
-	int	cnt=0;
-	btVector3 dir = rayTo-rayFrom;
-	
+	int cnt = 0;
+	btVector3 dir = rayTo - rayFrom;
 
-	if(bcountonly||m_fdbvt.empty())
-	{/* Full search	*/ 
-		
-		for(int i=0,ni=m_faces.size();i<ni;++i)
-		{
-			const btSoftBody::Face&	f=m_faces[i];
+	if (bcountonly || m_fdbvt.empty())
+	{ /* Full search	*/
 
-			const btScalar			t=RayFromToCaster::rayFromToTriangle(	rayFrom,rayTo,dir,
-				f.m_n[0]->m_x,
-				f.m_n[1]->m_x,
-				f.m_n[2]->m_x,
-				mint);
-			if(t>0)
+		for (int i = 0, ni = m_faces.size(); i < ni; ++i)
+		{
+			const btSoftBody::Face& f = m_faces[i];
+
+			const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir,
+																  f.m_n[0]->m_x,
+																  f.m_n[1]->m_x,
+																  f.m_n[2]->m_x,
+																  mint);
+			if (t > 0)
 			{
 				++cnt;
-				if(!bcountonly)
+				if (!bcountonly)
 				{
-					feature=btSoftBody::eFeature::Face;
-					index=i;
-					mint=t;
+					feature = btSoftBody::eFeature::Face;
+					index = i;
+					mint = t;
 				}
 			}
 		}
 	}
 	else
-	{/* Use dbvt	*/ 
-		RayFromToCaster	collider(rayFrom,rayTo,mint);
+	{ /* Use dbvt	*/
+		RayFromToCaster collider(rayFrom, rayTo, mint);
 
-		btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider);
-		if(collider.m_face)
+		btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider);
+		if (collider.m_face)
 		{
-			mint=collider.m_mint;
-			feature=btSoftBody::eFeature::Face;
-			index=(int)(collider.m_face-&m_faces[0]);
-			cnt=1;
+			mint = collider.m_mint;
+			feature = btSoftBody::eFeature::Face;
+			index = (int)(collider.m_face - &m_faces[0]);
+			cnt = 1;
 		}
 	}
 
-	for (int i=0;i<m_tetras.size();i++)
+	for (int i = 0; i < m_tetras.size(); i++)
 	{
 		const btSoftBody::Tetra& tet = m_tetras[i];
-		int tetfaces[4][3] = {{0,1,2},{0,1,3},{1,2,3},{0,2,3}};
-		for (int f=0;f<4;f++)
+		int tetfaces[4][3] = {{0, 1, 2}, {0, 1, 3}, {1, 2, 3}, {0, 2, 3}};
+		for (int f = 0; f < 4; f++)
 		{
+			int index0 = tetfaces[f][0];
+			int index1 = tetfaces[f][1];
+			int index2 = tetfaces[f][2];
+			btVector3 v0 = tet.m_n[index0]->m_x;
+			btVector3 v1 = tet.m_n[index1]->m_x;
+			btVector3 v2 = tet.m_n[index2]->m_x;
+
+			const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir,
+																  v0, v1, v2,
+																  mint);
+			if (t > 0)
+			{
+				++cnt;
+				if (!bcountonly)
+				{
+					feature = btSoftBody::eFeature::Tetra;
+					index = i;
+					mint = t;
+				}
+			}
+		}
+	}
+	return (cnt);
+}
 
-			int index0=tetfaces[f][0];
-			int index1=tetfaces[f][1];
-			int index2=tetfaces[f][2];
-			btVector3 v0=tet.m_n[index0]->m_x;
-			btVector3 v1=tet.m_n[index1]->m_x;
-			btVector3 v2=tet.m_n[index2]->m_x;
+int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
+							btScalar& mint, int& index) const
+{
+	int cnt = 0;
+	{ /* Use dbvt	*/
+		RayFromToCaster collider(rayFrom, rayTo, mint);
 
+		btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider);
+		if (collider.m_face)
+		{
+			mint = collider.m_mint;
+			index = (int)(collider.m_face - &m_faces[0]);
+			cnt = 1;
+		}
+	}
+	return (cnt);
+}
+
+//
+static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
+{
+	if (n == 0)
+		return 0;
+	btDbvntNode* root = new btDbvntNode(n);
+	if (n->isinternal())
+	{
+		btDbvntNode* c0 = copyToDbvnt(n->childs[0]);
+		root->childs[0] = c0;
+		btDbvntNode* c1 = copyToDbvnt(n->childs[1]);
+		root->childs[1] = c1;
+	}
+	return root;
+}
 
-		const btScalar			t=RayFromToCaster::rayFromToTriangle(	rayFrom,rayTo,dir,
-			v0,v1,v2,
-				mint);
-		if(t>0)
+static inline void calculateNormalCone(btDbvntNode* root)
+{
+	if (!root)
+		return;
+	if (root->isleaf())
+	{
+		const btSoftBody::Face* face = (btSoftBody::Face*)root->data;
+		root->normal = face->m_normal;
+		root->angle = 0;
+	}
+	else
+	{
+		btVector3 n0(0, 0, 0), n1(0, 0, 0);
+		btScalar a0 = 0, a1 = 0;
+		if (root->childs[0])
+		{
+			calculateNormalCone(root->childs[0]);
+			n0 = root->childs[0]->normal;
+			a0 = root->childs[0]->angle;
+		}
+		if (root->childs[1])
+		{
+			calculateNormalCone(root->childs[1]);
+			n1 = root->childs[1]->normal;
+			a1 = root->childs[1]->angle;
+		}
+		root->normal = (n0 + n1).safeNormalize();
+		root->angle = btMax(a0, a1) + btAngle(n0, n1) * 0.5;
+	}
+}
+
+void btSoftBody::initializeFaceTree()
+{
+	BT_PROFILE("btSoftBody::initializeFaceTree");
+	m_fdbvt.clear();
+	// create leaf nodes;
+	btAlignedObjectArray<btDbvtNode*> leafNodes;
+	leafNodes.resize(m_faces.size());
+	for (int i = 0; i < m_faces.size(); ++i)
+	{
+		Face& f = m_faces[i];
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)
+		vol = VolumeOf(f, 0);
+		btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
+		node->parent = NULL;
+		node->data = &f;
+		node->childs[1] = 0;
+		node->volume = vol;
+		leafNodes[i] = node;
+		f.m_leaf = node;
+	}
+	btAlignedObjectArray<btAlignedObjectArray<int> > adj;
+	adj.resize(m_faces.size());
+	// construct the adjacency list for triangles
+	for (int i = 0; i < adj.size(); ++i)
+	{
+		for (int j = i + 1; j < adj.size(); ++j)
+		{
+			int dup = 0;
+			for (int k = 0; k < 3; ++k)
 			{
-				++cnt;
-				if(!bcountonly)
+				for (int l = 0; l < 3; ++l)
 				{
-					feature=btSoftBody::eFeature::Tetra;
-					index=i;
-					mint=t;
+					if (m_faces[i].m_n[k] == m_faces[j].m_n[l])
+					{
+						++dup;
+						break;
+					}
+				}
+				if (dup == 2)
+				{
+					adj[i].push_back(j);
+					adj[j].push_back(i);
 				}
 			}
 		}
 	}
-	return(cnt);
+	m_fdbvt.m_root = buildTreeBottomUp(leafNodes, adj);
+	if (m_fdbvnt)
+		delete m_fdbvnt;
+	m_fdbvnt = copyToDbvnt(m_fdbvt.m_root);
+	updateFaceTree(false, false);
+	rebuildNodeTree();
 }
 
 //
-void			btSoftBody::initializeFaceTree()
+void btSoftBody::rebuildNodeTree()
 {
-	m_fdbvt.clear();
-	for(int i=0;i<m_faces.size();++i)
+	m_ndbvt.clear();
+	btAlignedObjectArray<btDbvtNode*> leafNodes;
+	leafNodes.resize(m_nodes.size());
+	for (int i = 0; i < m_nodes.size(); ++i)
+	{
+		Node& n = m_nodes[i];
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)
+		vol = btDbvtVolume::FromCR(n.m_x, 0);
+		btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
+		node->parent = NULL;
+		node->data = &n;
+		node->childs[1] = 0;
+		node->volume = vol;
+		leafNodes[i] = node;
+		n.m_leaf = node;
+	}
+	btAlignedObjectArray<btAlignedObjectArray<int> > adj;
+	adj.resize(m_nodes.size());
+	btAlignedObjectArray<int> old_id;
+	old_id.resize(m_nodes.size());
+	for (int i = 0; i < m_nodes.size(); ++i)
+		old_id[i] = m_nodes[i].index;
+	for (int i = 0; i < m_nodes.size(); ++i)
+		m_nodes[i].index = i;
+	for (int i = 0; i < m_links.size(); ++i)
 	{
-		Face&	f=m_faces[i];
-		f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
+		Link& l = m_links[i];
+		adj[l.m_n[0]->index].push_back(l.m_n[1]->index);
+		adj[l.m_n[1]->index].push_back(l.m_n[0]->index);
 	}
+	m_ndbvt.m_root = buildTreeBottomUp(leafNodes, adj);
+	for (int i = 0; i < m_nodes.size(); ++i)
+		m_nodes[i].index = old_id[i];
 }
 
 //
-btVector3		btSoftBody::evaluateCom() const
+btVector3 btSoftBody::evaluateCom() const
 {
-	btVector3	com(0,0,0);
-	if(m_pose.m_bframe)
+	btVector3 com(0, 0, 0);
+	if (m_pose.m_bframe)
 	{
-		for(int i=0,ni=m_nodes.size();i<ni;++i)
+		for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
 		{
-			com+=m_nodes[i].m_x*m_pose.m_wgh[i];
+			com += m_nodes[i].m_x * m_pose.m_wgh[i];
 		}
 	}
-	return(com);
+	return (com);
 }
 
-//
-bool				btSoftBody::checkContact(	const btCollisionObjectWrapper* colObjWrap,
-											 const btVector3& x,
-											 btScalar margin,
-											 btSoftBody::sCti& cti) const
+bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
+							  const btVector3& x,
+							  btScalar margin,
+							  btSoftBody::sCti& cti) const
 {
 	btVector3 nrm;
-	const btCollisionShape *shp = colObjWrap->getCollisionShape();
-//	const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
+	const btCollisionShape* shp = colObjWrap->getCollisionShape();
+	//    const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
 	//const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
-	const btTransform &wtr = colObjWrap->getWorldTransform();
+	const btTransform& wtr = colObjWrap->getWorldTransform();
 	//todo: check which transform is needed here
 
-	btScalar dst = 
-		m_worldInfo->m_sparsesdf.Evaluate(	
+	btScalar dst =
+		m_worldInfo->m_sparsesdf.Evaluate(
 			wtr.invXform(x),
 			shp,
 			nrm,
 			margin);
-	if(dst<0)
+	if (dst < 0)
 	{
 		cti.m_colObj = colObjWrap->getCollisionObject();
-		cti.m_normal = wtr.getBasis()*nrm;
-		cti.m_offset = -btDot( cti.m_normal, x - cti.m_normal * dst );
-		return(true);
+		cti.m_normal = wtr.getBasis() * nrm;
+		cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
+		return (true);
 	}
-	return(false);
+	return (false);
 }
 
 //
-void					btSoftBody::updateNormals()
+bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap,
+										const btVector3& x,
+										btScalar margin,
+										btSoftBody::sCti& cti, bool predict) const
 {
+	btVector3 nrm;
+	const btCollisionShape* shp = colObjWrap->getCollisionShape();
+	const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
+	// use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
+	// but resolve contact at x_n
+	btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
+								: colObjWrap->getWorldTransform();
+	btScalar dst =
+		m_worldInfo->m_sparsesdf.Evaluate(
+			wtr.invXform(x),
+			shp,
+			nrm,
+			margin);
+
+	if (!predict)
+	{
+		cti.m_colObj = colObjWrap->getCollisionObject();
+		cti.m_normal = wtr.getBasis() * nrm;
+		cti.m_offset = dst;
+	}
+	if (dst < 0)
+		return true;
+	return (false);
+}
 
-	const btVector3	zv(0,0,0);
-	int i,ni;
+//
+// Compute barycentric coordinates (u, v, w) for
+// point p with respect to triangle (a, b, c)
+static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVector3& c, btVector3& bary)
+{
+	btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
+	btScalar d00 = v0.dot(v0);
+	btScalar d01 = v0.dot(v1);
+	btScalar d11 = v1.dot(v1);
+	btScalar d20 = v2.dot(v0);
+	btScalar d21 = v2.dot(v1);
+	btScalar denom = d00 * d11 - d01 * d01;
+	bary.setY((d11 * d20 - d01 * d21) / denom);
+	bary.setZ((d00 * d21 - d01 * d20) / denom);
+	bary.setX(btScalar(1) - bary.getY() - bary.getZ());
+}
 
-	for(i=0,ni=m_nodes.size();i<ni;++i)
+//
+bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap,
+											Face& f,
+											btVector3& contact_point,
+											btVector3& bary,
+											btScalar margin,
+											btSoftBody::sCti& cti, bool predict) const
+{
+	btVector3 nrm;
+	const btCollisionShape* shp = colObjWrap->getCollisionShape();
+	const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
+	// use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
+	// but resolve contact at x_n
+	btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
+								: colObjWrap->getWorldTransform();
+	btScalar dst;
+	btGjkEpaSolver2::sResults results;
+
+//	#define USE_QUADRATURE 1
+//#define CACHE_PREV_COLLISION
+
+	// use collision quadrature point
+#ifdef USE_QUADRATURE
+	{
+		dst = SIMD_INFINITY;
+		btVector3 local_nrm;
+		for (int q = 0; q < m_quads.size(); ++q)
+		{
+			btVector3 p;
+			if (predict)
+				p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]);
+			else
+				p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
+			btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate(
+				wtr.invXform(p),
+				shp,
+				local_nrm,
+				margin);
+			if (local_dst < dst)
+			{
+				if (local_dst < 0 && predict)
+					return true;
+				dst = local_dst;
+				contact_point = p;
+				bary = m_quads[q];
+				nrm = local_nrm;
+			}
+			if (!predict)
+			{
+				cti.m_colObj = colObjWrap->getCollisionObject();
+				cti.m_normal = wtr.getBasis() * nrm;
+				cti.m_offset = dst;
+			}
+		}
+		return (dst < 0);
+	}
+#endif
+
+	// collision detection using x*
+	btTransform triangle_transform;
+	triangle_transform.setIdentity();
+	triangle_transform.setOrigin(f.m_n[0]->m_q);
+	btTriangleShape triangle(btVector3(0, 0, 0), f.m_n[1]->m_q - f.m_n[0]->m_q, f.m_n[2]->m_q - f.m_n[0]->m_q);
+	btVector3 guess(0, 0, 0);
+	const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
+	btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
+	dst = results.distance - 2.0 * csh->getMargin() - margin;  // margin padding so that the distance = the actual distance between face and rigid - margin of rigid - margin of deformable
+	if (dst >= 0)
+		return false;
+
+// Use consistent barycenter to recalculate distance.
+#ifdef CACHE_PREV_COLLISION
+	if (f.m_pcontact[3] != 0)
+	{
+		for (int i = 0; i < 3; ++i)
+			bary[i] = f.m_pcontact[i];
+		contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
+		const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
+		btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results);
+		cti.m_colObj = colObjWrap->getCollisionObject();
+		dst = results.distance;
+		cti.m_normal = results.normal;
+		cti.m_offset = dst;
+
+		//point-convex CD
+		wtr = colObjWrap->getWorldTransform();
+		btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
+		triangle_transform.setOrigin(f.m_n[0]->m_x);
+		btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
+
+		dst = results.distance - csh->getMargin() - margin;
+		return true;
+	}
+#endif
+
+	// Use triangle-convex CD.
+	wtr = colObjWrap->getWorldTransform();
+	btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
+	triangle_transform.setOrigin(f.m_n[0]->m_x);
+	btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
+	contact_point = results.witnesses[0];
+	getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
+
+	for (int i = 0; i < 3; ++i)
+		f.m_pcontact[i] = bary[i];
+
+	dst = results.distance - csh->getMargin() - margin;
+	cti.m_colObj = colObjWrap->getCollisionObject();
+	cti.m_normal = results.normal;
+	cti.m_offset = dst;
+	return true;
+}
+
+void btSoftBody::updateNormals()
+{
+	const btVector3 zv(0, 0, 0);
+	int i, ni;
+
+	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 	{
-		m_nodes[i].m_n=zv;
+		m_nodes[i].m_n = zv;
 	}
-	for(i=0,ni=m_faces.size();i<ni;++i)
+	for (i = 0, ni = m_faces.size(); i < ni; ++i)
 	{
-		btSoftBody::Face&	f=m_faces[i];
-		const btVector3		n=btCross(f.m_n[1]->m_x-f.m_n[0]->m_x,
-			f.m_n[2]->m_x-f.m_n[0]->m_x);
-		f.m_normal=n.normalized();
-		f.m_n[0]->m_n+=n;
-		f.m_n[1]->m_n+=n;
-		f.m_n[2]->m_n+=n;
+		btSoftBody::Face& f = m_faces[i];
+		const btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x,
+									f.m_n[2]->m_x - f.m_n[0]->m_x);
+		f.m_normal = n;
+		f.m_normal.safeNormalize();
+		f.m_n[0]->m_n += n;
+		f.m_n[1]->m_n += n;
+		f.m_n[2]->m_n += n;
 	}
-	for(i=0,ni=m_nodes.size();i<ni;++i)
+	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 	{
 		btScalar len = m_nodes[i].m_n.length();
-		if (len>SIMD_EPSILON)
+		if (len > SIMD_EPSILON)
 			m_nodes[i].m_n /= len;
 	}
 }
 
 //
-void					btSoftBody::updateBounds()
+void btSoftBody::updateBounds()
 {
 	/*if( m_acceleratedSoftBody )
 	{
@@ -2315,258 +2958,291 @@ void					btSoftBody::updateBounds()
 		m_bounds[1] = btVector3(1000, 1000, 1000);
 
 	} else {*/
-		if(m_ndbvt.m_root)
-		{
-			const btVector3&	mins=m_ndbvt.m_root->volume.Mins();
-			const btVector3&	maxs=m_ndbvt.m_root->volume.Maxs();
-			const btScalar		csm=getCollisionShape()->getMargin();
-			const btVector3		mrg=btVector3(	csm,
-				csm,
-				csm)*1; // ??? to investigate...
-			m_bounds[0]=mins-mrg;
-			m_bounds[1]=maxs+mrg;
-			if(0!=getBroadphaseHandle())
-			{					
-				m_worldInfo->m_broadphase->setAabb(	getBroadphaseHandle(),
-					m_bounds[0],
-					m_bounds[1],
-					m_worldInfo->m_dispatcher);
+	//    if (m_ndbvt.m_root)
+	//    {
+	//        const btVector3& mins = m_ndbvt.m_root->volume.Mins();
+	//        const btVector3& maxs = m_ndbvt.m_root->volume.Maxs();
+	//        const btScalar csm = getCollisionShape()->getMargin();
+	//        const btVector3 mrg = btVector3(csm,
+	//                                        csm,
+	//                                        csm) *
+	//                              1;  // ??? to investigate...
+	//        m_bounds[0] = mins - mrg;
+	//        m_bounds[1] = maxs + mrg;
+	//        if (0 != getBroadphaseHandle())
+	//        {
+	//            m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
+	//                                               m_bounds[0],
+	//                                               m_bounds[1],
+	//                                               m_worldInfo->m_dispatcher);
+	//        }
+	//    }
+	//    else
+	//    {
+	//        m_bounds[0] =
+	//            m_bounds[1] = btVector3(0, 0, 0);
+	//    }
+	if (m_nodes.size())
+	{
+		btVector3 mins = m_nodes[0].m_x;
+		btVector3 maxs = m_nodes[0].m_x;
+		for (int i = 1; i < m_nodes.size(); ++i)
+		{
+			for (int d = 0; d < 3; ++d)
+			{
+				if (m_nodes[i].m_x[d] > maxs[d])
+					maxs[d] = m_nodes[i].m_x[d];
+				if (m_nodes[i].m_x[d] < mins[d])
+					mins[d] = m_nodes[i].m_x[d];
 			}
 		}
-		else
+		const btScalar csm = getCollisionShape()->getMargin();
+		const btVector3 mrg = btVector3(csm,
+										csm,
+										csm);
+		m_bounds[0] = mins - mrg;
+		m_bounds[1] = maxs + mrg;
+		if (0 != getBroadphaseHandle())
 		{
-			m_bounds[0]=
-				m_bounds[1]=btVector3(0,0,0);
-		}		
-	//}
+			m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
+											   m_bounds[0],
+											   m_bounds[1],
+											   m_worldInfo->m_dispatcher);
+		}
+	}
+	else
+	{
+		m_bounds[0] =
+			m_bounds[1] = btVector3(0, 0, 0);
+	}
 }
 
-
 //
-void					btSoftBody::updatePose()
+void btSoftBody::updatePose()
 {
-	if(m_pose.m_bframe)
+	if (m_pose.m_bframe)
 	{
-		btSoftBody::Pose&	pose=m_pose;
-		const btVector3		com=evaluateCom();
-		/* Com			*/ 
-		pose.m_com	=	com;
-		/* Rotation		*/ 
-		btMatrix3x3		Apq;
-		const btScalar	eps=SIMD_EPSILON;
-		Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
-		Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
-		for(int i=0,ni=m_nodes.size();i<ni;++i)
+		btSoftBody::Pose& pose = m_pose;
+		const btVector3 com = evaluateCom();
+		/* Com			*/
+		pose.m_com = com;
+		/* Rotation		*/
+		btMatrix3x3 Apq;
+		const btScalar eps = SIMD_EPSILON;
+		Apq[0] = Apq[1] = Apq[2] = btVector3(0, 0, 0);
+		Apq[0].setX(eps);
+		Apq[1].setY(eps * 2);
+		Apq[2].setZ(eps * 3);
+		for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
 		{
-			const btVector3		a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
-			const btVector3&	b=pose.m_pos[i];
-			Apq[0]+=a.x()*b;
-			Apq[1]+=a.y()*b;
-			Apq[2]+=a.z()*b;
+			const btVector3 a = pose.m_wgh[i] * (m_nodes[i].m_x - com);
+			const btVector3& b = pose.m_pos[i];
+			Apq[0] += a.x() * b;
+			Apq[1] += a.y() * b;
+			Apq[2] += a.z() * b;
 		}
-		btMatrix3x3		r,s;
-		PolarDecompose(Apq,r,s);
-		pose.m_rot=r;
-		pose.m_scl=pose.m_aqq*r.transpose()*Apq;
-		if(m_cfg.maxvolume>1)
+		btMatrix3x3 r, s;
+		PolarDecompose(Apq, r, s);
+		pose.m_rot = r;
+		pose.m_scl = pose.m_aqq * r.transpose() * Apq;
+		if (m_cfg.maxvolume > 1)
 		{
-			const btScalar	idet=Clamp<btScalar>(	1/pose.m_scl.determinant(),
-				1,m_cfg.maxvolume);
-			pose.m_scl=Mul(pose.m_scl,idet);
+			const btScalar idet = Clamp<btScalar>(1 / pose.m_scl.determinant(),
+												  1, m_cfg.maxvolume);
+			pose.m_scl = Mul(pose.m_scl, idet);
 		}
-
 	}
 }
 
 //
-void				btSoftBody::updateArea(bool averageArea)
+void btSoftBody::updateArea(bool averageArea)
 {
-	int i,ni;
+	int i, ni;
 
-	/* Face area		*/ 
-	for(i=0,ni=m_faces.size();i<ni;++i)
+	/* Face area		*/
+	for (i = 0, ni = m_faces.size(); i < ni; ++i)
 	{
-		Face&		f=m_faces[i];
-		f.m_ra	=	AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
+		Face& f = m_faces[i];
+		f.m_ra = AreaOf(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x);
 	}
-	
-	/* Node area		*/ 
+
+	/* Node area		*/
 
 	if (averageArea)
 	{
-		btAlignedObjectArray<int>	counts;
-		counts.resize(m_nodes.size(),0);
-		for(i=0,ni=m_nodes.size();i<ni;++i)
+		btAlignedObjectArray<int> counts;
+		counts.resize(m_nodes.size(), 0);
+		for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 		{
-			m_nodes[i].m_area	=	0;
+			m_nodes[i].m_area = 0;
 		}
-		for(i=0,ni=m_faces.size();i<ni;++i)
+		for (i = 0, ni = m_faces.size(); i < ni; ++i)
 		{
-			btSoftBody::Face&	f=m_faces[i];
-			for(int j=0;j<3;++j)
+			btSoftBody::Face& f = m_faces[i];
+			for (int j = 0; j < 3; ++j)
 			{
-				const int index=(int)(f.m_n[j]-&m_nodes[0]);
+				const int index = (int)(f.m_n[j] - &m_nodes[0]);
 				counts[index]++;
-				f.m_n[j]->m_area+=btFabs(f.m_ra);
+				f.m_n[j]->m_area += btFabs(f.m_ra);
 			}
 		}
-		for(i=0,ni=m_nodes.size();i<ni;++i)
+		for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 		{
-			if(counts[i]>0)
-				m_nodes[i].m_area/=(btScalar)counts[i];
+			if (counts[i] > 0)
+				m_nodes[i].m_area /= (btScalar)counts[i];
 			else
-				m_nodes[i].m_area=0;
+				m_nodes[i].m_area = 0;
 		}
 	}
 	else
 	{
 		// initialize node area as zero
-		for(i=0,ni=m_nodes.size();i<ni;++i)
+		for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 		{
-			m_nodes[i].m_area=0;	
+			m_nodes[i].m_area = 0;
 		}
 
-		for(i=0,ni=m_faces.size();i<ni;++i)
+		for (i = 0, ni = m_faces.size(); i < ni; ++i)
 		{
-			btSoftBody::Face&	f=m_faces[i];
+			btSoftBody::Face& f = m_faces[i];
 
-			for(int j=0;j<3;++j)
+			for (int j = 0; j < 3; ++j)
 			{
 				f.m_n[j]->m_area += f.m_ra;
 			}
 		}
 
-		for(i=0,ni=m_nodes.size();i<ni;++i)
+		for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 		{
 			m_nodes[i].m_area *= 0.3333333f;
 		}
 	}
 }
 
+void btSoftBody::updateLinkConstants()
+{
+	int i, ni;
 
-void				btSoftBody::updateLinkConstants()
-{	
-	int i,ni;
-
-	/* Links		*/ 
-	for(i=0,ni=m_links.size();i<ni;++i)
+	/* Links		*/
+	for (i = 0, ni = m_links.size(); i < ni; ++i)
 	{
-		Link&		l=m_links[i];
-		Material&	m=*l.m_material;
-		l.m_c0	=	(l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
+		Link& l = m_links[i];
+		Material& m = *l.m_material;
+		l.m_c0 = (l.m_n[0]->m_im + l.m_n[1]->m_im) / m.m_kLST;
 	}
 }
 
-void				btSoftBody::updateConstants()
+void btSoftBody::updateConstants()
 {
 	resetLinkRestLengths();
 	updateLinkConstants();
 	updateArea();
 }
 
-
-
 //
-void					btSoftBody::initializeClusters()
+void btSoftBody::initializeClusters()
 {
 	int i;
 
-	for( i=0;i<m_clusters.size();++i)
+	for (i = 0; i < m_clusters.size(); ++i)
 	{
-		Cluster&	c=*m_clusters[i];
-		c.m_imass=0;
+		Cluster& c = *m_clusters[i];
+		c.m_imass = 0;
 		c.m_masses.resize(c.m_nodes.size());
-		for(int j=0;j<c.m_nodes.size();++j)
+		for (int j = 0; j < c.m_nodes.size(); ++j)
 		{
-			if (c.m_nodes[j]->m_im==0)
+			if (c.m_nodes[j]->m_im == 0)
 			{
 				c.m_containsAnchor = true;
-				c.m_masses[j]	=	BT_LARGE_FLOAT;
-			} else
+				c.m_masses[j] = BT_LARGE_FLOAT;
+			}
+			else
 			{
-				c.m_masses[j]	=	btScalar(1.)/c.m_nodes[j]->m_im;
+				c.m_masses[j] = btScalar(1.) / c.m_nodes[j]->m_im;
 			}
-			c.m_imass		+=	c.m_masses[j];
+			c.m_imass += c.m_masses[j];
 		}
-		c.m_imass		=	btScalar(1.)/c.m_imass;
-		c.m_com			=	btSoftBody::clusterCom(&c);
-		c.m_lv			=	btVector3(0,0,0);
-		c.m_av			=	btVector3(0,0,0);
-		c.m_leaf		=	0;
-		/* Inertia	*/ 
-		btMatrix3x3&	ii=c.m_locii;
-		ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
+		c.m_imass = btScalar(1.) / c.m_imass;
+		c.m_com = btSoftBody::clusterCom(&c);
+		c.m_lv = btVector3(0, 0, 0);
+		c.m_av = btVector3(0, 0, 0);
+		c.m_leaf = 0;
+		/* Inertia	*/
+		btMatrix3x3& ii = c.m_locii;
+		ii[0] = ii[1] = ii[2] = btVector3(0, 0, 0);
 		{
-			int i,ni;
+			int i, ni;
 
-			for(i=0,ni=c.m_nodes.size();i<ni;++i)
+			for (i = 0, ni = c.m_nodes.size(); i < ni; ++i)
 			{
-				const btVector3	k=c.m_nodes[i]->m_x-c.m_com;
-				const btVector3	q=k*k;
-				const btScalar	m=c.m_masses[i];
-				ii[0][0]	+=	m*(q[1]+q[2]);
-				ii[1][1]	+=	m*(q[0]+q[2]);
-				ii[2][2]	+=	m*(q[0]+q[1]);
-				ii[0][1]	-=	m*k[0]*k[1];
-				ii[0][2]	-=	m*k[0]*k[2];
-				ii[1][2]	-=	m*k[1]*k[2];
+				const btVector3 k = c.m_nodes[i]->m_x - c.m_com;
+				const btVector3 q = k * k;
+				const btScalar m = c.m_masses[i];
+				ii[0][0] += m * (q[1] + q[2]);
+				ii[1][1] += m * (q[0] + q[2]);
+				ii[2][2] += m * (q[0] + q[1]);
+				ii[0][1] -= m * k[0] * k[1];
+				ii[0][2] -= m * k[0] * k[2];
+				ii[1][2] -= m * k[1] * k[2];
 			}
 		}
-		ii[1][0]=ii[0][1];
-		ii[2][0]=ii[0][2];
-		ii[2][1]=ii[1][2];
-		
+		ii[1][0] = ii[0][1];
+		ii[2][0] = ii[0][2];
+		ii[2][1] = ii[1][2];
+
 		ii = ii.inverse();
 
-		/* Frame	*/ 
+		/* Frame	*/
 		c.m_framexform.setIdentity();
 		c.m_framexform.setOrigin(c.m_com);
 		c.m_framerefs.resize(c.m_nodes.size());
 		{
 			int i;
-			for(i=0;i<c.m_framerefs.size();++i)
+			for (i = 0; i < c.m_framerefs.size(); ++i)
 			{
-				c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
+				c.m_framerefs[i] = c.m_nodes[i]->m_x - c.m_com;
 			}
 		}
 	}
 }
 
 //
-void					btSoftBody::updateClusters()
+void btSoftBody::updateClusters()
 {
 	BT_PROFILE("UpdateClusters");
 	int i;
 
-	for(i=0;i<m_clusters.size();++i)
+	for (i = 0; i < m_clusters.size(); ++i)
 	{
-		btSoftBody::Cluster&	c=*m_clusters[i];
-		const int				n=c.m_nodes.size();
+		btSoftBody::Cluster& c = *m_clusters[i];
+		const int n = c.m_nodes.size();
 		//const btScalar			invn=1/(btScalar)n;
-		if(n)
-		{
-			/* Frame				*/ 
-			const btScalar	eps=btScalar(0.0001);
-			btMatrix3x3		m,r,s;
-			m[0]=m[1]=m[2]=btVector3(0,0,0);
-			m[0][0]=eps*1;
-			m[1][1]=eps*2;
-			m[2][2]=eps*3;
-			c.m_com=clusterCom(&c);
-			for(int i=0;i<c.m_nodes.size();++i)
-			{
-				const btVector3		a=c.m_nodes[i]->m_x-c.m_com;
-				const btVector3&	b=c.m_framerefs[i];
-				m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
-			}
-			PolarDecompose(m,r,s);
+		if (n)
+		{
+			/* Frame				*/
+			const btScalar eps = btScalar(0.0001);
+			btMatrix3x3 m, r, s;
+			m[0] = m[1] = m[2] = btVector3(0, 0, 0);
+			m[0][0] = eps * 1;
+			m[1][1] = eps * 2;
+			m[2][2] = eps * 3;
+			c.m_com = clusterCom(&c);
+			for (int i = 0; i < c.m_nodes.size(); ++i)
+			{
+				const btVector3 a = c.m_nodes[i]->m_x - c.m_com;
+				const btVector3& b = c.m_framerefs[i];
+				m[0] += a[0] * b;
+				m[1] += a[1] * b;
+				m[2] += a[2] * b;
+			}
+			PolarDecompose(m, r, s);
 			c.m_framexform.setOrigin(c.m_com);
-			c.m_framexform.setBasis(r);		
-			/* Inertia			*/ 
-#if 1/* Constant	*/ 
-			c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
+			c.m_framexform.setBasis(r);
+			/* Inertia			*/
+#if 1 /* Constant	*/
+			c.m_invwi = c.m_framexform.getBasis() * c.m_locii * c.m_framexform.getBasis().transpose();
 #else
-#if 0/* Sphere	*/ 
+#if 0 /* Sphere	*/ 
 			const btScalar	rk=(2*c.m_extents.length2())/(5*c.m_imass);
 			const btVector3	inertia(rk,rk,rk);
 			const btVector3	iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
@@ -2574,847 +3250,1203 @@ void					btSoftBody::updateClusters()
 				btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
 
 			c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
-#else/* Actual	*/ 		
-			c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
-			for(int i=0;i<n;++i)
-			{
-				const btVector3	k=c.m_nodes[i]->m_x-c.m_com;
-				const btVector3		q=k*k;
-				const btScalar		m=1/c.m_nodes[i]->m_im;
-				c.m_invwi[0][0]	+=	m*(q[1]+q[2]);
-				c.m_invwi[1][1]	+=	m*(q[0]+q[2]);
-				c.m_invwi[2][2]	+=	m*(q[0]+q[1]);
-				c.m_invwi[0][1]	-=	m*k[0]*k[1];
-				c.m_invwi[0][2]	-=	m*k[0]*k[2];
-				c.m_invwi[1][2]	-=	m*k[1]*k[2];
-			}
-			c.m_invwi[1][0]=c.m_invwi[0][1];
-			c.m_invwi[2][0]=c.m_invwi[0][2];
-			c.m_invwi[2][1]=c.m_invwi[1][2];
-			c.m_invwi=c.m_invwi.inverse();
+#else /* Actual	*/
+			c.m_invwi[0] = c.m_invwi[1] = c.m_invwi[2] = btVector3(0, 0, 0);
+			for (int i = 0; i < n; ++i)
+			{
+				const btVector3 k = c.m_nodes[i]->m_x - c.m_com;
+				const btVector3 q = k * k;
+				const btScalar m = 1 / c.m_nodes[i]->m_im;
+				c.m_invwi[0][0] += m * (q[1] + q[2]);
+				c.m_invwi[1][1] += m * (q[0] + q[2]);
+				c.m_invwi[2][2] += m * (q[0] + q[1]);
+				c.m_invwi[0][1] -= m * k[0] * k[1];
+				c.m_invwi[0][2] -= m * k[0] * k[2];
+				c.m_invwi[1][2] -= m * k[1] * k[2];
+			}
+			c.m_invwi[1][0] = c.m_invwi[0][1];
+			c.m_invwi[2][0] = c.m_invwi[0][2];
+			c.m_invwi[2][1] = c.m_invwi[1][2];
+			c.m_invwi = c.m_invwi.inverse();
 #endif
 #endif
-			/* Velocities			*/ 
-			c.m_lv=btVector3(0,0,0);
-			c.m_av=btVector3(0,0,0);
+			/* Velocities			*/
+			c.m_lv = btVector3(0, 0, 0);
+			c.m_av = btVector3(0, 0, 0);
 			{
 				int i;
 
-				for(i=0;i<n;++i)
+				for (i = 0; i < n; ++i)
 				{
-					const btVector3	v=c.m_nodes[i]->m_v*c.m_masses[i];
-					c.m_lv	+=	v;
-					c.m_av	+=	btCross(c.m_nodes[i]->m_x-c.m_com,v);
+					const btVector3 v = c.m_nodes[i]->m_v * c.m_masses[i];
+					c.m_lv += v;
+					c.m_av += btCross(c.m_nodes[i]->m_x - c.m_com, v);
 				}
 			}
-			c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
-			c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
-			c.m_vimpulses[0]	=
-				c.m_vimpulses[1]	= btVector3(0,0,0);
-			c.m_dimpulses[0]	=
-				c.m_dimpulses[1]	= btVector3(0,0,0);
-			c.m_nvimpulses		= 0;
-			c.m_ndimpulses		= 0;
-			/* Matching				*/ 
-			if(c.m_matching>0)
-			{
-				for(int j=0;j<c.m_nodes.size();++j)
+			c.m_lv = c.m_imass * c.m_lv * (1 - c.m_ldamping);
+			c.m_av = c.m_invwi * c.m_av * (1 - c.m_adamping);
+			c.m_vimpulses[0] =
+				c.m_vimpulses[1] = btVector3(0, 0, 0);
+			c.m_dimpulses[0] =
+				c.m_dimpulses[1] = btVector3(0, 0, 0);
+			c.m_nvimpulses = 0;
+			c.m_ndimpulses = 0;
+			/* Matching				*/
+			if (c.m_matching > 0)
+			{
+				for (int j = 0; j < c.m_nodes.size(); ++j)
 				{
-					Node&			n=*c.m_nodes[j];
-					const btVector3	x=c.m_framexform*c.m_framerefs[j];
-					n.m_x=Lerp(n.m_x,x,c.m_matching);
+					Node& n = *c.m_nodes[j];
+					const btVector3 x = c.m_framexform * c.m_framerefs[j];
+					n.m_x = Lerp(n.m_x, x, c.m_matching);
 				}
-			}			
-			/* Dbvt					*/ 
-			if(c.m_collide)
+			}
+			/* Dbvt					*/
+			if (c.m_collide)
 			{
-				btVector3	mi=c.m_nodes[0]->m_x;
-				btVector3	mx=mi;
-				for(int j=1;j<n;++j)
+				btVector3 mi = c.m_nodes[0]->m_x;
+				btVector3 mx = mi;
+				for (int j = 1; j < n; ++j)
 				{
 					mi.setMin(c.m_nodes[j]->m_x);
 					mx.setMax(c.m_nodes[j]->m_x);
-				}			
-				ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds=btDbvtVolume::FromMM(mi,mx);
-				if(c.m_leaf)
-					m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
+				}
+				ATTRIBUTE_ALIGNED16(btDbvtVolume)
+				bounds = btDbvtVolume::FromMM(mi, mx);
+				if (c.m_leaf)
+					m_cdbvt.update(c.m_leaf, bounds, c.m_lv * m_sst.sdt * 3, m_sst.radmrg);
 				else
-					c.m_leaf=m_cdbvt.insert(bounds,&c);
+					c.m_leaf = m_cdbvt.insert(bounds, &c);
 			}
 		}
 	}
-
-
 }
 
-
-
-
 //
-void					btSoftBody::cleanupClusters()
+void btSoftBody::cleanupClusters()
 {
-	for(int i=0;i<m_joints.size();++i)
+	for (int i = 0; i < m_joints.size(); ++i)
 	{
 		m_joints[i]->Terminate(m_sst.sdt);
-		if(m_joints[i]->m_delete)
+		if (m_joints[i]->m_delete)
 		{
 			btAlignedFree(m_joints[i]);
 			m_joints.remove(m_joints[i--]);
-		}	
+		}
 	}
 }
 
 //
-void					btSoftBody::prepareClusters(int iterations)
+void btSoftBody::prepareClusters(int iterations)
 {
-	for(int i=0;i<m_joints.size();++i)
+	for (int i = 0; i < m_joints.size(); ++i)
 	{
-		m_joints[i]->Prepare(m_sst.sdt,iterations);
+		m_joints[i]->Prepare(m_sst.sdt, iterations);
 	}
 }
 
-
 //
-void					btSoftBody::solveClusters(btScalar sor)
+void btSoftBody::solveClusters(btScalar sor)
 {
-	for(int i=0,ni=m_joints.size();i<ni;++i)
+	for (int i = 0, ni = m_joints.size(); i < ni; ++i)
 	{
-		m_joints[i]->Solve(m_sst.sdt,sor);
+		m_joints[i]->Solve(m_sst.sdt, sor);
 	}
 }
 
 //
-void					btSoftBody::applyClusters(bool drift)
+void btSoftBody::applyClusters(bool drift)
 {
 	BT_PROFILE("ApplyClusters");
-//	const btScalar					f0=m_sst.sdt;
+	//	const btScalar					f0=m_sst.sdt;
 	//const btScalar					f1=f0/2;
 	btAlignedObjectArray<btVector3> deltas;
 	btAlignedObjectArray<btScalar> weights;
-	deltas.resize(m_nodes.size(),btVector3(0,0,0));
-	weights.resize(m_nodes.size(),0);
+	deltas.resize(m_nodes.size(), btVector3(0, 0, 0));
+	weights.resize(m_nodes.size(), 0);
 	int i;
 
-	if(drift)
+	if (drift)
 	{
-		for(i=0;i<m_clusters.size();++i)
+		for (i = 0; i < m_clusters.size(); ++i)
 		{
-			Cluster&	c=*m_clusters[i];
-			if(c.m_ndimpulses)
+			Cluster& c = *m_clusters[i];
+			if (c.m_ndimpulses)
 			{
-				c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
-				c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
+				c.m_dimpulses[0] /= (btScalar)c.m_ndimpulses;
+				c.m_dimpulses[1] /= (btScalar)c.m_ndimpulses;
 			}
 		}
 	}
-	
-	for(i=0;i<m_clusters.size();++i)
+
+	for (i = 0; i < m_clusters.size(); ++i)
 	{
-		Cluster&	c=*m_clusters[i];	
-		if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
+		Cluster& c = *m_clusters[i];
+		if (0 < (drift ? c.m_ndimpulses : c.m_nvimpulses))
 		{
-			const btVector3		v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
-			const btVector3		w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
-			for(int j=0;j<c.m_nodes.size();++j)
+			const btVector3 v = (drift ? c.m_dimpulses[0] : c.m_vimpulses[0]) * m_sst.sdt;
+			const btVector3 w = (drift ? c.m_dimpulses[1] : c.m_vimpulses[1]) * m_sst.sdt;
+			for (int j = 0; j < c.m_nodes.size(); ++j)
 			{
-				const int			idx=int(c.m_nodes[j]-&m_nodes[0]);
-				const btVector3&	x=c.m_nodes[j]->m_x;
-				const btScalar		q=c.m_masses[j];
-				deltas[idx]		+=	(v+btCross(w,x-c.m_com))*q;
-				weights[idx]	+=	q;
+				const int idx = int(c.m_nodes[j] - &m_nodes[0]);
+				const btVector3& x = c.m_nodes[j]->m_x;
+				const btScalar q = c.m_masses[j];
+				deltas[idx] += (v + btCross(w, x - c.m_com)) * q;
+				weights[idx] += q;
 			}
 		}
 	}
-	for(i=0;i<deltas.size();++i)
+	for (i = 0; i < deltas.size(); ++i)
 	{
-		if(weights[i]>0) 
+		if (weights[i] > 0)
 		{
-			m_nodes[i].m_x+=deltas[i]/weights[i];
+			m_nodes[i].m_x += deltas[i] / weights[i];
 		}
 	}
 }
 
 //
-void					btSoftBody::dampClusters()
+void btSoftBody::dampClusters()
 {
 	int i;
 
-	for(i=0;i<m_clusters.size();++i)
+	for (i = 0; i < m_clusters.size(); ++i)
 	{
-		Cluster&	c=*m_clusters[i];	
-		if(c.m_ndamping>0)
+		Cluster& c = *m_clusters[i];
+		if (c.m_ndamping > 0)
 		{
-			for(int j=0;j<c.m_nodes.size();++j)
+			for (int j = 0; j < c.m_nodes.size(); ++j)
 			{
-				Node&			n=*c.m_nodes[j];
-				if(n.m_im>0)
+				Node& n = *c.m_nodes[j];
+				if (n.m_im > 0)
 				{
-					const btVector3	vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
-					if(vx.length2()<=n.m_v.length2())
-						{
-						n.m_v	+=	c.m_ndamping*(vx-n.m_v);
-						}
+					const btVector3 vx = c.m_lv + btCross(c.m_av, c.m_nodes[j]->m_q - c.m_com);
+					if (vx.length2() <= n.m_v.length2())
+					{
+						n.m_v += c.m_ndamping * (vx - n.m_v);
+					}
 				}
 			}
 		}
 	}
 }
 
+void btSoftBody::setSpringStiffness(btScalar k)
+{
+	for (int i = 0; i < m_links.size(); ++i)
+	{
+		m_links[i].Feature::m_material->m_kLST = k;
+	}
+	m_repulsionStiffness = k;
+}
+
+void btSoftBody::setGravityFactor(btScalar gravFactor)
+{
+	m_gravityFactor = gravFactor;
+}
+
+void btSoftBody::initializeDmInverse()
+{
+	btScalar unit_simplex_measure = 1. / 6.;
+
+	for (int i = 0; i < m_tetras.size(); ++i)
+	{
+		Tetra& t = m_tetras[i];
+		btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x;
+		btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x;
+		btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x;
+		btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(),
+					   c1.getY(), c2.getY(), c3.getY(),
+					   c1.getZ(), c2.getZ(), c3.getZ());
+		t.m_element_measure = Dm.determinant() * unit_simplex_measure;
+		t.m_Dm_inverse = Dm.inverse();
+
+		// calculate the first three columns of P^{-1}
+		btVector3 a = t.m_n[0]->m_x;
+		btVector3 b = t.m_n[1]->m_x;
+		btVector3 c = t.m_n[2]->m_x;
+		btVector3 d = t.m_n[3]->m_x;
+
+		btScalar det = 1 / (a[0] * b[1] * c[2] - a[0] * b[1] * d[2] - a[0] * b[2] * c[1] + a[0] * b[2] * d[1] + a[0] * c[1] * d[2] - a[0] * c[2] * d[1] + a[1] * (-b[0] * c[2] + b[0] * d[2] + b[2] * c[0] - b[2] * d[0] - c[0] * d[2] + c[2] * d[0]) + a[2] * (b[0] * c[1] - b[0] * d[1] + b[1] * (d[0] - c[0]) + c[0] * d[1] - c[1] * d[0]) - b[0] * c[1] * d[2] + b[0] * c[2] * d[1] + b[1] * c[0] * d[2] - b[1] * c[2] * d[0] - b[2] * c[0] * d[1] + b[2] * c[1] * d[0]);
+
+		btScalar P11 = -b[2] * c[1] + d[2] * c[1] + b[1] * c[2] + b[2] * d[1] - c[2] * d[1] - b[1] * d[2];
+		btScalar P12 = b[2] * c[0] - d[2] * c[0] - b[0] * c[2] - b[2] * d[0] + c[2] * d[0] + b[0] * d[2];
+		btScalar P13 = -b[1] * c[0] + d[1] * c[0] + b[0] * c[1] + b[1] * d[0] - c[1] * d[0] - b[0] * d[1];
+		btScalar P21 = a[2] * c[1] - d[2] * c[1] - a[1] * c[2] - a[2] * d[1] + c[2] * d[1] + a[1] * d[2];
+		btScalar P22 = -a[2] * c[0] + d[2] * c[0] + a[0] * c[2] + a[2] * d[0] - c[2] * d[0] - a[0] * d[2];
+		btScalar P23 = a[1] * c[0] - d[1] * c[0] - a[0] * c[1] - a[1] * d[0] + c[1] * d[0] + a[0] * d[1];
+		btScalar P31 = -a[2] * b[1] + d[2] * b[1] + a[1] * b[2] + a[2] * d[1] - b[2] * d[1] - a[1] * d[2];
+		btScalar P32 = a[2] * b[0] - d[2] * b[0] - a[0] * b[2] - a[2] * d[0] + b[2] * d[0] + a[0] * d[2];
+		btScalar P33 = -a[1] * b[0] + d[1] * b[0] + a[0] * b[1] + a[1] * d[0] - b[1] * d[0] - a[0] * d[1];
+		btScalar P41 = a[2] * b[1] - c[2] * b[1] - a[1] * b[2] - a[2] * c[1] + b[2] * c[1] + a[1] * c[2];
+		btScalar P42 = -a[2] * b[0] + c[2] * b[0] + a[0] * b[2] + a[2] * c[0] - b[2] * c[0] - a[0] * c[2];
+		btScalar P43 = a[1] * b[0] - c[1] * b[0] - a[0] * b[1] - a[1] * c[0] + b[1] * c[0] + a[0] * c[1];
+
+		btVector4 p1(P11 * det, P21 * det, P31 * det, P41 * det);
+		btVector4 p2(P12 * det, P22 * det, P32 * det, P42 * det);
+		btVector4 p3(P13 * det, P23 * det, P33 * det, P43 * det);
+
+		t.m_P_inv[0] = p1;
+		t.m_P_inv[1] = p2;
+		t.m_P_inv[2] = p3;
+	}
+}
+
+static btScalar Dot4(const btVector4& a, const btVector4& b)
+{
+	return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
+}
+
+void btSoftBody::updateDeformation()
+{
+	btQuaternion q;
+	for (int i = 0; i < m_tetras.size(); ++i)
+	{
+		btSoftBody::Tetra& t = m_tetras[i];
+		btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q;
+		btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q;
+		btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q;
+		btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(),
+					   c1.getY(), c2.getY(), c3.getY(),
+					   c1.getZ(), c2.getZ(), c3.getZ());
+		t.m_F = Ds * t.m_Dm_inverse;
+
+		btSoftBody::TetraScratch& s = m_tetraScratches[i];
+		s.m_F = t.m_F;
+		s.m_J = t.m_F.determinant();
+		btMatrix3x3 C = t.m_F.transpose() * t.m_F;
+		s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ();
+		s.m_cofF = t.m_F.adjoint().transpose();
+
+		btVector3 a = t.m_n[0]->m_q;
+		btVector3 b = t.m_n[1]->m_q;
+		btVector3 c = t.m_n[2]->m_q;
+		btVector3 d = t.m_n[3]->m_q;
+		btVector4 q1(a[0], b[0], c[0], d[0]);
+		btVector4 q2(a[1], b[1], c[1], d[1]);
+		btVector4 q3(a[2], b[2], c[2], d[2]);
+		btMatrix3x3 B(Dot4(q1, t.m_P_inv[0]), Dot4(q1, t.m_P_inv[1]), Dot4(q1, t.m_P_inv[2]),
+					  Dot4(q2, t.m_P_inv[0]), Dot4(q2, t.m_P_inv[1]), Dot4(q2, t.m_P_inv[2]),
+					  Dot4(q3, t.m_P_inv[0]), Dot4(q3, t.m_P_inv[1]), Dot4(q3, t.m_P_inv[2]));
+		q.setRotation(btVector3(0, 0, 1), 0);
+		B.extractRotation(q, 0.01);  // precision of the rotation is not very important for visual correctness.
+		btMatrix3x3 Q(q);
+		s.m_corotation = Q;
+	}
+}
+
+void btSoftBody::advanceDeformation()
+{
+	updateDeformation();
+	for (int i = 0; i < m_tetras.size(); ++i)
+	{
+		m_tetraScratchesTn[i] = m_tetraScratches[i];
+	}
+}
 //
-void				btSoftBody::Joint::Prepare(btScalar dt,int)
+void btSoftBody::Joint::Prepare(btScalar dt, int)
 {
 	m_bodies[0].activate();
 	m_bodies[1].activate();
 }
 
 //
-void				btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
+void btSoftBody::LJoint::Prepare(btScalar dt, int iterations)
 {
-	static const btScalar	maxdrift=4;
-	Joint::Prepare(dt,iterations);
-	m_rpos[0]		=	m_bodies[0].xform()*m_refs[0];
-	m_rpos[1]		=	m_bodies[1].xform()*m_refs[1];
-	m_drift			=	Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
-	m_rpos[0]		-=	m_bodies[0].xform().getOrigin();
-	m_rpos[1]		-=	m_bodies[1].xform().getOrigin();
-	m_massmatrix	=	ImpulseMatrix(	m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
-		m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
-	if(m_split>0)
+	static const btScalar maxdrift = 4;
+	Joint::Prepare(dt, iterations);
+	m_rpos[0] = m_bodies[0].xform() * m_refs[0];
+	m_rpos[1] = m_bodies[1].xform() * m_refs[1];
+	m_drift = Clamp(m_rpos[0] - m_rpos[1], maxdrift) * m_erp / dt;
+	m_rpos[0] -= m_bodies[0].xform().getOrigin();
+	m_rpos[1] -= m_bodies[1].xform().getOrigin();
+	m_massmatrix = ImpulseMatrix(m_bodies[0].invMass(), m_bodies[0].invWorldInertia(), m_rpos[0],
+								 m_bodies[1].invMass(), m_bodies[1].invWorldInertia(), m_rpos[1]);
+	if (m_split > 0)
 	{
-		m_sdrift	=	m_massmatrix*(m_drift*m_split);
-		m_drift		*=	1-m_split;
+		m_sdrift = m_massmatrix * (m_drift * m_split);
+		m_drift *= 1 - m_split;
 	}
-	m_drift	/=(btScalar)iterations;
+	m_drift /= (btScalar)iterations;
 }
 
 //
-void				btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
+void btSoftBody::LJoint::Solve(btScalar dt, btScalar sor)
 {
-	const btVector3		va=m_bodies[0].velocity(m_rpos[0]);
-	const btVector3		vb=m_bodies[1].velocity(m_rpos[1]);
-	const btVector3		vr=va-vb;
-	btSoftBody::Impulse	impulse;
-	impulse.m_asVelocity	=	1;
-	impulse.m_velocity		=	m_massmatrix*(m_drift+vr*m_cfm)*sor;
-	m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
-	m_bodies[1].applyImpulse( impulse,m_rpos[1]);
+	const btVector3 va = m_bodies[0].velocity(m_rpos[0]);
+	const btVector3 vb = m_bodies[1].velocity(m_rpos[1]);
+	const btVector3 vr = va - vb;
+	btSoftBody::Impulse impulse;
+	impulse.m_asVelocity = 1;
+	impulse.m_velocity = m_massmatrix * (m_drift + vr * m_cfm) * sor;
+	m_bodies[0].applyImpulse(-impulse, m_rpos[0]);
+	m_bodies[1].applyImpulse(impulse, m_rpos[1]);
 }
 
 //
-void				btSoftBody::LJoint::Terminate(btScalar dt)
+void btSoftBody::LJoint::Terminate(btScalar dt)
 {
-	if(m_split>0)
+	if (m_split > 0)
 	{
-		m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
-		m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
+		m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]);
+		m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]);
 	}
 }
 
 //
-void				btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
+void btSoftBody::AJoint::Prepare(btScalar dt, int iterations)
 {
-	static const btScalar	maxdrift=SIMD_PI/16;
+	static const btScalar maxdrift = SIMD_PI / 16;
 	m_icontrol->Prepare(this);
-	Joint::Prepare(dt,iterations);
-	m_axis[0]	=	m_bodies[0].xform().getBasis()*m_refs[0];
-	m_axis[1]	=	m_bodies[1].xform().getBasis()*m_refs[1];
-	m_drift		=	NormalizeAny(btCross(m_axis[1],m_axis[0]));
-	m_drift		*=	btMin(maxdrift,btAcos(Clamp<btScalar>(btDot(m_axis[0],m_axis[1]),-1,+1)));
-	m_drift		*=	m_erp/dt;
-	m_massmatrix=	AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
-	if(m_split>0)
+	Joint::Prepare(dt, iterations);
+	m_axis[0] = m_bodies[0].xform().getBasis() * m_refs[0];
+	m_axis[1] = m_bodies[1].xform().getBasis() * m_refs[1];
+	m_drift = NormalizeAny(btCross(m_axis[1], m_axis[0]));
+	m_drift *= btMin(maxdrift, btAcos(Clamp<btScalar>(btDot(m_axis[0], m_axis[1]), -1, +1)));
+	m_drift *= m_erp / dt;
+	m_massmatrix = AngularImpulseMatrix(m_bodies[0].invWorldInertia(), m_bodies[1].invWorldInertia());
+	if (m_split > 0)
 	{
-		m_sdrift	=	m_massmatrix*(m_drift*m_split);
-		m_drift		*=	1-m_split;
+		m_sdrift = m_massmatrix * (m_drift * m_split);
+		m_drift *= 1 - m_split;
 	}
-	m_drift	/=(btScalar)iterations;
+	m_drift /= (btScalar)iterations;
 }
 
 //
-void				btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
+void btSoftBody::AJoint::Solve(btScalar dt, btScalar sor)
 {
-	const btVector3		va=m_bodies[0].angularVelocity();
-	const btVector3		vb=m_bodies[1].angularVelocity();
-	const btVector3		vr=va-vb;
-	const btScalar		sp=btDot(vr,m_axis[0]);
-	const btVector3		vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
-	btSoftBody::Impulse	impulse;
-	impulse.m_asVelocity	=	1;
-	impulse.m_velocity		=	m_massmatrix*(m_drift+vc*m_cfm)*sor;
+	const btVector3 va = m_bodies[0].angularVelocity();
+	const btVector3 vb = m_bodies[1].angularVelocity();
+	const btVector3 vr = va - vb;
+	const btScalar sp = btDot(vr, m_axis[0]);
+	const btVector3 vc = vr - m_axis[0] * m_icontrol->Speed(this, sp);
+	btSoftBody::Impulse impulse;
+	impulse.m_asVelocity = 1;
+	impulse.m_velocity = m_massmatrix * (m_drift + vc * m_cfm) * sor;
 	m_bodies[0].applyAImpulse(-impulse);
-	m_bodies[1].applyAImpulse( impulse);
+	m_bodies[1].applyAImpulse(impulse);
 }
 
 //
-void				btSoftBody::AJoint::Terminate(btScalar dt)
+void btSoftBody::AJoint::Terminate(btScalar dt)
 {
-	if(m_split>0)
+	if (m_split > 0)
 	{
 		m_bodies[0].applyDAImpulse(-m_sdrift);
-		m_bodies[1].applyDAImpulse( m_sdrift);
+		m_bodies[1].applyDAImpulse(m_sdrift);
 	}
 }
 
 //
-void				btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
+void btSoftBody::CJoint::Prepare(btScalar dt, int iterations)
 {
-	Joint::Prepare(dt,iterations);
-	const bool	dodrift=(m_life==0);
-	m_delete=(++m_life)>m_maxlife;
-	if(dodrift)
+	Joint::Prepare(dt, iterations);
+	const bool dodrift = (m_life == 0);
+	m_delete = (++m_life) > m_maxlife;
+	if (dodrift)
 	{
-		m_drift=m_drift*m_erp/dt;
-		if(m_split>0)
+		m_drift = m_drift * m_erp / dt;
+		if (m_split > 0)
 		{
-			m_sdrift	=	m_massmatrix*(m_drift*m_split);
-			m_drift		*=	1-m_split;
+			m_sdrift = m_massmatrix * (m_drift * m_split);
+			m_drift *= 1 - m_split;
 		}
-		m_drift/=(btScalar)iterations;
+		m_drift /= (btScalar)iterations;
 	}
 	else
 	{
-		m_drift=m_sdrift=btVector3(0,0,0);
+		m_drift = m_sdrift = btVector3(0, 0, 0);
 	}
 }
 
 //
-void				btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
+void btSoftBody::CJoint::Solve(btScalar dt, btScalar sor)
 {
-	const btVector3		va=m_bodies[0].velocity(m_rpos[0]);
-	const btVector3		vb=m_bodies[1].velocity(m_rpos[1]);
-	const btVector3		vrel=va-vb;
-	const btScalar		rvac=btDot(vrel,m_normal);
-	btSoftBody::Impulse	impulse;
-	impulse.m_asVelocity	=	1;
-	impulse.m_velocity		=	m_drift;
-	if(rvac<0)
+	const btVector3 va = m_bodies[0].velocity(m_rpos[0]);
+	const btVector3 vb = m_bodies[1].velocity(m_rpos[1]);
+	const btVector3 vrel = va - vb;
+	const btScalar rvac = btDot(vrel, m_normal);
+	btSoftBody::Impulse impulse;
+	impulse.m_asVelocity = 1;
+	impulse.m_velocity = m_drift;
+	if (rvac < 0)
 	{
-		const btVector3	iv=m_normal*rvac;
-		const btVector3	fv=vrel-iv;
-		impulse.m_velocity	+=	iv+fv*m_friction;
+		const btVector3 iv = m_normal * rvac;
+		const btVector3 fv = vrel - iv;
+		impulse.m_velocity += iv + fv * m_friction;
 	}
-	impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
-	
-	if (m_bodies[0].m_soft==m_bodies[1].m_soft)
+	impulse.m_velocity = m_massmatrix * impulse.m_velocity * sor;
+
+	if (m_bodies[0].m_soft == m_bodies[1].m_soft)
 	{
-		if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&&
-			(impulse.m_velocity.getZ() ==impulse.m_velocity.getZ()))
+		if ((impulse.m_velocity.getX() == impulse.m_velocity.getX()) && (impulse.m_velocity.getY() == impulse.m_velocity.getY()) &&
+			(impulse.m_velocity.getZ() == impulse.m_velocity.getZ()))
 		{
 			if (impulse.m_asVelocity)
 			{
-				if (impulse.m_velocity.length() <m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
+				if (impulse.m_velocity.length() < m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
 				{
-					
-				} else
+				}
+				else
 				{
-					m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]);
-					m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]);
+					m_bodies[0].applyImpulse(-impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[0]);
+					m_bodies[1].applyImpulse(impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[1]);
 				}
 			}
 		}
-	} else
+	}
+	else
 	{
-		m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
-		m_bodies[1].applyImpulse( impulse,m_rpos[1]);
+		m_bodies[0].applyImpulse(-impulse, m_rpos[0]);
+		m_bodies[1].applyImpulse(impulse, m_rpos[1]);
 	}
 }
 
 //
-void				btSoftBody::CJoint::Terminate(btScalar dt)
+void btSoftBody::CJoint::Terminate(btScalar dt)
 {
-	if(m_split>0)
+	if (m_split > 0)
 	{
-		m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
-		m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
+		m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]);
+		m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]);
 	}
 }
 
 //
-void				btSoftBody::applyForces()
+void btSoftBody::applyForces()
 {
-
 	BT_PROFILE("SoftBody applyForces");
-//	const btScalar					dt =			m_sst.sdt;
-	const btScalar					kLF =			m_cfg.kLF;
-	const btScalar					kDG =			m_cfg.kDG;
-	const btScalar					kPR =			m_cfg.kPR;
-	const btScalar					kVC =			m_cfg.kVC;
-	const bool						as_lift =		kLF>0;
-	const bool						as_drag =		kDG>0;
-	const bool						as_pressure =	kPR!=0;
-	const bool						as_volume =		kVC>0;
-	const bool						as_aero =		as_lift	||
-													as_drag		;
+	//	const btScalar					dt =			m_sst.sdt;
+	const btScalar kLF = m_cfg.kLF;
+	const btScalar kDG = m_cfg.kDG;
+	const btScalar kPR = m_cfg.kPR;
+	const btScalar kVC = m_cfg.kVC;
+	const bool as_lift = kLF > 0;
+	const bool as_drag = kDG > 0;
+	const bool as_pressure = kPR != 0;
+	const bool as_volume = kVC > 0;
+	const bool as_aero = as_lift ||
+						 as_drag;
 	//const bool						as_vaero =		as_aero	&&
 	//												(m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
 	//const bool						as_faero =		as_aero	&&
 	//												(m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
-	const bool						use_medium =	as_aero;
-	const bool						use_volume =	as_pressure	||
-		as_volume	;
-	btScalar						volume =		0;
-	btScalar						ivolumetp =		0;
-	btScalar						dvolumetv =		0;
-	btSoftBody::sMedium	medium;
-	if(use_volume)
+	const bool use_medium = as_aero;
+	const bool use_volume = as_pressure ||
+							as_volume;
+	btScalar volume = 0;
+	btScalar ivolumetp = 0;
+	btScalar dvolumetv = 0;
+	btSoftBody::sMedium medium;
+	if (use_volume)
 	{
-		volume		=	getVolume();
-		ivolumetp	=	1/btFabs(volume)*kPR;
-		dvolumetv	=	(m_pose.m_volume-volume)*kVC;
+		volume = getVolume();
+		ivolumetp = 1 / btFabs(volume) * kPR;
+		dvolumetv = (m_pose.m_volume - volume) * kVC;
 	}
-	/* Per vertex forces			*/ 
-	int i,ni;
+	/* Per vertex forces			*/
+	int i, ni;
 
-	for(i=0,ni=m_nodes.size();i<ni;++i)
+	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
 	{
-		btSoftBody::Node&	n=m_nodes[i];
-		if(n.m_im>0)
+		btSoftBody::Node& n = m_nodes[i];
+		if (n.m_im > 0)
 		{
-			if(use_medium)
+			if (use_medium)
 			{
-				/* Aerodynamics			*/ 
+				/* Aerodynamics			*/
 				addAeroForceToNode(m_windVelocity, i);
 			}
-			/* Pressure				*/ 
-			if(as_pressure)
+			/* Pressure				*/
+			if (as_pressure)
 			{
-				n.m_f	+=	n.m_n*(n.m_area*ivolumetp);
+				n.m_f += n.m_n * (n.m_area * ivolumetp);
 			}
-			/* Volume				*/ 
-			if(as_volume)
+			/* Volume				*/
+			if (as_volume)
 			{
-				n.m_f	+=	n.m_n*(n.m_area*dvolumetv);
+				n.m_f += n.m_n * (n.m_area * dvolumetv);
 			}
 		}
 	}
 
-	/* Per face forces				*/ 
-	for(i=0,ni=m_faces.size();i<ni;++i)
+	/* Per face forces				*/
+	for (i = 0, ni = m_faces.size(); i < ni; ++i)
 	{
-	//	btSoftBody::Face&	f=m_faces[i];
+		//	btSoftBody::Face&	f=m_faces[i];
+
+		/* Aerodynamics			*/
+		addAeroForceToFace(m_windVelocity, i);
+	}
+}
+
+//
+void btSoftBody::setMaxStress(btScalar maxStress)
+{
+	m_cfg.m_maxStress = maxStress;
+}
+
+//
+void btSoftBody::interpolateRenderMesh()
+{
+	if (m_z.size() > 0)
+	{
+		for (int i = 0; i < m_renderNodes.size(); ++i)
+		{
+			const Node* p0 = m_renderNodesParents[i][0];
+			const Node* p1 = m_renderNodesParents[i][1];
+			const Node* p2 = m_renderNodesParents[i][2];
+			btVector3 normal = btCross(p1->m_x - p0->m_x, p2->m_x - p0->m_x);
+			btVector3 unit_normal = normal.normalized();
+			Node& n = m_renderNodes[i];
+			n.m_x.setZero();
+			for (int j = 0; j < 3; ++j)
+			{
+				n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j];
+			}
+			n.m_x += m_z[i] * unit_normal;
+		}
+	}
+	else
+	{
+		for (int i = 0; i < m_renderNodes.size(); ++i)
+		{
+			Node& n = m_renderNodes[i];
+			n.m_x.setZero();
+			for (int j = 0; j < 4; ++j)
+			{
+				if (m_renderNodesParents[i].size())
+				{
+					n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j];
+				}
+			}
+		}
+	}
+}
 
-		/* Aerodynamics			*/ 
-		addAeroForceToFace(m_windVelocity, i);	
+void btSoftBody::setCollisionQuadrature(int N)
+{
+	for (int i = 0; i <= N; ++i)
+	{
+		for (int j = 0; i + j <= N; ++j)
+		{
+			m_quads.push_back(btVector3(btScalar(i) / btScalar(N), btScalar(j) / btScalar(N), btScalar(N - i - j) / btScalar(N)));
+		}
 	}
 }
 
 //
-void				btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
+void btSoftBody::PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti)
 {
-	const btScalar	kAHR=psb->m_cfg.kAHR*kst;
-	const btScalar	dt=psb->m_sst.sdt;
-	for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
+	BT_PROFILE("PSolve_Anchors");
+	const btScalar kAHR = psb->m_cfg.kAHR * kst;
+	const btScalar dt = psb->m_sst.sdt;
+	for (int i = 0, ni = psb->m_anchors.size(); i < ni; ++i)
 	{
-		const Anchor&		a=psb->m_anchors[i];
-		const btTransform&	t=a.m_body->getWorldTransform();
-		Node&				n=*a.m_node;
-		const btVector3		wa=t*a.m_local;
-		const btVector3		va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
-		const btVector3		vb=n.m_x-n.m_q;
-		const btVector3		vr=(va-vb)+(wa-n.m_x)*kAHR;
-		const btVector3		impulse=a.m_c0*vr*a.m_influence;
-		n.m_x+=impulse*a.m_c2;
-		a.m_body->applyImpulse(-impulse,a.m_c1);
+		const Anchor& a = psb->m_anchors[i];
+		const btTransform& t = a.m_body->getWorldTransform();
+		Node& n = *a.m_node;
+		const btVector3 wa = t * a.m_local;
+		const btVector3 va = a.m_body->getVelocityInLocalPoint(a.m_c1) * dt;
+		const btVector3 vb = n.m_x - n.m_q;
+		const btVector3 vr = (va - vb) + (wa - n.m_x) * kAHR;
+		const btVector3 impulse = a.m_c0 * vr * a.m_influence;
+		n.m_x += impulse * a.m_c2;
+		a.m_body->applyImpulse(-impulse, a.m_c1);
 	}
 }
 
 //
 void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti)
 {
-	const btScalar	dt = psb->m_sst.sdt;
-	const btScalar	mrg = psb->getCollisionShape()->getMargin();
-	for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
+	BT_PROFILE("PSolve_RContacts");
+	const btScalar dt = psb->m_sst.sdt;
+	const btScalar mrg = psb->getCollisionShape()->getMargin();
+	btMultiBodyJacobianData jacobianData;
+	for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i)
 	{
-		const RContact&		c = psb->m_rcontacts[i];
-		const sCti&			cti = c.m_cti;	
-		if (cti.m_colObj->hasContactResponse()) 
+		const RContact& c = psb->m_rcontacts[i];
+		const sCti& cti = c.m_cti;
+		if (cti.m_colObj->hasContactResponse())
 		{
-			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)
+			btVector3 va(0, 0, 0);
+			btRigidBody* rigidCol = 0;
+			btMultiBodyLinkCollider* multibodyLinkCol = 0;
+			btScalar* deltaV;
+
+			if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+			{
+				rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+				va = rigidCol ? rigidCol->getVelocityInLocalPoint(c.m_c1) * dt : btVector3(0, 0, 0);
+			}
+			else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+			{
+				multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+				if (multibodyLinkCol)
+				{
+					const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+					jacobianData.m_jacobians.resize(ndof);
+					jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
+					btScalar* jac = &jacobianData.m_jacobians[0];
+
+					multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
+					deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
+					multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
+
+					btScalar vel = 0.0;
+					for (int j = 0; j < ndof; ++j)
+					{
+						vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
+					}
+					va = cti.m_normal * vel * dt;
+				}
+			}
+
+			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);
+				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 );
+				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);
+
+				if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+				{
+					if (rigidCol)
+						rigidCol->applyImpulse(impulse, c.m_c1);
+				}
+				else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+				{
+					if (multibodyLinkCol)
+					{
+						double multiplier = 0.5;
+						multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
+					}
+				}
 			}
 		}
 	}
 }
 
 //
-void				btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
+void btSoftBody::PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti)
 {
-	for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
+	BT_PROFILE("PSolve_SContacts");
+
+	for (int i = 0, ni = psb->m_scontacts.size(); i < ni; ++i)
 	{
-		const SContact&		c=psb->m_scontacts[i];
-		const btVector3&	nr=c.m_normal;
-		Node&				n=*c.m_node;
-		Face&				f=*c.m_face;
-		const btVector3		p=BaryEval(	f.m_n[0]->m_x,
-			f.m_n[1]->m_x,
-			f.m_n[2]->m_x,
-			c.m_weights);
-		const btVector3		q=BaryEval(	f.m_n[0]->m_q,
-			f.m_n[1]->m_q,
-			f.m_n[2]->m_q,
-			c.m_weights);											
-		const btVector3		vr=(n.m_x-n.m_q)-(p-q);
-		btVector3			corr(0,0,0);
-		btScalar dot = btDot(vr,nr);
-		if(dot<0)
+		const SContact& c = psb->m_scontacts[i];
+		const btVector3& nr = c.m_normal;
+		Node& n = *c.m_node;
+		Face& f = *c.m_face;
+		const btVector3 p = BaryEval(f.m_n[0]->m_x,
+									 f.m_n[1]->m_x,
+									 f.m_n[2]->m_x,
+									 c.m_weights);
+		const btVector3 q = BaryEval(f.m_n[0]->m_q,
+									 f.m_n[1]->m_q,
+									 f.m_n[2]->m_q,
+									 c.m_weights);
+		const btVector3 vr = (n.m_x - n.m_q) - (p - q);
+		btVector3 corr(0, 0, 0);
+		btScalar dot = btDot(vr, nr);
+		if (dot < 0)
 		{
-			const btScalar	j=c.m_margin-(btDot(nr,n.m_x)-btDot(nr,p));
-			corr+=c.m_normal*j;
+			const btScalar j = c.m_margin - (btDot(nr, n.m_x) - btDot(nr, p));
+			corr += c.m_normal * j;
 		}
-		corr			-=	ProjectOnPlane(vr,nr)*c.m_friction;
-		n.m_x			+=	corr*c.m_cfm[0];
-		f.m_n[0]->m_x	-=	corr*(c.m_cfm[1]*c.m_weights.x());
-		f.m_n[1]->m_x	-=	corr*(c.m_cfm[1]*c.m_weights.y());
-		f.m_n[2]->m_x	-=	corr*(c.m_cfm[1]*c.m_weights.z());
+		corr -= ProjectOnPlane(vr, nr) * c.m_friction;
+		n.m_x += corr * c.m_cfm[0];
+		f.m_n[0]->m_x -= corr * (c.m_cfm[1] * c.m_weights.x());
+		f.m_n[1]->m_x -= corr * (c.m_cfm[1] * c.m_weights.y());
+		f.m_n[2]->m_x -= corr * (c.m_cfm[1] * c.m_weights.z());
 	}
 }
 
 //
-void				btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
+void btSoftBody::PSolve_Links(btSoftBody* psb, btScalar kst, btScalar ti)
 {
-	for(int i=0,ni=psb->m_links.size();i<ni;++i)
-	{			
-		Link&	l=psb->m_links[i];
-		if(l.m_c0>0)
+	BT_PROFILE("PSolve_Links");
+	for (int i = 0, ni = psb->m_links.size(); i < ni; ++i)
+	{
+		Link& l = psb->m_links[i];
+		if (l.m_c0 > 0)
 		{
-			Node&			a=*l.m_n[0];
-			Node&			b=*l.m_n[1];
-			const btVector3	del=b.m_x-a.m_x;
-			const btScalar	len=del.length2();
-			if (l.m_c1+len > SIMD_EPSILON)
+			Node& a = *l.m_n[0];
+			Node& b = *l.m_n[1];
+			const btVector3 del = b.m_x - a.m_x;
+			const btScalar len = del.length2();
+			if (l.m_c1 + len > SIMD_EPSILON)
 			{
-				const btScalar	k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
-				a.m_x-=del*(k*a.m_im);
-				b.m_x+=del*(k*b.m_im);
+				const btScalar k = ((l.m_c1 - len) / (l.m_c0 * (l.m_c1 + len))) * kst;
+				a.m_x -= del * (k * a.m_im);
+				b.m_x += del * (k * b.m_im);
 			}
 		}
 	}
 }
 
 //
-void				btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
+void btSoftBody::VSolve_Links(btSoftBody* psb, btScalar kst)
 {
-	for(int i=0,ni=psb->m_links.size();i<ni;++i)
-	{			
-		Link&			l=psb->m_links[i];
-		Node**			n=l.m_n;
-		const btScalar	j=-btDot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
-		n[0]->m_v+=	l.m_c3*(j*n[0]->m_im);
-		n[1]->m_v-=	l.m_c3*(j*n[1]->m_im);
+	BT_PROFILE("VSolve_Links");
+	for (int i = 0, ni = psb->m_links.size(); i < ni; ++i)
+	{
+		Link& l = psb->m_links[i];
+		Node** n = l.m_n;
+		const btScalar j = -btDot(l.m_c3, n[0]->m_v - n[1]->m_v) * l.m_c2 * kst;
+		n[0]->m_v += l.m_c3 * (j * n[0]->m_im);
+		n[1]->m_v -= l.m_c3 * (j * n[1]->m_im);
 	}
 }
 
 //
-btSoftBody::psolver_t	btSoftBody::getSolver(ePSolver::_ solver)
+btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver)
 {
-	switch(solver)
+	switch (solver)
 	{
-	case	ePSolver::Anchors:		
-		return(&btSoftBody::PSolve_Anchors);
-	case	ePSolver::Linear:		
-		return(&btSoftBody::PSolve_Links);
-	case	ePSolver::RContacts:	
-		return(&btSoftBody::PSolve_RContacts);
-	case	ePSolver::SContacts:	
-		return(&btSoftBody::PSolve_SContacts);	
+		case ePSolver::Anchors:
+			return (&btSoftBody::PSolve_Anchors);
+		case ePSolver::Linear:
+			return (&btSoftBody::PSolve_Links);
+		case ePSolver::RContacts:
+			return (&btSoftBody::PSolve_RContacts);
+		case ePSolver::SContacts:
+			return (&btSoftBody::PSolve_SContacts);
 		default:
 		{
 		}
 	}
-	return(0);
+	return (0);
 }
 
 //
-btSoftBody::vsolver_t	btSoftBody::getSolver(eVSolver::_ solver)
+btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver)
 {
-	switch(solver)
+	switch (solver)
 	{
-	case	eVSolver::Linear:		return(&btSoftBody::VSolve_Links);
+		case eVSolver::Linear:
+			return (&btSoftBody::VSolve_Links);
 		default:
 		{
 		}
 	}
-	return(0);
+	return (0);
 }
 
-//
-void			btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap)
+void btSoftBody::setSelfCollision(bool useSelfCollision)
+{
+	m_useSelfCollision = useSelfCollision;
+}
+
+bool btSoftBody::useSelfCollision()
 {
+	return m_useSelfCollision;
+}
 
-	switch(m_cfg.collisions&fCollision::RVSmask)
+//
+void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap)
+{
+	switch (m_cfg.collisions & fCollision::RVSmask)
 	{
-	case	fCollision::SDF_RS:
+		case fCollision::SDF_RS:
 		{
-			btSoftColliders::CollideSDF_RS	docollide;		
-			btRigidBody*		prb1=(btRigidBody*) btRigidBody::upcast(pcoWrap->getCollisionObject());
-			btTransform	wtr=pcoWrap->getWorldTransform();
-
-			const btTransform	ctr=pcoWrap->getWorldTransform();
-			const btScalar		timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
-			const btScalar		basemargin=getCollisionShape()->getMargin();
-			btVector3			mins;
-			btVector3			maxs;
-			ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume;
-			pcoWrap->getCollisionShape()->getAabb(	pcoWrap->getWorldTransform(),
-				mins,
-				maxs);
-			volume=btDbvtVolume::FromMM(mins,maxs);
-			volume.Expand(btVector3(basemargin,basemargin,basemargin));		
-			docollide.psb		=	this;
+			btSoftColliders::CollideSDF_RS docollide;
+			btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
+			btTransform wtr = pcoWrap->getWorldTransform();
+
+			const btTransform ctr = pcoWrap->getWorldTransform();
+			const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
+			const btScalar basemargin = getCollisionShape()->getMargin();
+			btVector3 mins;
+			btVector3 maxs;
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)
+			volume;
+			pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(),
+												  mins,
+												  maxs);
+			volume = btDbvtVolume::FromMM(mins, maxs);
+			volume.Expand(btVector3(basemargin, basemargin, basemargin));
+			docollide.psb = this;
 			docollide.m_colObj1Wrap = pcoWrap;
 			docollide.m_rigidBody = prb1;
 
-			docollide.dynmargin	=	basemargin+timemargin;
-			docollide.stamargin	=	basemargin;
-			m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide);
+			docollide.dynmargin = basemargin + timemargin;
+			docollide.stamargin = basemargin;
+			m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide);
 		}
 		break;
-	case	fCollision::CL_RS:
+		case fCollision::CL_RS:
 		{
-			btSoftColliders::CollideCL_RS	collider;
-			collider.ProcessColObj(this,pcoWrap);
+			btSoftColliders::CollideCL_RS collider;
+			collider.ProcessColObj(this, pcoWrap);
+		}
+		break;
+		case fCollision::SDF_RD:
+		{
+			btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
+			if (pcoWrap->getCollisionObject()->isActive() || this->isActive())
+			{
+				const btTransform wtr = pcoWrap->getWorldTransform();
+				const btScalar timemargin = 0;
+				const btScalar basemargin = getCollisionShape()->getMargin();
+				btVector3 mins;
+				btVector3 maxs;
+				ATTRIBUTE_ALIGNED16(btDbvtVolume)
+				volume;
+				pcoWrap->getCollisionShape()->getAabb(wtr,
+													  mins,
+													  maxs);
+				volume = btDbvtVolume::FromMM(mins, maxs);
+				volume.Expand(btVector3(basemargin, basemargin, basemargin));
+				if (m_cfg.collisions & fCollision::SDF_RDN)
+				{
+					btSoftColliders::CollideSDF_RD docollideNode;
+					docollideNode.psb = this;
+					docollideNode.m_colObj1Wrap = pcoWrap;
+					docollideNode.m_rigidBody = prb1;
+					docollideNode.dynmargin = basemargin + timemargin;
+					docollideNode.stamargin = basemargin;
+					m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
+				}
+
+				if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF)))
+				{
+					btSoftColliders::CollideSDF_RDF docollideFace;
+					docollideFace.psb = this;
+					docollideFace.m_colObj1Wrap = pcoWrap;
+					docollideFace.m_rigidBody = prb1;
+					docollideFace.dynmargin = basemargin + timemargin;
+					docollideFace.stamargin = basemargin;
+					m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
+				}
+			}
 		}
 		break;
 	}
 }
 
 //
-void			btSoftBody::defaultCollisionHandler(btSoftBody* psb)
+void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 {
-	const int cf=m_cfg.collisions&psb->m_cfg.collisions;
-	switch(cf&fCollision::SVSmask)
+	BT_PROFILE("Deformable Collision");
+	const int cf = m_cfg.collisions & psb->m_cfg.collisions;
+	switch (cf & fCollision::SVSmask)
 	{
-	case	fCollision::CL_SS:
+		case fCollision::CL_SS:
 		{
-			
 			//support self-collision if CL_SELF flag set
-			if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF)
+			if (this != psb || psb->m_cfg.collisions & fCollision::CL_SELF)
 			{
-				btSoftColliders::CollideCL_SS	docollide;
-				docollide.ProcessSoftSoft(this,psb);
+				btSoftColliders::CollideCL_SS docollide;
+				docollide.ProcessSoftSoft(this, psb);
 			}
-			
 		}
 		break;
-	case	fCollision::VF_SS:
+		case fCollision::VF_SS:
 		{
 			//only self-collision for Cluster, not Vertex-Face yet
-			if (this!=psb)
-			{
-				btSoftColliders::CollideVF_SS	docollide;
-				/* common					*/ 
-				docollide.mrg=	getCollisionShape()->getMargin()+
-					psb->getCollisionShape()->getMargin();
-				/* psb0 nodes vs psb1 faces	*/ 
-				docollide.psb[0]=this;
-				docollide.psb[1]=psb;
-				docollide.psb[0]->m_ndbvt.collideTT(	docollide.psb[0]->m_ndbvt.m_root,
-					docollide.psb[1]->m_fdbvt.m_root,
-					docollide);
-				/* psb1 nodes vs psb0 faces	*/ 
-				docollide.psb[0]=psb;
-				docollide.psb[1]=this;
-				docollide.psb[0]->m_ndbvt.collideTT(	docollide.psb[0]->m_ndbvt.m_root,
-					docollide.psb[1]->m_fdbvt.m_root,
-					docollide);
+			if (this != psb)
+			{
+				btSoftColliders::CollideVF_SS docollide;
+				/* common					*/
+				docollide.mrg = getCollisionShape()->getMargin() +
+								psb->getCollisionShape()->getMargin();
+				/* psb0 nodes vs psb1 faces	*/
+				docollide.psb[0] = this;
+				docollide.psb[1] = psb;
+				docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+													docollide.psb[1]->m_fdbvt.m_root,
+													docollide);
+				/* psb1 nodes vs psb0 faces	*/
+				docollide.psb[0] = psb;
+				docollide.psb[1] = this;
+				docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+													docollide.psb[1]->m_fdbvt.m_root,
+													docollide);
 			}
 		}
 		break;
-	default:
+		case fCollision::VF_DD:
+		{
+			if (!psb->m_softSoftCollision)
+				return;
+			if (psb->isActive() || this->isActive())
+			{
+				if (this != psb)
+				{
+					btSoftColliders::CollideVF_DD docollide;
+					/* common                    */
+					docollide.mrg = getCollisionShape()->getMargin() +
+									psb->getCollisionShape()->getMargin();
+					/* psb0 nodes vs psb1 faces    */
+					if (psb->m_tetras.size() > 0)
+						docollide.useFaceNormal = true;
+					else
+						docollide.useFaceNormal = false;
+					docollide.psb[0] = this;
+					docollide.psb[1] = psb;
+					docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+														docollide.psb[1]->m_fdbvt.m_root,
+														docollide);
+
+					/* psb1 nodes vs psb0 faces    */
+					if (this->m_tetras.size() > 0)
+						docollide.useFaceNormal = true;
+					else
+						docollide.useFaceNormal = false;
+					docollide.psb[0] = psb;
+					docollide.psb[1] = this;
+					docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+														docollide.psb[1]->m_fdbvt.m_root,
+														docollide);
+				}
+				else
+				{
+					if (psb->useSelfCollision())
+					{
+						btSoftColliders::CollideFF_DD docollide;
+						docollide.mrg = 2 * getCollisionShape()->getMargin();
+						docollide.psb[0] = this;
+						docollide.psb[1] = psb;
+						if (this->m_tetras.size() > 0)
+							docollide.useFaceNormal = true;
+						else
+							docollide.useFaceNormal = false;
+						/* psb0 faces vs psb0 faces    */
+						calculateNormalCone(this->m_fdbvnt);
+						this->m_fdbvt.selfCollideT(m_fdbvnt, docollide);
+					}
+				}
+			}
+		}
+		break;
+		default:
 		{
-			
 		}
 	}
 }
 
+void btSoftBody::geometricCollisionHandler(btSoftBody* psb)
+{
+	if (psb->isActive() || this->isActive())
+	{
+		if (this != psb)
+		{
+			btSoftColliders::CollideCCD docollide;
+			/* common                    */
+			docollide.mrg = SAFE_EPSILON;  // for rounding error instead of actual margin
+			docollide.dt = psb->m_sst.sdt;
+			/* psb0 nodes vs psb1 faces    */
+			if (psb->m_tetras.size() > 0)
+				docollide.useFaceNormal = true;
+			else
+				docollide.useFaceNormal = false;
+			docollide.psb[0] = this;
+			docollide.psb[1] = psb;
+			docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+												docollide.psb[1]->m_fdbvt.m_root,
+												docollide);
+			/* psb1 nodes vs psb0 faces    */
+			if (this->m_tetras.size() > 0)
+				docollide.useFaceNormal = true;
+			else
+				docollide.useFaceNormal = false;
+			docollide.psb[0] = psb;
+			docollide.psb[1] = this;
+			docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+												docollide.psb[1]->m_fdbvt.m_root,
+												docollide);
+		}
+		else
+		{
+			if (psb->useSelfCollision())
+			{
+				btSoftColliders::CollideCCD docollide;
+				docollide.mrg = SAFE_EPSILON;
+				docollide.psb[0] = this;
+				docollide.psb[1] = psb;
+				docollide.dt = psb->m_sst.sdt;
+				if (this->m_tetras.size() > 0)
+					docollide.useFaceNormal = true;
+				else
+					docollide.useFaceNormal = false;
+				/* psb0 faces vs psb0 faces    */
+				calculateNormalCone(this->m_fdbvnt);  // should compute this outside of this scope
+				this->m_fdbvt.selfCollideT(m_fdbvnt, docollide);
+			}
+		}
+	}
+}
 
-
-void btSoftBody::setWindVelocity( const btVector3 &velocity )
+void btSoftBody::setWindVelocity(const btVector3& velocity)
 {
 	m_windVelocity = velocity;
 }
 
-
 const btVector3& btSoftBody::getWindVelocity()
 {
 	return m_windVelocity;
 }
 
-
-
-int	btSoftBody::calculateSerializeBufferSize()	const
+int btSoftBody::calculateSerializeBufferSize() const
 {
 	int sz = sizeof(btSoftBodyData);
 	return sz;
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const
 {
-	btSoftBodyData* sbd = (btSoftBodyData*) dataBuffer;
+	btSoftBodyData* sbd = (btSoftBodyData*)dataBuffer;
 
 	btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer);
 
-	btHashMap<btHashPtr,int>	m_nodeIndexMap;
+	btHashMap<btHashPtr, int> m_nodeIndexMap;
 
 	sbd->m_numMaterials = m_materials.size();
-	sbd->m_materials = sbd->m_numMaterials? (SoftBodyMaterialData**) serializer->getUniquePointer((void*)&m_materials): 0;
+	sbd->m_materials = sbd->m_numMaterials ? (SoftBodyMaterialData**)serializer->getUniquePointer((void*)&m_materials) : 0;
 
 	if (sbd->m_materials)
 	{
 		int sz = sizeof(SoftBodyMaterialData*);
 		int numElem = sbd->m_numMaterials;
-		btChunk* chunk = serializer->allocate(sz,numElem);
+		btChunk* chunk = serializer->allocate(sz, numElem);
 		//SoftBodyMaterialData** memPtr = chunk->m_oldPtr;
 		SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
 			btSoftBody::Material* mat = m_materials[i];
 			*memPtr = mat ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)mat) : 0;
 			if (!serializer->findPointer(mat))
 			{
 				//serialize it here
-				btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData),1);
+				btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData), 1);
 				SoftBodyMaterialData* memPtr = (SoftBodyMaterialData*)chunk->m_oldPtr;
 				memPtr->m_flags = mat->m_flags;
 				memPtr->m_angularStiffness = mat->m_kAST;
 				memPtr->m_linearStiffness = mat->m_kLST;
 				memPtr->m_volumeStiffness = mat->m_kVST;
-				serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_SBMATERIAL_CODE,mat);
+				serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_SBMATERIAL_CODE, mat);
 			}
 		}
-		serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_ARRAY_CODE,(void*) &m_materials);
+		serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_ARRAY_CODE, (void*)&m_materials);
 	}
 
-
-	
-
 	sbd->m_numNodes = m_nodes.size();
-	sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes): 0;
+	sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes) : 0;
 	if (sbd->m_nodes)
 	{
 		int sz = sizeof(SoftBodyNodeData);
 		int numElem = sbd->m_numNodes;
-		btChunk* chunk = serializer->allocate(sz,numElem);
+		btChunk* chunk = serializer->allocate(sz, numElem);
 		SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
-			m_nodes[i].m_f.serializeFloat( memPtr->m_accumulatedForce);
+			m_nodes[i].m_f.serializeFloat(memPtr->m_accumulatedForce);
 			memPtr->m_area = m_nodes[i].m_area;
 			memPtr->m_attach = m_nodes[i].m_battach;
 			memPtr->m_inverseMass = m_nodes[i].m_im;
-			memPtr->m_material = m_nodes[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_nodes[i].m_material):0;
+			memPtr->m_material = m_nodes[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_nodes[i].m_material) : 0;
 			m_nodes[i].m_n.serializeFloat(memPtr->m_normal);
 			m_nodes[i].m_x.serializeFloat(memPtr->m_position);
 			m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition);
 			m_nodes[i].m_v.serializeFloat(memPtr->m_velocity);
-			m_nodeIndexMap.insert(&m_nodes[i],i);
+			m_nodeIndexMap.insert(&m_nodes[i], i);
 		}
-		serializer->finalizeChunk(chunk,"SoftBodyNodeData",BT_SBNODE_CODE,(void*) &m_nodes);
+		serializer->finalizeChunk(chunk, "SoftBodyNodeData", BT_SBNODE_CODE, (void*)&m_nodes);
 	}
 
 	sbd->m_numLinks = m_links.size();
-	sbd->m_links = sbd->m_numLinks? (SoftBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
+	sbd->m_links = sbd->m_numLinks ? (SoftBodyLinkData*)serializer->getUniquePointer((void*)&m_links[0]) : 0;
 	if (sbd->m_links)
 	{
 		int sz = sizeof(SoftBodyLinkData);
 		int numElem = sbd->m_numLinks;
-		btChunk* chunk = serializer->allocate(sz,numElem);
+		btChunk* chunk = serializer->allocate(sz, numElem);
 		SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
 			memPtr->m_bbending = m_links[i].m_bbending;
-			memPtr->m_material = m_links[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_links[i].m_material):0;
-			memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0]: -1;
-			memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0]: -1;
-			btAssert(memPtr->m_nodeIndices[0]<m_nodes.size());
-			btAssert(memPtr->m_nodeIndices[1]<m_nodes.size());
+			memPtr->m_material = m_links[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_links[i].m_material) : 0;
+			memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0] : -1;
+			memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0] : -1;
+			btAssert(memPtr->m_nodeIndices[0] < m_nodes.size());
+			btAssert(memPtr->m_nodeIndices[1] < m_nodes.size());
 			memPtr->m_restLength = m_links[i].m_rl;
 		}
-		serializer->finalizeChunk(chunk,"SoftBodyLinkData",BT_ARRAY_CODE,(void*) &m_links[0]);
-
+		serializer->finalizeChunk(chunk, "SoftBodyLinkData", BT_ARRAY_CODE, (void*)&m_links[0]);
 	}
 
-
 	sbd->m_numFaces = m_faces.size();
-	sbd->m_faces = sbd->m_numFaces? (SoftBodyFaceData*) serializer->getUniquePointer((void*)&m_faces[0]):0;
+	sbd->m_faces = sbd->m_numFaces ? (SoftBodyFaceData*)serializer->getUniquePointer((void*)&m_faces[0]) : 0;
 	if (sbd->m_faces)
 	{
 		int sz = sizeof(SoftBodyFaceData);
 		int numElem = sbd->m_numFaces;
-		btChunk* chunk = serializer->allocate(sz,numElem);
+		btChunk* chunk = serializer->allocate(sz, numElem);
 		SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
-			memPtr->m_material = m_faces[i].m_material ?  (SoftBodyMaterialData*) serializer->getUniquePointer((void*)m_faces[i].m_material): 0;
-			m_faces[i].m_normal.serializeFloat(	memPtr->m_normal);
-			for (int j=0;j<3;j++)
+			memPtr->m_material = m_faces[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_faces[i].m_material) : 0;
+			m_faces[i].m_normal.serializeFloat(memPtr->m_normal);
+			for (int j = 0; j < 3; j++)
 			{
-				memPtr->m_nodeIndices[j] = m_faces[i].m_n[j]? m_faces[i].m_n[j] - &m_nodes[0]: -1;
+				memPtr->m_nodeIndices[j] = m_faces[i].m_n[j] ? m_faces[i].m_n[j] - &m_nodes[0] : -1;
 			}
 			memPtr->m_restArea = m_faces[i].m_ra;
 		}
-		serializer->finalizeChunk(chunk,"SoftBodyFaceData",BT_ARRAY_CODE,(void*) &m_faces[0]);
+		serializer->finalizeChunk(chunk, "SoftBodyFaceData", BT_ARRAY_CODE, (void*)&m_faces[0]);
 	}
 
-
 	sbd->m_numTetrahedra = m_tetras.size();
-	sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*) serializer->getUniquePointer((void*)&m_tetras[0]):0;
+	sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*)serializer->getUniquePointer((void*)&m_tetras[0]) : 0;
 	if (sbd->m_tetrahedra)
 	{
 		int sz = sizeof(SoftBodyTetraData);
 		int numElem = sbd->m_numTetrahedra;
-		btChunk* chunk = serializer->allocate(sz,numElem);
+		btChunk* chunk = serializer->allocate(sz, numElem);
 		SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
-			for (int j=0;j<4;j++)
+			for (int j = 0; j < 4; j++)
 			{
-				m_tetras[i].m_c0[j].serializeFloat(	memPtr->m_c0[j] );
-				memPtr->m_nodeIndices[j] = m_tetras[j].m_n[j]? m_tetras[j].m_n[j]-&m_nodes[0] : -1;
+				m_tetras[i].m_c0[j].serializeFloat(memPtr->m_c0[j]);
+				memPtr->m_nodeIndices[j] = m_tetras[i].m_n[j] ? m_tetras[i].m_n[j] - &m_nodes[0] : -1;
 			}
 			memPtr->m_c1 = m_tetras[i].m_c1;
 			memPtr->m_c2 = m_tetras[i].m_c2;
-			memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_tetras[i].m_material): 0;
+			memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_tetras[i].m_material) : 0;
 			memPtr->m_restVolume = m_tetras[i].m_rv;
 		}
-		serializer->finalizeChunk(chunk,"SoftBodyTetraData",BT_ARRAY_CODE,(void*) &m_tetras[0]);
+		serializer->finalizeChunk(chunk, "SoftBodyTetraData", BT_ARRAY_CODE, (void*)&m_tetras[0]);
 	}
 
 	sbd->m_numAnchors = m_anchors.size();
-	sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*) serializer->getUniquePointer((void*)&m_anchors[0]):0;
+	sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*)serializer->getUniquePointer((void*)&m_anchors[0]) : 0;
 	if (sbd->m_anchors)
 	{
 		int sz = sizeof(SoftRigidAnchorData);
 		int numElem = sbd->m_numAnchors;
-		btChunk* chunk = serializer->allocate(sz,numElem);
+		btChunk* chunk = serializer->allocate(sz, numElem);
 		SoftRigidAnchorData* memPtr = (SoftRigidAnchorData*)chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
 			m_anchors[i].m_c0.serializeFloat(memPtr->m_c0);
 			m_anchors[i].m_c1.serializeFloat(memPtr->m_c1);
 			memPtr->m_c2 = m_anchors[i].m_c2;
 			m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame);
-			memPtr->m_nodeIndex = m_anchors[i].m_node? m_anchors[i].m_node-&m_nodes[0]: -1;
-			
-			memPtr->m_rigidBody = m_anchors[i].m_body? (btRigidBodyData*)  serializer->getUniquePointer((void*)m_anchors[i].m_body): 0;
+			memPtr->m_nodeIndex = m_anchors[i].m_node ? m_anchors[i].m_node - &m_nodes[0] : -1;
+
+			memPtr->m_rigidBody = m_anchors[i].m_body ? (btRigidBodyData*)serializer->getUniquePointer((void*)m_anchors[i].m_body) : 0;
 			btAssert(memPtr->m_nodeIndex < m_nodes.size());
 		}
-		serializer->finalizeChunk(chunk,"SoftRigidAnchorData",BT_ARRAY_CODE,(void*) &m_anchors[0]);
+		serializer->finalizeChunk(chunk, "SoftRigidAnchorData", BT_ARRAY_CODE, (void*)&m_anchors[0]);
 	}
-	
 
 	sbd->m_config.m_dynamicFriction = m_cfg.kDF;
 	sbd->m_config.m_baumgarte = m_cfg.kVCF;
@@ -3449,64 +4481,63 @@ const char*	btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ
 		sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose);
 
 		int sz = sizeof(SoftBodyPoseData);
-		btChunk* chunk = serializer->allocate(sz,1);
+		btChunk* chunk = serializer->allocate(sz, 1);
 		SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr;
-		
+
 		m_pose.m_aqq.serializeFloat(memPtr->m_aqq);
 		memPtr->m_bframe = m_pose.m_bframe;
 		memPtr->m_bvolume = m_pose.m_bvolume;
 		m_pose.m_com.serializeFloat(memPtr->m_com);
-		
+
 		memPtr->m_numPositions = m_pose.m_pos.size();
-		memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]): 0;
+		memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]) : 0;
 		if (memPtr->m_numPositions)
 		{
 			int numElem = memPtr->m_numPositions;
 			int sz = sizeof(btVector3Data);
-			btChunk* chunk = serializer->allocate(sz,numElem);
+			btChunk* chunk = serializer->allocate(sz, numElem);
 			btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr;
-			for (int i=0;i<numElem;i++,memPtr++)
+			for (int i = 0; i < numElem; i++, memPtr++)
 			{
 				m_pose.m_pos[i].serializeFloat(*memPtr);
 			}
-			serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_pose.m_pos[0]);
+			serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_pose.m_pos[0]);
 		}
 		memPtr->m_restVolume = m_pose.m_volume;
 		m_pose.m_rot.serializeFloat(memPtr->m_rot);
 		m_pose.m_scl.serializeFloat(memPtr->m_scale);
 
 		memPtr->m_numWeigts = m_pose.m_wgh.size();
-		memPtr->m_weights = memPtr->m_numWeigts? (float*) serializer->getUniquePointer((void*) &m_pose.m_wgh[0]) : 0;
+		memPtr->m_weights = memPtr->m_numWeigts ? (float*)serializer->getUniquePointer((void*)&m_pose.m_wgh[0]) : 0;
 		if (memPtr->m_numWeigts)
 		{
-			
 			int numElem = memPtr->m_numWeigts;
 			int sz = sizeof(float);
-			btChunk* chunk = serializer->allocate(sz,numElem);
-			float* memPtr = (float*) chunk->m_oldPtr;
-			for (int i=0;i<numElem;i++,memPtr++)
+			btChunk* chunk = serializer->allocate(sz, numElem);
+			float* memPtr = (float*)chunk->m_oldPtr;
+			for (int i = 0; i < numElem; i++, memPtr++)
 			{
 				*memPtr = m_pose.m_wgh[i];
 			}
-			serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_pose.m_wgh[0]);
+			serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_pose.m_wgh[0]);
 		}
 
-		serializer->finalizeChunk(chunk,"SoftBodyPoseData",BT_ARRAY_CODE,(void*)&m_pose);
+		serializer->finalizeChunk(chunk, "SoftBodyPoseData", BT_ARRAY_CODE, (void*)&m_pose);
 	}
 
 	//clusters for convex-cluster collision detection
 
 	sbd->m_numClusters = m_clusters.size();
-	sbd->m_clusters = sbd->m_numClusters? (SoftBodyClusterData*) serializer->getUniquePointer((void*)m_clusters[0]) : 0;
+	sbd->m_clusters = sbd->m_numClusters ? (SoftBodyClusterData*)serializer->getUniquePointer((void*)m_clusters[0]) : 0;
 	if (sbd->m_numClusters)
 	{
 		int numElem = sbd->m_numClusters;
 		int sz = sizeof(SoftBodyClusterData);
-		btChunk* chunk = serializer->allocate(sz,numElem);
-		SoftBodyClusterData* memPtr = (SoftBodyClusterData*) chunk->m_oldPtr;
-		for (int i=0;i<numElem;i++,memPtr++)
+		btChunk* chunk = serializer->allocate(sz, numElem);
+		SoftBodyClusterData* memPtr = (SoftBodyClusterData*)chunk->m_oldPtr;
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
-			memPtr->m_adamping= m_clusters[i]->m_adamping;
+			memPtr->m_adamping = m_clusters[i]->m_adamping;
 			m_clusters[i]->m_av.serializeFloat(memPtr->m_av);
 			memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex;
 			memPtr->m_collide = m_clusters[i]->m_collide;
@@ -3537,69 +4568,64 @@ const char*	btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ
 			m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]);
 			memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses;
 
-			
-
-			memPtr->m_framerefs = memPtr->m_numFrameRefs? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0;
+			memPtr->m_framerefs = memPtr->m_numFrameRefs ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0;
 			if (memPtr->m_framerefs)
 			{
 				int numElem = memPtr->m_numFrameRefs;
 				int sz = sizeof(btVector3FloatData);
-				btChunk* chunk = serializer->allocate(sz,numElem);
-				btVector3FloatData* memPtr = (btVector3FloatData*) chunk->m_oldPtr;
-				for (int j=0;j<numElem;j++,memPtr++)
+				btChunk* chunk = serializer->allocate(sz, numElem);
+				btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr;
+				for (int j = 0; j < numElem; j++, memPtr++)
 				{
 					m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr);
 				}
-				serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_framerefs[0]);
+				serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_framerefs[0]);
 			}
-			
-			memPtr->m_masses = memPtr->m_numMasses ? (float*) serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]): 0;
+
+			memPtr->m_masses = memPtr->m_numMasses ? (float*)serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]) : 0;
 			if (memPtr->m_masses)
 			{
 				int numElem = memPtr->m_numMasses;
 				int sz = sizeof(float);
-				btChunk* chunk = serializer->allocate(sz,numElem);
-				float* memPtr = (float*) chunk->m_oldPtr;
-				for (int j=0;j<numElem;j++,memPtr++)
+				btChunk* chunk = serializer->allocate(sz, numElem);
+				float* memPtr = (float*)chunk->m_oldPtr;
+				for (int j = 0; j < numElem; j++, memPtr++)
 				{
 					*memPtr = m_clusters[i]->m_masses[j];
 				}
-				serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_masses[0]);
+				serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_masses[0]);
 			}
 
-			memPtr->m_nodeIndices  = memPtr->m_numNodes ? (int*) serializer->getUniquePointer((void*) &m_clusters[i]->m_nodes) : 0;
-			if (memPtr->m_nodeIndices )
+			memPtr->m_nodeIndices = memPtr->m_numNodes ? (int*)serializer->getUniquePointer((void*)&m_clusters[i]->m_nodes) : 0;
+			if (memPtr->m_nodeIndices)
 			{
 				int numElem = memPtr->m_numMasses;
 				int sz = sizeof(int);
-				btChunk* chunk = serializer->allocate(sz,numElem);
-				int* memPtr = (int*) chunk->m_oldPtr;
-				for (int j=0;j<numElem;j++,memPtr++)
+				btChunk* chunk = serializer->allocate(sz, numElem);
+				int* memPtr = (int*)chunk->m_oldPtr;
+				for (int j = 0; j < numElem; j++, memPtr++)
 				{
 					int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]);
 					btAssert(indexPtr);
 					*memPtr = *indexPtr;
 				}
-				serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_nodes);
+				serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_nodes);
 			}
 		}
-		serializer->finalizeChunk(chunk,"SoftBodyClusterData",BT_ARRAY_CODE,(void*)m_clusters[0]);
-
+		serializer->finalizeChunk(chunk, "SoftBodyClusterData", BT_ARRAY_CODE, (void*)m_clusters[0]);
 	}
-	
 
-	
 	sbd->m_numJoints = m_joints.size();
-	sbd->m_joints = m_joints.size()? (btSoftBodyJointData*) serializer->getUniquePointer((void*)&m_joints[0]) : 0;
+	sbd->m_joints = m_joints.size() ? (btSoftBodyJointData*)serializer->getUniquePointer((void*)&m_joints[0]) : 0;
 
 	if (sbd->m_joints)
 	{
 		int sz = sizeof(btSoftBodyJointData);
 		int numElem = m_joints.size();
-		btChunk* chunk = serializer->allocate(sz,numElem);
+		btChunk* chunk = serializer->allocate(sz, numElem);
 		btSoftBodyJointData* memPtr = (btSoftBodyJointData*)chunk->m_oldPtr;
 
-		for (int i=0;i<numElem;i++,memPtr++)
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
 			memPtr->m_jointType = (int)m_joints[i]->Type();
 			m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]);
@@ -3608,8 +4634,8 @@ const char*	btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ
 			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++)
+
+			for (int j = 0; j < 4; j++)
 			{
 				memPtr->m_relPosition[0].m_floats[j] = 0.f;
 				memPtr->m_relPosition[1].m_floats[j] = 0.f;
@@ -3648,10 +4674,51 @@ const char*	btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ
 				memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid);
 			}
 		}
-		serializer->finalizeChunk(chunk,"btSoftBodyJointData",BT_ARRAY_CODE,(void*) &m_joints[0]);
+		serializer->finalizeChunk(chunk, "btSoftBodyJointData", BT_ARRAY_CODE, (void*)&m_joints[0]);
 	}
 
-
 	return btSoftBodyDataName;
 }
 
+void btSoftBody::updateDeactivation(btScalar timeStep)
+{
+	if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
+		return;
+
+	if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold)
+	{
+		m_deactivationTime += timeStep;
+	}
+	else
+	{
+		m_deactivationTime = btScalar(0.);
+		setActivationState(0);
+	}
+}
+
+void btSoftBody::setZeroVelocity()
+{
+	for (int i = 0; i < m_nodes.size(); ++i)
+	{
+		m_nodes[i].m_v.setZero();
+	}
+}
+
+bool btSoftBody::wantsSleeping()
+{
+	if (getActivationState() == DISABLE_DEACTIVATION)
+		return false;
+
+	//disable deactivation
+	if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
+		return false;
+
+	if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
+		return true;
+
+	if (m_deactivationTime > gDeactivationTime)
+	{
+		return true;
+	}
+	return false;
+}
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.h b/extern/bullet2/src/BulletSoftBody/btSoftBody.h
index bd5846bfb67..efe3f5f3cdc 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftBody.h
+++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.h
@@ -20,870 +20,1067 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btVector3.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
 #include "BulletCollision/CollisionShapes/btConcaveShape.h"
 #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
 #include "btSparseSDF.h"
 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
-
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 //#ifdef BT_USE_DOUBLE_PRECISION
 //#define btRigidBodyData	btRigidBodyDoubleData
 //#define btRigidBodyDataName	"btRigidBodyDoubleData"
 //#else
-#define btSoftBodyData	btSoftBodyFloatData
-#define btSoftBodyDataName	"btSoftBodyFloatData"
+#define btSoftBodyData btSoftBodyFloatData
+#define btSoftBodyDataName "btSoftBodyFloatData"
+static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1;
+static unsigned long seed = 243703;
 //#endif //BT_USE_DOUBLE_PRECISION
 
 class btBroadphaseInterface;
 class btDispatcher;
 class btSoftBodySolver;
 
-/* btSoftBodyWorldInfo	*/ 
-struct	btSoftBodyWorldInfo
+/* btSoftBodyWorldInfo	*/
+struct btSoftBodyWorldInfo
 {
-	btScalar				air_density;
-	btScalar				water_density;
-	btScalar				water_offset;
-	btScalar				m_maxDisplacement;
-	btVector3				water_normal;
-	btBroadphaseInterface*	m_broadphase;
-	btDispatcher*	m_dispatcher;
-	btVector3				m_gravity;
-	btSparseSdf<3>			m_sparsesdf;
+	btScalar air_density;
+	btScalar water_density;
+	btScalar water_offset;
+	btScalar m_maxDisplacement;
+	btVector3 water_normal;
+	btBroadphaseInterface* m_broadphase;
+	btDispatcher* m_dispatcher;
+	btVector3 m_gravity;
+	btSparseSdf<3> m_sparsesdf;
 
 	btSoftBodyWorldInfo()
-		:air_density((btScalar)1.2),
-		water_density(0),
-		water_offset(0),
-		m_maxDisplacement(1000.f),//avoid soft body from 'exploding' so use some upper threshold of maximum motion that a node can travel per frame
-		water_normal(0,0,0),
-		m_broadphase(0),
-		m_dispatcher(0),
-		m_gravity(0,-10,0)
+		: air_density((btScalar)1.2),
+		  water_density(0),
+		  water_offset(0),
+		  m_maxDisplacement(1000.f),  //avoid soft body from 'exploding' so use some upper threshold of maximum motion that a node can travel per frame
+		  water_normal(0, 0, 0),
+		  m_broadphase(0),
+		  m_dispatcher(0),
+		  m_gravity(0, -10, 0)
 	{
 	}
-};	
-
+};
 
-///The btSoftBody is an class to simulate cloth and volumetric soft bodies. 
+///The btSoftBody is an class to simulate cloth and volumetric soft bodies.
 ///There is two-way interaction between btSoftBody and btRigidBody/btCollisionObject.
-class	btSoftBody : public btCollisionObject
+class btSoftBody : public btCollisionObject
 {
 public:
 	btAlignedObjectArray<const class btCollisionObject*> m_collisionDisabledObjects;
 
 	// The solver object that handles this soft body
-	btSoftBodySolver *m_softBodySolver;
+	btSoftBodySolver* m_softBodySolver;
 
 	//
 	// Enumerations
 	//
 
-	///eAeroModel 
-	struct eAeroModel { enum _ {
-		V_Point,			///Vertex normals are oriented toward velocity
-		V_TwoSided,			///Vertex normals are flipped to match velocity	
-		V_TwoSidedLiftDrag, ///Vertex normals are flipped to match velocity and lift and drag forces are applied
-		V_OneSided,			///Vertex normals are taken as it is	
-		F_TwoSided,			///Face normals are flipped to match velocity
-		F_TwoSidedLiftDrag,	///Face normals are flipped to match velocity and lift and drag forces are applied 
-		F_OneSided,			///Face normals are taken as it is		
-		END
-	};};
+	///eAeroModel
+	struct eAeroModel
+	{
+		enum _
+		{
+			V_Point,             ///Vertex normals are oriented toward velocity
+			V_TwoSided,          ///Vertex normals are flipped to match velocity
+			V_TwoSidedLiftDrag,  ///Vertex normals are flipped to match velocity and lift and drag forces are applied
+			V_OneSided,          ///Vertex normals are taken as it is
+			F_TwoSided,          ///Face normals are flipped to match velocity
+			F_TwoSidedLiftDrag,  ///Face normals are flipped to match velocity and lift and drag forces are applied
+			F_OneSided,          ///Face normals are taken as it is
+			END
+		};
+	};
 
 	///eVSolver : velocities solvers
-	struct	eVSolver { enum _ {
-		Linear,		///Linear solver
-		END
-	};};
+	struct eVSolver
+	{
+		enum _
+		{
+			Linear,  ///Linear solver
+			END
+		};
+	};
 
 	///ePSolver : positions solvers
-	struct	ePSolver { enum _ {
-		Linear,		///Linear solver
-		Anchors,	///Anchor solver
-		RContacts,	///Rigid contacts solver
-		SContacts,	///Soft contacts solver
-		END
-	};};
+	struct ePSolver
+	{
+		enum _
+		{
+			Linear,     ///Linear solver
+			Anchors,    ///Anchor solver
+			RContacts,  ///Rigid contacts solver
+			SContacts,  ///Soft contacts solver
+			END
+		};
+	};
 
 	///eSolverPresets
-	struct	eSolverPresets { enum _ {
-		Positions,
-		Velocities,
-		Default	=	Positions,
-		END
-	};};
+	struct eSolverPresets
+	{
+		enum _
+		{
+			Positions,
+			Velocities,
+			Default = Positions,
+			END
+		};
+	};
 
 	///eFeature
-	struct	eFeature { enum _ {
-		None,
-		Node,
-		Link,
-		Face,
-		Tetra,
-		END
-	};};
-
-	typedef btAlignedObjectArray<eVSolver::_>	tVSolverArray;
-	typedef btAlignedObjectArray<ePSolver::_>	tPSolverArray;
+	struct eFeature
+	{
+		enum _
+		{
+			None,
+			Node,
+			Link,
+			Face,
+			Tetra,
+			END
+		};
+	};
+
+	typedef btAlignedObjectArray<eVSolver::_> tVSolverArray;
+	typedef btAlignedObjectArray<ePSolver::_> tPSolverArray;
 
 	//
 	// Flags
 	//
 
 	///fCollision
-	struct fCollision { enum _ {
-		RVSmask	=	0x000f,	///Rigid versus soft mask
-		SDF_RS	=	0x0001,	///SDF based rigid vs soft
-		CL_RS	=	0x0002, ///Cluster vs convex rigid vs soft
-
-		SVSmask	=	0x0030,	///Rigid versus soft mask		
-		VF_SS	=	0x0010,	///Vertex vs face soft vs soft handling
-		CL_SS	=	0x0020, ///Cluster vs cluster soft vs soft handling
-		CL_SELF =	0x0040, ///Cluster soft body self collision
-		/* presets	*/ 
-		Default	=	SDF_RS,
-		END
-	};};
+	struct fCollision
+	{
+		enum _
+		{
+			RVSmask = 0x000f,  ///Rigid versus soft mask
+			SDF_RS = 0x0001,   ///SDF based rigid vs soft
+			CL_RS = 0x0002,    ///Cluster vs convex rigid vs soft
+			SDF_RD = 0x0004,   ///rigid vs deformable
+
+			SVSmask = 0x00f0,  ///Rigid versus soft mask
+			VF_SS = 0x0010,    ///Vertex vs face soft vs soft handling
+			CL_SS = 0x0020,    ///Cluster vs cluster soft vs soft handling
+			CL_SELF = 0x0040,  ///Cluster soft body self collision
+			VF_DD = 0x0080,    ///Vertex vs face soft vs soft handling
+
+			RVDFmask = 0x0f00,  /// Rigid versus deformable face mask
+			SDF_RDF = 0x0100,   /// GJK based Rigid vs. deformable face
+			SDF_MDF = 0x0200,   /// GJK based Multibody vs. deformable face
+			SDF_RDN = 0x0400,   /// SDF based Rigid vs. deformable node
+			/* presets	*/
+			Default = SDF_RS,
+			END
+		};
+	};
 
 	///fMaterial
-	struct fMaterial { enum _ {
-		DebugDraw	=	0x0001,	/// Enable debug draw
-		/* presets	*/ 
-		Default		=	DebugDraw,
-		END
-	};};
+	struct fMaterial
+	{
+		enum _
+		{
+			DebugDraw = 0x0001,  /// Enable debug draw
+			/* presets	*/
+			Default = DebugDraw,
+			END
+		};
+	};
 
 	//
 	// API Types
 	//
 
-	/* sRayCast		*/ 
+	/* sRayCast		*/
 	struct sRayCast
 	{
-		btSoftBody*	body;		/// soft body
-		eFeature::_	feature;	/// feature type
-		int			index;		/// feature index
-		btScalar	fraction;		/// time of impact fraction (rayorg+(rayto-rayfrom)*fraction)
+		btSoftBody* body;     /// soft body
+		eFeature::_ feature;  /// feature type
+		int index;            /// feature index
+		btScalar fraction;    /// time of impact fraction (rayorg+(rayto-rayfrom)*fraction)
 	};
 
-	/* ImplicitFn	*/ 
-	struct	ImplicitFn
+	/* ImplicitFn	*/
+	struct ImplicitFn
 	{
 		virtual ~ImplicitFn() {}
-		virtual btScalar	Eval(const btVector3& x)=0;
+		virtual btScalar Eval(const btVector3& x) = 0;
 	};
 
 	//
 	// Internal types
 	//
 
-	typedef btAlignedObjectArray<btScalar>	tScalarArray;
-	typedef btAlignedObjectArray<btVector3>	tVector3Array;
+	typedef btAlignedObjectArray<btScalar> tScalarArray;
+	typedef btAlignedObjectArray<btVector3> tVector3Array;
 
-	/* sCti is Softbody contact info	*/ 
-	struct	sCti
+	/* sCti is Softbody contact info	*/
+	struct sCti
 	{
-		const btCollisionObject*	m_colObj;		/* Rigid body			*/ 
-		btVector3		m_normal;	/* Outward normal		*/ 
-		btScalar		m_offset;	/* Offset from origin	*/ 
-	};	
+		const btCollisionObject* m_colObj; /* Rigid body			*/
+		btVector3 m_normal;                /* Outward normal		*/
+		btScalar m_offset;                 /* Offset from origin	*/
+		btVector3 m_bary;                  /* Barycentric weights for faces */
+	};
 
-	/* sMedium		*/ 
-	struct	sMedium
+	/* sMedium		*/
+	struct sMedium
 	{
-		btVector3		m_velocity;	/* Velocity				*/ 
-		btScalar		m_pressure;	/* Pressure				*/ 
-		btScalar		m_density;	/* Density				*/ 
+		btVector3 m_velocity; /* Velocity				*/
+		btScalar m_pressure;  /* Pressure				*/
+		btScalar m_density;   /* Density				*/
 	};
 
-	/* Base type	*/ 
-	struct	Element
+	/* Base type	*/
+	struct Element
 	{
-		void*			m_tag;			// User data
+		void* m_tag;  // User data
 		Element() : m_tag(0) {}
 	};
-	/* Material		*/ 
-	struct	Material : Element
-	{
-		btScalar				m_kLST;			// Linear stiffness coefficient [0,1]
-		btScalar				m_kAST;			// Area/Angular stiffness coefficient [0,1]
-		btScalar				m_kVST;			// Volume stiffness coefficient [0,1]
-		int						m_flags;		// Flags
-	};
-
-	/* Feature		*/ 
-	struct	Feature : Element
-	{
-		Material*				m_material;		// Material
-	};
-	/* Node			*/ 
-	struct	Node : Feature
-	{
-		btVector3				m_x;			// Position
-		btVector3				m_q;			// Previous step position
-		btVector3				m_v;			// Velocity
-		btVector3				m_f;			// Force accumulator
-		btVector3				m_n;			// Normal
-		btScalar				m_im;			// 1/mass
-		btScalar				m_area;			// Area
-		btDbvtNode*				m_leaf;			// Leaf data
-		int						m_battach:1;	// Attached
-	};
-	/* Link			*/ 
-	struct	Link : Feature
-	{
-		Node*					m_n[2];			// Node pointers
-		btScalar				m_rl;			// Rest length		
-		int						m_bbending:1;	// Bending link
-		btScalar				m_c0;			// (ima+imb)*kLST
-		btScalar				m_c1;			// rl^2
-		btScalar				m_c2;			// |gradient|^2/c0
-		btVector3				m_c3;			// gradient
-	};
-	/* Face			*/ 
-	struct	Face : Feature
-	{
-		Node*					m_n[3];			// Node pointers
-		btVector3				m_normal;		// Normal
-		btScalar				m_ra;			// Rest area
-		btDbvtNode*				m_leaf;			// Leaf data
-	};
-	/* Tetra		*/ 
-	struct	Tetra : Feature
-	{
-		Node*					m_n[4];			// Node pointers		
-		btScalar				m_rv;			// Rest volume
-		btDbvtNode*				m_leaf;			// Leaf data
-		btVector3				m_c0[4];		// gradients
-		btScalar				m_c1;			// (4*kVST)/(im0+im1+im2+im3)
-		btScalar				m_c2;			// m_c1/sum(|g0..3|^2)
-	};
-	/* RContact		*/ 
-	struct	RContact
-	{
-		sCti		m_cti;			// Contact infos
-		Node*					m_node;			// Owner node
-		btMatrix3x3				m_c0;			// Impulse matrix
-		btVector3				m_c1;			// Relative anchor
-		btScalar				m_c2;			// ima*dt
-		btScalar				m_c3;			// Friction
-		btScalar				m_c4;			// Hardness
-	};
-	/* SContact		*/ 
-	struct	SContact
-	{
-		Node*					m_node;			// Node
-		Face*					m_face;			// Face
-		btVector3				m_weights;		// Weigths
-		btVector3				m_normal;		// Normal
-		btScalar				m_margin;		// Margin
-		btScalar				m_friction;		// Friction
-		btScalar				m_cfm[2];		// Constraint force mixing
-	};
-	/* Anchor		*/ 
-	struct	Anchor
-	{
-		Node*					m_node;			// Node pointer
-		btVector3				m_local;		// Anchor position in body space
-		btRigidBody*			m_body;			// Body
-		btScalar				m_influence;
-		btMatrix3x3				m_c0;			// Impulse matrix
-		btVector3				m_c1;			// Relative anchor
-		btScalar				m_c2;			// ima*dt
-	};
-	/* Note			*/ 
-	struct	Note : Element
-	{
-		const char*				m_text;			// Text
-		btVector3				m_offset;		// Offset
-		int						m_rank;			// Rank
-		Node*					m_nodes[4];		// Nodes
-		btScalar				m_coords[4];	// Coordinates
-	};	
-	/* Pose			*/ 
-	struct	Pose
-	{
-		bool					m_bvolume;		// Is valid
-		bool					m_bframe;		// Is frame
-		btScalar				m_volume;		// Rest volume
-		tVector3Array			m_pos;			// Reference positions
-		tScalarArray			m_wgh;			// Weights
-		btVector3				m_com;			// COM
-		btMatrix3x3				m_rot;			// Rotation
-		btMatrix3x3				m_scl;			// Scale
-		btMatrix3x3				m_aqq;			// Base scaling
-	};
-	/* Cluster		*/ 
-	struct	Cluster
-	{
-		tScalarArray				m_masses;
-		btAlignedObjectArray<Node*>	m_nodes;		
-		tVector3Array				m_framerefs;
-		btTransform					m_framexform;
-		btScalar					m_idmass;
-		btScalar					m_imass;
-		btMatrix3x3					m_locii;
-		btMatrix3x3					m_invwi;
-		btVector3					m_com;
-		btVector3					m_vimpulses[2];
-		btVector3					m_dimpulses[2];
-		int							m_nvimpulses;
-		int							m_ndimpulses;
-		btVector3					m_lv;
-		btVector3					m_av;
-		btDbvtNode*					m_leaf;
-		btScalar					m_ndamping;	/* Node damping		*/ 
-		btScalar					m_ldamping;	/* Linear damping	*/ 
-		btScalar					m_adamping;	/* Angular damping	*/ 
-		btScalar					m_matching;
-		btScalar					m_maxSelfCollisionImpulse;
-		btScalar					m_selfCollisionImpulseFactor;
-		bool						m_containsAnchor;
-		bool						m_collide;
-		int							m_clusterIndex;
-		Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) 
-		,m_maxSelfCollisionImpulse(100.f),
-		m_selfCollisionImpulseFactor(0.01f),
-		m_containsAnchor(false)
-		{}
-	};
-	/* Impulse		*/ 
-	struct	Impulse
-	{
-		btVector3					m_velocity;
-		btVector3					m_drift;
-		int							m_asVelocity:1;
-		int							m_asDrift:1;
-		Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0)	{}
-		Impulse						operator -() const
-		{
-			Impulse i=*this;
-			i.m_velocity=-i.m_velocity;
-			i.m_drift=-i.m_drift;
-			return(i);
+	/* Material		*/
+	struct Material : Element
+	{
+		btScalar m_kLST;  // Linear stiffness coefficient [0,1]
+		btScalar m_kAST;  // Area/Angular stiffness coefficient [0,1]
+		btScalar m_kVST;  // Volume stiffness coefficient [0,1]
+		int m_flags;      // Flags
+	};
+
+	/* Feature		*/
+	struct Feature : Element
+	{
+		Material* m_material;  // Material
+	};
+	/* Node			*/
+	struct Node : Feature
+	{
+		btVector3 m_x;       // Position
+		btVector3 m_q;       // Previous step position/Test position
+		btVector3 m_v;       // Velocity
+		btVector3 m_vn;      // Previous step velocity
+		btVector3 m_f;       // Force accumulator
+		btVector3 m_n;       // Normal
+		btScalar m_im;       // 1/mass
+		btScalar m_area;     // Area
+		btDbvtNode* m_leaf;  // Leaf data
+		int m_constrained;   // depth of penetration
+		int m_battach : 1;   // Attached
+		int index;
+		btVector3 m_splitv;               // velocity associated with split impulse
+		btMatrix3x3 m_effectiveMass;      // effective mass in contact
+		btMatrix3x3 m_effectiveMass_inv;  // inverse of effective mass
+	};
+	/* Link			*/
+	ATTRIBUTE_ALIGNED16(struct)
+	Link : Feature
+	{
+		btVector3 m_c3;      // gradient
+		Node* m_n[2];        // Node pointers
+		btScalar m_rl;       // Rest length
+		int m_bbending : 1;  // Bending link
+		btScalar m_c0;       // (ima+imb)*kLST
+		btScalar m_c1;       // rl^2
+		btScalar m_c2;       // |gradient|^2/c0
+
+		BT_DECLARE_ALIGNED_ALLOCATOR();
+	};
+	/* Face			*/
+	struct Face : Feature
+	{
+		Node* m_n[3];          // Node pointers
+		btVector3 m_normal;    // Normal
+		btScalar m_ra;         // Rest area
+		btDbvtNode* m_leaf;    // Leaf data
+		btVector4 m_pcontact;  // barycentric weights of the persistent contact
+		btVector3 m_n0, m_n1, m_vn;
+		int m_index;
+	};
+	/* Tetra		*/
+	struct Tetra : Feature
+	{
+		Node* m_n[4];              // Node pointers
+		btScalar m_rv;             // Rest volume
+		btDbvtNode* m_leaf;        // Leaf data
+		btVector3 m_c0[4];         // gradients
+		btScalar m_c1;             // (4*kVST)/(im0+im1+im2+im3)
+		btScalar m_c2;             // m_c1/sum(|g0..3|^2)
+		btMatrix3x3 m_Dm_inverse;  // rest Dm^-1
+		btMatrix3x3 m_F;
+		btScalar m_element_measure;
+		btVector4 m_P_inv[3];  // first three columns of P_inv matrix
+	};
+
+	/*  TetraScratch  */
+	struct TetraScratch
+	{
+		btMatrix3x3 m_F;           // deformation gradient F
+		btScalar m_trace;          // trace of F^T * F
+		btScalar m_J;              // det(F)
+		btMatrix3x3 m_cofF;        // cofactor of F
+		btMatrix3x3 m_corotation;  // corotatio of the tetra
+	};
+
+	/* RContact		*/
+	struct RContact
+	{
+		sCti m_cti;        // Contact infos
+		Node* m_node;      // Owner node
+		btMatrix3x3 m_c0;  // Impulse matrix
+		btVector3 m_c1;    // Relative anchor
+		btScalar m_c2;     // ima*dt
+		btScalar m_c3;     // Friction
+		btScalar m_c4;     // Hardness
+
+		// jacobians and unit impulse responses for multibody
+		btMultiBodyJacobianData jacobianData_normal;
+		btMultiBodyJacobianData jacobianData_t1;
+		btMultiBodyJacobianData jacobianData_t2;
+		btVector3 t1;
+		btVector3 t2;
+	};
+
+	class DeformableRigidContact
+	{
+	public:
+		sCti m_cti;        // Contact infos
+		btMatrix3x3 m_c0;  // Impulse matrix
+		btVector3 m_c1;    // Relative anchor
+		btScalar m_c2;     // inverse mass of node/face
+		btScalar m_c3;     // Friction
+		btScalar m_c4;     // Hardness
+		btMatrix3x3 m_c5;  // inverse effective mass
+
+		// jacobians and unit impulse responses for multibody
+		btMultiBodyJacobianData jacobianData_normal;
+		btMultiBodyJacobianData jacobianData_t1;
+		btMultiBodyJacobianData jacobianData_t2;
+		btVector3 t1;
+		btVector3 t2;
+	};
+
+	class DeformableNodeRigidContact : public DeformableRigidContact
+	{
+	public:
+		Node* m_node;  // Owner node
+	};
+
+	class DeformableNodeRigidAnchor : public DeformableNodeRigidContact
+	{
+	public:
+		btVector3 m_local;  // Anchor position in body space
+	};
+
+	class DeformableFaceRigidContact : public DeformableRigidContact
+	{
+	public:
+		Face* m_face;              // Owner face
+		btVector3 m_contactPoint;  // Contact point
+		btVector3 m_bary;          // Barycentric weights
+		btVector3 m_weights;       // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
+	};
+
+	struct DeformableFaceNodeContact
+	{
+		Node* m_node;         // Node
+		Face* m_face;         // Face
+		btVector3 m_bary;     // Barycentric weights
+		btVector3 m_weights;  // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
+		btVector3 m_normal;   // Normal
+		btScalar m_margin;    // Margin
+		btScalar m_friction;  // Friction
+		btScalar m_imf;       // inverse mass of the face at contact point
+		btScalar m_c0;        // scale of the impulse matrix;
+	};
+
+	/* SContact		*/
+	struct SContact
+	{
+		Node* m_node;         // Node
+		Face* m_face;         // Face
+		btVector3 m_weights;  // Weigths
+		btVector3 m_normal;   // Normal
+		btScalar m_margin;    // Margin
+		btScalar m_friction;  // Friction
+		btScalar m_cfm[2];    // Constraint force mixing
+	};
+	/* Anchor		*/
+	struct Anchor
+	{
+		Node* m_node;         // Node pointer
+		btVector3 m_local;    // Anchor position in body space
+		btRigidBody* m_body;  // Body
+		btScalar m_influence;
+		btMatrix3x3 m_c0;  // Impulse matrix
+		btVector3 m_c1;    // Relative anchor
+		btScalar m_c2;     // ima*dt
+	};
+	/* Note			*/
+	struct Note : Element
+	{
+		const char* m_text;    // Text
+		btVector3 m_offset;    // Offset
+		int m_rank;            // Rank
+		Node* m_nodes[4];      // Nodes
+		btScalar m_coords[4];  // Coordinates
+	};
+	/* Pose			*/
+	struct Pose
+	{
+		bool m_bvolume;       // Is valid
+		bool m_bframe;        // Is frame
+		btScalar m_volume;    // Rest volume
+		tVector3Array m_pos;  // Reference positions
+		tScalarArray m_wgh;   // Weights
+		btVector3 m_com;      // COM
+		btMatrix3x3 m_rot;    // Rotation
+		btMatrix3x3 m_scl;    // Scale
+		btMatrix3x3 m_aqq;    // Base scaling
+	};
+	/* Cluster		*/
+	struct Cluster
+	{
+		tScalarArray m_masses;
+		btAlignedObjectArray<Node*> m_nodes;
+		tVector3Array m_framerefs;
+		btTransform m_framexform;
+		btScalar m_idmass;
+		btScalar m_imass;
+		btMatrix3x3 m_locii;
+		btMatrix3x3 m_invwi;
+		btVector3 m_com;
+		btVector3 m_vimpulses[2];
+		btVector3 m_dimpulses[2];
+		int m_nvimpulses;
+		int m_ndimpulses;
+		btVector3 m_lv;
+		btVector3 m_av;
+		btDbvtNode* m_leaf;
+		btScalar m_ndamping; /* Node damping		*/
+		btScalar m_ldamping; /* Linear damping	*/
+		btScalar m_adamping; /* Angular damping	*/
+		btScalar m_matching;
+		btScalar m_maxSelfCollisionImpulse;
+		btScalar m_selfCollisionImpulseFactor;
+		bool m_containsAnchor;
+		bool m_collide;
+		int m_clusterIndex;
+		Cluster() : m_leaf(0), m_ndamping(0), m_ldamping(0), m_adamping(0), m_matching(0), m_maxSelfCollisionImpulse(100.f), m_selfCollisionImpulseFactor(0.01f), m_containsAnchor(false)
+		{
+		}
+	};
+	/* Impulse		*/
+	struct Impulse
+	{
+		btVector3 m_velocity;
+		btVector3 m_drift;
+		int m_asVelocity : 1;
+		int m_asDrift : 1;
+		Impulse() : m_velocity(0, 0, 0), m_drift(0, 0, 0), m_asVelocity(0), m_asDrift(0) {}
+		Impulse operator-() const
+		{
+			Impulse i = *this;
+			i.m_velocity = -i.m_velocity;
+			i.m_drift = -i.m_drift;
+			return (i);
 		}
-		Impulse						operator*(btScalar x) const
+		Impulse operator*(btScalar x) const
 		{
-			Impulse i=*this;
-			i.m_velocity*=x;
-			i.m_drift*=x;
-			return(i);
+			Impulse i = *this;
+			i.m_velocity *= x;
+			i.m_drift *= x;
+			return (i);
 		}
 	};
-	/* Body			*/ 
-	struct	Body
+	/* Body			*/
+	struct Body
 	{
-		Cluster*			m_soft;
-		btRigidBody*		m_rigid;
-		const btCollisionObject*	m_collisionObject;
+		Cluster* m_soft;
+		btRigidBody* m_rigid;
+		const btCollisionObject* m_collisionObject;
 
-		Body() : m_soft(0),m_rigid(0),m_collisionObject(0)				{}
-		Body(Cluster* p) : m_soft(p),m_rigid(0),m_collisionObject(0)	{}
-		Body(const btCollisionObject* colObj) : m_soft(0),m_collisionObject(colObj)
+		Body() : m_soft(0), m_rigid(0), m_collisionObject(0) {}
+		Body(Cluster* p) : m_soft(p), m_rigid(0), m_collisionObject(0) {}
+		Body(const btCollisionObject* colObj) : m_soft(0), m_collisionObject(colObj)
 		{
 			m_rigid = (btRigidBody*)btRigidBody::upcast(m_collisionObject);
 		}
 
-		void						activate() const
+		void activate() const
 		{
-			if(m_rigid) 
+			if (m_rigid)
 				m_rigid->activate();
 			if (m_collisionObject)
 				m_collisionObject->activate();
-
 		}
-		const btMatrix3x3&			invWorldInertia() const
+		const btMatrix3x3& invWorldInertia() const
 		{
-			static const btMatrix3x3	iwi(0,0,0,0,0,0,0,0,0);
-			if(m_rigid) return(m_rigid->getInvInertiaTensorWorld());
-			if(m_soft)	return(m_soft->m_invwi);
-			return(iwi);
+			static const btMatrix3x3 iwi(0, 0, 0, 0, 0, 0, 0, 0, 0);
+			if (m_rigid) return (m_rigid->getInvInertiaTensorWorld());
+			if (m_soft) return (m_soft->m_invwi);
+			return (iwi);
 		}
-		btScalar					invMass() const
+		btScalar invMass() const
 		{
-			if(m_rigid) return(m_rigid->getInvMass());
-			if(m_soft)	return(m_soft->m_imass);
-			return(0);
+			if (m_rigid) return (m_rigid->getInvMass());
+			if (m_soft) return (m_soft->m_imass);
+			return (0);
 		}
-		const btTransform&			xform() const
+		const btTransform& xform() const
 		{
-			static const btTransform	identity=btTransform::getIdentity();		
-			if(m_collisionObject) return(m_collisionObject->getWorldTransform());
-			if(m_soft)	return(m_soft->m_framexform);
-			return(identity);
+			static const btTransform identity = btTransform::getIdentity();
+			if (m_collisionObject) return (m_collisionObject->getWorldTransform());
+			if (m_soft) return (m_soft->m_framexform);
+			return (identity);
 		}
-		btVector3					linearVelocity() const
+		btVector3 linearVelocity() const
 		{
-			if(m_rigid) return(m_rigid->getLinearVelocity());
-			if(m_soft)	return(m_soft->m_lv);
-			return(btVector3(0,0,0));
+			if (m_rigid) return (m_rigid->getLinearVelocity());
+			if (m_soft) return (m_soft->m_lv);
+			return (btVector3(0, 0, 0));
 		}
-		btVector3					angularVelocity(const btVector3& rpos) const
-		{			
-			if(m_rigid) return(btCross(m_rigid->getAngularVelocity(),rpos));
-			if(m_soft)	return(btCross(m_soft->m_av,rpos));
-			return(btVector3(0,0,0));
+		btVector3 angularVelocity(const btVector3& rpos) const
+		{
+			if (m_rigid) return (btCross(m_rigid->getAngularVelocity(), rpos));
+			if (m_soft) return (btCross(m_soft->m_av, rpos));
+			return (btVector3(0, 0, 0));
 		}
-		btVector3					angularVelocity() const
-		{			
-			if(m_rigid) return(m_rigid->getAngularVelocity());
-			if(m_soft)	return(m_soft->m_av);
-			return(btVector3(0,0,0));
+		btVector3 angularVelocity() const
+		{
+			if (m_rigid) return (m_rigid->getAngularVelocity());
+			if (m_soft) return (m_soft->m_av);
+			return (btVector3(0, 0, 0));
 		}
-		btVector3					velocity(const btVector3& rpos) const
+		btVector3 velocity(const btVector3& rpos) const
 		{
-			return(linearVelocity()+angularVelocity(rpos));
+			return (linearVelocity() + angularVelocity(rpos));
 		}
-		void						applyVImpulse(const btVector3& impulse,const btVector3& rpos) const
+		void applyVImpulse(const btVector3& impulse, const btVector3& rpos) const
 		{
-			if(m_rigid)	m_rigid->applyImpulse(impulse,rpos);
-			if(m_soft)	btSoftBody::clusterVImpulse(m_soft,rpos,impulse);
+			if (m_rigid) m_rigid->applyImpulse(impulse, rpos);
+			if (m_soft) btSoftBody::clusterVImpulse(m_soft, rpos, impulse);
 		}
-		void						applyDImpulse(const btVector3& impulse,const btVector3& rpos) const
+		void applyDImpulse(const btVector3& impulse, const btVector3& rpos) const
 		{
-			if(m_rigid)	m_rigid->applyImpulse(impulse,rpos);
-			if(m_soft)	btSoftBody::clusterDImpulse(m_soft,rpos,impulse);
-		}		
-		void						applyImpulse(const Impulse& impulse,const btVector3& rpos) const
+			if (m_rigid) m_rigid->applyImpulse(impulse, rpos);
+			if (m_soft) btSoftBody::clusterDImpulse(m_soft, rpos, impulse);
+		}
+		void applyImpulse(const Impulse& impulse, const btVector3& rpos) const
 		{
-			if(impulse.m_asVelocity)	
+			if (impulse.m_asVelocity)
 			{
-//				printf("impulse.m_velocity = %f,%f,%f\n",impulse.m_velocity.getX(),impulse.m_velocity.getY(),impulse.m_velocity.getZ());
-				applyVImpulse(impulse.m_velocity,rpos);
+				//				printf("impulse.m_velocity = %f,%f,%f\n",impulse.m_velocity.getX(),impulse.m_velocity.getY(),impulse.m_velocity.getZ());
+				applyVImpulse(impulse.m_velocity, rpos);
 			}
-			if(impulse.m_asDrift)		
+			if (impulse.m_asDrift)
 			{
-//				printf("impulse.m_drift = %f,%f,%f\n",impulse.m_drift.getX(),impulse.m_drift.getY(),impulse.m_drift.getZ());
-				applyDImpulse(impulse.m_drift,rpos);
+				//				printf("impulse.m_drift = %f,%f,%f\n",impulse.m_drift.getX(),impulse.m_drift.getY(),impulse.m_drift.getZ());
+				applyDImpulse(impulse.m_drift, rpos);
 			}
 		}
-		void						applyVAImpulse(const btVector3& impulse) const
+		void applyVAImpulse(const btVector3& impulse) const
 		{
-			if(m_rigid)	m_rigid->applyTorqueImpulse(impulse);
-			if(m_soft)	btSoftBody::clusterVAImpulse(m_soft,impulse);
+			if (m_rigid) m_rigid->applyTorqueImpulse(impulse);
+			if (m_soft) btSoftBody::clusterVAImpulse(m_soft, impulse);
 		}
-		void						applyDAImpulse(const btVector3& impulse) const
+		void applyDAImpulse(const btVector3& impulse) const
 		{
-			if(m_rigid)	m_rigid->applyTorqueImpulse(impulse);
-			if(m_soft)	btSoftBody::clusterDAImpulse(m_soft,impulse);
+			if (m_rigid) m_rigid->applyTorqueImpulse(impulse);
+			if (m_soft) btSoftBody::clusterDAImpulse(m_soft, impulse);
 		}
-		void						applyAImpulse(const Impulse& impulse) const
+		void applyAImpulse(const Impulse& impulse) const
 		{
-			if(impulse.m_asVelocity)	applyVAImpulse(impulse.m_velocity);
-			if(impulse.m_asDrift)		applyDAImpulse(impulse.m_drift);
+			if (impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity);
+			if (impulse.m_asDrift) applyDAImpulse(impulse.m_drift);
 		}
-		void						applyDCImpulse(const btVector3& impulse) const
+		void applyDCImpulse(const btVector3& impulse) const
 		{
-			if(m_rigid)	m_rigid->applyCentralImpulse(impulse);
-			if(m_soft)	btSoftBody::clusterDCImpulse(m_soft,impulse);
+			if (m_rigid) m_rigid->applyCentralImpulse(impulse);
+			if (m_soft) btSoftBody::clusterDCImpulse(m_soft, impulse);
 		}
 	};
-	/* Joint		*/ 
-	struct	Joint
+	/* Joint		*/
+	struct Joint
 	{
-		struct eType { enum _ {
-			Linear=0,
-			Angular,
-			Contact
-		};};
+		struct eType
+		{
+			enum _
+			{
+				Linear = 0,
+				Angular,
+				Contact
+			};
+		};
 		struct Specs
 		{
-			Specs() : erp(1),cfm(1),split(1) {}
-			btScalar	erp;
-			btScalar	cfm;
-			btScalar	split;
+			Specs() : erp(1), cfm(1), split(1) {}
+			btScalar erp;
+			btScalar cfm;
+			btScalar split;
 		};
-		Body						m_bodies[2];
-		btVector3					m_refs[2];
-		btScalar					m_cfm;
-		btScalar					m_erp;
-		btScalar					m_split;
-		btVector3					m_drift;
-		btVector3					m_sdrift;
-		btMatrix3x3					m_massmatrix;
-		bool						m_delete;
-		virtual						~Joint() {}
+		Body m_bodies[2];
+		btVector3 m_refs[2];
+		btScalar m_cfm;
+		btScalar m_erp;
+		btScalar m_split;
+		btVector3 m_drift;
+		btVector3 m_sdrift;
+		btMatrix3x3 m_massmatrix;
+		bool m_delete;
+		virtual ~Joint() {}
 		Joint() : m_delete(false) {}
-		virtual void				Prepare(btScalar dt,int iterations);
-		virtual void				Solve(btScalar dt,btScalar sor)=0;
-		virtual void				Terminate(btScalar dt)=0;
-		virtual eType::_			Type() const=0;
+		virtual void Prepare(btScalar dt, int iterations);
+		virtual void Solve(btScalar dt, btScalar sor) = 0;
+		virtual void Terminate(btScalar dt) = 0;
+		virtual eType::_ Type() const = 0;
 	};
-	/* LJoint		*/ 
-	struct	LJoint : Joint
+	/* LJoint		*/
+	struct LJoint : Joint
 	{
 		struct Specs : Joint::Specs
 		{
-			btVector3	position;
-		};		
-		btVector3					m_rpos[2];
-		void						Prepare(btScalar dt,int iterations);
-		void						Solve(btScalar dt,btScalar sor);
-		void						Terminate(btScalar dt);
-		eType::_					Type() const { return(eType::Linear); }
+			btVector3 position;
+		};
+		btVector3 m_rpos[2];
+		void Prepare(btScalar dt, int iterations);
+		void Solve(btScalar dt, btScalar sor);
+		void Terminate(btScalar dt);
+		eType::_ Type() const { return (eType::Linear); }
 	};
-	/* AJoint		*/ 
-	struct	AJoint : Joint
+	/* AJoint		*/
+	struct AJoint : Joint
 	{
 		struct IControl
 		{
 			virtual ~IControl() {}
-			virtual void			Prepare(AJoint*)				{}
-			virtual btScalar		Speed(AJoint*,btScalar current) { return(current); }
-			static IControl*		Default()						{ static IControl def;return(&def); }
+			virtual void Prepare(AJoint*) {}
+			virtual btScalar Speed(AJoint*, btScalar current) { return (current); }
+			static IControl* Default()
+			{
+				static IControl def;
+				return (&def);
+			}
 		};
 		struct Specs : Joint::Specs
 		{
 			Specs() : icontrol(IControl::Default()) {}
-			btVector3	axis;
-			IControl*	icontrol;
-		};		
-		btVector3					m_axis[2];
-		IControl*					m_icontrol;
-		void						Prepare(btScalar dt,int iterations);
-		void						Solve(btScalar dt,btScalar sor);
-		void						Terminate(btScalar dt);
-		eType::_					Type() const { return(eType::Angular); }
-	};
-	/* CJoint		*/ 
-	struct	CJoint : Joint
-	{		
-		int							m_life;
-		int							m_maxlife;
-		btVector3					m_rpos[2];
-		btVector3					m_normal;
-		btScalar					m_friction;
-		void						Prepare(btScalar dt,int iterations);
-		void						Solve(btScalar dt,btScalar sor);
-		void						Terminate(btScalar dt);
-		eType::_					Type() const { return(eType::Contact); }
-	};
-	/* Config		*/ 
-	struct	Config
-	{
-		eAeroModel::_			aeromodel;		// Aerodynamic model (default: V_Point)
-		btScalar				kVCF;			// Velocities correction factor (Baumgarte)
-		btScalar				kDP;			// Damping coefficient [0,1]
-		btScalar				kDG;			// Drag coefficient [0,+inf]
-		btScalar				kLF;			// Lift coefficient [0,+inf]
-		btScalar				kPR;			// Pressure coefficient [-inf,+inf]
-		btScalar				kVC;			// Volume conversation coefficient [0,+inf]
-		btScalar				kDF;			// Dynamic friction coefficient [0,1]
-		btScalar				kMT;			// Pose matching coefficient [0,1]		
-		btScalar				kCHR;			// Rigid contacts hardness [0,1]
-		btScalar				kKHR;			// Kinetic contacts hardness [0,1]
-		btScalar				kSHR;			// Soft contacts hardness [0,1]
-		btScalar				kAHR;			// Anchors hardness [0,1]
-		btScalar				kSRHR_CL;		// Soft vs rigid hardness [0,1] (cluster only)
-		btScalar				kSKHR_CL;		// Soft vs kinetic hardness [0,1] (cluster only)
-		btScalar				kSSHR_CL;		// Soft vs soft hardness [0,1] (cluster only)
-		btScalar				kSR_SPLT_CL;	// Soft vs rigid impulse split [0,1] (cluster only)
-		btScalar				kSK_SPLT_CL;	// Soft vs rigid impulse split [0,1] (cluster only)
-		btScalar				kSS_SPLT_CL;	// Soft vs rigid impulse split [0,1] (cluster only)
-		btScalar				maxvolume;		// Maximum volume ratio for pose
-		btScalar				timescale;		// Time scale
-		int						viterations;	// Velocities solver iterations
-		int						piterations;	// Positions solver iterations
-		int						diterations;	// Drift solver iterations
-		int						citerations;	// Cluster solver iterations
-		int						collisions;		// Collisions flags
-		tVSolverArray			m_vsequence;	// Velocity solvers sequence
-		tPSolverArray			m_psequence;	// Position solvers sequence
-		tPSolverArray			m_dsequence;	// Drift solvers sequence
-	};
-	/* SolverState	*/ 
-	struct	SolverState
-	{
-		btScalar				sdt;			// dt*timescale
-		btScalar				isdt;			// 1/sdt
-		btScalar				velmrg;			// velocity margin
-		btScalar				radmrg;			// radial margin
-		btScalar				updmrg;			// Update margin
-	};	
+			btVector3 axis;
+			IControl* icontrol;
+		};
+		btVector3 m_axis[2];
+		IControl* m_icontrol;
+		void Prepare(btScalar dt, int iterations);
+		void Solve(btScalar dt, btScalar sor);
+		void Terminate(btScalar dt);
+		eType::_ Type() const { return (eType::Angular); }
+	};
+	/* CJoint		*/
+	struct CJoint : Joint
+	{
+		int m_life;
+		int m_maxlife;
+		btVector3 m_rpos[2];
+		btVector3 m_normal;
+		btScalar m_friction;
+		void Prepare(btScalar dt, int iterations);
+		void Solve(btScalar dt, btScalar sor);
+		void Terminate(btScalar dt);
+		eType::_ Type() const { return (eType::Contact); }
+	};
+	/* Config		*/
+	struct Config
+	{
+		eAeroModel::_ aeromodel;    // Aerodynamic model (default: V_Point)
+		btScalar kVCF;              // Velocities correction factor (Baumgarte)
+		btScalar kDP;               // Damping coefficient [0,1]
+		btScalar kDG;               // Drag coefficient [0,+inf]
+		btScalar kLF;               // Lift coefficient [0,+inf]
+		btScalar kPR;               // Pressure coefficient [-inf,+inf]
+		btScalar kVC;               // Volume conversation coefficient [0,+inf]
+		btScalar kDF;               // Dynamic friction coefficient [0,1]
+		btScalar kMT;               // Pose matching coefficient [0,1]
+		btScalar kCHR;              // Rigid contacts hardness [0,1]
+		btScalar kKHR;              // Kinetic contacts hardness [0,1]
+		btScalar kSHR;              // Soft contacts hardness [0,1]
+		btScalar kAHR;              // Anchors hardness [0,1]
+		btScalar kSRHR_CL;          // Soft vs rigid hardness [0,1] (cluster only)
+		btScalar kSKHR_CL;          // Soft vs kinetic hardness [0,1] (cluster only)
+		btScalar kSSHR_CL;          // Soft vs soft hardness [0,1] (cluster only)
+		btScalar kSR_SPLT_CL;       // Soft vs rigid impulse split [0,1] (cluster only)
+		btScalar kSK_SPLT_CL;       // Soft vs rigid impulse split [0,1] (cluster only)
+		btScalar kSS_SPLT_CL;       // Soft vs rigid impulse split [0,1] (cluster only)
+		btScalar maxvolume;         // Maximum volume ratio for pose
+		btScalar timescale;         // Time scale
+		int viterations;            // Velocities solver iterations
+		int piterations;            // Positions solver iterations
+		int diterations;            // Drift solver iterations
+		int citerations;            // Cluster solver iterations
+		int collisions;             // Collisions flags
+		tVSolverArray m_vsequence;  // Velocity solvers sequence
+		tPSolverArray m_psequence;  // Position solvers sequence
+		tPSolverArray m_dsequence;  // Drift solvers sequence
+		btScalar drag;              // deformable air drag
+		btScalar m_maxStress;       // Maximum principle first Piola stress
+	};
+	/* SolverState	*/
+	struct SolverState
+	{
+		//if you add new variables, always initialize them!
+		SolverState()
+			: sdt(0),
+			  isdt(0),
+			  velmrg(0),
+			  radmrg(0),
+			  updmrg(0)
+		{
+		}
+		btScalar sdt;     // dt*timescale
+		btScalar isdt;    // 1/sdt
+		btScalar velmrg;  // velocity margin
+		btScalar radmrg;  // radial margin
+		btScalar updmrg;  // Update margin
+	};
 	/// RayFromToCaster takes a ray from, ray to (instead of direction!)
-	struct	RayFromToCaster : btDbvt::ICollide
-	{
-		btVector3			m_rayFrom;
-		btVector3			m_rayTo;
-		btVector3			m_rayNormalizedDirection;
-		btScalar			m_mint;
-		Face*				m_face;
-		int					m_tests;
-		RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt);
-		void					Process(const btDbvtNode* leaf);
-
-		static inline btScalar	rayFromToTriangle(const btVector3& rayFrom,
-			const btVector3& rayTo,
-			const btVector3& rayNormalizedDirection,
-			const btVector3& a,
-			const btVector3& b,
-			const btVector3& c,
-			btScalar maxt=SIMD_INFINITY);
+	struct RayFromToCaster : btDbvt::ICollide
+	{
+		btVector3 m_rayFrom;
+		btVector3 m_rayTo;
+		btVector3 m_rayNormalizedDirection;
+		btScalar m_mint;
+		Face* m_face;
+		int m_tests;
+		RayFromToCaster(const btVector3& rayFrom, const btVector3& rayTo, btScalar mxt);
+		void Process(const btDbvtNode* leaf);
+
+		static /*inline*/ btScalar rayFromToTriangle(const btVector3& rayFrom,
+													 const btVector3& rayTo,
+													 const btVector3& rayNormalizedDirection,
+													 const btVector3& a,
+													 const btVector3& b,
+													 const btVector3& c,
+													 btScalar maxt = SIMD_INFINITY);
 	};
 
 	//
 	// Typedefs
 	//
 
-	typedef void								(*psolver_t)(btSoftBody*,btScalar,btScalar);
-	typedef void								(*vsolver_t)(btSoftBody*,btScalar);
-	typedef btAlignedObjectArray<Cluster*>		tClusterArray;
-	typedef btAlignedObjectArray<Note>			tNoteArray;
-	typedef btAlignedObjectArray<Node>			tNodeArray;
-	typedef btAlignedObjectArray<btDbvtNode*>	tLeafArray;
-	typedef btAlignedObjectArray<Link>			tLinkArray;
-	typedef btAlignedObjectArray<Face>			tFaceArray;
-	typedef btAlignedObjectArray<Tetra>			tTetraArray;
-	typedef btAlignedObjectArray<Anchor>		tAnchorArray;
-	typedef btAlignedObjectArray<RContact>		tRContactArray;
-	typedef btAlignedObjectArray<SContact>		tSContactArray;
-	typedef btAlignedObjectArray<Material*>		tMaterialArray;
-	typedef btAlignedObjectArray<Joint*>		tJointArray;
-	typedef btAlignedObjectArray<btSoftBody*>	tSoftBodyArray;	
+	typedef void (*psolver_t)(btSoftBody*, btScalar, btScalar);
+	typedef void (*vsolver_t)(btSoftBody*, btScalar);
+	typedef btAlignedObjectArray<Cluster*> tClusterArray;
+	typedef btAlignedObjectArray<Note> tNoteArray;
+	typedef btAlignedObjectArray<Node> tNodeArray;
+	typedef btAlignedObjectArray<btDbvtNode*> tLeafArray;
+	typedef btAlignedObjectArray<Link> tLinkArray;
+	typedef btAlignedObjectArray<Face> tFaceArray;
+	typedef btAlignedObjectArray<Tetra> tTetraArray;
+	typedef btAlignedObjectArray<Anchor> tAnchorArray;
+	typedef btAlignedObjectArray<RContact> tRContactArray;
+	typedef btAlignedObjectArray<SContact> tSContactArray;
+	typedef btAlignedObjectArray<Material*> tMaterialArray;
+	typedef btAlignedObjectArray<Joint*> tJointArray;
+	typedef btAlignedObjectArray<btSoftBody*> tSoftBodyArray;
 
 	//
 	// Fields
 	//
 
-	Config					m_cfg;			// Configuration
-	SolverState				m_sst;			// Solver state
-	Pose					m_pose;			// Pose
-	void*					m_tag;			// User data
-	btSoftBodyWorldInfo*	m_worldInfo;	// World info
-	tNoteArray				m_notes;		// Notes
-	tNodeArray				m_nodes;		// Nodes
-	tLinkArray				m_links;		// Links
-	tFaceArray				m_faces;		// Faces
-	tTetraArray				m_tetras;		// Tetras
-	tAnchorArray			m_anchors;		// Anchors
-	tRContactArray			m_rcontacts;	// Rigid contacts
-	tSContactArray			m_scontacts;	// Soft contacts
-	tJointArray				m_joints;		// Joints
-	tMaterialArray			m_materials;	// Materials
-	btScalar				m_timeacc;		// Time accumulator
-	btVector3				m_bounds[2];	// Spatial bounds	
-	bool					m_bUpdateRtCst;	// Update runtime constants
-	btDbvt					m_ndbvt;		// Nodes tree
-	btDbvt					m_fdbvt;		// Faces tree
-	btDbvt					m_cdbvt;		// Clusters tree
-	tClusterArray			m_clusters;		// Clusters
-
-	btAlignedObjectArray<bool>m_clusterConnectivity;//cluster connectivity, for self-collision
-
-	btTransform			m_initialWorldTransform;
-
-	btVector3			m_windVelocity;
-	
-	btScalar        m_restLengthScale;
-	
+	Config m_cfg;                      // Configuration
+	SolverState m_sst;                 // Solver state
+	Pose m_pose;                       // Pose
+	void* m_tag;                       // User data
+	btSoftBodyWorldInfo* m_worldInfo;  // World info
+	tNoteArray m_notes;                // Notes
+	tNodeArray m_nodes;                // Nodes
+	tNodeArray m_renderNodes;          // Nodes
+	tLinkArray m_links;                // Links
+	tFaceArray m_faces;                // Faces
+	tFaceArray m_renderFaces;          // Faces
+	tTetraArray m_tetras;              // Tetras
+	btAlignedObjectArray<TetraScratch> m_tetraScratches;
+	btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
+	tAnchorArray m_anchors;  // Anchors
+	btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors;
+	tRContactArray m_rcontacts;  // Rigid contacts
+	btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
+	btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
+	btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
+	tSContactArray m_scontacts;     // Soft contacts
+	tJointArray m_joints;           // Joints
+	tMaterialArray m_materials;     // Materials
+	btScalar m_timeacc;             // Time accumulator
+	btVector3 m_bounds[2];          // Spatial bounds
+	bool m_bUpdateRtCst;            // Update runtime constants
+	btDbvt m_ndbvt;                 // Nodes tree
+	btDbvt m_fdbvt;                 // Faces tree
+	btDbvntNode* m_fdbvnt;          // Faces tree with normals
+	btDbvt m_cdbvt;                 // Clusters tree
+	tClusterArray m_clusters;       // Clusters
+	btScalar m_dampingCoefficient;  // Damping Coefficient
+	btScalar m_sleepingThreshold;
+	btScalar m_maxSpeedSquared;
+	btAlignedObjectArray<btVector3> m_quads;  // quadrature points for collision detection
+	btScalar m_repulsionStiffness;
+	btScalar m_gravityFactor;
+	btAlignedObjectArray<btVector3> m_X;  // initial positions
+
+	btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
+	btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
+	btAlignedObjectArray<btScalar> m_z;  // vertical distance used in extrapolation
+	bool m_useSelfCollision;
+	bool m_softSoftCollision;
+
+	btAlignedObjectArray<bool> m_clusterConnectivity;  //cluster connectivity, for self-collision
+
+	btVector3 m_windVelocity;
+
+	btScalar m_restLengthScale;
+
 	//
 	// Api
 	//
 
-	/* ctor																	*/ 
-	btSoftBody(	btSoftBodyWorldInfo* worldInfo,int node_count,		const btVector3* x,		const btScalar* m);
+	/* ctor																	*/
+	btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m);
 
-	/* ctor																	*/ 
-	btSoftBody(	btSoftBodyWorldInfo* worldInfo);
+	/* ctor																	*/
+	btSoftBody(btSoftBodyWorldInfo* worldInfo);
 
-	void	initDefaults();
+	void initDefaults();
 
-	/* dtor																	*/ 
+	/* dtor																	*/
 	virtual ~btSoftBody();
-	/* Check for existing link												*/ 
+	/* Check for existing link												*/
 
-	btAlignedObjectArray<int>	m_userIndexMapping;
+	btAlignedObjectArray<int> m_userIndexMapping;
 
-	btSoftBodyWorldInfo*	getWorldInfo()
+	btSoftBodyWorldInfo* getWorldInfo()
 	{
 		return m_worldInfo;
 	}
 
+	void setDampingCoefficient(btScalar damping_coeff)
+	{
+		m_dampingCoefficient = damping_coeff;
+	}
+
 	///@todo: avoid internal softbody shape hack and move collision code to collision library
-	virtual void	setCollisionShape(btCollisionShape* collisionShape)
+	virtual void setCollisionShape(btCollisionShape* collisionShape)
 	{
-		
 	}
 
-	bool				checkLink(	int node0,
-		int node1) const;
-	bool				checkLink(	const Node* node0,
-		const Node* node1) const;
-	/* Check for existring face												*/ 
-	bool				checkFace(	int node0,
-		int node1,
-		int node2) const;
-	/* Append material														*/ 
-	Material*			appendMaterial();
-	/* Append note															*/ 
-	void				appendNote(	const char* text,
-		const btVector3& o,
-		const btVector4& c=btVector4(1,0,0,0),
-		Node* n0=0,
-		Node* n1=0,
-		Node* n2=0,
-		Node* n3=0);
-	void				appendNote(	const char* text,
-		const btVector3& o,
-		Node* feature);
-	void				appendNote(	const char* text,
-		const btVector3& o,
-		Link* feature);
-	void				appendNote(	const char* text,
-		const btVector3& o,
-		Face* feature);
-	/* Append node															*/ 
-	void				appendNode(	const btVector3& x,btScalar m);
-	/* Append link															*/ 
-	void				appendLink(int model=-1,Material* mat=0);
-	void				appendLink(	int node0,
-		int node1,
-		Material* mat=0,
-		bool bcheckexist=false);
-	void				appendLink(	Node* node0,
-		Node* node1,
-		Material* mat=0,
-		bool bcheckexist=false);
-	/* Append face															*/ 
-	void				appendFace(int model=-1,Material* mat=0);
-	void				appendFace(	int node0,
-		int node1,
-		int node2,
-		Material* mat=0);
-	void			appendTetra(int model,Material* mat);
+	bool checkLink(int node0,
+				   int node1) const;
+	bool checkLink(const Node* node0,
+				   const Node* node1) const;
+	/* Check for existring face												*/
+	bool checkFace(int node0,
+				   int node1,
+				   int node2) const;
+	/* Append material														*/
+	Material* appendMaterial();
+	/* Append note															*/
+	void appendNote(const char* text,
+					const btVector3& o,
+					const btVector4& c = btVector4(1, 0, 0, 0),
+					Node* n0 = 0,
+					Node* n1 = 0,
+					Node* n2 = 0,
+					Node* n3 = 0);
+	void appendNote(const char* text,
+					const btVector3& o,
+					Node* feature);
+	void appendNote(const char* text,
+					const btVector3& o,
+					Link* feature);
+	void appendNote(const char* text,
+					const btVector3& o,
+					Face* feature);
+	/* Append node															*/
+	void appendNode(const btVector3& x, btScalar m);
+	/* Append link															*/
+	void appendLink(int model = -1, Material* mat = 0);
+	void appendLink(int node0,
+					int node1,
+					Material* mat = 0,
+					bool bcheckexist = false);
+	void appendLink(Node* node0,
+					Node* node1,
+					Material* mat = 0,
+					bool bcheckexist = false);
+	/* Append face															*/
+	void appendFace(int model = -1, Material* mat = 0);
+	void appendFace(int node0,
+					int node1,
+					int node2,
+					Material* mat = 0);
+	void appendTetra(int model, Material* mat);
 	//
-	void			appendTetra(int node0,
-										int node1,
-										int node2,
-										int node3,
-										Material* mat=0);
-
-
-	/* Append anchor														*/ 
-	void				appendAnchor(	int node,
-		btRigidBody* body, bool disableCollisionBetweenLinkedBodies=false,btScalar influence = 1);
-	void			appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies=false,btScalar influence = 1);
-	/* Append linear joint													*/ 
-	void				appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1);
-	void				appendLinearJoint(const LJoint::Specs& specs,Body body=Body());
-	void				appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body);
-	/* Append linear joint													*/ 
-	void				appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1);
-	void				appendAngularJoint(const AJoint::Specs& specs,Body body=Body());
-	void				appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body);
-	/* Add force (or gravity) to the entire body							*/ 
-	void				addForce(		const btVector3& force);
-	/* Add force (or gravity) to a node of the body							*/ 
-	void				addForce(		const btVector3& force,
-		int node);
+	void appendTetra(int node0,
+					 int node1,
+					 int node2,
+					 int node3,
+					 Material* mat = 0);
+
+	/* Append anchor														*/
+	void appendDeformableAnchor(int node, btRigidBody* body);
+	void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link);
+	void appendAnchor(int node,
+					  btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
+	void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
+	void removeAnchor(int node);
+	/* Append linear joint													*/
+	void appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1);
+	void appendLinearJoint(const LJoint::Specs& specs, Body body = Body());
+	void appendLinearJoint(const LJoint::Specs& specs, btSoftBody* body);
+	/* Append linear joint													*/
+	void appendAngularJoint(const AJoint::Specs& specs, Cluster* body0, Body body1);
+	void appendAngularJoint(const AJoint::Specs& specs, Body body = Body());
+	void appendAngularJoint(const AJoint::Specs& specs, btSoftBody* body);
+	/* Add force (or gravity) to the entire body							*/
+	void addForce(const btVector3& force);
+	/* Add force (or gravity) to a node of the body							*/
+	void addForce(const btVector3& force,
+				  int node);
 	/* Add aero force to a node of the body */
-	void			    addAeroForceToNode(const btVector3& windVelocity,int nodeIndex);
+	void addAeroForceToNode(const btVector3& windVelocity, int nodeIndex);
 
 	/* Add aero force to a face of the body */
-	void			    addAeroForceToFace(const btVector3& windVelocity,int faceIndex);
-
-	/* Add velocity to the entire body										*/ 
-	void				addVelocity(	const btVector3& velocity);
-
-	/* Set velocity for the entire body										*/ 
-	void				setVelocity(	const btVector3& velocity);
-
-	/* Add velocity to a node of the body									*/ 
-	void				addVelocity(	const btVector3& velocity,
-		int node);
-	/* Set mass																*/ 
-	void				setMass(		int node,
-		btScalar mass);
-	/* Get mass																*/ 
-	btScalar			getMass(		int node) const;
-	/* Get total mass														*/ 
-	btScalar			getTotalMass() const;
-	/* Set total mass (weighted by previous masses)							*/ 
-	void				setTotalMass(	btScalar mass,
-		bool fromfaces=false);
-	/* Set total density													*/ 
-	void				setTotalDensity(btScalar density);
+	void addAeroForceToFace(const btVector3& windVelocity, int faceIndex);
+
+	/* Add velocity to the entire body										*/
+	void addVelocity(const btVector3& velocity);
+
+	/* Set velocity for the entire body										*/
+	void setVelocity(const btVector3& velocity);
+
+	/* Add velocity to a node of the body									*/
+	void addVelocity(const btVector3& velocity,
+					 int node);
+	/* Set mass																*/
+	void setMass(int node,
+				 btScalar mass);
+	/* Get mass																*/
+	btScalar getMass(int node) const;
+	/* Get total mass														*/
+	btScalar getTotalMass() const;
+	/* Set total mass (weighted by previous masses)							*/
+	void setTotalMass(btScalar mass,
+					  bool fromfaces = false);
+	/* Set total density													*/
+	void setTotalDensity(btScalar density);
 	/* Set volume mass (using tetrahedrons)									*/
-	void				setVolumeMass(		btScalar mass);
+	void setVolumeMass(btScalar mass);
 	/* Set volume density (using tetrahedrons)								*/
-	void				setVolumeDensity(	btScalar density);
-	/* Transform															*/ 
-	void				transform(		const btTransform& trs);
-	/* Translate															*/ 
-	void				translate(		const btVector3& trs);
-	/* Rotate															*/ 
-	void				rotate(	const btQuaternion& rot);
-	/* Scale																*/ 
-	void				scale(	const btVector3& scl);
+	void setVolumeDensity(btScalar density);
+	/* Get the linear velocity of the center of mass                        */
+	btVector3 getLinearVelocity();
+	/* Set the linear velocity of the center of mass                        */
+	void setLinearVelocity(const btVector3& linVel);
+	/* Set the angular velocity of the center of mass                       */
+	void setAngularVelocity(const btVector3& angVel);
+	/* Get best fit rigid transform                                         */
+	btTransform getRigidTransform();
+	/* Transform to given pose                                              */
+	void transformTo(const btTransform& trs);
+	/* Transform															*/
+	void transform(const btTransform& trs);
+	/* Translate															*/
+	void translate(const btVector3& trs);
+	/* Rotate															*/
+	void rotate(const btQuaternion& rot);
+	/* Scale																*/
+	void scale(const btVector3& scl);
 	/* Get link resting lengths scale										*/
-	btScalar			getRestLengthScale();
+	btScalar getRestLengthScale();
 	/* Scale resting length of all springs									*/
-	void				setRestLengthScale(btScalar restLength);
-	/* Set current state as pose											*/ 
-	void				setPose(		bool bvolume,
-		bool bframe);
-	/* Set current link lengths as resting lengths							*/ 
-	void				resetLinkRestLengths();
-	/* Return the volume													*/ 
-	btScalar			getVolume() const;
-	/* Cluster count														*/ 
-	int					clusterCount() const;
-	/* Cluster center of mass												*/ 
-	static btVector3	clusterCom(const Cluster* cluster);
-	btVector3			clusterCom(int cluster) const;
-	/* Cluster velocity at rpos												*/ 
-	static btVector3	clusterVelocity(const Cluster* cluster,const btVector3& rpos);
-	/* Cluster impulse														*/ 
-	static void			clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse);
-	static void			clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse);
-	static void			clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse);
-	static void			clusterVAImpulse(Cluster* cluster,const btVector3& impulse);
-	static void			clusterDAImpulse(Cluster* cluster,const btVector3& impulse);
-	static void			clusterAImpulse(Cluster* cluster,const Impulse& impulse);
-	static void			clusterDCImpulse(Cluster* cluster,const btVector3& impulse);
-	/* Generate bending constraints based on distance in the adjency graph	*/ 
-	int					generateBendingConstraints(	int distance,
-		Material* mat=0);
-	/* Randomize constraints to reduce solver bias							*/ 
-	void				randomizeConstraints();
-	/* Release clusters														*/ 
-	void				releaseCluster(int index);
-	void				releaseClusters();
-	/* Generate clusters (K-mean)											*/ 
+	void setRestLengthScale(btScalar restLength);
+	/* Set current state as pose											*/
+	void setPose(bool bvolume,
+				 bool bframe);
+	/* Set current link lengths as resting lengths							*/
+	void resetLinkRestLengths();
+	/* Return the volume													*/
+	btScalar getVolume() const;
+	/* Cluster count														*/
+	btVector3 getCenterOfMass() const
+	{
+		btVector3 com(0, 0, 0);
+		for (int i = 0; i < m_nodes.size(); i++)
+		{
+			com += (m_nodes[i].m_x * this->getMass(i));
+		}
+		com /= this->getTotalMass();
+		return com;
+	}
+	int clusterCount() const;
+	/* Cluster center of mass												*/
+	static btVector3 clusterCom(const Cluster* cluster);
+	btVector3 clusterCom(int cluster) const;
+	/* Cluster velocity at rpos												*/
+	static btVector3 clusterVelocity(const Cluster* cluster, const btVector3& rpos);
+	/* Cluster impulse														*/
+	static void clusterVImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse);
+	static void clusterDImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse);
+	static void clusterImpulse(Cluster* cluster, const btVector3& rpos, const Impulse& impulse);
+	static void clusterVAImpulse(Cluster* cluster, const btVector3& impulse);
+	static void clusterDAImpulse(Cluster* cluster, const btVector3& impulse);
+	static void clusterAImpulse(Cluster* cluster, const Impulse& impulse);
+	static void clusterDCImpulse(Cluster* cluster, const btVector3& impulse);
+	/* Generate bending constraints based on distance in the adjency graph	*/
+	int generateBendingConstraints(int distance,
+								   Material* mat = 0);
+	/* Randomize constraints to reduce solver bias							*/
+	void randomizeConstraints();
+	/* Release clusters														*/
+	void releaseCluster(int index);
+	void releaseClusters();
+	/* Generate clusters (K-mean)											*/
 	///generateClusters with k=0 will create a convex cluster for each tetrahedron or triangle
 	///otherwise an approximation will be used (better performance)
-	int					generateClusters(int k,int maxiterations=8192);
-	/* Refine																*/ 
-	void				refine(ImplicitFn* ifn,btScalar accurary,bool cut);
-	/* CutLink																*/ 
-	bool				cutLink(int node0,int node1,btScalar position);
-	bool				cutLink(const Node* node0,const Node* node1,btScalar position);
+	int generateClusters(int k, int maxiterations = 8192);
+	/* Refine																*/
+	void refine(ImplicitFn* ifn, btScalar accurary, bool cut);
+	/* CutLink																*/
+	bool cutLink(int node0, int node1, btScalar position);
+	bool cutLink(const Node* node0, const Node* node1, btScalar position);
 
 	///Ray casting using rayFrom and rayTo in worldspace, (not direction!)
-	bool				rayTest(const btVector3& rayFrom,
-		const btVector3& rayTo,
-		sRayCast& results);
-	/* Solver presets														*/ 
-	void				setSolver(eSolverPresets::_ preset);
-	/* predictMotion														*/ 
-	void				predictMotion(btScalar dt);
-	/* solveConstraints														*/ 
-	void				solveConstraints();
-	/* staticSolve															*/ 
-	void				staticSolve(int iterations);
-	/* solveCommonConstraints												*/ 
-	static void			solveCommonConstraints(btSoftBody** bodies,int count,int iterations);
-	/* solveClusters														*/ 
-	static void			solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies);
-	/* integrateMotion														*/ 
-	void				integrateMotion();
-	/* defaultCollisionHandlers												*/ 
-	void				defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap);
-	void				defaultCollisionHandler(btSoftBody* psb);
-
-
+	bool rayTest(const btVector3& rayFrom,
+				 const btVector3& rayTo,
+				 sRayCast& results);
+	bool rayFaceTest(const btVector3& rayFrom,
+					 const btVector3& rayTo,
+					 sRayCast& results);
+	int rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
+					btScalar& mint, int& index) const;
+	/* Solver presets														*/
+	void setSolver(eSolverPresets::_ preset);
+	/* predictMotion														*/
+	void predictMotion(btScalar dt);
+	/* solveConstraints														*/
+	void solveConstraints();
+	/* staticSolve															*/
+	void staticSolve(int iterations);
+	/* solveCommonConstraints												*/
+	static void solveCommonConstraints(btSoftBody** bodies, int count, int iterations);
+	/* solveClusters														*/
+	static void solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies);
+	/* integrateMotion														*/
+	void integrateMotion();
+	/* defaultCollisionHandlers												*/
+	void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap);
+	void defaultCollisionHandler(btSoftBody* psb);
+	void setSelfCollision(bool useSelfCollision);
+	bool useSelfCollision();
+	void updateDeactivation(btScalar timeStep);
+	void setZeroVelocity();
+	bool wantsSleeping();
 
 	//
 	// Functionality to deal with new accelerated solvers.
@@ -892,8 +1089,7 @@ public:
 	/**
 	 * Set a wind velocity for interaction with the air.
 	 */
-	void setWindVelocity( const btVector3 &velocity );
-
+	void setWindVelocity(const btVector3& velocity);
 
 	/**
 	 * Return the wind velocity for interaction with the air.
@@ -904,41 +1100,40 @@ public:
 	// Set the solver that handles this soft body
 	// Should not be allowed to get out of sync with reality
 	// Currently called internally on addition to the world
-	void setSoftBodySolver( btSoftBodySolver *softBodySolver )
+	void setSoftBodySolver(btSoftBodySolver* softBodySolver)
 	{
 		m_softBodySolver = softBodySolver;
 	}
 
 	//
 	// Return the solver that handles this soft body
-	// 
-	btSoftBodySolver *getSoftBodySolver()
+	//
+	btSoftBodySolver* getSoftBodySolver()
 	{
 		return m_softBodySolver;
 	}
 
 	//
 	// Return the solver that handles this soft body
-	// 
-	btSoftBodySolver *getSoftBodySolver() const
+	//
+	btSoftBodySolver* getSoftBodySolver() const
 	{
 		return m_softBodySolver;
 	}
 
-
 	//
 	// Cast
 	//
 
-	static const btSoftBody*	upcast(const btCollisionObject* colObj)
+	static const btSoftBody* upcast(const btCollisionObject* colObj)
 	{
-		if (colObj->getInternalType()==CO_SOFT_BODY)
+		if (colObj->getInternalType() == CO_SOFT_BODY)
 			return (const btSoftBody*)colObj;
 		return 0;
 	}
-	static btSoftBody*			upcast(btCollisionObject* colObj)
+	static btSoftBody* upcast(btCollisionObject* colObj)
 	{
-		if (colObj->getInternalType()==CO_SOFT_BODY)
+		if (colObj->getInternalType() == CO_SOFT_BODY)
 			return (btSoftBody*)colObj;
 		return 0;
 	}
@@ -947,7 +1142,7 @@ public:
 	// ::btCollisionObject
 	//
 
-	virtual void getAabb(btVector3& aabbMin,btVector3& aabbMax) const
+	virtual void getAabb(btVector3& aabbMin, btVector3& aabbMax) const
 	{
 		aabbMin = m_bounds[0];
 		aabbMax = m_bounds[1];
@@ -955,48 +1150,230 @@ public:
 	//
 	// Private
 	//
-	void				pointersToIndices();
-	void				indicesToPointers(const int* map=0);
-
-	int					rayTest(const btVector3& rayFrom,const btVector3& rayTo,
-		btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const;
-	void				initializeFaceTree();
-	btVector3			evaluateCom() const;
-	bool				checkContact(const btCollisionObjectWrapper* colObjWrap,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const;
-	void				updateNormals();
-	void				updateBounds();
-	void				updatePose();
-	void				updateConstants();
-	void				updateLinkConstants();
-	void				updateArea(bool averageArea = true);
-	void				initializeClusters();
-	void				updateClusters();
-	void				cleanupClusters();
-	void				prepareClusters(int iterations);
-	void				solveClusters(btScalar sor);
-	void				applyClusters(bool drift);
-	void				dampClusters();
-	void				applyForces();	
-	static void			PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti);
-	static void			PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti);
-	static void			PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti);
-	static void			PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti);
-	static void			VSolve_Links(btSoftBody* psb,btScalar kst);
-	static psolver_t	getSolver(ePSolver::_ solver);
-	static vsolver_t	getSolver(eVSolver::_ solver);
-
-
-	virtual	int	calculateSerializeBufferSize()	const;
-
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer,  class btSerializer* serializer) const;
+	void pointersToIndices();
+	void indicesToPointers(const int* map = 0);
+
+	int rayTest(const btVector3& rayFrom, const btVector3& rayTo,
+				btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const;
+	void initializeFaceTree();
+	void rebuildNodeTree();
+	btVector3 evaluateCom() const;
+	bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
+	bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
+	bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
+	void updateNormals();
+	void updateBounds();
+	void updatePose();
+	void updateConstants();
+	void updateLinkConstants();
+	void updateArea(bool averageArea = true);
+	void initializeClusters();
+	void updateClusters();
+	void cleanupClusters();
+	void prepareClusters(int iterations);
+	void solveClusters(btScalar sor);
+	void applyClusters(bool drift);
+	void dampClusters();
+	void setSpringStiffness(btScalar k);
+	void setGravityFactor(btScalar gravFactor);
+	void initializeDmInverse();
+	void updateDeformation();
+	void advanceDeformation();
+	void applyForces();
+	void setMaxStress(btScalar maxStress);
+	void interpolateRenderMesh();
+	void setCollisionQuadrature(int N);
+	static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
+	static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);
+	static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti);
+	static void PSolve_Links(btSoftBody* psb, btScalar kst, btScalar ti);
+	static void VSolve_Links(btSoftBody* psb, btScalar kst);
+	static psolver_t getSolver(ePSolver::_ solver);
+	static vsolver_t getSolver(eVSolver::_ solver);
+	void geometricCollisionHandler(btSoftBody* psb);
+#define SAFE_EPSILON SIMD_EPSILON * 100.0
+	void updateNode(btDbvtNode* node, bool use_velocity, bool margin)
+	{
+		if (node->isleaf())
+		{
+			btSoftBody::Node* n = (btSoftBody::Node*)(node->data);
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)
+			vol;
+			btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON;  // use user defined margin or margin for floating point precision
+			if (use_velocity)
+			{
+				btVector3 points[2] = {n->m_x, n->m_x + m_sst.sdt * n->m_v};
+				vol = btDbvtVolume::FromPoints(points, 2);
+				vol.Expand(btVector3(pad, pad, pad));
+			}
+			else
+			{
+				vol = btDbvtVolume::FromCR(n->m_x, pad);
+			}
+			node->volume = vol;
+			return;
+		}
+		else
+		{
+			updateNode(node->childs[0], use_velocity, margin);
+			updateNode(node->childs[1], use_velocity, margin);
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)
+			vol;
+			Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
+			node->volume = vol;
+		}
+	}
 
-	//virtual void serializeSingleObject(class btSerializer* serializer) const;
+	void updateNodeTree(bool use_velocity, bool margin)
+	{
+		if (m_ndbvt.m_root)
+			updateNode(m_ndbvt.m_root, use_velocity, margin);
+	}
 
+	template <class DBVTNODE>  // btDbvtNode or btDbvntNode
+	void updateFace(DBVTNODE* node, bool use_velocity, bool margin)
+	{
+		if (node->isleaf())
+		{
+			btSoftBody::Face* f = (btSoftBody::Face*)(node->data);
+			btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON;  // use user defined margin or margin for floating point precision
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)
+			vol;
+			if (use_velocity)
+			{
+				btVector3 points[6] = {f->m_n[0]->m_x, f->m_n[0]->m_x + m_sst.sdt * f->m_n[0]->m_v,
+									   f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v,
+									   f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v};
+				vol = btDbvtVolume::FromPoints(points, 6);
+			}
+			else
+			{
+				btVector3 points[3] = {f->m_n[0]->m_x,
+									   f->m_n[1]->m_x,
+									   f->m_n[2]->m_x};
+				vol = btDbvtVolume::FromPoints(points, 3);
+			}
+			vol.Expand(btVector3(pad, pad, pad));
+			node->volume = vol;
+			return;
+		}
+		else
+		{
+			updateFace(node->childs[0], use_velocity, margin);
+			updateFace(node->childs[1], use_velocity, margin);
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)
+			vol;
+			Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
+			node->volume = vol;
+		}
+	}
+	void updateFaceTree(bool use_velocity, bool margin)
+	{
+		if (m_fdbvt.m_root)
+			updateFace(m_fdbvt.m_root, use_velocity, margin);
+		if (m_fdbvnt)
+			updateFace(m_fdbvnt, use_velocity, margin);
+	}
 
-};
+	template <typename T>
+	static inline T BaryEval(const T& a,
+							 const T& b,
+							 const T& c,
+							 const btVector3& coord)
+	{
+		return (a * coord.x() + b * coord.y() + c * coord.z());
+	}
 
+	void applyRepulsionForce(btScalar timeStep, bool applySpringForce)
+	{
+		btAlignedObjectArray<int> indices;
+		{
+			// randomize the order of repulsive force
+			indices.resize(m_faceNodeContacts.size());
+			for (int i = 0; i < m_faceNodeContacts.size(); ++i)
+				indices[i] = i;
+#define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff)
+			int i, ni;
+
+			for (i = 0, ni = indices.size(); i < ni; ++i)
+			{
+				btSwap(indices[i], indices[NEXTRAND % ni]);
+			}
+		}
+		for (int k = 0; k < m_faceNodeContacts.size(); ++k)
+		{
+			int i = indices[k];
+			btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[i];
+			btSoftBody::Node* node = c.m_node;
+			btSoftBody::Face* face = c.m_face;
+			const btVector3& w = c.m_bary;
+			const btVector3& n = c.m_normal;
+			btVector3 l = node->m_x - BaryEval(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, w);
+			btScalar d = c.m_margin - n.dot(l);
+			d = btMax(btScalar(0), d);
+
+			const btVector3& va = node->m_v;
+			btVector3 vb = BaryEval(face->m_n[0]->m_v, face->m_n[1]->m_v, face->m_n[2]->m_v, w);
+			btVector3 vr = va - vb;
+			const btScalar vn = btDot(vr, n);  // dn < 0 <==> opposing
+			if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep)
+				continue;
+			btVector3 vt = vr - vn * n;
+			btScalar I = 0;
+			btScalar mass = node->m_im == 0 ? 0 : btScalar(1) / node->m_im;
+			if (applySpringForce)
+				I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
+			if (vn < 0)
+				I += 0.5 * mass * vn;
+			int face_penetration = 0, node_penetration = node->m_constrained;
+			for (int i = 0; i < 3; ++i)
+				face_penetration |= face->m_n[i]->m_constrained;
+			btScalar I_tilde = 2.0 * I / (1.0 + w.length2());
+
+			//             double the impulse if node or face is constrained.
+			if (face_penetration > 0 || node_penetration > 0)
+			{
+				I_tilde *= 2.0;
+			}
+			if (face_penetration <= 0)
+			{
+				for (int j = 0; j < 3; ++j)
+					face->m_n[j]->m_v += w[j] * n * I_tilde * node->m_im;
+			}
+			if (node_penetration <= 0)
+			{
+				node->m_v -= I_tilde * node->m_im * n;
+			}
 
+			// apply frictional impulse
+			btScalar vt_norm = vt.safeNorm();
+			if (vt_norm > SIMD_EPSILON)
+			{
+				btScalar delta_vn = -2 * I * node->m_im;
+				btScalar mu = c.m_friction;
+				btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0)) * vt_norm;
+				I = 0.5 * mass * (vt_norm - vt_new);
+				vt.safeNormalize();
+				I_tilde = 2.0 * I / (1.0 + w.length2());
+				//                 double the impulse if node or face is constrained.
+				if (face_penetration > 0 || node_penetration > 0)
+					I_tilde *= 2.0;
+				if (face_penetration <= 0)
+				{
+					for (int j = 0; j < 3; ++j)
+						face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im;
+				}
+				if (node_penetration <= 0)
+				{
+					node->m_v -= I_tilde * node->m_im * vt;
+				}
+			}
+		}
+	}
+	virtual int calculateSerializeBufferSize() const;
 
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
+};
 
-#endif //_BT_SOFT_BODY_H
+#endif  //_BT_SOFT_BODY_H
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp
index 9f0d44526bf..750718f57f8 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btSoftBodyConcaveCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
@@ -27,34 +26,28 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btConvexHullShape.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 
-
 #include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
 #include "BulletSoftBody/btSoftBody.h"
 
-#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)//make this configurable
+#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)  //make this configurable
 
-btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
-: btCollisionAlgorithm(ci),
-m_isSwapped(isSwapped),
-m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped)
+btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
+	: btCollisionAlgorithm(ci),
+	  m_isSwapped(isSwapped),
+	  m_btSoftBodyTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped)
 {
 }
 
-
-
 btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm()
 {
 }
 
-
-
-btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher*  dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped):
-m_dispatcher(dispatcher),
-m_dispatchInfoPtr(0)
+btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher),
+																																														 m_dispatchInfoPtr(0)
 {
-	m_softBody = (isSwapped? (btSoftBody*)body1Wrap->getCollisionObject():(btSoftBody*)body0Wrap->getCollisionObject());
-	m_triBody = isSwapped? body0Wrap->getCollisionObject():body1Wrap->getCollisionObject();
+	m_softBody = (isSwapped ? (btSoftBody*)body1Wrap->getCollisionObject() : (btSoftBody*)body0Wrap->getCollisionObject());
+	m_triBody = isSwapped ? body0Wrap->getCollisionObject() : body1Wrap->getCollisionObject();
 
 	//
 	// create the manifold from the dispatcher 'manifold pool'
@@ -68,46 +61,42 @@ btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
 {
 	clearCache();
 	//	m_dispatcher->releaseManifold( m_manifoldPtr );
-
 }
 
-
-void	btSoftBodyTriangleCallback::clearCache()
+void btSoftBodyTriangleCallback::clearCache()
 {
-	for (int i=0;i<m_shapeCache.size();i++)
+	for (int i = 0; i < m_shapeCache.size(); i++)
 	{
 		btTriIndex* tmp = m_shapeCache.getAtIndex(i);
 		btAssert(tmp);
 		btAssert(tmp->m_childShape);
-		m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape);//necessary?
+		m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape);  //necessary?
 		delete tmp->m_childShape;
 	}
 	m_shapeCache.clear();
 }
 
-
-void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
+void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex)
 {
 	//just for debugging purposes
 	//printf("triangle %d",m_triangleCount++);
-	
+
 	btCollisionAlgorithmConstructionInfo ci;
 	ci.m_dispatcher1 = m_dispatcher;
 
 	///debug drawing of the overlapping triangles
-	if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe))
+	if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawWireframe))
 	{
-		btVector3 color(1,1,0);
+		btVector3 color(1, 1, 0);
 		const btTransform& tr = m_triBody->getWorldTransform();
-		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
-		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
-		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
+		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]), tr(triangle[1]), color);
+		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]), tr(triangle[2]), color);
+		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]), tr(triangle[0]), color);
 	}
 
-	btTriIndex	triIndex(partId,triangleIndex,0);
+	btTriIndex triIndex(partId, triangleIndex, 0);
 	btHashKey<btTriIndex> triKey(triIndex.getUid());
 
-
 	btTriIndex* shapeIndex = m_shapeCache[triKey];
 	if (shapeIndex)
 	{
@@ -117,81 +106,73 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId,
 		//copy over user pointers to temporary shape
 		tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer());
 
-		btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1);
+		btCollisionObjectWrapper softBody(0, m_softBody->getCollisionShape(), m_softBody, m_softBody->getWorldTransform(), -1, -1);
 		//btCollisionObjectWrapper triBody(0,tm, ob, btTransform::getIdentity());//ob->getWorldTransform());//??
-		btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);
+		btCollisionObjectWrapper triBody(0, tm, m_triBody, m_triBody->getWorldTransform(), partId, triangleIndex);
+		ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS;
+		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody, &triBody, 0, algoType);  //m_manifoldPtr);
 
-		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr);
-
-		colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut);
+		colAlgo->processCollision(&softBody, &triBody, *m_dispatchInfoPtr, m_resultOut);
 		colAlgo->~btCollisionAlgorithm();
 		ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
-		
+
 		return;
 	}
 
-	//aabb filter is already applied!	
+	//aabb filter is already applied!
 
 	//btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
 
 	//	if (m_softBody->getCollisionShape()->getShapeType()==
 	{
 		//		btVector3 other;
-		btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
+		btVector3 normal = (triangle[1] - triangle[0]).cross(triangle[2] - triangle[0]);
 		normal.normalize();
-		normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION;
+		normal *= BT_SOFTBODY_TRIANGLE_EXTRUSION;
 		//		other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
 		//		other+=normal*22.f;
-		btVector3	pts[6] = {triangle[0]+normal,
-			triangle[1]+normal,
-			triangle[2]+normal,
-			triangle[0]-normal,
-			triangle[1]-normal,
-			triangle[2]-normal};
-
-		btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6);
+		btVector3 pts[6] = {triangle[0] + normal,
+							triangle[1] + normal,
+							triangle[2] + normal,
+							triangle[0] - normal,
+							triangle[1] - normal,
+							triangle[2] - normal};
 
+		btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(), 6);
 
 		//		btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other);
 
-		//btTriangleShape tm(triangle[0],triangle[1],triangle[2]);	
+		//btTriangleShape tm(triangle[0],triangle[1],triangle[2]);
 		//	tm.setMargin(m_collisionMarginTriangle);
 
 		//copy over user pointers to temporary shape
 		tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer());
 
-		
-		btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1);
-		btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);//btTransform::getIdentity());//??
+		btCollisionObjectWrapper softBody(0, m_softBody->getCollisionShape(), m_softBody, m_softBody->getWorldTransform(), -1, -1);
+		btCollisionObjectWrapper triBody(0, tm, m_triBody, m_triBody->getWorldTransform(), partId, triangleIndex);  //btTransform::getIdentity());//??
 
-		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr);
+		ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS;
+		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody, &triBody, 0, algoType);  //m_manifoldPtr);
 
-		colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut);
+		colAlgo->processCollision(&softBody, &triBody, *m_dispatchInfoPtr, m_resultOut);
 		colAlgo->~btCollisionAlgorithm();
 		ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
 
 		triIndex.m_childShape = tm;
-		m_shapeCache.insert(triKey,triIndex);
-
+		m_shapeCache.insert(triKey, triIndex);
 	}
-
-
-
 }
 
-
-
-void	btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btCollisionObjectWrapper* triBodyWrap, const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btCollisionObjectWrapper* triBodyWrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	m_dispatchInfoPtr = &dispatchInfo;
-	m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION);
+	m_collisionMarginTriangle = collisionMarginTriangle + btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION);
 	m_resultOut = resultOut;
 
-
-	btVector3	aabbWorldSpaceMin,aabbWorldSpaceMax;
-	m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax);
-	btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5);
-	btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5);
+	btVector3 aabbWorldSpaceMin, aabbWorldSpaceMax;
+	m_softBody->getAabb(aabbWorldSpaceMin, aabbWorldSpaceMax);
+	btVector3 halfExtents = (aabbWorldSpaceMax - aabbWorldSpaceMin) * btScalar(0.5);
+	btVector3 softBodyCenter = (aabbWorldSpaceMax + aabbWorldSpaceMin) * btScalar(0.5);
 
 	btTransform softTransform;
 	softTransform.setIdentity();
@@ -199,56 +180,45 @@ void	btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMargin
 
 	btTransform convexInTriangleSpace;
 	convexInTriangleSpace = triBodyWrap->getWorldTransform().inverse() * softTransform;
-	btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax);
+	btTransformAabb(halfExtents, m_collisionMarginTriangle, convexInTriangleSpace, m_aabbMin, m_aabbMax);
 }
 
 void btSoftBodyConcaveCollisionAlgorithm::clearCache()
 {
 	m_btSoftBodyTriangleCallback.clearCache();
-
 }
 
-void btSoftBodyConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btSoftBodyConcaveCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
-
-
 	//btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
 	const btCollisionObjectWrapper* triBody = m_isSwapped ? body0Wrap : body1Wrap;
 
 	if (triBody->getCollisionShape()->isConcave())
 	{
-
-
-		const btCollisionObject*	triOb = triBody->getCollisionObject();
-		const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triOb->getCollisionShape());
+		const btCollisionObject* triOb = triBody->getCollisionObject();
+		const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>(triOb->getCollisionShape());
 
 		//	if (convexBody->getCollisionShape()->isConvex())
 		{
 			btScalar collisionMarginTriangle = concaveShape->getMargin();
 
 			//			resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
-			m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,triBody,dispatchInfo,resultOut);
+			m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, triBody, dispatchInfo, resultOut);
 
-		
-			concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
+			concaveShape->processAllTriangles(&m_btSoftBodyTriangleCallback, m_btSoftBodyTriangleCallback.getAabbMin(), m_btSoftBodyTriangleCallback.getAabbMax());
 
 			//	resultOut->refreshContactPoints();
-
 		}
-
 	}
-
 }
 
-
-btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	(void)resultOut;
 	(void)dispatchInfo;
 	btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
 	btCollisionObject* triBody = m_isSwapped ? body0 : body1;
 
-
 	//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
 
 	//only perform CCD above a certain threshold, this prevents blocking on the long run
@@ -267,25 +237,23 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO
 	btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
 	btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
 
-	struct LocalTriangleSphereCastCallback	: public btTriangleCallback
+	struct LocalTriangleSphereCastCallback : public btTriangleCallback
 	{
 		btTransform m_ccdSphereFromTrans;
 		btTransform m_ccdSphereToTrans;
-		btTransform	m_meshTransform;
+		btTransform m_meshTransform;
 
-		btScalar	m_ccdSphereRadius;
-		btScalar	m_hitFraction;
+		btScalar m_ccdSphereRadius;
+		btScalar m_hitFraction;
 
-
-		LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
-			:m_ccdSphereFromTrans(from),
-			m_ccdSphereToTrans(to),
-			m_ccdSphereRadius(ccdSphereRadius),
-			m_hitFraction(hitFraction)
-		{			
+		LocalTriangleSphereCastCallback(const btTransform& from, const btTransform& to, btScalar ccdSphereRadius, btScalar hitFraction)
+			: m_ccdSphereFromTrans(from),
+			  m_ccdSphereToTrans(to),
+			  m_ccdSphereRadius(ccdSphereRadius),
+			  m_hitFraction(hitFraction)
+		{
 		}
 
-
 		virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 		{
 			(void)partId;
@@ -295,29 +263,23 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO
 			ident.setIdentity();
 			btConvexCast::CastResult castResult;
 			castResult.m_fraction = m_hitFraction;
-			btSphereShape	pointShape(m_ccdSphereRadius);
-			btTriangleShape	triShape(triangle[0],triangle[1],triangle[2]);
-			btVoronoiSimplexSolver	simplexSolver;
-			btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
+			btSphereShape pointShape(m_ccdSphereRadius);
+			btTriangleShape triShape(triangle[0], triangle[1], triangle[2]);
+			btVoronoiSimplexSolver simplexSolver;
+			btSubsimplexConvexCast convexCaster(&pointShape, &triShape, &simplexSolver);
 			//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
 			//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
 			//local space?
 
-			if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
-				ident,ident,castResult))
+			if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans,
+											  ident, ident, castResult))
 			{
 				if (m_hitFraction > castResult.m_fraction)
 					m_hitFraction = castResult.m_fraction;
 			}
-
 		}
-
 	};
 
-
-
-
-
 	if (triBody->getCollisionShape()->isConcave())
 	{
 		btVector3 rayAabbMin = convexFromLocal.getOrigin();
@@ -325,33 +287,30 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO
 		btVector3 rayAabbMax = convexFromLocal.getOrigin();
 		rayAabbMax.setMax(convexToLocal.getOrigin());
 		btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
-		rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
-		rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+		rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
+		rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
 
-		btScalar curHitFraction = btScalar(1.); //is this available?
-		LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
-			convexbody->getCcdSweptSphereRadius(),curHitFraction);
+		btScalar curHitFraction = btScalar(1.);  //is this available?
+		LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal,
+														convexbody->getCcdSweptSphereRadius(), curHitFraction);
 
 		raycastCallback.m_hitFraction = convexbody->getHitFraction();
 
 		btCollisionObject* concavebody = triBody;
 
-		btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
+		btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape();
 
 		if (triangleMesh)
 		{
-			triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
+			triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax);
 		}
 
-
-
 		if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
 		{
-			convexbody->setHitFraction( raycastCallback.m_hitFraction);
+			convexbody->setHitFraction(raycastCallback.m_hitFraction);
 			return raycastCallback.m_hitFraction;
 		}
 	}
 
 	return btScalar(1.);
-
 }
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h
index 11c7b88f98e..3adedbd8051 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h
@@ -29,63 +29,62 @@ class btCollisionShape;
 
 #include "LinearMath/btHashMap.h"
 
-#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" //for definition of MAX_NUM_PARTS_IN_BITS
+#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h"  //for definition of MAX_NUM_PARTS_IN_BITS
 
 struct btTriIndex
 {
 	int m_PartIdTriangleIndex;
-	class btCollisionShape*	m_childShape;
+	class btCollisionShape* m_childShape;
 
-	btTriIndex(int partId,int triangleIndex,btCollisionShape* shape)
+	btTriIndex(int partId, int triangleIndex, btCollisionShape* shape)
 	{
-		m_PartIdTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
+		m_PartIdTriangleIndex = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
 		m_childShape = shape;
 	}
 
-	int	getTriangleIndex() const
+	int getTriangleIndex() const
 	{
 		// Get only the lower bits where the triangle index is stored
 		unsigned int x = 0;
-		unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);
-		return (m_PartIdTriangleIndex&~(y));
+		unsigned int y = (~(x & 0)) << (31 - MAX_NUM_PARTS_IN_BITS);
+		return (m_PartIdTriangleIndex & ~(y));
 	}
-	int	getPartId() const
+	int getPartId() const
 	{
 		// Get only the highest bits where the part index is stored
-		return (m_PartIdTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));
+		return (m_PartIdTriangleIndex >> (31 - MAX_NUM_PARTS_IN_BITS));
 	}
-	int	getUid() const
+	int getUid() const
 	{
 		return m_PartIdTriangleIndex;
 	}
 };
 
-
 ///For each triangle in the concave mesh that overlaps with the AABB of a soft body (m_softBody), processTriangle is called.
 class btSoftBodyTriangleCallback : public btTriangleCallback
 {
 	btSoftBody* m_softBody;
 	const btCollisionObject* m_triBody;
 
-	btVector3	m_aabbMin;
-	btVector3	m_aabbMax ;
+	btVector3 m_aabbMin;
+	btVector3 m_aabbMax;
 
 	btManifoldResult* m_resultOut;
 
-	btDispatcher*	m_dispatcher;
+	btDispatcher* m_dispatcher;
 	const btDispatcherInfo* m_dispatchInfoPtr;
 	btScalar m_collisionMarginTriangle;
 
-	btHashMap<btHashKey<btTriIndex>,btTriIndex> m_shapeCache;
+	btHashMap<btHashKey<btTriIndex>, btTriIndex> m_shapeCache;
 
 public:
-	int	m_triangleCount;
+	int m_triangleCount;
 
 	//	btPersistentManifold*	m_manifoldPtr;
 
-	btSoftBodyTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
+	btSoftBodyTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped);
 
-	void	setTimeStepAndCounters(btScalar collisionMarginTriangle,const btCollisionObjectWrapper* triObjWrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	void setTimeStepAndCounters(btScalar collisionMarginTriangle, const btCollisionObjectWrapper* triObjWrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
 	virtual ~btSoftBodyTriangleCallback();
 
@@ -101,55 +100,48 @@ public:
 	{
 		return m_aabbMax;
 	}
-
 };
 
-
-
-
 /// btSoftBodyConcaveCollisionAlgorithm  supports collision between soft body shapes and (concave) trianges meshes.
-class btSoftBodyConcaveCollisionAlgorithm  : public btCollisionAlgorithm
+class btSoftBodyConcaveCollisionAlgorithm : public btCollisionAlgorithm
 {
-
-	bool	m_isSwapped;
+	bool m_isSwapped;
 
 	btSoftBodyTriangleCallback m_btSoftBodyTriangleCallback;
 
 public:
-
-	btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
+	btSoftBodyConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped);
 
 	virtual ~btSoftBodyConcaveCollisionAlgorithm();
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
 	{
 		//we don't add any manifolds
 	}
 
-	void	clearCache();
+	void clearCache();
 
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm));
-			return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,false);
+			return new (mem) btSoftBodyConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, false);
 		}
 	};
 
-	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm));
-			return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,true);
+			return new (mem) btSoftBodyConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, true);
 		}
 	};
-
 };
 
-#endif //BT_SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H
+#endif  //BT_SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyData.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyData.h
index 87d8841cfa4..cec6f401ecc 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftBodyData.h
+++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyData.h
@@ -19,199 +19,194 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
-
-struct	SoftBodyMaterialData
+struct SoftBodyMaterialData
 {
-	float	m_linearStiffness;
-	float	m_angularStiffness;
-	float	m_volumeStiffness;
-	int		m_flags;
+	float m_linearStiffness;
+	float m_angularStiffness;
+	float m_volumeStiffness;
+	int m_flags;
 };
 
-struct	SoftBodyNodeData
+struct SoftBodyNodeData
 {
-	SoftBodyMaterialData		*m_material;
-	btVector3FloatData			m_position;
-	btVector3FloatData			m_previousPosition;
-	btVector3FloatData			m_velocity;
-	btVector3FloatData			m_accumulatedForce;
-	btVector3FloatData			m_normal;
-	float						m_inverseMass;
-	float						m_area;
-	int							m_attach;
-	int							m_pad;
+	SoftBodyMaterialData *m_material;
+	btVector3FloatData m_position;
+	btVector3FloatData m_previousPosition;
+	btVector3FloatData m_velocity;
+	btVector3FloatData m_accumulatedForce;
+	btVector3FloatData m_normal;
+	float m_inverseMass;
+	float m_area;
+	int m_attach;
+	int m_pad;
 };
 
-struct	SoftBodyLinkData
+struct SoftBodyLinkData
 {
-	SoftBodyMaterialData	*m_material;
-	int						m_nodeIndices[2];			// Node pointers
-	float					m_restLength;			// Rest length		
-	int						m_bbending;		// Bending link
+	SoftBodyMaterialData *m_material;
+	int m_nodeIndices[2];  // Node pointers
+	float m_restLength;    // Rest length
+	int m_bbending;        // Bending link
 };
 
-struct	SoftBodyFaceData
+struct SoftBodyFaceData
 {
-	btVector3FloatData		m_normal;		// Normal
-	SoftBodyMaterialData	*m_material;
-	int						m_nodeIndices[3];			// Node pointers
-	float					m_restArea;			// Rest area
-};	
+	btVector3FloatData m_normal;  // Normal
+	SoftBodyMaterialData *m_material;
+	int m_nodeIndices[3];  // Node pointers
+	float m_restArea;      // Rest area
+};
 
-struct	SoftBodyTetraData
+struct SoftBodyTetraData
 {
-	btVector3FloatData		m_c0[4];		// gradients
-	SoftBodyMaterialData	*m_material;
-	int						m_nodeIndices[4];			// Node pointers		
-	float					m_restVolume;			// Rest volume
-	float					m_c1;			// (4*kVST)/(im0+im1+im2+im3)
-	float					m_c2;			// m_c1/sum(|g0..3|^2)
-	int						m_pad;
+	btVector3FloatData m_c0[4];  // gradients
+	SoftBodyMaterialData *m_material;
+	int m_nodeIndices[4];  // Node pointers
+	float m_restVolume;    // Rest volume
+	float m_c1;            // (4*kVST)/(im0+im1+im2+im3)
+	float m_c2;            // m_c1/sum(|g0..3|^2)
+	int m_pad;
 };
 
-struct	SoftRigidAnchorData
+struct SoftRigidAnchorData
 {
-	btMatrix3x3FloatData	m_c0;			// Impulse matrix
-	btVector3FloatData		m_c1;			// Relative anchor
-	btVector3FloatData		m_localFrame;		// Anchor position in body space
-	btRigidBodyData			*m_rigidBody;
-	int						m_nodeIndex;			// Node pointer
-	float					m_c2;			// ima*dt
+	btMatrix3x3FloatData m_c0;        // Impulse matrix
+	btVector3FloatData m_c1;          // Relative anchor
+	btVector3FloatData m_localFrame;  // Anchor position in body space
+	btRigidBodyData *m_rigidBody;
+	int m_nodeIndex;  // Node pointer
+	float m_c2;       // ima*dt
 };
 
-
-
-struct	SoftBodyConfigData
+struct SoftBodyConfigData
 {
-	int					m_aeroModel;		// Aerodynamic model (default: V_Point)
-	float				m_baumgarte;			// Velocities correction factor (Baumgarte)
-	float				m_damping;			// Damping coefficient [0,1]
-	float				m_drag;			// Drag coefficient [0,+inf]
-	float				m_lift;			// Lift coefficient [0,+inf]
-	float				m_pressure;			// Pressure coefficient [-inf,+inf]
-	float				m_volume;			// Volume conversation coefficient [0,+inf]
-	float				m_dynamicFriction;			// Dynamic friction coefficient [0,1]
-	float				m_poseMatch;			// Pose matching coefficient [0,1]		
-	float				m_rigidContactHardness;			// Rigid contacts hardness [0,1]
-	float				m_kineticContactHardness;			// Kinetic contacts hardness [0,1]
-	float				m_softContactHardness;			// Soft contacts hardness [0,1]
-	float				m_anchorHardness;			// Anchors hardness [0,1]
-	float				m_softRigidClusterHardness;		// Soft vs rigid hardness [0,1] (cluster only)
-	float				m_softKineticClusterHardness;		// Soft vs kinetic hardness [0,1] (cluster only)
-	float				m_softSoftClusterHardness;		// Soft vs soft hardness [0,1] (cluster only)
-	float				m_softRigidClusterImpulseSplit;	// Soft vs rigid impulse split [0,1] (cluster only)
-	float				m_softKineticClusterImpulseSplit;	// Soft vs rigid impulse split [0,1] (cluster only)
-	float				m_softSoftClusterImpulseSplit;	// Soft vs rigid impulse split [0,1] (cluster only)
-	float				m_maxVolume;		// Maximum volume ratio for pose
-	float				m_timeScale;		// Time scale
-	int					m_velocityIterations;	// Velocities solver iterations
-	int					m_positionIterations;	// Positions solver iterations
-	int					m_driftIterations;	// Drift solver iterations
-	int					m_clusterIterations;	// Cluster solver iterations
-	int					m_collisionFlags;	// Collisions flags
+	int m_aeroModel;                         // Aerodynamic model (default: V_Point)
+	float m_baumgarte;                       // Velocities correction factor (Baumgarte)
+	float m_damping;                         // Damping coefficient [0,1]
+	float m_drag;                            // Drag coefficient [0,+inf]
+	float m_lift;                            // Lift coefficient [0,+inf]
+	float m_pressure;                        // Pressure coefficient [-inf,+inf]
+	float m_volume;                          // Volume conversation coefficient [0,+inf]
+	float m_dynamicFriction;                 // Dynamic friction coefficient [0,1]
+	float m_poseMatch;                       // Pose matching coefficient [0,1]
+	float m_rigidContactHardness;            // Rigid contacts hardness [0,1]
+	float m_kineticContactHardness;          // Kinetic contacts hardness [0,1]
+	float m_softContactHardness;             // Soft contacts hardness [0,1]
+	float m_anchorHardness;                  // Anchors hardness [0,1]
+	float m_softRigidClusterHardness;        // Soft vs rigid hardness [0,1] (cluster only)
+	float m_softKineticClusterHardness;      // Soft vs kinetic hardness [0,1] (cluster only)
+	float m_softSoftClusterHardness;         // Soft vs soft hardness [0,1] (cluster only)
+	float m_softRigidClusterImpulseSplit;    // Soft vs rigid impulse split [0,1] (cluster only)
+	float m_softKineticClusterImpulseSplit;  // Soft vs rigid impulse split [0,1] (cluster only)
+	float m_softSoftClusterImpulseSplit;     // Soft vs rigid impulse split [0,1] (cluster only)
+	float m_maxVolume;                       // Maximum volume ratio for pose
+	float m_timeScale;                       // Time scale
+	int m_velocityIterations;                // Velocities solver iterations
+	int m_positionIterations;                // Positions solver iterations
+	int m_driftIterations;                   // Drift solver iterations
+	int m_clusterIterations;                 // Cluster solver iterations
+	int m_collisionFlags;                    // Collisions flags
 };
 
-struct	SoftBodyPoseData
+struct SoftBodyPoseData
 {
-	btMatrix3x3FloatData	m_rot;			// Rotation
-	btMatrix3x3FloatData	m_scale;			// Scale
-	btMatrix3x3FloatData	m_aqq;			// Base scaling
-	btVector3FloatData		m_com;			// COM
-
-	btVector3FloatData		*m_positions;			// Reference positions
-	float					*m_weights;	// Weights
-	int						m_numPositions;
-	int						m_numWeigts;
-
-	int						m_bvolume;		// Is valid
-	int						m_bframe;		// Is frame
-	float					m_restVolume;		// Rest volume
-	int						m_pad;
+	btMatrix3x3FloatData m_rot;    // Rotation
+	btMatrix3x3FloatData m_scale;  // Scale
+	btMatrix3x3FloatData m_aqq;    // Base scaling
+	btVector3FloatData m_com;      // COM
+
+	btVector3FloatData *m_positions;  // Reference positions
+	float *m_weights;                 // Weights
+	int m_numPositions;
+	int m_numWeigts;
+
+	int m_bvolume;       // Is valid
+	int m_bframe;        // Is frame
+	float m_restVolume;  // Rest volume
+	int m_pad;
 };
 
-struct	SoftBodyClusterData
+struct SoftBodyClusterData
 {
-		btTransformFloatData		m_framexform;
-		btMatrix3x3FloatData		m_locii;
-		btMatrix3x3FloatData		m_invwi;
-		btVector3FloatData			m_com;
-		btVector3FloatData			m_vimpulses[2];
-		btVector3FloatData			m_dimpulses[2];
-		btVector3FloatData			m_lv;
-		btVector3FloatData			m_av;
-		
-		btVector3FloatData			*m_framerefs;
-		int							*m_nodeIndices;
-		float						*m_masses;
-
-		int							m_numFrameRefs;
-		int							m_numNodes;
-		int							m_numMasses;
-
-		float						m_idmass;
-		float						m_imass;
-		int							m_nvimpulses;
-		int							m_ndimpulses;
-		float						m_ndamping;
-		float						m_ldamping;
-		float						m_adamping;
-		float						m_matching;
-		float						m_maxSelfCollisionImpulse;
-		float						m_selfCollisionImpulseFactor;
-		int							m_containsAnchor;
-		int							m_collide;
-		int							m_clusterIndex;
+	btTransformFloatData m_framexform;
+	btMatrix3x3FloatData m_locii;
+	btMatrix3x3FloatData m_invwi;
+	btVector3FloatData m_com;
+	btVector3FloatData m_vimpulses[2];
+	btVector3FloatData m_dimpulses[2];
+	btVector3FloatData m_lv;
+	btVector3FloatData m_av;
+
+	btVector3FloatData *m_framerefs;
+	int *m_nodeIndices;
+	float *m_masses;
+
+	int m_numFrameRefs;
+	int m_numNodes;
+	int m_numMasses;
+
+	float m_idmass;
+	float m_imass;
+	int m_nvimpulses;
+	int m_ndimpulses;
+	float m_ndamping;
+	float m_ldamping;
+	float m_adamping;
+	float m_matching;
+	float m_maxSelfCollisionImpulse;
+	float m_selfCollisionImpulseFactor;
+	int m_containsAnchor;
+	int m_collide;
+	int m_clusterIndex;
 };
 
-
-enum	btSoftJointBodyType
+enum btSoftJointBodyType
 {
-	BT_JOINT_SOFT_BODY_CLUSTER=1,
+	BT_JOINT_SOFT_BODY_CLUSTER = 1,
 	BT_JOINT_RIGID_BODY,
 	BT_JOINT_COLLISION_OBJECT
 };
 
-struct	btSoftBodyJointData
+struct btSoftBodyJointData
 {
-	void						*m_bodyA;
-	void						*m_bodyB;
-	btVector3FloatData			m_refs[2];
-	float						m_cfm;
-	float						m_erp;
-	float						m_split;
-	int							m_delete;
-	btVector3FloatData			m_relPosition[2];//linear
-	int							m_bodyAtype;
-	int							m_bodyBtype;
-	int							m_jointType;
-	int							m_pad;
+	void *m_bodyA;
+	void *m_bodyB;
+	btVector3FloatData m_refs[2];
+	float m_cfm;
+	float m_erp;
+	float m_split;
+	int m_delete;
+	btVector3FloatData m_relPosition[2];  //linear
+	int m_bodyAtype;
+	int m_bodyBtype;
+	int m_jointType;
+	int m_pad;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btSoftBodyFloatData
+struct btSoftBodyFloatData
 {
-	btCollisionObjectFloatData	m_collisionObjectData;
-
-	SoftBodyPoseData		*m_pose;
-	SoftBodyMaterialData	**m_materials;
-	SoftBodyNodeData		*m_nodes;
-	SoftBodyLinkData		*m_links;
-	SoftBodyFaceData		*m_faces;
-	SoftBodyTetraData		*m_tetrahedra;
-	SoftRigidAnchorData		*m_anchors;
-	SoftBodyClusterData		*m_clusters;
-	btSoftBodyJointData		*m_joints;
-
-	int						m_numMaterials;
-	int						m_numNodes;
-	int						m_numLinks;
-	int						m_numFaces;
-	int						m_numTetrahedra;
-	int						m_numAnchors;
-	int						m_numClusters;
-	int						m_numJoints;
-	SoftBodyConfigData		m_config;
+	btCollisionObjectFloatData m_collisionObjectData;
+
+	SoftBodyPoseData *m_pose;
+	SoftBodyMaterialData **m_materials;
+	SoftBodyNodeData *m_nodes;
+	SoftBodyLinkData *m_links;
+	SoftBodyFaceData *m_faces;
+	SoftBodyTetraData *m_tetrahedra;
+	SoftRigidAnchorData *m_anchors;
+	SoftBodyClusterData *m_clusters;
+	btSoftBodyJointData *m_joints;
+
+	int m_numMaterials;
+	int m_numNodes;
+	int m_numLinks;
+	int m_numFaces;
+	int m_numTetrahedra;
+	int m_numAnchors;
+	int m_numClusters;
+	int m_numJoints;
+	SoftBodyConfigData m_config;
 };
 
-#endif //BT_SOFTBODY_FLOAT_DATA
-
+#endif  //BT_SOFTBODY_FLOAT_DATA
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp
index d96f85ec630..c30b65e895a 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp
+++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp
@@ -16,115 +16,126 @@ subject to the following restrictions:
 
 #include "btSoftBodyInternals.h"
 #include <stdio.h>
+#include <string>
+#include <iostream>
+#include <sstream>
 #include <string.h>
+#include <algorithm>
 #include "btSoftBodyHelpers.h"
 #include "LinearMath/btConvexHull.h"
 #include "LinearMath/btConvexHullComputer.h"
+#include <map>
+#include <vector>
 
-
-//
-static void				drawVertex(	btIDebugDraw* idraw,
-								   const btVector3& x,btScalar s,const btVector3& c)
+static void drawVertex(btIDebugDraw* idraw,
+					   const btVector3& x, btScalar s, const btVector3& c)
 {
-	idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
-	idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
-	idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
+	idraw->drawLine(x - btVector3(s, 0, 0), x + btVector3(s, 0, 0), c);
+	idraw->drawLine(x - btVector3(0, s, 0), x + btVector3(0, s, 0), c);
+	idraw->drawLine(x - btVector3(0, 0, s), x + btVector3(0, 0, s), c);
 }
 
 //
-static void				drawBox(	btIDebugDraw* idraw,
-								const btVector3& mins,
-								const btVector3& maxs,
-								const btVector3& color)
+static void drawBox(btIDebugDraw* idraw,
+					const btVector3& mins,
+					const btVector3& maxs,
+					const btVector3& color)
 {
-	const btVector3	c[]={	btVector3(mins.x(),mins.y(),mins.z()),
-		btVector3(maxs.x(),mins.y(),mins.z()),
-		btVector3(maxs.x(),maxs.y(),mins.z()),
-		btVector3(mins.x(),maxs.y(),mins.z()),
-		btVector3(mins.x(),mins.y(),maxs.z()),
-		btVector3(maxs.x(),mins.y(),maxs.z()),
-		btVector3(maxs.x(),maxs.y(),maxs.z()),
-		btVector3(mins.x(),maxs.y(),maxs.z())};
-	idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
-	idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
-	idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
-	idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
-	idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
-	idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
+	const btVector3 c[] = {btVector3(mins.x(), mins.y(), mins.z()),
+						   btVector3(maxs.x(), mins.y(), mins.z()),
+						   btVector3(maxs.x(), maxs.y(), mins.z()),
+						   btVector3(mins.x(), maxs.y(), mins.z()),
+						   btVector3(mins.x(), mins.y(), maxs.z()),
+						   btVector3(maxs.x(), mins.y(), maxs.z()),
+						   btVector3(maxs.x(), maxs.y(), maxs.z()),
+						   btVector3(mins.x(), maxs.y(), maxs.z())};
+	idraw->drawLine(c[0], c[1], color);
+	idraw->drawLine(c[1], c[2], color);
+	idraw->drawLine(c[2], c[3], color);
+	idraw->drawLine(c[3], c[0], color);
+	idraw->drawLine(c[4], c[5], color);
+	idraw->drawLine(c[5], c[6], color);
+	idraw->drawLine(c[6], c[7], color);
+	idraw->drawLine(c[7], c[4], color);
+	idraw->drawLine(c[0], c[4], color);
+	idraw->drawLine(c[1], c[5], color);
+	idraw->drawLine(c[2], c[6], color);
+	idraw->drawLine(c[3], c[7], color);
 }
 
 //
-static void				drawTree(	btIDebugDraw* idraw,
-								 const btDbvtNode* node,
-								 int depth,
-								 const btVector3& ncolor,
-								 const btVector3& lcolor,
-								 int mindepth,
-								 int maxdepth)
+static void drawTree(btIDebugDraw* idraw,
+					 const btDbvtNode* node,
+					 int depth,
+					 const btVector3& ncolor,
+					 const btVector3& lcolor,
+					 int mindepth,
+					 int maxdepth)
 {
-	if(node)
+	if (node)
 	{
-		if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
+		if (node->isinternal() && ((depth < maxdepth) || (maxdepth < 0)))
 		{
-			drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);
-			drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);
+			drawTree(idraw, node->childs[0], depth + 1, ncolor, lcolor, mindepth, maxdepth);
+			drawTree(idraw, node->childs[1], depth + 1, ncolor, lcolor, mindepth, maxdepth);
 		}
-		if(depth>=mindepth)
+		if (depth >= mindepth)
 		{
-			const btScalar	scl=(btScalar)(node->isinternal()?1:1);
-			const btVector3	mi=node->volume.Center()-node->volume.Extents()*scl;
-			const btVector3	mx=node->volume.Center()+node->volume.Extents()*scl;
-			drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
+			const btScalar scl = (btScalar)(node->isinternal() ? 1 : 1);
+			const btVector3 mi = node->volume.Center() - node->volume.Extents() * scl;
+			const btVector3 mx = node->volume.Center() + node->volume.Extents() * scl;
+			drawBox(idraw, mi, mx, node->isleaf() ? lcolor : ncolor);
 		}
 	}
 }
 
 //
 template <typename T>
-static inline T				sum(const btAlignedObjectArray<T>& items)
+static inline T sum(const btAlignedObjectArray<T>& items)
 {
-	T	v;
-	if(items.size())
+	T v;
+	if (items.size())
 	{
-		v=items[0];
-		for(int i=1,ni=items.size();i<ni;++i)
+		v = items[0];
+		for (int i = 1, ni = items.size(); i < ni; ++i)
 		{
-			v+=items[i];
+			v += items[i];
 		}
 	}
-	return(v);
+	return (v);
 }
 
 //
-template <typename T,typename Q>
-static inline void			add(btAlignedObjectArray<T>& items,const Q& value)
+template <typename T, typename Q>
+static inline void add(btAlignedObjectArray<T>& items, const Q& value)
 {
-	for(int i=0,ni=items.size();i<ni;++i)
+	for (int i = 0, ni = items.size(); i < ni; ++i)
 	{
-		items[i]+=value;
+		items[i] += value;
 	}
 }
 
 //
-template <typename T,typename Q>
-static inline void			mul(btAlignedObjectArray<T>& items,const Q& value)
+template <typename T, typename Q>
+static inline void mul(btAlignedObjectArray<T>& items, const Q& value)
 {
-	for(int i=0,ni=items.size();i<ni;++i)
+	for (int i = 0, ni = items.size(); i < ni; ++i)
 	{
-		items[i]*=value;
+		items[i] *= value;
 	}
 }
 
 //
 template <typename T>
-static inline T				average(const btAlignedObjectArray<T>& items)
+static inline T average(const btAlignedObjectArray<T>& items)
 {
-	const btScalar	n=(btScalar)(items.size()>0?items.size():1);
-	return(sum(items)/n);
+	const btScalar n = (btScalar)(items.size() > 0 ? items.size() : 1);
+	return (sum(items) / n);
 }
 
+#if 0
 //
-static inline btScalar		tetravolume(const btVector3& x0,
+ inline static btScalar		tetravolume(const btVector3& x0,
 										const btVector3& x1,
 										const btVector3& x2,
 										const btVector3& x3)
@@ -134,6 +145,7 @@ static inline btScalar		tetravolume(const btVector3& x0,
 	const btVector3	c=x3-x0;
 	return(btDot(a,btCross(b,c)));
 }
+#endif
 
 //
 #if 0
@@ -156,86 +168,84 @@ static btVector3		stresscolor(btScalar stress)
 #endif
 
 //
-void			btSoftBodyHelpers::Draw(	btSoftBody* psb,
-										btIDebugDraw* idraw,
-										int drawflags)
+void btSoftBodyHelpers::Draw(btSoftBody* psb,
+							 btIDebugDraw* idraw,
+							 int drawflags)
 {
-	const btScalar		scl=(btScalar)0.1;
-	const btScalar		nscl=scl*5;
-	const btVector3		lcolor=btVector3(0,0,0);
-	const btVector3		ncolor=btVector3(1,1,1);
-	const btVector3		ccolor=btVector3(1,0,0);
-	int i,j,nj;
-
-		/* Clusters	*/ 
-	if(0!=(drawflags&fDrawFlags::Clusters))
+	const btScalar scl = (btScalar)0.1;
+	const btScalar nscl = scl * 5;
+	const btVector3 lcolor = btVector3(0, 0, 0);
+	const btVector3 ncolor = btVector3(1, 1, 1);
+	const btVector3 ccolor = btVector3(1, 0, 0);
+	int i, j, nj;
+
+	/* Clusters	*/
+	if (0 != (drawflags & fDrawFlags::Clusters))
 	{
 		srand(1806);
-		for(i=0;i<psb->m_clusters.size();++i)
+		for (i = 0; i < psb->m_clusters.size(); ++i)
 		{
-			if(psb->m_clusters[i]->m_collide)
+			if (psb->m_clusters[i]->m_collide)
 			{
-				btVector3						color(	rand()/(btScalar)RAND_MAX,
-					rand()/(btScalar)RAND_MAX,
-					rand()/(btScalar)RAND_MAX);
-				color=color.normalized()*0.75;
-				btAlignedObjectArray<btVector3>	vertices;
+				btVector3 color(rand() / (btScalar)RAND_MAX,
+								rand() / (btScalar)RAND_MAX,
+								rand() / (btScalar)RAND_MAX);
+				color = color.normalized() * 0.75;
+				btAlignedObjectArray<btVector3> vertices;
 				vertices.resize(psb->m_clusters[i]->m_nodes.size());
-				for(j=0,nj=vertices.size();j<nj;++j)
-				{				
-					vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x;
+				for (j = 0, nj = vertices.size(); j < nj; ++j)
+				{
+					vertices[j] = psb->m_clusters[i]->m_nodes[j]->m_x;
 				}
 #define USE_NEW_CONVEX_HULL_COMPUTER
 #ifdef USE_NEW_CONVEX_HULL_COMPUTER
-				btConvexHullComputer	computer;
+				btConvexHullComputer computer;
 				int stride = sizeof(btVector3);
 				int count = vertices.size();
-				btScalar shrink=0.f;
-				btScalar shrinkClamp=0.f;
-				computer.compute(&vertices[0].getX(),stride,count,shrink,shrinkClamp);
-				for (int i=0;i<computer.faces.size();i++)
+				btScalar shrink = 0.f;
+				btScalar shrinkClamp = 0.f;
+				computer.compute(&vertices[0].getX(), stride, count, shrink, shrinkClamp);
+				for (int i = 0; i < computer.faces.size(); i++)
 				{
-
 					int face = computer.faces[i];
 					//printf("face=%d\n",face);
-					const btConvexHullComputer::Edge*  firstEdge = &computer.edges[face];
-					const btConvexHullComputer::Edge*  edge = firstEdge->getNextEdgeOfFace();
+					const btConvexHullComputer::Edge* firstEdge = &computer.edges[face];
+					const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace();
 
 					int v0 = firstEdge->getSourceVertex();
 					int v1 = firstEdge->getTargetVertex();
-					while (edge!=firstEdge)
+					while (edge != firstEdge)
 					{
 						int v2 = edge->getTargetVertex();
-						idraw->drawTriangle(computer.vertices[v0],computer.vertices[v1],computer.vertices[v2],color,1);
+						idraw->drawTriangle(computer.vertices[v0], computer.vertices[v1], computer.vertices[v2], color, 1);
 						edge = edge->getNextEdgeOfFace();
-						v0=v1;
-						v1=v2;
+						v0 = v1;
+						v1 = v2;
 					};
 				}
 #else
 
-				HullDesc		hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]);
-				HullResult		hres;
-				HullLibrary		hlib;
-				hdsc.mMaxVertices=vertices.size();
-				hlib.CreateConvexHull(hdsc,hres);
-				const btVector3	center=average(hres.m_OutputVertices);
-				add(hres.m_OutputVertices,-center);
-				mul(hres.m_OutputVertices,(btScalar)1);
-				add(hres.m_OutputVertices,center);
-				for(j=0;j<(int)hres.mNumFaces;++j)
+				HullDesc hdsc(QF_TRIANGLES, vertices.size(), &vertices[0]);
+				HullResult hres;
+				HullLibrary hlib;
+				hdsc.mMaxVertices = vertices.size();
+				hlib.CreateConvexHull(hdsc, hres);
+				const btVector3 center = average(hres.m_OutputVertices);
+				add(hres.m_OutputVertices, -center);
+				mul(hres.m_OutputVertices, (btScalar)1);
+				add(hres.m_OutputVertices, center);
+				for (j = 0; j < (int)hres.mNumFaces; ++j)
 				{
-					const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]};
+					const int idx[] = {hres.m_Indices[j * 3 + 0], hres.m_Indices[j * 3 + 1], hres.m_Indices[j * 3 + 2]};
 					idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
-						hres.m_OutputVertices[idx[1]],
-						hres.m_OutputVertices[idx[2]],
-						color,1);
+										hres.m_OutputVertices[idx[1]],
+										hres.m_OutputVertices[idx[2]],
+										color, 1);
 				}
 				hlib.ReleaseResult(hres);
 #endif
-
 			}
-			/* Velocities	*/ 
+			/* Velocities	*/
 #if 0
 			for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
 			{
@@ -245,273 +255,269 @@ void			btSoftBodyHelpers::Draw(	btSoftBody* psb,
 				idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
 			}
 #endif
-			/* Frame		*/ 
-	//		btSoftBody::Cluster& c=*psb->m_clusters[i];
-	//		idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));
-	//		idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
-	//		idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
+			/* Frame		*/
+			//		btSoftBody::Cluster& c=*psb->m_clusters[i];
+			//		idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));
+			//		idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
+			//		idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
 		}
 	}
 	else
 	{
-		/* Nodes	*/ 
-		if(0!=(drawflags&fDrawFlags::Nodes))
+		/* Nodes	*/
+		if (0 != (drawflags & fDrawFlags::Nodes))
 		{
-			for(i=0;i<psb->m_nodes.size();++i)
+			for (i = 0; i < psb->m_nodes.size(); ++i)
 			{
-				const btSoftBody::Node&	n=psb->m_nodes[i];
-				if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
-				idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0));
-				idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0));
-				idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1));
+				const btSoftBody::Node& n = psb->m_nodes[i];
+				if (0 == (n.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
+				idraw->drawLine(n.m_x - btVector3(scl, 0, 0), n.m_x + btVector3(scl, 0, 0), btVector3(1, 0, 0));
+				idraw->drawLine(n.m_x - btVector3(0, scl, 0), n.m_x + btVector3(0, scl, 0), btVector3(0, 1, 0));
+				idraw->drawLine(n.m_x - btVector3(0, 0, scl), n.m_x + btVector3(0, 0, scl), btVector3(0, 0, 1));
 			}
 		}
-		/* Links	*/ 
-		if(0!=(drawflags&fDrawFlags::Links))
+		/* Links	*/
+		if (0 != (drawflags & fDrawFlags::Links))
 		{
-			for(i=0;i<psb->m_links.size();++i)
+			for (i = 0; i < psb->m_links.size(); ++i)
 			{
-				const btSoftBody::Link&	l=psb->m_links[i];
-				if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
-				idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor);
+				const btSoftBody::Link& l = psb->m_links[i];
+				if (0 == (l.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
+				idraw->drawLine(l.m_n[0]->m_x, l.m_n[1]->m_x, lcolor);
 			}
 		}
-		/* Normals	*/ 
-		if(0!=(drawflags&fDrawFlags::Normals))
+		/* Normals	*/
+		if (0 != (drawflags & fDrawFlags::Normals))
 		{
-			for(i=0;i<psb->m_nodes.size();++i)
+			for (i = 0; i < psb->m_nodes.size(); ++i)
 			{
-				const btSoftBody::Node&	n=psb->m_nodes[i];
-				if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
-				const btVector3			d=n.m_n*nscl;
-				idraw->drawLine(n.m_x,n.m_x+d,ncolor);
-				idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5);
+				const btSoftBody::Node& n = psb->m_nodes[i];
+				if (0 == (n.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
+				const btVector3 d = n.m_n * nscl;
+				idraw->drawLine(n.m_x, n.m_x + d, ncolor);
+				idraw->drawLine(n.m_x, n.m_x - d, ncolor * 0.5);
 			}
 		}
-		/* Contacts	*/ 
-		if(0!=(drawflags&fDrawFlags::Contacts))
+		/* Contacts	*/
+		if (0 != (drawflags & fDrawFlags::Contacts))
 		{
-			static const btVector3		axis[]={btVector3(1,0,0),
-				btVector3(0,1,0),
-				btVector3(0,0,1)};
-			for(i=0;i<psb->m_rcontacts.size();++i)
-			{		
-				const btSoftBody::RContact&	c=psb->m_rcontacts[i];
-				const btVector3				o=	c.m_node->m_x-c.m_cti.m_normal*
-					(btDot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset);
-				const btVector3				x=btCross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized();
-				const btVector3				y=btCross(x,c.m_cti.m_normal).normalized();
-				idraw->drawLine(o-x*nscl,o+x*nscl,ccolor);
-				idraw->drawLine(o-y*nscl,o+y*nscl,ccolor);
-				idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0));
+			static const btVector3 axis[] = {btVector3(1, 0, 0),
+											 btVector3(0, 1, 0),
+											 btVector3(0, 0, 1)};
+			for (i = 0; i < psb->m_rcontacts.size(); ++i)
+			{
+				const btSoftBody::RContact& c = psb->m_rcontacts[i];
+				const btVector3 o = c.m_node->m_x - c.m_cti.m_normal *
+														(btDot(c.m_node->m_x, c.m_cti.m_normal) + c.m_cti.m_offset);
+				const btVector3 x = btCross(c.m_cti.m_normal, axis[c.m_cti.m_normal.minAxis()]).normalized();
+				const btVector3 y = btCross(x, c.m_cti.m_normal).normalized();
+				idraw->drawLine(o - x * nscl, o + x * nscl, ccolor);
+				idraw->drawLine(o - y * nscl, o + y * nscl, ccolor);
+				idraw->drawLine(o, o + c.m_cti.m_normal * nscl * 3, btVector3(1, 1, 0));
 			}
 		}
-		/* Faces	*/ 
-	if(0!=(drawflags&fDrawFlags::Faces))
-	{
-		const btScalar	scl=(btScalar)0.8;
-		const btScalar	alp=(btScalar)1;
-		const btVector3	col(0,(btScalar)0.7,0);
-		for(i=0;i<psb->m_faces.size();++i)
+		/* Faces	*/
+		if (0 != (drawflags & fDrawFlags::Faces))
 		{
-			const btSoftBody::Face&	f=psb->m_faces[i];
-			if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
-			const btVector3			x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x};
-			const btVector3			c=(x[0]+x[1]+x[2])/3;
-			idraw->drawTriangle((x[0]-c)*scl+c,
-				(x[1]-c)*scl+c,
-				(x[2]-c)*scl+c,
-				col,alp);
-		}	
-	}
-	/* Tetras	*/ 
-	if(0!=(drawflags&fDrawFlags::Tetras))
-	{
-		const btScalar	scl=(btScalar)0.8;
-		const btScalar	alp=(btScalar)1;
-		const btVector3	col((btScalar)0.3,(btScalar)0.3,(btScalar)0.7);
-		for(int i=0;i<psb->m_tetras.size();++i)
+			const btScalar scl = (btScalar)0.8;
+			const btScalar alp = (btScalar)1;
+			const btVector3 col(0, (btScalar)0.7, 0);
+			for (i = 0; i < psb->m_faces.size(); ++i)
+			{
+				const btSoftBody::Face& f = psb->m_faces[i];
+				if (0 == (f.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
+				const btVector3 x[] = {f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x};
+				const btVector3 c = (x[0] + x[1] + x[2]) / 3;
+				idraw->drawTriangle((x[0] - c) * scl + c,
+									(x[1] - c) * scl + c,
+									(x[2] - c) * scl + c,
+									col, alp);
+			}
+		}
+		/* Tetras	*/
+		if (0 != (drawflags & fDrawFlags::Tetras))
 		{
-			const btSoftBody::Tetra&	t=psb->m_tetras[i];
-			if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
-			const btVector3				x[]={t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x};
-			const btVector3				c=(x[0]+x[1]+x[2]+x[3])/4;
-			idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp);
-			idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
-			idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
-			idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
-		}	
-	}
+			const btScalar scl = (btScalar)0.8;
+			const btScalar alp = (btScalar)1;
+			const btVector3 col((btScalar)0.3, (btScalar)0.3, (btScalar)0.7);
+			for (int i = 0; i < psb->m_tetras.size(); ++i)
+			{
+				const btSoftBody::Tetra& t = psb->m_tetras[i];
+				if (0 == (t.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
+				const btVector3 x[] = {t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x};
+				const btVector3 c = (x[0] + x[1] + x[2] + x[3]) / 4;
+				idraw->drawTriangle((x[0] - c) * scl + c, (x[1] - c) * scl + c, (x[2] - c) * scl + c, col, alp);
+				idraw->drawTriangle((x[0] - c) * scl + c, (x[1] - c) * scl + c, (x[3] - c) * scl + c, col, alp);
+				idraw->drawTriangle((x[1] - c) * scl + c, (x[2] - c) * scl + c, (x[3] - c) * scl + c, col, alp);
+				idraw->drawTriangle((x[2] - c) * scl + c, (x[0] - c) * scl + c, (x[3] - c) * scl + c, col, alp);
+			}
+		}
 	}
-	/* Anchors	*/ 
-	if(0!=(drawflags&fDrawFlags::Anchors))
+	/* Anchors	*/
+	if (0 != (drawflags & fDrawFlags::Anchors))
 	{
-		for(i=0;i<psb->m_anchors.size();++i)
+		for (i = 0; i < psb->m_anchors.size(); ++i)
 		{
-			const btSoftBody::Anchor&	a=psb->m_anchors[i];
-			const btVector3				q=a.m_body->getWorldTransform()*a.m_local;
-			drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0));
-			drawVertex(idraw,q,0.25,btVector3(0,1,0));
-			idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1));
+			const btSoftBody::Anchor& a = psb->m_anchors[i];
+			const btVector3 q = a.m_body->getWorldTransform() * a.m_local;
+			drawVertex(idraw, a.m_node->m_x, 0.25, btVector3(1, 0, 0));
+			drawVertex(idraw, q, 0.25, btVector3(0, 1, 0));
+			idraw->drawLine(a.m_node->m_x, q, btVector3(1, 1, 1));
 		}
-		for(i=0;i<psb->m_nodes.size();++i)
+		for (i = 0; i < psb->m_nodes.size(); ++i)
 		{
-			const btSoftBody::Node&	n=psb->m_nodes[i];		
-			if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
-			if(n.m_im<=0)
+			const btSoftBody::Node& n = psb->m_nodes[i];
+			if (0 == (n.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
+			if (n.m_im <= 0)
 			{
-				drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0));
+				drawVertex(idraw, n.m_x, 0.25, btVector3(1, 0, 0));
 			}
 		}
 	}
-	
 
-	/* Notes	*/ 
-	if(0!=(drawflags&fDrawFlags::Notes))
+	/* Notes	*/
+	if (0 != (drawflags & fDrawFlags::Notes))
 	{
-		for(i=0;i<psb->m_notes.size();++i)
+		for (i = 0; i < psb->m_notes.size(); ++i)
 		{
-			const btSoftBody::Note&	n=psb->m_notes[i];
-			btVector3				p=n.m_offset;
-			for(int j=0;j<n.m_rank;++j)
+			const btSoftBody::Note& n = psb->m_notes[i];
+			btVector3 p = n.m_offset;
+			for (int j = 0; j < n.m_rank; ++j)
 			{
-				p+=n.m_nodes[j]->m_x*n.m_coords[j];
+				p += n.m_nodes[j]->m_x * n.m_coords[j];
 			}
-			idraw->draw3dText(p,n.m_text);
+			idraw->draw3dText(p, n.m_text);
 		}
 	}
-	/* Node tree	*/ 
-	if(0!=(drawflags&fDrawFlags::NodeTree))		DrawNodeTree(psb,idraw);
-	/* Face tree	*/ 
-	if(0!=(drawflags&fDrawFlags::FaceTree))		DrawFaceTree(psb,idraw);
-	/* Cluster tree	*/ 
-	if(0!=(drawflags&fDrawFlags::ClusterTree))	DrawClusterTree(psb,idraw);
-	/* Joints		*/ 
-	if(0!=(drawflags&fDrawFlags::Joints))
+	/* Node tree	*/
+	if (0 != (drawflags & fDrawFlags::NodeTree)) DrawNodeTree(psb, idraw);
+	/* Face tree	*/
+	if (0 != (drawflags & fDrawFlags::FaceTree)) DrawFaceTree(psb, idraw);
+	/* Cluster tree	*/
+	if (0 != (drawflags & fDrawFlags::ClusterTree)) DrawClusterTree(psb, idraw);
+	/* Joints		*/
+	if (0 != (drawflags & fDrawFlags::Joints))
 	{
-		for(i=0;i<psb->m_joints.size();++i)
+		for (i = 0; i < psb->m_joints.size(); ++i)
 		{
-			const btSoftBody::Joint*	pj=psb->m_joints[i];
-			switch(pj->Type())
+			const btSoftBody::Joint* pj = psb->m_joints[i];
+			switch (pj->Type())
 			{
-			case	btSoftBody::Joint::eType::Linear:
+				case btSoftBody::Joint::eType::Linear:
 				{
-					const btSoftBody::LJoint*	pjl=(const btSoftBody::LJoint*)pj;
-					const btVector3	a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
-					const btVector3	a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
-					idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
-					idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
-					drawVertex(idraw,a0,0.25,btVector3(1,1,0));
-					drawVertex(idraw,a1,0.25,btVector3(0,1,1));
+					const btSoftBody::LJoint* pjl = (const btSoftBody::LJoint*)pj;
+					const btVector3 a0 = pj->m_bodies[0].xform() * pjl->m_refs[0];
+					const btVector3 a1 = pj->m_bodies[1].xform() * pjl->m_refs[1];
+					idraw->drawLine(pj->m_bodies[0].xform().getOrigin(), a0, btVector3(1, 1, 0));
+					idraw->drawLine(pj->m_bodies[1].xform().getOrigin(), a1, btVector3(0, 1, 1));
+					drawVertex(idraw, a0, 0.25, btVector3(1, 1, 0));
+					drawVertex(idraw, a1, 0.25, btVector3(0, 1, 1));
 				}
 				break;
-			case	btSoftBody::Joint::eType::Angular:
+				case btSoftBody::Joint::eType::Angular:
 				{
 					//const btSoftBody::AJoint*	pja=(const btSoftBody::AJoint*)pj;
-					const btVector3	o0=pj->m_bodies[0].xform().getOrigin();
-					const btVector3	o1=pj->m_bodies[1].xform().getOrigin();
-					const btVector3	a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
-					const btVector3	a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
-					idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
-					idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
-					idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
-					idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
+					const btVector3 o0 = pj->m_bodies[0].xform().getOrigin();
+					const btVector3 o1 = pj->m_bodies[1].xform().getOrigin();
+					const btVector3 a0 = pj->m_bodies[0].xform().getBasis() * pj->m_refs[0];
+					const btVector3 a1 = pj->m_bodies[1].xform().getBasis() * pj->m_refs[1];
+					idraw->drawLine(o0, o0 + a0 * 10, btVector3(1, 1, 0));
+					idraw->drawLine(o0, o0 + a1 * 10, btVector3(1, 1, 0));
+					idraw->drawLine(o1, o1 + a0 * 10, btVector3(0, 1, 1));
+					idraw->drawLine(o1, o1 + a1 * 10, btVector3(0, 1, 1));
 					break;
 				}
 				default:
 				{
 				}
-					
-			}		
+			}
 		}
 	}
 }
 
 //
-void			btSoftBodyHelpers::DrawInfos(		btSoftBody* psb,
-											 btIDebugDraw* idraw,
-											 bool masses,
-											 bool areas,
-											 bool /*stress*/)
+void btSoftBodyHelpers::DrawInfos(btSoftBody* psb,
+								  btIDebugDraw* idraw,
+								  bool masses,
+								  bool areas,
+								  bool /*stress*/)
 {
-	for(int i=0;i<psb->m_nodes.size();++i)
+	for (int i = 0; i < psb->m_nodes.size(); ++i)
 	{
-		const btSoftBody::Node&	n=psb->m_nodes[i];
-		char					text[2048]={0};
-		char					buff[1024];
-		if(masses)
+		const btSoftBody::Node& n = psb->m_nodes[i];
+		char text[2048] = {0};
+		char buff[1024];
+		if (masses)
 		{
-			sprintf(buff," M(%.2f)",1/n.m_im);
-			strcat(text,buff);
+			sprintf(buff, " M(%.2f)", 1 / n.m_im);
+			strcat(text, buff);
 		}
-		if(areas)
+		if (areas)
 		{
-			sprintf(buff," A(%.2f)",n.m_area);
-			strcat(text,buff);
+			sprintf(buff, " A(%.2f)", n.m_area);
+			strcat(text, buff);
 		}
-		if(text[0]) idraw->draw3dText(n.m_x,text);
+		if (text[0]) idraw->draw3dText(n.m_x, text);
 	}
 }
 
 //
-void			btSoftBodyHelpers::DrawNodeTree(	btSoftBody* psb,
-												btIDebugDraw* idraw,
-												int mindepth,
-												int maxdepth)
+void btSoftBodyHelpers::DrawNodeTree(btSoftBody* psb,
+									 btIDebugDraw* idraw,
+									 int mindepth,
+									 int maxdepth)
 {
-	drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
+	drawTree(idraw, psb->m_ndbvt.m_root, 0, btVector3(1, 0, 1), btVector3(1, 1, 1), mindepth, maxdepth);
 }
 
 //
-void			btSoftBodyHelpers::DrawFaceTree(	btSoftBody* psb,
-												btIDebugDraw* idraw,
-												int mindepth,
-												int maxdepth)
+void btSoftBodyHelpers::DrawFaceTree(btSoftBody* psb,
+									 btIDebugDraw* idraw,
+									 int mindepth,
+									 int maxdepth)
 {
-	drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
+	drawTree(idraw, psb->m_fdbvt.m_root, 0, btVector3(0, 1, 0), btVector3(1, 0, 0), mindepth, maxdepth);
 }
 
 //
-void			btSoftBodyHelpers::DrawClusterTree(	btSoftBody* psb,
-												   btIDebugDraw* idraw,
-												   int mindepth,
-												   int maxdepth)
+void btSoftBodyHelpers::DrawClusterTree(btSoftBody* psb,
+										btIDebugDraw* idraw,
+										int mindepth,
+										int maxdepth)
 {
-	drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);
+	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]. 
+// 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, 
+// - 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, 
+//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 
+// 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
@@ -520,93 +526,103 @@ void			btSoftBodyHelpers::DrawClusterTree(	btSoftBody* psb,
 //===================================================================
 
 // 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
+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;
+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
-
+#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 */)
+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 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]);
+	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
-		
+	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);
+	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++) {
+	for (i = 0; i < nNodes + 1; i++)
+	{
 		nodeWrittenAt[i] = REOP_NOT_DEPENDENT;
 	}
-	for (i=0; i < nLinks; i++) {
+	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++) {
-	
+	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) {
+		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 {
+		}
+		else
+		{
 			linkDepA[i] = REOP_NOT_DEPENDENT;
 		}
-		if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT) {
+		if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT)
+		{
 			linkDepB[i] = nodeWrittenAt[br];
 			linkDep = &linkDepFreeList[linkDepFrees++];
-			linkDep->value = -(i+1);
+			linkDep->value = -(i + 1);
 			linkDep->next = linkDepListStarts[nodeWrittenAt[br]];
 			linkDepListStarts[nodeWrittenAt[br]] = linkDep;
-		} else {
+		}
+		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)) {
+		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
+			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) {
+	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
@@ -614,180 +630,190 @@ void btSoftBodyHelpers::ReoptimizeLinkOrder(btSoftBody *psb /* This can be repla
 
 		// Free up any link inputs that are dependent on this one
 		linkDep = linkDepListStarts[linkNum];
-		while (linkDep) {
+		while (linkDep)
+		{
 			depLink = linkDep->value;
-			if (depLink >= 0) {
+			if (depLink >= 0)
+			{
 				linkDepA[depLink] = REOP_NOT_DEPENDENT;
-			} else {
+			}
+			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)) {
+			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
+				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;
+	delete[] nodeWrittenAt;
+	delete[] linkDepA;
+	delete[] linkDepB;
+	delete[] readyList;
+	delete[] linkDepFreeList;
+	delete[] linkDepListStarts;
+	delete[] linkBuffer;
 }
 
-
 //
-void			btSoftBodyHelpers::DrawFrame(		btSoftBody* psb,
-											 btIDebugDraw* idraw)
+void btSoftBodyHelpers::DrawFrame(btSoftBody* psb,
+								  btIDebugDraw* idraw)
 {
-	if(psb->m_pose.m_bframe)
+	if (psb->m_pose.m_bframe)
 	{
-		static const btScalar	ascl=10;
-		static const btScalar	nscl=(btScalar)0.1;
-		const btVector3			com=psb->m_pose.m_com;
-		const btMatrix3x3		trs=psb->m_pose.m_rot*psb->m_pose.m_scl;
-		const btVector3			Xaxis=(trs*btVector3(1,0,0)).normalized();
-		const btVector3			Yaxis=(trs*btVector3(0,1,0)).normalized();
-		const btVector3			Zaxis=(trs*btVector3(0,0,1)).normalized();
-		idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0));
-		idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0));
-		idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1));
-		for(int i=0;i<psb->m_pose.m_pos.size();++i)
+		static const btScalar ascl = 10;
+		static const btScalar nscl = (btScalar)0.1;
+		const btVector3 com = psb->m_pose.m_com;
+		const btMatrix3x3 trs = psb->m_pose.m_rot * psb->m_pose.m_scl;
+		const btVector3 Xaxis = (trs * btVector3(1, 0, 0)).normalized();
+		const btVector3 Yaxis = (trs * btVector3(0, 1, 0)).normalized();
+		const btVector3 Zaxis = (trs * btVector3(0, 0, 1)).normalized();
+		idraw->drawLine(com, com + Xaxis * ascl, btVector3(1, 0, 0));
+		idraw->drawLine(com, com + Yaxis * ascl, btVector3(0, 1, 0));
+		idraw->drawLine(com, com + Zaxis * ascl, btVector3(0, 0, 1));
+		for (int i = 0; i < psb->m_pose.m_pos.size(); ++i)
 		{
-			const btVector3	x=com+trs*psb->m_pose.m_pos[i];
-			drawVertex(idraw,x,nscl,btVector3(1,0,1));
+			const btVector3 x = com + trs * psb->m_pose.m_pos[i];
+			drawVertex(idraw, x, nscl, btVector3(1, 0, 1));
 		}
 	}
 }
 
 //
-btSoftBody*		btSoftBodyHelpers::CreateRope(	btSoftBodyWorldInfo& worldInfo, const btVector3& from,
-											  const btVector3& to,
-											  int res,
-											  int fixeds)
+btSoftBody* btSoftBodyHelpers::CreateRope(btSoftBodyWorldInfo& worldInfo, const btVector3& from,
+										  const btVector3& to,
+										  int res,
+										  int fixeds)
 {
-	/* Create nodes	*/ 
-	const int		r=res+2;
-	btVector3*		x=new btVector3[r];
-	btScalar*		m=new btScalar[r];
+	/* Create nodes	*/
+	const int r = res + 2;
+	btVector3* x = new btVector3[r];
+	btScalar* m = new btScalar[r];
 	int i;
 
-	for(i=0;i<r;++i)
+	for (i = 0; i < r; ++i)
 	{
-		const btScalar	t=i/(btScalar)(r-1);
-		x[i]=lerp(from,to,t);
-		m[i]=1;
+		const btScalar t = i / (btScalar)(r - 1);
+		x[i] = lerp(from, to, t);
+		m[i] = 1;
 	}
-	btSoftBody*		psb= new btSoftBody(&worldInfo,r,x,m);
-	if(fixeds&1) psb->setMass(0,0);
-	if(fixeds&2) psb->setMass(r-1,0);
+	btSoftBody* psb = new btSoftBody(&worldInfo, r, x, m);
+	if (fixeds & 1) psb->setMass(0, 0);
+	if (fixeds & 2) psb->setMass(r - 1, 0);
 	delete[] x;
 	delete[] m;
-	/* Create links	*/ 
-	for(i=1;i<r;++i)
+	/* Create links	*/
+	for (i = 1; i < r; ++i)
 	{
-		psb->appendLink(i-1,i);
+		psb->appendLink(i - 1, i);
 	}
-	/* Finished		*/ 
-	return(psb);
+	/* Finished		*/
+	return (psb);
 }
 
 //
-btSoftBody*		btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00,
-											   const btVector3& corner10,
-											   const btVector3& corner01,
-											   const btVector3& corner11,
-											   int resx,
-											   int resy,
-											   int fixeds,
-											   bool gendiags)
+btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const btVector3& corner00,
+										   const btVector3& corner10,
+										   const btVector3& corner01,
+										   const btVector3& corner11,
+										   int resx,
+										   int resy,
+										   int fixeds,
+										   bool gendiags,
+										   btScalar perturbation)
 {
-#define IDX(_x_,_y_)	((_y_)*rx+(_x_))
-	/* Create nodes	*/ 
-	if((resx<2)||(resy<2)) return(0);
-	const int	rx=resx;
-	const int	ry=resy;
-	const int	tot=rx*ry;
-	btVector3*	x=new btVector3[tot];
-	btScalar*	m=new btScalar[tot];
+#define IDX(_x_, _y_) ((_y_)*rx + (_x_))
+	/* Create nodes	*/
+	if ((resx < 2) || (resy < 2)) return (0);
+	const int rx = resx;
+	const int ry = resy;
+	const int tot = rx * ry;
+	btVector3* x = new btVector3[tot];
+	btScalar* m = new btScalar[tot];
 	int iy;
 
-	for(iy=0;iy<ry;++iy)
+	for (iy = 0; iy < ry; ++iy)
 	{
-		const btScalar	ty=iy/(btScalar)(ry-1);
-		const btVector3	py0=lerp(corner00,corner01,ty);
-		const btVector3	py1=lerp(corner10,corner11,ty);
-		for(int ix=0;ix<rx;++ix)
+		const btScalar ty = iy / (btScalar)(ry - 1);
+		const btVector3 py0 = lerp(corner00, corner01, ty);
+		const btVector3 py1 = lerp(corner10, corner11, ty);
+		for (int ix = 0; ix < rx; ++ix)
 		{
-			const btScalar	tx=ix/(btScalar)(rx-1);
-			x[IDX(ix,iy)]=lerp(py0,py1,tx);
-			m[IDX(ix,iy)]=1;
+			const btScalar tx = ix / (btScalar)(rx - 1);
+			btScalar pert = perturbation * btScalar(rand()) / RAND_MAX;
+			btVector3 temp1 = py1;
+			temp1.setY(py1.getY() + pert);
+			btVector3 temp = py0;
+			pert = perturbation * btScalar(rand()) / RAND_MAX;
+			temp.setY(py0.getY() + pert);
+			x[IDX(ix, iy)] = lerp(temp, temp1, tx);
+			m[IDX(ix, iy)] = 1;
 		}
 	}
-	btSoftBody*		psb=new btSoftBody(&worldInfo,tot,x,m);
-	if(fixeds&1)	psb->setMass(IDX(0,0),0);
-	if(fixeds&2)	psb->setMass(IDX(rx-1,0),0);
-	if(fixeds&4)	psb->setMass(IDX(0,ry-1),0);
-	if(fixeds&8)	psb->setMass(IDX(rx-1,ry-1),0);
+	btSoftBody* psb = new btSoftBody(&worldInfo, tot, x, m);
+	if (fixeds & 1) psb->setMass(IDX(0, 0), 0);
+	if (fixeds & 2) psb->setMass(IDX(rx - 1, 0), 0);
+	if (fixeds & 4) psb->setMass(IDX(0, ry - 1), 0);
+	if (fixeds & 8) psb->setMass(IDX(rx - 1, ry - 1), 0);
 	delete[] x;
 	delete[] m;
-	/* Create links	and faces */ 
-	for(iy=0;iy<ry;++iy)
+	/* Create links	and faces */
+	for (iy = 0; iy < ry; ++iy)
 	{
-		for(int ix=0;ix<rx;++ix)
+		for (int ix = 0; ix < rx; ++ix)
 		{
-			const int	idx=IDX(ix,iy);
-			const bool	mdx=(ix+1)<rx;
-			const bool	mdy=(iy+1)<ry;
-			if(mdx) psb->appendLink(idx,IDX(ix+1,iy));
-			if(mdy) psb->appendLink(idx,IDX(ix,iy+1));
-			if(mdx&&mdy)
+			const int idx = IDX(ix, iy);
+			const bool mdx = (ix + 1) < rx;
+			const bool mdy = (iy + 1) < ry;
+			if (mdx) psb->appendLink(idx, IDX(ix + 1, iy));
+			if (mdy) psb->appendLink(idx, IDX(ix, iy + 1));
+			if (mdx && mdy)
 			{
-				if((ix+iy)&1)
+				if ((ix + iy) & 1)
 				{
-					psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1));
-					psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1));
-					if(gendiags)
+					psb->appendFace(IDX(ix, iy), IDX(ix + 1, iy), IDX(ix + 1, iy + 1));
+					psb->appendFace(IDX(ix, iy), IDX(ix + 1, iy + 1), IDX(ix, iy + 1));
+					if (gendiags)
 					{
-						psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1));
+						psb->appendLink(IDX(ix, iy), IDX(ix + 1, iy + 1));
 					}
 				}
 				else
 				{
-					psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy));
-					psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1));
-					if(gendiags)
+					psb->appendFace(IDX(ix, iy + 1), IDX(ix, iy), IDX(ix + 1, iy));
+					psb->appendFace(IDX(ix, iy + 1), IDX(ix + 1, iy), IDX(ix + 1, iy + 1));
+					if (gendiags)
 					{
-						psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1));
+						psb->appendLink(IDX(ix + 1, iy), IDX(ix, iy + 1));
 					}
 				}
 			}
 		}
 	}
-	/* Finished		*/ 
+	/* Finished		*/
 #undef IDX
-	return(psb);
+	return (psb);
 }
 
 //
-btSoftBody*		btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
-												 const btVector3& corner00,
-												 const btVector3& corner10,
-												 const btVector3& corner01,
-												 const btVector3& corner11,
-												 int resx,
-												 int resy,
-												 int fixeds,
-												 bool gendiags,
-												 float* tex_coords)
+btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
+											 const btVector3& corner00,
+											 const btVector3& corner10,
+											 const btVector3& corner01,
+											 const btVector3& corner11,
+											 int resx,
+											 int resy,
+											 int fixeds,
+											 bool gendiags,
+											 float* tex_coords)
 {
-
 	/*
 	*
 	*  corners:
@@ -855,92 +881,92 @@ btSoftBody*		btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
 	*
 	*/
 
-#define IDX(_x_,_y_)	((_y_)*rx+(_x_))
-	/* Create nodes		*/ 
-	if((resx<2)||(resy<2)) return(0);
-	const int	rx=resx;
-	const int	ry=resy;
-	const int	tot=rx*ry;
-	btVector3*	x=new btVector3[tot];
-	btScalar*	m=new btScalar[tot];
+#define IDX(_x_, _y_) ((_y_)*rx + (_x_))
+	/* Create nodes		*/
+	if ((resx < 2) || (resy < 2)) return (0);
+	const int rx = resx;
+	const int ry = resy;
+	const int tot = rx * ry;
+	btVector3* x = new btVector3[tot];
+	btScalar* m = new btScalar[tot];
 
 	int iy;
 
-	for(iy=0;iy<ry;++iy)
+	for (iy = 0; iy < ry; ++iy)
 	{
-		const btScalar	ty=iy/(btScalar)(ry-1);
-		const btVector3	py0=lerp(corner00,corner01,ty);
-		const btVector3	py1=lerp(corner10,corner11,ty);
-		for(int ix=0;ix<rx;++ix)
+		const btScalar ty = iy / (btScalar)(ry - 1);
+		const btVector3 py0 = lerp(corner00, corner01, ty);
+		const btVector3 py1 = lerp(corner10, corner11, ty);
+		for (int ix = 0; ix < rx; ++ix)
 		{
-			const btScalar	tx=ix/(btScalar)(rx-1);
-			x[IDX(ix,iy)]=lerp(py0,py1,tx);
-			m[IDX(ix,iy)]=1;
+			const btScalar tx = ix / (btScalar)(rx - 1);
+			x[IDX(ix, iy)] = lerp(py0, py1, tx);
+			m[IDX(ix, iy)] = 1;
 		}
 	}
-	btSoftBody*	psb=new btSoftBody(&worldInfo,tot,x,m);
-	if(fixeds&1)		psb->setMass(IDX(0,0),0);
-	if(fixeds&2)		psb->setMass(IDX(rx-1,0),0);
-	if(fixeds&4)		psb->setMass(IDX(0,ry-1),0);
-	if(fixeds&8)		psb->setMass(IDX(rx-1,ry-1),0);
-	if(fixeds&16)		psb->setMass(IDX((rx-1)/2,0),0);
-	if(fixeds&32)		psb->setMass(IDX(0,(ry-1)/2),0);
-	if(fixeds&64)		psb->setMass(IDX(rx-1,(ry-1)/2),0);
-	if(fixeds&128)		psb->setMass(IDX((rx-1)/2,ry-1),0);
-	if(fixeds&256)		psb->setMass(IDX((rx-1)/2,(ry-1)/2),0);
+	btSoftBody* psb = new btSoftBody(&worldInfo, tot, x, m);
+	if (fixeds & 1) psb->setMass(IDX(0, 0), 0);
+	if (fixeds & 2) psb->setMass(IDX(rx - 1, 0), 0);
+	if (fixeds & 4) psb->setMass(IDX(0, ry - 1), 0);
+	if (fixeds & 8) psb->setMass(IDX(rx - 1, ry - 1), 0);
+	if (fixeds & 16) psb->setMass(IDX((rx - 1) / 2, 0), 0);
+	if (fixeds & 32) psb->setMass(IDX(0, (ry - 1) / 2), 0);
+	if (fixeds & 64) psb->setMass(IDX(rx - 1, (ry - 1) / 2), 0);
+	if (fixeds & 128) psb->setMass(IDX((rx - 1) / 2, ry - 1), 0);
+	if (fixeds & 256) psb->setMass(IDX((rx - 1) / 2, (ry - 1) / 2), 0);
 	delete[] x;
 	delete[] m;
 
-
 	int z = 0;
-	/* Create links	and faces	*/ 
-	for(iy=0;iy<ry;++iy)
+	/* Create links	and faces	*/
+	for (iy = 0; iy < ry; ++iy)
 	{
-		for(int ix=0;ix<rx;++ix)
+		for (int ix = 0; ix < rx; ++ix)
 		{
-			const bool	mdx=(ix+1)<rx;
-			const bool	mdy=(iy+1)<ry;
+			const bool mdx = (ix + 1) < rx;
+			const bool mdy = (iy + 1) < ry;
 
-			int node00=IDX(ix,iy);
-			int node01=IDX(ix+1,iy);
-			int node10=IDX(ix,iy+1);
-			int node11=IDX(ix+1,iy+1);
+			int node00 = IDX(ix, iy);
+			int node01 = IDX(ix + 1, iy);
+			int node10 = IDX(ix, iy + 1);
+			int node11 = IDX(ix + 1, iy + 1);
 
-			if(mdx) psb->appendLink(node00,node01);
-			if(mdy) psb->appendLink(node00,node10);
-			if(mdx&&mdy)
+			if (mdx) psb->appendLink(node00, node01);
+			if (mdy) psb->appendLink(node00, node10);
+			if (mdx && mdy)
 			{
-				psb->appendFace(node00,node10,node11);
-				if (tex_coords) {
-					tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0);
-					tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1);
-					tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0);
-					tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2);
-					tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3);
-					tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2);
+				psb->appendFace(node00, node10, node11);
+				if (tex_coords)
+				{
+					tex_coords[z + 0] = CalculateUV(resx, resy, ix, iy, 0);
+					tex_coords[z + 1] = CalculateUV(resx, resy, ix, iy, 1);
+					tex_coords[z + 2] = CalculateUV(resx, resy, ix, iy, 0);
+					tex_coords[z + 3] = CalculateUV(resx, resy, ix, iy, 2);
+					tex_coords[z + 4] = CalculateUV(resx, resy, ix, iy, 3);
+					tex_coords[z + 5] = CalculateUV(resx, resy, ix, iy, 2);
 				}
-				psb->appendFace(node11,node01,node00);
-				if (tex_coords) {
-					tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3);
-					tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2);
-					tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3);
-					tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1);
-					tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0);
-					tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1);
+				psb->appendFace(node11, node01, node00);
+				if (tex_coords)
+				{
+					tex_coords[z + 6] = CalculateUV(resx, resy, ix, iy, 3);
+					tex_coords[z + 7] = CalculateUV(resx, resy, ix, iy, 2);
+					tex_coords[z + 8] = CalculateUV(resx, resy, ix, iy, 3);
+					tex_coords[z + 9] = CalculateUV(resx, resy, ix, iy, 1);
+					tex_coords[z + 10] = CalculateUV(resx, resy, ix, iy, 0);
+					tex_coords[z + 11] = CalculateUV(resx, resy, ix, iy, 1);
 				}
-				if (gendiags) psb->appendLink(node00,node11);
+				if (gendiags) psb->appendLink(node00, node11);
 				z += 12;
 			}
 		}
 	}
-	/* Finished	*/ 
+	/* Finished	*/
 #undef IDX
-	return(psb);
+	return (psb);
 }
 
-float   btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id)
+float btSoftBodyHelpers::CalculateUV(int resx, int resy, int ix, int iy, int id)
 {
-
 	/*
 	*
 	*
@@ -966,90 +992,93 @@ float   btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id)
 	*
 	*/
 
-	float tc=0.0f;
-	if (id == 0) {
-		tc = (1.0f/((resx-1))*ix);
+	float tc = 0.0f;
+	if (id == 0)
+	{
+		tc = (1.0f / ((resx - 1)) * ix);
 	}
-	else if (id==1) {
-		tc = (1.0f/((resy-1))*(resy-1-iy));
+	else if (id == 1)
+	{
+		tc = (1.0f / ((resy - 1)) * (resy - 1 - iy));
 	}
-	else if (id==2) {
-		tc = (1.0f/((resy-1))*(resy-1-iy-1));
+	else if (id == 2)
+	{
+		tc = (1.0f / ((resy - 1)) * (resy - 1 - iy - 1));
 	}
-	else if (id==3) {
-		tc = (1.0f/((resx-1))*(ix+1));
+	else if (id == 3)
+	{
+		tc = (1.0f / ((resx - 1)) * (ix + 1));
 	}
 	return tc;
 }
 //
-btSoftBody*		btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,
-												   const btVector3& radius,
-												   int res)
+btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, const btVector3& center,
+											   const btVector3& radius,
+											   int res)
 {
-	struct	Hammersley
+	struct Hammersley
 	{
-		static void	Generate(btVector3* x,int n)
+		static void Generate(btVector3* x, int n)
 		{
-			for(int i=0;i<n;i++)
+			for (int i = 0; i < n; i++)
 			{
-				btScalar	p=0.5,t=0;
-				for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p;
-				btScalar	w=2*t-1;
-				btScalar	a=(SIMD_PI+2*i*SIMD_PI)/n;
-				btScalar	s=btSqrt(1-w*w);
-				*x++=btVector3(s*btCos(a),s*btSin(a),w);
+				btScalar p = 0.5, t = 0;
+				for (int j = i; j; p *= 0.5, j >>= 1)
+					if (j & 1) t += p;
+				btScalar w = 2 * t - 1;
+				btScalar a = (SIMD_PI + 2 * i * SIMD_PI) / n;
+				btScalar s = btSqrt(1 - w * w);
+				*x++ = btVector3(s * btCos(a), s * btSin(a), w);
 			}
 		}
 	};
-	btAlignedObjectArray<btVector3>	vtx;
-	vtx.resize(3+res);
-	Hammersley::Generate(&vtx[0],vtx.size());
-	for(int i=0;i<vtx.size();++i)
+	btAlignedObjectArray<btVector3> vtx;
+	vtx.resize(3 + res);
+	Hammersley::Generate(&vtx[0], vtx.size());
+	for (int i = 0; i < vtx.size(); ++i)
 	{
-		vtx[i]=vtx[i]*radius+center;
+		vtx[i] = vtx[i] * radius + center;
 	}
-	return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size()));
+	return (CreateFromConvexHull(worldInfo, &vtx[0], vtx.size()));
 }
 
-
-
 //
-btSoftBody*		btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar*	vertices,
-													 const int* triangles,
-													 int ntriangles, bool randomizeConstraints)
+btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo, const btScalar* vertices,
+												 const int* triangles,
+												 int ntriangles, bool randomizeConstraints)
 {
-	int		maxidx=0;
-	int i,j,ni;
+	int maxidx = 0;
+	int i, j, ni;
 
-	for(i=0,ni=ntriangles*3;i<ni;++i)
+	for (i = 0, ni = ntriangles * 3; i < ni; ++i)
 	{
-		maxidx=btMax(triangles[i],maxidx);
+		maxidx = btMax(triangles[i], maxidx);
 	}
 	++maxidx;
-	btAlignedObjectArray<bool>		chks;
-	btAlignedObjectArray<btVector3>	vtx;
-	chks.resize(maxidx*maxidx,false);
+	btAlignedObjectArray<bool> chks;
+	btAlignedObjectArray<btVector3> vtx;
+	chks.resize(maxidx * maxidx, false);
 	vtx.resize(maxidx);
-	for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3)
+	for (i = 0, j = 0, ni = maxidx * 3; i < ni; ++j, i += 3)
 	{
-		vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]);
+		vtx[j] = btVector3(vertices[i], vertices[i + 1], vertices[i + 2]);
 	}
-	btSoftBody*		psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0);
-	for( i=0,ni=ntriangles*3;i<ni;i+=3)
+	btSoftBody* psb = new btSoftBody(&worldInfo, vtx.size(), &vtx[0], 0);
+	for (i = 0, ni = ntriangles * 3; i < ni; i += 3)
 	{
-		const int idx[]={triangles[i],triangles[i+1],triangles[i+2]};
-#define IDX(_x_,_y_) ((_y_)*maxidx+(_x_))
-		for(int j=2,k=0;k<3;j=k++)
+		const int idx[] = {triangles[i], triangles[i + 1], triangles[i + 2]};
+#define IDX(_x_, _y_) ((_y_)*maxidx + (_x_))
+		for (int j = 2, k = 0; k < 3; j = k++)
 		{
-			if(!chks[IDX(idx[j],idx[k])])
+			if (!chks[IDX(idx[j], idx[k])])
 			{
-				chks[IDX(idx[j],idx[k])]=true;
-				chks[IDX(idx[k],idx[j])]=true;
-				psb->appendLink(idx[j],idx[k]);
+				chks[IDX(idx[j], idx[k])] = true;
+				chks[IDX(idx[k], idx[j])] = true;
+				psb->appendLink(idx[j], idx[k]);
 			}
 		}
 #undef IDX
-		psb->appendFace(idx[0],idx[1],idx[2]);
+		psb->appendFace(idx[0], idx[1], idx[2]);
 	}
 
 	if (randomizeConstraints)
@@ -1057,44 +1086,41 @@ btSoftBody*		btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo
 		psb->randomizeConstraints();
 	}
 
-	return(psb);
+	return (psb);
 }
 
 //
-btSoftBody*		btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo,	const btVector3* vertices,
-														int nvertices, bool randomizeConstraints)
+btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
+													int nvertices, bool randomizeConstraints)
 {
-	HullDesc		hdsc(QF_TRIANGLES,nvertices,vertices);
-	HullResult		hres;
-	HullLibrary		hlib;/*??*/ 
-	hdsc.mMaxVertices=nvertices;
-	hlib.CreateConvexHull(hdsc,hres);
-	btSoftBody*		psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
-		&hres.m_OutputVertices[0],0);
-	for(int i=0;i<(int)hres.mNumFaces;++i)
+	HullDesc hdsc(QF_TRIANGLES, nvertices, vertices);
+	HullResult hres;
+	HullLibrary hlib; /*??*/
+	hdsc.mMaxVertices = nvertices;
+	hlib.CreateConvexHull(hdsc, hres);
+	btSoftBody* psb = new btSoftBody(&worldInfo, (int)hres.mNumOutputVertices,
+									 &hres.m_OutputVertices[0], 0);
+	for (int i = 0; i < (int)hres.mNumFaces; ++i)
 	{
-		const int idx[]={	static_cast<int>(hres.m_Indices[i*3+0]),
-							static_cast<int>(hres.m_Indices[i*3+1]),
-							static_cast<int>(hres.m_Indices[i*3+2])};
-		if(idx[0]<idx[1]) psb->appendLink(	idx[0],idx[1]);
-		if(idx[1]<idx[2]) psb->appendLink(	idx[1],idx[2]);
-		if(idx[2]<idx[0]) psb->appendLink(	idx[2],idx[0]);
-		psb->appendFace(idx[0],idx[1],idx[2]);
+		const int idx[] = {static_cast<int>(hres.m_Indices[i * 3 + 0]),
+						   static_cast<int>(hres.m_Indices[i * 3 + 1]),
+						   static_cast<int>(hres.m_Indices[i * 3 + 2])};
+		if (idx[0] < idx[1]) psb->appendLink(idx[0], idx[1]);
+		if (idx[1] < idx[2]) psb->appendLink(idx[1], idx[2]);
+		if (idx[2] < idx[0]) psb->appendLink(idx[2], idx[0]);
+		psb->appendFace(idx[0], idx[1], idx[2]);
 	}
 	hlib.ReleaseResult(hres);
 	if (randomizeConstraints)
 	{
 		psb->randomizeConstraints();
 	}
-	return(psb);
+	return (psb);
 }
 
-
-
-
 static int nextLine(const char* buffer)
 {
-	int numBytesRead=0;
+	int numBytesRead = 0;
 
 	while (*buffer != '\n')
 	{
@@ -1102,8 +1128,7 @@ static int nextLine(const char* buffer)
 		numBytesRead++;
 	}
 
-	
-	if (buffer[0]==0x0a)
+	if (buffer[0] == 0x0a)
 	{
 		buffer++;
 		numBytesRead++;
@@ -1111,8 +1136,8 @@ static int nextLine(const char* buffer)
 	return numBytesRead;
 }
 
-/* Create from TetGen .ele, .face, .node data							*/ 
-btSoftBody*	btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo,
+/* Create from TetGen .ele, .face, .node data							*/
+btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo,
 													const char* ele,
 													const char* face,
 													const char* node,
@@ -1120,39 +1145,38 @@ btSoftBody*	btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldIn
 													bool btetralinks,
 													bool bfacesfromtetras)
 {
-btAlignedObjectArray<btVector3>	pos;
-int								nnode=0;
-int								ndims=0;
-int								nattrb=0;
-int								hasbounds=0;
-int result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds);
-result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds);
-(void)result;
-node += nextLine(node);
-
-pos.resize(nnode);
-for(int i=0;i<pos.size();++i)
+	btAlignedObjectArray<btVector3> pos;
+	int nnode = 0;
+	int ndims = 0;
+	int nattrb = 0;
+	int hasbounds = 0;
+	int result = sscanf(node, "%d %d %d %d", &nnode, &ndims, &nattrb, &hasbounds);
+	result = sscanf(node, "%d %d %d %d", &nnode, &ndims, &nattrb, &hasbounds);
+	node += nextLine(node);
+
+	pos.resize(nnode);
+	for (int i = 0; i < pos.size(); ++i)
 	{
-	int			index=0;
-	//int			bound=0;
-	float	x,y,z;
-	sscanf(node,"%d %f %f %f",&index,&x,&y,&z);
+		int index = 0;
+		//int			bound=0;
+		float x, y, z;
+		sscanf(node, "%d %f %f %f", &index, &x, &y, &z);
 
-//	sn>>index;
-//	sn>>x;sn>>y;sn>>z;
-	node += nextLine(node);
+		//	sn>>index;
+		//	sn>>x;sn>>y;sn>>z;
+		node += nextLine(node);
 
-	//for(int j=0;j<nattrb;++j) 
-	//	sn>>a;
+		//for(int j=0;j<nattrb;++j)
+		//	sn>>a;
 
-	//if(hasbounds) 
-	//	sn>>bound;
+		//if(hasbounds)
+		//	sn>>bound;
 
-	pos[index].setX(btScalar(x));
-	pos[index].setY(btScalar(y));
-	pos[index].setZ(btScalar(z));
+		pos[index].setX(btScalar(x));
+		pos[index].setY(btScalar(y));
+		pos[index].setZ(btScalar(z));
 	}
-btSoftBody*						psb=new btSoftBody(&worldInfo,nnode,&pos[0],0);
+	btSoftBody* psb = new btSoftBody(&worldInfo, nnode, &pos[0], 0);
 #if 0
 if(face&&face[0])
 	{
@@ -1177,42 +1201,461 @@ if(face&&face[0])
 	}
 #endif
 
-if(ele&&ele[0])
+	if (ele && ele[0])
 	{
-	int								ntetra=0;
-	int								ncorner=0;
-	int								neattrb=0;
-	sscanf(ele,"%d %d %d",&ntetra,&ncorner,&neattrb);
-	ele += nextLine(ele);
-	
-	//se>>ntetra;se>>ncorner;se>>neattrb;
-	for(int i=0;i<ntetra;++i)
+		int ntetra = 0;
+		int ncorner = 0;
+		int neattrb = 0;
+		sscanf(ele, "%d %d %d", &ntetra, &ncorner, &neattrb);
+		ele += nextLine(ele);
+
+		//se>>ntetra;se>>ncorner;se>>neattrb;
+		for (int i = 0; i < ntetra; ++i)
 		{
-		int			index=0;
-		int			ni[4];
-
-		//se>>index;
-		//se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3];
-		sscanf(ele,"%d %d %d %d %d",&index,&ni[0],&ni[1],&ni[2],&ni[3]);
-		ele+=nextLine(ele);
-		//for(int j=0;j<neattrb;++j) 
-		//	se>>a;
-		psb->appendTetra(ni[0],ni[1],ni[2],ni[3]);
-		if(btetralinks)
+			int index = 0;
+			int ni[4];
+
+			//se>>index;
+			//se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3];
+			sscanf(ele, "%d %d %d %d %d", &index, &ni[0], &ni[1], &ni[2], &ni[3]);
+			ele += nextLine(ele);
+			//for(int j=0;j<neattrb;++j)
+			//	se>>a;
+			psb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
+			if (btetralinks)
 			{
-			psb->appendLink(ni[0],ni[1],0,true);
-			psb->appendLink(ni[1],ni[2],0,true);
-			psb->appendLink(ni[2],ni[0],0,true);
-			psb->appendLink(ni[0],ni[3],0,true);
-			psb->appendLink(ni[1],ni[3],0,true);
-			psb->appendLink(ni[2],ni[3],0,true);
+				psb->appendLink(ni[0], ni[1], 0, true);
+				psb->appendLink(ni[1], ni[2], 0, true);
+				psb->appendLink(ni[2], ni[0], 0, true);
+				psb->appendLink(ni[0], ni[3], 0, true);
+				psb->appendLink(ni[1], ni[3], 0, true);
+				psb->appendLink(ni[2], ni[3], 0, true);
+			}
+		}
+	}
+	psb->initializeDmInverse();
+	psb->m_tetraScratches.resize(psb->m_tetras.size());
+	psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
+	printf("Nodes:  %u\r\n", psb->m_nodes.size());
+	printf("Links:  %u\r\n", psb->m_links.size());
+	printf("Faces:  %u\r\n", psb->m_faces.size());
+	printf("Tetras: %u\r\n", psb->m_tetras.size());
+	return (psb);
+}
+
+btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file)
+{
+	std::ifstream fs;
+	fs.open(vtk_file);
+	btAssert(fs);
+
+	typedef btAlignedObjectArray<int> Index;
+	std::string line;
+	btAlignedObjectArray<btVector3> X;
+	btVector3 position;
+	btAlignedObjectArray<Index> indices;
+	bool reading_points = false;
+	bool reading_tets = false;
+	size_t n_points = 0;
+	size_t n_tets = 0;
+	size_t x_count = 0;
+	size_t indices_count = 0;
+	while (std::getline(fs, line))
+	{
+		std::stringstream ss(line);
+		if (line.size() == (size_t)(0))
+		{
+		}
+		else if (line.substr(0, 6) == "POINTS")
+		{
+			reading_points = true;
+			reading_tets = false;
+			ss.ignore(128, ' ');  // ignore "POINTS"
+			ss >> n_points;
+			X.resize(n_points);
+		}
+		else if (line.substr(0, 5) == "CELLS")
+		{
+			reading_points = false;
+			reading_tets = true;
+			ss.ignore(128, ' ');  // ignore "CELLS"
+			ss >> n_tets;
+			indices.resize(n_tets);
+		}
+		else if (line.substr(0, 10) == "CELL_TYPES")
+		{
+			reading_points = false;
+			reading_tets = false;
+		}
+		else if (reading_points)
+		{
+			btScalar p;
+			ss >> p;
+			position.setX(p);
+			ss >> p;
+			position.setY(p);
+			ss >> p;
+			position.setZ(p);
+			X[x_count++] = position;
+		}
+		else if (reading_tets)
+		{
+			int d;
+			ss >> d;
+			if (d != 4)
+			{
+				printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n");
+				fs.close();
+				return 0;
 			}
+			ss.ignore(128, ' ');  // ignore "4"
+			Index tet;
+			tet.resize(4);
+			for (size_t i = 0; i < 4; i++)
+			{
+				ss >> tet[i];
+				printf("%d ", tet[i]);
+			}
+			printf("\n");
+			indices[indices_count++] = tet;
+		}
+	}
+	btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0);
+
+	for (int i = 0; i < n_tets; ++i)
+	{
+		const Index& ni = indices[i];
+		psb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
+		{
+			psb->appendLink(ni[0], ni[1], 0, true);
+			psb->appendLink(ni[1], ni[2], 0, true);
+			psb->appendLink(ni[2], ni[0], 0, true);
+			psb->appendLink(ni[0], ni[3], 0, true);
+			psb->appendLink(ni[1], ni[3], 0, true);
+			psb->appendLink(ni[2], ni[3], 0, true);
 		}
 	}
-printf("Nodes:  %d\r\n",psb->m_nodes.size());
-printf("Links:  %d\r\n",psb->m_links.size());
-printf("Faces:  %d\r\n",psb->m_faces.size());
-printf("Tetras: %d\r\n",psb->m_tetras.size());
-return(psb);
+
+	generateBoundaryFaces(psb);
+	psb->initializeDmInverse();
+	psb->m_tetraScratches.resize(psb->m_tetras.size());
+	psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
+	printf("Nodes:  %u\r\n", psb->m_nodes.size());
+	printf("Links:  %u\r\n", psb->m_links.size());
+	printf("Faces:  %u\r\n", psb->m_faces.size());
+	printf("Tetras: %u\r\n", psb->m_tetras.size());
+
+	fs.close();
+	return psb;
 }
 
+void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb)
+{
+	int counter = 0;
+	for (int i = 0; i < psb->m_nodes.size(); ++i)
+	{
+		psb->m_nodes[i].index = counter++;
+	}
+	typedef btAlignedObjectArray<int> Index;
+	btAlignedObjectArray<Index> indices;
+	indices.resize(psb->m_tetras.size());
+	for (int i = 0; i < indices.size(); ++i)
+	{
+		Index index;
+		index.push_back(psb->m_tetras[i].m_n[0]->index);
+		index.push_back(psb->m_tetras[i].m_n[1]->index);
+		index.push_back(psb->m_tetras[i].m_n[2]->index);
+		index.push_back(psb->m_tetras[i].m_n[3]->index);
+		indices[i] = index;
+	}
+
+	std::map<std::vector<int>, std::vector<int> > dict;
+	for (int i = 0; i < indices.size(); ++i)
+	{
+		for (int j = 0; j < 4; ++j)
+		{
+			std::vector<int> f;
+			if (j == 0)
+			{
+				f.push_back(indices[i][1]);
+				f.push_back(indices[i][0]);
+				f.push_back(indices[i][2]);
+			}
+			if (j == 1)
+			{
+				f.push_back(indices[i][3]);
+				f.push_back(indices[i][0]);
+				f.push_back(indices[i][1]);
+			}
+			if (j == 2)
+			{
+				f.push_back(indices[i][3]);
+				f.push_back(indices[i][1]);
+				f.push_back(indices[i][2]);
+			}
+			if (j == 3)
+			{
+				f.push_back(indices[i][2]);
+				f.push_back(indices[i][0]);
+				f.push_back(indices[i][3]);
+			}
+			std::vector<int> f_sorted = f;
+			std::sort(f_sorted.begin(), f_sorted.end());
+			if (dict.find(f_sorted) != dict.end())
+			{
+				dict.erase(f_sorted);
+			}
+			else
+			{
+				dict.insert(std::make_pair(f_sorted, f));
+			}
+		}
+	}
+
+	for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it)
+	{
+		std::vector<int> f = it->second;
+		psb->appendFace(f[0], f[1], f[2]);
+	}
+}
+
+//Write the surface mesh to an obj file.
+void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
+{
+	std::ofstream fs;
+	fs.open(filename);
+	btAssert(fs);
+
+	if (psb->m_tetras.size() > 0)
+	{
+		// For tetrahedron mesh, we need to re-index the surface mesh for it to be in obj file/
+		std::map<int, int> dict;
+		for (int i = 0; i < psb->m_faces.size(); i++)
+		{
+			for (int d = 0; d < 3; d++)
+			{
+				int index = psb->m_faces[i].m_n[d]->index;
+				if (dict.find(index) == dict.end())
+				{
+					int dict_size = dict.size();
+					dict[index] = dict_size;
+					fs << "v";
+					for (int k = 0; k < 3; k++)
+					{
+						fs << " " << psb->m_nodes[index].m_x[k];
+					}
+					fs << "\n";
+				}
+			}
+		}
+		// Write surface mesh.
+		for (int i = 0; i < psb->m_faces.size(); ++i)
+		{
+			fs << "f";
+			for (int n = 0; n < 3; n++)
+			{
+				fs << " " << dict[psb->m_faces[i].m_n[n]->index] + 1;
+			}
+			fs << "\n";
+		}
+	}
+	else
+	{
+		// For trimesh, directly write out all the nodes and faces.xs
+		for (int i = 0; i < psb->m_nodes.size(); ++i)
+		{
+			fs << "v";
+			for (int d = 0; d < 3; d++)
+			{
+				fs << " " << psb->m_nodes[i].m_x[d];
+			}
+			fs << "\n";
+		}
+
+		for (int i = 0; i < psb->m_faces.size(); ++i)
+		{
+			fs << "f";
+			for (int n = 0; n < 3; n++)
+			{
+				fs << " " << psb->m_faces[i].m_n[n]->index + 1;
+			}
+			fs << "\n";
+		}
+	}
+	fs.close();
+}
+
+void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb)
+{
+	std::ifstream fs_read;
+	fs_read.open(filename);
+	std::string line;
+	btVector3 pos;
+	btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces;
+	while (std::getline(fs_read, line))
+	{
+		std::stringstream ss(line);
+		if (line[0] == 'v')
+		{
+		}
+		else if (line[0] == 'f')
+		{
+			ss.ignore();
+			int id0, id1, id2;
+			ss >> id0;
+			ss >> id1;
+			ss >> id2;
+			btAlignedObjectArray<int> new_face;
+			new_face.push_back(id1);
+			new_face.push_back(id0);
+			new_face.push_back(id2);
+			additional_faces.push_back(new_face);
+		}
+	}
+	fs_read.close();
+
+	std::ofstream fs_write;
+	fs_write.open(filename, std::ios_base::app);
+	for (int i = 0; i < additional_faces.size(); ++i)
+	{
+		fs_write << "f";
+		for (int n = 0; n < 3; n++)
+		{
+			fs_write << " " << additional_faces[i][n];
+		}
+		fs_write << "\n";
+	}
+	fs_write.close();
+}
+
+// Given a simplex with vertices a,b,c,d, find the barycentric weights of p in this simplex
+void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary)
+{
+	btVector3 vap = p - a;
+	btVector3 vbp = p - b;
+
+	btVector3 vab = b - a;
+	btVector3 vac = c - a;
+	btVector3 vad = d - a;
+
+	btVector3 vbc = c - b;
+	btVector3 vbd = d - b;
+	btScalar va6 = (vbp.cross(vbd)).dot(vbc);
+	btScalar vb6 = (vap.cross(vac)).dot(vad);
+	btScalar vc6 = (vap.cross(vad)).dot(vab);
+	btScalar vd6 = (vap.cross(vab)).dot(vac);
+	btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad));
+	bary = btVector4(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6);
+}
+
+// Given a simplex with vertices a,b,c, find the barycentric weights of p in this simplex. bary[3] = 0.
+void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary)
+{
+	btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
+	btScalar d00 = btDot(v0, v0);
+	btScalar d01 = btDot(v0, v1);
+	btScalar d11 = btDot(v1, v1);
+	btScalar d20 = btDot(v2, v0);
+	btScalar d21 = btDot(v2, v1);
+	btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01);
+	bary[1] = (d11 * d20 - d01 * d21) * invDenom;
+	bary[2] = (d00 * d21 - d01 * d20) * invDenom;
+	bary[0] = 1.0 - bary[1] - bary[2];
+	bary[3] = 0;
+}
+
+// Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights
+// If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight
+void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
+{
+	psb->m_z.resize(0);
+	psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
+	psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
+	for (int i = 0; i < psb->m_renderNodes.size(); ++i)
+	{
+		const btVector3& p = psb->m_renderNodes[i].m_x;
+		btVector4 bary;
+		btVector4 optimal_bary;
+		btScalar min_bary_weight = -1e3;
+		btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
+		for (int j = 0; j < psb->m_tetras.size(); ++j)
+		{
+			const btSoftBody::Tetra& t = psb->m_tetras[j];
+			getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary);
+			btScalar new_min_bary_weight = bary[0];
+			for (int k = 1; k < 4; ++k)
+			{
+				new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
+			}
+			if (new_min_bary_weight > min_bary_weight)
+			{
+				btAlignedObjectArray<const btSoftBody::Node*> parents;
+				parents.push_back(t.m_n[0]);
+				parents.push_back(t.m_n[1]);
+				parents.push_back(t.m_n[2]);
+				parents.push_back(t.m_n[3]);
+				optimal_parents = parents;
+				optimal_bary = bary;
+				min_bary_weight = new_min_bary_weight;
+				// stop searching if p is inside the tetrahedron at hand
+				if (bary[0] >= 0. && bary[1] >= 0. && bary[2] >= 0. && bary[3] >= 0.)
+				{
+					break;
+				}
+			}
+		}
+		psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
+		psb->m_renderNodesParents[i] = optimal_parents;
+	}
+}
+
+// Iterate through all render nodes to find the simulation triangle that's closest to the node in the barycentric sense.
+void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb)
+{
+	psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
+	psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
+	psb->m_z.resize(psb->m_renderNodes.size());
+	for (int i = 0; i < psb->m_renderNodes.size(); ++i)
+	{
+		const btVector3& p = psb->m_renderNodes[i].m_x;
+		btVector4 bary;
+		btVector4 optimal_bary;
+		btScalar min_bary_weight = -SIMD_INFINITY;
+		btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
+		btScalar dist = 0, optimal_dist = 0;
+		for (int j = 0; j < psb->m_faces.size(); ++j)
+		{
+			const btSoftBody::Face& f = psb->m_faces[j];
+			btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
+			btVector3 unit_n = n.normalized();
+			dist = (p - f.m_n[0]->m_x).dot(unit_n);
+			btVector3 proj_p = p - dist * unit_n;
+			getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary);
+			btScalar new_min_bary_weight = bary[0];
+			for (int k = 1; k < 3; ++k)
+			{
+				new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
+			}
+
+			// p is out of the current best triangle, we found a traingle that's better
+			bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight < 0.);
+			// p is inside of the current best triangle, we found a triangle that's better
+			bool better_than_best_inside = (new_min_bary_weight >= 0 && min_bary_weight >= 0 && btFabs(dist) < btFabs(optimal_dist));
+
+			if (better_than_closest_outisde || better_than_best_inside)
+			{
+				btAlignedObjectArray<const btSoftBody::Node*> parents;
+				parents.push_back(f.m_n[0]);
+				parents.push_back(f.m_n[1]);
+				parents.push_back(f.m_n[2]);
+				optimal_parents = parents;
+				optimal_bary = bary;
+				optimal_dist = dist;
+				min_bary_weight = new_min_bary_weight;
+			}
+		}
+		psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
+		psb->m_renderNodesParents[i] = optimal_parents;
+		psb->m_z[i] = optimal_dist;
+	}
+}
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h
index 7271530109a..237d29761d7 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h
+++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h
@@ -17,132 +17,151 @@ subject to the following restrictions:
 #define BT_SOFT_BODY_HELPERS_H
 
 #include "btSoftBody.h"
-
+#include <fstream>
+#include <string>
 //
 // Helpers
 //
 
-/* fDrawFlags															*/ 
-struct	fDrawFlags { enum _ {
-	Nodes		=	0x0001,
-	Links		=	0x0002,
-	Faces		=	0x0004,
-	Tetras		=	0x0008,
-	Normals		=	0x0010,
-	Contacts	=	0x0020,
-	Anchors		=	0x0040,
-	Notes		=	0x0080,
-	Clusters	=	0x0100,
-	NodeTree	=	0x0200,
-	FaceTree	=	0x0400,
-	ClusterTree	=	0x0800,
-	Joints		=	0x1000,
-	/* presets	*/ 
-	Std			=	Links+Faces+Tetras+Anchors+Notes+Joints,
-	StdTetra	=	Std-Faces+Tetras
-};};
+/* fDrawFlags															*/
+struct fDrawFlags
+{
+	enum _
+	{
+		Nodes = 0x0001,
+		Links = 0x0002,
+		Faces = 0x0004,
+		Tetras = 0x0008,
+		Normals = 0x0010,
+		Contacts = 0x0020,
+		Anchors = 0x0040,
+		Notes = 0x0080,
+		Clusters = 0x0100,
+		NodeTree = 0x0200,
+		FaceTree = 0x0400,
+		ClusterTree = 0x0800,
+		Joints = 0x1000,
+		/* presets	*/
+		Std = Links + Faces + Tetras + Anchors + Notes + Joints,
+		StdTetra = Std - Faces + Tetras
+	};
+};
 
-struct	btSoftBodyHelpers
+struct btSoftBodyHelpers
 {
-	/* Draw body															*/ 
-	static void				Draw(		btSoftBody* psb,
-		btIDebugDraw* idraw,
-		int drawflags=fDrawFlags::Std);
-	/* Draw body infos														*/ 
-	static	void			DrawInfos(	btSoftBody* psb,
-		btIDebugDraw* idraw,
-		bool masses,
-		bool areas,
-		bool stress);
-	/* Draw node tree														*/ 
-	static void				DrawNodeTree(	btSoftBody* psb,
-		btIDebugDraw* idraw,
-		int mindepth=0,
-		int maxdepth=-1);
-	/* Draw face tree														*/ 
-	static void				DrawFaceTree(	btSoftBody* psb,
-		btIDebugDraw* idraw,
-		int mindepth=0,
-		int maxdepth=-1);
-	/* Draw cluster tree													*/ 
-	static void				DrawClusterTree(btSoftBody* psb,
-		btIDebugDraw* idraw,
-		int mindepth=0,
-		int maxdepth=-1);
-	/* Draw rigid frame														*/ 
-	static	void			DrawFrame(		btSoftBody* psb,
-		btIDebugDraw* idraw);
-	/* Create a rope														*/ 
-	static	btSoftBody*		CreateRope( btSoftBodyWorldInfo& worldInfo,
-		const btVector3& from,
-		const btVector3& to,
-		int res,
-		int fixeds);
-	/* Create a patch														*/ 
-	static	btSoftBody*		CreatePatch(btSoftBodyWorldInfo& worldInfo,
-		const btVector3& corner00,
-		const btVector3& corner10,
-		const btVector3& corner01,
-		const btVector3& corner11,
-		int resx,
-		int resy,
-		int fixeds,
-		bool gendiags);
-	/* Create a patch with UV Texture Coordinates	*/ 
-	static	btSoftBody*		CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
-		const btVector3& corner00,
-		const btVector3& corner10,
-		const btVector3& corner01,
-		const btVector3& corner11,
-		int resx,
-		int resy,
-		int fixeds,
-		bool gendiags,
-		float* tex_coords=0);
-	static	float	CalculateUV(int resx,int resy,int ix,int iy,int id);
-	/* Create an ellipsoid													*/ 
-	static	btSoftBody*		CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,
-		const btVector3& center,
-		const btVector3& radius,
-		int res);	
-	/* Create from trimesh													*/ 
-	static	btSoftBody*		CreateFromTriMesh(	btSoftBodyWorldInfo& worldInfo,
-		const btScalar*	vertices,
-		const int* triangles,
-		int ntriangles,
-		bool randomizeConstraints = true);
-	/* Create from convex-hull												*/ 
-	static	btSoftBody*		CreateFromConvexHull(	btSoftBodyWorldInfo& worldInfo,
-		const btVector3* vertices,
-		int nvertices,
-		bool randomizeConstraints = true);
+	/* Draw body															*/
+	static void Draw(btSoftBody* psb,
+					 btIDebugDraw* idraw,
+					 int drawflags = fDrawFlags::Std);
+	/* Draw body infos														*/
+	static void DrawInfos(btSoftBody* psb,
+						  btIDebugDraw* idraw,
+						  bool masses,
+						  bool areas,
+						  bool stress);
+	/* Draw node tree														*/
+	static void DrawNodeTree(btSoftBody* psb,
+							 btIDebugDraw* idraw,
+							 int mindepth = 0,
+							 int maxdepth = -1);
+	/* Draw face tree														*/
+	static void DrawFaceTree(btSoftBody* psb,
+							 btIDebugDraw* idraw,
+							 int mindepth = 0,
+							 int maxdepth = -1);
+	/* Draw cluster tree													*/
+	static void DrawClusterTree(btSoftBody* psb,
+								btIDebugDraw* idraw,
+								int mindepth = 0,
+								int maxdepth = -1);
+	/* Draw rigid frame														*/
+	static void DrawFrame(btSoftBody* psb,
+						  btIDebugDraw* idraw);
+	/* Create a rope														*/
+	static btSoftBody* CreateRope(btSoftBodyWorldInfo& worldInfo,
+								  const btVector3& from,
+								  const btVector3& to,
+								  int res,
+								  int fixeds);
+	/* Create a patch														*/
+	static btSoftBody* CreatePatch(btSoftBodyWorldInfo& worldInfo,
+								   const btVector3& corner00,
+								   const btVector3& corner10,
+								   const btVector3& corner01,
+								   const btVector3& corner11,
+								   int resx,
+								   int resy,
+								   int fixeds,
+								   bool gendiags,
+								   btScalar perturbation = 0.);
+	/* Create a patch with UV Texture Coordinates	*/
+	static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
+									 const btVector3& corner00,
+									 const btVector3& corner10,
+									 const btVector3& corner01,
+									 const btVector3& corner11,
+									 int resx,
+									 int resy,
+									 int fixeds,
+									 bool gendiags,
+									 float* tex_coords = 0);
+	static float CalculateUV(int resx, int resy, int ix, int iy, int id);
+	/* Create an ellipsoid													*/
+	static btSoftBody* CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,
+									   const btVector3& center,
+									   const btVector3& radius,
+									   int res);
+	/* Create from trimesh													*/
+	static btSoftBody* CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,
+										 const btScalar* vertices,
+										 const int* triangles,
+										 int ntriangles,
+										 bool randomizeConstraints = true);
+	/* Create from convex-hull												*/
+	static btSoftBody* CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo,
+											const btVector3* vertices,
+											int nvertices,
+											bool randomizeConstraints = true);
+
+	/* Export TetGen compatible .smesh file									*/
+	//	static void				ExportAsSMeshFile(	btSoftBody* psb,
+	//												const char* filename);
+	/* Create from TetGen .ele, .face, .node files							*/
+	//	static btSoftBody*		CreateFromTetGenFile(	btSoftBodyWorldInfo& worldInfo,
+	//													const char* ele,
+	//													const char* face,
+	//													const char* node,
+	//													bool bfacelinks,
+	//													bool btetralinks,
+	//													bool bfacesfromtetras);
+	/* Create from TetGen .ele, .face, .node data							*/
+	static btSoftBody* CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo,
+											const char* ele,
+											const char* face,
+											const char* node,
+											bool bfacelinks,
+											bool btetralinks,
+											bool bfacesfromtetras);
+	static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);
+
+	static void writeObj(const char* file, const btSoftBody* psb);
+
+	static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary);
+
+	static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary);
+
+	static void interpolateBarycentricWeights(btSoftBody* psb);
 
+	static void extrapolateBarycentricWeights(btSoftBody* psb);
 
-	/* Export TetGen compatible .smesh file									*/ 
-//	static void				ExportAsSMeshFile(	btSoftBody* psb,
-//												const char* filename);	
-	/* Create from TetGen .ele, .face, .node files							*/ 
-//	static btSoftBody*		CreateFromTetGenFile(	btSoftBodyWorldInfo& worldInfo,
-//													const char* ele,
-//													const char* face,
-//													const char* node,
-//													bool bfacelinks,
-//													bool btetralinks,
-//													bool bfacesfromtetras);
-	/* Create from TetGen .ele, .face, .node data							*/ 
-	static btSoftBody*		CreateFromTetGenData(	btSoftBodyWorldInfo& worldInfo,
-													const char* ele,
-													const char* face,
-													const char* node,
-													bool bfacelinks,
-													bool btetralinks,
-													bool bfacesfromtetras);
+	static void generateBoundaryFaces(btSoftBody* psb);
 
+	static void duplicateFaces(const char* filename, const btSoftBody* psb);
 	/// 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 );
+	static void ReoptimizeLinkOrder(btSoftBody* psb);
 };
 
-#endif //BT_SOFT_BODY_HELPERS_H
+#endif  //BT_SOFT_BODY_HELPERS_H
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h
index 1b9d02d79f9..c17bbb5cd4c 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h
@@ -18,30 +18,643 @@ subject to the following restrictions:
 #define _BT_SOFT_BODY_INTERNALS_H
 
 #include "btSoftBody.h"
-
-
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btPolarDecomposition.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 #include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
-#include <string.h> //for memset
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+#include <string.h>  //for memset
+#include <cmath>
+#include "poly34.h"
+
+// Given a multibody link, a contact point and a contact direction, fill in the jacobian data needed to calculate the velocity change given an impulse in the contact direction
+static SIMD_FORCE_INLINE void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
+										   btMultiBodyJacobianData& jacobianData,
+										   const btVector3& contact_point,
+										   const btVector3& dir)
+{
+	const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+	jacobianData.m_jacobians.resize(ndof);
+	jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
+	btScalar* jac = &jacobianData.m_jacobians[0];
+
+	multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
+	multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
+}
+static SIMD_FORCE_INLINE btVector3 generateUnitOrthogonalVector(const btVector3& u)
+{
+	btScalar ux = u.getX();
+	btScalar uy = u.getY();
+	btScalar uz = u.getZ();
+	btScalar ax = std::abs(ux);
+	btScalar ay = std::abs(uy);
+	btScalar az = std::abs(uz);
+	btVector3 v;
+	if (ax <= ay && ax <= az)
+		v = btVector3(0, -uz, uy);
+	else if (ay <= ax && ay <= az)
+		v = btVector3(-uz, 0, ux);
+	else
+		v = btVector3(-uy, ux, 0);
+	v.normalize();
+	return v;
+}
+
+static SIMD_FORCE_INLINE bool proximityTest(const btVector3& x1, const btVector3& x2, const btVector3& x3, const btVector3& x4, const btVector3& normal, const btScalar& mrg, btVector3& bary)
+{
+	btVector3 x43 = x4 - x3;
+	if (std::abs(x43.dot(normal)) > mrg)
+		return false;
+	btVector3 x13 = x1 - x3;
+	btVector3 x23 = x2 - x3;
+	btScalar a11 = x13.length2();
+	btScalar a22 = x23.length2();
+	btScalar a12 = x13.dot(x23);
+	btScalar b1 = x13.dot(x43);
+	btScalar b2 = x23.dot(x43);
+	btScalar det = a11 * a22 - a12 * a12;
+	if (det < SIMD_EPSILON)
+		return false;
+	btScalar w1 = (b1 * a22 - b2 * a12) / det;
+	btScalar w2 = (b2 * a11 - b1 * a12) / det;
+	btScalar w3 = 1 - w1 - w2;
+	btScalar delta = mrg / std::sqrt(0.5 * std::abs(x13.cross(x23).safeNorm()));
+	bary = btVector3(w1, w2, w3);
+	for (int i = 0; i < 3; ++i)
+	{
+		if (bary[i] < -delta || bary[i] > 1 + delta)
+			return false;
+	}
+	return true;
+}
+static const int KDOP_COUNT = 13;
+static btVector3 dop[KDOP_COUNT] = {btVector3(1, 0, 0),
+									btVector3(0, 1, 0),
+									btVector3(0, 0, 1),
+									btVector3(1, 1, 0),
+									btVector3(1, 0, 1),
+									btVector3(0, 1, 1),
+									btVector3(1, -1, 0),
+									btVector3(1, 0, -1),
+									btVector3(0, 1, -1),
+									btVector3(1, 1, 1),
+									btVector3(1, -1, 1),
+									btVector3(1, 1, -1),
+									btVector3(1, -1, -1)};
+
+static inline int getSign(const btVector3& n, const btVector3& x)
+{
+	btScalar d = n.dot(x);
+	if (d > SIMD_EPSILON)
+		return 1;
+	if (d < -SIMD_EPSILON)
+		return -1;
+	return 0;
+}
+
+static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
+{
+	btVector3 hex[6] = {face->m_n[0]->m_x - node->m_x,
+						face->m_n[1]->m_x - node->m_x,
+						face->m_n[2]->m_x - node->m_x,
+						face->m_n[0]->m_x + dt * face->m_n[0]->m_v - node->m_x,
+						face->m_n[1]->m_x + dt * face->m_n[1]->m_v - node->m_x,
+						face->m_n[2]->m_x + dt * face->m_n[2]->m_v - node->m_x};
+	btVector3 segment = dt * node->m_v;
+	for (int i = 0; i < KDOP_COUNT; ++i)
+	{
+		int s = getSign(dop[i], segment);
+		int j = 0;
+		for (; j < 6; ++j)
+		{
+			if (getSign(dop[i], hex[j]) == s)
+				break;
+		}
+		if (j == 6)
+			return true;
+	}
+	return false;
+}
+
+static SIMD_FORCE_INLINE bool nearZero(const btScalar& a)
+{
+	return (a > -SAFE_EPSILON && a < SAFE_EPSILON);
+}
+static SIMD_FORCE_INLINE bool sameSign(const btScalar& a, const btScalar& b)
+{
+	return (nearZero(a) || nearZero(b) || (a > SAFE_EPSILON && b > SAFE_EPSILON) || (a < -SAFE_EPSILON && b < -SAFE_EPSILON));
+}
+static SIMD_FORCE_INLINE bool diffSign(const btScalar& a, const btScalar& b)
+{
+	return !sameSign(a, b);
+}
+inline btScalar evaluateBezier2(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& t, const btScalar& s)
+{
+	btScalar s2 = s * s;
+	btScalar t2 = t * t;
+
+	return p0 * s2 + p1 * btScalar(2.0) * s * t + p2 * t2;
+}
+inline btScalar evaluateBezier(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& p3, const btScalar& t, const btScalar& s)
+{
+	btScalar s2 = s * s;
+	btScalar s3 = s2 * s;
+	btScalar t2 = t * t;
+	btScalar t3 = t2 * t;
+
+	return p0 * s3 + p1 * btScalar(3.0) * s2 * t + p2 * btScalar(3.0) * s * t2 + p3 * t3;
+}
+static SIMD_FORCE_INLINE bool getSigns(bool type_c, const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, const btScalar& t1, btScalar& lt0, btScalar& lt1)
+{
+	if (sameSign(t0, t1))
+	{
+		lt0 = t0;
+		lt1 = t0;
+		return true;
+	}
+
+	if (type_c || diffSign(k0, k3))
+	{
+		btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
+		if (t0 < -0)
+			ft = -ft;
+
+		if (sameSign(ft, k0))
+		{
+			lt0 = t1;
+			lt1 = t1;
+		}
+		else
+		{
+			lt0 = t0;
+			lt1 = t0;
+		}
+		return true;
+	}
+
+	if (!type_c)
+	{
+		btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
+		if (t0 < -0)
+			ft = -ft;
+
+		if (diffSign(ft, k0))
+		{
+			lt0 = t0;
+			lt1 = t1;
+			return true;
+		}
+
+		btScalar fk = evaluateBezier2(k1 - k0, k2 - k1, k3 - k2, t0, -t1);
+
+		if (sameSign(fk, k1 - k0))
+			lt0 = lt1 = t1;
+		else
+			lt0 = lt1 = t0;
+
+		return true;
+	}
+	return false;
+}
+
+static SIMD_FORCE_INLINE void getBernsteinCoeff(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, btScalar& k0, btScalar& k1, btScalar& k2, btScalar& k3)
+{
+	const btVector3& n0 = face->m_n0;
+	const btVector3& n1 = face->m_n1;
+	btVector3 n_hat = n0 + n1 - face->m_vn;
+	btVector3 p0ma0 = node->m_x - face->m_n[0]->m_x;
+	btVector3 p1ma1 = node->m_q - face->m_n[0]->m_q;
+	k0 = (p0ma0).dot(n0) * 3.0;
+	k1 = (p0ma0).dot(n_hat) + (p1ma1).dot(n0);
+	k2 = (p1ma1).dot(n_hat) + (p0ma0).dot(n1);
+	k3 = (p1ma1).dot(n1) * 3.0;
+}
+
+static SIMD_FORCE_INLINE void polyDecomposition(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, btScalar& u0, btScalar& u1, btScalar& v0, btScalar& v1)
+{
+	btScalar denom = 4.0 * (j1 - j2) * (j1 - j0) + (j2 - j0) * (j2 - j0);
+	u0 = (2.0 * (j1 - j2) * (3.0 * k1 - 2.0 * k0 - k3) - (j0 - j2) * (3.0 * k2 - 2.0 * k3 - k0)) / denom;
+	u1 = (2.0 * (j1 - j0) * (3.0 * k2 - 2.0 * k3 - k0) - (j2 - j0) * (3.0 * k1 - 2.0 * k0 - k3)) / denom;
+	v0 = k0 - u0 * j0;
+	v1 = k3 - u1 * j2;
+}
+
+static SIMD_FORCE_INLINE bool rootFindingLemma(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3)
+{
+	btScalar u0, u1, v0, v1;
+	btScalar j0 = 3.0 * (k1 - k0);
+	btScalar j1 = 3.0 * (k2 - k1);
+	btScalar j2 = 3.0 * (k3 - k2);
+	polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+	if (sameSign(v0, v1))
+	{
+		btScalar Ypa = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * v0 * (1.0 - v0) + j2 * v0 * v0;  // Y'(v0)
+		if (sameSign(Ypa, j0))
+		{
+			return (diffSign(k0, v1));
+		}
+	}
+	return diffSign(k0, v0);
+}
+
+static SIMD_FORCE_INLINE void getJs(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Node* a, const btSoftBody::Node* b, const btSoftBody::Node* c, const btSoftBody::Node* p, const btScalar& dt, btScalar& j0, btScalar& j1, btScalar& j2)
+{
+	const btVector3& a0 = a->m_x;
+	const btVector3& b0 = b->m_x;
+	const btVector3& c0 = c->m_x;
+	const btVector3& va = a->m_v;
+	const btVector3& vb = b->m_v;
+	const btVector3& vc = c->m_v;
+	const btVector3 a1 = a0 + dt * va;
+	const btVector3 b1 = b0 + dt * vb;
+	const btVector3 c1 = c0 + dt * vc;
+	btVector3 n0 = (b0 - a0).cross(c0 - a0);
+	btVector3 n1 = (b1 - a1).cross(c1 - a1);
+	btVector3 n_hat = n0 + n1 - dt * dt * (vb - va).cross(vc - va);
+	const btVector3& p0 = p->m_x;
+	const btVector3& vp = p->m_v;
+	btVector3 p1 = p0 + dt * vp;
+	btVector3 m0 = (b0 - p0).cross(c0 - p0);
+	btVector3 m1 = (b1 - p1).cross(c1 - p1);
+	btVector3 m_hat = m0 + m1 - dt * dt * (vb - vp).cross(vc - vp);
+	btScalar l0 = m0.dot(n0);
+	btScalar l1 = 0.25 * (m0.dot(n_hat) + m_hat.dot(n0));
+	btScalar l2 = btScalar(1) / btScalar(6) * (m0.dot(n1) + m_hat.dot(n_hat) + m1.dot(n0));
+	btScalar l3 = 0.25 * (m_hat.dot(n1) + m1.dot(n_hat));
+	btScalar l4 = m1.dot(n1);
+
+	btScalar k1p = 0.25 * k0 + 0.75 * k1;
+	btScalar k2p = 0.5 * k1 + 0.5 * k2;
+	btScalar k3p = 0.75 * k2 + 0.25 * k3;
+
+	btScalar s0 = (l1 * k0 - l0 * k1p) * 4.0;
+	btScalar s1 = (l2 * k0 - l0 * k2p) * 2.0;
+	btScalar s2 = (l3 * k0 - l0 * k3p) * btScalar(4) / btScalar(3);
+	btScalar s3 = l4 * k0 - l0 * k3;
+
+	j0 = (s1 * k0 - s0 * k1) * 3.0;
+	j1 = (s2 * k0 - s0 * k2) * 1.5;
+	j2 = (s3 * k0 - s0 * k3);
+}
+
+static SIMD_FORCE_INLINE bool signDetermination1Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1)
+{
+	btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0;  // Y(u0)
+	btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0;  // Y(v0)
+
+	btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0;
+	btScalar L = sameSign(sign_Ytp, k0) ? u1 : u0;
+	sign_Ytp = (v0 > v1) ? Yv0 : -Yv0;
+	btScalar K = (sameSign(sign_Ytp, k0)) ? v1 : v0;
+	return diffSign(L, K);
+}
+
+static SIMD_FORCE_INLINE bool signDetermination2Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1)
+{
+	btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0;  // Y(u0)
+	btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0, L1, L2;
+	if (diffSign(sign_Ytp, k0))
+	{
+		L1 = u0;
+		L2 = u1;
+	}
+	else
+	{
+		btScalar Yp_u0 = j0 * (1.0 - u0) * (1.0 - u0) + 2.0 * j1 * (1.0 - u0) * u0 + j2 * u0 * u0;
+		if (sameSign(Yp_u0, j0))
+		{
+			L1 = u1;
+			L2 = u1;
+		}
+		else
+		{
+			L1 = u0;
+			L2 = u0;
+		}
+	}
+	btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0;  // Y(uv0)
+	sign_Ytp = (v0 > v1) ? Yv0 : -Yv0;
+	btScalar K1, K2;
+	if (diffSign(sign_Ytp, k0))
+	{
+		K1 = v0;
+		K2 = v1;
+	}
+	else
+	{
+		btScalar Yp_v0 = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * (1.0 - v0) * v0 + j2 * v0 * v0;
+		if (sameSign(Yp_v0, j0))
+		{
+			K1 = v1;
+			K2 = v1;
+		}
+		else
+		{
+			K1 = v0;
+			K2 = v0;
+		}
+	}
+	return (diffSign(K1, L1) || diffSign(L2, K2));
+}
+
+static SIMD_FORCE_INLINE bool signDetermination1(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
+{
+	btScalar j0, j1, j2, u0, u1, v0, v1;
+	// p1
+	getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2);
+	if (nearZero(j0 + j2 - j1 * 2.0))
+	{
+		btScalar lt0, lt1;
+		getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
+		if (lt0 < -SAFE_EPSILON)
+			return false;
+	}
+	else
+	{
+		polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+		if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1))
+			return false;
+	}
+	// p2
+	getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2);
+	if (nearZero(j0 + j2 - j1 * 2.0))
+	{
+		btScalar lt0, lt1;
+		getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
+		if (lt0 < -SAFE_EPSILON)
+			return false;
+	}
+	else
+	{
+		polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+		if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1))
+			return false;
+	}
+	// p3
+	getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2);
+	if (nearZero(j0 + j2 - j1 * 2.0))
+	{
+		btScalar lt0, lt1;
+		getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
+		if (lt0 < -SAFE_EPSILON)
+			return false;
+	}
+	else
+	{
+		polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+		if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1))
+			return false;
+	}
+	return true;
+}
+
+static SIMD_FORCE_INLINE bool signDetermination2(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
+{
+	btScalar j0, j1, j2, u0, u1, v0, v1;
+	// p1
+	getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2);
+	if (nearZero(j0 + j2 - j1 * 2.0))
+	{
+		btScalar lt0, lt1;
+		bool bt0 = true, bt1 = true;
+		getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
+		if (lt0 < -SAFE_EPSILON)
+			bt0 = false;
+		if (lt1 < -SAFE_EPSILON)
+			bt1 = false;
+		if (!bt0 && !bt1)
+			return false;
+	}
+	else
+	{
+		polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+		if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1))
+			return false;
+	}
+	// p2
+	getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2);
+	if (nearZero(j0 + j2 - j1 * 2.0))
+	{
+		btScalar lt0, lt1;
+		bool bt0 = true, bt1 = true;
+		getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
+		if (lt0 < -SAFE_EPSILON)
+			bt0 = false;
+		if (lt1 < -SAFE_EPSILON)
+			bt1 = false;
+		if (!bt0 && !bt1)
+			return false;
+	}
+	else
+	{
+		polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+		if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1))
+			return false;
+	}
+	// p3
+	getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2);
+	if (nearZero(j0 + j2 - j1 * 2.0))
+	{
+		btScalar lt0, lt1;
+		bool bt0 = true, bt1 = true;
+		getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
+		if (lt0 < -SAFE_EPSILON)
+			bt0 = false;
+		if (lt1 < -SAFE_EPSILON)
+			bt1 = false;
+		if (!bt0 && !bt1)
+			return false;
+	}
+	else
+	{
+		polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+		if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1))
+			return false;
+	}
+	return true;
+}
+
+static SIMD_FORCE_INLINE bool coplanarAndInsideTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
+{
+	// Coplanar test
+	if (diffSign(k1 - k0, k3 - k2))
+	{
+		// Case b:
+		if (sameSign(k0, k3) && !rootFindingLemma(k0, k1, k2, k3))
+			return false;
+		// inside test
+		return signDetermination2(k0, k1, k2, k3, face, node, dt);
+	}
+	else
+	{
+		// Case c:
+		if (sameSign(k0, k3))
+			return false;
+		// inside test
+		return signDetermination1(k0, k1, k2, k3, face, node, dt);
+	}
+	return false;
+}
+static SIMD_FORCE_INLINE bool conservativeCulling(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg)
+{
+	if (k0 > mrg && k1 > mrg && k2 > mrg && k3 > mrg)
+		return true;
+	if (k0 < -mrg && k1 < -mrg && k2 < -mrg && k3 < -mrg)
+		return true;
+	return false;
+}
+
+static SIMD_FORCE_INLINE bool bernsteinVFTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
+{
+	if (conservativeCulling(k0, k1, k2, k3, mrg))
+		return false;
+	return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt);
+}
+
+static SIMD_FORCE_INLINE void deCasteljau(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, btScalar& k10, btScalar& k20, btScalar& k30, btScalar& k21, btScalar& k12)
+{
+	k10 = k0 * (1.0 - t0) + k1 * t0;
+	btScalar k11 = k1 * (1.0 - t0) + k2 * t0;
+	k12 = k2 * (1.0 - t0) + k3 * t0;
+	k20 = k10 * (1.0 - t0) + k11 * t0;
+	k21 = k11 * (1.0 - t0) + k12 * t0;
+	k30 = k20 * (1.0 - t0) + k21 * t0;
+}
+static SIMD_FORCE_INLINE bool bernsteinVFTest(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg)
+{
+	btScalar k0, k1, k2, k3;
+	getBernsteinCoeff(face, node, dt, k0, k1, k2, k3);
+	if (conservativeCulling(k0, k1, k2, k3, mrg))
+		return false;
+	return true;
+	if (diffSign(k2 - 2.0 * k1 + k0, k3 - 2.0 * k2 + k1))
+	{
+		btScalar k10, k20, k30, k21, k12;
+		btScalar t0 = (k2 - 2.0 * k1 + k0) / (k0 - 3.0 * k1 + 3.0 * k2 - k3);
+		deCasteljau(k0, k1, k2, k3, t0, k10, k20, k30, k21, k12);
+		return bernsteinVFTest(k0, k10, k20, k30, mrg, face, node, dt) || bernsteinVFTest(k30, k21, k12, k3, mrg, face, node, dt);
+	}
+	return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt);
+}
+
+static SIMD_FORCE_INLINE bool continuousCollisionDetection(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary)
+{
+	if (hasSeparatingPlane(face, node, dt))
+		return false;
+	btVector3 x21 = face->m_n[1]->m_x - face->m_n[0]->m_x;
+	btVector3 x31 = face->m_n[2]->m_x - face->m_n[0]->m_x;
+	btVector3 x41 = node->m_x - face->m_n[0]->m_x;
+	btVector3 v21 = face->m_n[1]->m_v - face->m_n[0]->m_v;
+	btVector3 v31 = face->m_n[2]->m_v - face->m_n[0]->m_v;
+	btVector3 v41 = node->m_v - face->m_n[0]->m_v;
+	btVector3 a = x21.cross(x31);
+	btVector3 b = x21.cross(v31) + v21.cross(x31);
+	btVector3 c = v21.cross(v31);
+	btVector3 d = x41;
+	btVector3 e = v41;
+	btScalar a0 = a.dot(d);
+	btScalar a1 = a.dot(e) + b.dot(d);
+	btScalar a2 = c.dot(d) + b.dot(e);
+	btScalar a3 = c.dot(e);
+	btScalar eps = SAFE_EPSILON;
+	int num_roots = 0;
+	btScalar roots[3];
+	if (std::abs(a3) < eps)
+	{
+		// cubic term is zero
+		if (std::abs(a2) < eps)
+		{
+			if (std::abs(a1) < eps)
+			{
+				if (std::abs(a0) < eps)
+				{
+					num_roots = 2;
+					roots[0] = 0;
+					roots[1] = dt;
+				}
+			}
+			else
+			{
+				num_roots = 1;
+				roots[0] = -a0 / a1;
+			}
+		}
+		else
+		{
+			num_roots = SolveP2(roots, a1 / a2, a0 / a2);
+		}
+	}
+	else
+	{
+		num_roots = SolveP3(roots, a2 / a3, a1 / a3, a0 / a3);
+	}
+	//    std::sort(roots, roots+num_roots);
+	if (num_roots > 1)
+	{
+		if (roots[0] > roots[1])
+			btSwap(roots[0], roots[1]);
+	}
+	if (num_roots > 2)
+	{
+		if (roots[0] > roots[2])
+			btSwap(roots[0], roots[2]);
+		if (roots[1] > roots[2])
+			btSwap(roots[1], roots[2]);
+	}
+	for (int r = 0; r < num_roots; ++r)
+	{
+		double root = roots[r];
+		if (root <= 0)
+			continue;
+		if (root > dt + SIMD_EPSILON)
+			return false;
+		btVector3 x1 = face->m_n[0]->m_x + root * face->m_n[0]->m_v;
+		btVector3 x2 = face->m_n[1]->m_x + root * face->m_n[1]->m_v;
+		btVector3 x3 = face->m_n[2]->m_x + root * face->m_n[2]->m_v;
+		btVector3 x4 = node->m_x + root * node->m_v;
+		btVector3 normal = (x2 - x1).cross(x3 - x1);
+		normal.safeNormalize();
+		if (proximityTest(x1, x2, x3, x4, normal, mrg, bary))
+			return true;
+	}
+	return false;
+}
+static SIMD_FORCE_INLINE bool bernsteinCCD(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary)
+{
+	if (!bernsteinVFTest(face, node, dt, mrg))
+		return false;
+	if (!continuousCollisionDetection(face, node, dt, 1e-6, bary))
+		return false;
+	return true;
+}
+
 //
 // btSymMatrix
 //
 template <typename T>
 struct btSymMatrix
 {
-	btSymMatrix() : dim(0)					{}
-	btSymMatrix(int n,const T& init=T())	{ resize(n,init); }
-	void					resize(int n,const T& init=T())			{ dim=n;store.resize((n*(n+1))/2,init); }
-	int						index(int c,int r) const				{ if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
-	T&						operator()(int c,int r)					{ return(store[index(c,r)]); }
-	const T&				operator()(int c,int r) const			{ return(store[index(c,r)]); }
-	btAlignedObjectArray<T>	store;
-	int						dim;
-};	
+	btSymMatrix() : dim(0) {}
+	btSymMatrix(int n, const T& init = T()) { resize(n, init); }
+	void resize(int n, const T& init = T())
+	{
+		dim = n;
+		store.resize((n * (n + 1)) / 2, init);
+	}
+	int index(int c, int r) const
+	{
+		if (c > r) btSwap(c, r);
+		btAssert(r < dim);
+		return ((r * (r + 1)) / 2 + c);
+	}
+	T& operator()(int c, int r) { return (store[index(c, r)]); }
+	const T& operator()(int c, int r) const { return (store[index(c, r)]); }
+	btAlignedObjectArray<T> store;
+	int dim;
+};
 
 //
 // btSoftBodyCollisionShape
@@ -49,67 +662,64 @@ struct btSymMatrix
 class btSoftBodyCollisionShape : public btConcaveShape
 {
 public:
-	btSoftBody*						m_body;
+	btSoftBody* m_body;
 
 	btSoftBodyCollisionShape(btSoftBody* backptr)
 	{
 		m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
-		m_body=backptr;
+		m_body = backptr;
 	}
 
 	virtual ~btSoftBodyCollisionShape()
 	{
-
 	}
 
-	void	processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
+	void processAllTriangles(btTriangleCallback* /*callback*/, const btVector3& /*aabbMin*/, const btVector3& /*aabbMax*/) const
 	{
 		//not yet
 		btAssert(0);
 	}
 
 	///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 	{
 		/* t is usually identity, except when colliding against btCompoundShape. See Issue 512 */
-		const btVector3	mins=m_body->m_bounds[0];
-		const btVector3	maxs=m_body->m_bounds[1];
-		const btVector3	crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
-			t*btVector3(maxs.x(),mins.y(),mins.z()),
-			t*btVector3(maxs.x(),maxs.y(),mins.z()),
-			t*btVector3(mins.x(),maxs.y(),mins.z()),
-			t*btVector3(mins.x(),mins.y(),maxs.z()),
-			t*btVector3(maxs.x(),mins.y(),maxs.z()),
-			t*btVector3(maxs.x(),maxs.y(),maxs.z()),
-			t*btVector3(mins.x(),maxs.y(),maxs.z())};
-		aabbMin=aabbMax=crns[0];
-		for(int i=1;i<8;++i)
+		const btVector3 mins = m_body->m_bounds[0];
+		const btVector3 maxs = m_body->m_bounds[1];
+		const btVector3 crns[] = {t * btVector3(mins.x(), mins.y(), mins.z()),
+								  t * btVector3(maxs.x(), mins.y(), mins.z()),
+								  t * btVector3(maxs.x(), maxs.y(), mins.z()),
+								  t * btVector3(mins.x(), maxs.y(), mins.z()),
+								  t * btVector3(mins.x(), mins.y(), maxs.z()),
+								  t * btVector3(maxs.x(), mins.y(), maxs.z()),
+								  t * btVector3(maxs.x(), maxs.y(), maxs.z()),
+								  t * btVector3(mins.x(), maxs.y(), maxs.z())};
+		aabbMin = aabbMax = crns[0];
+		for (int i = 1; i < 8; ++i)
 		{
 			aabbMin.setMin(crns[i]);
 			aabbMax.setMax(crns[i]);
 		}
 	}
 
-
-	virtual void	setLocalScaling(const btVector3& /*scaling*/)
-	{		
+	virtual void setLocalScaling(const btVector3& /*scaling*/)
+	{
 		///na
 	}
 	virtual const btVector3& getLocalScaling() const
 	{
-		static const btVector3 dummy(1,1,1);
+		static const btVector3 dummy(1, 1, 1);
 		return dummy;
 	}
-	virtual void	calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
+	virtual void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const
 	{
 		///not yet
 		btAssert(0);
 	}
-	virtual const char*	getName()const
+	virtual const char* getName() const
 	{
 		return "SoftBody";
 	}
-
 };
 
 //
@@ -118,48 +728,53 @@ public:
 class btSoftClusterCollisionShape : public btConvexInternalShape
 {
 public:
-	const btSoftBody::Cluster*	m_cluster;
-
-	btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
+	const btSoftBody::Cluster* m_cluster;
 
+	btSoftClusterCollisionShape(const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
 
-	virtual btVector3	localGetSupportingVertex(const btVector3& vec) const
+	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
 	{
-		btSoftBody::Node* const *						n=&m_cluster->m_nodes[0];
-		btScalar										d=btDot(vec,n[0]->m_x);
-		int												j=0;
-		for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
+		btSoftBody::Node* const* n = &m_cluster->m_nodes[0];
+		btScalar d = btDot(vec, n[0]->m_x);
+		int j = 0;
+		for (int i = 1, ni = m_cluster->m_nodes.size(); i < ni; ++i)
 		{
-			const btScalar	k=btDot(vec,n[i]->m_x);
-			if(k>d) { d=k;j=i; }
+			const btScalar k = btDot(vec, n[i]->m_x);
+			if (k > d)
+			{
+				d = k;
+				j = i;
+			}
 		}
-		return(n[j]->m_x);
+		return (n[j]->m_x);
 	}
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 	{
-		return(localGetSupportingVertex(vec));
+		return (localGetSupportingVertex(vec));
 	}
 	//notice that the vectors should be unit length
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
-	{}
-
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
+	{
+	}
 
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const
-	{}
+	virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const
+	{
+	}
 
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
-	{}
+	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
+	{
+	}
 
-	virtual int	getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
+	virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
 
 	//debugging
-	virtual const char*	getName()const {return "SOFTCLUSTER";}
+	virtual const char* getName() const { return "SOFTCLUSTER"; }
 
-	virtual void	setMargin(btScalar margin)
+	virtual void setMargin(btScalar margin)
 	{
 		btConvexInternalShape::setMargin(margin);
 	}
-	virtual btScalar	getMargin() const
+	virtual btScalar getMargin() const
 	{
 		return btConvexInternalShape::getMargin();
 	}
@@ -171,382 +786,548 @@ public:
 
 //
 template <typename T>
-static inline void			ZeroInitialize(T& value)
+static inline void ZeroInitialize(T& value)
 {
-	memset(&value,0,sizeof(T));
+	memset(&value, 0, sizeof(T));
 }
 //
 template <typename T>
-static inline bool			CompLess(const T& a,const T& b)
-{ return(a<b); }
+static inline bool CompLess(const T& a, const T& b)
+{
+	return (a < b);
+}
 //
 template <typename T>
-static inline bool			CompGreater(const T& a,const T& b)
-{ return(a>b); }
+static inline bool CompGreater(const T& a, const T& b)
+{
+	return (a > b);
+}
 //
 template <typename T>
-static inline T				Lerp(const T& a,const T& b,btScalar t)
-{ return(a+(b-a)*t); }
+static inline T Lerp(const T& a, const T& b, btScalar t)
+{
+	return (a + (b - a) * t);
+}
 //
 template <typename T>
-static inline T				InvLerp(const T& a,const T& b,btScalar t)
-{ return((b+a*t-b*t)/(a*b)); }
+static inline T InvLerp(const T& a, const T& b, btScalar t)
+{
+	return ((b + a * t - b * t) / (a * b));
+}
 //
-static inline btMatrix3x3	Lerp(	const btMatrix3x3& a,
-								 const btMatrix3x3& b,
-								 btScalar t)
+static inline btMatrix3x3 Lerp(const btMatrix3x3& a,
+							   const btMatrix3x3& b,
+							   btScalar t)
 {
-	btMatrix3x3	r;
-	r[0]=Lerp(a[0],b[0],t);
-	r[1]=Lerp(a[1],b[1],t);
-	r[2]=Lerp(a[2],b[2],t);
-	return(r);
+	btMatrix3x3 r;
+	r[0] = Lerp(a[0], b[0], t);
+	r[1] = Lerp(a[1], b[1], t);
+	r[2] = Lerp(a[2], b[2], t);
+	return (r);
 }
 //
-static inline btVector3		Clamp(const btVector3& v,btScalar maxlength)
+static inline btVector3 Clamp(const btVector3& v, btScalar maxlength)
 {
-	const btScalar sql=v.length2();
-	if(sql>(maxlength*maxlength))
-		return((v*maxlength)/btSqrt(sql));
+	const btScalar sql = v.length2();
+	if (sql > (maxlength * maxlength))
+		return ((v * maxlength) / btSqrt(sql));
 	else
-		return(v);
+		return (v);
 }
 //
 template <typename T>
-static inline T				Clamp(const T& x,const T& l,const T& h)
-{ return(x<l?l:x>h?h:x); }
+static inline T Clamp(const T& x, const T& l, const T& h)
+{
+	return (x < l ? l : x > h ? h : x);
+}
 //
 template <typename T>
-static inline T				Sq(const T& x)
-{ return(x*x); }
+static inline T Sq(const T& x)
+{
+	return (x * x);
+}
 //
 template <typename T>
-static inline T				Cube(const T& x)
-{ return(x*x*x); }
+static inline T Cube(const T& x)
+{
+	return (x * x * x);
+}
 //
 template <typename T>
-static inline T				Sign(const T& x)
-{ return((T)(x<0?-1:+1)); }
+static inline T Sign(const T& x)
+{
+	return ((T)(x < 0 ? -1 : +1));
+}
 //
 template <typename T>
-static inline bool			SameSign(const T& x,const T& y)
-{ return((x*y)>0); }
+static inline bool SameSign(const T& x, const T& y)
+{
+	return ((x * y) > 0);
+}
 //
-static inline btScalar		ClusterMetric(const btVector3& x,const btVector3& y)
+static inline btScalar ClusterMetric(const btVector3& x, const btVector3& y)
 {
-	const btVector3	d=x-y;
-	return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
+	const btVector3 d = x - y;
+	return (btFabs(d[0]) + btFabs(d[1]) + btFabs(d[2]));
 }
 //
-static inline btMatrix3x3	ScaleAlongAxis(const btVector3& a,btScalar s)
+static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a, btScalar s)
 {
-	const btScalar	xx=a.x()*a.x();
-	const btScalar	yy=a.y()*a.y();
-	const btScalar	zz=a.z()*a.z();
-	const btScalar	xy=a.x()*a.y();
-	const btScalar	yz=a.y()*a.z();
-	const btScalar	zx=a.z()*a.x();
-	btMatrix3x3		m;
-	m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx);
-	m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz);
-	m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s);
-	return(m);
+	const btScalar xx = a.x() * a.x();
+	const btScalar yy = a.y() * a.y();
+	const btScalar zz = a.z() * a.z();
+	const btScalar xy = a.x() * a.y();
+	const btScalar yz = a.y() * a.z();
+	const btScalar zx = a.z() * a.x();
+	btMatrix3x3 m;
+	m[0] = btVector3(1 - xx + xx * s, xy * s - xy, zx * s - zx);
+	m[1] = btVector3(xy * s - xy, 1 - yy + yy * s, yz * s - yz);
+	m[2] = btVector3(zx * s - zx, yz * s - yz, 1 - zz + zz * s);
+	return (m);
 }
 //
-static inline btMatrix3x3	Cross(const btVector3& v)
+static inline btMatrix3x3 Cross(const btVector3& v)
+{
+	btMatrix3x3 m;
+	m[0] = btVector3(0, -v.z(), +v.y());
+	m[1] = btVector3(+v.z(), 0, -v.x());
+	m[2] = btVector3(-v.y(), +v.x(), 0);
+	return (m);
+}
+//
+static inline btMatrix3x3 Diagonal(btScalar x)
+{
+	btMatrix3x3 m;
+	m[0] = btVector3(x, 0, 0);
+	m[1] = btVector3(0, x, 0);
+	m[2] = btVector3(0, 0, x);
+	return (m);
+}
+
+static inline btMatrix3x3 Diagonal(const btVector3& v)
+{
+	btMatrix3x3 m;
+	m[0] = btVector3(v.getX(), 0, 0);
+	m[1] = btVector3(0, v.getY(), 0);
+	m[2] = btVector3(0, 0, v.getZ());
+	return (m);
+}
+
+static inline btScalar Dot(const btScalar* a, const btScalar* b, int ndof)
+{
+	btScalar result = 0;
+	for (int i = 0; i < ndof; ++i)
+		result += a[i] * b[i];
+	return result;
+}
+
+static inline btMatrix3x3 OuterProduct(const btScalar* v1, const btScalar* v2, const btScalar* v3,
+									   const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof)
+{
+	btMatrix3x3 m;
+	btScalar a11 = Dot(v1, u1, ndof);
+	btScalar a12 = Dot(v1, u2, ndof);
+	btScalar a13 = Dot(v1, u3, ndof);
+
+	btScalar a21 = Dot(v2, u1, ndof);
+	btScalar a22 = Dot(v2, u2, ndof);
+	btScalar a23 = Dot(v2, u3, ndof);
+
+	btScalar a31 = Dot(v3, u1, ndof);
+	btScalar a32 = Dot(v3, u2, ndof);
+	btScalar a33 = Dot(v3, u3, ndof);
+	m[0] = btVector3(a11, a12, a13);
+	m[1] = btVector3(a21, a22, a23);
+	m[2] = btVector3(a31, a32, a33);
+	return (m);
+}
+
+static inline btMatrix3x3 OuterProduct(const btVector3& v1, const btVector3& v2)
 {
-	btMatrix3x3	m;
-	m[0]=btVector3(0,-v.z(),+v.y());
-	m[1]=btVector3(+v.z(),0,-v.x());
-	m[2]=btVector3(-v.y(),+v.x(),0);
-	return(m);
+	btMatrix3x3 m;
+	btScalar a11 = v1[0] * v2[0];
+	btScalar a12 = v1[0] * v2[1];
+	btScalar a13 = v1[0] * v2[2];
+
+	btScalar a21 = v1[1] * v2[0];
+	btScalar a22 = v1[1] * v2[1];
+	btScalar a23 = v1[1] * v2[2];
+
+	btScalar a31 = v1[2] * v2[0];
+	btScalar a32 = v1[2] * v2[1];
+	btScalar a33 = v1[2] * v2[2];
+	m[0] = btVector3(a11, a12, a13);
+	m[1] = btVector3(a21, a22, a23);
+	m[2] = btVector3(a31, a32, a33);
+	return (m);
 }
+
 //
-static inline btMatrix3x3	Diagonal(btScalar x)
+static inline btMatrix3x3 Add(const btMatrix3x3& a,
+							  const btMatrix3x3& b)
 {
-	btMatrix3x3	m;
-	m[0]=btVector3(x,0,0);
-	m[1]=btVector3(0,x,0);
-	m[2]=btVector3(0,0,x);
-	return(m);
+	btMatrix3x3 r;
+	for (int i = 0; i < 3; ++i) r[i] = a[i] + b[i];
+	return (r);
 }
 //
-static inline btMatrix3x3	Add(const btMatrix3x3& a,
-								const btMatrix3x3& b)
+static inline btMatrix3x3 Sub(const btMatrix3x3& a,
+							  const btMatrix3x3& b)
 {
-	btMatrix3x3	r;
-	for(int i=0;i<3;++i) r[i]=a[i]+b[i];
-	return(r);
+	btMatrix3x3 r;
+	for (int i = 0; i < 3; ++i) r[i] = a[i] - b[i];
+	return (r);
 }
 //
-static inline btMatrix3x3	Sub(const btMatrix3x3& a,
-								const btMatrix3x3& b)
+static inline btMatrix3x3 Mul(const btMatrix3x3& a,
+							  btScalar b)
 {
-	btMatrix3x3	r;
-	for(int i=0;i<3;++i) r[i]=a[i]-b[i];
-	return(r);
+	btMatrix3x3 r;
+	for (int i = 0; i < 3; ++i) r[i] = a[i] * b;
+	return (r);
 }
 //
-static inline btMatrix3x3	Mul(const btMatrix3x3& a,
-								btScalar b)
+static inline void Orthogonalize(btMatrix3x3& m)
 {
-	btMatrix3x3	r;
-	for(int i=0;i<3;++i) r[i]=a[i]*b;
-	return(r);
+	m[2] = btCross(m[0], m[1]).normalized();
+	m[1] = btCross(m[2], m[0]).normalized();
+	m[0] = btCross(m[1], m[2]).normalized();
 }
 //
-static inline void			Orthogonalize(btMatrix3x3& m)
+static inline btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3& iwi, const btVector3& r)
 {
-	m[2]=btCross(m[0],m[1]).normalized();
-	m[1]=btCross(m[2],m[0]).normalized();
-	m[0]=btCross(m[1],m[2]).normalized();
+	const btMatrix3x3 cr = Cross(r);
+	return (Sub(Diagonal(im), cr * iwi * cr));
 }
+
 //
-static inline btMatrix3x3	MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r)
+static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
+										btScalar ima,
+										btScalar imb,
+										const btMatrix3x3& iwi,
+										const btVector3& r)
 {
-	const btMatrix3x3	cr=Cross(r);
-	return(Sub(Diagonal(im),cr*iwi*cr));
+	return (Diagonal(1 / dt) * Add(Diagonal(ima), MassMatrix(imb, iwi, r)).inverse());
 }
 
 //
-static inline btMatrix3x3	ImpulseMatrix(	btScalar dt,
-										  btScalar ima,
-										  btScalar imb,
-										  const btMatrix3x3& iwi,
-										  const btVector3& r)
+static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
+										const btMatrix3x3& effective_mass_inv,
+										btScalar imb,
+										const btMatrix3x3& iwi,
+										const btVector3& r)
 {
-	return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
+	return (Diagonal(1 / dt) * Add(effective_mass_inv, MassMatrix(imb, iwi, r)).inverse());
+	//    btMatrix3x3 iimb = MassMatrix(imb, iwi, r);
+	//    if (iimb.determinant() == 0)
+	//        return effective_mass_inv.inverse();
+	//    return effective_mass_inv.inverse() *  Add(effective_mass_inv.inverse(), iimb.inverse()).inverse() * iimb.inverse();
 }
 
 //
-static inline btMatrix3x3	ImpulseMatrix(	btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
-										  btScalar imb,const btMatrix3x3& iib,const btVector3& rb)	
+static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra,
+										btScalar imb, const btMatrix3x3& iib, const btVector3& rb)
 {
-	return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
+	return (Add(MassMatrix(ima, iia, ra), MassMatrix(imb, iib, rb)).inverse());
 }
 
 //
-static inline btMatrix3x3	AngularImpulseMatrix(	const btMatrix3x3& iia,
-												 const btMatrix3x3& iib)
+static inline btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3& iia,
+											   const btMatrix3x3& iib)
 {
-	return(Add(iia,iib).inverse());
+	return (Add(iia, iib).inverse());
 }
 
 //
-static inline btVector3		ProjectOnAxis(	const btVector3& v,
-										  const btVector3& a)
+static inline btVector3 ProjectOnAxis(const btVector3& v,
+									  const btVector3& a)
 {
-	return(a*btDot(v,a));
+	return (a * btDot(v, a));
 }
 //
-static inline btVector3		ProjectOnPlane(	const btVector3& v,
-										   const btVector3& a)
+static inline btVector3 ProjectOnPlane(const btVector3& v,
+									   const btVector3& a)
 {
-	return(v-ProjectOnAxis(v,a));
+	return (v - ProjectOnAxis(v, a));
 }
 
 //
-static inline void			ProjectOrigin(	const btVector3& a,
-										  const btVector3& b,
-										  btVector3& prj,
-										  btScalar& sqd)
+static inline void ProjectOrigin(const btVector3& a,
+								 const btVector3& b,
+								 btVector3& prj,
+								 btScalar& sqd)
 {
-	const btVector3	d=b-a;
-	const btScalar	m2=d.length2();
-	if(m2>SIMD_EPSILON)
-	{	
-		const btScalar	t=Clamp<btScalar>(-btDot(a,d)/m2,0,1);
-		const btVector3	p=a+d*t;
-		const btScalar	l2=p.length2();
-		if(l2<sqd)
+	const btVector3 d = b - a;
+	const btScalar m2 = d.length2();
+	if (m2 > SIMD_EPSILON)
+	{
+		const btScalar t = Clamp<btScalar>(-btDot(a, d) / m2, 0, 1);
+		const btVector3 p = a + d * t;
+		const btScalar l2 = p.length2();
+		if (l2 < sqd)
 		{
-			prj=p;
-			sqd=l2;
+			prj = p;
+			sqd = l2;
 		}
 	}
 }
 //
-static inline void			ProjectOrigin(	const btVector3& a,
-										  const btVector3& b,
-										  const btVector3& c,
-										  btVector3& prj,
-										  btScalar& sqd)
+static inline void ProjectOrigin(const btVector3& a,
+								 const btVector3& b,
+								 const btVector3& c,
+								 btVector3& prj,
+								 btScalar& sqd)
 {
-	const btVector3&	q=btCross(b-a,c-a);
-	const btScalar		m2=q.length2();
-	if(m2>SIMD_EPSILON)
+	const btVector3& q = btCross(b - a, c - a);
+	const btScalar m2 = q.length2();
+	if (m2 > SIMD_EPSILON)
 	{
-		const btVector3	n=q/btSqrt(m2);
-		const btScalar	k=btDot(a,n);
-		const btScalar	k2=k*k;
-		if(k2<sqd)
+		const btVector3 n = q / btSqrt(m2);
+		const btScalar k = btDot(a, n);
+		const btScalar k2 = k * k;
+		if (k2 < sqd)
 		{
-			const btVector3	p=n*k;
-			if(	(btDot(btCross(a-p,b-p),q)>0)&&
-				(btDot(btCross(b-p,c-p),q)>0)&&
-				(btDot(btCross(c-p,a-p),q)>0))
-			{			
-				prj=p;
-				sqd=k2;
+			const btVector3 p = n * k;
+			if ((btDot(btCross(a - p, b - p), q) > 0) &&
+				(btDot(btCross(b - p, c - p), q) > 0) &&
+				(btDot(btCross(c - p, a - p), q) > 0))
+			{
+				prj = p;
+				sqd = k2;
 			}
 			else
 			{
-				ProjectOrigin(a,b,prj,sqd);
-				ProjectOrigin(b,c,prj,sqd);
-				ProjectOrigin(c,a,prj,sqd);
+				ProjectOrigin(a, b, prj, sqd);
+				ProjectOrigin(b, c, prj, sqd);
+				ProjectOrigin(c, a, prj, sqd);
 			}
 		}
 	}
 }
 
 //
+static inline bool rayIntersectsTriangle(const btVector3& origin, const btVector3& dir, const btVector3& v0, const btVector3& v1, const btVector3& v2, btScalar& t)
+{
+	btScalar a, f, u, v;
+
+	btVector3 e1 = v1 - v0;
+	btVector3 e2 = v2 - v0;
+	btVector3 h = dir.cross(e2);
+	a = e1.dot(h);
+
+	if (a > -0.00001 && a < 0.00001)
+		return (false);
+
+	f = btScalar(1) / a;
+	btVector3 s = origin - v0;
+	u = f * s.dot(h);
+
+	if (u < 0.0 || u > 1.0)
+		return (false);
+
+	btVector3 q = s.cross(e1);
+	v = f * dir.dot(q);
+	if (v < 0.0 || u + v > 1.0)
+		return (false);
+	// at this stage we can compute t to find out where
+	// the intersection point is on the line
+	t = f * e2.dot(q);
+	if (t > 0)  // ray intersection
+		return (true);
+	else  // this means that there is a line intersection
+		// but not a ray intersection
+		return (false);
+}
+
+static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVector3& rayEnd, const btVector3& p1, const btVector3& p2, const btVector3& p3, btVector3& sect, btVector3& normal)
+{
+	btVector3 dir = rayEnd - rayStart;
+	btScalar dir_norm = dir.norm();
+	if (dir_norm < SIMD_EPSILON)
+		return false;
+	dir.normalize();
+	btScalar t;
+	bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t);
+
+	if (ret)
+	{
+		if (t <= dir_norm)
+		{
+			sect = rayStart + dir * t;
+		}
+		else
+		{
+			ret = false;
+		}
+	}
+
+	if (ret)
+	{
+		btVector3 n = (p3 - p1).cross(p2 - p1);
+		n.safeNormalize();
+		if (n.dot(dir) < 0)
+			normal = n;
+		else
+			normal = -n;
+	}
+	return ret;
+}
+
+//
 template <typename T>
-static inline T				BaryEval(		const T& a,
-									 const T& b,
-									 const T& c,
-									 const btVector3& coord)
+static inline T BaryEval(const T& a,
+						 const T& b,
+						 const T& c,
+						 const btVector3& coord)
 {
-	return(a*coord.x()+b*coord.y()+c*coord.z());
+	return (a * coord.x() + b * coord.y() + c * coord.z());
 }
 //
-static inline btVector3		BaryCoord(	const btVector3& a,
-									  const btVector3& b,
-									  const btVector3& c,
-									  const btVector3& p)
+static inline btVector3 BaryCoord(const btVector3& a,
+								  const btVector3& b,
+								  const btVector3& c,
+								  const btVector3& p)
 {
-	const btScalar	w[]={	btCross(a-p,b-p).length(),
-		btCross(b-p,c-p).length(),
-		btCross(c-p,a-p).length()};
-	const btScalar	isum=1/(w[0]+w[1]+w[2]);
-	return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
+	const btScalar w[] = {btCross(a - p, b - p).length(),
+						  btCross(b - p, c - p).length(),
+						  btCross(c - p, a - p).length()};
+	const btScalar isum = 1 / (w[0] + w[1] + w[2]);
+	return (btVector3(w[1] * isum, w[2] * isum, w[0] * isum));
 }
 
 //
-static inline btScalar				ImplicitSolve(	btSoftBody::ImplicitFn* fn,
-										  const btVector3& a,
-										  const btVector3& b,
-										  const btScalar accuracy,
-										  const int maxiterations=256)
+inline static btScalar ImplicitSolve(btSoftBody::ImplicitFn* fn,
+									 const btVector3& a,
+									 const btVector3& b,
+									 const btScalar accuracy,
+									 const int maxiterations = 256)
 {
-	btScalar	span[2]={0,1};
-	btScalar	values[2]={fn->Eval(a),fn->Eval(b)};
-	if(values[0]>values[1])
+	btScalar span[2] = {0, 1};
+	btScalar values[2] = {fn->Eval(a), fn->Eval(b)};
+	if (values[0] > values[1])
 	{
-		btSwap(span[0],span[1]);
-		btSwap(values[0],values[1]);
+		btSwap(span[0], span[1]);
+		btSwap(values[0], values[1]);
 	}
-	if(values[0]>-accuracy) return(-1);
-	if(values[1]<+accuracy) return(-1);
-	for(int i=0;i<maxiterations;++i)
+	if (values[0] > -accuracy) return (-1);
+	if (values[1] < +accuracy) return (-1);
+	for (int i = 0; i < maxiterations; ++i)
 	{
-		const btScalar	t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
-		const btScalar	v=fn->Eval(Lerp(a,b,t));
-		if((t<=0)||(t>=1))		break;
-		if(btFabs(v)<accuracy)	return(t);
-		if(v<0)
-		{ span[0]=t;values[0]=v; }
+		const btScalar t = Lerp(span[0], span[1], values[0] / (values[0] - values[1]));
+		const btScalar v = fn->Eval(Lerp(a, b, t));
+		if ((t <= 0) || (t >= 1)) break;
+		if (btFabs(v) < accuracy) return (t);
+		if (v < 0)
+		{
+			span[0] = t;
+			values[0] = v;
+		}
 		else
-		{ span[1]=t;values[1]=v; }
+		{
+			span[1] = t;
+			values[1] = v;
+		}
 	}
-	return(-1);
+	return (-1);
 }
 
-//
-static inline btVector3		NormalizeAny(const btVector3& v)
+inline static void EvaluateMedium(const btSoftBodyWorldInfo* wfi,
+								  const btVector3& x,
+								  btSoftBody::sMedium& medium)
 {
-	const btScalar l=v.length();
-	if(l>SIMD_EPSILON)
-		return(v/l);
-	else
-		return(btVector3(0,0,0));
+	medium.m_velocity = btVector3(0, 0, 0);
+	medium.m_pressure = 0;
+	medium.m_density = wfi->air_density;
+	if (wfi->water_density > 0)
+	{
+		const btScalar depth = -(btDot(x, wfi->water_normal) + wfi->water_offset);
+		if (depth > 0)
+		{
+			medium.m_density = wfi->water_density;
+			medium.m_pressure = depth * wfi->water_density * wfi->m_gravity.length();
+		}
+	}
 }
 
 //
-static inline btDbvtVolume	VolumeOf(	const btSoftBody::Face& f,
-									 btScalar margin)
+static inline btVector3 NormalizeAny(const btVector3& v)
 {
-	const btVector3*	pts[]={	&f.m_n[0]->m_x,
-		&f.m_n[1]->m_x,
-		&f.m_n[2]->m_x};
-	btDbvtVolume		vol=btDbvtVolume::FromPoints(pts,3);
-	vol.Expand(btVector3(margin,margin,margin));
-	return(vol);
+	const btScalar l = v.length();
+	if (l > SIMD_EPSILON)
+		return (v / l);
+	else
+		return (btVector3(0, 0, 0));
 }
 
 //
-static inline btVector3			CenterOf(	const btSoftBody::Face& f)
+static inline btDbvtVolume VolumeOf(const btSoftBody::Face& f,
+									btScalar margin)
 {
-	return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
+	const btVector3* pts[] = {&f.m_n[0]->m_x,
+							  &f.m_n[1]->m_x,
+							  &f.m_n[2]->m_x};
+	btDbvtVolume vol = btDbvtVolume::FromPoints(pts, 3);
+	vol.Expand(btVector3(margin, margin, margin));
+	return (vol);
 }
 
 //
-static inline btScalar			AreaOf(		const btVector3& x0,
-									   const btVector3& x1,
-									   const btVector3& x2)
+static inline btVector3 CenterOf(const btSoftBody::Face& f)
 {
-	const btVector3	a=x1-x0;
-	const btVector3	b=x2-x0;
-	const btVector3	cr=btCross(a,b);
-	const btScalar	area=cr.length();
-	return(area);
+	return ((f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3);
 }
 
 //
-static inline btScalar		VolumeOf(	const btVector3& x0,
-									 const btVector3& x1,
-									 const btVector3& x2,
-									 const btVector3& x3)
+static inline btScalar AreaOf(const btVector3& x0,
+							  const btVector3& x1,
+							  const btVector3& x2)
 {
-	const btVector3	a=x1-x0;
-	const btVector3	b=x2-x0;
-	const btVector3	c=x3-x0;
-	return(btDot(a,btCross(b,c)));
+	const btVector3 a = x1 - x0;
+	const btVector3 b = x2 - x0;
+	const btVector3 cr = btCross(a, b);
+	const btScalar area = cr.length();
+	return (area);
 }
 
 //
-static inline void					EvaluateMedium(	const btSoftBodyWorldInfo* wfi,
-										   const btVector3& x,
-										   btSoftBody::sMedium& medium)
+static inline btScalar VolumeOf(const btVector3& x0,
+								const btVector3& x1,
+								const btVector3& x2,
+								const btVector3& x3)
 {
-	medium.m_velocity	=	btVector3(0,0,0);
-	medium.m_pressure	=	0;
-	medium.m_density	=	wfi->air_density;
-	if(wfi->water_density>0)
-	{
-		const btScalar	depth=-(btDot(x,wfi->water_normal)+wfi->water_offset);
-		if(depth>0)
-		{
-			medium.m_density	=	wfi->water_density;
-			medium.m_pressure	=	depth*wfi->water_density*wfi->m_gravity.length();
-		}
-	}
+	const btVector3 a = x1 - x0;
+	const btVector3 b = x2 - x0;
+	const btVector3 c = x3 - x0;
+	return (btDot(a, btCross(b, c)));
 }
 
 //
-static inline void			ApplyClampedForce(	btSoftBody::Node& n,
-											  const btVector3& f,
-											  btScalar dt)
+
+//
+static inline void ApplyClampedForce(btSoftBody::Node& n,
+									 const btVector3& f,
+									 btScalar dt)
 {
-	const btScalar	dtim=dt*n.m_im;
-	if((f*dtim).length2()>n.m_v.length2())
-	{/* Clamp	*/ 
-		n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim;						
+	const btScalar dtim = dt * n.m_im;
+	if ((f * dtim).length2() > n.m_v.length2())
+	{ /* Clamp	*/
+		n.m_f -= ProjectOnAxis(n.m_v, f.normalized()) / dtim;
 	}
 	else
-	{/* Apply	*/ 
-		n.m_f+=f;
+	{ /* Apply	*/
+		n.m_f += f;
 	}
 }
 
 //
-static inline int		MatchEdge(	const btSoftBody::Node* a,
-								  const btSoftBody::Node* b,
-								  const btSoftBody::Node* ma,
-								  const btSoftBody::Node* mb)
+static inline int MatchEdge(const btSoftBody::Node* a,
+							const btSoftBody::Node* b,
+							const btSoftBody::Node* ma,
+							const btSoftBody::Node* mb)
 {
-	if((a==ma)&&(b==mb)) return(0);
-	if((a==mb)&&(b==ma)) return(1);
-	return(-1);
+	if ((a == ma) && (b == mb)) return (0);
+	if ((a == mb) && (b == ma)) return (1);
+	return (-1);
 }
 
 //
@@ -554,58 +1335,72 @@ static inline int		MatchEdge(	const btSoftBody::Node* a,
 // straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
 // outputs are NOT sorted.
 //
-struct	btEigen
+struct btEigen
 {
-	static int			system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
+	static int system(btMatrix3x3& a, btMatrix3x3* vectors, btVector3* values = 0)
 	{
-		static const int		maxiterations=16;
-		static const btScalar	accuracy=(btScalar)0.0001;
-		btMatrix3x3&			v=*vectors;
-		int						iterations=0;
+		static const int maxiterations = 16;
+		static const btScalar accuracy = (btScalar)0.0001;
+		btMatrix3x3& v = *vectors;
+		int iterations = 0;
 		vectors->setIdentity();
-		do	{
-			int				p=0,q=1;
-			if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
-			if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
-			if(btFabs(a[p][q])>accuracy)
+		do
+		{
+			int p = 0, q = 1;
+			if (btFabs(a[p][q]) < btFabs(a[0][2]))
+			{
+				p = 0;
+				q = 2;
+			}
+			if (btFabs(a[p][q]) < btFabs(a[1][2]))
 			{
-				const btScalar	w=(a[q][q]-a[p][p])/(2*a[p][q]);
-				const btScalar	z=btFabs(w);
-				const btScalar	t=w/(z*(btSqrt(1+w*w)+z));
-				if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ 
+				p = 1;
+				q = 2;
+			}
+			if (btFabs(a[p][q]) > accuracy)
+			{
+				const btScalar w = (a[q][q] - a[p][p]) / (2 * a[p][q]);
+				const btScalar z = btFabs(w);
+				const btScalar t = w / (z * (btSqrt(1 + w * w) + z));
+				if (t == t) /* [WARNING] let hope that one does not get thrown aways by some compilers... */
 				{
-					const btScalar	c=1/btSqrt(t*t+1);
-					const btScalar	s=c*t;
-					mulPQ(a,c,s,p,q);
-					mulTPQ(a,c,s,p,q);
-					mulPQ(v,c,s,p,q);
-				} else break;
-			} else break;
-		} while((++iterations)<maxiterations);
-		if(values)
+					const btScalar c = 1 / btSqrt(t * t + 1);
+					const btScalar s = c * t;
+					mulPQ(a, c, s, p, q);
+					mulTPQ(a, c, s, p, q);
+					mulPQ(v, c, s, p, q);
+				}
+				else
+					break;
+			}
+			else
+				break;
+		} while ((++iterations) < maxiterations);
+		if (values)
 		{
-			*values=btVector3(a[0][0],a[1][1],a[2][2]);
+			*values = btVector3(a[0][0], a[1][1], a[2][2]);
 		}
-		return(iterations);
+		return (iterations);
 	}
+
 private:
-	static inline void	mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
+	static inline void mulTPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q)
 	{
-		const btScalar	m[2][3]={	{a[p][0],a[p][1],a[p][2]},
-		{a[q][0],a[q][1],a[q][2]}};
+		const btScalar m[2][3] = {{a[p][0], a[p][1], a[p][2]},
+								  {a[q][0], a[q][1], a[q][2]}};
 		int i;
 
-		for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
-		for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
+		for (i = 0; i < 3; ++i) a[p][i] = c * m[0][i] - s * m[1][i];
+		for (i = 0; i < 3; ++i) a[q][i] = c * m[1][i] + s * m[0][i];
 	}
-	static inline void	mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
+	static inline void mulPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q)
 	{
-		const btScalar	m[2][3]={	{a[0][p],a[1][p],a[2][p]},
-		{a[0][q],a[1][q],a[2][q]}};
+		const btScalar m[2][3] = {{a[0][p], a[1][p], a[2][p]},
+								  {a[0][q], a[1][q], a[2][q]}};
 		int i;
 
-		for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
-		for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
+		for (i = 0; i < 3; ++i) a[i][p] = c * m[0][i] - s * m[1][i];
+		for (i = 0; i < 3; ++i) a[i][q] = c * m[1][i] + s * m[0][i];
 	}
 };
 
@@ -613,9 +1408,9 @@ private:
 // Polar decomposition,
 // "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
 //
-static inline int			PolarDecompose(	const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s)
+static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
 {
-	static const btPolarDecomposition polar;  
+	static const btPolarDecomposition polar;
 	return polar.decompose(m, q, s);
 }
 
@@ -627,282 +1422,687 @@ struct btSoftColliders
 	//
 	// ClusterBase
 	//
-	struct	ClusterBase : btDbvt::ICollide
+	struct ClusterBase : btDbvt::ICollide
 	{
-		btScalar			erp;
-		btScalar			idt;
-		btScalar			m_margin;
-		btScalar			friction;
-		btScalar			threshold;
+		btScalar erp;
+		btScalar idt;
+		btScalar m_margin;
+		btScalar friction;
+		btScalar threshold;
 		ClusterBase()
 		{
-			erp			=(btScalar)1;
-			idt			=0;
-			m_margin		=0;
-			friction	=0;
-			threshold	=(btScalar)0;
+			erp = (btScalar)1;
+			idt = 0;
+			m_margin = 0;
+			friction = 0;
+			threshold = (btScalar)0;
 		}
-		bool				SolveContact(	const btGjkEpaSolver2::sResults& res,
-			btSoftBody::Body ba,const btSoftBody::Body bb,
-			btSoftBody::CJoint& joint)
+		bool SolveContact(const btGjkEpaSolver2::sResults& res,
+						  btSoftBody::Body ba, const btSoftBody::Body bb,
+						  btSoftBody::CJoint& joint)
 		{
-			if(res.distance<m_margin)
+			if (res.distance < m_margin)
 			{
 				btVector3 norm = res.normal;
-				norm.normalize();//is it necessary?
-
-				const btVector3		ra=res.witnesses[0]-ba.xform().getOrigin();
-				const btVector3		rb=res.witnesses[1]-bb.xform().getOrigin();
-				const btVector3		va=ba.velocity(ra);
-				const btVector3		vb=bb.velocity(rb);
-				const btVector3		vrel=va-vb;
-				const btScalar		rvac=btDot(vrel,norm);
-				 btScalar		depth=res.distance-m_margin;
-				
-//				printf("depth=%f\n",depth);
-				const btVector3		iv=norm*rvac;
-				const btVector3		fv=vrel-iv;
-				joint.m_bodies[0]	=	ba;
-				joint.m_bodies[1]	=	bb;
-				joint.m_refs[0]		=	ra*ba.xform().getBasis();
-				joint.m_refs[1]		=	rb*bb.xform().getBasis();
-				joint.m_rpos[0]		=	ra;
-				joint.m_rpos[1]		=	rb;
-				joint.m_cfm			=	1;
-				joint.m_erp			=	1;
-				joint.m_life		=	0;
-				joint.m_maxlife		=	0;
-				joint.m_split		=	1;
-				
-				joint.m_drift		=	depth*norm;
-
-				joint.m_normal		=	norm;
-//				printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ());
-				joint.m_delete		=	false;
-				joint.m_friction	=	fv.length2()<(rvac*friction*rvac*friction)?1:friction;
-				joint.m_massmatrix	=	ImpulseMatrix(	ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
-					bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
-
-				return(true);
+				norm.normalize();  //is it necessary?
+
+				const btVector3 ra = res.witnesses[0] - ba.xform().getOrigin();
+				const btVector3 rb = res.witnesses[1] - bb.xform().getOrigin();
+				const btVector3 va = ba.velocity(ra);
+				const btVector3 vb = bb.velocity(rb);
+				const btVector3 vrel = va - vb;
+				const btScalar rvac = btDot(vrel, norm);
+				btScalar depth = res.distance - m_margin;
+
+				//				printf("depth=%f\n",depth);
+				const btVector3 iv = norm * rvac;
+				const btVector3 fv = vrel - iv;
+				joint.m_bodies[0] = ba;
+				joint.m_bodies[1] = bb;
+				joint.m_refs[0] = ra * ba.xform().getBasis();
+				joint.m_refs[1] = rb * bb.xform().getBasis();
+				joint.m_rpos[0] = ra;
+				joint.m_rpos[1] = rb;
+				joint.m_cfm = 1;
+				joint.m_erp = 1;
+				joint.m_life = 0;
+				joint.m_maxlife = 0;
+				joint.m_split = 1;
+
+				joint.m_drift = depth * norm;
+
+				joint.m_normal = norm;
+				//				printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ());
+				joint.m_delete = false;
+				joint.m_friction = fv.length2() < (rvac * friction * rvac * friction) ? 1 : friction;
+				joint.m_massmatrix = ImpulseMatrix(ba.invMass(), ba.invWorldInertia(), joint.m_rpos[0],
+												   bb.invMass(), bb.invWorldInertia(), joint.m_rpos[1]);
+
+				return (true);
 			}
-			return(false);
+			return (false);
 		}
 	};
 	//
 	// CollideCL_RS
 	//
-	struct	CollideCL_RS : ClusterBase
+	struct CollideCL_RS : ClusterBase
 	{
-		btSoftBody*		psb;
-		const btCollisionObjectWrapper*	m_colObjWrap;
+		btSoftBody* psb;
+		const btCollisionObjectWrapper* m_colObjWrap;
 
-		void		Process(const btDbvtNode* leaf)
+		void Process(const btDbvtNode* leaf)
 		{
-			btSoftBody::Cluster*		cluster=(btSoftBody::Cluster*)leaf->data;
-			btSoftClusterCollisionShape	cshape(cluster);
-			
-			const btConvexShape*		rshape=(const btConvexShape*)m_colObjWrap->getCollisionShape();
+			btSoftBody::Cluster* cluster = (btSoftBody::Cluster*)leaf->data;
+			btSoftClusterCollisionShape cshape(cluster);
+
+			const btConvexShape* rshape = (const btConvexShape*)m_colObjWrap->getCollisionShape();
 
 			///don't collide an anchored cluster with a static/kinematic object
-			if(m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject() && cluster->m_containsAnchor)
+			if (m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject() && cluster->m_containsAnchor)
 				return;
 
-			btGjkEpaSolver2::sResults	res;		
-			if(btGjkEpaSolver2::SignedDistance(	&cshape,btTransform::getIdentity(),
-				rshape,m_colObjWrap->getWorldTransform(),
-				btVector3(1,0,0),res))
+			btGjkEpaSolver2::sResults res;
+			if (btGjkEpaSolver2::SignedDistance(&cshape, btTransform::getIdentity(),
+												rshape, m_colObjWrap->getWorldTransform(),
+												btVector3(1, 0, 0), res))
 			{
-				btSoftBody::CJoint	joint;
-				if(SolveContact(res,cluster,m_colObjWrap->getCollisionObject(),joint))//prb,joint))
+				btSoftBody::CJoint joint;
+				if (SolveContact(res, cluster, m_colObjWrap->getCollisionObject(), joint))  //prb,joint))
 				{
-					btSoftBody::CJoint*	pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
-					*pj=joint;psb->m_joints.push_back(pj);
-					if(m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject())
+					btSoftBody::CJoint* pj = new (btAlignedAlloc(sizeof(btSoftBody::CJoint), 16)) btSoftBody::CJoint();
+					*pj = joint;
+					psb->m_joints.push_back(pj);
+					if (m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject())
 					{
-						pj->m_erp	*=	psb->m_cfg.kSKHR_CL;
-						pj->m_split	*=	psb->m_cfg.kSK_SPLT_CL;
+						pj->m_erp *= psb->m_cfg.kSKHR_CL;
+						pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
 					}
 					else
 					{
-						pj->m_erp	*=	psb->m_cfg.kSRHR_CL;
-						pj->m_split	*=	psb->m_cfg.kSR_SPLT_CL;
+						pj->m_erp *= psb->m_cfg.kSRHR_CL;
+						pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
 					}
 				}
 			}
 		}
-		void		ProcessColObj(btSoftBody* ps,const btCollisionObjectWrapper* colObWrap)
+		void ProcessColObj(btSoftBody* ps, const btCollisionObjectWrapper* colObWrap)
 		{
-			psb			=	ps;
-			m_colObjWrap			=	colObWrap;
-			idt			=	ps->m_sst.isdt;
-			m_margin		=	m_colObjWrap->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin();
+			psb = ps;
+			m_colObjWrap = colObWrap;
+			idt = ps->m_sst.isdt;
+			m_margin = m_colObjWrap->getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin();
 			///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful.
-			friction	=	btMin(psb->m_cfg.kDF,m_colObjWrap->getCollisionObject()->getFriction());
-			btVector3			mins;
-			btVector3			maxs;
-
-			ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume;
-			colObWrap->getCollisionShape()->getAabb(colObWrap->getWorldTransform(),mins,maxs);
-			volume=btDbvtVolume::FromMM(mins,maxs);
-			volume.Expand(btVector3(1,1,1)*m_margin);
-			ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this);
-		}	
+			friction = btMin(psb->m_cfg.kDF, m_colObjWrap->getCollisionObject()->getFriction());
+			btVector3 mins;
+			btVector3 maxs;
+
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)
+			volume;
+			colObWrap->getCollisionShape()->getAabb(colObWrap->getWorldTransform(), mins, maxs);
+			volume = btDbvtVolume::FromMM(mins, maxs);
+			volume.Expand(btVector3(1, 1, 1) * m_margin);
+			ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root, volume, *this);
+		}
 	};
 	//
 	// CollideCL_SS
 	//
-	struct	CollideCL_SS : ClusterBase
+	struct CollideCL_SS : ClusterBase
 	{
-		btSoftBody*	bodies[2];
-		void		Process(const btDbvtNode* la,const btDbvtNode* lb)
+		btSoftBody* bodies[2];
+		void Process(const btDbvtNode* la, const btDbvtNode* lb)
 		{
-			btSoftBody::Cluster*		cla=(btSoftBody::Cluster*)la->data;
-			btSoftBody::Cluster*		clb=(btSoftBody::Cluster*)lb->data;
-
+			btSoftBody::Cluster* cla = (btSoftBody::Cluster*)la->data;
+			btSoftBody::Cluster* clb = (btSoftBody::Cluster*)lb->data;
 
-			bool connected=false;
-			if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size()))
+			bool connected = false;
+			if ((bodies[0] == bodies[1]) && (bodies[0]->m_clusterConnectivity.size()))
 			{
-				connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex];
+				connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex + bodies[0]->m_clusters.size() * clb->m_clusterIndex];
 			}
 
 			if (!connected)
 			{
-				btSoftClusterCollisionShape	csa(cla);
-				btSoftClusterCollisionShape	csb(clb);
-				btGjkEpaSolver2::sResults	res;		
-				if(btGjkEpaSolver2::SignedDistance(	&csa,btTransform::getIdentity(),
-					&csb,btTransform::getIdentity(),
-					cla->m_com-clb->m_com,res))
+				btSoftClusterCollisionShape csa(cla);
+				btSoftClusterCollisionShape csb(clb);
+				btGjkEpaSolver2::sResults res;
+				if (btGjkEpaSolver2::SignedDistance(&csa, btTransform::getIdentity(),
+													&csb, btTransform::getIdentity(),
+													cla->m_com - clb->m_com, res))
 				{
-					btSoftBody::CJoint	joint;
-					if(SolveContact(res,cla,clb,joint))
+					btSoftBody::CJoint joint;
+					if (SolveContact(res, cla, clb, joint))
 					{
-						btSoftBody::CJoint*	pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
-						*pj=joint;bodies[0]->m_joints.push_back(pj);
-						pj->m_erp	*=	btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
-						pj->m_split	*=	(bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
+						btSoftBody::CJoint* pj = new (btAlignedAlloc(sizeof(btSoftBody::CJoint), 16)) btSoftBody::CJoint();
+						*pj = joint;
+						bodies[0]->m_joints.push_back(pj);
+						pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL, bodies[1]->m_cfg.kSSHR_CL);
+						pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL + bodies[1]->m_cfg.kSS_SPLT_CL) / 2;
 					}
 				}
-			} else
+			}
+			else
 			{
-				static int count=0;
+				static int count = 0;
 				count++;
 				//printf("count=%d\n",count);
-				
 			}
 		}
-		void		ProcessSoftSoft(btSoftBody* psa,btSoftBody* psb)
+		void ProcessSoftSoft(btSoftBody* psa, btSoftBody* psb)
 		{
-			idt			=	psa->m_sst.isdt;
+			idt = psa->m_sst.isdt;
 			//m_margin		=	(psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
-			m_margin		=	(psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin());
-			friction	=	btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
-			bodies[0]	=	psa;
-			bodies[1]	=	psb;
-			psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
-		}	
+			m_margin = (psa->getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin());
+			friction = btMin(psa->m_cfg.kDF, psb->m_cfg.kDF);
+			bodies[0] = psa;
+			bodies[1] = psb;
+			psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this);
+		}
 	};
 	//
 	// CollideSDF_RS
 	//
-	struct	CollideSDF_RS : btDbvt::ICollide
+	struct CollideSDF_RS : btDbvt::ICollide
 	{
-		void		Process(const btDbvtNode* leaf)
+		void Process(const btDbvtNode* leaf)
 		{
-			btSoftBody::Node*	node=(btSoftBody::Node*)leaf->data;
+			btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
 			DoNode(*node);
 		}
-		void		DoNode(btSoftBody::Node& n) const
+		void DoNode(btSoftBody::Node& n) const
 		{
-			const btScalar			m=n.m_im>0?dynmargin:stamargin;
-			btSoftBody::RContact	c;
+			const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
+			btSoftBody::RContact c;
 
-			if(	(!n.m_battach)&&
-				psb->checkContact(m_colObj1Wrap,n.m_x,m,c.m_cti))
+			if ((!n.m_battach) &&
+				psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
 			{
-				const btScalar	ima=n.m_im;
-				const btScalar	imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f;
-				const btScalar	ms=ima+imb;
-				if(ms>0)
+				const btScalar ima = n.m_im;
+				const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
+				const btScalar ms = ima + imb;
+				if (ms > 0)
 				{
-					const btTransform&	wtr=m_rigidBody?m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
-					static const btMatrix3x3	iwiStatic(0,0,0,0,0,0,0,0,0);
-					const btMatrix3x3&	iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
-					const btVector3		ra=n.m_x-wtr.getOrigin();
-					const btVector3		va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0);
-					const btVector3		vb=n.m_x-n.m_q;	
-					const btVector3		vr=vb-va;
-					const btScalar		dn=btDot(vr,c.m_cti.m_normal);
-					const btVector3		fv=vr-c.m_cti.m_normal*dn;
-					const btScalar		fc=psb->m_cfg.kDF*m_colObj1Wrap->getCollisionObject()->getFriction();
-					c.m_node	=	&n;
-					c.m_c0		=	ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
-					c.m_c1		=	ra;
-					c.m_c2		=	ima*psb->m_sst.sdt;
-			        c.m_c3		=	fv.length2()<(dn*fc*dn*fc)?0:1-fc;
-					c.m_c4		=	m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
+					const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
+					static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+					const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+					const btVector3 ra = n.m_x - wtr.getOrigin();
+					const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
+					const btVector3 vb = n.m_x - n.m_q;
+					const btVector3 vr = vb - va;
+					const btScalar dn = btDot(vr, c.m_cti.m_normal);
+					const btVector3 fv = vr - c.m_cti.m_normal * dn;
+					const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
+					c.m_node = &n;
+					c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
+					c.m_c1 = ra;
+					c.m_c2 = ima * psb->m_sst.sdt;
+					c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
+					c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
 					psb->m_rcontacts.push_back(c);
 					if (m_rigidBody)
 						m_rigidBody->activate();
 				}
 			}
 		}
-		btSoftBody*		psb;
-		const btCollisionObjectWrapper*	m_colObj1Wrap;
-		btRigidBody*	m_rigidBody;
-		btScalar		dynmargin;
-		btScalar		stamargin;
+		btSoftBody* psb;
+		const btCollisionObjectWrapper* m_colObj1Wrap;
+		btRigidBody* m_rigidBody;
+		btScalar dynmargin;
+		btScalar stamargin;
+	};
+
+	//
+	// CollideSDF_RD
+	//
+	struct CollideSDF_RD : btDbvt::ICollide
+	{
+		void Process(const btDbvtNode* leaf)
+		{
+			btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
+			DoNode(*node);
+		}
+		void DoNode(btSoftBody::Node& n) const
+		{
+			const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
+			btSoftBody::DeformableNodeRigidContact c;
+
+			if (!n.m_battach)
+			{
+				// check for collision at x_{n+1}^*
+				if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
+				{
+					const btScalar ima = n.m_im;
+					// todo: collision between multibody and fixed deformable node will be missed.
+					const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
+					const btScalar ms = ima + imb;
+					if (ms > 0)
+					{
+						// resolve contact at x_n
+						psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false);
+						btSoftBody::sCti& cti = c.m_cti;
+						c.m_node = &n;
+						const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
+						c.m_c2 = ima;
+						c.m_c3 = fc;
+						c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
+						c.m_c5 = n.m_effectiveMass_inv;
+
+						if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+						{
+							const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
+							static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+							const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+							const btVector3 ra = n.m_x - wtr.getOrigin();
+
+							c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
+							//                            c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
+							c.m_c1 = ra;
+						}
+						else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+						{
+							btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+							if (multibodyLinkCol)
+							{
+								btVector3 normal = cti.m_normal;
+								btVector3 t1 = generateUnitOrthogonalVector(normal);
+								btVector3 t2 = btCross(normal, t1);
+								btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+								findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal);
+								findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1);
+								findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2);
+
+								btScalar* J_n = &jacobianData_normal.m_jacobians[0];
+								btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
+								btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
+
+								btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+								btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+								btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+
+								btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
+												t1.getX(), t1.getY(), t1.getZ(),
+												t2.getX(), t2.getY(), t2.getZ());  // world frame to local frame
+								const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+								btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+								c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
+								c.jacobianData_normal = jacobianData_normal;
+								c.jacobianData_t1 = jacobianData_t1;
+								c.jacobianData_t2 = jacobianData_t2;
+								c.t1 = t1;
+								c.t2 = t2;
+							}
+						}
+						psb->m_nodeRigidContacts.push_back(c);
+					}
+				}
+			}
+		}
+		btSoftBody* psb;
+		const btCollisionObjectWrapper* m_colObj1Wrap;
+		btRigidBody* m_rigidBody;
+		btScalar dynmargin;
+		btScalar stamargin;
+	};
+
+	//
+	// CollideSDF_RDF
+	//
+	struct CollideSDF_RDF : btDbvt::ICollide
+	{
+		void Process(const btDbvtNode* leaf)
+		{
+			btSoftBody::Face* face = (btSoftBody::Face*)leaf->data;
+			DoNode(*face);
+		}
+		void DoNode(btSoftBody::Face& f) const
+		{
+			btSoftBody::Node* n0 = f.m_n[0];
+			btSoftBody::Node* n1 = f.m_n[1];
+			btSoftBody::Node* n2 = f.m_n[2];
+			const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0) ? dynmargin : stamargin;
+			btSoftBody::DeformableFaceRigidContact c;
+			btVector3 contact_point;
+			btVector3 bary;
+			if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
+			{
+				btScalar ima = n0->m_im + n1->m_im + n2->m_im;
+				const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
+				// todo: collision between multibody and fixed deformable face will be missed.
+				const btScalar ms = ima + imb;
+				if (ms > 0)
+				{
+					// resolve contact at x_n
+					//                    psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false);
+					btSoftBody::sCti& cti = c.m_cti;
+					c.m_contactPoint = contact_point;
+					c.m_bary = bary;
+					// todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
+					c.m_weights = btScalar(2) / (btScalar(1) + bary.length2()) * bary;
+					c.m_face = &f;
+					// friction is handled by the nodes to prevent sticking
+					//                    const btScalar fc = 0;
+					const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
+
+					// the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
+					ima = bary.getX() * c.m_weights.getX() * n0->m_im + bary.getY() * c.m_weights.getY() * n1->m_im + bary.getZ() * c.m_weights.getZ() * n2->m_im;
+					c.m_c2 = ima;
+					c.m_c3 = fc;
+					c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
+					c.m_c5 = Diagonal(ima);
+					if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+					{
+						const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
+						static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+						const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+						const btVector3 ra = contact_point - wtr.getOrigin();
+
+						// we do not scale the impulse matrix by dt
+						c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
+						c.m_c1 = ra;
+					}
+					else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+					{
+						btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+						if (multibodyLinkCol)
+						{
+							btVector3 normal = cti.m_normal;
+							btVector3 t1 = generateUnitOrthogonalVector(normal);
+							btVector3 t2 = btCross(normal, t1);
+							btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+							findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal);
+							findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1);
+							findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2);
+
+							btScalar* J_n = &jacobianData_normal.m_jacobians[0];
+							btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
+							btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
+
+							btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+							btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+							btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+
+							btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
+											t1.getX(), t1.getY(), t1.getZ(),
+											t2.getX(), t2.getY(), t2.getZ());  // world frame to local frame
+							const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+							btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+							c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
+							c.jacobianData_normal = jacobianData_normal;
+							c.jacobianData_t1 = jacobianData_t1;
+							c.jacobianData_t2 = jacobianData_t2;
+							c.t1 = t1;
+							c.t2 = t2;
+						}
+					}
+					psb->m_faceRigidContacts.push_back(c);
+				}
+			}
+			// Set caching barycenters to be false after collision detection.
+			// Only turn on when contact is static.
+			f.m_pcontact[3] = 0;
+		}
+		btSoftBody* psb;
+		const btCollisionObjectWrapper* m_colObj1Wrap;
+		btRigidBody* m_rigidBody;
+		btScalar dynmargin;
+		btScalar stamargin;
 	};
+
 	//
 	// CollideVF_SS
 	//
-	struct	CollideVF_SS : btDbvt::ICollide
-	{
-		void		Process(const btDbvtNode* lnode,
-			const btDbvtNode* lface)
-		{
-			btSoftBody::Node*	node=(btSoftBody::Node*)lnode->data;
-			btSoftBody::Face*	face=(btSoftBody::Face*)lface->data;
-			btVector3			o=node->m_x;
-			btVector3			p;
-			btScalar			d=SIMD_INFINITY;
-			ProjectOrigin(	face->m_n[0]->m_x-o,
-				face->m_n[1]->m_x-o,
-				face->m_n[2]->m_x-o,
-				p,d);
-			const btScalar	m=mrg+(o-node->m_q).length()*2;
-			if(d<(m*m))
+	struct CollideVF_SS : btDbvt::ICollide
+	{
+		void Process(const btDbvtNode* lnode,
+					 const btDbvtNode* lface)
+		{
+			btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
+			btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
+			for (int i = 0; i < 3; ++i)
 			{
-				const btSoftBody::Node*	n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
-				const btVector3			w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
-				const btScalar			ma=node->m_im;
-				btScalar				mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
-				if(	(n[0]->m_im<=0)||
-					(n[1]->m_im<=0)||
-					(n[2]->m_im<=0))
+				if (face->m_n[i] == node)
+					continue;
+			}
+
+			btVector3 o = node->m_x;
+			btVector3 p;
+			btScalar d = SIMD_INFINITY;
+			ProjectOrigin(face->m_n[0]->m_x - o,
+						  face->m_n[1]->m_x - o,
+						  face->m_n[2]->m_x - o,
+						  p, d);
+			const btScalar m = mrg + (o - node->m_q).length() * 2;
+			if (d < (m * m))
+			{
+				const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
+				const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
+				const btScalar ma = node->m_im;
+				btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
+				if ((n[0]->m_im <= 0) ||
+					(n[1]->m_im <= 0) ||
+					(n[2]->m_im <= 0))
 				{
-					mb=0;
+					mb = 0;
 				}
-				const btScalar	ms=ma+mb;
-				if(ms>0)
+				const btScalar ms = ma + mb;
+				if (ms > 0)
 				{
-					btSoftBody::SContact	c;
-					c.m_normal		=	p/-btSqrt(d);
-					c.m_margin		=	m;
-					c.m_node		=	node;
-					c.m_face		=	face;
-					c.m_weights		=	w;
-					c.m_friction	=	btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
-					c.m_cfm[0]		=	ma/ms*psb[0]->m_cfg.kSHR;
-					c.m_cfm[1]		=	mb/ms*psb[1]->m_cfg.kSHR;
+					btSoftBody::SContact c;
+					c.m_normal = p / -btSqrt(d);
+					c.m_margin = m;
+					c.m_node = node;
+					c.m_face = face;
+					c.m_weights = w;
+					c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF);
+					c.m_cfm[0] = ma / ms * psb[0]->m_cfg.kSHR;
+					c.m_cfm[1] = mb / ms * psb[1]->m_cfg.kSHR;
 					psb[0]->m_scontacts.push_back(c);
 				}
-			}	
+			}
 		}
-		btSoftBody*		psb[2];
-		btScalar		mrg;
+		btSoftBody* psb[2];
+		btScalar mrg;
 	};
-};
 
-#endif //_BT_SOFT_BODY_INTERNALS_H
+	//
+	// CollideVF_DD
+	//
+	struct CollideVF_DD : btDbvt::ICollide
+	{
+		void Process(const btDbvtNode* lnode,
+					 const btDbvtNode* lface)
+		{
+			btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
+			btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
+			btVector3 bary;
+			if (proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary))
+			{
+				const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
+				const btVector3 w = bary;
+				const btScalar ma = node->m_im;
+				btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
+				if ((n[0]->m_im <= 0) ||
+					(n[1]->m_im <= 0) ||
+					(n[2]->m_im <= 0))
+				{
+					mb = 0;
+				}
+				const btScalar ms = ma + mb;
+				if (ms > 0)
+				{
+					btSoftBody::DeformableFaceNodeContact c;
+					c.m_normal = face->m_normal;
+					if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
+						c.m_normal = -face->m_normal;
+					c.m_margin = mrg;
+					c.m_node = node;
+					c.m_face = face;
+					c.m_bary = w;
+					c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
+					psb[0]->m_faceNodeContacts.push_back(c);
+				}
+			}
+		}
+		btSoftBody* psb[2];
+		btScalar mrg;
+		bool useFaceNormal;
+	};
+
+	//
+	// CollideFF_DD
+	//
+	struct CollideFF_DD : btDbvt::ICollide
+	{
+		void Process(const btDbvntNode* lface1,
+					 const btDbvntNode* lface2)
+		{
+			btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
+			btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
+			if (f1 != f2)
+			{
+				Repel(f1, f2);
+				Repel(f2, f1);
+			}
+		}
+		void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2)
+		{
+			//#define REPEL_NEIGHBOR 1
+#ifndef REPEL_NEIGHBOR
+			for (int node_id = 0; node_id < 3; ++node_id)
+			{
+				btSoftBody::Node* node = f1->m_n[node_id];
+				for (int i = 0; i < 3; ++i)
+				{
+					if (f2->m_n[i] == node)
+						return;
+				}
+			}
+#endif
+			bool skip = false;
+			for (int node_id = 0; node_id < 3; ++node_id)
+			{
+				btSoftBody::Node* node = f1->m_n[node_id];
+#ifdef REPEL_NEIGHBOR
+				for (int i = 0; i < 3; ++i)
+				{
+					if (f2->m_n[i] == node)
+					{
+						skip = true;
+						break;
+					}
+				}
+				if (skip)
+				{
+					skip = false;
+					continue;
+				}
+#endif
+				btSoftBody::Face* face = f2;
+				btVector3 bary;
+				if (!proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary))
+					continue;
+				btSoftBody::DeformableFaceNodeContact c;
+				c.m_normal = face->m_normal;
+				if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
+					c.m_normal = -face->m_normal;
+				c.m_margin = mrg;
+				c.m_node = node;
+				c.m_face = face;
+				c.m_bary = bary;
+				c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
+				psb[0]->m_faceNodeContacts.push_back(c);
+			}
+		}
+		btSoftBody* psb[2];
+		btScalar mrg;
+		bool useFaceNormal;
+	};
+
+	struct CollideCCD : btDbvt::ICollide
+	{
+		void Process(const btDbvtNode* lnode,
+					 const btDbvtNode* lface)
+		{
+			btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
+			btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
+			btVector3 bary;
+			if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary))
+			{
+				btSoftBody::DeformableFaceNodeContact c;
+				c.m_normal = face->m_normal;
+				if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
+					c.m_normal = -face->m_normal;
+				c.m_node = node;
+				c.m_face = face;
+				c.m_bary = bary;
+				c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
+				psb[0]->m_faceNodeContacts.push_back(c);
+			}
+		}
+		void Process(const btDbvntNode* lface1,
+					 const btDbvntNode* lface2)
+		{
+			btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
+			btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
+			if (f1 != f2)
+			{
+				Repel(f1, f2);
+				Repel(f2, f1);
+			}
+		}
+		void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2)
+		{
+			//#define REPEL_NEIGHBOR 1
+#ifndef REPEL_NEIGHBOR
+			for (int node_id = 0; node_id < 3; ++node_id)
+			{
+				btSoftBody::Node* node = f1->m_n[node_id];
+				for (int i = 0; i < 3; ++i)
+				{
+					if (f2->m_n[i] == node)
+						return;
+				}
+			}
+#endif
+			bool skip = false;
+			for (int node_id = 0; node_id < 3; ++node_id)
+			{
+				btSoftBody::Node* node = f1->m_n[node_id];
+#ifdef REPEL_NEIGHBOR
+				for (int i = 0; i < 3; ++i)
+				{
+					if (f2->m_n[i] == node)
+					{
+						skip = true;
+						break;
+					}
+				}
+				if (skip)
+				{
+					skip = false;
+					continue;
+				}
+#endif
+				btSoftBody::Face* face = f2;
+				btVector3 bary;
+				if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary))
+				{
+					btSoftBody::DeformableFaceNodeContact c;
+					c.m_normal = face->m_normal;
+					if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
+						c.m_normal = -face->m_normal;
+					c.m_node = node;
+					c.m_face = face;
+					c.m_bary = bary;
+					c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
+					psb[0]->m_faceNodeContacts.push_back(c);
+				}
+			}
+		}
+		btSoftBody* psb[2];
+		btScalar dt, mrg;
+		bool useFaceNormal;
+	};
+};
+#endif  //_BT_SOFT_BODY_INTERNALS_H
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp
index f5a67f6d895..3127369ccd5 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp
+++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp
@@ -23,27 +23,27 @@ subject to the following restrictions:
 #define ENABLE_SOFTBODY_CONCAVE_COLLISIONS 1
 
 btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo)
-:btDefaultCollisionConfiguration(constructionInfo)
+	: btDefaultCollisionConfiguration(constructionInfo)
 {
 	void* mem;
 
-	mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16);
-	m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc), 16);
+	m_softSoftCreateFunc = new (mem) btSoftSoftCollisionAlgorithm::CreateFunc;
 
-	mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16);
-	m_softRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc), 16);
+	m_softRigidConvexCreateFunc = new (mem) btSoftRigidCollisionAlgorithm::CreateFunc;
 
-	mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16);
-	m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc;
-	m_swappedSoftRigidConvexCreateFunc->m_swapped=true;
+	mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc), 16);
+	m_swappedSoftRigidConvexCreateFunc = new (mem) btSoftRigidCollisionAlgorithm::CreateFunc;
+	m_swappedSoftRigidConvexCreateFunc->m_swapped = true;
 
 #ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS
-	mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16);
-	m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc), 16);
+	m_softRigidConcaveCreateFunc = new (mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc;
 
-	mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16);
-	m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc;
-	m_swappedSoftRigidConcaveCreateFunc->m_swapped=true;
+	mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc), 16);
+	m_swappedSoftRigidConcaveCreateFunc = new (mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc;
+	m_swappedSoftRigidConcaveCreateFunc->m_swapped = true;
 #endif
 
 	//replace pool by a new one, with potential larger size
@@ -53,82 +53,79 @@ btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfigura
 		int curElemSize = m_collisionAlgorithmPool->getElementSize();
 		///calculate maximum element size, big enough to fit any collision algorithm in the memory pool
 
-
 		int maxSize0 = sizeof(btSoftSoftCollisionAlgorithm);
 		int maxSize1 = sizeof(btSoftRigidCollisionAlgorithm);
 		int maxSize2 = sizeof(btSoftBodyConcaveCollisionAlgorithm);
 
-		int	collisionAlgorithmMaxElementSize = btMax(maxSize0,maxSize1);
-		collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
-		
+		int collisionAlgorithmMaxElementSize = btMax(maxSize0, maxSize1);
+		collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize2);
+
 		if (collisionAlgorithmMaxElementSize > curElemSize)
 		{
 			m_collisionAlgorithmPool->~btPoolAllocator();
 			btAlignedFree(m_collisionAlgorithmPool);
-			void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
-			m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize);
+			void* mem = btAlignedAlloc(sizeof(btPoolAllocator), 16);
+			m_collisionAlgorithmPool = new (mem) btPoolAllocator(collisionAlgorithmMaxElementSize, constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize);
 		}
 	}
-
 }
 
 btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfiguration()
 {
 	m_softSoftCreateFunc->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree(	m_softSoftCreateFunc);
+	btAlignedFree(m_softSoftCreateFunc);
 
 	m_softRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree(	m_softRigidConvexCreateFunc);
+	btAlignedFree(m_softRigidConvexCreateFunc);
 
 	m_swappedSoftRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree(	m_swappedSoftRigidConvexCreateFunc);
+	btAlignedFree(m_swappedSoftRigidConvexCreateFunc);
 
 #ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS
 	m_softRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree(	m_softRigidConcaveCreateFunc);
+	btAlignedFree(m_softRigidConcaveCreateFunc);
 
 	m_swappedSoftRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree(	m_swappedSoftRigidConcaveCreateFunc);
+	btAlignedFree(m_swappedSoftRigidConcaveCreateFunc);
 #endif
 }
 
 ///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation
-btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)
+btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1)
 {
-
 	///try to handle the softbody interactions first
 
-	if ((proxyType0 == SOFTBODY_SHAPE_PROXYTYPE  ) && (proxyType1==SOFTBODY_SHAPE_PROXYTYPE))
+	if ((proxyType0 == SOFTBODY_SHAPE_PROXYTYPE) && (proxyType1 == SOFTBODY_SHAPE_PROXYTYPE))
 	{
-		return	m_softSoftCreateFunc;
+		return m_softSoftCreateFunc;
 	}
 
 	///softbody versus convex
-	if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE  && btBroadphaseProxy::isConvex(proxyType1))
+	if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConvex(proxyType1))
 	{
-		return	m_softRigidConvexCreateFunc;
+		return m_softRigidConvexCreateFunc;
 	}
 
 	///convex versus soft body
-	if (btBroadphaseProxy::isConvex(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE )
+	if (btBroadphaseProxy::isConvex(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE)
 	{
-		return	m_swappedSoftRigidConvexCreateFunc;
+		return m_swappedSoftRigidConvexCreateFunc;
 	}
 
 #ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS
 	///softbody versus convex
-	if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE  && btBroadphaseProxy::isConcave(proxyType1))
+	if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConcave(proxyType1))
 	{
-		return	m_softRigidConcaveCreateFunc;
+		return m_softRigidConcaveCreateFunc;
 	}
 
 	///convex versus soft body
-	if (btBroadphaseProxy::isConcave(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE )
+	if (btBroadphaseProxy::isConcave(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE)
 	{
-		return	m_swappedSoftRigidConcaveCreateFunc;
+		return m_swappedSoftRigidConcaveCreateFunc;
 	}
 #endif
 
 	///fallback to the regular rigid collision shape
-	return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0,proxyType1);
+	return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0, proxyType1);
 }
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h
index 21addcfe2e1..0396a52dac7 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h
+++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h
@@ -21,28 +21,23 @@ subject to the following restrictions:
 class btVoronoiSimplexSolver;
 class btGjkEpaPenetrationDepthSolver;
 
-
 ///btSoftBodyRigidBodyCollisionConfiguration add softbody interaction on top of btDefaultCollisionConfiguration
-class	btSoftBodyRigidBodyCollisionConfiguration : public btDefaultCollisionConfiguration
+class btSoftBodyRigidBodyCollisionConfiguration : public btDefaultCollisionConfiguration
 {
-
 	//default CreationFunctions, filling the m_doubleDispatch table
-	btCollisionAlgorithmCreateFunc*	m_softSoftCreateFunc;
-	btCollisionAlgorithmCreateFunc*	m_softRigidConvexCreateFunc;
-	btCollisionAlgorithmCreateFunc*	m_swappedSoftRigidConvexCreateFunc;
-	btCollisionAlgorithmCreateFunc*	m_softRigidConcaveCreateFunc;
-	btCollisionAlgorithmCreateFunc*	m_swappedSoftRigidConcaveCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_softSoftCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_softRigidConvexCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConvexCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_softRigidConcaveCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConcaveCreateFunc;
 
 public:
-
 	btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo());
 
 	virtual ~btSoftBodyRigidBodyCollisionConfiguration();
 
 	///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation
-	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
-
+	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1);
 };
 
-#endif //BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION
-
+#endif  //BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h b/extern/bullet2/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h
index c4733d64000..bc538db4a25 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h
+++ b/extern/bullet2/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h
@@ -16,7 +16,6 @@ subject to the following restrictions:
 #ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H
 #define BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H
 
-
 class btVertexBufferDescriptor
 {
 public:
@@ -27,8 +26,7 @@ public:
 		OPENGL_BUFFER
 	};
 
-protected:	
-
+protected:
 	bool m_hasVertexPositions;
 	bool m_hasNormals;
 
@@ -51,7 +49,6 @@ public:
 
 	virtual ~btVertexBufferDescriptor()
 	{
-
 	}
 
 	virtual bool hasVertexPositions() const
@@ -102,7 +99,6 @@ public:
 	}
 };
 
-
 class btCPUVertexBufferDescriptor : public btVertexBufferDescriptor
 {
 protected:
@@ -114,7 +110,7 @@ public:
 	 * vertexOffset is the offset in floats to the first vertex.
 	 * vertexStride is the stride in floats between vertices.
 	 */
-	btCPUVertexBufferDescriptor( float *basePointer, int vertexOffset, int vertexStride )
+	btCPUVertexBufferDescriptor(float *basePointer, int vertexOffset, int vertexStride)
 	{
 		m_basePointer = basePointer;
 		m_vertexOffset = vertexOffset;
@@ -127,7 +123,7 @@ public:
 	 * vertexOffset is the offset in floats to the first vertex.
 	 * vertexStride is the stride in floats between vertices.
 	 */
-	btCPUVertexBufferDescriptor( float *basePointer, int vertexOffset, int vertexStride, int normalOffset, int normalStride )
+	btCPUVertexBufferDescriptor(float *basePointer, int vertexOffset, int vertexStride, int normalOffset, int normalStride)
 	{
 		m_basePointer = basePointer;
 
@@ -142,7 +138,6 @@ public:
 
 	virtual ~btCPUVertexBufferDescriptor()
 	{
-
 	}
 
 	/**
@@ -162,4 +157,4 @@ public:
 	}
 };
 
-#endif // #ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H
+#endif  // #ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h b/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h
index 6947bc27d28..dbb2624eee0 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h
+++ b/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h
@@ -18,7 +18,6 @@ subject to the following restrictions:
 
 #include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
 
-
 class btSoftBodyTriangleData;
 class btSoftBodyLinkData;
 class btSoftBodyVertexData;
@@ -26,7 +25,6 @@ class btVertexBufferDescriptor;
 class btCollisionObject;
 class btSoftBody;
 
-
 class btSoftBodySolver
 {
 public:
@@ -37,20 +35,19 @@ public:
 		CL_SOLVER,
 		CL_SIMD_SOLVER,
 		DX_SOLVER,
-		DX_SIMD_SOLVER
+		DX_SIMD_SOLVER,
+		DEFORMABLE_SOLVER
 	};
 
-
 protected:
 	int m_numberOfPositionIterations;
 	int m_numberOfVelocityIterations;
 	// Simulation timescale
 	float m_timeScale;
-	
+
 public:
-	btSoftBodySolver() :
-		m_numberOfPositionIterations( 10 ),
-		m_timeScale( 1 )
+	btSoftBodySolver() : m_numberOfPositionIterations(10),
+						 m_timeScale(1)
 	{
 		m_numberOfVelocityIterations = 0;
 		m_numberOfPositionIterations = 5;
@@ -59,39 +56,38 @@ public:
 	virtual ~btSoftBodySolver()
 	{
 	}
-	
+
 	/**
 	 * Return the type of the solver.
 	 */
 	virtual SolverTypes getSolverType() const = 0;
 
-
 	/** Ensure that this solver is initialized. */
 	virtual bool checkInitialized() = 0;
 
 	/** Optimize soft bodies in this solver. */
-	virtual void optimize( btAlignedObjectArray< btSoftBody * > &softBodies , bool forceUpdate=false) = 0;
+	virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false) = 0;
 
 	/** Copy necessary data back to the original soft body source objects. */
 	virtual void copyBackToSoftBodies(bool bMove = true) = 0;
 
 	/** Predict motion of soft bodies into next timestep */
-	virtual void predictMotion( float solverdt ) = 0;
+	virtual void predictMotion(btScalar solverdt) = 0;
 
 	/** Solve constraints for a set of soft bodies */
-	virtual void solveConstraints( float solverdt ) = 0;
+	virtual void solveConstraints(btScalar solverdt) = 0;
 
 	/** Perform necessary per-step updates of soft bodies such as recomputing normals and bounding boxes */
 	virtual void updateSoftBodies() = 0;
 
 	/** Process a collision between one of the world's soft bodies and another collision object */
-	virtual void processCollision( btSoftBody *, const struct btCollisionObjectWrapper* ) = 0;
+	virtual void processCollision(btSoftBody *, const struct btCollisionObjectWrapper *) = 0;
 
 	/** Process a collision between two soft bodies */
-	virtual void processCollision( btSoftBody*, btSoftBody* ) = 0;
+	virtual void processCollision(btSoftBody *, btSoftBody *) = 0;
 
 	/** Set the number of velocity constraint solver iterations this solver uses. */
-	virtual void setNumberOfPositionIterations( int iterations )
+	virtual void setNumberOfPositionIterations(int iterations)
 	{
 		m_numberOfPositionIterations = iterations;
 	}
@@ -103,7 +99,7 @@ public:
 	}
 
 	/** Set the number of velocity constraint solver iterations this solver uses. */
-	virtual void setNumberOfVelocityIterations( int iterations )
+	virtual void setNumberOfVelocityIterations(int iterations)
 	{
 		m_numberOfVelocityIterations = iterations;
 	}
@@ -135,7 +131,6 @@ public:
 class btSoftBodySolverOutput
 {
 protected:
-
 public:
 	btSoftBodySolverOutput()
 	{
@@ -145,10 +140,8 @@ public:
 	{
 	}
 
-
 	/** Output current computed vertex data to the vertex buffers for all cloths in the solver. */
-	virtual void copySoftBodyToVertexBuffer( const btSoftBody * const softBody, btVertexBufferDescriptor *vertexBuffer ) = 0;
+	virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) = 0;
 };
 
-
-#endif // #ifndef BT_SOFT_BODY_SOLVERS_H
+#endif  // #ifndef BT_SOFT_BODY_SOLVERS_H
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp b/extern/bullet2/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp
new file mode 100644
index 00000000000..282dbf75f08
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp
@@ -0,0 +1,350 @@
+/*
+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 "btSoftMultiBodyDynamicsWorld.h"
+#include "LinearMath/btQuickprof.h"
+
+//softbody & helpers
+#include "BulletSoftBody/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btSoftBodySolvers.h"
+#include "BulletSoftBody/btDefaultSoftBodySolver.h"
+#include "LinearMath/btSerializer.h"
+
+btSoftMultiBodyDynamicsWorld::btSoftMultiBodyDynamicsWorld(
+	btDispatcher* dispatcher,
+	btBroadphaseInterface* pairCache,
+	btMultiBodyConstraintSolver* constraintSolver,
+	btCollisionConfiguration* collisionConfiguration,
+	btSoftBodySolver* softBodySolver) : btMultiBodyDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
+										m_softBodySolver(softBodySolver),
+										m_ownsSolver(false)
+{
+	if (!m_softBodySolver)
+	{
+		void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver), 16);
+		m_softBodySolver = new (ptr) btDefaultSoftBodySolver();
+		m_ownsSolver = true;
+	}
+
+	m_drawFlags = fDrawFlags::Std;
+	m_drawNodeTree = true;
+	m_drawFaceTree = false;
+	m_drawClusterTree = false;
+	m_sbi.m_broadphase = pairCache;
+	m_sbi.m_dispatcher = dispatcher;
+	m_sbi.m_sparsesdf.Initialize();
+	m_sbi.m_sparsesdf.Reset();
+
+	m_sbi.air_density = (btScalar)1.2;
+	m_sbi.water_density = 0;
+	m_sbi.water_offset = 0;
+	m_sbi.water_normal = btVector3(0, 0, 0);
+	m_sbi.m_gravity.setValue(0, -10, 0);
+
+	m_sbi.m_sparsesdf.Initialize();
+}
+
+btSoftMultiBodyDynamicsWorld::~btSoftMultiBodyDynamicsWorld()
+{
+	if (m_ownsSolver)
+	{
+		m_softBodySolver->~btSoftBodySolver();
+		btAlignedFree(m_softBodySolver);
+	}
+}
+
+void btSoftMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+	btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
+	{
+		BT_PROFILE("predictUnconstraintMotionSoftBody");
+		m_softBodySolver->predictMotion(float(timeStep));
+	}
+}
+
+void btSoftMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
+{
+	// Let the solver grab the soft bodies and if necessary optimize for it
+	m_softBodySolver->optimize(getSoftBodyArray());
+
+	if (!m_softBodySolver->checkInitialized())
+	{
+		btAssert("Solver initialization failed\n");
+	}
+
+	btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep);
+
+	///solve soft bodies constraints
+	solveSoftBodiesConstraints(timeStep);
+
+	//self collisions
+	for (int i = 0; i < m_softBodies.size(); i++)
+	{
+		btSoftBody* psb = (btSoftBody*)m_softBodies[i];
+		psb->defaultCollisionHandler(psb);
+	}
+
+	///update soft bodies
+	m_softBodySolver->updateSoftBodies();
+
+	// End solver-wise simulation step
+	// ///////////////////////////////
+}
+
+void btSoftMultiBodyDynamicsWorld::solveSoftBodiesConstraints(btScalar timeStep)
+{
+	BT_PROFILE("solveSoftConstraints");
+
+	if (m_softBodies.size())
+	{
+		btSoftBody::solveClusters(m_softBodies);
+	}
+
+	// Solve constraints solver-wise
+	m_softBodySolver->solveConstraints(timeStep * m_softBodySolver->getTimeScale());
+}
+
+void btSoftMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask)
+{
+	m_softBodies.push_back(body);
+
+	// Set the soft body solver that will deal with this body
+	// to be the world's solver
+	body->setSoftBodySolver(m_softBodySolver);
+
+	btCollisionWorld::addCollisionObject(body,
+										 collisionFilterGroup,
+										 collisionFilterMask);
+}
+
+void btSoftMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
+{
+	m_softBodies.remove(body);
+
+	btCollisionWorld::removeCollisionObject(body);
+}
+
+void btSoftMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+	btSoftBody* body = btSoftBody::upcast(collisionObject);
+	if (body)
+		removeSoftBody(body);
+	else
+		btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
+}
+
+void btSoftMultiBodyDynamicsWorld::debugDrawWorld()
+{
+	btMultiBodyDynamicsWorld::debugDrawWorld();
+
+	if (getDebugDrawer())
+	{
+		int i;
+		for (i = 0; i < this->m_softBodies.size(); i++)
+		{
+			btSoftBody* psb = (btSoftBody*)this->m_softBodies[i];
+			if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
+			{
+				btSoftBodyHelpers::DrawFrame(psb, m_debugDrawer);
+				btSoftBodyHelpers::Draw(psb, m_debugDrawer, m_drawFlags);
+			}
+
+			if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+			{
+				if (m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb, m_debugDrawer);
+				if (m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb, m_debugDrawer);
+				if (m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb, m_debugDrawer);
+			}
+		}
+	}
+}
+
+struct btSoftSingleRayCallback : public btBroadphaseRayCallback
+{
+	btVector3 m_rayFromWorld;
+	btVector3 m_rayToWorld;
+	btTransform m_rayFromTrans;
+	btTransform m_rayToTrans;
+	btVector3 m_hitNormal;
+
+	const btSoftMultiBodyDynamicsWorld* m_world;
+	btCollisionWorld::RayResultCallback& m_resultCallback;
+
+	btSoftSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btSoftMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
+		: m_rayFromWorld(rayFromWorld),
+		  m_rayToWorld(rayToWorld),
+		  m_world(world),
+		  m_resultCallback(resultCallback)
+	{
+		m_rayFromTrans.setIdentity();
+		m_rayFromTrans.setOrigin(m_rayFromWorld);
+		m_rayToTrans.setIdentity();
+		m_rayToTrans.setOrigin(m_rayToWorld);
+
+		btVector3 rayDir = (rayToWorld - rayFromWorld);
+
+		rayDir.normalize();
+		///what about division by zero? --> just set rayDirection[i] to INF/1e30
+		m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
+		m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
+		m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
+		m_signs[0] = m_rayDirectionInverse[0] < 0.0;
+		m_signs[1] = m_rayDirectionInverse[1] < 0.0;
+		m_signs[2] = m_rayDirectionInverse[2] < 0.0;
+
+		m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
+	}
+
+	virtual bool process(const btBroadphaseProxy* proxy)
+	{
+		///terminate further ray tests, once the closestHitFraction reached zero
+		if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
+			return false;
+
+		btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
+
+		//only perform raycast if filterMask matches
+		if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
+		{
+			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+			//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+#if 0
+#ifdef RECALCULATE_AABB
+			btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+#else
+			//getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
+			const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
+			const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
+#endif
+#endif
+			//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
+			//culling already done by broadphase
+			//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
+			{
+				m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
+									   collisionObject,
+									   collisionObject->getCollisionShape(),
+									   collisionObject->getWorldTransform(),
+									   m_resultCallback);
+			}
+		}
+		return true;
+	}
+};
+
+void btSoftMultiBodyDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
+{
+	BT_PROFILE("rayTest");
+	/// use the broadphase to accelerate the search for objects, based on their aabb
+	/// and for each object with ray-aabb overlap, perform an exact ray test
+	btSoftSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback);
+
+#ifndef USE_BRUTEFORCE_RAYBROADPHASE
+	m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
+#else
+	for (int i = 0; i < this->getNumCollisionObjects(); i++)
+	{
+		rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
+	}
+#endif  //USE_BRUTEFORCE_RAYBROADPHASE
+}
+
+void btSoftMultiBodyDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
+												 btCollisionObject* collisionObject,
+												 const btCollisionShape* collisionShape,
+												 const btTransform& colObjWorldTransform,
+												 RayResultCallback& resultCallback)
+{
+	if (collisionShape->isSoftBody())
+	{
+		btSoftBody* softBody = btSoftBody::upcast(collisionObject);
+		if (softBody)
+		{
+			btSoftBody::sRayCast softResult;
+			if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
+			{
+				if (softResult.fraction <= resultCallback.m_closestHitFraction)
+				{
+					btCollisionWorld::LocalShapeInfo shapeInfo;
+					shapeInfo.m_shapePart = 0;
+					shapeInfo.m_triangleIndex = softResult.index;
+					// get the normal
+					btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
+					btVector3 normal = -rayDir;
+					normal.normalize();
+
+					if (softResult.feature == btSoftBody::eFeature::Face)
+					{
+						normal = softBody->m_faces[softResult.index].m_normal;
+						if (normal.dot(rayDir) > 0)
+						{
+							// normal always point toward origin of the ray
+							normal = -normal;
+						}
+					}
+
+					btCollisionWorld::LocalRayResult rayResult(collisionObject,
+															   &shapeInfo,
+															   normal,
+															   softResult.fraction);
+					bool normalInWorldSpace = true;
+					resultCallback.addSingleResult(rayResult, normalInWorldSpace);
+				}
+			}
+		}
+	}
+	else
+	{
+		btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback);
+	}
+}
+
+void btSoftMultiBodyDynamicsWorld::serializeSoftBodies(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_SOFT_BODY)
+		{
+			int len = colObj->calculateSerializeBufferSize();
+			btChunk* chunk = serializer->allocate(len, 1);
+			const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk, structType, BT_SOFTBODY_CODE, colObj);
+		}
+	}
+}
+
+void btSoftMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
+{
+	serializer->startSerialization();
+
+	serializeDynamicsWorldInfo(serializer);
+
+	serializeSoftBodies(serializer);
+
+	serializeMultiBodies(serializer);
+
+	serializeRigidBodies(serializer);
+
+	serializeCollisionObjects(serializer);
+
+	serializeContactManifolds(serializer);
+
+	serializer->finishSerialization();
+}
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h b/extern/bullet2/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h
new file mode 100644
index 00000000000..f295945a6db
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h
@@ -0,0 +1,105 @@
+/*
+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_SOFT_MULTIBODY_DYNAMICS_WORLD_H
+#define BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H
+
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/btSoftBody.h"
+
+#ifndef BT_SOFT_RIGID_DYNAMICS_WORLD_H
+typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
+#endif
+
+class btSoftBodySolver;
+
+class btSoftMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
+{
+	btSoftBodyArray m_softBodies;
+	int m_drawFlags;
+	bool m_drawNodeTree;
+	bool m_drawFaceTree;
+	bool m_drawClusterTree;
+	btSoftBodyWorldInfo m_sbi;
+	///Solver classes that encapsulate multiple soft bodies for solving
+	btSoftBodySolver* m_softBodySolver;
+	bool m_ownsSolver;
+
+protected:
+	virtual void predictUnconstraintMotion(btScalar timeStep);
+
+	virtual void internalSingleStepSimulation(btScalar timeStep);
+
+	void solveSoftBodiesConstraints(btScalar timeStep);
+
+	void serializeSoftBodies(btSerializer* serializer);
+
+public:
+	btSoftMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver* softBodySolver = 0);
+
+	virtual ~btSoftMultiBodyDynamicsWorld();
+
+	virtual void debugDrawWorld();
+
+	void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
+
+	void removeSoftBody(btSoftBody* body);
+
+	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btDiscreteDynamicsWorld::removeCollisionObject
+	virtual void removeCollisionObject(btCollisionObject* collisionObject);
+
+	int getDrawFlags() const { return (m_drawFlags); }
+	void setDrawFlags(int f) { m_drawFlags = f; }
+
+	btSoftBodyWorldInfo& getWorldInfo()
+	{
+		return m_sbi;
+	}
+	const btSoftBodyWorldInfo& getWorldInfo() const
+	{
+		return m_sbi;
+	}
+
+	virtual btDynamicsWorldType getWorldType() const
+	{
+		return BT_SOFT_MULTIBODY_DYNAMICS_WORLD;
+	}
+
+	btSoftBodyArray& getSoftBodyArray()
+	{
+		return m_softBodies;
+	}
+
+	const btSoftBodyArray& getSoftBodyArray() const
+	{
+		return m_softBodies;
+	}
+
+	virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const;
+
+	/// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
+	/// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape.
+	/// This allows more customization.
+	static void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
+							  btCollisionObject* collisionObject,
+							  const btCollisionShape* collisionShape,
+							  const btTransform& colObjWorldTransform,
+							  RayResultCallback& resultCallback);
+
+	virtual void serialize(btSerializer* serializer);
+};
+
+#endif  //BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp
index 01c148a2ca8..5b65216e4b5 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp
@@ -27,18 +27,16 @@ subject to the following restrictions:
 
 //#include <stdio.h>
 
-btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* , bool isSwapped)
-: btCollisionAlgorithm(ci),
-//m_ownManifold(false),
-//m_manifoldPtr(mf),
-m_isSwapped(isSwapped)
+btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifold* /*mf*/, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper*, const btCollisionObjectWrapper*, bool isSwapped)
+	: btCollisionAlgorithm(ci),
+	  //m_ownManifold(false),
+	  //m_manifoldPtr(mf),
+	  m_isSwapped(isSwapped)
 {
 }
 
-
 btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm()
 {
-
 	//m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject);
 
 	/*if (m_ownManifold)
@@ -47,31 +45,28 @@ btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm()
 	m_dispatcher->releaseManifold(m_manifoldPtr);
 	}
 	*/
-
 }
 
-
 #include <stdio.h>
-
-void btSoftRigidCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+#include "LinearMath/btQuickprof.h"
+void btSoftRigidCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
+	BT_PROFILE("btSoftRigidCollisionAlgorithm::processCollision");
 	(void)dispatchInfo;
 	(void)resultOut;
 	//printf("btSoftRigidCollisionAlgorithm\n");
-//	const btCollisionObjectWrapper* softWrap = m_isSwapped?body1Wrap:body0Wrap;
-//	const btCollisionObjectWrapper* rigidWrap = m_isSwapped?body0Wrap:body1Wrap;
-	btSoftBody* softBody =  m_isSwapped? (btSoftBody*)body1Wrap->getCollisionObject() : (btSoftBody*)body0Wrap->getCollisionObject();
-	const btCollisionObjectWrapper* rigidCollisionObjectWrap = m_isSwapped? body0Wrap : body1Wrap;
-	
-	if (softBody->m_collisionDisabledObjects.findLinearSearch(rigidCollisionObjectWrap->getCollisionObject())==softBody->m_collisionDisabledObjects.size())
+	//	const btCollisionObjectWrapper* softWrap = m_isSwapped?body1Wrap:body0Wrap;
+	//	const btCollisionObjectWrapper* rigidWrap = m_isSwapped?body0Wrap:body1Wrap;
+	btSoftBody* softBody = m_isSwapped ? (btSoftBody*)body1Wrap->getCollisionObject() : (btSoftBody*)body0Wrap->getCollisionObject();
+	const btCollisionObjectWrapper* rigidCollisionObjectWrap = m_isSwapped ? body0Wrap : body1Wrap;
+
+	if (softBody->m_collisionDisabledObjects.findLinearSearch(rigidCollisionObjectWrap->getCollisionObject()) == softBody->m_collisionDisabledObjects.size())
 	{
 		softBody->getSoftBodySolver()->processCollision(softBody, rigidCollisionObjectWrap);
 	}
-
-
 }
 
-btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	(void)resultOut;
 	(void)dispatchInfo;
@@ -81,6 +76,3 @@ btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject*
 	//not yet
 	return btScalar(1.);
 }
-
-
-
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h
index a9b513e3639..9773af19a0c 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h
@@ -31,45 +31,41 @@ class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm
 	//	bool	m_ownManifold;
 	//	btPersistentManifold*	m_manifoldPtr;
 
-	btSoftBody*				m_softBody;
-	btCollisionObject*		m_rigidCollisionObject;
+	//btSoftBody*				m_softBody;
+	//btCollisionObject*		m_rigidCollisionObject;
 
 	///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean
-	bool	m_isSwapped;
+	bool m_isSwapped;
 
 public:
-
-	btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0,const btCollisionObjectWrapper* col1Wrap, bool isSwapped);
+	btSoftRigidCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0, const btCollisionObjectWrapper* col1Wrap, bool isSwapped);
 
 	virtual ~btSoftRigidCollisionAlgorithm();
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
 	{
 		//we don't add any manifolds
 	}
 
-
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftRigidCollisionAlgorithm));
 			if (!m_swapped)
 			{
-				return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false);
-			} else
+				return new (mem) btSoftRigidCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, false);
+			}
+			else
 			{
-				return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true);
+				return new (mem) btSoftRigidCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, true);
 			}
 		}
 	};
-
 };
 
-#endif //BT_SOFT_RIGID_COLLISION_ALGORITHM_H
-
-
+#endif  //BT_SOFT_RIGID_COLLISION_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp
index 653d5a06b49..510b731fc15 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btSoftRigidDynamicsWorld.h"
 #include "LinearMath/btQuickprof.h"
 
@@ -24,42 +23,38 @@ subject to the following restrictions:
 #include "btDefaultSoftBodySolver.h"
 #include "LinearMath/btSerializer.h"
 
-
 btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(
 	btDispatcher* dispatcher,
 	btBroadphaseInterface* pairCache,
 	btConstraintSolver* constraintSolver,
 	btCollisionConfiguration* collisionConfiguration,
-	btSoftBodySolver *softBodySolver ) : 
-		btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
-		m_softBodySolver( softBodySolver ),
-		m_ownsSolver(false)
+	btSoftBodySolver* softBodySolver) : btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
+										m_softBodySolver(softBodySolver),
+										m_ownsSolver(false)
 {
-	if( !m_softBodySolver )
+	if (!m_softBodySolver)
 	{
-		void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16);
-		m_softBodySolver = new(ptr) btDefaultSoftBodySolver();
+		void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver), 16);
+		m_softBodySolver = new (ptr) btDefaultSoftBodySolver();
 		m_ownsSolver = true;
 	}
 
-	m_drawFlags			=	fDrawFlags::Std;
-	m_drawNodeTree		=	true;
-	m_drawFaceTree		=	false;
-	m_drawClusterTree	=	false;
+	m_drawFlags = fDrawFlags::Std;
+	m_drawNodeTree = true;
+	m_drawFaceTree = false;
+	m_drawClusterTree = false;
 	m_sbi.m_broadphase = pairCache;
 	m_sbi.m_dispatcher = dispatcher;
 	m_sbi.m_sparsesdf.Initialize();
 	m_sbi.m_sparsesdf.Reset();
 
-	m_sbi.air_density		=	(btScalar)1.2;
-	m_sbi.water_density	=	0;
-	m_sbi.water_offset		=	0;
-	m_sbi.water_normal		=	btVector3(0,0,0);
-	m_sbi.m_gravity.setValue(0,-10,0);
+	m_sbi.air_density = (btScalar)1.2;
+	m_sbi.water_density = 0;
+	m_sbi.water_offset = 0;
+	m_sbi.water_normal = btVector3(0, 0, 0);
+	m_sbi.m_gravity.setValue(0, -10, 0);
 
 	m_sbi.m_sparsesdf.Initialize();
-
-
 }
 
 btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld()
@@ -71,82 +66,78 @@ btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld()
 	}
 }
 
-void	btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
-	btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep );
+	btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
 	{
 		BT_PROFILE("predictUnconstraintMotionSoftBody");
-		m_softBodySolver->predictMotion( float(timeStep) );
+		m_softBodySolver->predictMotion(float(timeStep));
 	}
 }
 
-void	btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep )
+void btSoftRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
-
 	// Let the solver grab the soft bodies and if necessary optimize for it
-	m_softBodySolver->optimize( getSoftBodyArray() );
+	m_softBodySolver->optimize(getSoftBodyArray());
 
-	if( !m_softBodySolver->checkInitialized() )
+	if (!m_softBodySolver->checkInitialized())
 	{
-		btAssert( "Solver initialization failed\n" );
+		btAssert("Solver initialization failed\n");
 	}
 
-	btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep );
+	btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep);
 
 	///solve soft bodies constraints
-	solveSoftBodiesConstraints( timeStep );
+	solveSoftBodiesConstraints(timeStep);
 
 	//self collisions
-	for ( int i=0;i<m_softBodies.size();i++)
+	for (int i = 0; i < m_softBodies.size(); i++)
 	{
-		btSoftBody*	psb=(btSoftBody*)m_softBodies[i];
+		btSoftBody* psb = (btSoftBody*)m_softBodies[i];
 		psb->defaultCollisionHandler(psb);
 	}
 
 	///update soft bodies
-	m_softBodySolver->updateSoftBodies( );
-	
+	m_softBodySolver->updateSoftBodies();
+
 	// End solver-wise simulation step
 	// ///////////////////////////////
-
 }
 
-void	btSoftRigidDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep )
+void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints(btScalar timeStep)
 {
 	BT_PROFILE("solveSoftConstraints");
 
-	if(m_softBodies.size())
+	if (m_softBodies.size())
 	{
 		btSoftBody::solveClusters(m_softBodies);
 	}
 
 	// Solve constraints solver-wise
-	m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() );
-
+	m_softBodySolver->solveConstraints(timeStep * m_softBodySolver->getTimeScale());
 }
 
-void	btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body,short int collisionFilterGroup,short int collisionFilterMask)
+void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask)
 {
 	m_softBodies.push_back(body);
 
 	// Set the soft body solver that will deal with this body
 	// to be the world's solver
-	body->setSoftBodySolver( m_softBodySolver );
+	body->setSoftBodySolver(m_softBodySolver);
 
 	btCollisionWorld::addCollisionObject(body,
-		collisionFilterGroup,
-		collisionFilterMask);
-
+										 collisionFilterGroup,
+										 collisionFilterMask);
 }
 
-void	btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body)
+void btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body)
 {
 	m_softBodies.remove(body);
 
 	btCollisionWorld::removeCollisionObject(body);
 }
 
-void	btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+void btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
 {
 	btSoftBody* body = btSoftBody::upcast(collisionObject);
 	if (body)
@@ -155,60 +146,57 @@ void	btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisio
 		btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
 }
 
-void	btSoftRigidDynamicsWorld::debugDrawWorld()
+void btSoftRigidDynamicsWorld::debugDrawWorld()
 {
 	btDiscreteDynamicsWorld::debugDrawWorld();
 
 	if (getDebugDrawer())
 	{
 		int i;
-		for (  i=0;i<this->m_softBodies.size();i++)
+		for (i = 0; i < this->m_softBodies.size(); i++)
 		{
-			btSoftBody*	psb=(btSoftBody*)this->m_softBodies[i];
+			btSoftBody* psb = (btSoftBody*)this->m_softBodies[i];
 			if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
 			{
-				btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
-				btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags);
+				btSoftBodyHelpers::DrawFrame(psb, m_debugDrawer);
+				btSoftBodyHelpers::Draw(psb, m_debugDrawer, m_drawFlags);
 			}
-			
+
 			if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
 			{
-				if(m_drawNodeTree)		btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
-				if(m_drawFaceTree)		btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
-				if(m_drawClusterTree)	btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer);
+				if (m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb, m_debugDrawer);
+				if (m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb, m_debugDrawer);
+				if (m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb, m_debugDrawer);
 			}
-		}		
-	}	
+		}
+	}
 }
 
-
-
-
 struct btSoftSingleRayCallback : public btBroadphaseRayCallback
 {
-	btVector3	m_rayFromWorld;
-	btVector3	m_rayToWorld;
-	btTransform	m_rayFromTrans;
-	btTransform	m_rayToTrans;
-	btVector3	m_hitNormal;
-
-	const btSoftRigidDynamicsWorld*	m_world;
-	btCollisionWorld::RayResultCallback&	m_resultCallback;
-
-	btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftRigidDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
-	:m_rayFromWorld(rayFromWorld),
-	m_rayToWorld(rayToWorld),
-	m_world(world),
-	m_resultCallback(resultCallback)
+	btVector3 m_rayFromWorld;
+	btVector3 m_rayToWorld;
+	btTransform m_rayFromTrans;
+	btTransform m_rayToTrans;
+	btVector3 m_hitNormal;
+
+	const btSoftRigidDynamicsWorld* m_world;
+	btCollisionWorld::RayResultCallback& m_resultCallback;
+
+	btSoftSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btSoftRigidDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
+		: m_rayFromWorld(rayFromWorld),
+		  m_rayToWorld(rayToWorld),
+		  m_world(world),
+		  m_resultCallback(resultCallback)
 	{
 		m_rayFromTrans.setIdentity();
 		m_rayFromTrans.setOrigin(m_rayFromWorld);
 		m_rayToTrans.setIdentity();
 		m_rayToTrans.setOrigin(m_rayToWorld);
 
-		btVector3 rayDir = (rayToWorld-rayFromWorld);
+		btVector3 rayDir = (rayToWorld - rayFromWorld);
 
-		rayDir.normalize ();
+		rayDir.normalize();
 		///what about division by zero? --> just set rayDirection[i] to INF/1e30
 		m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
 		m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
@@ -217,22 +205,19 @@ struct btSoftSingleRayCallback : public btBroadphaseRayCallback
 		m_signs[1] = m_rayDirectionInverse[1] < 0.0;
 		m_signs[2] = m_rayDirectionInverse[2] < 0.0;
 
-		m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
-
+		m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
 	}
 
-	
-
-	virtual bool	process(const btBroadphaseProxy* proxy)
+	virtual bool process(const btBroadphaseProxy* proxy)
 	{
 		///terminate further ray tests, once the closestHitFraction reached zero
 		if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
 			return false;
 
-		btCollisionObject*	collisionObject = (btCollisionObject*)proxy->m_clientObject;
+		btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
 
 		//only perform raycast if filterMask matches
-		if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
+		if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
 		{
 			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
 			//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
@@ -250,110 +235,106 @@ struct btSoftSingleRayCallback : public btBroadphaseRayCallback
 			//culling already done by broadphase
 			//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
 			{
-				m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
-					collisionObject,
-						collisionObject->getCollisionShape(),
-						collisionObject->getWorldTransform(),
-						m_resultCallback);
+				m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
+									   collisionObject,
+									   collisionObject->getCollisionShape(),
+									   collisionObject->getWorldTransform(),
+									   m_resultCallback);
 			}
 		}
 		return true;
 	}
 };
 
-void	btSoftRigidDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
+void btSoftRigidDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
 {
 	BT_PROFILE("rayTest");
 	/// use the broadphase to accelerate the search for objects, based on their aabb
 	/// and for each object with ray-aabb overlap, perform an exact ray test
-	btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
+	btSoftSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback);
 
 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
-	m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
+	m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
 #else
-	for (int i=0;i<this->getNumCollisionObjects();i++)
+	for (int i = 0; i < this->getNumCollisionObjects(); i++)
 	{
 		rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
-	}	
-#endif //USE_BRUTEFORCE_RAYBROADPHASE
-
+	}
+#endif  //USE_BRUTEFORCE_RAYBROADPHASE
 }
 
-
-void	btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
-					  btCollisionObject* collisionObject,
-					  const btCollisionShape* collisionShape,
-					  const btTransform& colObjWorldTransform,
-					  RayResultCallback& resultCallback)
+void btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
+											 btCollisionObject* collisionObject,
+											 const btCollisionShape* collisionShape,
+											 const btTransform& colObjWorldTransform,
+											 RayResultCallback& resultCallback)
 {
-	if (collisionShape->isSoftBody()) {
+	if (collisionShape->isSoftBody())
+	{
 		btSoftBody* softBody = btSoftBody::upcast(collisionObject);
-		if (softBody) {
+		if (softBody)
+		{
 			btSoftBody::sRayCast softResult;
-			if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) 
+			if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
 			{
-				
-				if (softResult.fraction<= resultCallback.m_closestHitFraction)
+				if (softResult.fraction <= resultCallback.m_closestHitFraction)
 				{
-
 					btCollisionWorld::LocalShapeInfo shapeInfo;
 					shapeInfo.m_shapePart = 0;
 					shapeInfo.m_triangleIndex = softResult.index;
 					// get the normal
 					btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
-					btVector3 normal=-rayDir;
+					btVector3 normal = -rayDir;
 					normal.normalize();
 
 					if (softResult.feature == btSoftBody::eFeature::Face)
 					{
 						normal = softBody->m_faces[softResult.index].m_normal;
-						if (normal.dot(rayDir) > 0) {
+						if (normal.dot(rayDir) > 0)
+						{
 							// normal always point toward origin of the ray
 							normal = -normal;
 						}
 					}
-	
-					btCollisionWorld::LocalRayResult rayResult
-						(collisionObject,
-						 &shapeInfo,
-						 normal,
-						 softResult.fraction);
-					bool	normalInWorldSpace = true;
-					resultCallback.addSingleResult(rayResult,normalInWorldSpace);
+
+					btCollisionWorld::LocalRayResult rayResult(collisionObject,
+															   &shapeInfo,
+															   normal,
+															   softResult.fraction);
+					bool normalInWorldSpace = true;
+					resultCallback.addSingleResult(rayResult, normalInWorldSpace);
 				}
 			}
 		}
-	} 
-	else {
-		btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback);
+	}
+	else
+	{
+		btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback);
 	}
 }
 
-
-void	btSoftRigidDynamicsWorld::serializeSoftBodies(btSerializer* serializer)
+void btSoftRigidDynamicsWorld::serializeSoftBodies(btSerializer* serializer)
 {
 	int i;
 	//serialize all collision objects
-	for (i=0;i<m_collisionObjects.size();i++)
+	for (i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		if (colObj->getInternalType() & btCollisionObject::CO_SOFT_BODY)
 		{
 			int len = colObj->calculateSerializeBufferSize();
-			btChunk* chunk = serializer->allocate(len,1);
+			btChunk* chunk = serializer->allocate(len, 1);
 			const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
-			serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj);
+			serializer->finalizeChunk(chunk, structType, BT_SOFTBODY_CODE, colObj);
 		}
 	}
-
 }
 
-void	btSoftRigidDynamicsWorld::serialize(btSerializer* serializer)
+void btSoftRigidDynamicsWorld::serialize(btSerializer* serializer)
 {
-
 	serializer->startSerialization();
 
-	serializeDynamicsWorldInfo( serializer);
+	serializeDynamicsWorldInfo(serializer);
 
 	serializeSoftBodies(serializer);
 
@@ -363,5 +344,3 @@ void	btSoftRigidDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializer->finishSerialization();
 }
-
-
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h
index 3e0efafd6c7..be49c444d7d 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h
+++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h
@@ -19,63 +19,60 @@ subject to the following restrictions:
 #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
 #include "btSoftBody.h"
 
-typedef	btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
+typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
 
 class btSoftBodySolver;
 
 class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld
 {
-
-	btSoftBodyArray	m_softBodies;
-	int				m_drawFlags;
-	bool			m_drawNodeTree;
-	bool			m_drawFaceTree;
-	bool			m_drawClusterTree;
+	btSoftBodyArray m_softBodies;
+	int m_drawFlags;
+	bool m_drawNodeTree;
+	bool m_drawFaceTree;
+	bool m_drawClusterTree;
 	btSoftBodyWorldInfo m_sbi;
 	///Solver classes that encapsulate multiple soft bodies for solving
-	btSoftBodySolver *m_softBodySolver;
-	bool			m_ownsSolver;
+	btSoftBodySolver* m_softBodySolver;
+	bool m_ownsSolver;
 
 protected:
+	virtual void predictUnconstraintMotion(btScalar timeStep);
 
-	virtual void	predictUnconstraintMotion(btScalar timeStep);
-
-	virtual void	internalSingleStepSimulation( btScalar timeStep);
+	virtual void internalSingleStepSimulation(btScalar timeStep);
 
-	void	solveSoftBodiesConstraints( btScalar timeStep );
+	void solveSoftBodiesConstraints(btScalar timeStep);
 
-	void	serializeSoftBodies(btSerializer* serializer);
+	void serializeSoftBodies(btSerializer* serializer);
 
 public:
-
-	btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver *softBodySolver = 0 );
+	btSoftRigidDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver* softBodySolver = 0);
 
 	virtual ~btSoftRigidDynamicsWorld();
 
-	virtual void	debugDrawWorld();
+	virtual void debugDrawWorld();
 
-	void	addSoftBody(btSoftBody* body,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter);
+	void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
 
-	void	removeSoftBody(btSoftBody* body);
+	void removeSoftBody(btSoftBody* body);
 
 	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btDiscreteDynamicsWorld::removeCollisionObject
-	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
+	virtual void removeCollisionObject(btCollisionObject* collisionObject);
 
-	int		getDrawFlags() const { return(m_drawFlags); }
-	void	setDrawFlags(int f)	{ m_drawFlags=f; }
+	int getDrawFlags() const { return (m_drawFlags); }
+	void setDrawFlags(int f) { m_drawFlags = f; }
 
-	btSoftBodyWorldInfo&	getWorldInfo()
+	btSoftBodyWorldInfo& getWorldInfo()
 	{
 		return m_sbi;
 	}
-	const btSoftBodyWorldInfo&	getWorldInfo() const
+	const btSoftBodyWorldInfo& getWorldInfo() const
 	{
 		return m_sbi;
 	}
 
-	virtual btDynamicsWorldType	getWorldType() const
+	virtual btDynamicsWorldType getWorldType() const
 	{
-		return	BT_SOFT_RIGID_DYNAMICS_WORLD;
+		return BT_SOFT_RIGID_DYNAMICS_WORLD;
 	}
 
 	btSoftBodyArray& getSoftBodyArray()
@@ -88,20 +85,18 @@ public:
 		return m_softBodies;
 	}
 
-
-	virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; 
+	virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const;
 
 	/// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
 	/// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape.
 	/// This allows more customization.
-	static void	rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
-					  btCollisionObject* collisionObject,
-					  const btCollisionShape* collisionShape,
-					  const btTransform& colObjWorldTransform,
-					  RayResultCallback& resultCallback);
-
-	virtual	void	serialize(btSerializer* serializer);
+	static void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
+							  btCollisionObject* collisionObject,
+							  const btCollisionShape* collisionShape,
+							  const btTransform& colObjWorldTransform,
+							  RayResultCallback& resultCallback);
 
+	virtual void serialize(btSerializer* serializer);
 };
 
-#endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H
+#endif  //BT_SOFT_RIGID_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp
index 72043e69e2f..9c3e904f645 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp
@@ -23,8 +23,8 @@ subject to the following restrictions:
 
 #define USE_PERSISTENT_CONTACTS 1
 
-btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* /*obj0*/,const btCollisionObjectWrapper* /*obj1*/)
-: btCollisionAlgorithm(ci)
+btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* /*mf*/, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* /*obj0*/, const btCollisionObjectWrapper* /*obj1*/)
+	: btCollisionAlgorithm(ci)
 //m_ownManifold(false),
 //m_manifoldPtr(mf)
 {
@@ -34,14 +34,14 @@ btSoftSoftCollisionAlgorithm::~btSoftSoftCollisionAlgorithm()
 {
 }
 
-void btSoftSoftCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
+void btSoftSoftCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/)
 {
-	btSoftBody* soft0 =	(btSoftBody*)body0Wrap->getCollisionObject();
-	btSoftBody* soft1 =	(btSoftBody*)body1Wrap->getCollisionObject();
+	btSoftBody* soft0 = (btSoftBody*)body0Wrap->getCollisionObject();
+	btSoftBody* soft1 = (btSoftBody*)body1Wrap->getCollisionObject();
 	soft0->getSoftBodySolver()->processCollision(soft0, soft1);
 }
 
-btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
+btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/, btCollisionObject* /*body1*/, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/)
 {
 	//not yet
 	return 1.f;
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h
index 43b1439cc53..6f871f5b855 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h
@@ -27,43 +27,39 @@ class btSoftBody;
 ///collision detection between two btSoftBody shapes
 class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm
 {
-	bool	m_ownManifold;
-	btPersistentManifold*	m_manifoldPtr;
-
-	btSoftBody*	m_softBody0;
-	btSoftBody*	m_softBody1;
+	bool m_ownManifold;
+	btPersistentManifold* m_manifoldPtr;
 
+	//	btSoftBody*	m_softBody0;
+	//	btSoftBody*	m_softBody1;
 
 public:
 	btSoftSoftCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
 		: btCollisionAlgorithm(ci) {}
 
-	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
 
-	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
 	{
 		if (m_manifoldPtr && m_ownManifold)
 			manifoldArray.push_back(m_manifoldPtr);
 	}
 
-	btSoftSoftCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+	btSoftSoftCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap);
 
 	virtual ~btSoftSoftCollisionAlgorithm();
 
-	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc
 	{
-		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 		{
 			int bbsize = sizeof(btSoftSoftCollisionAlgorithm);
 			void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
-			return new(ptr) btSoftSoftCollisionAlgorithm(0,ci,body0Wrap,body1Wrap);
+			return new (ptr) btSoftSoftCollisionAlgorithm(0, ci, body0Wrap, body1Wrap);
 		}
 	};
-
 };
 
-#endif //BT_SOFT_SOFT_COLLISION_ALGORITHM_H
-
-
+#endif  //BT_SOFT_SOFT_COLLISION_ALGORITHM_H
diff --git a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h
index 8992ddbb68d..d611726bcdc 100644
--- a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h
+++ b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h
@@ -20,299 +20,353 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
 
-// Modified Paul Hsieh hash
-template <const int DWORDLEN>
-unsigned int HsiehHash(const void* pdata)
+// Fast Hash
+
+#if !defined(get16bits)
+#define get16bits(d) ((((unsigned int)(((const unsigned char*)(d))[1])) << 8) + (unsigned int)(((const unsigned char*)(d))[0]))
+#endif
+//
+// super hash function by Paul Hsieh
+//
+inline unsigned int HsiehHash(const char* data, int len)
 {
-	const unsigned short*	data=(const unsigned short*)pdata;
-	unsigned				hash=DWORDLEN<<2,tmp;
-	for(int i=0;i<DWORDLEN;++i)
+	unsigned int hash = len, tmp;
+	len >>= 2;
+
+	/* Main loop */
+	for (; len > 0; len--)
 	{
-		hash	+=	data[0];
-		tmp		=	(data[1]<<11)^hash;
-		hash	=	(hash<<16)^tmp;
-		data	+=	2;
-		hash	+=	hash>>11;
+		hash += get16bits(data);
+		tmp = (get16bits(data + 2) << 11) ^ hash;
+		hash = (hash << 16) ^ tmp;
+		data += 2 * sizeof(unsigned short);
+		hash += hash >> 11;
 	}
-	hash^=hash<<3;hash+=hash>>5;
-	hash^=hash<<4;hash+=hash>>17;
-	hash^=hash<<25;hash+=hash>>6;
-	return(hash);
+
+	/* Force "avalanching" of final 127 bits */
+	hash ^= hash << 3;
+	hash += hash >> 5;
+	hash ^= hash << 4;
+	hash += hash >> 17;
+	hash ^= hash << 25;
+	hash += hash >> 6;
+
+	return hash;
 }
 
 template <const int CELLSIZE>
-struct	btSparseSdf
+struct btSparseSdf
 {
 	//
 	// Inner types
 	//
 	struct IntFrac
 	{
-		int					b;
-		int					i;
-		btScalar			f;
+		int b;
+		int i;
+		btScalar f;
 	};
-	struct	Cell
+	struct Cell
 	{
-		btScalar			d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1];
-		int					c[3];
-		int					puid;
-		unsigned			hash;
-		const btCollisionShape*	pclient;
-		Cell*				next;
+		btScalar d[CELLSIZE + 1][CELLSIZE + 1][CELLSIZE + 1];
+		int c[3];
+		int puid;
+		unsigned hash;
+		const btCollisionShape* pclient;
+		Cell* next;
 	};
 	//
 	// Fields
 	//
 
-	btAlignedObjectArray<Cell*>		cells;	
-	btScalar						voxelsz;
-	int								puid;
-	int								ncells;
-	int								m_clampCells;
-	int								nprobes;
-	int								nqueries;	
+	btAlignedObjectArray<Cell*> cells;
+	btScalar voxelsz;
+	btScalar m_defaultVoxelsz;
+	int puid;
+	int ncells;
+	int m_clampCells;
+	int nprobes;
+	int nqueries;
 
+	~btSparseSdf()
+	{
+		Reset();
+	}
 	//
 	// Methods
 	//
 
 	//
-	void					Initialize(int hashsize=2383, int clampCells = 256*1024)
+	void Initialize(int hashsize = 2383, int clampCells = 256 * 1024)
 	{
 		//avoid a crash due to running out of memory, so clamp the maximum number of cells allocated
 		//if this limit is reached, the SDF is reset (at the cost of some performance during the reset)
 		m_clampCells = clampCells;
-		cells.resize(hashsize,0);
+		cells.resize(hashsize, 0);
+		m_defaultVoxelsz = 0.25;
 		Reset();
 	}
 	//
-	void					Reset()
+
+	void setDefaultVoxelsz(btScalar sz)
+	{
+		m_defaultVoxelsz = sz;
+	}
+
+	void Reset()
 	{
-		for(int i=0,ni=cells.size();i<ni;++i)
+		for (int i = 0, ni = cells.size(); i < ni; ++i)
 		{
-			Cell*	pc=cells[i];
-			cells[i]=0;
-			while(pc)
+			Cell* pc = cells[i];
+			cells[i] = 0;
+			while (pc)
 			{
-				Cell*	pn=pc->next;
+				Cell* pn = pc->next;
 				delete pc;
-				pc=pn;
+				pc = pn;
 			}
 		}
-		voxelsz		=0.25;
-		puid		=0;
-		ncells		=0;
-		nprobes		=1;
-		nqueries	=1;
+		voxelsz = m_defaultVoxelsz;
+		puid = 0;
+		ncells = 0;
+		nprobes = 1;
+		nqueries = 1;
 	}
 	//
-	void					GarbageCollect(int lifetime=256)
+	void GarbageCollect(int lifetime = 256)
 	{
-		const int life=puid-lifetime;
-		for(int i=0;i<cells.size();++i)
+		const int life = puid - lifetime;
+		for (int i = 0; i < cells.size(); ++i)
 		{
-			Cell*&	root=cells[i];
-			Cell*	pp=0;
-			Cell*	pc=root;
-			while(pc)
+			Cell*& root = cells[i];
+			Cell* pp = 0;
+			Cell* pc = root;
+			while (pc)
 			{
-				Cell*	pn=pc->next;
-				if(pc->puid<life)
+				Cell* pn = pc->next;
+				if (pc->puid < life)
 				{
-					if(pp) pp->next=pn; else root=pn;
-					delete pc;pc=pp;--ncells;
+					if (pp)
+						pp->next = pn;
+					else
+						root = pn;
+					delete pc;
+					pc = pp;
+					--ncells;
 				}
-				pp=pc;pc=pn;
+				pp = pc;
+				pc = pn;
 			}
 		}
 		//printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries);
-		nqueries=1;
-		nprobes=1;
-		++puid;	///@todo: Reset puid's when int range limit is reached	*/ 
-		/* else setup a priority list...						*/ 
+		nqueries = 1;
+		nprobes = 1;
+		++puid;  ///@todo: Reset puid's when int range limit is reached	*/
+		/* else setup a priority list...						*/
 	}
 	//
-	int						RemoveReferences(btCollisionShape* pcs)
+	int RemoveReferences(btCollisionShape* pcs)
 	{
-		int	refcount=0;
-		for(int i=0;i<cells.size();++i)
+		int refcount = 0;
+		for (int i = 0; i < cells.size(); ++i)
 		{
-			Cell*&	root=cells[i];
-			Cell*	pp=0;
-			Cell*	pc=root;
-			while(pc)
+			Cell*& root = cells[i];
+			Cell* pp = 0;
+			Cell* pc = root;
+			while (pc)
 			{
-				Cell*	pn=pc->next;
-				if(pc->pclient==pcs)
+				Cell* pn = pc->next;
+				if (pc->pclient == pcs)
 				{
-					if(pp) pp->next=pn; else root=pn;
-					delete pc;pc=pp;++refcount;
+					if (pp)
+						pp->next = pn;
+					else
+						root = pn;
+					delete pc;
+					pc = pp;
+					++refcount;
 				}
-				pp=pc;pc=pn;
+				pp = pc;
+				pc = pn;
 			}
 		}
-		return(refcount);
+		return (refcount);
 	}
 	//
-	btScalar				Evaluate(	const btVector3& x,
-		const btCollisionShape* shape,
-		btVector3& normal,
-		btScalar margin)
+	btScalar Evaluate(const btVector3& x,
+					  const btCollisionShape* shape,
+					  btVector3& normal,
+					  btScalar margin)
 	{
-		/* Lookup cell			*/ 
-		const btVector3	scx=x/voxelsz;
-		const IntFrac	ix=Decompose(scx.x());
-		const IntFrac	iy=Decompose(scx.y());
-		const IntFrac	iz=Decompose(scx.z());
-		const unsigned	h=Hash(ix.b,iy.b,iz.b,shape);
-		Cell*&			root=cells[static_cast<int>(h%cells.size())];
-		Cell*			c=root;
+		/* Lookup cell			*/
+		const btVector3 scx = x / voxelsz;
+		const IntFrac ix = Decompose(scx.x());
+		const IntFrac iy = Decompose(scx.y());
+		const IntFrac iz = Decompose(scx.z());
+		const unsigned h = Hash(ix.b, iy.b, iz.b, shape);
+		Cell*& root = cells[static_cast<int>(h % cells.size())];
+		Cell* c = root;
 		++nqueries;
-		while(c)
+		while (c)
 		{
 			++nprobes;
-			if(	(c->hash==h)	&&
-				(c->c[0]==ix.b)	&&
-				(c->c[1]==iy.b)	&&
-				(c->c[2]==iz.b)	&&
-				(c->pclient==shape))
-			{ break; }
+			if ((c->hash == h) &&
+				(c->c[0] == ix.b) &&
+				(c->c[1] == iy.b) &&
+				(c->c[2] == iz.b) &&
+				(c->pclient == shape))
+			{
+				break;
+			}
 			else
-			{ c=c->next; }
+			{
+				// printf("c->hash/c[0][1][2]=%d,%d,%d,%d\n", c->hash, c->c[0], c->c[1],c->c[2]);
+				//printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b);
+
+				c = c->next;
+			}
 		}
-		if(!c)
+		if (!c)
 		{
-			++nprobes;		
+			++nprobes;
 			++ncells;
-			if (ncells>m_clampCells)
+			//int sz = sizeof(Cell);
+			if (ncells > m_clampCells)
 			{
-				static int numResets=0;
+				static int numResets = 0;
 				numResets++;
-//				printf("numResets=%d\n",numResets);
+				//				printf("numResets=%d\n",numResets);
 				Reset();
 			}
 
-			c=new Cell();
-			c->next=root;root=c;
-			c->pclient=shape;
-			c->hash=h;
-			c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b;
+			c = new Cell();
+			c->next = root;
+			root = c;
+			c->pclient = shape;
+			c->hash = h;
+			c->c[0] = ix.b;
+			c->c[1] = iy.b;
+			c->c[2] = iz.b;
 			BuildCell(*c);
 		}
-		c->puid=puid;
-		/* Extract infos		*/ 
-		const int		o[]={	ix.i,iy.i,iz.i};
-		const btScalar	d[]={	c->d[o[0]+0][o[1]+0][o[2]+0],
-			c->d[o[0]+1][o[1]+0][o[2]+0],
-			c->d[o[0]+1][o[1]+1][o[2]+0],
-			c->d[o[0]+0][o[1]+1][o[2]+0],
-			c->d[o[0]+0][o[1]+0][o[2]+1],
-			c->d[o[0]+1][o[1]+0][o[2]+1],
-			c->d[o[0]+1][o[1]+1][o[2]+1],
-			c->d[o[0]+0][o[1]+1][o[2]+1]};
-		/* Normal	*/ 
+		c->puid = puid;
+		/* Extract infos		*/
+		const int o[] = {ix.i, iy.i, iz.i};
+		const btScalar d[] = {c->d[o[0] + 0][o[1] + 0][o[2] + 0],
+							  c->d[o[0] + 1][o[1] + 0][o[2] + 0],
+							  c->d[o[0] + 1][o[1] + 1][o[2] + 0],
+							  c->d[o[0] + 0][o[1] + 1][o[2] + 0],
+							  c->d[o[0] + 0][o[1] + 0][o[2] + 1],
+							  c->d[o[0] + 1][o[1] + 0][o[2] + 1],
+							  c->d[o[0] + 1][o[1] + 1][o[2] + 1],
+							  c->d[o[0] + 0][o[1] + 1][o[2] + 1]};
+		/* Normal	*/
 #if 1
-		const btScalar	gx[]={	d[1]-d[0],d[2]-d[3],
-			d[5]-d[4],d[6]-d[7]};
-		const btScalar	gy[]={	d[3]-d[0],d[2]-d[1],
-			d[7]-d[4],d[6]-d[5]};
-		const btScalar	gz[]={	d[4]-d[0],d[5]-d[1],
-			d[7]-d[3],d[6]-d[2]};
-		normal.setX(Lerp(	Lerp(gx[0],gx[1],iy.f),
-			Lerp(gx[2],gx[3],iy.f),iz.f));
-		normal.setY(Lerp(	Lerp(gy[0],gy[1],ix.f),
-			Lerp(gy[2],gy[3],ix.f),iz.f));
-		normal.setZ(Lerp(	Lerp(gz[0],gz[1],ix.f),
-			Lerp(gz[2],gz[3],ix.f),iy.f));
-		normal		=	normal.normalized();
+		const btScalar gx[] = {d[1] - d[0], d[2] - d[3],
+							   d[5] - d[4], d[6] - d[7]};
+		const btScalar gy[] = {d[3] - d[0], d[2] - d[1],
+							   d[7] - d[4], d[6] - d[5]};
+		const btScalar gz[] = {d[4] - d[0], d[5] - d[1],
+							   d[7] - d[3], d[6] - d[2]};
+		normal.setX(Lerp(Lerp(gx[0], gx[1], iy.f),
+						 Lerp(gx[2], gx[3], iy.f), iz.f));
+		normal.setY(Lerp(Lerp(gy[0], gy[1], ix.f),
+						 Lerp(gy[2], gy[3], ix.f), iz.f));
+		normal.setZ(Lerp(Lerp(gz[0], gz[1], ix.f),
+						 Lerp(gz[2], gz[3], ix.f), iy.f));
+		normal.safeNormalize();
 #else
-		normal		=	btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized();
+		normal = btVector3(d[1] - d[0], d[3] - d[0], d[4] - d[0]).normalized();
 #endif
-		/* Distance	*/ 
-		const btScalar	d0=Lerp(Lerp(d[0],d[1],ix.f),
-			Lerp(d[3],d[2],ix.f),iy.f);
-		const btScalar	d1=Lerp(Lerp(d[4],d[5],ix.f),
-			Lerp(d[7],d[6],ix.f),iy.f);
-		return(Lerp(d0,d1,iz.f)-margin);
+		/* Distance	*/
+		const btScalar d0 = Lerp(Lerp(d[0], d[1], ix.f),
+								 Lerp(d[3], d[2], ix.f), iy.f);
+		const btScalar d1 = Lerp(Lerp(d[4], d[5], ix.f),
+								 Lerp(d[7], d[6], ix.f), iy.f);
+		return (Lerp(d0, d1, iz.f) - margin);
 	}
 	//
-	void					BuildCell(Cell& c)
+	void BuildCell(Cell& c)
 	{
-		const btVector3	org=btVector3(	(btScalar)c.c[0],
-			(btScalar)c.c[1],
-			(btScalar)c.c[2])	*
-			CELLSIZE*voxelsz;
-		for(int k=0;k<=CELLSIZE;++k)
+		const btVector3 org = btVector3((btScalar)c.c[0],
+										(btScalar)c.c[1],
+										(btScalar)c.c[2]) *
+							  CELLSIZE * voxelsz;
+		for (int k = 0; k <= CELLSIZE; ++k)
 		{
-			const btScalar	z=voxelsz*k+org.z();
-			for(int j=0;j<=CELLSIZE;++j)
+			const btScalar z = voxelsz * k + org.z();
+			for (int j = 0; j <= CELLSIZE; ++j)
 			{
-				const btScalar	y=voxelsz*j+org.y();
-				for(int i=0;i<=CELLSIZE;++i)
+				const btScalar y = voxelsz * j + org.y();
+				for (int i = 0; i <= CELLSIZE; ++i)
 				{
-					const btScalar	x=voxelsz*i+org.x();
-					c.d[i][j][k]=DistanceToShape(	btVector3(x,y,z),
-						c.pclient);
+					const btScalar x = voxelsz * i + org.x();
+					c.d[i][j][k] = DistanceToShape(btVector3(x, y, z),
+												   c.pclient);
 				}
 			}
 		}
 	}
 	//
-	static inline btScalar	DistanceToShape(const btVector3& x,
-		const btCollisionShape* shape)
+	static inline btScalar DistanceToShape(const btVector3& x,
+										   const btCollisionShape* shape)
 	{
-		btTransform	unit;
+		btTransform unit;
 		unit.setIdentity();
-		if(shape->isConvex())
+		if (shape->isConvex())
 		{
-			btGjkEpaSolver2::sResults	res;
-			const btConvexShape*				csh=static_cast<const btConvexShape*>(shape);
-			return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res));
+			btGjkEpaSolver2::sResults res;
+			const btConvexShape* csh = static_cast<const btConvexShape*>(shape);
+			return (btGjkEpaSolver2::SignedDistance(x, 0, csh, unit, res));
 		}
-		return(0);
+		return (0);
 	}
 	//
-	static inline IntFrac	Decompose(btScalar x)
+	static inline IntFrac Decompose(btScalar x)
 	{
 		/* That one need a lot of improvements...	*/
-		/* Remove test, faster floor...				*/ 
-		IntFrac			r;
-		x/=CELLSIZE;
-		const int		o=x<0?(int)(-x+1):0;
-		x+=o;r.b=(int)x;
-		const btScalar	k=(x-r.b)*CELLSIZE;
-		r.i=(int)k;r.f=k-r.i;r.b-=o;
-		return(r);
+		/* Remove test, faster floor...				*/
+		IntFrac r;
+		x /= CELLSIZE;
+		const int o = x < 0 ? (int)(-x + 1) : 0;
+		x += o;
+		r.b = (int)x;
+		const btScalar k = (x - r.b) * CELLSIZE;
+		r.i = (int)k;
+		r.f = k - r.i;
+		r.b -= o;
+		return (r);
 	}
 	//
-	static inline btScalar	Lerp(btScalar a,btScalar b,btScalar t)
+	static inline btScalar Lerp(btScalar a, btScalar b, btScalar t)
 	{
-		return(a+(b-a)*t);
+		return (a + (b - a) * t);
 	}
 
-
-
 	//
-	static inline unsigned int	Hash(int x,int y,int z,const btCollisionShape* shape)
+	static inline unsigned int Hash(int x, int y, int z, const btCollisionShape* shape)
 	{
 		struct btS
-		{ 
-			int x,y,z;
+		{
+			int x, y, z, w;
 			void* p;
 		};
 
 		btS myset;
+		//memset may be needed in case of additional (uninitialized) padding!
+		//memset(&myset, 0, sizeof(btS));
 
-		myset.x=x;myset.y=y;myset.z=z;myset.p=(void*)shape;
-		const void* ptr = &myset;
-
-		unsigned int result = HsiehHash<sizeof(btS)/4> (ptr);
+		myset.x = x;
+		myset.y = y;
+		myset.z = z;
+		myset.w = 0;
+		myset.p = (void*)shape;
+		const char* ptr = (const char*)&myset;
 
+		unsigned int result = HsiehHash(ptr, sizeof(btS));
 
 		return result;
 	}
 };
 
-
-#endif //BT_SPARSE_SDF_H
+#endif  //BT_SPARSE_SDF_H
diff --git a/extern/bullet2/src/BulletSoftBody/poly34.cpp b/extern/bullet2/src/BulletSoftBody/poly34.cpp
new file mode 100644
index 00000000000..ec7549c8e8d
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/poly34.cpp
@@ -0,0 +1,447 @@
+// poly34.cpp : solution of cubic and quartic equation
+// (c) Khashin S.I. http://math.ivanovo.ac.ru/dalgebra/Khashin/index.html
+// khash2 (at) gmail.com
+// Thanks to Alexandr Rakhmanin <rakhmanin (at) gmail.com>
+// public domain
+//
+#include <math.h>
+
+#include "poly34.h"  // solution of cubic and quartic equation
+#define TwoPi 6.28318530717958648
+const btScalar eps = SIMD_EPSILON;
+
+//=============================================================================
+// _root3, root3 from http://prografix.narod.ru
+//=============================================================================
+static SIMD_FORCE_INLINE btScalar _root3(btScalar x)
+{
+	btScalar s = 1.;
+	while (x < 1.)
+	{
+		x *= 8.;
+		s *= 0.5;
+	}
+	while (x > 8.)
+	{
+		x *= 0.125;
+		s *= 2.;
+	}
+	btScalar r = 1.5;
+	r -= 1. / 3. * (r - x / (r * r));
+	r -= 1. / 3. * (r - x / (r * r));
+	r -= 1. / 3. * (r - x / (r * r));
+	r -= 1. / 3. * (r - x / (r * r));
+	r -= 1. / 3. * (r - x / (r * r));
+	r -= 1. / 3. * (r - x / (r * r));
+	return r * s;
+}
+
+btScalar SIMD_FORCE_INLINE root3(btScalar x)
+{
+	if (x > 0)
+		return _root3(x);
+	else if (x < 0)
+		return -_root3(-x);
+	else
+		return 0.;
+}
+
+// x - array of size 2
+// return 2: 2 real roots x[0], x[1]
+// return 0: pair of complex roots: x[0]i*x[1]
+int SolveP2(btScalar* x, btScalar a, btScalar b)
+{  // solve equation x^2 + a*x + b = 0
+	btScalar D = 0.25 * a * a - b;
+	if (D >= 0)
+	{
+		D = sqrt(D);
+		x[0] = -0.5 * a + D;
+		x[1] = -0.5 * a - D;
+		return 2;
+	}
+	x[0] = -0.5 * a;
+	x[1] = sqrt(-D);
+	return 0;
+}
+//---------------------------------------------------------------------------
+// x - array of size 3
+// In case 3 real roots: => x[0], x[1], x[2], return 3
+//         2 real roots: x[0], x[1],          return 2
+//         1 real root : x[0], x[1]  i*x[2], return 1
+int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c)
+{  // solve cubic equation x^3 + a*x^2 + b*x + c = 0
+	btScalar a2 = a * a;
+	btScalar q = (a2 - 3 * b) / 9;
+	if (q < 0)
+		q = eps;
+	btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54;
+	// equation x^3 + q*x + r = 0
+	btScalar r2 = r * r;
+	btScalar q3 = q * q * q;
+	btScalar A, B;
+	if (r2 <= (q3 + eps))
+	{  //<<-- FIXED!
+		btScalar t = r / sqrt(q3);
+		if (t < -1)
+			t = -1;
+		if (t > 1)
+			t = 1;
+		t = acos(t);
+		a /= 3;
+		q = -2 * sqrt(q);
+		x[0] = q * cos(t / 3) - a;
+		x[1] = q * cos((t + TwoPi) / 3) - a;
+		x[2] = q * cos((t - TwoPi) / 3) - a;
+		return (3);
+	}
+	else
+	{
+		//A =-pow(fabs(r)+sqrt(r2-q3),1./3);
+		A = -root3(fabs(r) + sqrt(r2 - q3));
+		if (r < 0)
+			A = -A;
+		B = (A == 0 ? 0 : q / A);
+
+		a /= 3;
+		x[0] = (A + B) - a;
+		x[1] = -0.5 * (A + B) - a;
+		x[2] = 0.5 * sqrt(3.) * (A - B);
+		if (fabs(x[2]) < eps)
+		{
+			x[2] = x[1];
+			return (2);
+		}
+		return (1);
+	}
+}  // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) {
+//---------------------------------------------------------------------------
+// a>=0!
+void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b)  // returns:  a+i*s = sqrt(x+i*y)
+{
+	btScalar r = sqrt(x * x + y * y);
+	if (y == 0)
+	{
+		r = sqrt(r);
+		if (x >= 0)
+		{
+			a = r;
+			b = 0;
+		}
+		else
+		{
+			a = 0;
+			b = r;
+		}
+	}
+	else
+	{  // y != 0
+		a = sqrt(0.5 * (x + r));
+		b = 0.5 * y / a;
+	}
+}
+//---------------------------------------------------------------------------
+int SolveP4Bi(btScalar* x, btScalar b, btScalar d)  // solve equation x^4 + b*x^2 + d = 0
+{
+	btScalar D = b * b - 4 * d;
+	if (D >= 0)
+	{
+		btScalar sD = sqrt(D);
+		btScalar x1 = (-b + sD) / 2;
+		btScalar x2 = (-b - sD) / 2;  // x2 <= x1
+		if (x2 >= 0)                  // 0 <= x2 <= x1, 4 real roots
+		{
+			btScalar sx1 = sqrt(x1);
+			btScalar sx2 = sqrt(x2);
+			x[0] = -sx1;
+			x[1] = sx1;
+			x[2] = -sx2;
+			x[3] = sx2;
+			return 4;
+		}
+		if (x1 < 0)  // x2 <= x1 < 0, two pair of imaginary roots
+		{
+			btScalar sx1 = sqrt(-x1);
+			btScalar sx2 = sqrt(-x2);
+			x[0] = 0;
+			x[1] = sx1;
+			x[2] = 0;
+			x[3] = sx2;
+			return 0;
+		}
+		// now x2 < 0 <= x1 , two real roots and one pair of imginary root
+		btScalar sx1 = sqrt(x1);
+		btScalar sx2 = sqrt(-x2);
+		x[0] = -sx1;
+		x[1] = sx1;
+		x[2] = 0;
+		x[3] = sx2;
+		return 2;
+	}
+	else
+	{  // if( D < 0 ), two pair of compex roots
+		btScalar sD2 = 0.5 * sqrt(-D);
+		CSqrt(-0.5 * b, sD2, x[0], x[1]);
+		CSqrt(-0.5 * b, -sD2, x[2], x[3]);
+		return 0;
+	}  // if( D>=0 )
+}  // SolveP4Bi(btScalar *x, btScalar b, btScalar d)    // solve equation x^4 + b*x^2 d
+//---------------------------------------------------------------------------
+#define SWAP(a, b) \
+	{              \
+		t = b;     \
+		b = a;     \
+		a = t;     \
+	}
+static void dblSort3(btScalar& a, btScalar& b, btScalar& c)  // make: a <= b <= c
+{
+	btScalar t;
+	if (a > b)
+		SWAP(a, b);  // now a<=b
+	if (c < b)
+	{
+		SWAP(b, c);  // now a<=b, b<=c
+		if (a > b)
+			SWAP(a, b);  // now a<=b
+	}
+}
+//---------------------------------------------------------------------------
+int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d)  // solve equation x^4 + b*x^2 + c*x + d
+{
+	//if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0
+	if (fabs(c) < 1e-14 * (fabs(b) + fabs(d)))
+		return SolveP4Bi(x, b, d);  // After that, c!=0
+
+	int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c);  // solve resolvent
+	// by Viet theorem:  x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0
+	if (res3 > 1)  // 3 real roots,
+	{
+		dblSort3(x[0], x[1], x[2]);  // sort roots to x[0] <= x[1] <= x[2]
+		// Note: x[0]*x[1]*x[2]= c*c > 0
+		if (x[0] > 0)  // all roots are positive
+		{
+			btScalar sz1 = sqrt(x[0]);
+			btScalar sz2 = sqrt(x[1]);
+			btScalar sz3 = sqrt(x[2]);
+			// Note: sz1*sz2*sz3= -c (and not equal to 0)
+			if (c > 0)
+			{
+				x[0] = (-sz1 - sz2 - sz3) / 2;
+				x[1] = (-sz1 + sz2 + sz3) / 2;
+				x[2] = (+sz1 - sz2 + sz3) / 2;
+				x[3] = (+sz1 + sz2 - sz3) / 2;
+				return 4;
+			}
+			// now: c<0
+			x[0] = (-sz1 - sz2 + sz3) / 2;
+			x[1] = (-sz1 + sz2 - sz3) / 2;
+			x[2] = (+sz1 - sz2 - sz3) / 2;
+			x[3] = (+sz1 + sz2 + sz3) / 2;
+			return 4;
+		}  // if( x[0] > 0) // all roots are positive
+		// now x[0] <= x[1] < 0, x[2] > 0
+		// two pair of comlex roots
+		btScalar sz1 = sqrt(-x[0]);
+		btScalar sz2 = sqrt(-x[1]);
+		btScalar sz3 = sqrt(x[2]);
+
+		if (c > 0)  // sign = -1
+		{
+			x[0] = -sz3 / 2;
+			x[1] = (sz1 - sz2) / 2;  // x[0]i*x[1]
+			x[2] = sz3 / 2;
+			x[3] = (-sz1 - sz2) / 2;  // x[2]i*x[3]
+			return 0;
+		}
+		// now: c<0 , sign = +1
+		x[0] = sz3 / 2;
+		x[1] = (-sz1 + sz2) / 2;
+		x[2] = -sz3 / 2;
+		x[3] = (sz1 + sz2) / 2;
+		return 0;
+	}  // if( res3>1 )    // 3 real roots,
+	// now resoventa have 1 real and pair of compex roots
+	// x[0] - real root, and x[0]>0,
+	// x[1]i*x[2] - complex roots,
+	// x[0] must be >=0. But one times x[0]=~ 1e-17, so:
+	if (x[0] < 0)
+		x[0] = 0;
+	btScalar sz1 = sqrt(x[0]);
+	btScalar szr, szi;
+	CSqrt(x[1], x[2], szr, szi);  // (szr+i*szi)^2 = x[1]+i*x[2]
+	if (c > 0)                    // sign = -1
+	{
+		x[0] = -sz1 / 2 - szr;  // 1st real root
+		x[1] = -sz1 / 2 + szr;  // 2nd real root
+		x[2] = sz1 / 2;
+		x[3] = szi;
+		return 2;
+	}
+	// now: c<0 , sign = +1
+	x[0] = sz1 / 2 - szr;  // 1st real root
+	x[1] = sz1 / 2 + szr;  // 2nd real root
+	x[2] = -sz1 / 2;
+	x[3] = szi;
+	return 2;
+}  // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d)    // solve equation x^4 + b*x^2 + c*x + d
+//-----------------------------------------------------------------------------
+btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d)  // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
+{
+	btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c;  // f'(x)
+	if (fxs == 0)
+		return x;                                       //return 1e99; <<-- FIXED!
+	btScalar fx = (((x + a) * x + b) * x + c) * x + d;  // f(x)
+	return x - fx / fxs;
+}
+//-----------------------------------------------------------------------------
+// x - array of size 4
+// return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots
+// return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3],
+// return 0: two pair of complex roots: x[0]i*x[1],  x[2]i*x[3],
+int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d)
+{  // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method
+	// move to a=0:
+	btScalar d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c);
+	btScalar c1 = c + 0.5 * a * (0.25 * a * a - b);
+	btScalar b1 = b - 0.375 * a * a;
+	int res = SolveP4De(x, b1, c1, d1);
+	if (res == 4)
+	{
+		x[0] -= a / 4;
+		x[1] -= a / 4;
+		x[2] -= a / 4;
+		x[3] -= a / 4;
+	}
+	else if (res == 2)
+	{
+		x[0] -= a / 4;
+		x[1] -= a / 4;
+		x[2] -= a / 4;
+	}
+	else
+	{
+		x[0] -= a / 4;
+		x[2] -= a / 4;
+	}
+	// one Newton step for each real root:
+	if (res > 0)
+	{
+		x[0] = N4Step(x[0], a, b, c, d);
+		x[1] = N4Step(x[1], a, b, c, d);
+	}
+	if (res > 2)
+	{
+		x[2] = N4Step(x[2], a, b, c, d);
+		x[3] = N4Step(x[3], a, b, c, d);
+	}
+	return res;
+}
+//-----------------------------------------------------------------------------
+#define F5(t) (((((t + a) * t + b) * t + c) * t + d) * t + e)
+//-----------------------------------------------------------------------------
+btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e)  // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
+{
+	int cnt;
+	if (fabs(e) < eps)
+		return 0;
+
+	btScalar brd = fabs(a);  // brd - border of real roots
+	if (fabs(b) > brd)
+		brd = fabs(b);
+	if (fabs(c) > brd)
+		brd = fabs(c);
+	if (fabs(d) > brd)
+		brd = fabs(d);
+	if (fabs(e) > brd)
+		brd = fabs(e);
+	brd++;  // brd - border of real roots
+
+	btScalar x0, f0;       // less than root
+	btScalar x1, f1;       // greater than root
+	btScalar x2, f2, f2s;  // next values, f(x2), f'(x2)
+	btScalar dx = 0;
+
+	if (e < 0)
+	{
+		x0 = 0;
+		x1 = brd;
+		f0 = e;
+		f1 = F5(x1);
+		x2 = 0.01 * brd;
+	}  // positive root
+	else
+	{
+		x0 = -brd;
+		x1 = 0;
+		f0 = F5(x0);
+		f1 = e;
+		x2 = -0.01 * brd;
+	}  // negative root
+
+	if (fabs(f0) < eps)
+		return x0;
+	if (fabs(f1) < eps)
+		return x1;
+
+	// now x0<x1, f(x0)<0, f(x1)>0
+	// Firstly 10 bisections
+	for (cnt = 0; cnt < 10; cnt++)
+	{
+		x2 = (x0 + x1) / 2;  // next point
+		//x2 = x0 - f0*(x1 - x0) / (f1 - f0);        // next point
+		f2 = F5(x2);  // f(x2)
+		if (fabs(f2) < eps)
+			return x2;
+		if (f2 > 0)
+		{
+			x1 = x2;
+			f1 = f2;
+		}
+		else
+		{
+			x0 = x2;
+			f0 = f2;
+		}
+	}
+
+	// At each step:
+	// x0<x1, f(x0)<0, f(x1)>0.
+	// x2 - next value
+	// we hope that x0 < x2 < x1, but not necessarily
+	do
+	{
+		if (cnt++ > 50)
+			break;
+		if (x2 <= x0 || x2 >= x1)
+			x2 = (x0 + x1) / 2;  // now  x0 < x2 < x1
+		f2 = F5(x2);             // f(x2)
+		if (fabs(f2) < eps)
+			return x2;
+		if (f2 > 0)
+		{
+			x1 = x2;
+			f1 = f2;
+		}
+		else
+		{
+			x0 = x2;
+			f0 = f2;
+		}
+		f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d;  // f'(x2)
+		if (fabs(f2s) < eps)
+		{
+			x2 = 1e99;
+			continue;
+		}
+		dx = f2 / f2s;
+		x2 -= dx;
+	} while (fabs(dx) > eps);
+	return x2;
+}  // SolveP5_1(btScalar a,btScalar b,btScalar c,btScalar d,btScalar e)    // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
+//-----------------------------------------------------------------------------
+int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e)  // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
+{
+	btScalar r = x[0] = SolveP5_1(a, b, c, d, e);
+	btScalar a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1;
+	return 1 + SolveP4(x + 1, a1, b1, c1, d1);
+}  // SolveP5(btScalar *x,btScalar a,btScalar b,btScalar c,btScalar d,btScalar e)    // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
+//-----------------------------------------------------------------------------
diff --git a/extern/bullet2/src/BulletSoftBody/poly34.h b/extern/bullet2/src/BulletSoftBody/poly34.h
new file mode 100644
index 00000000000..35a52c5fecf
--- /dev/null
+++ b/extern/bullet2/src/BulletSoftBody/poly34.h
@@ -0,0 +1,38 @@
+// poly34.h : solution of cubic and quartic equation
+// (c) Khashin S.I. http://math.ivanovo.ac.ru/dalgebra/Khashin/index.html
+// khash2 (at) gmail.com
+
+#ifndef POLY_34
+#define POLY_34
+#include "LinearMath/btScalar.h"
+// x - array of size 2
+// return 2: 2 real roots x[0], x[1]
+// return 0: pair of complex roots: x[0]i*x[1]
+int SolveP2(btScalar* x, btScalar a, btScalar b);  // solve equation x^2 + a*x + b = 0
+
+// x - array of size 3
+// return 3: 3 real roots x[0], x[1], x[2]
+// return 1: 1 real root x[0] and pair of complex roots: x[1]i*x[2]
+int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c);  // solve cubic equation x^3 + a*x^2 + b*x + c = 0
+
+// x - array of size 4
+// return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots
+// return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3],
+// return 0: two pair of complex roots: x[0]i*x[1],  x[2]i*x[3],
+int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d);  // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method
+
+// x - array of size 5
+// return 5: 5 real roots x[0], x[1], x[2], x[3], x[4], possible multiple roots
+// return 3: 3 real roots x[0], x[1], x[2] and complex x[3]i*x[4],
+// return 1: 1 real root x[0] and two pair of complex roots: x[1]i*x[2],  x[3]i*x[4],
+int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e);  // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
+
+//-----------------------------------------------------------------------------
+// And some additional functions for internal use.
+// Your may remove this definitions from here
+int SolveP4Bi(btScalar* x, btScalar b, btScalar d);                              // solve equation x^4 + b*x^2 + d = 0
+int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d);                  // solve equation x^4 + b*x^2 + c*x + d = 0
+void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b);                    // returns as a+i*s,  sqrt(x+i*y)
+btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d);     // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
+btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e);  // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
+#endif
diff --git a/extern/bullet2/src/LinearMath/TaskScheduler/btTaskScheduler.cpp b/extern/bullet2/src/LinearMath/TaskScheduler/btTaskScheduler.cpp
new file mode 100644
index 00000000000..5f1115c4027
--- /dev/null
+++ b/extern/bullet2/src/LinearMath/TaskScheduler/btTaskScheduler.cpp
@@ -0,0 +1,792 @@
+
+#include "LinearMath/btMinMax.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btThreads.h"
+#include "LinearMath/btQuickprof.h"
+#include <stdio.h>
+#include <algorithm>
+
+#if BT_THREADSAFE
+
+#include "btThreadSupportInterface.h"
+
+#if defined(_WIN32)
+
+#define WIN32_LEAN_AND_MEAN
+
+#include <windows.h>
+
+#endif
+
+typedef unsigned long long btU64;
+static const int kCacheLineSize = 64;
+
+void btSpinPause()
+{
+#if defined(_WIN32)
+	YieldProcessor();
+#endif
+}
+
+struct WorkerThreadStatus
+{
+	enum Type
+	{
+		kInvalid,
+		kWaitingForWork,
+		kWorking,
+		kSleeping,
+	};
+};
+
+ATTRIBUTE_ALIGNED64(class)
+WorkerThreadDirectives
+{
+	static const int kMaxThreadCount = BT_MAX_THREAD_COUNT;
+	// directives for all worker threads packed into a single cacheline
+	char m_threadDirs[kMaxThreadCount];
+
+public:
+	enum Type
+	{
+		kInvalid,
+		kGoToSleep,         // go to sleep
+		kStayAwakeButIdle,  // wait for not checking job queue
+		kScanForJobs,       // actively scan job queue for jobs
+	};
+	WorkerThreadDirectives()
+	{
+		for (int i = 0; i < kMaxThreadCount; ++i)
+		{
+			m_threadDirs[i] = 0;
+		}
+	}
+
+	Type getDirective(int threadId)
+	{
+		btAssert(threadId < kMaxThreadCount);
+		return static_cast<Type>(m_threadDirs[threadId]);
+	}
+
+	void setDirectiveByRange(int threadBegin, int threadEnd, Type dir)
+	{
+		btAssert(threadBegin < threadEnd);
+		btAssert(threadEnd <= kMaxThreadCount);
+		char dirChar = static_cast<char>(dir);
+		for (int i = threadBegin; i < threadEnd; ++i)
+		{
+			m_threadDirs[i] = dirChar;
+		}
+	}
+};
+
+class JobQueue;
+
+ATTRIBUTE_ALIGNED64(struct)
+ThreadLocalStorage
+{
+	int m_threadId;
+	WorkerThreadStatus::Type m_status;
+	int m_numJobsFinished;
+	btSpinMutex m_mutex;
+	btScalar m_sumResult;
+	WorkerThreadDirectives* m_directive;
+	JobQueue* m_queue;
+	btClock* m_clock;
+	unsigned int m_cooldownTime;
+};
+
+struct IJob
+{
+	virtual void executeJob(int threadId) = 0;
+};
+
+class ParallelForJob : public IJob
+{
+	const btIParallelForBody* m_body;
+	int m_begin;
+	int m_end;
+
+public:
+	ParallelForJob(int iBegin, int iEnd, const btIParallelForBody& body)
+	{
+		m_body = &body;
+		m_begin = iBegin;
+		m_end = iEnd;
+	}
+	virtual void executeJob(int threadId) BT_OVERRIDE
+	{
+		BT_PROFILE("executeJob");
+
+		// call the functor body to do the work
+		m_body->forLoop(m_begin, m_end);
+	}
+};
+
+class ParallelSumJob : public IJob
+{
+	const btIParallelSumBody* m_body;
+	ThreadLocalStorage* m_threadLocalStoreArray;
+	int m_begin;
+	int m_end;
+
+public:
+	ParallelSumJob(int iBegin, int iEnd, const btIParallelSumBody& body, ThreadLocalStorage* tls)
+	{
+		m_body = &body;
+		m_threadLocalStoreArray = tls;
+		m_begin = iBegin;
+		m_end = iEnd;
+	}
+	virtual void executeJob(int threadId) BT_OVERRIDE
+	{
+		BT_PROFILE("executeJob");
+
+		// call the functor body to do the work
+		btScalar val = m_body->sumLoop(m_begin, m_end);
+#if BT_PARALLEL_SUM_DETERMINISTISM
+		// by truncating bits of the result, we can make the parallelSum deterministic (at the expense of precision)
+		const float TRUNC_SCALE = float(1 << 19);
+		val = floor(val * TRUNC_SCALE + 0.5f) / TRUNC_SCALE;  // truncate some bits
+#endif
+		m_threadLocalStoreArray[threadId].m_sumResult += val;
+	}
+};
+
+ATTRIBUTE_ALIGNED64(class)
+JobQueue
+{
+	btThreadSupportInterface* m_threadSupport;
+	btCriticalSection* m_queueLock;
+	btSpinMutex m_mutex;
+
+	btAlignedObjectArray<IJob*> m_jobQueue;
+	char* m_jobMem;
+	int m_jobMemSize;
+	bool m_queueIsEmpty;
+	int m_tailIndex;
+	int m_headIndex;
+	int m_allocSize;
+	bool m_useSpinMutex;
+	btAlignedObjectArray<JobQueue*> m_neighborContexts;
+	char m_cachePadding[kCacheLineSize];  // prevent false sharing
+
+	void freeJobMem()
+	{
+		if (m_jobMem)
+		{
+			// free old
+			btAlignedFree(m_jobMem);
+			m_jobMem = NULL;
+		}
+	}
+	void resizeJobMem(int newSize)
+	{
+		if (newSize > m_jobMemSize)
+		{
+			freeJobMem();
+			m_jobMem = static_cast<char*>(btAlignedAlloc(newSize, kCacheLineSize));
+			m_jobMemSize = newSize;
+		}
+	}
+
+public:
+	JobQueue()
+	{
+		m_jobMem = NULL;
+		m_jobMemSize = 0;
+		m_threadSupport = NULL;
+		m_queueLock = NULL;
+		m_headIndex = 0;
+		m_tailIndex = 0;
+		m_useSpinMutex = false;
+	}
+	~JobQueue()
+	{
+		exit();
+	}
+	void exit()
+	{
+		freeJobMem();
+		if (m_queueLock && m_threadSupport)
+		{
+			m_threadSupport->deleteCriticalSection(m_queueLock);
+			m_queueLock = NULL;
+			m_threadSupport = 0;
+		}
+	}
+
+	void init(btThreadSupportInterface * threadSup, btAlignedObjectArray<JobQueue> * contextArray)
+	{
+		m_threadSupport = threadSup;
+		if (threadSup)
+		{
+			m_queueLock = m_threadSupport->createCriticalSection();
+		}
+		setupJobStealing(contextArray, contextArray->size());
+	}
+	void setupJobStealing(btAlignedObjectArray<JobQueue> * contextArray, int numActiveContexts)
+	{
+		btAlignedObjectArray<JobQueue>& contexts = *contextArray;
+		int selfIndex = 0;
+		for (int i = 0; i < contexts.size(); ++i)
+		{
+			if (this == &contexts[i])
+			{
+				selfIndex = i;
+				break;
+			}
+		}
+		int numNeighbors = btMin(2, contexts.size() - 1);
+		int neighborOffsets[] = {-1, 1, -2, 2, -3, 3};
+		int numOffsets = sizeof(neighborOffsets) / sizeof(neighborOffsets[0]);
+		m_neighborContexts.reserve(numNeighbors);
+		m_neighborContexts.resizeNoInitialize(0);
+		for (int i = 0; i < numOffsets && m_neighborContexts.size() < numNeighbors; i++)
+		{
+			int neighborIndex = selfIndex + neighborOffsets[i];
+			if (neighborIndex >= 0 && neighborIndex < numActiveContexts)
+			{
+				m_neighborContexts.push_back(&contexts[neighborIndex]);
+			}
+		}
+	}
+
+	bool isQueueEmpty() const { return m_queueIsEmpty; }
+	void lockQueue()
+	{
+		if (m_useSpinMutex)
+		{
+			m_mutex.lock();
+		}
+		else
+		{
+			m_queueLock->lock();
+		}
+	}
+	void unlockQueue()
+	{
+		if (m_useSpinMutex)
+		{
+			m_mutex.unlock();
+		}
+		else
+		{
+			m_queueLock->unlock();
+		}
+	}
+	void clearQueue(int jobCount, int jobSize)
+	{
+		lockQueue();
+		m_headIndex = 0;
+		m_tailIndex = 0;
+		m_allocSize = 0;
+		m_queueIsEmpty = true;
+		int jobBufSize = jobSize * jobCount;
+		// make sure we have enough memory allocated to store jobs
+		if (jobBufSize > m_jobMemSize)
+		{
+			resizeJobMem(jobBufSize);
+		}
+		// make sure job queue is big enough
+		if (jobCount > m_jobQueue.capacity())
+		{
+			m_jobQueue.reserve(jobCount);
+		}
+		unlockQueue();
+		m_jobQueue.resizeNoInitialize(0);
+	}
+	void* allocJobMem(int jobSize)
+	{
+		btAssert(m_jobMemSize >= (m_allocSize + jobSize));
+		void* jobMem = &m_jobMem[m_allocSize];
+		m_allocSize += jobSize;
+		return jobMem;
+	}
+	void submitJob(IJob * job)
+	{
+		btAssert(reinterpret_cast<char*>(job) >= &m_jobMem[0] && reinterpret_cast<char*>(job) < &m_jobMem[0] + m_allocSize);
+		m_jobQueue.push_back(job);
+		lockQueue();
+		m_tailIndex++;
+		m_queueIsEmpty = false;
+		unlockQueue();
+	}
+	IJob* consumeJobFromOwnQueue()
+	{
+		if (m_queueIsEmpty)
+		{
+			// lock free path. even if this is taken erroneously it isn't harmful
+			return NULL;
+		}
+		IJob* job = NULL;
+		lockQueue();
+		if (!m_queueIsEmpty)
+		{
+			job = m_jobQueue[m_headIndex++];
+			btAssert(reinterpret_cast<char*>(job) >= &m_jobMem[0] && reinterpret_cast<char*>(job) < &m_jobMem[0] + m_allocSize);
+			if (m_headIndex == m_tailIndex)
+			{
+				m_queueIsEmpty = true;
+			}
+		}
+		unlockQueue();
+		return job;
+	}
+	IJob* consumeJob()
+	{
+		if (IJob* job = consumeJobFromOwnQueue())
+		{
+			return job;
+		}
+		// own queue is empty, try to steal from neighbor
+		for (int i = 0; i < m_neighborContexts.size(); ++i)
+		{
+			JobQueue* otherContext = m_neighborContexts[i];
+			if (IJob* job = otherContext->consumeJobFromOwnQueue())
+			{
+				return job;
+			}
+		}
+		return NULL;
+	}
+};
+
+static void WorkerThreadFunc(void* userPtr)
+{
+	BT_PROFILE("WorkerThreadFunc");
+	ThreadLocalStorage* localStorage = (ThreadLocalStorage*)userPtr;
+	JobQueue* jobQueue = localStorage->m_queue;
+
+	bool shouldSleep = false;
+	int threadId = localStorage->m_threadId;
+	while (!shouldSleep)
+	{
+		// do work
+		localStorage->m_mutex.lock();
+		while (IJob* job = jobQueue->consumeJob())
+		{
+			localStorage->m_status = WorkerThreadStatus::kWorking;
+			job->executeJob(threadId);
+			localStorage->m_numJobsFinished++;
+		}
+		localStorage->m_status = WorkerThreadStatus::kWaitingForWork;
+		localStorage->m_mutex.unlock();
+		btU64 clockStart = localStorage->m_clock->getTimeMicroseconds();
+		// while queue is empty,
+		while (jobQueue->isQueueEmpty())
+		{
+			// todo: spin wait a bit to avoid hammering the empty queue
+			btSpinPause();
+			if (localStorage->m_directive->getDirective(threadId) == WorkerThreadDirectives::kGoToSleep)
+			{
+				shouldSleep = true;
+				break;
+			}
+			// if jobs are incoming,
+			if (localStorage->m_directive->getDirective(threadId) == WorkerThreadDirectives::kScanForJobs)
+			{
+				clockStart = localStorage->m_clock->getTimeMicroseconds();  // reset clock
+			}
+			else
+			{
+				for (int i = 0; i < 50; ++i)
+				{
+					btSpinPause();
+					btSpinPause();
+					btSpinPause();
+					btSpinPause();
+					if (localStorage->m_directive->getDirective(threadId) == WorkerThreadDirectives::kScanForJobs || !jobQueue->isQueueEmpty())
+					{
+						break;
+					}
+				}
+				// if no jobs incoming and queue has been empty for the cooldown time, sleep
+				btU64 timeElapsed = localStorage->m_clock->getTimeMicroseconds() - clockStart;
+				if (timeElapsed > localStorage->m_cooldownTime)
+				{
+					shouldSleep = true;
+					break;
+				}
+			}
+		}
+	}
+	{
+		BT_PROFILE("sleep");
+		// go sleep
+		localStorage->m_mutex.lock();
+		localStorage->m_status = WorkerThreadStatus::kSleeping;
+		localStorage->m_mutex.unlock();
+	}
+}
+
+class btTaskSchedulerDefault : public btITaskScheduler
+{
+	btThreadSupportInterface* m_threadSupport;
+	WorkerThreadDirectives* m_workerDirective;
+	btAlignedObjectArray<JobQueue> m_jobQueues;
+	btAlignedObjectArray<JobQueue*> m_perThreadJobQueues;
+	btAlignedObjectArray<ThreadLocalStorage> m_threadLocalStorage;
+	btSpinMutex m_antiNestingLock;  // prevent nested parallel-for
+	btClock m_clock;
+	int m_numThreads;
+	int m_numWorkerThreads;
+	int m_numActiveJobQueues;
+	int m_maxNumThreads;
+	int m_numJobs;
+	static const int kFirstWorkerThreadId = 1;
+
+public:
+	btTaskSchedulerDefault() : btITaskScheduler("ThreadSupport")
+	{
+		m_threadSupport = NULL;
+		m_workerDirective = NULL;
+	}
+
+	virtual ~btTaskSchedulerDefault()
+	{
+		waitForWorkersToSleep();
+
+		for (int i = 0; i < m_jobQueues.size(); ++i)
+		{
+			m_jobQueues[i].exit();
+		}
+
+		if (m_threadSupport)
+		{
+			delete m_threadSupport;
+			m_threadSupport = NULL;
+		}
+		if (m_workerDirective)
+		{
+			btAlignedFree(m_workerDirective);
+			m_workerDirective = NULL;
+		}
+	}
+
+	void init()
+	{
+		btThreadSupportInterface::ConstructionInfo constructionInfo("TaskScheduler", WorkerThreadFunc);
+		m_threadSupport = btThreadSupportInterface::create(constructionInfo);
+		m_workerDirective = static_cast<WorkerThreadDirectives*>(btAlignedAlloc(sizeof(*m_workerDirective), 64));
+
+		m_numWorkerThreads = m_threadSupport->getNumWorkerThreads();
+		m_maxNumThreads = m_threadSupport->getNumWorkerThreads() + 1;
+		m_numThreads = m_maxNumThreads;
+		// ideal to have one job queue for each physical processor (except for the main thread which needs no queue)
+		int numThreadsPerQueue = m_threadSupport->getLogicalToPhysicalCoreRatio();
+		int numJobQueues = (numThreadsPerQueue == 1) ? (m_maxNumThreads - 1) : (m_maxNumThreads / numThreadsPerQueue);
+		m_jobQueues.resize(numJobQueues);
+		m_numActiveJobQueues = numJobQueues;
+		for (int i = 0; i < m_jobQueues.size(); ++i)
+		{
+			m_jobQueues[i].init(m_threadSupport, &m_jobQueues);
+		}
+		m_perThreadJobQueues.resize(m_numThreads);
+		for (int i = 0; i < m_numThreads; i++)
+		{
+			JobQueue* jq = NULL;
+			// only worker threads get a job queue
+			if (i > 0)
+			{
+				if (numThreadsPerQueue == 1)
+				{
+					// one queue per worker thread
+					jq = &m_jobQueues[i - kFirstWorkerThreadId];
+				}
+				else
+				{
+					// 2 threads share each queue
+					jq = &m_jobQueues[i / numThreadsPerQueue];
+				}
+			}
+			m_perThreadJobQueues[i] = jq;
+		}
+		m_threadLocalStorage.resize(m_numThreads);
+		for (int i = 0; i < m_numThreads; i++)
+		{
+			ThreadLocalStorage& storage = m_threadLocalStorage[i];
+			storage.m_threadId = i;
+			storage.m_directive = m_workerDirective;
+			storage.m_status = WorkerThreadStatus::kSleeping;
+			storage.m_cooldownTime = 100;  // 100 microseconds, threads go to sleep after this long if they have nothing to do
+			storage.m_clock = &m_clock;
+			storage.m_queue = m_perThreadJobQueues[i];
+		}
+		setWorkerDirectives(WorkerThreadDirectives::kGoToSleep);  // no work for them yet
+		setNumThreads(m_threadSupport->getCacheFriendlyNumThreads());
+	}
+
+	void setWorkerDirectives(WorkerThreadDirectives::Type dir)
+	{
+		m_workerDirective->setDirectiveByRange(kFirstWorkerThreadId, m_numThreads, dir);
+	}
+
+	virtual int getMaxNumThreads() const BT_OVERRIDE
+	{
+		return m_maxNumThreads;
+	}
+
+	virtual int getNumThreads() const BT_OVERRIDE
+	{
+		return m_numThreads;
+	}
+
+	virtual void setNumThreads(int numThreads) BT_OVERRIDE
+	{
+		m_numThreads = btMax(btMin(numThreads, int(m_maxNumThreads)), 1);
+		m_numWorkerThreads = m_numThreads - 1;
+		m_numActiveJobQueues = 0;
+		// if there is at least 1 worker,
+		if (m_numWorkerThreads > 0)
+		{
+			// re-setup job stealing between queues to avoid attempting to steal from an inactive job queue
+			JobQueue* lastActiveContext = m_perThreadJobQueues[m_numThreads - 1];
+			int iLastActiveContext = lastActiveContext - &m_jobQueues[0];
+			m_numActiveJobQueues = iLastActiveContext + 1;
+			for (int i = 0; i < m_jobQueues.size(); ++i)
+			{
+				m_jobQueues[i].setupJobStealing(&m_jobQueues, m_numActiveJobQueues);
+			}
+		}
+		m_workerDirective->setDirectiveByRange(m_numThreads, BT_MAX_THREAD_COUNT, WorkerThreadDirectives::kGoToSleep);
+	}
+
+	void waitJobs()
+	{
+		BT_PROFILE("waitJobs");
+		// have the main thread work until the job queues are empty
+		int numMainThreadJobsFinished = 0;
+		for (int i = 0; i < m_numActiveJobQueues; ++i)
+		{
+			while (IJob* job = m_jobQueues[i].consumeJob())
+			{
+				job->executeJob(0);
+				numMainThreadJobsFinished++;
+			}
+		}
+
+		// done with jobs for now, tell workers to rest (but not sleep)
+		setWorkerDirectives(WorkerThreadDirectives::kStayAwakeButIdle);
+
+		btU64 clockStart = m_clock.getTimeMicroseconds();
+		// wait for workers to finish any jobs in progress
+		while (true)
+		{
+			int numWorkerJobsFinished = 0;
+			for (int iThread = kFirstWorkerThreadId; iThread < m_numThreads; ++iThread)
+			{
+				ThreadLocalStorage* storage = &m_threadLocalStorage[iThread];
+				storage->m_mutex.lock();
+				numWorkerJobsFinished += storage->m_numJobsFinished;
+				storage->m_mutex.unlock();
+			}
+			if (numWorkerJobsFinished + numMainThreadJobsFinished == m_numJobs)
+			{
+				break;
+			}
+			btU64 timeElapsed = m_clock.getTimeMicroseconds() - clockStart;
+			btAssert(timeElapsed < 1000);
+			if (timeElapsed > 100000)
+			{
+				break;
+			}
+			btSpinPause();
+		}
+	}
+
+	void wakeWorkers(int numWorkersToWake)
+	{
+		BT_PROFILE("wakeWorkers");
+		btAssert(m_workerDirective->getDirective(1) == WorkerThreadDirectives::kScanForJobs);
+		int numDesiredWorkers = btMin(numWorkersToWake, m_numWorkerThreads);
+		int numActiveWorkers = 0;
+		for (int iWorker = 0; iWorker < m_numWorkerThreads; ++iWorker)
+		{
+			// note this count of active workers is not necessarily totally reliable, because a worker thread could be
+			// just about to put itself to sleep. So we may on occasion fail to wake up all the workers. It should be rare.
+			ThreadLocalStorage& storage = m_threadLocalStorage[kFirstWorkerThreadId + iWorker];
+			if (storage.m_status != WorkerThreadStatus::kSleeping)
+			{
+				numActiveWorkers++;
+			}
+		}
+		for (int iWorker = 0; iWorker < m_numWorkerThreads && numActiveWorkers < numDesiredWorkers; ++iWorker)
+		{
+			ThreadLocalStorage& storage = m_threadLocalStorage[kFirstWorkerThreadId + iWorker];
+			if (storage.m_status == WorkerThreadStatus::kSleeping)
+			{
+				m_threadSupport->runTask(iWorker, &storage);
+				numActiveWorkers++;
+			}
+		}
+	}
+
+	void waitForWorkersToSleep()
+	{
+		BT_PROFILE("waitForWorkersToSleep");
+		setWorkerDirectives(WorkerThreadDirectives::kGoToSleep);
+		m_threadSupport->waitForAllTasks();
+		for (int i = kFirstWorkerThreadId; i < m_numThreads; i++)
+		{
+			ThreadLocalStorage& storage = m_threadLocalStorage[i];
+			btAssert(storage.m_status == WorkerThreadStatus::kSleeping);
+		}
+	}
+
+	virtual void sleepWorkerThreadsHint() BT_OVERRIDE
+	{
+		BT_PROFILE("sleepWorkerThreadsHint");
+		// hint the task scheduler that we may not be using these threads for a little while
+		setWorkerDirectives(WorkerThreadDirectives::kGoToSleep);
+	}
+
+	void prepareWorkerThreads()
+	{
+		for (int i = kFirstWorkerThreadId; i < m_numThreads; ++i)
+		{
+			ThreadLocalStorage& storage = m_threadLocalStorage[i];
+			storage.m_mutex.lock();
+			storage.m_numJobsFinished = 0;
+			storage.m_mutex.unlock();
+		}
+		setWorkerDirectives(WorkerThreadDirectives::kScanForJobs);
+	}
+
+	virtual void parallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody& body) BT_OVERRIDE
+	{
+		BT_PROFILE("parallelFor_ThreadSupport");
+		btAssert(iEnd >= iBegin);
+		btAssert(grainSize >= 1);
+		int iterationCount = iEnd - iBegin;
+		if (iterationCount > grainSize && m_numWorkerThreads > 0 && m_antiNestingLock.tryLock())
+		{
+			typedef ParallelForJob JobType;
+			int jobCount = (iterationCount + grainSize - 1) / grainSize;
+			m_numJobs = jobCount;
+			btAssert(jobCount >= 2);  // need more than one job for multithreading
+			int jobSize = sizeof(JobType);
+
+			for (int i = 0; i < m_numActiveJobQueues; ++i)
+			{
+				m_jobQueues[i].clearQueue(jobCount, jobSize);
+			}
+			// prepare worker threads for incoming work
+			prepareWorkerThreads();
+			// submit all of the jobs
+			int iJob = 0;
+			int iThread = kFirstWorkerThreadId;  // first worker thread
+			for (int i = iBegin; i < iEnd; i += grainSize)
+			{
+				btAssert(iJob < jobCount);
+				int iE = btMin(i + grainSize, iEnd);
+				JobQueue* jq = m_perThreadJobQueues[iThread];
+				btAssert(jq);
+				btAssert((jq - &m_jobQueues[0]) < m_numActiveJobQueues);
+				void* jobMem = jq->allocJobMem(jobSize);
+				JobType* job = new (jobMem) ParallelForJob(i, iE, body);  // placement new
+				jq->submitJob(job);
+				iJob++;
+				iThread++;
+				if (iThread >= m_numThreads)
+				{
+					iThread = kFirstWorkerThreadId;  // first worker thread
+				}
+			}
+			wakeWorkers(jobCount - 1);
+
+			// put the main thread to work on emptying the job queue and then wait for all workers to finish
+			waitJobs();
+			m_antiNestingLock.unlock();
+		}
+		else
+		{
+			BT_PROFILE("parallelFor_mainThread");
+			// just run on main thread
+			body.forLoop(iBegin, iEnd);
+		}
+	}
+	virtual btScalar parallelSum(int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body) BT_OVERRIDE
+	{
+		BT_PROFILE("parallelSum_ThreadSupport");
+		btAssert(iEnd >= iBegin);
+		btAssert(grainSize >= 1);
+		int iterationCount = iEnd - iBegin;
+		if (iterationCount > grainSize && m_numWorkerThreads > 0 && m_antiNestingLock.tryLock())
+		{
+			typedef ParallelSumJob JobType;
+			int jobCount = (iterationCount + grainSize - 1) / grainSize;
+			m_numJobs = jobCount;
+			btAssert(jobCount >= 2);  // need more than one job for multithreading
+			int jobSize = sizeof(JobType);
+			for (int i = 0; i < m_numActiveJobQueues; ++i)
+			{
+				m_jobQueues[i].clearQueue(jobCount, jobSize);
+			}
+
+			// initialize summation
+			for (int iThread = 0; iThread < m_numThreads; ++iThread)
+			{
+				m_threadLocalStorage[iThread].m_sumResult = btScalar(0);
+			}
+
+			// prepare worker threads for incoming work
+			prepareWorkerThreads();
+			// submit all of the jobs
+			int iJob = 0;
+			int iThread = kFirstWorkerThreadId;  // first worker thread
+			for (int i = iBegin; i < iEnd; i += grainSize)
+			{
+				btAssert(iJob < jobCount);
+				int iE = btMin(i + grainSize, iEnd);
+				JobQueue* jq = m_perThreadJobQueues[iThread];
+				btAssert(jq);
+				btAssert((jq - &m_jobQueues[0]) < m_numActiveJobQueues);
+				void* jobMem = jq->allocJobMem(jobSize);
+				JobType* job = new (jobMem) ParallelSumJob(i, iE, body, &m_threadLocalStorage[0]);  // placement new
+				jq->submitJob(job);
+				iJob++;
+				iThread++;
+				if (iThread >= m_numThreads)
+				{
+					iThread = kFirstWorkerThreadId;  // first worker thread
+				}
+			}
+			wakeWorkers(jobCount - 1);
+
+			// put the main thread to work on emptying the job queue and then wait for all workers to finish
+			waitJobs();
+
+			// add up all the thread sums
+			btScalar sum = btScalar(0);
+			for (int iThread = 0; iThread < m_numThreads; ++iThread)
+			{
+				sum += m_threadLocalStorage[iThread].m_sumResult;
+			}
+			m_antiNestingLock.unlock();
+			return sum;
+		}
+		else
+		{
+			BT_PROFILE("parallelSum_mainThread");
+			// just run on main thread
+			return body.sumLoop(iBegin, iEnd);
+		}
+	}
+};
+
+btITaskScheduler* btCreateDefaultTaskScheduler()
+{
+	btTaskSchedulerDefault* ts = new btTaskSchedulerDefault();
+	ts->init();
+	return ts;
+}
+
+#else  // #if BT_THREADSAFE
+
+btITaskScheduler* btCreateDefaultTaskScheduler()
+{
+	return NULL;
+}
+
+#endif  // #else // #if BT_THREADSAFE
diff --git a/extern/bullet2/src/LinearMath/TaskScheduler/btThreadSupportInterface.h b/extern/bullet2/src/LinearMath/TaskScheduler/btThreadSupportInterface.h
new file mode 100644
index 00000000000..1fe49335a19
--- /dev/null
+++ b/extern/bullet2/src/LinearMath/TaskScheduler/btThreadSupportInterface.h
@@ -0,0 +1,64 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2018 Erwin Coumans  http://bulletphysics.com
+
+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_THREAD_SUPPORT_INTERFACE_H
+#define BT_THREAD_SUPPORT_INTERFACE_H
+
+class btCriticalSection
+{
+public:
+	btCriticalSection() {}
+	virtual ~btCriticalSection() {}
+
+	virtual void lock() = 0;
+	virtual void unlock() = 0;
+};
+
+class btThreadSupportInterface
+{
+public:
+	virtual ~btThreadSupportInterface() {}
+
+	virtual int getNumWorkerThreads() const = 0;            // number of worker threads (total number of logical processors - 1)
+	virtual int getCacheFriendlyNumThreads() const = 0;     // the number of logical processors sharing a single L3 cache
+	virtual int getLogicalToPhysicalCoreRatio() const = 0;  // the number of logical processors per physical processor (usually 1 or 2)
+	virtual void runTask(int threadIndex, void* userData) = 0;
+	virtual void waitForAllTasks() = 0;
+
+	virtual btCriticalSection* createCriticalSection() = 0;
+	virtual void deleteCriticalSection(btCriticalSection* criticalSection) = 0;
+
+	typedef void (*ThreadFunc)(void* userPtr);
+
+	struct ConstructionInfo
+	{
+		ConstructionInfo(const char* uniqueName,
+						 ThreadFunc userThreadFunc,
+						 int threadStackSize = 65535)
+			: m_uniqueName(uniqueName),
+			  m_userThreadFunc(userThreadFunc),
+			  m_threadStackSize(threadStackSize)
+		{
+		}
+
+		const char* m_uniqueName;
+		ThreadFunc m_userThreadFunc;
+		int m_threadStackSize;
+	};
+
+	static btThreadSupportInterface* create(const ConstructionInfo& info);
+};
+
+#endif  //BT_THREAD_SUPPORT_INTERFACE_H
diff --git a/extern/bullet2/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp b/extern/bullet2/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp
new file mode 100644
index 00000000000..a03f6dc5703
--- /dev/null
+++ b/extern/bullet2/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp
@@ -0,0 +1,353 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2018 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+#if BT_THREADSAFE && !defined(_WIN32)
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btThreads.h"
+#include "LinearMath/btMinMax.h"
+#include "btThreadSupportInterface.h"
+
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+
+#ifndef _XOPEN_SOURCE
+#define _XOPEN_SOURCE 600  //for definition of pthread_barrier_t, see http://pages.cs.wisc.edu/~travitch/pthreads_primer.html
+#endif                     //_XOPEN_SOURCE
+#include <pthread.h>
+#include <semaphore.h>
+#include <unistd.h>  //for sysconf
+
+///
+/// getNumHardwareThreads()
+///
+///
+/// https://stackoverflow.com/questions/150355/programmatically-find-the-number-of-cores-on-a-machine
+///
+#if __cplusplus >= 201103L
+
+#include <thread>
+
+int btGetNumHardwareThreads()
+{
+	return btMax(1u, btMin(BT_MAX_THREAD_COUNT, std::thread::hardware_concurrency()));
+}
+
+#else
+
+int btGetNumHardwareThreads()
+{
+	return btMax(1, btMin<int>(BT_MAX_THREAD_COUNT, sysconf(_SC_NPROCESSORS_ONLN)));
+}
+
+#endif
+
+// btThreadSupportPosix helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
+class btThreadSupportPosix : public btThreadSupportInterface
+{
+public:
+	struct btThreadStatus
+	{
+		int m_taskId;
+		int m_commandId;
+		int m_status;
+
+		ThreadFunc m_userThreadFunc;
+		void* m_userPtr;  //for taskDesc etc
+
+		pthread_t thread;
+		//each tread will wait until this signal to start its work
+		sem_t* startSemaphore;
+		btCriticalSection* m_cs;
+		// this is a copy of m_mainSemaphore,
+		//each tread will signal once it is finished with its work
+		sem_t* m_mainSemaphore;
+		unsigned long threadUsed;
+	};
+
+private:
+	typedef unsigned long long UINT64;
+
+	btAlignedObjectArray<btThreadStatus> m_activeThreadStatus;
+	// m_mainSemaphoresemaphore will signal, if and how many threads are finished with their work
+	sem_t* m_mainSemaphore;
+	int m_numThreads;
+	UINT64 m_startedThreadsMask;
+	void startThreads(const ConstructionInfo& threadInfo);
+	void stopThreads();
+	int waitForResponse();
+	btCriticalSection* m_cs;
+public:
+	btThreadSupportPosix(const ConstructionInfo& threadConstructionInfo);
+	virtual ~btThreadSupportPosix();
+
+	virtual int getNumWorkerThreads() const BT_OVERRIDE { return m_numThreads; }
+	// TODO: return the number of logical processors sharing the first L3 cache
+	virtual int getCacheFriendlyNumThreads() const BT_OVERRIDE { return m_numThreads + 1; }
+	// TODO: detect if CPU has hyperthreading enabled
+	virtual int getLogicalToPhysicalCoreRatio() const BT_OVERRIDE { return 1; }
+
+	virtual void runTask(int threadIndex, void* userData) BT_OVERRIDE;
+	virtual void waitForAllTasks() BT_OVERRIDE;
+
+	virtual btCriticalSection* createCriticalSection() BT_OVERRIDE;
+	virtual void deleteCriticalSection(btCriticalSection* criticalSection) BT_OVERRIDE;
+};
+
+#define checkPThreadFunction(returnValue)                                                                 \
+	if (0 != returnValue)                                                                                 \
+	{                                                                                                     \
+		printf("PThread problem at line %i in file %s: %i %d\n", __LINE__, __FILE__, returnValue, errno); \
+	}
+
+// The number of threads should be equal to the number of available cores
+// Todo: each worker should be linked to a single core, using SetThreadIdealProcessor.
+
+btThreadSupportPosix::btThreadSupportPosix(const ConstructionInfo& threadConstructionInfo)
+{
+	m_cs = createCriticalSection();
+	startThreads(threadConstructionInfo);
+}
+
+// cleanup/shutdown Libspe2
+btThreadSupportPosix::~btThreadSupportPosix()
+{
+	stopThreads();
+	deleteCriticalSection(m_cs);
+	m_cs=0;
+}
+
+#if (defined(__APPLE__))
+#define NAMED_SEMAPHORES
+#endif
+
+static sem_t* createSem(const char* baseName)
+{
+	static int semCount = 0;
+#ifdef NAMED_SEMAPHORES
+	/// Named semaphore begin
+	char name[32];
+	snprintf(name, 32, "/%8.s-%4.d-%4.4d", baseName, getpid(), semCount++);
+	sem_t* tempSem = sem_open(name, O_CREAT, 0600, 0);
+
+	if (tempSem != reinterpret_cast<sem_t*>(SEM_FAILED))
+	{
+		//        printf("Created \"%s\" Semaphore %p\n", name, tempSem);
+	}
+	else
+	{
+		//printf("Error creating Semaphore %d\n", errno);
+		exit(-1);
+	}
+	/// Named semaphore end
+#else
+	sem_t* tempSem = new sem_t;
+	checkPThreadFunction(sem_init(tempSem, 0, 0));
+#endif
+	return tempSem;
+}
+
+static void destroySem(sem_t* semaphore)
+{
+#ifdef NAMED_SEMAPHORES
+	checkPThreadFunction(sem_close(semaphore));
+#else
+	checkPThreadFunction(sem_destroy(semaphore));
+	delete semaphore;
+#endif
+}
+
+static void* threadFunction(void* argument)
+{
+	btThreadSupportPosix::btThreadStatus* status = (btThreadSupportPosix::btThreadStatus*)argument;
+
+	while (1)
+	{
+		checkPThreadFunction(sem_wait(status->startSemaphore));
+		void* userPtr = status->m_userPtr;
+
+		if (userPtr)
+		{
+			btAssert(status->m_status);
+			status->m_userThreadFunc(userPtr);
+			status->m_cs->lock();
+			status->m_status = 2;
+			status->m_cs->unlock();
+			checkPThreadFunction(sem_post(status->m_mainSemaphore));
+			status->threadUsed++;
+		}
+		else
+		{
+			//exit Thread
+			status->m_cs->lock();
+			status->m_status = 3;
+			status->m_cs->unlock();
+			checkPThreadFunction(sem_post(status->m_mainSemaphore));
+			break;
+		}
+	}
+
+	return 0;
+}
+
+///send messages to SPUs
+void btThreadSupportPosix::runTask(int threadIndex, void* userData)
+{
+	///we should spawn an SPU task here, and in 'waitForResponse' it should wait for response of the (one of) the first tasks that finished
+	btThreadStatus& threadStatus = m_activeThreadStatus[threadIndex];
+	btAssert(threadIndex >= 0);
+	btAssert(threadIndex < m_activeThreadStatus.size());
+	threadStatus.m_cs = m_cs;
+	threadStatus.m_commandId = 1;
+	threadStatus.m_status = 1;
+	threadStatus.m_userPtr = userData;
+	m_startedThreadsMask |= UINT64(1) << threadIndex;
+
+	// fire event to start new task
+	checkPThreadFunction(sem_post(threadStatus.startSemaphore));
+}
+
+///check for messages from SPUs
+int btThreadSupportPosix::waitForResponse()
+{
+	///We should wait for (one of) the first tasks to finish (or other SPU messages), and report its response
+	///A possible response can be 'yes, SPU handled it', or 'no, please do a PPU fallback'
+
+	btAssert(m_activeThreadStatus.size());
+
+	// wait for any of the threads to finish
+	checkPThreadFunction(sem_wait(m_mainSemaphore));
+	// get at least one thread which has finished
+	size_t last = -1;
+
+	for (size_t t = 0; t < size_t(m_activeThreadStatus.size()); ++t)
+	{
+		m_cs->lock();
+		bool hasFinished = (2 == m_activeThreadStatus[t].m_status);
+		m_cs->unlock(); 
+		if (hasFinished)
+		{
+			last = t;
+			break;
+		}
+	}
+
+	btThreadStatus& threadStatus = m_activeThreadStatus[last];
+
+	btAssert(threadStatus.m_status > 1);
+	threadStatus.m_status = 0;
+
+	// need to find an active spu
+	btAssert(last >= 0);
+	m_startedThreadsMask &= ~(UINT64(1) << last);
+
+	return last;
+}
+
+void btThreadSupportPosix::waitForAllTasks()
+{
+	while (m_startedThreadsMask)
+	{
+		waitForResponse();
+	}
+}
+
+void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructionInfo)
+{
+	m_numThreads = btGetNumHardwareThreads() - 1;  // main thread exists already
+	m_activeThreadStatus.resize(m_numThreads);
+	m_startedThreadsMask = 0;
+
+	m_mainSemaphore = createSem("main");
+	//checkPThreadFunction(sem_wait(mainSemaphore));
+
+	for (int i = 0; i < m_numThreads; i++)
+	{
+		btThreadStatus& threadStatus = m_activeThreadStatus[i];
+		threadStatus.startSemaphore = createSem("threadLocal");
+		threadStatus.m_userPtr = 0;
+		threadStatus.m_cs = m_cs;
+		threadStatus.m_taskId = i;
+		threadStatus.m_commandId = 0;
+		threadStatus.m_status = 0;
+		threadStatus.m_mainSemaphore = m_mainSemaphore;
+		threadStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
+		threadStatus.threadUsed = 0;
+		checkPThreadFunction(pthread_create(&threadStatus.thread, NULL, &threadFunction, (void*)&threadStatus));
+
+	}
+}
+
+///tell the task scheduler we are done with the SPU tasks
+void btThreadSupportPosix::stopThreads()
+{
+	for (size_t t = 0; t < size_t(m_activeThreadStatus.size()); ++t)
+	{
+		btThreadStatus& threadStatus = m_activeThreadStatus[t];
+
+		threadStatus.m_userPtr = 0;
+		checkPThreadFunction(sem_post(threadStatus.startSemaphore));
+		checkPThreadFunction(sem_wait(m_mainSemaphore));
+
+		checkPThreadFunction(pthread_join(threadStatus.thread, 0));
+		destroySem(threadStatus.startSemaphore);
+	}
+	destroySem(m_mainSemaphore);
+	m_activeThreadStatus.clear();
+}
+
+class btCriticalSectionPosix : public btCriticalSection
+{
+	pthread_mutex_t m_mutex;
+
+public:
+	btCriticalSectionPosix()
+	{
+		pthread_mutex_init(&m_mutex, NULL);
+	}
+	virtual ~btCriticalSectionPosix()
+	{
+		pthread_mutex_destroy(&m_mutex);
+	}
+
+	virtual void lock()
+	{
+		pthread_mutex_lock(&m_mutex);
+	}
+	virtual void unlock()
+	{
+		pthread_mutex_unlock(&m_mutex);
+	}
+};
+
+btCriticalSection* btThreadSupportPosix::createCriticalSection()
+{
+	return new btCriticalSectionPosix();
+}
+
+void btThreadSupportPosix::deleteCriticalSection(btCriticalSection* cs)
+{
+	delete cs;
+}
+
+btThreadSupportInterface* btThreadSupportInterface::create(const ConstructionInfo& info)
+{
+	return new btThreadSupportPosix(info);
+}
+
+#endif  // BT_THREADSAFE && !defined( _WIN32 )
diff --git a/extern/bullet2/src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp b/extern/bullet2/src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp
new file mode 100644
index 00000000000..922e449cce2
--- /dev/null
+++ b/extern/bullet2/src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp
@@ -0,0 +1,452 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2018 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+#if defined(_WIN32) && BT_THREADSAFE
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btMinMax.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btThreads.h"
+#include "btThreadSupportInterface.h"
+#include <windows.h>
+#include <stdio.h>
+
+struct btProcessorInfo
+{
+	int numLogicalProcessors;
+	int numCores;
+	int numNumaNodes;
+	int numL1Cache;
+	int numL2Cache;
+	int numL3Cache;
+	int numPhysicalPackages;
+	static const int maxNumTeamMasks = 32;
+	int numTeamMasks;
+	UINT64 processorTeamMasks[maxNumTeamMasks];
+};
+
+UINT64 getProcessorTeamMask(const btProcessorInfo& procInfo, int procId)
+{
+	UINT64 procMask = UINT64(1) << procId;
+	for (int i = 0; i < procInfo.numTeamMasks; ++i)
+	{
+		if (procMask & procInfo.processorTeamMasks[i])
+		{
+			return procInfo.processorTeamMasks[i];
+		}
+	}
+	return 0;
+}
+
+int getProcessorTeamIndex(const btProcessorInfo& procInfo, int procId)
+{
+	UINT64 procMask = UINT64(1) << procId;
+	for (int i = 0; i < procInfo.numTeamMasks; ++i)
+	{
+		if (procMask & procInfo.processorTeamMasks[i])
+		{
+			return i;
+		}
+	}
+	return -1;
+}
+
+int countSetBits(ULONG64 bits)
+{
+	int count = 0;
+	while (bits)
+	{
+		if (bits & 1)
+		{
+			count++;
+		}
+		bits >>= 1;
+	}
+	return count;
+}
+
+typedef BOOL(WINAPI* Pfn_GetLogicalProcessorInformation)(PSYSTEM_LOGICAL_PROCESSOR_INFORMATION, PDWORD);
+
+void getProcessorInformation(btProcessorInfo* procInfo)
+{
+	memset(procInfo, 0, sizeof(*procInfo));
+	Pfn_GetLogicalProcessorInformation getLogicalProcInfo =
+		(Pfn_GetLogicalProcessorInformation)GetProcAddress(GetModuleHandle(TEXT("kernel32")), "GetLogicalProcessorInformation");
+	if (getLogicalProcInfo == NULL)
+	{
+		// no info
+		return;
+	}
+	PSYSTEM_LOGICAL_PROCESSOR_INFORMATION buf = NULL;
+	DWORD bufSize = 0;
+	while (true)
+	{
+		if (getLogicalProcInfo(buf, &bufSize))
+		{
+			break;
+		}
+		else
+		{
+			if (GetLastError() == ERROR_INSUFFICIENT_BUFFER)
+			{
+				if (buf)
+				{
+					free(buf);
+				}
+				buf = (PSYSTEM_LOGICAL_PROCESSOR_INFORMATION)malloc(bufSize);
+			}
+		}
+	}
+
+	int len = bufSize / sizeof(*buf);
+	for (int i = 0; i < len; ++i)
+	{
+		PSYSTEM_LOGICAL_PROCESSOR_INFORMATION info = buf + i;
+		switch (info->Relationship)
+		{
+			case RelationNumaNode:
+				procInfo->numNumaNodes++;
+				break;
+
+			case RelationProcessorCore:
+				procInfo->numCores++;
+				procInfo->numLogicalProcessors += countSetBits(info->ProcessorMask);
+				break;
+
+			case RelationCache:
+				if (info->Cache.Level == 1)
+				{
+					procInfo->numL1Cache++;
+				}
+				else if (info->Cache.Level == 2)
+				{
+					procInfo->numL2Cache++;
+				}
+				else if (info->Cache.Level == 3)
+				{
+					procInfo->numL3Cache++;
+					// processors that share L3 cache are considered to be on the same team
+					// because they can more easily work together on the same data.
+					// Large performance penalties will occur if 2 or more threads from different
+					// teams attempt to frequently read and modify the same cache lines.
+					//
+					// On the AMD Ryzen 7 CPU for example, the 8 cores on the CPU are split into
+					// 2 CCX units of 4 cores each. Each CCX has a separate L3 cache, so if both
+					// CCXs are operating on the same data, many cycles will be spent keeping the
+					// two caches coherent.
+					if (procInfo->numTeamMasks < btProcessorInfo::maxNumTeamMasks)
+					{
+						procInfo->processorTeamMasks[procInfo->numTeamMasks] = info->ProcessorMask;
+						procInfo->numTeamMasks++;
+					}
+				}
+				break;
+
+			case RelationProcessorPackage:
+				procInfo->numPhysicalPackages++;
+				break;
+		}
+	}
+	free(buf);
+}
+
+///btThreadSupportWin32 helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
+class btThreadSupportWin32 : public btThreadSupportInterface
+{
+public:
+	struct btThreadStatus
+	{
+		int m_taskId;
+		int m_commandId;
+		int m_status;
+
+		ThreadFunc m_userThreadFunc;
+		void* m_userPtr;  //for taskDesc etc
+
+		void* m_threadHandle;  //this one is calling 'Win32ThreadFunc'
+
+		void* m_eventStartHandle;
+		char m_eventStartHandleName[32];
+
+		void* m_eventCompleteHandle;
+		char m_eventCompleteHandleName[32];
+	};
+
+private:
+	btAlignedObjectArray<btThreadStatus> m_activeThreadStatus;
+	btAlignedObjectArray<void*> m_completeHandles;
+	int m_numThreads;
+	DWORD_PTR m_startedThreadMask;
+	btProcessorInfo m_processorInfo;
+
+	void startThreads(const ConstructionInfo& threadInfo);
+	void stopThreads();
+	int waitForResponse();
+
+public:
+	btThreadSupportWin32(const ConstructionInfo& threadConstructionInfo);
+	virtual ~btThreadSupportWin32();
+
+	virtual int getNumWorkerThreads() const BT_OVERRIDE { return m_numThreads; }
+	virtual int getCacheFriendlyNumThreads() const BT_OVERRIDE { return countSetBits(m_processorInfo.processorTeamMasks[0]); }
+	virtual int getLogicalToPhysicalCoreRatio() const BT_OVERRIDE { return m_processorInfo.numLogicalProcessors / m_processorInfo.numCores; }
+
+	virtual void runTask(int threadIndex, void* userData) BT_OVERRIDE;
+	virtual void waitForAllTasks() BT_OVERRIDE;
+
+	virtual btCriticalSection* createCriticalSection() BT_OVERRIDE;
+	virtual void deleteCriticalSection(btCriticalSection* criticalSection) BT_OVERRIDE;
+};
+
+btThreadSupportWin32::btThreadSupportWin32(const ConstructionInfo& threadConstructionInfo)
+{
+	startThreads(threadConstructionInfo);
+}
+
+btThreadSupportWin32::~btThreadSupportWin32()
+{
+	stopThreads();
+}
+
+DWORD WINAPI win32threadStartFunc(LPVOID lpParam)
+{
+	btThreadSupportWin32::btThreadStatus* status = (btThreadSupportWin32::btThreadStatus*)lpParam;
+
+	while (1)
+	{
+		WaitForSingleObject(status->m_eventStartHandle, INFINITE);
+		void* userPtr = status->m_userPtr;
+
+		if (userPtr)
+		{
+			btAssert(status->m_status);
+			status->m_userThreadFunc(userPtr);
+			status->m_status = 2;
+			SetEvent(status->m_eventCompleteHandle);
+		}
+		else
+		{
+			//exit Thread
+			status->m_status = 3;
+			printf("Thread with taskId %i with handle %p exiting\n", status->m_taskId, status->m_threadHandle);
+			SetEvent(status->m_eventCompleteHandle);
+			break;
+		}
+	}
+	printf("Thread TERMINATED\n");
+	return 0;
+}
+
+void btThreadSupportWin32::runTask(int threadIndex, void* userData)
+{
+	btThreadStatus& threadStatus = m_activeThreadStatus[threadIndex];
+	btAssert(threadIndex >= 0);
+	btAssert(int(threadIndex) < m_activeThreadStatus.size());
+
+	threadStatus.m_commandId = 1;
+	threadStatus.m_status = 1;
+	threadStatus.m_userPtr = userData;
+	m_startedThreadMask |= DWORD_PTR(1) << threadIndex;
+
+	///fire event to start new task
+	SetEvent(threadStatus.m_eventStartHandle);
+}
+
+int btThreadSupportWin32::waitForResponse()
+{
+	btAssert(m_activeThreadStatus.size());
+
+	int last = -1;
+	DWORD res = WaitForMultipleObjects(m_completeHandles.size(), &m_completeHandles[0], FALSE, INFINITE);
+	btAssert(res != WAIT_FAILED);
+	last = res - WAIT_OBJECT_0;
+
+	btThreadStatus& threadStatus = m_activeThreadStatus[last];
+	btAssert(threadStatus.m_threadHandle);
+	btAssert(threadStatus.m_eventCompleteHandle);
+
+	//WaitForSingleObject(threadStatus.m_eventCompleteHandle, INFINITE);
+	btAssert(threadStatus.m_status > 1);
+	threadStatus.m_status = 0;
+
+	///need to find an active spu
+	btAssert(last >= 0);
+	m_startedThreadMask &= ~(DWORD_PTR(1) << last);
+
+	return last;
+}
+
+void btThreadSupportWin32::waitForAllTasks()
+{
+	while (m_startedThreadMask)
+	{
+		waitForResponse();
+	}
+}
+
+void btThreadSupportWin32::startThreads(const ConstructionInfo& threadConstructionInfo)
+{
+	static int uniqueId = 0;
+	uniqueId++;
+	btProcessorInfo& procInfo = m_processorInfo;
+	getProcessorInformation(&procInfo);
+	DWORD_PTR dwProcessAffinityMask = 0;
+	DWORD_PTR dwSystemAffinityMask = 0;
+	if (!GetProcessAffinityMask(GetCurrentProcess(), &dwProcessAffinityMask, &dwSystemAffinityMask))
+	{
+		dwProcessAffinityMask = 0;
+	}
+	///The number of threads should be equal to the number of available cores - 1
+	m_numThreads = btMin(procInfo.numLogicalProcessors, int(BT_MAX_THREAD_COUNT)) - 1;  // cap to max thread count (-1 because main thread already exists)
+
+	m_activeThreadStatus.resize(m_numThreads);
+	m_completeHandles.resize(m_numThreads);
+	m_startedThreadMask = 0;
+
+	// set main thread affinity
+	if (DWORD_PTR mask = dwProcessAffinityMask & getProcessorTeamMask(procInfo, 0))
+	{
+		SetThreadAffinityMask(GetCurrentThread(), mask);
+		SetThreadIdealProcessor(GetCurrentThread(), 0);
+	}
+
+	for (int i = 0; i < m_numThreads; i++)
+	{
+		printf("starting thread %d\n", i);
+
+		btThreadStatus& threadStatus = m_activeThreadStatus[i];
+
+		LPSECURITY_ATTRIBUTES lpThreadAttributes = NULL;
+		SIZE_T dwStackSize = threadConstructionInfo.m_threadStackSize;
+		LPTHREAD_START_ROUTINE lpStartAddress = &win32threadStartFunc;
+		LPVOID lpParameter = &threadStatus;
+		DWORD dwCreationFlags = 0;
+		LPDWORD lpThreadId = 0;
+
+		threadStatus.m_userPtr = 0;
+
+		sprintf(threadStatus.m_eventStartHandleName, "es%.8s%d%d", threadConstructionInfo.m_uniqueName, uniqueId, i);
+		threadStatus.m_eventStartHandle = CreateEventA(0, false, false, threadStatus.m_eventStartHandleName);
+
+		sprintf(threadStatus.m_eventCompleteHandleName, "ec%.8s%d%d", threadConstructionInfo.m_uniqueName, uniqueId, i);
+		threadStatus.m_eventCompleteHandle = CreateEventA(0, false, false, threadStatus.m_eventCompleteHandleName);
+
+		m_completeHandles[i] = threadStatus.m_eventCompleteHandle;
+
+		HANDLE handle = CreateThread(lpThreadAttributes, dwStackSize, lpStartAddress, lpParameter, dwCreationFlags, lpThreadId);
+		//SetThreadPriority( handle, THREAD_PRIORITY_HIGHEST );
+		// highest priority -- can cause erratic performance when numThreads > numCores
+		//                     we don't want worker threads to be higher priority than the main thread or the main thread could get
+		//                     totally shut out and unable to tell the workers to stop
+		//SetThreadPriority( handle, THREAD_PRIORITY_BELOW_NORMAL );
+
+		{
+			int processorId = i + 1;  // leave processor 0 for main thread
+			DWORD_PTR teamMask = getProcessorTeamMask(procInfo, processorId);
+			if (teamMask)
+			{
+				// bind each thread to only execute on processors of it's assigned team
+				//  - for single-socket Intel x86 CPUs this has no effect (only a single, shared L3 cache so there is only 1 team)
+				//  - for multi-socket Intel this will keep threads from migrating from one socket to another
+				//  - for AMD Ryzen this will keep threads from migrating from one CCX to another
+				DWORD_PTR mask = teamMask & dwProcessAffinityMask;
+				if (mask)
+				{
+					SetThreadAffinityMask(handle, mask);
+				}
+			}
+			SetThreadIdealProcessor(handle, processorId);
+		}
+
+		threadStatus.m_taskId = i;
+		threadStatus.m_commandId = 0;
+		threadStatus.m_status = 0;
+		threadStatus.m_threadHandle = handle;
+		threadStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
+
+		printf("started %s thread %d with threadHandle %p\n", threadConstructionInfo.m_uniqueName, i, handle);
+	}
+}
+
+///tell the task scheduler we are done with the SPU tasks
+void btThreadSupportWin32::stopThreads()
+{
+	for (int i = 0; i < m_activeThreadStatus.size(); i++)
+	{
+		btThreadStatus& threadStatus = m_activeThreadStatus[i];
+		if (threadStatus.m_status > 0)
+		{
+			WaitForSingleObject(threadStatus.m_eventCompleteHandle, INFINITE);
+		}
+
+		threadStatus.m_userPtr = NULL;
+		SetEvent(threadStatus.m_eventStartHandle);
+		WaitForSingleObject(threadStatus.m_eventCompleteHandle, INFINITE);
+
+		CloseHandle(threadStatus.m_eventCompleteHandle);
+		CloseHandle(threadStatus.m_eventStartHandle);
+		CloseHandle(threadStatus.m_threadHandle);
+	}
+
+	m_activeThreadStatus.clear();
+	m_completeHandles.clear();
+}
+
+class btWin32CriticalSection : public btCriticalSection
+{
+private:
+	CRITICAL_SECTION mCriticalSection;
+
+public:
+	btWin32CriticalSection()
+	{
+		InitializeCriticalSection(&mCriticalSection);
+	}
+
+	~btWin32CriticalSection()
+	{
+		DeleteCriticalSection(&mCriticalSection);
+	}
+
+	void lock()
+	{
+		EnterCriticalSection(&mCriticalSection);
+	}
+
+	void unlock()
+	{
+		LeaveCriticalSection(&mCriticalSection);
+	}
+};
+
+btCriticalSection* btThreadSupportWin32::createCriticalSection()
+{
+	unsigned char* mem = (unsigned char*)btAlignedAlloc(sizeof(btWin32CriticalSection), 16);
+	btWin32CriticalSection* cs = new (mem) btWin32CriticalSection();
+	return cs;
+}
+
+void btThreadSupportWin32::deleteCriticalSection(btCriticalSection* criticalSection)
+{
+	criticalSection->~btCriticalSection();
+	btAlignedFree(criticalSection);
+}
+
+btThreadSupportInterface* btThreadSupportInterface::create(const ConstructionInfo& info)
+{
+	return new btThreadSupportWin32(info);
+}
+
+#endif  //defined(_WIN32) && BT_THREADSAFE
diff --git a/extern/bullet2/src/LinearMath/btAabbUtil2.h b/extern/bullet2/src/LinearMath/btAabbUtil2.h
index d2997b4e65c..eea49dd33f9 100644
--- a/extern/bullet2/src/LinearMath/btAabbUtil2.h
+++ b/extern/bullet2/src/LinearMath/btAabbUtil2.h
@@ -12,8 +12,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef BT_AABB_UTIL2
 #define BT_AABB_UTIL2
 
@@ -21,20 +19,18 @@ subject to the following restrictions:
 #include "btVector3.h"
 #include "btMinMax.h"
 
-
-
-SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin,
-								   btVector3& aabbMax,
-								   const btVector3& expansionMin,
-								   const btVector3& expansionMax)
+SIMD_FORCE_INLINE void AabbExpand(btVector3& aabbMin,
+								  btVector3& aabbMax,
+								  const btVector3& expansionMin,
+								  const btVector3& expansionMax)
 {
 	aabbMin = aabbMin + expansionMin;
 	aabbMax = aabbMax + expansionMax;
 }
 
 /// conservative test for overlap between two aabbs
-SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
-								const btVector3 &point)
+SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3& aabbMin1, const btVector3& aabbMax1,
+											 const btVector3& point)
 {
 	bool overlap = true;
 	overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
@@ -43,10 +39,9 @@ SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const bt
 	return overlap;
 }
 
-
 /// conservative test for overlap between two aabbs
-SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
-								const btVector3 &aabbMin2, const btVector3 &aabbMax2)
+SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3& aabbMin1, const btVector3& aabbMax1,
+											const btVector3& aabbMin2, const btVector3& aabbMax2)
 {
 	bool overlap = true;
 	overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
@@ -56,37 +51,34 @@ SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btV
 }
 
 /// conservative test for overlap between triangle and aabb
-SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices,
-									const btVector3 &aabbMin, const btVector3 &aabbMax)
+SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3* vertices,
+												const btVector3& aabbMin, const btVector3& aabbMax)
 {
-	const btVector3 &p1 = vertices[0];
-	const btVector3 &p2 = vertices[1];
-	const btVector3 &p3 = vertices[2];
+	const btVector3& p1 = vertices[0];
+	const btVector3& p2 = vertices[1];
+	const btVector3& p3 = vertices[2];
 
 	if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
 	if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
 
 	if (btMin(btMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
 	if (btMax(btMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
-  
+
 	if (btMin(btMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
 	if (btMax(btMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
 	return true;
 }
 
-
-SIMD_FORCE_INLINE int	btOutcode(const btVector3& p,const btVector3& halfExtent) 
+SIMD_FORCE_INLINE int btOutcode(const btVector3& p, const btVector3& halfExtent)
 {
-	return (p.getX()  < -halfExtent.getX() ? 0x01 : 0x0) |    
-		   (p.getX() >  halfExtent.getX() ? 0x08 : 0x0) |
-		   (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |    
-		   (p.getY() >  halfExtent.getY() ? 0x10 : 0x0) |
-		   (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |    
-		   (p.getZ() >  halfExtent.getZ() ? 0x20 : 0x0);
+	return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
+		   (p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
+		   (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |
+		   (p.getY() > halfExtent.getY() ? 0x10 : 0x0) |
+		   (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |
+		   (p.getZ() > halfExtent.getZ() ? 0x20 : 0x0);
 }
 
-
-
 SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom,
 								  const btVector3& rayInvDirection,
 								  const unsigned int raySign[3],
@@ -97,11 +89,11 @@ SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom,
 {
 	btScalar tmax, tymin, tymax, tzmin, tzmax;
 	tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
-	tmax = (bounds[1-raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
+	tmax = (bounds[1 - raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
 	tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
-	tymax = (bounds[1-raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
+	tymax = (bounds[1 - raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
 
-	if ( (tmin > tymax) || (tymin > tmax) )
+	if ((tmin > tymax) || (tymin > tmax))
 		return false;
 
 	if (tymin > tmin)
@@ -111,59 +103,59 @@ SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom,
 		tmax = tymax;
 
 	tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
-	tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
+	tzmax = (bounds[1 - raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
 
-	if ( (tmin > tzmax) || (tzmin > tmax) )
+	if ((tmin > tzmax) || (tzmin > tmax))
 		return false;
 	if (tzmin > tmin)
 		tmin = tzmin;
 	if (tzmax < tmax)
 		tmax = tzmax;
-	return ( (tmin < lambda_max) && (tmax > lambda_min) );
+	return ((tmin < lambda_max) && (tmax > lambda_min));
 }
 
-SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, 
-								 const btVector3& rayTo, 
-								 const btVector3& aabbMin, 
+SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
+								 const btVector3& rayTo,
+								 const btVector3& aabbMin,
 								 const btVector3& aabbMax,
-					  btScalar& param, btVector3& normal) 
+								 btScalar& param, btVector3& normal)
 {
-	btVector3 aabbHalfExtent = (aabbMax-aabbMin)* btScalar(0.5);
-	btVector3 aabbCenter = (aabbMax+aabbMin)* btScalar(0.5);
-	btVector3	source = rayFrom - aabbCenter;
-	btVector3	target = rayTo - aabbCenter;
-	int	sourceOutcode = btOutcode(source,aabbHalfExtent);
-	int targetOutcode = btOutcode(target,aabbHalfExtent);
+	btVector3 aabbHalfExtent = (aabbMax - aabbMin) * btScalar(0.5);
+	btVector3 aabbCenter = (aabbMax + aabbMin) * btScalar(0.5);
+	btVector3 source = rayFrom - aabbCenter;
+	btVector3 target = rayTo - aabbCenter;
+	int sourceOutcode = btOutcode(source, aabbHalfExtent);
+	int targetOutcode = btOutcode(target, aabbHalfExtent);
 	if ((sourceOutcode & targetOutcode) == 0x0)
 	{
 		btScalar lambda_enter = btScalar(0.0);
-		btScalar lambda_exit  = param;
+		btScalar lambda_exit = param;
 		btVector3 r = target - source;
 		int i;
-		btScalar	normSign = 1;
-		btVector3	hitNormal(0,0,0);
-		int bit=1;
+		btScalar normSign = 1;
+		btVector3 hitNormal(0, 0, 0);
+		int bit = 1;
 
-		for (int j=0;j<2;j++)
+		for (int j = 0; j < 2; j++)
 		{
 			for (i = 0; i != 3; ++i)
 			{
 				if (sourceOutcode & bit)
 				{
-					btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
+					btScalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
 					if (lambda_enter <= lambda)
 					{
 						lambda_enter = lambda;
-						hitNormal.setValue(0,0,0);
+						hitNormal.setValue(0, 0, 0);
 						hitNormal[i] = normSign;
 					}
 				}
-				else if (targetOutcode & bit) 
+				else if (targetOutcode & bit)
 				{
-					btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
+					btScalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
 					btSetMin(lambda_exit, lambda);
 				}
-				bit<<=1;
+				bit <<= 1;
 			}
 			normSign = btScalar(-1.);
 		}
@@ -177,56 +169,49 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
 	return false;
 }
 
-
-
-SIMD_FORCE_INLINE	void btTransformAabb(const btVector3& halfExtents, btScalar margin,const btTransform& t,btVector3& aabbMinOut,btVector3& aabbMaxOut)
+SIMD_FORCE_INLINE void btTransformAabb(const btVector3& halfExtents, btScalar margin, const btTransform& t, btVector3& aabbMinOut, btVector3& aabbMaxOut)
 {
-	btVector3 halfExtentsWithMargin = halfExtents+btVector3(margin,margin,margin);
-	btMatrix3x3 abs_b = t.getBasis().absolute();  
+	btVector3 halfExtentsWithMargin = halfExtents + btVector3(margin, margin, margin);
+	btMatrix3x3 abs_b = t.getBasis().absolute();
 	btVector3 center = t.getOrigin();
-    btVector3 extent = halfExtentsWithMargin.dot3( abs_b[0], abs_b[1], abs_b[2] );
+	btVector3 extent = halfExtentsWithMargin.dot3(abs_b[0], abs_b[1], abs_b[2]);
 	aabbMinOut = center - extent;
 	aabbMaxOut = center + extent;
 }
 
-
-SIMD_FORCE_INLINE	void btTransformAabb(const btVector3& localAabbMin,const btVector3& localAabbMax, btScalar margin,const btTransform& trans,btVector3& aabbMinOut,btVector3& aabbMaxOut)
+SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin, const btVector3& localAabbMax, btScalar margin, const btTransform& trans, btVector3& aabbMinOut, btVector3& aabbMaxOut)
 {
-		btAssert(localAabbMin.getX() <= localAabbMax.getX());
-		btAssert(localAabbMin.getY() <= localAabbMax.getY());
-		btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
-		btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
-		localHalfExtents+=btVector3(margin,margin,margin);
-
-		btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
-		btMatrix3x3 abs_b = trans.getBasis().absolute();  
-		btVector3 center = trans(localCenter);
-        btVector3 extent = localHalfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] );
-		aabbMinOut = center-extent;
-		aabbMaxOut = center+extent;
+	btAssert(localAabbMin.getX() <= localAabbMax.getX());
+	btAssert(localAabbMin.getY() <= localAabbMax.getY());
+	btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
+	btVector3 localHalfExtents = btScalar(0.5) * (localAabbMax - localAabbMin);
+	localHalfExtents += btVector3(margin, margin, margin);
+
+	btVector3 localCenter = btScalar(0.5) * (localAabbMax + localAabbMin);
+	btMatrix3x3 abs_b = trans.getBasis().absolute();
+	btVector3 center = trans(localCenter);
+	btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
+	aabbMinOut = center - extent;
+	aabbMaxOut = center + extent;
 }
 
 #define USE_BANCHLESS 1
 #ifdef USE_BANCHLESS
-	//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
-	SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
-	{		
-		return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
-			& (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
-			& (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
-			1, 0));
-	}
+//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
+SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1, const unsigned short int* aabbMax1, const unsigned short int* aabbMin2, const unsigned short int* aabbMax2)
+{
+	return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
+											  1, 0));
+}
 #else
-	SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
-	{
-		bool overlap = true;
-		overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
-		overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
-		overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
-		return overlap;
-	}
-#endif //USE_BANCHLESS
-
-#endif //BT_AABB_UTIL2
-
+SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1, const unsigned short int* aabbMax1, const unsigned short int* aabbMin2, const unsigned short int* aabbMax2)
+{
+	bool overlap = true;
+	overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
+	overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
+	overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
+	return overlap;
+}
+#endif  //USE_BANCHLESS
 
+#endif  //BT_AABB_UTIL2
diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp
index a65296c6abe..be8f8aa6d0b 100644
--- a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp
+++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp
@@ -15,9 +15,11 @@ subject to the following restrictions:
 
 #include "btAlignedAllocator.h"
 
+#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
 int gNumAlignedAllocs = 0;
 int gNumAlignedFree = 0;
-int gTotalBytesAlignedAllocs = 0;//detect memory leaks
+int gTotalBytesAlignedAllocs = 0;  //detect memory leaks
+#endif                             //BT_DEBUG_MEMORY_ALLOCATIONST_DEBUG_ALLOCATIONS
 
 static void *btAllocDefault(size_t size)
 {
@@ -32,9 +34,7 @@ static void btFreeDefault(void *ptr)
 static btAllocFunc *sAllocFunc = btAllocDefault;
 static btFreeFunc *sFreeFunc = btFreeDefault;
 
-
-
-#if defined (BT_HAS_ALIGNED_ALLOCATOR)
+#if defined(BT_HAS_ALIGNED_ALLOCATOR)
 #include <malloc.h>
 static void *btAlignedAllocDefault(size_t size, int alignment)
 {
@@ -59,123 +59,205 @@ static inline void btAlignedFreeDefault(void *ptr)
 }
 #else
 
-
-
-
-
 static inline void *btAlignedAllocDefault(size_t size, int alignment)
 {
-  void *ret;
-  char *real;
-  real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1));
-  if (real) {
-	ret = btAlignPointer(real + sizeof(void *),alignment);
-    *((void **)(ret)-1) = (void *)(real);
-  } else {
-    ret = (void *)(real);
-  }
-  return (ret);
+	void *ret;
+	char *real;
+	real = (char *)sAllocFunc(size + sizeof(void *) + (alignment - 1));
+	if (real)
+	{
+		ret = btAlignPointer(real + sizeof(void *), alignment);
+		*((void **)(ret)-1) = (void *)(real);
+	}
+	else
+	{
+		ret = (void *)(real);
+	}
+	return (ret);
 }
 
 static inline void btAlignedFreeDefault(void *ptr)
 {
-  void* real;
+	void *real;
 
-  if (ptr) {
-    real = *((void **)(ptr)-1);
-    sFreeFunc(real);
-  }
+	if (ptr)
+	{
+		real = *((void **)(ptr)-1);
+		sFreeFunc(real);
+	}
 }
 #endif
 
-
 static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault;
 static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault;
 
 void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc)
 {
-  sAlignedAllocFunc = allocFunc ? allocFunc : btAlignedAllocDefault;
-  sAlignedFreeFunc = freeFunc ? freeFunc : btAlignedFreeDefault;
+	sAlignedAllocFunc = allocFunc ? allocFunc : btAlignedAllocDefault;
+	sAlignedFreeFunc = freeFunc ? freeFunc : btAlignedFreeDefault;
 }
 
 void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc)
 {
-  sAllocFunc = allocFunc ? allocFunc : btAllocDefault;
-  sFreeFunc = freeFunc ? freeFunc : btFreeDefault;
+	sAllocFunc = allocFunc ? allocFunc : btAllocDefault;
+	sFreeFunc = freeFunc ? freeFunc : btFreeDefault;
 }
 
 #ifdef BT_DEBUG_MEMORY_ALLOCATIONS
-//this generic allocator provides the total allocated number of bytes
+
+static int allocations_id[10241024];
+static int allocations_bytes[10241024];
+static int mynumallocs = 0;
 #include <stdio.h>
 
-void*   btAlignedAllocInternal  (size_t size, int alignment,int line,char* filename)
+int btDumpMemoryLeaks()
 {
- void *ret;
- char *real;
+	int totalLeak = 0;
+
+	for (int i = 0; i < mynumallocs; i++)
+	{
+		printf("Error: leaked memory of allocation #%d (%d bytes)\n", allocations_id[i], allocations_bytes[i]);
+		totalLeak += allocations_bytes[i];
+	}
+	if (totalLeak)
+	{
+		printf("Error: memory leaks: %d allocations were not freed and leaked together %d bytes\n", mynumallocs, totalLeak);
+	}
+	return totalLeak;
+}
+//this generic allocator provides the total allocated number of bytes
+#include <stdio.h>
 
- gTotalBytesAlignedAllocs += size;
- gNumAlignedAllocs++;
+struct btDebugPtrMagic
+{
+	union {
+		void **vptrptr;
+		void *vptr;
+		int *iptr;
+		char *cptr;
+	};
+};
+
+void *btAlignedAllocInternal(size_t size, int alignment, int line, const char *filename)
+{
+	if (size == 0)
+	{
+		printf("Whaat? size==0");
+		return 0;
+	}
+	static int allocId = 0;
+
+	void *ret;
+	char *real;
+
+	// to find some particular memory leak, you could do something like this:
+	//	if (allocId==172)
+	//	{
+	//		printf("catch me!\n");
+	//	}
+	//	if (size>1024*1024)
+	//	{
+	//		printf("big alloc!%d\n", size);
+	//	}
+
+	gTotalBytesAlignedAllocs += size;
+	gNumAlignedAllocs++;
 
- 
- real = (char *)sAllocFunc(size + 2*sizeof(void *) + (alignment-1));
- if (real) {
-   ret = (void*) btAlignPointer(real + 2*sizeof(void *), alignment);
-   *((void **)(ret)-1) = (void *)(real);
-       *((int*)(ret)-2) = size;
+	int sz4prt = 4 * sizeof(void *);
 
- } else {
-   ret = (void *)(real);//??
- }
+	real = (char *)sAllocFunc(size + sz4prt + (alignment - 1));
+	if (real)
+	{
+		ret = (void *)btAlignPointer(real + sz4prt, alignment);
+		btDebugPtrMagic p;
+		p.vptr = ret;
+		p.cptr -= sizeof(void *);
+		*p.vptrptr = (void *)real;
+		p.cptr -= sizeof(void *);
+		*p.iptr = size;
+		p.cptr -= sizeof(void *);
+		*p.iptr = allocId;
+
+		allocations_id[mynumallocs] = allocId;
+		allocations_bytes[mynumallocs] = size;
+		mynumallocs++;
+	}
+	else
+	{
+		ret = (void *)(real);  //??
+	}
 
- printf("allocation#%d at address %x, from %s,line %d, size %d\n",gNumAlignedAllocs,real, filename,line,size);
+	printf("allocation %d at address %x, from %s,line %d, size %d (total allocated = %d)\n", allocId, real, filename, line, size, gTotalBytesAlignedAllocs);
+	allocId++;
 
- int* ptr = (int*)ret;
- *ptr = 12;
- return (ret);
+	int *ptr = (int *)ret;
+	*ptr = 12;
+	return (ret);
 }
 
-void    btAlignedFreeInternal   (void* ptr,int line,char* filename)
+void btAlignedFreeInternal(void *ptr, int line, const char *filename)
 {
+	void *real;
 
- void* real;
- gNumAlignedFree++;
-
- if (ptr) {
-   real = *((void **)(ptr)-1);
-       int size = *((int*)(ptr)-2);
-       gTotalBytesAlignedAllocs -= size;
-
-	   printf("free #%d at address %x, from %s,line %d, size %d\n",gNumAlignedFree,real, filename,line,size);
-
-   sFreeFunc(real);
- } else
- {
-	 printf("NULL ptr\n");
- }
+	if (ptr)
+	{
+		gNumAlignedFree++;
+
+		btDebugPtrMagic p;
+		p.vptr = ptr;
+		p.cptr -= sizeof(void *);
+		real = *p.vptrptr;
+		p.cptr -= sizeof(void *);
+		int size = *p.iptr;
+		p.cptr -= sizeof(void *);
+		int allocId = *p.iptr;
+
+		bool found = false;
+
+		for (int i = 0; i < mynumallocs; i++)
+		{
+			if (allocations_id[i] == allocId)
+			{
+				allocations_id[i] = allocations_id[mynumallocs - 1];
+				allocations_bytes[i] = allocations_bytes[mynumallocs - 1];
+				mynumallocs--;
+				found = true;
+				break;
+			}
+		}
+
+		gTotalBytesAlignedAllocs -= size;
+
+		int diff = gNumAlignedAllocs - gNumAlignedFree;
+		printf("free %d at address %x, from %s,line %d, size %d (total remain = %d in %d non-freed allocations)\n", allocId, real, filename, line, size, gTotalBytesAlignedAllocs, diff);
+
+		sFreeFunc(real);
+	}
+	else
+	{
+		//printf("deleting a NULL ptr, no effect\n");
+	}
 }
 
-#else //BT_DEBUG_MEMORY_ALLOCATIONS
+#else  //BT_DEBUG_MEMORY_ALLOCATIONS
 
-void*	btAlignedAllocInternal	(size_t size, int alignment)
+void *btAlignedAllocInternal(size_t size, int alignment)
 {
-	gNumAlignedAllocs++;
-	void* ptr;
+	void *ptr;
 	ptr = sAlignedAllocFunc(size, alignment);
-//	printf("btAlignedAllocInternal %d, %x\n",size,ptr);
+	//	printf("btAlignedAllocInternal %d, %x\n",size,ptr);
 	return ptr;
 }
 
-void	btAlignedFreeInternal	(void* ptr)
+void btAlignedFreeInternal(void *ptr)
 {
 	if (!ptr)
 	{
 		return;
 	}
 
-	gNumAlignedFree++;
-//	printf("btAlignedFreeInternal %x\n",ptr);
+	//	printf("btAlignedFreeInternal %x\n",ptr);
 	sAlignedFreeFunc(ptr);
 }
 
-#endif //BT_DEBUG_MEMORY_ALLOCATIONS
-
+#endif  //BT_DEBUG_MEMORY_ALLOCATIONS
diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.h b/extern/bullet2/src/LinearMath/btAlignedAllocator.h
index f168f3c66c7..971f62bfb07 100644
--- a/extern/bullet2/src/LinearMath/btAlignedAllocator.h
+++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.h
@@ -21,87 +21,95 @@ subject to the following restrictions:
 ///that is better portable and more predictable
 
 #include "btScalar.h"
-//#define BT_DEBUG_MEMORY_ALLOCATIONS 1
+
+///BT_DEBUG_MEMORY_ALLOCATIONS preprocessor can be set in build system
+///for regression tests to detect memory leaks
+///#define BT_DEBUG_MEMORY_ALLOCATIONS 1
 #ifdef BT_DEBUG_MEMORY_ALLOCATIONS
 
-#define btAlignedAlloc(a,b) \
-		btAlignedAllocInternal(a,b,__LINE__,__FILE__)
+int btDumpMemoryLeaks();
+
+#define btAlignedAlloc(a, b) \
+	btAlignedAllocInternal(a, b, __LINE__, __FILE__)
 
 #define btAlignedFree(ptr) \
-		btAlignedFreeInternal(ptr,__LINE__,__FILE__)
+	btAlignedFreeInternal(ptr, __LINE__, __FILE__)
 
-void*	btAlignedAllocInternal	(size_t size, int alignment,int line,char* filename);
+void* btAlignedAllocInternal(size_t size, int alignment, int line, const char* filename);
 
-void	btAlignedFreeInternal	(void* ptr,int line,char* filename);
+void btAlignedFreeInternal(void* ptr, int line, const char* filename);
 
 #else
-	void*	btAlignedAllocInternal	(size_t size, int alignment);
-	void	btAlignedFreeInternal	(void* ptr);
+void* btAlignedAllocInternal(size_t size, int alignment);
+void btAlignedFreeInternal(void* ptr);
 
-	#define btAlignedAlloc(size,alignment) btAlignedAllocInternal(size,alignment)
-	#define btAlignedFree(ptr) btAlignedFreeInternal(ptr)
+#define btAlignedAlloc(size, alignment) btAlignedAllocInternal(size, alignment)
+#define btAlignedFree(ptr) btAlignedFreeInternal(ptr)
 
 #endif
-typedef int	size_type;
+typedef int size_type;
 
-typedef void *(btAlignedAllocFunc)(size_t size, int alignment);
-typedef void (btAlignedFreeFunc)(void *memblock);
-typedef void *(btAllocFunc)(size_t size);
-typedef void (btFreeFunc)(void *memblock);
+typedef void*(btAlignedAllocFunc)(size_t size, int alignment);
+typedef void(btAlignedFreeFunc)(void* memblock);
+typedef void*(btAllocFunc)(size_t size);
+typedef void(btFreeFunc)(void* memblock);
 
 ///The developer can let all Bullet memory allocations go through a custom memory allocator, using btAlignedAllocSetCustom
-void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc);
+void btAlignedAllocSetCustom(btAllocFunc* allocFunc, btFreeFunc* freeFunc);
 ///If the developer has already an custom aligned allocator, then btAlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
-void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc);
-
+void btAlignedAllocSetCustomAligned(btAlignedAllocFunc* allocFunc, btAlignedFreeFunc* freeFunc);
 
 ///The btAlignedAllocator is a portable class for aligned memory allocations.
 ///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using btAlignedAllocSetCustom and btAlignedAllocSetCustomAligned.
-template < typename T , unsigned Alignment >
-class btAlignedAllocator {
-	
-	typedef btAlignedAllocator< T , Alignment > self_type;
-	
-public:
+template <typename T, unsigned Alignment>
+class btAlignedAllocator
+{
+	typedef btAlignedAllocator<T, Alignment> self_type;
 
+public:
 	//just going down a list:
 	btAlignedAllocator() {}
 	/*
 	btAlignedAllocator( const self_type & ) {}
 	*/
 
-	template < typename Other >
-	btAlignedAllocator( const btAlignedAllocator< Other , Alignment > & ) {}
+	template <typename Other>
+	btAlignedAllocator(const btAlignedAllocator<Other, Alignment>&)
+	{
+	}
 
-	typedef const T*         const_pointer;
-	typedef const T&         const_reference;
-	typedef T*               pointer;
-	typedef T&               reference;
-	typedef T                value_type;
+	typedef const T* const_pointer;
+	typedef const T& const_reference;
+	typedef T* pointer;
+	typedef T& reference;
+	typedef T value_type;
 
-	pointer       address   ( reference        ref ) const                           { return &ref; }
-	const_pointer address   ( const_reference  ref ) const                           { return &ref; }
-	pointer       allocate  ( size_type        n   , const_pointer *      hint = 0 ) {
+	pointer address(reference ref) const { return &ref; }
+	const_pointer address(const_reference ref) const { return &ref; }
+	pointer allocate(size_type n, const_pointer* hint = 0)
+	{
 		(void)hint;
-		return reinterpret_cast< pointer >(btAlignedAlloc( sizeof(value_type) * n , Alignment ));
+		return reinterpret_cast<pointer>(btAlignedAlloc(sizeof(value_type) * n, Alignment));
 	}
-	void          construct ( pointer          ptr , const value_type &   value    ) { new (ptr) value_type( value ); }
-	void          deallocate( pointer          ptr ) {
-		btAlignedFree( reinterpret_cast< void * >( ptr ) );
+	void construct(pointer ptr, const value_type& value) { new (ptr) value_type(value); }
+	void deallocate(pointer ptr)
+	{
+		btAlignedFree(reinterpret_cast<void*>(ptr));
 	}
-	void          destroy   ( pointer          ptr )                                 { ptr->~value_type(); }
-	
+	void destroy(pointer ptr) { ptr->~value_type(); }
 
-	template < typename O > struct rebind {
-		typedef btAlignedAllocator< O , Alignment > other;
+	template <typename O>
+	struct rebind
+	{
+		typedef btAlignedAllocator<O, Alignment> other;
 	};
-	template < typename O >
-	self_type & operator=( const btAlignedAllocator< O , Alignment > & ) { return *this; }
+	template <typename O>
+	self_type& operator=(const btAlignedAllocator<O, Alignment>&)
+	{
+		return *this;
+	}
 
-	friend bool operator==( const self_type & , const self_type & ) { return true; }
+	friend bool operator==(const self_type&, const self_type&) { return true; }
 };
 
-
-
-#endif //BT_ALIGNED_ALLOCATOR
-
+#endif  //BT_ALIGNED_ALLOCATOR
diff --git a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h
index 6193ef7f427..b3d5d64b58b 100644
--- a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h
+++ b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h
@@ -13,11 +13,10 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_OBJECT_ARRAY__
 #define BT_OBJECT_ARRAY__
 
-#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE
+#include "btScalar.h"  // has definitions like SIMD_FORCE_INLINE
 #include "btAlignedAllocator.h"
 
 ///If the platform doesn't support placement new, you can disable BT_USE_PLACEMENT_NEW
@@ -28,394 +27,371 @@ subject to the following restrictions:
 
 #define BT_USE_PLACEMENT_NEW 1
 //#define BT_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
-#define BT_ALLOW_ARRAY_COPY_OPERATOR // enabling this can accidently perform deep copies of data if you are not careful
+#define BT_ALLOW_ARRAY_COPY_OPERATOR  // enabling this can accidently perform deep copies of data if you are not careful
 
 #ifdef BT_USE_MEMCPY
 #include <memory.h>
 #include <string.h>
-#endif //BT_USE_MEMCPY
+#endif  //BT_USE_MEMCPY
 
 #ifdef BT_USE_PLACEMENT_NEW
-#include <new> //for placement new
-#endif //BT_USE_PLACEMENT_NEW
-
-// The register keyword is deprecated in C++11 so don't use it.
-#if __cplusplus > 199711L
-#define BT_REGISTER
-#else
-#define BT_REGISTER register
-#endif
+#include <new>  //for placement new
+#endif          //BT_USE_PLACEMENT_NEW
 
 ///The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods
 ///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
-template <typename T> 
-//template <class T> 
+template <typename T>
+//template <class T>
 class btAlignedObjectArray
 {
-	btAlignedAllocator<T , 16>	m_allocator;
+	btAlignedAllocator<T, 16> m_allocator;
 
-	int					m_size;
-	int					m_capacity;
-	T*					m_data;
+	int m_size;
+	int m_capacity;
+	T* m_data;
 	//PCK: added this line
-	bool				m_ownsMemory;
+	bool m_ownsMemory;
 
 #ifdef BT_ALLOW_ARRAY_COPY_OPERATOR
 public:
-	SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T> &other)
+	SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T>& other)
 	{
 		copyFromArray(other);
 		return *this;
 	}
-#else//BT_ALLOW_ARRAY_COPY_OPERATOR
+#else   //BT_ALLOW_ARRAY_COPY_OPERATOR
 private:
-		SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T> &other);
-#endif//BT_ALLOW_ARRAY_COPY_OPERATOR
+	SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T>& other);
+#endif  //BT_ALLOW_ARRAY_COPY_OPERATOR
 
 protected:
-		SIMD_FORCE_INLINE	int	allocSize(int size)
-		{
-			return (size ? size*2 : 1);
-		}
-		SIMD_FORCE_INLINE	void	copy(int start,int end, T* dest) const
-		{
-			int i;
-			for (i=start;i<end;++i)
+	SIMD_FORCE_INLINE int allocSize(int size)
+	{
+		return (size ? size * 2 : 1);
+	}
+	SIMD_FORCE_INLINE void copy(int start, int end, T* dest) const
+	{
+		int i;
+		for (i = start; i < end; ++i)
 #ifdef BT_USE_PLACEMENT_NEW
-				new (&dest[i]) T(m_data[i]);
+			new (&dest[i]) T(m_data[i]);
 #else
-				dest[i] = m_data[i];
-#endif //BT_USE_PLACEMENT_NEW
-		}
+			dest[i] = m_data[i];
+#endif  //BT_USE_PLACEMENT_NEW
+	}
 
-		SIMD_FORCE_INLINE	void	init()
+	SIMD_FORCE_INLINE void init()
+	{
+		//PCK: added this line
+		m_ownsMemory = true;
+		m_data = 0;
+		m_size = 0;
+		m_capacity = 0;
+	}
+	SIMD_FORCE_INLINE void destroy(int first, int last)
+	{
+		int i;
+		for (i = first; i < last; i++)
 		{
-			//PCK: added this line
-			m_ownsMemory = true;
-			m_data = 0;
-			m_size = 0;
-			m_capacity = 0;
+			m_data[i].~T();
 		}
-		SIMD_FORCE_INLINE	void	destroy(int first,int last)
+	}
+
+	SIMD_FORCE_INLINE void* allocate(int size)
+	{
+		if (size)
+			return m_allocator.allocate(size);
+		return 0;
+	}
+
+	SIMD_FORCE_INLINE void deallocate()
+	{
+		if (m_data)
 		{
-			int i;
-			for (i=first; i<last;i++)
+			//PCK: enclosed the deallocation in this block
+			if (m_ownsMemory)
 			{
-				m_data[i].~T();
+				m_allocator.deallocate(m_data);
 			}
+			m_data = 0;
 		}
+	}
 
-		SIMD_FORCE_INLINE	void* allocate(int size)
-		{
-			if (size)
-				return m_allocator.allocate(size);
-			return 0;
-		}
+public:
+	btAlignedObjectArray()
+	{
+		init();
+	}
 
-		SIMD_FORCE_INLINE	void	deallocate()
-		{
-			if(m_data)	{
-				//PCK: enclosed the deallocation in this block
-				if (m_ownsMemory)
-				{
-					m_allocator.deallocate(m_data);
-				}
-				m_data = 0;
-			}
-		}
+	~btAlignedObjectArray()
+	{
+		clear();
+	}
 
-	
+	///Generally it is best to avoid using the copy constructor of an btAlignedObjectArray, and use a (const) reference to the array instead.
+	btAlignedObjectArray(const btAlignedObjectArray& otherArray)
+	{
+		init();
 
+		int otherSize = otherArray.size();
+		resize(otherSize);
+		otherArray.copy(0, otherSize, m_data);
+	}
 
-	public:
-		
-		btAlignedObjectArray()
-		{
-			init();
-		}
+	/// return the number of elements in the array
+	SIMD_FORCE_INLINE int size() const
+	{
+		return m_size;
+	}
 
-		~btAlignedObjectArray()
-		{
-			clear();
-		}
+	SIMD_FORCE_INLINE const T& at(int n) const
+	{
+		btAssert(n >= 0);
+		btAssert(n < size());
+		return m_data[n];
+	}
 
-		///Generally it is best to avoid using the copy constructor of an btAlignedObjectArray, and use a (const) reference to the array instead.
-		btAlignedObjectArray(const btAlignedObjectArray& otherArray)
-		{
-			init();
+	SIMD_FORCE_INLINE T& at(int n)
+	{
+		btAssert(n >= 0);
+		btAssert(n < size());
+		return m_data[n];
+	}
 
-			int otherSize = otherArray.size();
-			resize (otherSize);
-			otherArray.copy(0, otherSize, m_data);
-		}
+	SIMD_FORCE_INLINE const T& operator[](int n) const
+	{
+		btAssert(n >= 0);
+		btAssert(n < size());
+		return m_data[n];
+	}
 
-		
-		
-		/// return the number of elements in the array
-		SIMD_FORCE_INLINE	int size() const
-		{	
-			return m_size;
-		}
-		
-		SIMD_FORCE_INLINE const T& at(int n) const
-		{
-			btAssert(n>=0);
-			btAssert(n<size());
-			return m_data[n];
-		}
+	SIMD_FORCE_INLINE T& operator[](int n)
+	{
+		btAssert(n >= 0);
+		btAssert(n < size());
+		return m_data[n];
+	}
 
-		SIMD_FORCE_INLINE T& at(int n)
-		{
-			btAssert(n>=0);
-			btAssert(n<size());
-			return m_data[n];
-		}
+	///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
+	SIMD_FORCE_INLINE void clear()
+	{
+		destroy(0, size());
 
-		SIMD_FORCE_INLINE const T& operator[](int n) const
-		{
-			btAssert(n>=0);
-			btAssert(n<size());
-			return m_data[n];
-		}
+		deallocate();
 
-		SIMD_FORCE_INLINE T& operator[](int n)
-		{
-			btAssert(n>=0);
-			btAssert(n<size());
-			return m_data[n];
-		}
-		
+		init();
+	}
 
-		///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
-		SIMD_FORCE_INLINE	void	clear()
-		{
-			destroy(0,size());
-			
-			deallocate();
-			
-			init();
-		}
+	SIMD_FORCE_INLINE void pop_back()
+	{
+		btAssert(m_size > 0);
+		m_size--;
+		m_data[m_size].~T();
+	}
 
-		SIMD_FORCE_INLINE	void	pop_back()
+	///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
+	///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
+	SIMD_FORCE_INLINE void resizeNoInitialize(int newsize)
+	{
+		if (newsize > size())
 		{
-			btAssert(m_size>0);
-			m_size--;
-			m_data[m_size].~T();
+			reserve(newsize);
 		}
+		m_size = newsize;
+	}
 
+	SIMD_FORCE_INLINE void resize(int newsize, const T& fillData = T())
+	{
+		const int curSize = size();
 
-		///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
-		///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
-		SIMD_FORCE_INLINE	void	resizeNoInitialize(int newsize)
+		if (newsize < curSize)
 		{
-			if (newsize > size())
+			for (int i = newsize; i < curSize; i++)
 			{
-				reserve(newsize);
+				m_data[i].~T();
 			}
-			m_size = newsize;
 		}
-	
-		SIMD_FORCE_INLINE	void	resize(int newsize, const T& fillData=T())
+		else
 		{
-			const BT_REGISTER int curSize = size();
-
-			if (newsize < curSize)
-			{
-				for(int i = newsize; i < curSize; i++)
-				{
-					m_data[i].~T();
-				}
-			} else
+			if (newsize > curSize)
 			{
-				if (newsize > curSize)
-				{
-					reserve(newsize);
-				}
-#ifdef BT_USE_PLACEMENT_NEW
-				for (int i=curSize;i<newsize;i++)
-				{
-					new ( &m_data[i]) T(fillData);
-				}
-#endif //BT_USE_PLACEMENT_NEW
-
+				reserve(newsize);
 			}
-
-			m_size = newsize;
-		}
-		SIMD_FORCE_INLINE	T&  expandNonInitializing( )
-		{	
-			const BT_REGISTER int sz = size();
-			if( sz == capacity() )
+#ifdef BT_USE_PLACEMENT_NEW
+			for (int i = curSize; i < newsize; i++)
 			{
-				reserve( allocSize(size()) );
+				new (&m_data[i]) T(fillData);
 			}
-			m_size++;
+#endif  //BT_USE_PLACEMENT_NEW
+		}
 
-			return m_data[sz];		
+		m_size = newsize;
+	}
+	SIMD_FORCE_INLINE T& expandNonInitializing()
+	{
+		const int sz = size();
+		if (sz == capacity())
+		{
+			reserve(allocSize(size()));
 		}
+		m_size++;
 
+		return m_data[sz];
+	}
 
-		SIMD_FORCE_INLINE	T&  expand( const T& fillValue=T())
-		{	
-			const BT_REGISTER int sz = size();
-			if( sz == capacity() )
-			{
-				reserve( allocSize(size()) );
-			}
-			m_size++;
+	SIMD_FORCE_INLINE T& expand(const T& fillValue = T())
+	{
+		const int sz = size();
+		if (sz == capacity())
+		{
+			reserve(allocSize(size()));
+		}
+		m_size++;
 #ifdef BT_USE_PLACEMENT_NEW
-			new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory)
+		new (&m_data[sz]) T(fillValue);  //use the in-place new (not really allocating heap memory)
 #endif
 
-			return m_data[sz];		
-		}
+		return m_data[sz];
+	}
 
+	SIMD_FORCE_INLINE void push_back(const T& _Val)
+	{
+		const int sz = size();
+		if (sz == capacity())
+		{
+			reserve(allocSize(size()));
+		}
 
-		SIMD_FORCE_INLINE	void push_back(const T& _Val)
-		{	
-			const BT_REGISTER int sz = size();
-			if( sz == capacity() )
-			{
-				reserve( allocSize(size()) );
-			}
-			
 #ifdef BT_USE_PLACEMENT_NEW
-			new ( &m_data[m_size] ) T(_Val);
+		new (&m_data[m_size]) T(_Val);
 #else
-			m_data[size()] = _Val;			
-#endif //BT_USE_PLACEMENT_NEW
+		m_data[size()] = _Val;
+#endif  //BT_USE_PLACEMENT_NEW
 
-			m_size++;
-		}
-
-	
-		/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
-		SIMD_FORCE_INLINE	int capacity() const
-		{	
-			return m_capacity;
-		}
-		
-		SIMD_FORCE_INLINE	void reserve(int _Count)
-		{	// determine new minimum length of allocated storage
-			if (capacity() < _Count)
-			{	// not enough room, reallocate
-				T*	s = (T*)allocate(_Count);
+		m_size++;
+	}
 
-				copy(0, size(), s);
+	/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
+	SIMD_FORCE_INLINE int capacity() const
+	{
+		return m_capacity;
+	}
 
-				destroy(0,size());
+	SIMD_FORCE_INLINE void reserve(int _Count)
+	{  // determine new minimum length of allocated storage
+		if (capacity() < _Count)
+		{  // not enough room, reallocate
+			T* s = (T*)allocate(_Count);
 
-				deallocate();
-				
-				//PCK: added this line
-				m_ownsMemory = true;
+			copy(0, size(), s);
 
-				m_data = s;
-				
-				m_capacity = _Count;
+			destroy(0, size());
 
-			}
-		}
+			deallocate();
 
+			//PCK: added this line
+			m_ownsMemory = true;
 
-		class less
-		{
-			public:
+			m_data = s;
 
-				bool operator() ( const T& a, const T& b )
-				{
-					return ( a < b );
-				}
-		};
-	
+			m_capacity = _Count;
+		}
+	}
 
-		template <typename L>
-		void quickSortInternal(const L& CompareFunc,int lo, int hi)
+	class less
+	{
+	public:
+		bool operator()(const T& a, const T& b) const
 		{
-		//  lo is the lower index, hi is the upper index
-		//  of the region of array a that is to be sorted
-			int i=lo, j=hi;
-			T x=m_data[(lo+hi)/2];
-
-			//  partition
-			do
-			{    
-				while (CompareFunc(m_data[i],x)) 
-					i++; 
-				while (CompareFunc(x,m_data[j])) 
-					j--;
-				if (i<=j)
-				{
-					swap(i,j);
-					i++; j--;
-				}
-			} while (i<=j);
-
-			//  recursion
-			if (lo<j) 
-				quickSortInternal( CompareFunc, lo, j);
-			if (i<hi) 
-				quickSortInternal( CompareFunc, i, hi);
+			return (a < b);
 		}
+	};
 
+	template <typename L>
+	void quickSortInternal(const L& CompareFunc, int lo, int hi)
+	{
+		//  lo is the lower index, hi is the upper index
+		//  of the region of array a that is to be sorted
+		int i = lo, j = hi;
+		T x = m_data[(lo + hi) / 2];
 
-		template <typename L>
-		void quickSort(const L& CompareFunc)
+		//  partition
+		do
 		{
-			//don't sort 0 or 1 elements
-			if (size()>1)
+			while (CompareFunc(m_data[i], x))
+				i++;
+			while (CompareFunc(x, m_data[j]))
+				j--;
+			if (i <= j)
 			{
-				quickSortInternal(CompareFunc,0,size()-1);
+				swap(i, j);
+				i++;
+				j--;
 			}
+		} while (i <= j);
+
+		//  recursion
+		if (lo < j)
+			quickSortInternal(CompareFunc, lo, j);
+		if (i < hi)
+			quickSortInternal(CompareFunc, i, hi);
+	}
+
+	template <typename L>
+	void quickSort(const L& CompareFunc)
+	{
+		//don't sort 0 or 1 elements
+		if (size() > 1)
+		{
+			quickSortInternal(CompareFunc, 0, size() - 1);
 		}
+	}
 
+	///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
+	template <typename L>
+	void downHeap(T* pArr, int k, int n, const L& CompareFunc)
+	{
+		/*  PRE: a[k+1..N] is a heap */
+		/* POST:  a[k..N]  is a heap */
 
-		///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
-		template <typename L>
-		void downHeap(T *pArr, int k, int n, const L& CompareFunc)
+		T temp = pArr[k - 1];
+		/* k has child(s) */
+		while (k <= n / 2)
 		{
-			/*  PRE: a[k+1..N] is a heap */
-			/* POST:  a[k..N]  is a heap */
-			
-			T temp = pArr[k - 1];
-			/* k has child(s) */
-			while (k <= n/2) 
+			int child = 2 * k;
+
+			if ((child < n) && CompareFunc(pArr[child - 1], pArr[child]))
+			{
+				child++;
+			}
+			/* pick larger child */
+			if (CompareFunc(temp, pArr[child - 1]))
+			{
+				/* move child up */
+				pArr[k - 1] = pArr[child - 1];
+				k = child;
+			}
+			else
 			{
-				int child = 2*k;
-				
-				if ((child < n) && CompareFunc(pArr[child - 1] , pArr[child]))
-				{
-					child++;
-				}
-				/* pick larger child */
-				if (CompareFunc(temp , pArr[child - 1]))
-				{
-					/* move child up */
-					pArr[k - 1] = pArr[child - 1];
-					k = child;
-				}
-				else
-				{
-					break;
-				}
+				break;
 			}
-			pArr[k - 1] = temp;
-		} /*downHeap*/
+		}
+		pArr[k - 1] = temp;
+	} /*downHeap*/
 
-		void	swap(int index0,int index1)
-		{
+	void swap(int index0, int index1)
+	{
 #ifdef BT_USE_MEMCPY
-			char	temp[sizeof(T)];
-			memcpy(temp,&m_data[index0],sizeof(T));
-			memcpy(&m_data[index0],&m_data[index1],sizeof(T));
-			memcpy(&m_data[index1],temp,sizeof(T));
+		char temp[sizeof(T)];
+		memcpy(temp, &m_data[index0], sizeof(T));
+		memcpy(&m_data[index0], &m_data[index1], sizeof(T));
+		memcpy(&m_data[index1], temp, sizeof(T));
 #else
-			T temp = m_data[index0];
-			m_data[index0] = m_data[index1];
-			m_data[index1] = temp;
-#endif //BT_USE_PLACEMENT_NEW
-
-		}
+		T temp = m_data[index0];
+		m_data[index0] = m_data[index1];
+		m_data[index1] = temp;
+#endif  //BT_USE_PLACEMENT_NEW
+	}
 
 	template <typename L>
 	void heapSort(const L& CompareFunc)
@@ -423,49 +399,48 @@ protected:
 		/* sort a[0..N-1],  N.B. 0 to N-1 */
 		int k;
 		int n = m_size;
-		for (k = n/2; k > 0; k--) 
+		for (k = n / 2; k > 0; k--)
 		{
 			downHeap(m_data, k, n, CompareFunc);
 		}
 
 		/* a[1..N] is now a heap */
-		while ( n>=1 ) 
+		while (n >= 1)
 		{
-			swap(0,n-1); /* largest of a[0..n-1] */
-
+			swap(0, n - 1); /* largest of a[0..n-1] */
 
 			n = n - 1;
 			/* restore a[1..i-1] heap */
 			downHeap(m_data, 1, n, CompareFunc);
-		} 
+		}
 	}
 
 	///non-recursive binary search, assumes sorted array
-	int	findBinarySearch(const T& key) const
+	int findBinarySearch(const T& key) const
 	{
 		int first = 0;
-		int last = size()-1;
+		int last = size() - 1;
 
 		//assume sorted array
-		while (first <= last) {
+		while (first <= last)
+		{
 			int mid = (first + last) / 2;  // compute mid point.
-			if (key > m_data[mid]) 
+			if (key > m_data[mid])
 				first = mid + 1;  // repeat search in top half.
-			else if (key < m_data[mid]) 
-				last = mid - 1; // repeat search in bottom half.
+			else if (key < m_data[mid])
+				last = mid - 1;  // repeat search in bottom half.
 			else
-				return mid;     // found it. return position /////
+				return mid;  // found it. return position /////
 		}
-		return size();    // failed to find key
+		return size();  // failed to find key
 	}
 
-
-	int	findLinearSearch(const T& key) const
+	int findLinearSearch(const T& key) const
 	{
-		int index=size();
+		int index = size();
 		int i;
 
-		for (i=0;i<size();i++)
+		for (i = 0; i < size(); i++)
 		{
 			if (m_data[i] == key)
 			{
@@ -476,19 +451,40 @@ protected:
 		return index;
 	}
 
-	void	remove(const T& key)
+	// If the key is not in the array, return -1 instead of 0,
+	// since 0 also means the first element in the array.
+	int findLinearSearch2(const T& key) const
 	{
+		int index = -1;
+		int i;
 
-		int findIndex = findLinearSearch(key);
-		if (findIndex<size())
+		for (i = 0; i < size(); i++)
 		{
-			swap( findIndex,size()-1);
+			if (m_data[i] == key)
+			{
+				index = i;
+				break;
+			}
+		}
+		return index;
+	}
+
+	void removeAtIndex(int index)
+	{
+		if (index < size())
+		{
+			swap(index, size() - 1);
 			pop_back();
 		}
 	}
+	void remove(const T& key)
+	{
+		int findIndex = findLinearSearch(key);
+		removeAtIndex(findIndex);
+	}
 
 	//PCK: whole function
-	void initializeFromBuffer(void *buffer, int size, int capacity)
+	void initializeFromBuffer(void* buffer, int size, int capacity)
 	{
 		clear();
 		m_ownsMemory = false;
@@ -500,10 +496,9 @@ protected:
 	void copyFromArray(const btAlignedObjectArray& otherArray)
 	{
 		int otherSize = otherArray.size();
-		resize (otherSize);
+		resize(otherSize);
 		otherArray.copy(0, otherSize, m_data);
 	}
-
 };
 
-#endif //BT_OBJECT_ARRAY__
+#endif  //BT_OBJECT_ARRAY__
diff --git a/extern/bullet2/src/LinearMath/btConvexHull.cpp b/extern/bullet2/src/LinearMath/btConvexHull.cpp
index 2ae855dbc1f..e7de2a3694c 100644
--- a/extern/bullet2/src/LinearMath/btConvexHull.cpp
+++ b/extern/bullet2/src/LinearMath/btConvexHull.cpp
@@ -20,51 +20,52 @@ subject to the following restrictions:
 #include "btMinMax.h"
 #include "btVector3.h"
 
-
-
-
-
 //----------------------------------
 
-class int3  
+class int3
 {
 public:
-	int x,y,z;
+	int x, y, z;
 	int3(){};
-	int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;}
-	const int& operator[](int i) const {return (&x)[i];}
-	int& operator[](int i) {return (&x)[i];}
+	int3(int _x, int _y, int _z)
+	{
+		x = _x;
+		y = _y;
+		z = _z;
+	}
+	const int &operator[](int i) const { return (&x)[i]; }
+	int &operator[](int i) { return (&x)[i]; }
 };
 
-
 //------- btPlane ----------
 
-
-inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);}
-inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); }
-inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==PlaneFlip(b)); }
-
+inline btPlane PlaneFlip(const btPlane &plane) { return btPlane(-plane.normal, -plane.dist); }
+inline int operator==(const btPlane &a, const btPlane &b) { return (a.normal == b.normal && a.dist == b.dist); }
+inline int coplanar(const btPlane &a, const btPlane &b) { return (a == b || a == PlaneFlip(b)); }
 
 //--------- Utility Functions ------
 
-btVector3  PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1);
-btVector3  PlaneProject(const btPlane &plane, const btVector3 &point);
+btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1);
+btVector3 PlaneProject(const btPlane &plane, const btVector3 &point);
 
-btVector3  ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2);
-btVector3  ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2)
+btVector3 ThreePlaneIntersection(const btPlane &p0, const btPlane &p1, const btPlane &p2);
+btVector3 ThreePlaneIntersection(const btPlane &p0, const btPlane &p1, const btPlane &p2)
 {
 	btVector3 N1 = p0.normal;
 	btVector3 N2 = p1.normal;
 	btVector3 N3 = p2.normal;
 
-	btVector3 n2n3; n2n3 = N2.cross(N3);
-	btVector3 n3n1; n3n1 = N3.cross(N1);
-	btVector3 n1n2; n1n2 = N1.cross(N2);
+	btVector3 n2n3;
+	n2n3 = N2.cross(N3);
+	btVector3 n3n1;
+	n3n1 = N3.cross(N1);
+	btVector3 n1n2;
+	n1n2 = N1.cross(N2);
 
 	btScalar quotient = (N1.dot(n2n3));
 
 	btAssert(btFabs(quotient) > btScalar(0.000001));
-	
+
 	quotient = btScalar(-1.) / quotient;
 	n2n3 *= p0.dist;
 	n3n1 *= p1.dist;
@@ -74,105 +75,96 @@ btVector3  ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btP
 	potentialVertex += n1n2;
 	potentialVertex *= quotient;
 
-	btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ());
+	btVector3 result(potentialVertex.getX(), potentialVertex.getY(), potentialVertex.getZ());
 	return result;
-
 }
 
-btScalar   DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL);
-btVector3  TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2);
-btVector3  NormalOf(const btVector3 *vert, const int n);
-
+btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint = NULL, btVector3 *vpoint = NULL);
+btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2);
+btVector3 NormalOf(const btVector3 *vert, const int n);
 
 btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1)
 {
 	// returns the point where the line p0-p1 intersects the plane n&d
-				static btVector3 dif;
-		dif = p1-p0;
-				btScalar dn= btDot(plane.normal,dif);
-				btScalar t = -(plane.dist+btDot(plane.normal,p0) )/dn;
-				return p0 + (dif*t);
+	btVector3 dif;
+	dif = p1 - p0;
+	btScalar dn = btDot(plane.normal, dif);
+	btScalar t = -(plane.dist + btDot(plane.normal, p0)) / dn;
+	return p0 + (dif * t);
 }
 
 btVector3 PlaneProject(const btPlane &plane, const btVector3 &point)
 {
-	return point - plane.normal * (btDot(point,plane.normal)+plane.dist);
+	return point - plane.normal * (btDot(point, plane.normal) + plane.dist);
 }
 
 btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2)
 {
 	// return the normal of the triangle
 	// inscribed by v0, v1, and v2
-	btVector3 cp=btCross(v1-v0,v2-v1);
-	btScalar m=cp.length();
-	if(m==0) return btVector3(1,0,0);
-	return cp*(btScalar(1.0)/m);
+	btVector3 cp = btCross(v1 - v0, v2 - v1);
+	btScalar m = cp.length();
+	if (m == 0) return btVector3(1, 0, 0);
+	return cp * (btScalar(1.0) / m);
 }
 
-
 btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint)
 {
-	static btVector3 cp;
-	cp = btCross(udir,vdir).normalized();
+	btVector3 cp;
+	cp = btCross(udir, vdir).normalized();
 
-	btScalar distu = -btDot(cp,ustart);
-	btScalar distv = -btDot(cp,vstart);
-	btScalar dist = (btScalar)fabs(distu-distv);
-	if(upoint) 
-		{
+	btScalar distu = -btDot(cp, ustart);
+	btScalar distv = -btDot(cp, vstart);
+	btScalar dist = (btScalar)fabs(distu - distv);
+	if (upoint)
+	{
 		btPlane plane;
-		plane.normal = btCross(vdir,cp).normalized();
-		plane.dist = -btDot(plane.normal,vstart);
-		*upoint = PlaneLineIntersection(plane,ustart,ustart+udir);
+		plane.normal = btCross(vdir, cp).normalized();
+		plane.dist = -btDot(plane.normal, vstart);
+		*upoint = PlaneLineIntersection(plane, ustart, ustart + udir);
 	}
-	if(vpoint) 
-		{
+	if (vpoint)
+	{
 		btPlane plane;
-		plane.normal = btCross(udir,cp).normalized();
-		plane.dist = -btDot(plane.normal,ustart);
-		*vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir);
+		plane.normal = btCross(udir, cp).normalized();
+		plane.dist = -btDot(plane.normal, ustart);
+		*vpoint = PlaneLineIntersection(plane, vstart, vstart + vdir);
 	}
 	return dist;
 }
 
-
-
-
-
-
-
-#define COPLANAR   (0)
-#define UNDER      (1)
-#define OVER       (2)
-#define SPLIT      (OVER|UNDER)
+#define COPLANAR (0)
+#define UNDER (1)
+#define OVER (2)
+#define SPLIT (OVER | UNDER)
 #define PAPERWIDTH (btScalar(0.001))
 
 btScalar planetestepsilon = PAPERWIDTH;
 
-
-
 typedef ConvexH::HalfEdge HalfEdge;
 
-ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size)
+ConvexH::ConvexH(int vertices_size, int edges_size, int facets_size)
 {
 	vertices.resize(vertices_size);
 	edges.resize(edges_size);
 	facets.resize(facets_size);
 }
 
-
 int PlaneTest(const btPlane &p, const btVector3 &v);
-int PlaneTest(const btPlane &p, const btVector3 &v) {
-	btScalar a  = btDot(v,p.normal)+p.dist;
-	int   flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR);
+int PlaneTest(const btPlane &p, const btVector3 &v)
+{
+	btScalar a = btDot(v, p.normal) + p.dist;
+	int flag = (a > planetestepsilon) ? OVER : ((a < -planetestepsilon) ? UNDER : COPLANAR);
 	return flag;
 }
 
-int SplitTest(ConvexH &convex,const btPlane &plane);
-int SplitTest(ConvexH &convex,const btPlane &plane) {
-	int flag=0;
-	for(int i=0;i<convex.vertices.size();i++) {
-		flag |= PlaneTest(plane,convex.vertices[i]);
+int SplitTest(ConvexH &convex, const btPlane &plane);
+int SplitTest(ConvexH &convex, const btPlane &plane)
+{
+	int flag = 0;
+	for (int i = 0; i < convex.vertices.size(); i++)
+	{
+		flag |= PlaneTest(plane, convex.vertices[i]);
 	}
 	return flag;
 }
@@ -185,7 +177,7 @@ public:
 	unsigned char undermap;
 	unsigned char overmap;
 };
-class EdgeFlag 
+class EdgeFlag
 {
 public:
 	unsigned char planetest;
@@ -199,146 +191,135 @@ public:
 	unsigned char undermap;
 	unsigned char overmap;
 };
-class Coplanar{
+class Coplanar
+{
 public:
 	unsigned short ea;
 	unsigned char v0;
 	unsigned char v1;
 };
 
-
-
-
-
-
-
-
-template<class T>
-int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
+template <class T>
+int maxdirfiltered(const T *p, int count, const T &dir, btAlignedObjectArray<int> &allow)
 {
 	btAssert(count);
-	int m=-1;
-	for(int i=0;i<count;i++) 
-		if(allow[i])
+	int m = -1;
+	for (int i = 0; i < count; i++)
+		if (allow[i])
 		{
-			if(m==-1 || btDot(p[i],dir)>btDot(p[m],dir))
-				m=i;
+			if (m == -1 || btDot(p[i], dir) > btDot(p[m], dir))
+				m = i;
 		}
-	btAssert(m!=-1);
+	btAssert(m != -1);
 	return m;
-} 
+}
 
 btVector3 orth(const btVector3 &v);
 btVector3 orth(const btVector3 &v)
 {
-	btVector3 a=btCross(v,btVector3(0,0,1));
-	btVector3 b=btCross(v,btVector3(0,1,0));
+	btVector3 a = btCross(v, btVector3(0, 0, 1));
+	btVector3 b = btCross(v, btVector3(0, 1, 0));
 	if (a.length() > b.length())
 	{
 		return a.normalized();
-	} else {
+	}
+	else
+	{
 		return b.normalized();
 	}
 }
 
-
-template<class T>
-int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
+template <class T>
+int maxdirsterid(const T *p, int count, const T &dir, btAlignedObjectArray<int> &allow)
 {
-	int m=-1;
-	while(m==-1)
+	int m = -1;
+	while (m == -1)
 	{
-		m = maxdirfiltered(p,count,dir,allow);
-		if(allow[m]==3) return m;
+		m = maxdirfiltered(p, count, dir, allow);
+		if (allow[m] == 3) return m;
 		T u = orth(dir);
-		T v = btCross(u,dir);
-		int ma=-1;
-		for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
+		T v = btCross(u, dir);
+		int ma = -1;
+		for (btScalar x = btScalar(0.0); x <= btScalar(360.0); x += btScalar(45.0))
 		{
-			btScalar s = btSin(SIMD_RADS_PER_DEG*(x));
-			btScalar c = btCos(SIMD_RADS_PER_DEG*(x));
-			int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
-			if(ma==m && mb==m)
+			btScalar s = btSin(SIMD_RADS_PER_DEG * (x));
+			btScalar c = btCos(SIMD_RADS_PER_DEG * (x));
+			int mb = maxdirfiltered(p, count, dir + (u * s + v * c) * btScalar(0.025), allow);
+			if (ma == m && mb == m)
 			{
-				allow[m]=3;
+				allow[m] = 3;
 				return m;
 			}
-			if(ma!=-1 && ma!=mb)  // Yuck - this is really ugly
+			if (ma != -1 && ma != mb)  // Yuck - this is really ugly
 			{
 				int mc = ma;
-				for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0))
+				for (btScalar xx = x - btScalar(40.0); xx <= x; xx += btScalar(5.0))
 				{
-					btScalar s = btSin(SIMD_RADS_PER_DEG*(xx));
-					btScalar c = btCos(SIMD_RADS_PER_DEG*(xx));
-					int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
-					if(mc==m && md==m)
+					btScalar s = btSin(SIMD_RADS_PER_DEG * (xx));
+					btScalar c = btCos(SIMD_RADS_PER_DEG * (xx));
+					int md = maxdirfiltered(p, count, dir + (u * s + v * c) * btScalar(0.025), allow);
+					if (mc == m && md == m)
 					{
-						allow[m]=3;
+						allow[m] = 3;
 						return m;
 					}
-					mc=md;
+					mc = md;
 				}
 			}
-			ma=mb;
+			ma = mb;
 		}
-		allow[m]=0;
-		m=-1;
+		allow[m] = 0;
+		m = -1;
 	}
 	btAssert(0);
 	return m;
-} 
-
-
-
+}
 
-int operator ==(const int3 &a,const int3 &b);
-int operator ==(const int3 &a,const int3 &b) 
+int operator==(const int3 &a, const int3 &b);
+int operator==(const int3 &a, const int3 &b)
 {
-	for(int i=0;i<3;i++) 
+	for (int i = 0; i < 3; i++)
 	{
-		if(a[i]!=b[i]) return 0;
+		if (a[i] != b[i]) return 0;
 	}
 	return 1;
 }
 
-
-int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon);
-int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon) 
+int above(btVector3 *vertices, const int3 &t, const btVector3 &p, btScalar epsilon);
+int above(btVector3 *vertices, const int3 &t, const btVector3 &p, btScalar epsilon)
 {
-	btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
-	return (btDot(n,p-vertices[t[0]]) > epsilon); // EPSILON???
+	btVector3 n = TriNormal(vertices[t[0]], vertices[t[1]], vertices[t[2]]);
+	return (btDot(n, p - vertices[t[0]]) > epsilon);  // EPSILON???
 }
-int hasedge(const int3 &t, int a,int b);
-int hasedge(const int3 &t, int a,int b)
+int hasedge(const int3 &t, int a, int b);
+int hasedge(const int3 &t, int a, int b)
 {
-	for(int i=0;i<3;i++)
+	for (int i = 0; i < 3; i++)
 	{
-		int i1= (i+1)%3;
-		if(t[i]==a && t[i1]==b) return 1;
+		int i1 = (i + 1) % 3;
+		if (t[i] == a && t[i1] == b) return 1;
 	}
 	return 0;
 }
 int hasvert(const int3 &t, int v);
 int hasvert(const int3 &t, int v)
 {
-	return (t[0]==v || t[1]==v || t[2]==v) ;
+	return (t[0] == v || t[1] == v || t[2] == v);
 }
-int shareedge(const int3 &a,const int3 &b);
-int shareedge(const int3 &a,const int3 &b)
+int shareedge(const int3 &a, const int3 &b);
+int shareedge(const int3 &a, const int3 &b)
 {
 	int i;
-	for(i=0;i<3;i++)
+	for (i = 0; i < 3; i++)
 	{
-		int i1= (i+1)%3;
-		if(hasedge(a,b[i1],b[i])) return 1;
+		int i1 = (i + 1) % 3;
+		if (hasedge(a, b[i1], b[i])) return 1;
 	}
 	return 0;
 }
 
 class btHullTriangle;
 
-
-
 class btHullTriangle : public int3
 {
 public:
@@ -346,51 +327,50 @@ public:
 	int id;
 	int vmax;
 	btScalar rise;
-	btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1)
+	btHullTriangle(int a, int b, int c) : int3(a, b, c), n(-1, -1, -1)
 	{
-		vmax=-1;
+		vmax = -1;
 		rise = btScalar(0.0);
 	}
 	~btHullTriangle()
 	{
 	}
-	int &neib(int a,int b);
+	int &neib(int a, int b);
 };
 
-
-int &btHullTriangle::neib(int a,int b)
+int &btHullTriangle::neib(int a, int b)
 {
-	static int er=-1;
+	static int er = -1;
 	int i;
-	for(i=0;i<3;i++) 
+	for (i = 0; i < 3; i++)
 	{
-		int i1=(i+1)%3;
-		int i2=(i+2)%3;
-		if((*this)[i]==a && (*this)[i1]==b) return n[i2];
-		if((*this)[i]==b && (*this)[i1]==a) return n[i2];
+		int i1 = (i + 1) % 3;
+		int i2 = (i + 2) % 3;
+		if ((*this)[i] == a && (*this)[i1] == b) return n[i2];
+		if ((*this)[i] == b && (*this)[i1] == a) return n[i2];
 	}
 	btAssert(0);
 	return er;
 }
-void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t)
+void HullLibrary::b2bfix(btHullTriangle *s, btHullTriangle *t)
 {
 	int i;
-	for(i=0;i<3;i++) 
+	for (i = 0; i < 3; i++)
 	{
-		int i1=(i+1)%3;
-		int i2=(i+2)%3;
+		int i1 = (i + 1) % 3;
+		int i2 = (i + 2) % 3;
 		int a = (*s)[i1];
 		int b = (*s)[i2];
-		btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id);
-		btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->id);
-		m_tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a);
-		m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b);
+		btAssert(m_tris[s->neib(a, b)]->neib(b, a) == s->id);
+		btAssert(m_tris[t->neib(a, b)]->neib(b, a) == t->id);
+		m_tris[s->neib(a, b)]->neib(b, a) = t->neib(b, a);
+		m_tris[t->neib(b, a)]->neib(a, b) = s->neib(a, b);
 	}
 }
 
-void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t)
+void HullLibrary::removeb2b(btHullTriangle *s, btHullTriangle *t)
 {
-	b2bfix(s,t);
+	b2bfix(s, t);
 	deAllocateTriangle(s);
 
 	deAllocateTriangle(t);
@@ -401,11 +381,11 @@ void HullLibrary::checkit(btHullTriangle *t)
 	(void)t;
 
 	int i;
-	btAssert(m_tris[t->id]==t);
-	for(i=0;i<3;i++)
+	btAssert(m_tris[t->id] == t);
+	for (i = 0; i < 3; i++)
 	{
-		int i1=(i+1)%3;
-		int i2=(i+2)%3;
+		int i1 = (i + 1) % 3;
+		int i2 = (i + 2) % 3;
 		int a = (*t)[i1];
 		int b = (*t)[i2];
 
@@ -415,226 +395,233 @@ void HullLibrary::checkit(btHullTriangle *t)
 		(void)a;
 		(void)b;
 
-		btAssert(a!=b);
-		btAssert( m_tris[t->n[i]]->neib(b,a) == t->id);
+		btAssert(a != b);
+		btAssert(m_tris[t->n[i]]->neib(b, a) == t->id);
 	}
 }
 
-btHullTriangle*	HullLibrary::allocateTriangle(int a,int b,int c)
+btHullTriangle *HullLibrary::allocateTriangle(int a, int b, int c)
 {
-	void* mem = btAlignedAlloc(sizeof(btHullTriangle),16);
-	btHullTriangle* tr = new (mem)btHullTriangle(a,b,c);
+	void *mem = btAlignedAlloc(sizeof(btHullTriangle), 16);
+	btHullTriangle *tr = new (mem) btHullTriangle(a, b, c);
 	tr->id = m_tris.size();
 	m_tris.push_back(tr);
 
 	return tr;
 }
 
-void	HullLibrary::deAllocateTriangle(btHullTriangle* tri)
+void HullLibrary::deAllocateTriangle(btHullTriangle *tri)
 {
-	btAssert(m_tris[tri->id]==tri);
-	m_tris[tri->id]=NULL;
+	btAssert(m_tris[tri->id] == tri);
+	m_tris[tri->id] = NULL;
 	tri->~btHullTriangle();
 	btAlignedFree(tri);
 }
 
-
-void HullLibrary::extrude(btHullTriangle *t0,int v)
+void HullLibrary::extrude(btHullTriangle *t0, int v)
 {
-	int3 t= *t0;
+	int3 t = *t0;
 	int n = m_tris.size();
-	btHullTriangle* ta = allocateTriangle(v,t[1],t[2]);
-	ta->n = int3(t0->n[0],n+1,n+2);
-	m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0;
-	btHullTriangle* tb = allocateTriangle(v,t[2],t[0]);
-	tb->n = int3(t0->n[1],n+2,n+0);
-	m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1;
-	btHullTriangle* tc = allocateTriangle(v,t[0],t[1]);
-	tc->n = int3(t0->n[2],n+0,n+1);
-	m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2;
+	btHullTriangle *ta = allocateTriangle(v, t[1], t[2]);
+	ta->n = int3(t0->n[0], n + 1, n + 2);
+	m_tris[t0->n[0]]->neib(t[1], t[2]) = n + 0;
+	btHullTriangle *tb = allocateTriangle(v, t[2], t[0]);
+	tb->n = int3(t0->n[1], n + 2, n + 0);
+	m_tris[t0->n[1]]->neib(t[2], t[0]) = n + 1;
+	btHullTriangle *tc = allocateTriangle(v, t[0], t[1]);
+	tc->n = int3(t0->n[2], n + 0, n + 1);
+	m_tris[t0->n[2]]->neib(t[0], t[1]) = n + 2;
 	checkit(ta);
 	checkit(tb);
 	checkit(tc);
-	if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->n[0]]);
-	if(hasvert(*m_tris[tb->n[0]],v)) removeb2b(tb,m_tris[tb->n[0]]);
-	if(hasvert(*m_tris[tc->n[0]],v)) removeb2b(tc,m_tris[tc->n[0]]);
+	if (hasvert(*m_tris[ta->n[0]], v)) removeb2b(ta, m_tris[ta->n[0]]);
+	if (hasvert(*m_tris[tb->n[0]], v)) removeb2b(tb, m_tris[tb->n[0]]);
+	if (hasvert(*m_tris[tc->n[0]], v)) removeb2b(tc, m_tris[tc->n[0]]);
 	deAllocateTriangle(t0);
-
 }
 
-btHullTriangle* HullLibrary::extrudable(btScalar epsilon)
+btHullTriangle *HullLibrary::extrudable(btScalar epsilon)
 {
 	int i;
-	btHullTriangle *t=NULL;
-	for(i=0;i<m_tris.size();i++)
+	btHullTriangle *t = NULL;
+	for (i = 0; i < m_tris.size(); i++)
 	{
-		if(!t || (m_tris[i] && t->rise<m_tris[i]->rise))
+		if (!t || (m_tris[i] && t->rise < m_tris[i]->rise))
 		{
 			t = m_tris[i];
 		}
 	}
-	return (t->rise >epsilon)?t:NULL ;
+	return (t->rise > epsilon) ? t : NULL;
 }
 
-
-
-
-int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow)
+int4 HullLibrary::FindSimplex(btVector3 *verts, int verts_count, btAlignedObjectArray<int> &allow)
 {
 	btVector3 basis[3];
-	basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) );      
-	int p0 = maxdirsterid(verts,verts_count, basis[0],allow);   
-	int	p1 = maxdirsterid(verts,verts_count,-basis[0],allow);
-	basis[0] = verts[p0]-verts[p1];
-	if(p0==p1 || basis[0]==btVector3(0,0,0)) 
-		return int4(-1,-1,-1,-1);
-	basis[1] = btCross(btVector3(     btScalar(1),btScalar(0.02), btScalar(0)),basis[0]);
-	basis[2] = btCross(btVector3(btScalar(-0.02),     btScalar(1), btScalar(0)),basis[0]);
+	basis[0] = btVector3(btScalar(0.01), btScalar(0.02), btScalar(1.0));
+	int p0 = maxdirsterid(verts, verts_count, basis[0], allow);
+	int p1 = maxdirsterid(verts, verts_count, -basis[0], allow);
+	basis[0] = verts[p0] - verts[p1];
+	if (p0 == p1 || basis[0] == btVector3(0, 0, 0))
+		return int4(-1, -1, -1, -1);
+	basis[1] = btCross(btVector3(btScalar(1), btScalar(0.02), btScalar(0)), basis[0]);
+	basis[2] = btCross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)), basis[0]);
 	if (basis[1].length() > basis[2].length())
 	{
 		basis[1].normalize();
-	} else {
+	}
+	else
+	{
 		basis[1] = basis[2];
-		basis[1].normalize ();
+		basis[1].normalize();
 	}
-	int p2 = maxdirsterid(verts,verts_count,basis[1],allow);
-	if(p2 == p0 || p2 == p1)
+	int p2 = maxdirsterid(verts, verts_count, basis[1], allow);
+	if (p2 == p0 || p2 == p1)
 	{
-		p2 = maxdirsterid(verts,verts_count,-basis[1],allow);
+		p2 = maxdirsterid(verts, verts_count, -basis[1], allow);
 	}
-	if(p2 == p0 || p2 == p1) 
-		return int4(-1,-1,-1,-1);
+	if (p2 == p0 || p2 == p1)
+		return int4(-1, -1, -1, -1);
 	basis[1] = verts[p2] - verts[p0];
-	basis[2] = btCross(basis[1],basis[0]).normalized();
-	int p3 = maxdirsterid(verts,verts_count,basis[2],allow);
-	if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow);
-	if(p3==p0||p3==p1||p3==p2) 
-		return int4(-1,-1,-1,-1);
-	btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3));
-	if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {btSwap(p2,p3);}
-	return int4(p0,p1,p2,p3);
+	basis[2] = btCross(basis[1], basis[0]).normalized();
+	int p3 = maxdirsterid(verts, verts_count, basis[2], allow);
+	if (p3 == p0 || p3 == p1 || p3 == p2) p3 = maxdirsterid(verts, verts_count, -basis[2], allow);
+	if (p3 == p0 || p3 == p1 || p3 == p2)
+		return int4(-1, -1, -1, -1);
+	btAssert(!(p0 == p1 || p0 == p2 || p0 == p3 || p1 == p2 || p1 == p3 || p2 == p3));
+	if (btDot(verts[p3] - verts[p0], btCross(verts[p1] - verts[p0], verts[p2] - verts[p0])) < 0)
+	{
+		btSwap(p2, p3);
+	}
+	return int4(p0, p1, p2, p3);
 }
 
-int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit)
+int HullLibrary::calchullgen(btVector3 *verts, int verts_count, int vlimit)
 {
-	if(verts_count <4) return 0;
-	if(vlimit==0) vlimit=1000000000;
+	if (verts_count < 4) return 0;
+	if (vlimit == 0) vlimit = 1000000000;
 	int j;
-	btVector3 bmin(*verts),bmax(*verts);
+	btVector3 bmin(*verts), bmax(*verts);
 	btAlignedObjectArray<int> isextreme;
 	isextreme.reserve(verts_count);
 	btAlignedObjectArray<int> allow;
 	allow.reserve(verts_count);
 
-	for(j=0;j<verts_count;j++) 
+	for (j = 0; j < verts_count; j++)
 	{
 		allow.push_back(1);
 		isextreme.push_back(0);
-		bmin.setMin (verts[j]);
-		bmax.setMax (verts[j]);
+		bmin.setMin(verts[j]);
+		bmax.setMax(verts[j]);
 	}
-	btScalar epsilon = (bmax-bmin).length() * btScalar(0.001);
-	btAssert (epsilon != 0.0);
-
-
-	int4 p = FindSimplex(verts,verts_count,allow);
-	if(p.x==-1) return 0; // simplex failed
-
-
-
-	btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0);  // a valid interior point
-	btHullTriangle *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1);
-	btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0);
-	btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3);
-	btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2);
-	isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1;
-	checkit(t0);checkit(t1);checkit(t2);checkit(t3);
-
-	for(j=0;j<m_tris.size();j++)
+	btScalar epsilon = (bmax - bmin).length() * btScalar(0.001);
+	btAssert(epsilon != 0.0);
+
+	int4 p = FindSimplex(verts, verts_count, allow);
+	if (p.x == -1) return 0;  // simplex failed
+
+	btVector3 center = (verts[p[0]] + verts[p[1]] + verts[p[2]] + verts[p[3]]) / btScalar(4.0);  // a valid interior point
+	btHullTriangle *t0 = allocateTriangle(p[2], p[3], p[1]);
+	t0->n = int3(2, 3, 1);
+	btHullTriangle *t1 = allocateTriangle(p[3], p[2], p[0]);
+	t1->n = int3(3, 2, 0);
+	btHullTriangle *t2 = allocateTriangle(p[0], p[1], p[3]);
+	t2->n = int3(0, 1, 3);
+	btHullTriangle *t3 = allocateTriangle(p[1], p[0], p[2]);
+	t3->n = int3(1, 0, 2);
+	isextreme[p[0]] = isextreme[p[1]] = isextreme[p[2]] = isextreme[p[3]] = 1;
+	checkit(t0);
+	checkit(t1);
+	checkit(t2);
+	checkit(t3);
+
+	for (j = 0; j < m_tris.size(); j++)
 	{
-		btHullTriangle *t=m_tris[j];
+		btHullTriangle *t = m_tris[j];
 		btAssert(t);
-		btAssert(t->vmax<0);
-		btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
-		t->vmax = maxdirsterid(verts,verts_count,n,allow);
-		t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
+		btAssert(t->vmax < 0);
+		btVector3 n = TriNormal(verts[(*t)[0]], verts[(*t)[1]], verts[(*t)[2]]);
+		t->vmax = maxdirsterid(verts, verts_count, n, allow);
+		t->rise = btDot(n, verts[t->vmax] - verts[(*t)[0]]);
 	}
 	btHullTriangle *te;
-	vlimit-=4;
-	while(vlimit >0 && ((te=extrudable(epsilon)) != 0))
+	vlimit -= 4;
+	while (vlimit > 0 && ((te = extrudable(epsilon)) != 0))
 	{
 		//int3 ti=*te;
-		int v=te->vmax;
+		int v = te->vmax;
 		btAssert(v != -1);
 		btAssert(!isextreme[v]);  // wtf we've already done this vertex
-		isextreme[v]=1;
+		isextreme[v] = 1;
 		//if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
-		j=m_tris.size();
-		while(j--) {
-			if(!m_tris[j]) continue;
-			int3 t=*m_tris[j];
-			if(above(verts,t,verts[v],btScalar(0.01)*epsilon)) 
+		j = m_tris.size();
+		while (j--)
+		{
+			if (!m_tris[j]) continue;
+			int3 t = *m_tris[j];
+			if (above(verts, t, verts[v], btScalar(0.01) * epsilon))
 			{
-				extrude(m_tris[j],v);
+				extrude(m_tris[j], v);
 			}
 		}
 		// now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
-		j=m_tris.size();
-		while(j--)
+		j = m_tris.size();
+		while (j--)
 		{
-			if(!m_tris[j]) continue;
-			if(!hasvert(*m_tris[j],v)) break;
-			int3 nt=*m_tris[j];
-			if(above(verts,nt,center,btScalar(0.01)*epsilon)  || btCross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) )
+			if (!m_tris[j]) continue;
+			if (!hasvert(*m_tris[j], v)) break;
+			int3 nt = *m_tris[j];
+			if (above(verts, nt, center, btScalar(0.01) * epsilon) || btCross(verts[nt[1]] - verts[nt[0]], verts[nt[2]] - verts[nt[1]]).length() < epsilon * epsilon * btScalar(0.1))
 			{
 				btHullTriangle *nb = m_tris[m_tris[j]->n[0]];
-				btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j);
-				extrude(nb,v);
-				j=m_tris.size(); 
+				btAssert(nb);
+				btAssert(!hasvert(*nb, v));
+				btAssert(nb->id < j);
+				extrude(nb, v);
+				j = m_tris.size();
 			}
-		} 
-		j=m_tris.size();
-		while(j--)
+		}
+		j = m_tris.size();
+		while (j--)
 		{
-			btHullTriangle *t=m_tris[j];
-			if(!t) continue;
-			if(t->vmax>=0) break;
-			btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
-			t->vmax = maxdirsterid(verts,verts_count,n,allow);
-			if(isextreme[t->vmax]) 
+			btHullTriangle *t = m_tris[j];
+			if (!t) continue;
+			if (t->vmax >= 0) break;
+			btVector3 n = TriNormal(verts[(*t)[0]], verts[(*t)[1]], verts[(*t)[2]]);
+			t->vmax = maxdirsterid(verts, verts_count, n, allow);
+			if (isextreme[t->vmax])
 			{
-				t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate.
+				t->vmax = -1;  // already done that vertex - algorithm needs to be able to terminate.
 			}
 			else
 			{
-				t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
+				t->rise = btDot(n, verts[t->vmax] - verts[(*t)[0]]);
 			}
 		}
-		vlimit --;
+		vlimit--;
 	}
 	return 1;
 }
 
-int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit) 
+int HullLibrary::calchull(btVector3 *verts, int verts_count, TUIntArray &tris_out, int &tris_count, int vlimit)
 {
-	int rc=calchullgen(verts,verts_count,  vlimit) ;
-	if(!rc) return 0;
+	int rc = calchullgen(verts, verts_count, vlimit);
+	if (!rc) return 0;
 	btAlignedObjectArray<int> ts;
 	int i;
 
-	for(i=0;i<m_tris.size();i++)
+	for (i = 0; i < m_tris.size(); i++)
 	{
-		if(m_tris[i])
+		if (m_tris[i])
 		{
-			for(int j=0;j<3;j++)
+			for (int j = 0; j < 3; j++)
 				ts.push_back((*m_tris[i])[j]);
 			deAllocateTriangle(m_tris[i]);
 		}
 	}
-	tris_count = ts.size()/3;
+	tris_count = ts.size() / 3;
 	tris_out.resize(ts.size());
-	
-	for (i=0;i<ts.size();i++)
+
+	for (i = 0; i < ts.size(); i++)
 	{
 		tris_out[i] = static_cast<unsigned int>(ts[i]);
 	}
@@ -643,29 +630,22 @@ int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out
 	return 1;
 }
 
-
-
-
-
-bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit)
+bool HullLibrary::ComputeHull(unsigned int vcount, const btVector3 *vertices, PHullResult &result, unsigned int vlimit)
 {
-	
-	int    tris_count;
-	int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast<int>(vlimit) );
-	if(!ret) return false;
-	result.mIndexCount = (unsigned int) (tris_count*3);
-	result.mFaceCount  = (unsigned int) tris_count;
-	result.mVertices   = (btVector3*) vertices;
-	result.mVcount     = (unsigned int) vcount;
+	int tris_count;
+	int ret = calchull((btVector3 *)vertices, (int)vcount, result.m_Indices, tris_count, static_cast<int>(vlimit));
+	if (!ret) return false;
+	result.mIndexCount = (unsigned int)(tris_count * 3);
+	result.mFaceCount = (unsigned int)tris_count;
+	result.mVertices = (btVector3 *)vertices;
+	result.mVcount = (unsigned int)vcount;
 	return true;
-
 }
 
-
 void ReleaseHull(PHullResult &result);
 void ReleaseHull(PHullResult &result)
 {
-	if ( result.m_Indices.size() )
+	if (result.m_Indices.size())
 	{
 		result.m_Indices.clear();
 	}
@@ -675,7 +655,6 @@ void ReleaseHull(PHullResult &result)
 	result.mVertices = 0;
 }
 
-
 //*********************************************************************
 //*********************************************************************
 //********  HullLib header
@@ -688,16 +667,15 @@ void ReleaseHull(PHullResult &result)
 //*********************************************************************
 //*********************************************************************
 
-HullError HullLibrary::CreateConvexHull(const HullDesc       &desc,           // describes the input request
-																					HullResult           &result)         // contains the resulst
+HullError HullLibrary::CreateConvexHull(const HullDesc &desc,  // describes the input request
+										HullResult &result)    // contains the resulst
 {
 	HullError ret = QE_FAIL;
 
-
 	PHullResult hr;
 
 	unsigned int vcount = desc.mVcount;
-	if ( vcount < 8 ) vcount = 8;
+	if (vcount < 8) vcount = 8;
 
 	btAlignedObjectArray<btVector3> vertexSource;
 	vertexSource.resize(static_cast<int>(vcount));
@@ -706,87 +684,82 @@ HullError HullLibrary::CreateConvexHull(const HullDesc       &desc,           //
 
 	unsigned int ovcount;
 
-	bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates!
+	bool ok = CleanupVertices(desc.mVcount, desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale);  // normalize point cloud, remove duplicates!
 
-	if ( ok )
+	if (ok)
 	{
-
-
-//		if ( 1 ) // scale vertices back to their original size.
+		//		if ( 1 ) // scale vertices back to their original size.
 		{
-			for (unsigned int i=0; i<ovcount; i++)
+			for (unsigned int i = 0; i < ovcount; i++)
 			{
-				btVector3& v = vertexSource[static_cast<int>(i)];
-				v[0]*=scale[0];
-				v[1]*=scale[1];
-				v[2]*=scale[2];
+				btVector3 &v = vertexSource[static_cast<int>(i)];
+				v[0] *= scale[0];
+				v[1] *= scale[1];
+				v[2] *= scale[2];
 			}
 		}
 
-		ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices);
+		ok = ComputeHull(ovcount, &vertexSource[0], hr, desc.mMaxVertices);
 
-		if ( ok )
+		if (ok)
 		{
-
 			// re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table.
-			btAlignedObjectArray<btVector3>	vertexScratch;
+			btAlignedObjectArray<btVector3> vertexScratch;
 			vertexScratch.resize(static_cast<int>(hr.mVcount));
 
-			BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount );
+			BringOutYourDead(hr.mVertices, hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount);
 
 			ret = QE_OK;
 
-			if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle!
+			if (desc.HasHullFlag(QF_TRIANGLES))  // if he wants the results as triangle!
 			{
-				result.mPolygons          = false;
+				result.mPolygons = false;
 				result.mNumOutputVertices = ovcount;
 				result.m_OutputVertices.resize(static_cast<int>(ovcount));
-				result.mNumFaces          = hr.mFaceCount;
-				result.mNumIndices        = hr.mIndexCount;
+				result.mNumFaces = hr.mFaceCount;
+				result.mNumIndices = hr.mIndexCount;
 
 				result.m_Indices.resize(static_cast<int>(hr.mIndexCount));
 
-				memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
+				memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3) * ovcount);
 
-  			if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
+				if (desc.HasHullFlag(QF_REVERSE_ORDER))
 				{
-
 					const unsigned int *source = &hr.m_Indices[0];
-					unsigned int *dest   = &result.m_Indices[0];
+					unsigned int *dest = &result.m_Indices[0];
 
-					for (unsigned int i=0; i<hr.mFaceCount; i++)
+					for (unsigned int i = 0; i < hr.mFaceCount; i++)
 					{
 						dest[0] = source[2];
 						dest[1] = source[1];
 						dest[2] = source[0];
-						dest+=3;
-						source+=3;
+						dest += 3;
+						source += 3;
 					}
-
 				}
 				else
 				{
-					memcpy(&result.m_Indices[0], &hr.m_Indices[0], sizeof(unsigned int)*hr.mIndexCount);
+					memcpy(&result.m_Indices[0], &hr.m_Indices[0], sizeof(unsigned int) * hr.mIndexCount);
 				}
 			}
 			else
 			{
-				result.mPolygons          = true;
+				result.mPolygons = true;
 				result.mNumOutputVertices = ovcount;
 				result.m_OutputVertices.resize(static_cast<int>(ovcount));
-				result.mNumFaces          = hr.mFaceCount;
-				result.mNumIndices        = hr.mIndexCount+hr.mFaceCount;
+				result.mNumFaces = hr.mFaceCount;
+				result.mNumIndices = hr.mIndexCount + hr.mFaceCount;
 				result.m_Indices.resize(static_cast<int>(result.mNumIndices));
-				memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
+				memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3) * ovcount);
 
-//				if ( 1 )
+				//				if ( 1 )
 				{
 					const unsigned int *source = &hr.m_Indices[0];
-					unsigned int *dest   = &result.m_Indices[0];
-					for (unsigned int i=0; i<hr.mFaceCount; i++)
+					unsigned int *dest = &result.m_Indices[0];
+					for (unsigned int i = 0; i < hr.mFaceCount; i++)
 					{
 						dest[0] = 3;
-						if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
+						if (desc.HasHullFlag(QF_REVERSE_ORDER))
 						{
 							dest[1] = source[2];
 							dest[2] = source[1];
@@ -799,8 +772,8 @@ HullError HullLibrary::CreateConvexHull(const HullDesc       &desc,           //
 							dest[3] = source[2];
 						}
 
-						dest+=4;
-						source+=3;
+						dest += 4;
+						source += 3;
 					}
 				}
 			}
@@ -811,90 +784,83 @@ HullError HullLibrary::CreateConvexHull(const HullDesc       &desc,           //
 	return ret;
 }
 
-
-
-HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it.
+HullError HullLibrary::ReleaseResult(HullResult &result)  // release memory allocated for this result, we are done with it.
 {
-	if ( result.m_OutputVertices.size())
+	if (result.m_OutputVertices.size())
 	{
-		result.mNumOutputVertices=0;
+		result.mNumOutputVertices = 0;
 		result.m_OutputVertices.clear();
 	}
-	if ( result.m_Indices.size() )
+	if (result.m_Indices.size())
 	{
-		result.mNumIndices=0;
+		result.mNumIndices = 0;
 		result.m_Indices.clear();
 	}
 	return QE_OK;
 }
 
-
-static void addPoint(unsigned int &vcount,btVector3 *p,btScalar x,btScalar y,btScalar z)
+static void addPoint(unsigned int &vcount, btVector3 *p, btScalar x, btScalar y, btScalar z)
 {
 	// XXX, might be broken
-	btVector3& dest = p[vcount];
+	btVector3 &dest = p[vcount];
 	dest[0] = x;
 	dest[1] = y;
 	dest[2] = z;
 	vcount++;
 }
 
-btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2);
-btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2)
+btScalar GetDist(btScalar px, btScalar py, btScalar pz, const btScalar *p2);
+btScalar GetDist(btScalar px, btScalar py, btScalar pz, const btScalar *p2)
 {
-
 	btScalar dx = px - p2[0];
 	btScalar dy = py - p2[1];
 	btScalar dz = pz - p2[2];
 
-	return dx*dx+dy*dy+dz*dz;
+	return dx * dx + dy * dy + dz * dz;
 }
 
-
-
-bool  HullLibrary::CleanupVertices(unsigned int svcount,
-				   const btVector3 *svertices,
-				   unsigned int stride,
-				   unsigned int &vcount,       // output number of vertices
-				   btVector3 *vertices,                 // location to store the results.
-				   btScalar  normalepsilon,
-				   btVector3& scale)
+bool HullLibrary::CleanupVertices(unsigned int svcount,
+								  const btVector3 *svertices,
+								  unsigned int stride,
+								  unsigned int &vcount,  // output number of vertices
+								  btVector3 *vertices,   // location to store the results.
+								  btScalar normalepsilon,
+								  btVector3 &scale)
 {
-	if ( svcount == 0 ) return false;
+	if (svcount == 0) return false;
 
 	m_vertexIndexMapping.resize(0);
 
-
 #define EPSILON btScalar(0.000001) /* close enough to consider two btScalaring point numbers to be 'the same'. */
 
 	vcount = 0;
 
-	btScalar recip[3]={0.f,0.f,0.f};
+	btScalar recip[3] = {0.f, 0.f, 0.f};
 
-	if ( scale )
+	if (scale)
 	{
 		scale[0] = 1;
 		scale[1] = 1;
 		scale[2] = 1;
 	}
 
-	btScalar bmin[3] = {  FLT_MAX,  FLT_MAX,  FLT_MAX };
-	btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
+	btScalar bmin[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
+	btScalar bmax[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
 
-	const char *vtx = (const char *) svertices;
+	const char *vtx = (const char *)svertices;
 
-//	if ( 1 )
+	//	if ( 1 )
 	{
-		for (unsigned int i=0; i<svcount; i++)
+		for (unsigned int i = 0; i < svcount; i++)
 		{
-			const btScalar *p = (const btScalar *) vtx;
+			const btScalar *p = (const btScalar *)vtx;
 
-			vtx+=stride;
+			vtx += stride;
 
-			for (int j=0; j<3; j++)
+			for (int j = 0; j < 3; j++)
 			{
-				if ( p[j] < bmin[j] ) bmin[j] = p[j];
-				if ( p[j] > bmax[j] ) bmax[j] = p[j];
+				if (p[j] < bmin[j]) bmin[j] = p[j];
+				if (p[j] > bmax[j]) bmax[j] = p[j];
 			}
 		}
 	}
@@ -905,28 +871,27 @@ bool  HullLibrary::CleanupVertices(unsigned int svcount,
 
 	btVector3 center;
 
-	center[0] = dx*btScalar(0.5) + bmin[0];
-	center[1] = dy*btScalar(0.5) + bmin[1];
-	center[2] = dz*btScalar(0.5) + bmin[2];
+	center[0] = dx * btScalar(0.5) + bmin[0];
+	center[1] = dy * btScalar(0.5) + bmin[1];
+	center[2] = dz * btScalar(0.5) + bmin[2];
 
-	if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 )
+	if (dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3)
 	{
-
 		btScalar len = FLT_MAX;
 
-		if ( dx > EPSILON && dx < len ) len = dx;
-		if ( dy > EPSILON && dy < len ) len = dy;
-		if ( dz > EPSILON && dz < len ) len = dz;
+		if (dx > EPSILON && dx < len) len = dx;
+		if (dy > EPSILON && dy < len) len = dy;
+		if (dz > EPSILON && dz < len) len = dz;
 
-		if ( len == FLT_MAX )
+		if (len == FLT_MAX)
 		{
-			dx = dy = dz = btScalar(0.01); // one centimeter
+			dx = dy = dz = btScalar(0.01);  // one centimeter
 		}
 		else
 		{
-			if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
-			if ( dy < EPSILON ) dy = len * btScalar(0.05);
-			if ( dz < EPSILON ) dz = len * btScalar(0.05);
+			if (dx < EPSILON) dx = len * btScalar(0.05);  // 1/5th the shortest non-zero edge.
+			if (dy < EPSILON) dy = len * btScalar(0.05);
+			if (dz < EPSILON) dz = len * btScalar(0.05);
 		}
 
 		btScalar x1 = center[0] - dx;
@@ -938,22 +903,20 @@ bool  HullLibrary::CleanupVertices(unsigned int svcount,
 		btScalar z1 = center[2] - dz;
 		btScalar z2 = center[2] + dz;
 
-		addPoint(vcount,vertices,x1,y1,z1);
-		addPoint(vcount,vertices,x2,y1,z1);
-		addPoint(vcount,vertices,x2,y2,z1);
-		addPoint(vcount,vertices,x1,y2,z1);
-		addPoint(vcount,vertices,x1,y1,z2);
-		addPoint(vcount,vertices,x2,y1,z2);
-		addPoint(vcount,vertices,x2,y2,z2);
-		addPoint(vcount,vertices,x1,y2,z2);
-
-		return true; // return cube
-
+		addPoint(vcount, vertices, x1, y1, z1);
+		addPoint(vcount, vertices, x2, y1, z1);
+		addPoint(vcount, vertices, x2, y2, z1);
+		addPoint(vcount, vertices, x1, y2, z1);
+		addPoint(vcount, vertices, x1, y1, z2);
+		addPoint(vcount, vertices, x2, y1, z2);
+		addPoint(vcount, vertices, x2, y2, z2);
+		addPoint(vcount, vertices, x1, y2, z2);
 
+		return true;  // return cube
 	}
 	else
 	{
-		if ( scale )
+		if (scale)
 		{
 			scale[0] = dx;
 			scale[1] = dy;
@@ -963,75 +926,70 @@ bool  HullLibrary::CleanupVertices(unsigned int svcount,
 			recip[1] = 1 / dy;
 			recip[2] = 1 / dz;
 
-			center[0]*=recip[0];
-			center[1]*=recip[1];
-			center[2]*=recip[2];
-
+			center[0] *= recip[0];
+			center[1] *= recip[1];
+			center[2] *= recip[2];
 		}
-
 	}
 
+	vtx = (const char *)svertices;
 
-
-	vtx = (const char *) svertices;
-
-	for (unsigned int i=0; i<svcount; i++)
+	for (unsigned int i = 0; i < svcount; i++)
 	{
 		const btVector3 *p = (const btVector3 *)vtx;
-		vtx+=stride;
+		vtx += stride;
 
 		btScalar px = p->getX();
 		btScalar py = p->getY();
 		btScalar pz = p->getZ();
 
-		if ( scale )
+		if (scale)
 		{
-			px = px*recip[0]; // normalize
-			py = py*recip[1]; // normalize
-			pz = pz*recip[2]; // normalize
+			px = px * recip[0];  // normalize
+			py = py * recip[1];  // normalize
+			pz = pz * recip[2];  // normalize
 		}
 
-//		if ( 1 )
+		//		if ( 1 )
 		{
 			unsigned int j;
 
-			for (j=0; j<vcount; j++)
+			for (j = 0; j < vcount; j++)
 			{
 				/// XXX might be broken
-				btVector3& v = vertices[j];
+				btVector3 &v = vertices[j];
 
 				btScalar x = v[0];
 				btScalar y = v[1];
 				btScalar z = v[2];
 
-				btScalar dx = btFabs(x - px );
-				btScalar dy = btFabs(y - py );
-				btScalar dz = btFabs(z - pz );
+				btScalar dx = btFabs(x - px);
+				btScalar dy = btFabs(y - py);
+				btScalar dz = btFabs(z - pz);
 
-				if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
+				if (dx < normalepsilon && dy < normalepsilon && dz < normalepsilon)
 				{
 					// ok, it is close enough to the old one
 					// now let us see if it is further from the center of the point cloud than the one we already recorded.
 					// in which case we keep this one instead.
 
-					btScalar dist1 = GetDist(px,py,pz,center);
-					btScalar dist2 = GetDist(v[0],v[1],v[2],center);
+					btScalar dist1 = GetDist(px, py, pz, center);
+					btScalar dist2 = GetDist(v[0], v[1], v[2], center);
 
-					if ( dist1 > dist2 )
+					if (dist1 > dist2)
 					{
 						v[0] = px;
 						v[1] = py;
 						v[2] = pz;
-						
 					}
 
 					break;
 				}
 			}
 
-			if ( j == vcount )
+			if (j == vcount)
 			{
-				btVector3& dest = vertices[vcount];
+				btVector3 &dest = vertices[vcount];
 				dest[0] = px;
 				dest[1] = py;
 				dest[2] = pz;
@@ -1042,18 +1000,18 @@ bool  HullLibrary::CleanupVertices(unsigned int svcount,
 	}
 
 	// ok..now make sure we didn't prune so many vertices it is now invalid.
-//	if ( 1 )
+	//	if ( 1 )
 	{
-		btScalar bmin[3] = {  FLT_MAX,  FLT_MAX,  FLT_MAX };
-		btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
+		btScalar bmin[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
+		btScalar bmax[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
 
-		for (unsigned int i=0; i<vcount; i++)
+		for (unsigned int i = 0; i < vcount; i++)
 		{
-			const btVector3& p = vertices[i];
-			for (int j=0; j<3; j++)
+			const btVector3 &p = vertices[i];
+			for (int j = 0; j < 3; j++)
 			{
-				if ( p[j] < bmin[j] ) bmin[j] = p[j];
-				if ( p[j] > bmax[j] ) bmax[j] = p[j];
+				if (p[j] < bmin[j]) bmin[j] = p[j];
+				if (p[j] > bmax[j]) bmax[j] = p[j];
 			}
 		}
 
@@ -1061,27 +1019,27 @@ bool  HullLibrary::CleanupVertices(unsigned int svcount,
 		btScalar dy = bmax[1] - bmin[1];
 		btScalar dz = bmax[2] - bmin[2];
 
-		if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3)
+		if (dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3)
 		{
-			btScalar cx = dx*btScalar(0.5) + bmin[0];
-			btScalar cy = dy*btScalar(0.5) + bmin[1];
-			btScalar cz = dz*btScalar(0.5) + bmin[2];
+			btScalar cx = dx * btScalar(0.5) + bmin[0];
+			btScalar cy = dy * btScalar(0.5) + bmin[1];
+			btScalar cz = dz * btScalar(0.5) + bmin[2];
 
 			btScalar len = FLT_MAX;
 
-			if ( dx >= EPSILON && dx < len ) len = dx;
-			if ( dy >= EPSILON && dy < len ) len = dy;
-			if ( dz >= EPSILON && dz < len ) len = dz;
+			if (dx >= EPSILON && dx < len) len = dx;
+			if (dy >= EPSILON && dy < len) len = dy;
+			if (dz >= EPSILON && dz < len) len = dz;
 
-			if ( len == FLT_MAX )
+			if (len == FLT_MAX)
 			{
-				dx = dy = dz = btScalar(0.01); // one centimeter
+				dx = dy = dz = btScalar(0.01);  // one centimeter
 			}
 			else
 			{
-				if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
-				if ( dy < EPSILON ) dy = len * btScalar(0.05);
-				if ( dz < EPSILON ) dz = len * btScalar(0.05);
+				if (dx < EPSILON) dx = len * btScalar(0.05);  // 1/5th the shortest non-zero edge.
+				if (dy < EPSILON) dy = len * btScalar(0.05);
+				if (dz < EPSILON) dz = len * btScalar(0.05);
 			}
 
 			btScalar x1 = cx - dx;
@@ -1093,16 +1051,16 @@ bool  HullLibrary::CleanupVertices(unsigned int svcount,
 			btScalar z1 = cz - dz;
 			btScalar z2 = cz + dz;
 
-			vcount = 0; // add box
+			vcount = 0;  // add box
 
-			addPoint(vcount,vertices,x1,y1,z1);
-			addPoint(vcount,vertices,x2,y1,z1);
-			addPoint(vcount,vertices,x2,y2,z1);
-			addPoint(vcount,vertices,x1,y2,z1);
-			addPoint(vcount,vertices,x1,y1,z2);
-			addPoint(vcount,vertices,x2,y1,z2);
-			addPoint(vcount,vertices,x2,y2,z2);
-			addPoint(vcount,vertices,x1,y2,z2);
+			addPoint(vcount, vertices, x1, y1, z1);
+			addPoint(vcount, vertices, x2, y1, z1);
+			addPoint(vcount, vertices, x2, y2, z1);
+			addPoint(vcount, vertices, x1, y2, z1);
+			addPoint(vcount, vertices, x1, y1, z2);
+			addPoint(vcount, vertices, x2, y1, z2);
+			addPoint(vcount, vertices, x2, y2, z2);
+			addPoint(vcount, vertices, x1, y2, z2);
 
 			return true;
 		}
@@ -1111,57 +1069,52 @@ bool  HullLibrary::CleanupVertices(unsigned int svcount,
 	return true;
 }
 
-void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount)
+void HullLibrary::BringOutYourDead(const btVector3 *verts, unsigned int vcount, btVector3 *overts, unsigned int &ocount, unsigned int *indices, unsigned indexcount)
 {
-	btAlignedObjectArray<int>tmpIndices;
+	btAlignedObjectArray<int> tmpIndices;
 	tmpIndices.resize(m_vertexIndexMapping.size());
 	int i;
 
-	for (i=0;i<m_vertexIndexMapping.size();i++)
+	for (i = 0; i < m_vertexIndexMapping.size(); i++)
 	{
 		tmpIndices[i] = m_vertexIndexMapping[i];
 	}
 
 	TUIntArray usedIndices;
 	usedIndices.resize(static_cast<int>(vcount));
-	memset(&usedIndices[0],0,sizeof(unsigned int)*vcount);
+	memset(&usedIndices[0], 0, sizeof(unsigned int) * vcount);
 
 	ocount = 0;
 
-	for (i=0; i<int (indexcount); i++)
+	for (i = 0; i < int(indexcount); i++)
 	{
-		unsigned int v = indices[i]; // original array index
+		unsigned int v = indices[i];  // original array index
 
-		btAssert( v >= 0 && v < vcount );
+		btAssert(v >= 0 && v < vcount);
 
-		if ( usedIndices[static_cast<int>(v)] ) // if already remapped
+		if (usedIndices[static_cast<int>(v)])  // if already remapped
 		{
-			indices[i] = usedIndices[static_cast<int>(v)]-1; // index to new array
+			indices[i] = usedIndices[static_cast<int>(v)] - 1;  // index to new array
 		}
 		else
 		{
+			indices[i] = ocount;  // new index mapping
 
-			indices[i] = ocount;      // new index mapping
-
-			overts[ocount][0] = verts[v][0]; // copy old vert to new vert array
+			overts[ocount][0] = verts[v][0];  // copy old vert to new vert array
 			overts[ocount][1] = verts[v][1];
 			overts[ocount][2] = verts[v][2];
 
-			for (int k=0;k<m_vertexIndexMapping.size();k++)
+			for (int k = 0; k < m_vertexIndexMapping.size(); k++)
 			{
-				if (tmpIndices[k]==int(v))
-					m_vertexIndexMapping[k]=ocount;
+				if (tmpIndices[k] == int(v))
+					m_vertexIndexMapping[k] = ocount;
 			}
 
-			ocount++; // increment output vert count
-
-			btAssert( ocount >=0 && ocount <= vcount );
+			ocount++;  // increment output vert count
 
-			usedIndices[static_cast<int>(v)] = ocount; // assign new index remapping
+			btAssert(ocount >= 0 && ocount <= vcount);
 
-		
+			usedIndices[static_cast<int>(v)] = ocount;  // assign new index remapping
 		}
 	}
-
-	
 }
diff --git a/extern/bullet2/src/LinearMath/btConvexHull.h b/extern/bullet2/src/LinearMath/btConvexHull.h
index 69c52bc6f83..f890d75ea1d 100644
--- a/extern/bullet2/src/LinearMath/btConvexHull.h
+++ b/extern/bullet2/src/LinearMath/btConvexHull.h
@@ -34,106 +34,102 @@ public:
 		mNumFaces = 0;
 		mNumIndices = 0;
 	}
-	bool                    mPolygons;                  // true if indices represents polygons, false indices are triangles
-	unsigned int            mNumOutputVertices;         // number of vertices in the output hull
-	btAlignedObjectArray<btVector3>	m_OutputVertices;            // array of vertices
-	unsigned int            mNumFaces;                  // the number of faces produced
-	unsigned int            mNumIndices;                // the total number of indices
-	btAlignedObjectArray<unsigned int>    m_Indices;                   // pointer to indices.
-
-// If triangles, then indices are array indexes into the vertex list.
-// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc..
+	bool mPolygons;                                    // true if indices represents polygons, false indices are triangles
+	unsigned int mNumOutputVertices;                   // number of vertices in the output hull
+	btAlignedObjectArray<btVector3> m_OutputVertices;  // array of vertices
+	unsigned int mNumFaces;                            // the number of faces produced
+	unsigned int mNumIndices;                          // the total number of indices
+	btAlignedObjectArray<unsigned int> m_Indices;      // pointer to indices.
+
+	// If triangles, then indices are array indexes into the vertex list.
+	// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc..
 };
 
 enum HullFlag
 {
-	QF_TRIANGLES         = (1<<0),             // report results as triangles, not polygons.
-	QF_REVERSE_ORDER     = (1<<1),             // reverse order of the triangle indices.
-	QF_DEFAULT           = QF_TRIANGLES
+	QF_TRIANGLES = (1 << 0),      // report results as triangles, not polygons.
+	QF_REVERSE_ORDER = (1 << 1),  // reverse order of the triangle indices.
+	QF_DEFAULT = QF_TRIANGLES
 };
 
-
 class HullDesc
 {
 public:
 	HullDesc(void)
 	{
-		mFlags          = QF_DEFAULT;
-		mVcount         = 0;
-		mVertices       = 0;
-		mVertexStride   = sizeof(btVector3);
-		mNormalEpsilon  = 0.001f;
-		mMaxVertices	= 4096; // maximum number of points to be considered for a convex hull.
-		mMaxFaces	= 4096;
+		mFlags = QF_DEFAULT;
+		mVcount = 0;
+		mVertices = 0;
+		mVertexStride = sizeof(btVector3);
+		mNormalEpsilon = 0.001f;
+		mMaxVertices = 4096;  // maximum number of points to be considered for a convex hull.
+		mMaxFaces = 4096;
 	};
 
 	HullDesc(HullFlag flag,
-		 unsigned int vcount,
-		 const btVector3 *vertices,
-		 unsigned int stride = sizeof(btVector3))
+			 unsigned int vcount,
+			 const btVector3* vertices,
+			 unsigned int stride = sizeof(btVector3))
 	{
-		mFlags          = flag;
-		mVcount         = vcount;
-		mVertices       = vertices;
-		mVertexStride   = stride;
-		mNormalEpsilon  = btScalar(0.001);
-		mMaxVertices    = 4096;
+		mFlags = flag;
+		mVcount = vcount;
+		mVertices = vertices;
+		mVertexStride = stride;
+		mNormalEpsilon = btScalar(0.001);
+		mMaxVertices = 4096;
 	}
 
 	bool HasHullFlag(HullFlag flag) const
 	{
-		if ( mFlags & flag ) return true;
+		if (mFlags & flag) return true;
 		return false;
 	}
 
 	void SetHullFlag(HullFlag flag)
 	{
-		mFlags|=flag;
+		mFlags |= flag;
 	}
 
 	void ClearHullFlag(HullFlag flag)
 	{
-		mFlags&=~flag;
+		mFlags &= ~flag;
 	}
 
-	unsigned int      mFlags;           // flags to use when generating the convex hull.
-	unsigned int      mVcount;          // number of vertices in the input point cloud
-	const btVector3  *mVertices;        // the array of vertices.
-	unsigned int      mVertexStride;    // the stride of each vertex, in bytes.
-	btScalar             mNormalEpsilon;   // the epsilon for removing duplicates.  This is a normalized value, if normalized bit is on.
-	unsigned int      mMaxVertices;     // maximum number of vertices to be considered for the hull!
-	unsigned int      mMaxFaces;
+	unsigned int mFlags;         // flags to use when generating the convex hull.
+	unsigned int mVcount;        // number of vertices in the input point cloud
+	const btVector3* mVertices;  // the array of vertices.
+	unsigned int mVertexStride;  // the stride of each vertex, in bytes.
+	btScalar mNormalEpsilon;     // the epsilon for removing duplicates.  This is a normalized value, if normalized bit is on.
+	unsigned int mMaxVertices;   // maximum number of vertices to be considered for the hull!
+	unsigned int mMaxFaces;
 };
 
 enum HullError
 {
-	QE_OK,            // success!
-	QE_FAIL           // failed.
+	QE_OK,   // success!
+	QE_FAIL  // failed.
 };
 
 class btPlane
 {
-	public:
-	btVector3	normal;
-	btScalar	dist;   // distance below origin - the D from plane equasion Ax+By+Cz+D=0
-			btPlane(const btVector3 &n,btScalar d):normal(n),dist(d){}
-			btPlane():normal(),dist(0){}
-	
+public:
+	btVector3 normal;
+	btScalar dist;  // distance below origin - the D from plane equasion Ax+By+Cz+D=0
+	btPlane(const btVector3& n, btScalar d) : normal(n), dist(d) {}
+	btPlane() : normal(), dist(0) {}
 };
 
-
-
-class ConvexH 
+class ConvexH
 {
-  public:
+public:
 	class HalfEdge
 	{
-	  public:
+	public:
 		short ea;         // the other half of the edge (index into edges list)
 		unsigned char v;  // the vertex at the start of this edge (index into vertices list)
 		unsigned char p;  // the facet on which this edge lies (index into facets list)
-		HalfEdge(){}
-		HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){}
+		HalfEdge() {}
+		HalfEdge(short _ea, unsigned char _v, unsigned char _p) : ea(_ea), v(_v), p(_p) {}
 	};
 	ConvexH()
 	{
@@ -143,25 +139,29 @@ class ConvexH
 	}
 	btAlignedObjectArray<btVector3> vertices;
 	btAlignedObjectArray<HalfEdge> edges;
-	btAlignedObjectArray<btPlane>  facets;
-	ConvexH(int vertices_size,int edges_size,int facets_size);
+	btAlignedObjectArray<btPlane> facets;
+	ConvexH(int vertices_size, int edges_size, int facets_size);
 };
 
-
 class int4
 {
 public:
-	int x,y,z,w;
+	int x, y, z, w;
 	int4(){};
-	int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;}
-	const int& operator[](int i) const {return (&x)[i];}
-	int& operator[](int i) {return (&x)[i];}
+	int4(int _x, int _y, int _z, int _w)
+	{
+		x = _x;
+		y = _y;
+		z = _z;
+		w = _w;
+	}
+	const int& operator[](int i) const { return (&x)[i]; }
+	int& operator[](int i) { return (&x)[i]; }
 };
 
 class PHullResult
 {
 public:
-
 	PHullResult(void)
 	{
 		mVcount = 0;
@@ -173,69 +173,61 @@ public:
 	unsigned int mVcount;
 	unsigned int mIndexCount;
 	unsigned int mFaceCount;
-	btVector3*   mVertices;
+	btVector3* mVertices;
 	TUIntArray m_Indices;
 };
 
-
-
 ///The HullLibrary class can create a convex hull from a collection of vertices, using the ComputeHull method.
 ///The btShapeHull class uses this HullLibrary to create a approximate convex mesh given a general (non-polyhedral) convex shape.
 class HullLibrary
 {
-
 	btAlignedObjectArray<class btHullTriangle*> m_tris;
 
 public:
-
 	btAlignedObjectArray<int> m_vertexIndexMapping;
 
-
-	HullError CreateConvexHull(const HullDesc& desc, // describes the input request
-				   HullResult&     result);        // contains the resulst
-	HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it.
+	HullError CreateConvexHull(const HullDesc& desc,  // describes the input request
+							   HullResult& result);   // contains the resulst
+	HullError ReleaseResult(HullResult& result);      // release memory allocated for this result, we are done with it.
 
 private:
+	bool ComputeHull(unsigned int vcount, const btVector3* vertices, PHullResult& result, unsigned int vlimit);
 
-	bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit);
-
-	class btHullTriangle*	allocateTriangle(int a,int b,int c);
-	void	deAllocateTriangle(btHullTriangle*);
-	void b2bfix(btHullTriangle* s,btHullTriangle*t);
+	class btHullTriangle* allocateTriangle(int a, int b, int c);
+	void deAllocateTriangle(btHullTriangle*);
+	void b2bfix(btHullTriangle* s, btHullTriangle* t);
 
-	void removeb2b(btHullTriangle* s,btHullTriangle*t);
+	void removeb2b(btHullTriangle* s, btHullTriangle* t);
 
-	void checkit(btHullTriangle *t);
+	void checkit(btHullTriangle* t);
 
 	btHullTriangle* extrudable(btScalar epsilon);
 
-	int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit);
+	int calchull(btVector3* verts, int verts_count, TUIntArray& tris_out, int& tris_count, int vlimit);
 
-	int calchullgen(btVector3 *verts,int verts_count, int vlimit);
+	int calchullgen(btVector3* verts, int verts_count, int vlimit);
 
-	int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow);
+	int4 FindSimplex(btVector3* verts, int verts_count, btAlignedObjectArray<int>& allow);
 
-	class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice);
+	class ConvexH* ConvexHCrop(ConvexH& convex, const btPlane& slice);
 
-	void extrude(class btHullTriangle* t0,int v);
+	void extrude(class btHullTriangle* t0, int v);
 
 	ConvexH* test_cube();
 
-	//BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'. 
+	//BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'.
 	//After the hull is generated it give you back a set of polygon faces which index the *original* point cloud.
 	//The thing is, often times, there are many 'dead vertices' in the point cloud that are on longer referenced by the hull.
 	//The routine 'BringOutYourDead' find only the referenced vertices, copies them to an new buffer, and re-indexes the hull so that it is a minimal representation.
-	void BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int* indices,unsigned indexcount);
+	void BringOutYourDead(const btVector3* verts, unsigned int vcount, btVector3* overts, unsigned int& ocount, unsigned int* indices, unsigned indexcount);
 
 	bool CleanupVertices(unsigned int svcount,
-			     const btVector3* svertices,
-			     unsigned int stride,
-			     unsigned int &vcount, // output number of vertices
-			     btVector3* vertices, // location to store the results.
-			     btScalar  normalepsilon,
-			     btVector3& scale);
+						 const btVector3* svertices,
+						 unsigned int stride,
+						 unsigned int& vcount,  // output number of vertices
+						 btVector3* vertices,   // location to store the results.
+						 btScalar normalepsilon,
+						 btVector3& scale);
 };
 
-
-#endif //BT_CD_HULL_H
-
+#endif  //BT_CD_HULL_H
diff --git a/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp b/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp
index 3fd77df8da5..fe1981a9a24 100644
--- a/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp
+++ b/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp
@@ -20,846 +20,847 @@ subject to the following restrictions:
 #include "btVector3.h"
 
 #ifdef __GNUC__
-	#include <stdint.h>
+#include <stdint.h>
 #elif defined(_MSC_VER)
-	typedef __int32 int32_t;
-	typedef __int64 int64_t;
-	typedef unsigned __int32 uint32_t;
-	typedef unsigned __int64 uint64_t;
+typedef __int32 int32_t;
+typedef __int64 int64_t;
+typedef unsigned __int32 uint32_t;
+typedef unsigned __int64 uint64_t;
 #else
-	typedef int int32_t;
-	typedef long long int int64_t;
-	typedef unsigned int uint32_t;
-	typedef unsigned long long int uint64_t;
+typedef int int32_t;
+typedef long long int int64_t;
+typedef unsigned int uint32_t;
+typedef unsigned long long int uint64_t;
 #endif
 
-
 //The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines
 //#if (defined(__GNUC__) && defined(__x86_64__) && !defined(__ICL))  // || (defined(__ICL) && defined(_M_X64))   bug in Intel compiler, disable inline assembly
 //	#define USE_X86_64_ASM
 //#endif
 
-
 //#define DEBUG_CONVEX_HULL
 //#define SHOW_ITERATIONS
 
 #if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS)
-	#include <stdio.h>
+#include <stdio.h>
 #endif
 
 // Convex hull implementation based on Preparata and Hong
 // Ole Kniemeyer, MAXON Computer GmbH
 class btConvexHullInternal
 {
+public:
+	class Point64
+	{
 	public:
-		
-		class Point64
-		{
-			public:
-				int64_t x;
-				int64_t y;
-				int64_t z;
-				
-				Point64(int64_t x, int64_t y, int64_t z): x(x), y(y), z(z)
-				{
-				}
+		int64_t x;
+		int64_t y;
+		int64_t z;
 
-				bool isZero()
-				{
-					return (x == 0) && (y == 0) && (z == 0);
-				}
+		Point64(int64_t x, int64_t y, int64_t z) : x(x), y(y), z(z)
+		{
+		}
 
-				int64_t dot(const Point64& b) const
-				{
-					return x * b.x + y * b.y + z * b.z;
-				}
-		};
-		
-		class Point32
-		{
-			public:
-				int32_t x;
-				int32_t y;
-				int32_t z;
-				int index;
-				
-				Point32()
-				{
-				}
-				
-				Point32(int32_t x, int32_t y, int32_t z): x(x), y(y), z(z), index(-1)
-				{
-				}
-				
-				bool operator==(const Point32& b) const
-				{
-					return (x == b.x) && (y == b.y) && (z == b.z);
-				}
+		bool isZero()
+		{
+			return (x == 0) && (y == 0) && (z == 0);
+		}
 
-				bool operator!=(const Point32& b) const
-				{
-					return (x != b.x) || (y != b.y) || (z != b.z);
-				}
+		int64_t dot(const Point64& b) const
+		{
+			return x * b.x + y * b.y + z * b.z;
+		}
+	};
 
-				bool isZero()
-				{
-					return (x == 0) && (y == 0) && (z == 0);
-				}
+	class Point32
+	{
+	public:
+		int32_t x;
+		int32_t y;
+		int32_t z;
+		int index;
 
-				Point64 cross(const Point32& b) const
-				{
-					return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
-				}
+		Point32()
+		{
+		}
 
-				Point64 cross(const Point64& b) const
-				{
-					return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
-				}
+		Point32(int32_t x, int32_t y, int32_t z) : x(x), y(y), z(z), index(-1)
+		{
+		}
 
-				int64_t dot(const Point32& b) const
-				{
-					return x * b.x + y * b.y + z * b.z;
-				}
+		bool operator==(const Point32& b) const
+		{
+			return (x == b.x) && (y == b.y) && (z == b.z);
+		}
 
-				int64_t dot(const Point64& b) const
-				{
-					return x * b.x + y * b.y + z * b.z;
-				}
+		bool operator!=(const Point32& b) const
+		{
+			return (x != b.x) || (y != b.y) || (z != b.z);
+		}
 
-				Point32 operator+(const Point32& b) const
-				{
-					return Point32(x + b.x, y + b.y, z + b.z);
-				}
+		bool isZero()
+		{
+			return (x == 0) && (y == 0) && (z == 0);
+		}
 
-				Point32 operator-(const Point32& b) const
-				{
-					return Point32(x - b.x, y - b.y, z - b.z);
-				}
-		};
+		Point64 cross(const Point32& b) const
+		{
+			return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
+		}
 
-		class Int128
+		Point64 cross(const Point64& b) const
 		{
-			public:
-				uint64_t low;
-				uint64_t high;
+			return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
+		}
 
-				Int128()
-				{
-				}
+		int64_t dot(const Point32& b) const
+		{
+			return x * b.x + y * b.y + z * b.z;
+		}
 
-				Int128(uint64_t low, uint64_t high): low(low), high(high)
-				{
-				}
+		int64_t dot(const Point64& b) const
+		{
+			return x * b.x + y * b.y + z * b.z;
+		}
 
-				Int128(uint64_t low): low(low), high(0)
-				{
-				}
+		Point32 operator+(const Point32& b) const
+		{
+			return Point32(x + b.x, y + b.y, z + b.z);
+		}
 
-				Int128(int64_t value): low(value), high((value >= 0) ? 0 : (uint64_t) -1LL)
-				{
-				}
+		Point32 operator-(const Point32& b) const
+		{
+			return Point32(x - b.x, y - b.y, z - b.z);
+		}
+	};
 
-				static Int128 mul(int64_t a, int64_t b);
+	class Int128
+	{
+	public:
+		uint64_t low;
+		uint64_t high;
 
-				static Int128 mul(uint64_t a, uint64_t b);
+		Int128()
+		{
+		}
 
-				Int128 operator-() const
-				{
-					return Int128((uint64_t) -(int64_t)low, ~high + (low == 0));
-				}
+		Int128(uint64_t low, uint64_t high) : low(low), high(high)
+		{
+		}
 
-				Int128 operator+(const Int128& b) const
-				{
+		Int128(uint64_t low) : low(low), high(0)
+		{
+		}
+
+		Int128(int64_t value) : low(value), high((value >= 0) ? 0 : (uint64_t)-1LL)
+		{
+		}
+
+		static Int128 mul(int64_t a, int64_t b);
+
+		static Int128 mul(uint64_t a, uint64_t b);
+
+		Int128 operator-() const
+		{
+			return Int128((uint64_t) - (int64_t)low, ~high + (low == 0));
+		}
+
+		Int128 operator+(const Int128& b) const
+		{
 #ifdef USE_X86_64_ASM
-					Int128 result;
-					__asm__ ("addq %[bl], %[rl]\n\t"
-									 "adcq %[bh], %[rh]\n\t"
-									 : [rl] "=r" (result.low), [rh] "=r" (result.high)
-									 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
-									 : "cc" );
-					return result;
+			Int128 result;
+			__asm__(
+				"addq %[bl], %[rl]\n\t"
+				"adcq %[bh], %[rh]\n\t"
+				: [rl] "=r"(result.low), [rh] "=r"(result.high)
+				: "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
+				: "cc");
+			return result;
 #else
-					uint64_t lo = low + b.low;
-					return Int128(lo, high + b.high + (lo < low));
+			uint64_t lo = low + b.low;
+			return Int128(lo, high + b.high + (lo < low));
 #endif
-				}
+		}
 
-				Int128 operator-(const Int128& b) const
-				{
+		Int128 operator-(const Int128& b) const
+		{
 #ifdef USE_X86_64_ASM
-					Int128 result;
-					__asm__ ("subq %[bl], %[rl]\n\t"
-									 "sbbq %[bh], %[rh]\n\t"
-									 : [rl] "=r" (result.low), [rh] "=r" (result.high)
-									 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
-									 : "cc" );
-					return result;
+			Int128 result;
+			__asm__(
+				"subq %[bl], %[rl]\n\t"
+				"sbbq %[bh], %[rh]\n\t"
+				: [rl] "=r"(result.low), [rh] "=r"(result.high)
+				: "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
+				: "cc");
+			return result;
 #else
-					return *this + -b;
+			return *this + -b;
 #endif
-				}
+		}
 
-				Int128& operator+=(const Int128& b)
-				{
+		Int128& operator+=(const Int128& b)
+		{
 #ifdef USE_X86_64_ASM
-					__asm__ ("addq %[bl], %[rl]\n\t"
-									 "adcq %[bh], %[rh]\n\t"
-									 : [rl] "=r" (low), [rh] "=r" (high)
-									 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
-									 : "cc" );
+			__asm__(
+				"addq %[bl], %[rl]\n\t"
+				"adcq %[bh], %[rh]\n\t"
+				: [rl] "=r"(low), [rh] "=r"(high)
+				: "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
+				: "cc");
 #else
-					uint64_t lo = low + b.low;
-					if (lo < low)
-					{
-						++high;
-					}
-					low = lo;
-					high += b.high;
+			uint64_t lo = low + b.low;
+			if (lo < low)
+			{
+				++high;
+			}
+			low = lo;
+			high += b.high;
 #endif
-					return *this;
-				}
+			return *this;
+		}
 
-				Int128& operator++()
-				{
-					if (++low == 0)
-					{
-						++high;
-					}
-					return *this;
-				}
+		Int128& operator++()
+		{
+			if (++low == 0)
+			{
+				++high;
+			}
+			return *this;
+		}
 
-				Int128 operator*(int64_t b) const;
+		Int128 operator*(int64_t b) const;
 
-				btScalar toScalar() const
-				{
-					return ((int64_t) high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low)
-						: -(-*this).toScalar();
-				}
+		btScalar toScalar() const
+		{
+			return ((int64_t)high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low)
+										: -(-*this).toScalar();
+		}
 
-				int getSign() const
-				{
-					return ((int64_t) high < 0) ? -1 : (high || low) ? 1 : 0;
-				}
+		int getSign() const
+		{
+			return ((int64_t)high < 0) ? -1 : (high || low) ? 1 : 0;
+		}
 
-				bool operator<(const Int128& b) const
-				{
-					return (high < b.high) || ((high == b.high) && (low < b.low));
-				}
+		bool operator<(const Int128& b) const
+		{
+			return (high < b.high) || ((high == b.high) && (low < b.low));
+		}
 
-				int ucmp(const Int128&b) const
-				{
-					if (high < b.high)
-					{
-						return -1;
-					}
-					if (high > b.high)
-					{
-						return 1;
-					}
-					if (low < b.low)
-					{
-						return -1;
-					}
-					if (low > b.low)
-					{
-						return 1;
-					}
-					return 0;
-				}
-		};
+		int ucmp(const Int128& b) const
+		{
+			if (high < b.high)
+			{
+				return -1;
+			}
+			if (high > b.high)
+			{
+				return 1;
+			}
+			if (low < b.low)
+			{
+				return -1;
+			}
+			if (low > b.low)
+			{
+				return 1;
+			}
+			return 0;
+		}
+	};
 
+	class Rational64
+	{
+	private:
+		uint64_t m_numerator;
+		uint64_t m_denominator;
+		int sign;
 
-		class Rational64
+	public:
+		Rational64(int64_t numerator, int64_t denominator)
 		{
-			private:
-				uint64_t m_numerator;
-				uint64_t m_denominator;
-				int sign;
-				
-			public:
-				Rational64(int64_t numerator, int64_t denominator)
-				{
-					if (numerator > 0)
-					{
-						sign = 1;
-						m_numerator = (uint64_t) numerator;
-					}
-					else if (numerator < 0)
-					{
-						sign = -1;
-						m_numerator = (uint64_t) -numerator;
-					}
-					else
-					{
-						sign = 0;
-						m_numerator = 0;
-					}
-					if (denominator > 0)
-					{
-						m_denominator = (uint64_t) denominator;
-					}
-					else if (denominator < 0)
-					{
-						sign = -sign;
-						m_denominator = (uint64_t) -denominator;
-					}
-					else
-					{
-						m_denominator = 0;
-					}
-				}
-				
-				bool isNegativeInfinity() const
-				{
-					return (sign < 0) && (m_denominator == 0);
-				}
-				
-				bool isNaN() const
-				{
-					return (sign == 0) && (m_denominator == 0);
-				}
-				
-				int compare(const Rational64& b) const;
-				
-				btScalar toScalar() const
-				{
-					return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar) m_numerator / m_denominator);
-				}
-		};
+			if (numerator > 0)
+			{
+				sign = 1;
+				m_numerator = (uint64_t)numerator;
+			}
+			else if (numerator < 0)
+			{
+				sign = -1;
+				m_numerator = (uint64_t)-numerator;
+			}
+			else
+			{
+				sign = 0;
+				m_numerator = 0;
+			}
+			if (denominator > 0)
+			{
+				m_denominator = (uint64_t)denominator;
+			}
+			else if (denominator < 0)
+			{
+				sign = -sign;
+				m_denominator = (uint64_t)-denominator;
+			}
+			else
+			{
+				m_denominator = 0;
+			}
+		}
 
+		bool isNegativeInfinity() const
+		{
+			return (sign < 0) && (m_denominator == 0);
+		}
 
-		class Rational128
+		bool isNaN() const
 		{
-			private:
-				Int128 numerator;
-				Int128 denominator;
-				int sign;
-				bool isInt64;
+			return (sign == 0) && (m_denominator == 0);
+		}
 
-			public:
-				Rational128(int64_t value)
-				{
-					if (value > 0)
-					{
-						sign = 1;
-						this->numerator = value;
-					}
-					else if (value < 0)
-					{
-						sign = -1;
-						this->numerator = -value;
-					}
-					else
-					{
-						sign = 0;
-						this->numerator = (uint64_t) 0;
-					}
-					this->denominator = (uint64_t) 1;
-					isInt64 = true;
-				}
+		int compare(const Rational64& b) const;
 
-				Rational128(const Int128& numerator, const Int128& denominator)
-				{
-					sign = numerator.getSign();
-					if (sign >= 0)
-					{
-						this->numerator = numerator;
-					}
-					else
-					{
-						this->numerator = -numerator;
-					}
-					int dsign = denominator.getSign();
-					if (dsign >= 0)
-					{
-						this->denominator = denominator;
-					}
-					else
-					{
-						sign = -sign;
-						this->denominator = -denominator;
-					}
-					isInt64 = false;
-				}
-
-				int compare(const Rational128& b) const;
+		btScalar toScalar() const
+		{
+			return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar)m_numerator / m_denominator);
+		}
+	};
 
-				int compare(int64_t b) const;
+	class Rational128
+	{
+	private:
+		Int128 numerator;
+		Int128 denominator;
+		int sign;
+		bool isInt64;
 
-				btScalar toScalar() const
-				{
-					return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar());
-				}
-		};
+	public:
+		Rational128(int64_t value)
+		{
+			if (value > 0)
+			{
+				sign = 1;
+				this->numerator = value;
+			}
+			else if (value < 0)
+			{
+				sign = -1;
+				this->numerator = -value;
+			}
+			else
+			{
+				sign = 0;
+				this->numerator = (uint64_t)0;
+			}
+			this->denominator = (uint64_t)1;
+			isInt64 = true;
+		}
 
-		class PointR128
+		Rational128(const Int128& numerator, const Int128& denominator)
 		{
-			public:
-				Int128 x;
-				Int128 y;
-				Int128 z;
-				Int128 denominator;
+			sign = numerator.getSign();
+			if (sign >= 0)
+			{
+				this->numerator = numerator;
+			}
+			else
+			{
+				this->numerator = -numerator;
+			}
+			int dsign = denominator.getSign();
+			if (dsign >= 0)
+			{
+				this->denominator = denominator;
+			}
+			else
+			{
+				sign = -sign;
+				this->denominator = -denominator;
+			}
+			isInt64 = false;
+		}
 
-				PointR128()
-				{
-				}
+		int compare(const Rational128& b) const;
 
-				PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator): x(x), y(y), z(z), denominator(denominator)
-				{
-				}
+		int compare(int64_t b) const;
 
-				btScalar xvalue() const
-				{
-					return x.toScalar() / denominator.toScalar();
-				}
+		btScalar toScalar() const
+		{
+			return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar());
+		}
+	};
 
-				btScalar yvalue() const
-				{
-					return y.toScalar() / denominator.toScalar();
-				}
+	class PointR128
+	{
+	public:
+		Int128 x;
+		Int128 y;
+		Int128 z;
+		Int128 denominator;
 
-				btScalar zvalue() const
-				{
-					return z.toScalar() / denominator.toScalar();
-				}
-		};
+		PointR128()
+		{
+		}
 
+		PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator) : x(x), y(y), z(z), denominator(denominator)
+		{
+		}
 
-		class Edge;
-		class Face;
+		btScalar xvalue() const
+		{
+			return x.toScalar() / denominator.toScalar();
+		}
 
-		class Vertex
+		btScalar yvalue() const
 		{
-			public:
-				Vertex* next;
-				Vertex* prev;
-				Edge* edges;
-				Face* firstNearbyFace;
-				Face* lastNearbyFace;
-				PointR128 point128;
-				Point32 point;
-				int copy;
-				
-				Vertex(): next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1)
-				{
-				}
+			return y.toScalar() / denominator.toScalar();
+		}
 
-#ifdef DEBUG_CONVEX_HULL
-				void print()
-				{
-					printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z);
-				}
+		btScalar zvalue() const
+		{
+			return z.toScalar() / denominator.toScalar();
+		}
+	};
 
-				void printGraph();
-#endif
+	class Edge;
+	class Face;
 
-				Point32 operator-(const Vertex& b) const
-				{
-					return point - b.point;
-				}
+	class Vertex
+	{
+	public:
+		Vertex* next;
+		Vertex* prev;
+		Edge* edges;
+		Face* firstNearbyFace;
+		Face* lastNearbyFace;
+		PointR128 point128;
+		Point32 point;
+		int copy;
 
-				Rational128 dot(const Point64& b) const
-				{
-					return (point.index >= 0) ? Rational128(point.dot(b))
-						: Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator);
-				}
+		Vertex() : next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1)
+		{
+		}
 
-				btScalar xvalue() const
-				{
-					return (point.index >= 0) ? btScalar(point.x) : point128.xvalue();
-				}
+#ifdef DEBUG_CONVEX_HULL
+		void print()
+		{
+			printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z);
+		}
 
-				btScalar yvalue() const
-				{
-					return (point.index >= 0) ? btScalar(point.y) : point128.yvalue();
-				}
+		void printGraph();
+#endif
 
-				btScalar zvalue() const
-				{
-					return (point.index >= 0) ? btScalar(point.z) : point128.zvalue();
-				}
+		Point32 operator-(const Vertex& b) const
+		{
+			return point - b.point;
+		}
 
-				void receiveNearbyFaces(Vertex* src)
-				{
-					if (lastNearbyFace)
-					{
-						lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace;
-					}
-					else
-					{
-						firstNearbyFace = src->firstNearbyFace;
-					}
-					if (src->lastNearbyFace)
-					{
-						lastNearbyFace = src->lastNearbyFace;
-					}
-					for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex)
-					{
-						btAssert(f->nearbyVertex == src);
-						f->nearbyVertex = this;
-					}
-					src->firstNearbyFace = NULL;
-					src->lastNearbyFace = NULL;
-				}
-		};
+		Rational128 dot(const Point64& b) const
+		{
+			return (point.index >= 0) ? Rational128(point.dot(b))
+									  : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator);
+		}
 
+		btScalar xvalue() const
+		{
+			return (point.index >= 0) ? btScalar(point.x) : point128.xvalue();
+		}
 
-		class Edge
+		btScalar yvalue() const
 		{
-			public:
-				Edge* next;
-				Edge* prev;
-				Edge* reverse;
-				Vertex* target;
-				Face* face;
-				int copy;
+			return (point.index >= 0) ? btScalar(point.y) : point128.yvalue();
+		}
 
-				~Edge()
-				{
-					next = NULL;
-					prev = NULL;
-					reverse = NULL;
-					target = NULL;
-					face = NULL;
-				}
+		btScalar zvalue() const
+		{
+			return (point.index >= 0) ? btScalar(point.z) : point128.zvalue();
+		}
 
-				void link(Edge* n)
-				{
-					btAssert(reverse->target == n->reverse->target);
-					next = n;
-					n->prev = this;
-				}
+		void receiveNearbyFaces(Vertex* src)
+		{
+			if (lastNearbyFace)
+			{
+				lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace;
+			}
+			else
+			{
+				firstNearbyFace = src->firstNearbyFace;
+			}
+			if (src->lastNearbyFace)
+			{
+				lastNearbyFace = src->lastNearbyFace;
+			}
+			for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex)
+			{
+				btAssert(f->nearbyVertex == src);
+				f->nearbyVertex = this;
+			}
+			src->firstNearbyFace = NULL;
+			src->lastNearbyFace = NULL;
+		}
+	};
 
-#ifdef DEBUG_CONVEX_HULL
-				void print()
-				{
-					printf("E%p : %d -> %d,  n=%p p=%p   (0 %d\t%d\t%d) -> (%d %d %d)", this, reverse->target->point.index, target->point.index, next, prev,
-								 reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z);
-				}
-#endif
-		};
+	class Edge
+	{
+	public:
+		Edge* next;
+		Edge* prev;
+		Edge* reverse;
+		Vertex* target;
+		Face* face;
+		int copy;
+
+		~Edge()
+		{
+			next = NULL;
+			prev = NULL;
+			reverse = NULL;
+			target = NULL;
+			face = NULL;
+		}
 
-		class Face
+		void link(Edge* n)
 		{
-			public:
-				Face* next;
-				Vertex* nearbyVertex;
-				Face* nextWithSameNearbyVertex;
-				Point32 origin;
-				Point32 dir0;
-				Point32 dir1;
+			btAssert(reverse->target == n->reverse->target);
+			next = n;
+			n->prev = this;
+		}
 
-				Face(): next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL)
-				{
-				}
+#ifdef DEBUG_CONVEX_HULL
+		void print()
+		{
+			printf("E%p : %d -> %d,  n=%p p=%p   (0 %d\t%d\t%d) -> (%d %d %d)", this, reverse->target->point.index, target->point.index, next, prev,
+				   reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z);
+		}
+#endif
+	};
 
-				void init(Vertex* a, Vertex* b, Vertex* c)
-				{
-					nearbyVertex = a;
-					origin = a->point;
-					dir0 = *b - *a;
-					dir1 = *c - *a;
-					if (a->lastNearbyFace)
-					{
-						a->lastNearbyFace->nextWithSameNearbyVertex = this;
-					}
-					else
-					{
-						a->firstNearbyFace = this;
-					}
-					a->lastNearbyFace = this;
-				}
+	class Face
+	{
+	public:
+		Face* next;
+		Vertex* nearbyVertex;
+		Face* nextWithSameNearbyVertex;
+		Point32 origin;
+		Point32 dir0;
+		Point32 dir1;
 
-				Point64 getNormal()
-				{
-					return dir0.cross(dir1);
-				}
-		};
+		Face() : next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL)
+		{
+		}
 
-		template<typename UWord, typename UHWord> class DMul
+		void init(Vertex* a, Vertex* b, Vertex* c)
 		{
-			private:
-				static uint32_t high(uint64_t value)
-				{
-					return (uint32_t) (value >> 32);
-				}
-				
-				static uint32_t low(uint64_t value)
-				{
-					return (uint32_t) value;
-				}
-				
-				static uint64_t mul(uint32_t a, uint32_t b)
-				{
-					return (uint64_t) a * (uint64_t) b;
-				}
-				
-				static void shlHalf(uint64_t& value)
-				{
-					value <<= 32;
-				}
-				
-				static uint64_t high(Int128 value)
-				{
-					return value.high;
-				}
-				
-				static uint64_t low(Int128 value)
-				{
-					return value.low;
-				}
-				
-				static Int128 mul(uint64_t a, uint64_t b)
-				{
-					return Int128::mul(a, b);
-				}
-				
-				static void shlHalf(Int128& value)
-				{
-					value.high = value.low;
-					value.low = 0;
-				}
-				
-			public:
-				
-				static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh)
-				{
-					UWord p00 = mul(low(a), low(b));
-					UWord p01 = mul(low(a), high(b));
-					UWord p10 = mul(high(a), low(b));
-					UWord p11 = mul(high(a), high(b));
-					UWord p0110 = UWord(low(p01)) + UWord(low(p10));
-					p11 += high(p01);
-					p11 += high(p10);
-					p11 += high(p0110);
-					shlHalf(p0110);
-					p00 += p0110;
-					if (p00 < p0110)
-					{
-						++p11;
-					}
-					resLow = p00;
-					resHigh = p11;
-				}
-		};
-	
-	private:
+			nearbyVertex = a;
+			origin = a->point;
+			dir0 = *b - *a;
+			dir1 = *c - *a;
+			if (a->lastNearbyFace)
+			{
+				a->lastNearbyFace->nextWithSameNearbyVertex = this;
+			}
+			else
+			{
+				a->firstNearbyFace = this;
+			}
+			a->lastNearbyFace = this;
+		}
 
-		class IntermediateHull
+		Point64 getNormal()
 		{
-			public:
-				Vertex* minXy;
-				Vertex* maxXy;
-				Vertex* minYx;
-				Vertex* maxYx;
-				
-				IntermediateHull(): minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL)
-				{
-				}
-				
-				void print();
-		};
-	
-		enum Orientation {NONE, CLOCKWISE, COUNTER_CLOCKWISE};
+			return dir0.cross(dir1);
+		}
+	};
 
-		template <typename T> class PoolArray
+	template <typename UWord, typename UHWord>
+	class DMul
+	{
+	private:
+		static uint32_t high(uint64_t value)
 		{
-			private:
-				T* array;
-				int size;
+			return (uint32_t)(value >> 32);
+		}
 
-			public:
-				PoolArray<T>* next;
+		static uint32_t low(uint64_t value)
+		{
+			return (uint32_t)value;
+		}
 
-				PoolArray(int size): size(size), next(NULL)
-				{
-					array = (T*) btAlignedAlloc(sizeof(T) * size, 16);
-				}
+		static uint64_t mul(uint32_t a, uint32_t b)
+		{
+			return (uint64_t)a * (uint64_t)b;
+		}
 
-				~PoolArray()
-				{
-					btAlignedFree(array);
-				}
+		static void shlHalf(uint64_t& value)
+		{
+			value <<= 32;
+		}
 
-				T* init()
-				{
-					T* o = array;
-					for (int i = 0; i < size; i++, o++)
-					{
-						o->next = (i+1 < size) ? o + 1 : NULL;
-					}
-					return array;
-				}
-		};
+		static uint64_t high(Int128 value)
+		{
+			return value.high;
+		}
 
-		template <typename T> class Pool
+		static uint64_t low(Int128 value)
 		{
-			private:
-				PoolArray<T>* arrays;
-				PoolArray<T>* nextArray;
-				T* freeObjects;
-				int arraySize;
+			return value.low;
+		}
 
-			public:
-				Pool(): arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256)
-				{
-				}
+		static Int128 mul(uint64_t a, uint64_t b)
+		{
+			return Int128::mul(a, b);
+		}
 
-				~Pool()
-				{
-					while (arrays)
-					{
-						PoolArray<T>* p = arrays;
-						arrays = p->next;
-						p->~PoolArray<T>();
-						btAlignedFree(p);
-					}
-				}
+		static void shlHalf(Int128& value)
+		{
+			value.high = value.low;
+			value.low = 0;
+		}
 
-				void reset()
-				{
-					nextArray = arrays;
-					freeObjects = NULL;
-				}
+	public:
+		static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh)
+		{
+			UWord p00 = mul(low(a), low(b));
+			UWord p01 = mul(low(a), high(b));
+			UWord p10 = mul(high(a), low(b));
+			UWord p11 = mul(high(a), high(b));
+			UWord p0110 = UWord(low(p01)) + UWord(low(p10));
+			p11 += high(p01);
+			p11 += high(p10);
+			p11 += high(p0110);
+			shlHalf(p0110);
+			p00 += p0110;
+			if (p00 < p0110)
+			{
+				++p11;
+			}
+			resLow = p00;
+			resHigh = p11;
+		}
+	};
 
-				void setArraySize(int arraySize)
-				{
-					this->arraySize = arraySize;
-				}
+private:
+	class IntermediateHull
+	{
+	public:
+		Vertex* minXy;
+		Vertex* maxXy;
+		Vertex* minYx;
+		Vertex* maxYx;
 
-				T* newObject()
-				{
-					T* o = freeObjects;
-					if (!o)
-					{
-						PoolArray<T>* p = nextArray;
-						if (p)
-						{
-							nextArray = p->next;
-						}
-						else
-						{
-							p = new(btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize);
-							p->next = arrays;
-							arrays = p;
-						}
-						o = p->init();
-					}
-					freeObjects = o->next;
-					return new(o) T();
-				};
+		IntermediateHull() : minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL)
+		{
+		}
 
-				void freeObject(T* object)
-				{
-					object->~T();
-					object->next = freeObjects;
-					freeObjects = object;
-				}
-		};
+		void print();
+	};
 
-		btVector3 scaling;
-		btVector3 center;
-		Pool<Vertex> vertexPool;
-		Pool<Edge> edgePool;
-		Pool<Face> facePool;
-		btAlignedObjectArray<Vertex*> originalVertices;
-		int mergeStamp;
-		int minAxis;
-		int medAxis;
-		int maxAxis;
-		int usedEdgePairs;
-		int maxUsedEdgePairs;
+	enum Orientation
+	{
+		NONE,
+		CLOCKWISE,
+		COUNTER_CLOCKWISE
+	};
 
-		static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t);
-		Edge* findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot);
-		void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1);
+	template <typename T>
+	class PoolArray
+	{
+	private:
+		T* array;
+		int size;
 
-		Edge* newEdgePair(Vertex* from, Vertex* to);
+	public:
+		PoolArray<T>* next;
 
-		void removeEdgePair(Edge* edge)
+		PoolArray(int size) : size(size), next(NULL)
 		{
-			Edge* n = edge->next;
-			Edge* r = edge->reverse;
+			array = (T*)btAlignedAlloc(sizeof(T) * size, 16);
+		}
 
-			btAssert(edge->target && r->target);
+		~PoolArray()
+		{
+			btAlignedFree(array);
+		}
 
-			if (n != edge)
-			{
-				n->prev = edge->prev;
-				edge->prev->next = n;
-				r->target->edges = n;
-			}
-			else
+		T* init()
+		{
+			T* o = array;
+			for (int i = 0; i < size; i++, o++)
 			{
-				r->target->edges = NULL;
+				o->next = (i + 1 < size) ? o + 1 : NULL;
 			}
-			
-			n = r->next;
-			
-			if (n != r)
+			return array;
+		}
+	};
+
+	template <typename T>
+	class Pool
+	{
+	private:
+		PoolArray<T>* arrays;
+		PoolArray<T>* nextArray;
+		T* freeObjects;
+		int arraySize;
+
+	public:
+		Pool() : arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256)
+		{
+		}
+
+		~Pool()
+		{
+			while (arrays)
 			{
-				n->prev = r->prev;
-				r->prev->next = n;
-				edge->target->edges = n;
+				PoolArray<T>* p = arrays;
+				arrays = p->next;
+				p->~PoolArray<T>();
+				btAlignedFree(p);
 			}
-			else
+		}
+
+		void reset()
+		{
+			nextArray = arrays;
+			freeObjects = NULL;
+		}
+
+		void setArraySize(int arraySize)
+		{
+			this->arraySize = arraySize;
+		}
+
+		T* newObject()
+		{
+			T* o = freeObjects;
+			if (!o)
 			{
-				edge->target->edges = NULL;
+				PoolArray<T>* p = nextArray;
+				if (p)
+				{
+					nextArray = p->next;
+				}
+				else
+				{
+					p = new (btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize);
+					p->next = arrays;
+					arrays = p;
+				}
+				o = p->init();
 			}
+			freeObjects = o->next;
+			return new (o) T();
+		};
 
-			edgePool.freeObject(edge);
-			edgePool.freeObject(r);
-			usedEdgePairs--;
+		void freeObject(T* object)
+		{
+			object->~T();
+			object->next = freeObjects;
+			freeObjects = object;
 		}
-		
-		void computeInternal(int start, int end, IntermediateHull& result);
-		
-		bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1);
-		
-		void merge(IntermediateHull& h0, IntermediateHull& h1);
+	};
 
-		btVector3 toBtVector(const Point32& v);
+	btVector3 scaling;
+	btVector3 center;
+	Pool<Vertex> vertexPool;
+	Pool<Edge> edgePool;
+	Pool<Face> facePool;
+	btAlignedObjectArray<Vertex*> originalVertices;
+	int mergeStamp;
+	int minAxis;
+	int medAxis;
+	int maxAxis;
+	int usedEdgePairs;
+	int maxUsedEdgePairs;
 
-		btVector3 getBtNormal(Face* face);
+	static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t);
+	Edge* findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot);
+	void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1);
 
-		bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack);
+	Edge* newEdgePair(Vertex* from, Vertex* to);
 
-	public:
-		Vertex* vertexList;
+	void removeEdgePair(Edge* edge)
+	{
+		Edge* n = edge->next;
+		Edge* r = edge->reverse;
 
-		void compute(const void* coords, bool doubleCoords, int stride, int count);
+		btAssert(edge->target && r->target);
 
-		btVector3 getCoordinates(const Vertex* v);
+		if (n != edge)
+		{
+			n->prev = edge->prev;
+			edge->prev->next = n;
+			r->target->edges = n;
+		}
+		else
+		{
+			r->target->edges = NULL;
+		}
 
-		btScalar shrink(btScalar amount, btScalar clampAmount);
-};
+		n = r->next;
+
+		if (n != r)
+		{
+			n->prev = r->prev;
+			r->prev->next = n;
+			edge->target->edges = n;
+		}
+		else
+		{
+			edge->target->edges = NULL;
+		}
+
+		edgePool.freeObject(edge);
+		edgePool.freeObject(r);
+		usedEdgePairs--;
+	}
+
+	void computeInternal(int start, int end, IntermediateHull& result);
+
+	bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1);
+
+	void merge(IntermediateHull& h0, IntermediateHull& h1);
 
+	btVector3 toBtVector(const Point32& v);
+
+	btVector3 getBtNormal(Face* face);
+
+	bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack);
+
+public:
+	Vertex* vertexList;
+
+	void compute(const void* coords, bool doubleCoords, int stride, int count);
+
+	btVector3 getCoordinates(const Vertex* v);
+
+	btScalar shrink(btScalar amount, btScalar clampAmount);
+};
 
 btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b) const
 {
-	bool negative = (int64_t) high < 0;
+	bool negative = (int64_t)high < 0;
 	Int128 a = negative ? -*this : *this;
 	if (b < 0)
 	{
 		negative = !negative;
 		b = -b;
 	}
-	Int128 result = mul(a.low, (uint64_t) b);
-	result.high += a.high * (uint64_t) b;
+	Int128 result = mul(a.low, (uint64_t)b);
+	result.high += a.high * (uint64_t)b;
 	return negative ? -result : result;
 }
 
 btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b)
 {
 	Int128 result;
-	
+
 #ifdef USE_X86_64_ASM
-	__asm__ ("imulq %[b]"
-					 : "=a" (result.low), "=d" (result.high)
-					 : "0"(a), [b] "r"(b)
-					 : "cc" );
+	__asm__("imulq %[b]"
+			: "=a"(result.low), "=d"(result.high)
+			: "0"(a), [b] "r"(b)
+			: "cc");
 	return result;
-	
+
 #else
 	bool negative = a < 0;
 	if (negative)
@@ -871,7 +872,7 @@ btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_
 		negative = !negative;
 		b = -b;
 	}
-	DMul<uint64_t, uint32_t>::mul((uint64_t) a, (uint64_t) b, result.low, result.high);
+	DMul<uint64_t, uint32_t>::mul((uint64_t)a, (uint64_t)b, result.low, result.high);
 	return negative ? -result : result;
 #endif
 }
@@ -881,10 +882,10 @@ btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint6
 	Int128 result;
 
 #ifdef USE_X86_64_ASM
-	__asm__ ("mulq %[b]"
-					 : "=a" (result.low), "=d" (result.high)
-					 : "0"(a), [b] "r"(b)
-					 : "cc" );
+	__asm__("mulq %[b]"
+			: "=a"(result.low), "=d"(result.high)
+			: "0"(a), [b] "r"(b)
+			: "cc");
 
 #else
 	DMul<uint64_t, uint32_t>::mul(a, b, result.low, result.high);
@@ -911,24 +912,25 @@ int btConvexHullInternal::Rational64::compare(const Rational64& b) const
 	int result;
 	int64_t tmp;
 	int64_t dummy;
-	__asm__ ("mulq %[bn]\n\t"
-					 "movq %%rax, %[tmp]\n\t"
-					 "movq %%rdx, %%rbx\n\t"
-					 "movq %[tn], %%rax\n\t"
-					 "mulq %[bd]\n\t"
-					 "subq %[tmp], %%rax\n\t"
-					 "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator"
-					 "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise
-					 "orq %%rdx, %%rax\n\t"
-					 "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero
-					 "decb %%bh\n\t" // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference)
-					 "shll $16, %%ebx\n\t" // ebx has same sign as difference
-					 : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
-					 : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator)
-					 : "%rdx", "cc" );
-	return result ? result ^ sign // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero)
-																// if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero)
-								: 0;
+	__asm__(
+		"mulq %[bn]\n\t"
+		"movq %%rax, %[tmp]\n\t"
+		"movq %%rdx, %%rbx\n\t"
+		"movq %[tn], %%rax\n\t"
+		"mulq %[bd]\n\t"
+		"subq %[tmp], %%rax\n\t"
+		"sbbq %%rbx, %%rdx\n\t"  // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator"
+		"setnsb %%bh\n\t"        // bh=1 if difference is non-negative, bh=0 otherwise
+		"orq %%rdx, %%rax\n\t"
+		"setnzb %%bl\n\t"      // bl=1 if difference if non-zero, bl=0 if it is zero
+		"decb %%bh\n\t"        // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference)
+		"shll $16, %%ebx\n\t"  // ebx has same sign as difference
+		: "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
+		: "a"(m_denominator), [bn] "g"(b.m_numerator), [tn] "g"(m_numerator), [bd] "g"(b.m_denominator)
+		: "%rdx", "cc");
+	return result ? result ^ sign  // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero)
+								   // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero)
+				  : 0;
 
 #else
 
@@ -949,7 +951,7 @@ int btConvexHullInternal::Rational128::compare(const Rational128& b) const
 	}
 	if (isInt64)
 	{
-		return -b.compare(sign * (int64_t) numerator.low);
+		return -b.compare(sign * (int64_t)numerator.low);
 	}
 
 	Int128 nbdLow, nbdHigh, dbnLow, dbnHigh;
@@ -968,7 +970,7 @@ int btConvexHullInternal::Rational128::compare(int64_t b) const
 {
 	if (isInt64)
 	{
-		int64_t a = sign * (int64_t) numerator.low;
+		int64_t a = sign * (int64_t)numerator.low;
 		return (a > b) ? 1 : (a < b) ? -1 : 0;
 	}
 	if (b > 0)
@@ -994,7 +996,6 @@ int btConvexHullInternal::Rational128::compare(int64_t b) const
 	return numerator.ucmp(denominator * b) * sign;
 }
 
-
 btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to)
 {
 	btAssert(from && to);
@@ -1062,7 +1063,7 @@ bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul
 			}
 		}
 	}
-	
+
 	v0 = h0.maxXy;
 	v1 = h1.maxXy;
 	Vertex* v00 = NULL;
@@ -1070,7 +1071,7 @@ bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul
 	int32_t sign = 1;
 
 	for (int side = 0; side <= 1; side++)
-	{		
+	{
 		int32_t dx = (v1->point.x - v0->point.x) * sign;
 		if (dx > 0)
 		{
@@ -1113,7 +1114,7 @@ bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul
 			while (true)
 			{
 				int32_t dy = v1->point.y - v0->point.y;
-				
+
 				Vertex* w1 = side ? v1->prev : v1->next;
 				if (w1 != v1)
 				{
@@ -1126,7 +1127,7 @@ bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul
 						continue;
 					}
 				}
-				
+
 				Vertex* w0 = side ? v0->prev : v0->next;
 				if (w0 != v0)
 				{
@@ -1140,7 +1141,7 @@ bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul
 						continue;
 					}
 				}
-				
+
 				break;
 			}
 		}
@@ -1166,7 +1167,7 @@ bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul
 			}
 			v1 = w1;
 		}
-		
+
 		if (side == 0)
 		{
 			v00 = v0;
@@ -1192,7 +1193,7 @@ bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul
 	{
 		h0.maxXy = h1.maxXy;
 	}
-	
+
 	h0.maxYx = h1.maxYx;
 
 	c0 = v00;
@@ -1277,8 +1278,21 @@ void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull&
 
 				return;
 			}
+			{
+				Vertex* v = originalVertices[start];
+				v->edges = NULL;
+				v->next = v;
+				v->prev = v;
+
+				result.minXy = v;
+				result.maxXy = v;
+				result.minYx = v;
+				result.maxYx = v;
+			}
+
+			return;
 		}
-		// lint -fallthrough
+
 		case 1:
 		{
 			Vertex* v = originalVertices[start];
@@ -1296,7 +1310,7 @@ void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull&
 	}
 
 	int split0 = start + n / 2;
-	Point32 p = originalVertices[split0-1]->point;
+	Point32 p = originalVertices[split0 - 1]->point;
 	int split1 = split0;
 	while ((split1 < end) && (originalVertices[split1]->point == p))
 	{
@@ -1321,7 +1335,7 @@ void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull&
 void btConvexHullInternal::IntermediateHull::print()
 {
 	printf("    Hull\n");
-	for (Vertex* v = minXy; v; )
+	for (Vertex* v = minXy; v;)
 	{
 		printf("      ");
 		v->print();
@@ -1349,7 +1363,7 @@ void btConvexHullInternal::IntermediateHull::print()
 		}
 	}
 	if (minXy)
-	{		
+	{
 		minXy->copy = (minXy->copy == -1) ? -2 : -1;
 		minXy->printGraph();
 	}
@@ -1425,7 +1439,7 @@ btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const V
 				Point32 t = *e->target - *start;
 				Rational64 cot(t.dot(sxrxs), t.dot(rxs));
 #ifdef DEBUG_CONVEX_HULL
-				printf("      Angle is %f (%d) for ", (float) btAtan(cot.toScalar()), (int) cot.isNaN());
+				printf("      Angle is %f (%d) for ", (float)btAtan(cot.toScalar()), (int)cot.isNaN());
 				e->print();
 #endif
 				if (cot.isNaN())
@@ -1472,7 +1486,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
 	btAssert(!start1 || (start1->target->point.dot(normal) == dist));
 	Point64 perp = s.cross(normal);
 	btAssert(!perp.isZero());
-	
+
 #ifdef DEBUG_CONVEX_HULL
 	printf("   Advancing %d %d  (%p %p, %d %d)\n", c0->point.index, c1->point.index, start0, start1, start0 ? start0->target->point.index : -1, start1 ? start1->target->point.index : -1);
 #endif
@@ -1502,7 +1516,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
 			et0 = e->target->point;
 		}
 	}
-	
+
 	int64_t maxDot1 = et1.dot(perp);
 	if (e1)
 	{
@@ -1539,7 +1553,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
 		while (true)
 		{
 			int64_t dy = (et1 - et0).dot(s);
-			
+
 			if (e0 && (e0->target != stop0))
 			{
 				Edge* f0 = e0->next->reverse;
@@ -1556,7 +1570,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
 					}
 				}
 			}
-			
+
 			if (e1 && (e1->target != stop1))
 			{
 				Edge* f1 = e1->reverse->next;
@@ -1591,7 +1605,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
 		while (true)
 		{
 			int64_t dy = (et1 - et0).dot(s);
-			
+
 			if (e1 && (e1->target != stop1))
 			{
 				Edge* f1 = e1->prev->reverse;
@@ -1608,7 +1622,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
 					}
 				}
 			}
-			
+
 			if (e0 && (e0->target != stop0))
 			{
 				Edge* f0 = e0->reverse->prev;
@@ -1643,7 +1657,6 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
 #endif
 }
 
-
 void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
 {
 	if (!h1.maxXy)
@@ -1655,7 +1668,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
 		h0 = h1;
 		return;
 	}
-	
+
 	mergeStamp--;
 
 	Vertex* c0 = NULL;
@@ -1695,7 +1708,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
 				e = e->next;
 			} while (e != c0->edges);
 		}
-		
+
 		e = c1->edges;
 		Edge* start1 = NULL;
 		if (e)
@@ -1747,7 +1760,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
 		Point32 r = prevPoint - c0->point;
 		Point64 rxs = r.cross(s);
 		Point64 sxrxs = s.cross(rxs);
-		
+
 #ifdef DEBUG_CONVEX_HULL
 		printf("\n  Checking %d %d\n", c0->point.index, c1->point.index);
 #endif
@@ -1798,7 +1811,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
 				e->prev = pendingTail1;
 				pendingTail1 = e;
 			}
-			
+
 			Edge* e0 = min0;
 			Edge* e1 = min1;
 
@@ -1815,7 +1828,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
 			{
 				if (toPrev1)
 				{
-					for (Edge* e = toPrev1->next, *n = NULL; e != min1; e = n)
+					for (Edge *e = toPrev1->next, *n = NULL; e != min1; e = n)
 					{
 						n = e->next;
 						removeEdgePair(e);
@@ -1851,7 +1864,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
 			{
 				if (toPrev0)
 				{
-					for (Edge* e = toPrev0->prev, *n = NULL; e != min0; e = n)
+					for (Edge *e = toPrev0->prev, *n = NULL; e != min0; e = n)
 					{
 						n = e->prev;
 						removeEdgePair(e);
@@ -1893,7 +1906,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
 			}
 			else
 			{
-				for (Edge* e = toPrev0->prev, *n = NULL; e != firstNew0; e = n)
+				for (Edge *e = toPrev0->prev, *n = NULL; e != firstNew0; e = n)
 				{
 					n = e->prev;
 					removeEdgePair(e);
@@ -1912,7 +1925,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
 			}
 			else
 			{
-				for (Edge* e = toPrev1->next, *n = NULL; e != firstNew1; e = n)
+				for (Edge *e = toPrev1->next, *n = NULL; e != firstNew1; e = n)
 				{
 					n = e->next;
 					removeEdgePair(e);
@@ -1923,7 +1936,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
 					pendingTail1->link(firstNew1);
 				}
 			}
-			
+
 			return;
 		}
 
@@ -1933,24 +1946,23 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
 
 class pointCmp
 {
-	public:
-
-    bool operator() ( const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q ) const
-		{
-			return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
-		}
+public:
+	bool operator()(const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q) const
+	{
+		return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
+	}
 };
 
 void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
 {
 	btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30));
-	const char* ptr = (const char*) coords;
+	const char* ptr = (const char*)coords;
 	if (doubleCoords)
 	{
 		for (int i = 0; i < count; i++)
 		{
-			const double* v = (const double*) ptr;
-			btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
+			const double* v = (const double*)ptr;
+			btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]);
 			ptr += stride;
 			min.setMin(p);
 			max.setMax(p);
@@ -1960,7 +1972,7 @@ void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int st
 	{
 		for (int i = 0; i < count; i++)
 		{
-			const float* v = (const float*) ptr;
+			const float* v = (const float*)ptr;
 			btVector3 p(v[0], v[1], v[2]);
 			ptr += stride;
 			min.setMin(p);
@@ -2001,18 +2013,18 @@ void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int st
 
 	btAlignedObjectArray<Point32> points;
 	points.resize(count);
-	ptr = (const char*) coords;
+	ptr = (const char*)coords;
 	if (doubleCoords)
 	{
 		for (int i = 0; i < count; i++)
 		{
-			const double* v = (const double*) ptr;
-			btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
+			const double* v = (const double*)ptr;
+			btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]);
 			ptr += stride;
 			p = (p - center) * s;
-			points[i].x = (int32_t) p[medAxis];
-			points[i].y = (int32_t) p[maxAxis];
-			points[i].z = (int32_t) p[minAxis];
+			points[i].x = (int32_t)p[medAxis];
+			points[i].y = (int32_t)p[maxAxis];
+			points[i].z = (int32_t)p[minAxis];
 			points[i].index = i;
 		}
 	}
@@ -2020,13 +2032,13 @@ void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int st
 	{
 		for (int i = 0; i < count; i++)
 		{
-			const float* v = (const float*) ptr;
+			const float* v = (const float*)ptr;
 			btVector3 p(v[0], v[1], v[2]);
 			ptr += stride;
 			p = (p - center) * s;
-			points[i].x = (int32_t) p[medAxis];
-			points[i].y = (int32_t) p[maxAxis];
-			points[i].z = (int32_t) p[minAxis];
+			points[i].x = (int32_t)p[medAxis];
+			points[i].y = (int32_t)p[maxAxis];
+			points[i].z = (int32_t)p[minAxis];
 			points[i].index = i;
 		}
 	}
@@ -2180,7 +2192,7 @@ btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
 				minDist = dist;
 			}
 		}
-		
+
 		if (minDist <= 0)
 		{
 			return 0;
@@ -2221,7 +2233,7 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec
 	{
 		origShift[2] /= scaling[2];
 	}
-	Point32 shift((int32_t) origShift[medAxis], (int32_t) origShift[maxAxis], (int32_t) origShift[minAxis]);
+	Point32 shift((int32_t)origShift[medAxis], (int32_t)origShift[maxAxis], (int32_t)origShift[minAxis]);
 	if (shift.isZero())
 	{
 		return true;
@@ -2229,7 +2241,7 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec
 	Point64 normal = face->getNormal();
 #ifdef DEBUG_CONVEX_HULL
 	printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
-				 face->origin.x, face->origin.y, face->origin.z, face->dir0.x, face->dir0.y, face->dir0.z, face->dir1.x, face->dir1.y, face->dir1.z, shift.x, shift.y, shift.z);
+		   face->origin.x, face->origin.y, face->origin.z, face->dir0.x, face->dir0.y, face->dir0.z, face->dir1.x, face->dir1.y, face->dir1.z, shift.x, shift.y, shift.z);
 #endif
 	int64_t origDot = face->origin.dot(normal);
 	Point32 shiftedOrigin = face->origin + shift;
@@ -2266,7 +2278,7 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec
 #ifdef DEBUG_CONVEX_HULL
 			printf("Moving downwards, edge is ");
 			e->print();
-			printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
+			printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot);
 #endif
 			if (dot.compare(optDot) < 0)
 			{
@@ -2302,7 +2314,7 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec
 #ifdef DEBUG_CONVEX_HULL
 			printf("Moving upwards, edge is ");
 			e->print();
-			printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
+			printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot);
 #endif
 			if (dot.compare(optDot) > 0)
 			{
@@ -2318,7 +2330,7 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec
 			}
 			e = e->prev;
 		} while (e != startEdge);
-		
+
 		if (!intersection)
 		{
 			return true;
@@ -2355,7 +2367,7 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec
 		printf("Needed %d iterations to check for complete containment\n", n);
 #endif
 	}
-	
+
 	Edge* firstIntersection = NULL;
 	Edge* faceEdge = NULL;
 	Edge* firstFaceEdge = NULL;
@@ -2464,7 +2476,7 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec
 #ifdef DEBUG_CONVEX_HULL
 			printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
 #endif
-			
+
 			Point64 n0 = intersection->face->getNormal();
 			Point64 n1 = intersection->reverse->face->getNormal();
 			int64_t m00 = face->dir0.dot(n0);
@@ -2478,16 +2490,13 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec
 			Vertex* v = vertexPool.newObject();
 			v->point.index = -1;
 			v->copy = -1;
-			v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01)
-															+ Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
-															Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01)
-															+ Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
-															Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01)
-															+ Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
-															det);
-			v->point.x = (int32_t) v->point128.xvalue();
-			v->point.y = (int32_t) v->point128.yvalue();
-			v->point.z = (int32_t) v->point128.zvalue();
+			v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01) + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
+									Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01) + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
+									Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01) + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
+									det);
+			v->point.x = (int32_t)v->point128.xvalue();
+			v->point.y = (int32_t)v->point128.yvalue();
+			v->point.z = (int32_t)v->point128.zvalue();
 			intersection->target = v;
 			v->edges = e;
 
@@ -2626,7 +2635,6 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec
 	return true;
 }
 
-
 static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices)
 {
 	int index = vertex->copy;
@@ -2750,8 +2758,3 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in
 
 	return shift;
 }
-
-
-
-
-
diff --git a/extern/bullet2/src/LinearMath/btConvexHullComputer.h b/extern/bullet2/src/LinearMath/btConvexHullComputer.h
index 6871ce80e00..18b26eea9a8 100644
--- a/extern/bullet2/src/LinearMath/btConvexHullComputer.h
+++ b/extern/bullet2/src/LinearMath/btConvexHullComputer.h
@@ -23,59 +23,59 @@ subject to the following restrictions:
 /// Ole Kniemeyer, MAXON Computer GmbH
 class btConvexHullComputer
 {
+private:
+	btScalar compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp);
+
+public:
+	class Edge
+	{
 	private:
-		btScalar compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp);
+		int next;
+		int reverse;
+		int targetVertex;
 
-	public:
+		friend class btConvexHullComputer;
 
-		class Edge
+	public:
+		int getSourceVertex() const
 		{
-			private:
-				int next;
-				int reverse;
-				int targetVertex;
-
-				friend class btConvexHullComputer;
-
-			public:
-				int getSourceVertex() const
-				{
-					return (this + reverse)->targetVertex;
-				}
+			return (this + reverse)->targetVertex;
+		}
 
-				int getTargetVertex() const
-				{
-					return targetVertex;
-				}
+		int getTargetVertex() const
+		{
+			return targetVertex;
+		}
 
-				const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
-				{
-					return this + next;
-				}
+		const Edge* getNextEdgeOfVertex() const  // clockwise list of all edges of a vertex
+		{
+			return this + next;
+		}
 
-				const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
-				{
-					return (this + reverse)->getNextEdgeOfVertex();
-				}
+		const Edge* getNextEdgeOfFace() const  // counter-clockwise list of all edges of a face
+		{
+			return (this + reverse)->getNextEdgeOfVertex();
+		}
 
-				const Edge* getReverseEdge() const
-				{
-					return this + reverse;
-				}
-		};
+		const Edge* getReverseEdge() const
+		{
+			return this + reverse;
+		}
+	};
 
+	// Vertices of the output hull
+	btAlignedObjectArray<btVector3> vertices;
 
-		// Vertices of the output hull
-		btAlignedObjectArray<btVector3> vertices;
-		btAlignedObjectArray<int> original_vertex_index;
+	// The original vertex index in the input coords array
+	btAlignedObjectArray<int> original_vertex_index;
 
-		// Edges of the output hull
-		btAlignedObjectArray<Edge> edges;
+	// Edges of the output hull
+	btAlignedObjectArray<Edge> edges;
 
-		// Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
-		btAlignedObjectArray<int> faces;
+	// Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
+	btAlignedObjectArray<int> faces;
 
-		/*
+	/*
 		Compute convex hull of "count" vertices stored in "coords". "stride" is the difference in bytes
 		between the addresses of consecutive vertices. If "shrink" is positive, the convex hull is shrunken
 		by that amount (each face is moved by "shrink" length units towards the center along its normal).
@@ -87,18 +87,16 @@ class btConvexHullComputer
 
 		The output convex hull can be found in the member variables "vertices", "edges", "faces".
 		*/
-		btScalar compute(const float* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
-		{
-			return compute(coords, false, stride, count, shrink, shrinkClamp);
-		}
-
-		// same as above, but double precision
-		btScalar compute(const double* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
-		{
-			return compute(coords, true, stride, count, shrink, shrinkClamp);
-		}
+	btScalar compute(const float* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
+	{
+		return compute(coords, false, stride, count, shrink, shrinkClamp);
+	}
+
+	// same as above, but double precision
+	btScalar compute(const double* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
+	{
+		return compute(coords, true, stride, count, shrink, shrinkClamp);
+	}
 };
 
-
-#endif //BT_CONVEX_HULL_COMPUTER_H
-
+#endif  //BT_CONVEX_HULL_COMPUTER_H
diff --git a/extern/bullet2/src/LinearMath/btCpuFeatureUtility.h b/extern/bullet2/src/LinearMath/btCpuFeatureUtility.h
index d2cab52d488..5e4b9a313c7 100644
--- a/extern/bullet2/src/LinearMath/btCpuFeatureUtility.h
+++ b/extern/bullet2/src/LinearMath/btCpuFeatureUtility.h
@@ -4,20 +4,20 @@
 
 #include "LinearMath/btScalar.h"
 
-#include <string.h>//memset
-#ifdef  USE_SIMD
+#include <string.h>  //memset
+#ifdef USE_SIMD
 #include <emmintrin.h>
 #ifdef BT_ALLOW_SSE4
 #include <intrin.h>
-#endif //BT_ALLOW_SSE4
-#endif //USE_SIMD
+#endif  //BT_ALLOW_SSE4
+#endif  //USE_SIMD
 
 #if defined BT_USE_NEON
-#define ARM_NEON_GCC_COMPATIBILITY  1
+#define ARM_NEON_GCC_COMPATIBILITY 1
 #include <arm_neon.h>
 #include <sys/types.h>
-#include <sys/sysctl.h> //for sysctlbyname
-#endif //BT_USE_NEON
+#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
@@ -26,14 +26,13 @@ class btCpuFeatureUtility
 public:
 	enum btCpuFeature
 	{
-		CPU_FEATURE_FMA3=1,
-		CPU_FEATURE_SSE4_1=2,
-		CPU_FEATURE_NEON_HPFP=4
+		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)
@@ -49,15 +48,15 @@ public:
 			if (0 == err && hasFeature)
 				capabilities |= CPU_FEATURE_NEON_HPFP;
 		}
-#endif //BT_USE_NEON
+#endif  //BT_USE_NEON
 
-#ifdef  BT_ALLOW_SSE4
+#ifdef BT_ALLOW_SSE4
 		{
-			int					cpuInfo[4];
+			int cpuInfo[4];
 			memset(cpuInfo, 0, sizeof(cpuInfo));
-			unsigned long long	sseExt = 0;
+			unsigned long long sseExt = 0;
 			__cpuid(cpuInfo, 1);
-			
+
 			bool osUsesXSAVE_XRSTORE = cpuInfo[2] & (1 << 27) || false;
 			bool cpuAVXSuport = cpuInfo[2] & (1 << 28) || false;
 
@@ -79,14 +78,11 @@ public:
 				capabilities |= btCpuFeatureUtility::CPU_FEATURE_SSE4_1;
 			}
 		}
-#endif//BT_ALLOW_SSE4
+#endif  //BT_ALLOW_SSE4
 
 		testedCapabilities = true;
 		return capabilities;
 	}
-
-
 };
 
-
-#endif //BT_CPU_UTILITY_H
+#endif  //BT_CPU_UTILITY_H
diff --git a/extern/bullet2/src/LinearMath/btDefaultMotionState.h b/extern/bullet2/src/LinearMath/btDefaultMotionState.h
index 01c5f8d932d..14c40d36b07 100644
--- a/extern/bullet2/src/LinearMath/btDefaultMotionState.h
+++ b/extern/bullet2/src/LinearMath/btDefaultMotionState.h
@@ -4,39 +4,37 @@
 #include "btMotionState.h"
 
 ///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets.
-ATTRIBUTE_ALIGNED16(struct)	btDefaultMotionState : public btMotionState
+ATTRIBUTE_ALIGNED16(struct)
+btDefaultMotionState : public btMotionState
 {
 	btTransform m_graphicsWorldTrans;
-	btTransform	m_centerOfMassOffset;
+	btTransform m_centerOfMassOffset;
 	btTransform m_startWorldTrans;
-	void*		m_userPointer;
+	void* m_userPointer;
 
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btDefaultMotionState(const btTransform& startTrans = btTransform::getIdentity(),const btTransform& centerOfMassOffset = btTransform::getIdentity())
+	btDefaultMotionState(const btTransform& startTrans = btTransform::getIdentity(), const btTransform& centerOfMassOffset = btTransform::getIdentity())
 		: m_graphicsWorldTrans(startTrans),
-		m_centerOfMassOffset(centerOfMassOffset),
-		m_startWorldTrans(startTrans),
-		m_userPointer(0)
+		  m_centerOfMassOffset(centerOfMassOffset),
+		  m_startWorldTrans(startTrans),
+		  m_userPointer(0)
 
 	{
 	}
 
 	///synchronizes world transform from user to physics
-	virtual void	getWorldTransform(btTransform& centerOfMassWorldTrans ) const 
+	virtual void getWorldTransform(btTransform & centerOfMassWorldTrans) const
 	{
-			centerOfMassWorldTrans = m_graphicsWorldTrans * m_centerOfMassOffset.inverse() ;
+		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)
+	virtual void setWorldTransform(const btTransform& centerOfMassWorldTrans)
 	{
-			m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset;
+		m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset;
 	}
-
-	
-
 };
 
-#endif //BT_DEFAULT_MOTION_STATE_H
+#endif  //BT_DEFAULT_MOTION_STATE_H
diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.cpp b/extern/bullet2/src/LinearMath/btGeometryUtil.cpp
index 5ac230f712f..115e3eab810 100644
--- a/extern/bullet2/src/LinearMath/btGeometryUtil.cpp
+++ b/extern/bullet2/src/LinearMath/btGeometryUtil.cpp
@@ -12,49 +12,43 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #include "btGeometryUtil.h"
 
-
 /*
   Make sure this dummy function never changes so that it
   can be used by probes that are checking whether the
   library is actually installed.
 */
 extern "C"
-{	
-	void btBulletMathProbe ();
+{
+	void btBulletMathProbe();
 
-	void btBulletMathProbe () {}
+	void btBulletMathProbe() {}
 }
 
-
-bool	btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar	margin)
+bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin)
 {
 	int numbrushes = planeEquations.size();
-	for (int i=0;i<numbrushes;i++)
+	for (int i = 0; i < numbrushes; i++)
 	{
 		const btVector3& N1 = planeEquations[i];
-		btScalar dist = btScalar(N1.dot(point))+btScalar(N1[3])-margin;
-		if (dist>btScalar(0.))
+		btScalar dist = btScalar(N1.dot(point)) + btScalar(N1[3]) - margin;
+		if (dist > btScalar(0.))
 		{
 			return false;
 		}
 	}
 	return true;
-		
 }
 
-
-bool	btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar	margin)
+bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin)
 {
 	int numvertices = vertices.size();
-	for (int i=0;i<numvertices;i++)
+	for (int i = 0; i < numvertices; i++)
 	{
 		const btVector3& N1 = vertices[i];
-		btScalar dist = btScalar(planeNormal.dot(N1))+btScalar(planeNormal[3])-margin;
-		if (dist>btScalar(0.))
+		btScalar dist = btScalar(planeNormal.dot(N1)) + btScalar(planeNormal[3]) - margin;
+		if (dist > btScalar(0.))
 		{
 			return false;
 		}
@@ -62,102 +56,98 @@ bool	btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const
 	return true;
 }
 
-bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations);
+bool notExist(const btVector3& planeEquation, const btAlignedObjectArray<btVector3>& planeEquations);
 
-bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations)
+bool notExist(const btVector3& planeEquation, const btAlignedObjectArray<btVector3>& planeEquations)
 {
 	int numbrushes = planeEquations.size();
-	for (int i=0;i<numbrushes;i++)
+	for (int i = 0; i < numbrushes; i++)
 	{
 		const btVector3& N1 = planeEquations[i];
 		if (planeEquation.dot(N1) > btScalar(0.999))
 		{
 			return false;
-		} 
+		}
 	}
 	return true;
 }
 
-void	btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut )
+void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut)
 {
-		const int numvertices = vertices.size();
+	const int numvertices = vertices.size();
 	// brute force:
-	for (int i=0;i<numvertices;i++)
+	for (int i = 0; i < numvertices; i++)
 	{
 		const btVector3& N1 = vertices[i];
-		
 
-		for (int j=i+1;j<numvertices;j++)
+		for (int j = i + 1; j < numvertices; j++)
 		{
 			const btVector3& N2 = vertices[j];
-				
-			for (int k=j+1;k<numvertices;k++)
-			{
 
+			for (int k = j + 1; k < numvertices; k++)
+			{
 				const btVector3& N3 = vertices[k];
 
-				btVector3 planeEquation,edge0,edge1;
-				edge0 = N2-N1;
-				edge1 = N3-N1;
+				btVector3 planeEquation, edge0, edge1;
+				edge0 = N2 - N1;
+				edge1 = N3 - N1;
 				btScalar normalSign = btScalar(1.);
-				for (int ww=0;ww<2;ww++)
+				for (int ww = 0; ww < 2; ww++)
 				{
 					planeEquation = normalSign * edge0.cross(edge1);
 					if (planeEquation.length2() > btScalar(0.0001))
 					{
 						planeEquation.normalize();
-						if (notExist(planeEquation,planeEquationsOut))
+						if (notExist(planeEquation, planeEquationsOut))
 						{
 							planeEquation[3] = -planeEquation.dot(N1);
-							
-								//check if inside, and replace supportingVertexOut if needed
-								if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01)))
-								{
-									planeEquationsOut.push_back(planeEquation);
-								}
+
+							//check if inside, and replace supportingVertexOut if needed
+							if (areVerticesBehindPlane(planeEquation, vertices, btScalar(0.01)))
+							{
+								planeEquationsOut.push_back(planeEquation);
+							}
 						}
 					}
 					normalSign = btScalar(-1.);
 				}
-			
 			}
 		}
 	}
-
 }
 
-void	btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
+void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations, btAlignedObjectArray<btVector3>& verticesOut)
 {
 	const int numbrushes = planeEquations.size();
 	// brute force:
-	for (int i=0;i<numbrushes;i++)
+	for (int i = 0; i < numbrushes; i++)
 	{
 		const btVector3& N1 = planeEquations[i];
-		
 
-		for (int j=i+1;j<numbrushes;j++)
+		for (int j = i + 1; j < numbrushes; j++)
 		{
 			const btVector3& N2 = planeEquations[j];
-				
-			for (int k=j+1;k<numbrushes;k++)
-			{
 
+			for (int k = j + 1; k < numbrushes; k++)
+			{
 				const btVector3& N3 = planeEquations[k];
 
-				btVector3 n2n3; n2n3 = N2.cross(N3);
-				btVector3 n3n1; n3n1 = N3.cross(N1);
-				btVector3 n1n2; n1n2 = N1.cross(N2);
-				
-				if ( ( n2n3.length2() > btScalar(0.0001) ) &&
-					 ( n3n1.length2() > btScalar(0.0001) ) &&
-					 ( n1n2.length2() > btScalar(0.0001) ) )
+				btVector3 n2n3;
+				n2n3 = N2.cross(N3);
+				btVector3 n3n1;
+				n3n1 = N3.cross(N1);
+				btVector3 n1n2;
+				n1n2 = N1.cross(N2);
+
+				if ((n2n3.length2() > btScalar(0.0001)) &&
+					(n3n1.length2() > btScalar(0.0001)) &&
+					(n1n2.length2() > btScalar(0.0001)))
 				{
 					//point P out of 3 plane equations:
 
-					//	d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )  
-					//P =  -------------------------------------------------------------------------  
-					//   N1 . ( N2 * N3 )  
-
+					//	d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
+					//P =  -------------------------------------------------------------------------
+					//   N1 . ( N2 * N3 )
 
 					btScalar quotient = (N1.dot(n2n3));
 					if (btFabs(quotient) > btScalar(0.000001))
@@ -172,7 +162,7 @@ void	btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<bt
 						potentialVertex *= quotient;
 
 						//check if inside, and replace supportingVertexOut if needed
-						if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01)))
+						if (isPointInsidePlanes(planeEquations, potentialVertex, btScalar(0.01)))
 						{
 							verticesOut.push_back(potentialVertex);
 						}
@@ -182,4 +172,3 @@ void	btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<bt
 		}
 	}
 }
-
diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.h b/extern/bullet2/src/LinearMath/btGeometryUtil.h
index a4b13b45609..0ce5b76d921 100644
--- a/extern/bullet2/src/LinearMath/btGeometryUtil.h
+++ b/extern/bullet2/src/LinearMath/btGeometryUtil.h
@@ -12,7 +12,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_GEOMETRY_UTIL_H
 #define BT_GEOMETRY_UTIL_H
 
@@ -22,21 +21,16 @@ subject to the following restrictions:
 ///The btGeometryUtil helper class provides a few methods to convert between plane equations and vertices.
 class btGeometryUtil
 {
-	public:
-	
-	
-		static void	getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut );
-
-		static void	getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut );
-	
-		static bool	isInside(const btAlignedObjectArray<btVector3>& vertices, const btVector3& planeNormal, btScalar	margin);
-		
-		static bool	isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar	margin);
+public:
+	static void getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut);
 
-		static bool	areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar	margin);
+	static void getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations, btAlignedObjectArray<btVector3>& verticesOut);
 
-};
+	static bool isInside(const btAlignedObjectArray<btVector3>& vertices, const btVector3& planeNormal, btScalar margin);
 
+	static bool isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin);
 
-#endif //BT_GEOMETRY_UTIL_H
+	static bool areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin);
+};
 
+#endif  //BT_GEOMETRY_UTIL_H
diff --git a/extern/bullet2/src/LinearMath/btGrahamScan2dConvexHull.h b/extern/bullet2/src/LinearMath/btGrahamScan2dConvexHull.h
index 13a79aa5856..0fcb285971d 100644
--- a/extern/bullet2/src/LinearMath/btGrahamScan2dConvexHull.h
+++ b/extern/bullet2/src/LinearMath/btGrahamScan2dConvexHull.h
@@ -13,41 +13,40 @@ subject to the following restrictions:
 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)
+		: btVector3(org),
+		  m_orgIndex(orgIndex)
 	{
 	}
-	btScalar	m_angle;
+	btScalar m_angle;
 	int m_orgIndex;
 };
 
-
-struct btAngleCompareFunc {
+struct btAngleCompareFunc
+{
 	btVector3 m_anchor;
 	btAngleCompareFunc(const btVector3& anchor)
-	: m_anchor(anchor) 
+		: m_anchor(anchor)
 	{
 	}
-	bool operator()(const GrahamVector3& a, const GrahamVector3& b) const {
+	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();
+			btScalar al = (a - m_anchor).length2();
+			btScalar bl = (b - m_anchor).length2();
 			if (al != bl)
-				return  al < bl;
+				return al < bl;
 			else
 			{
 				return a.m_orgIndex < b.m_orgIndex;
@@ -58,73 +57,73 @@ struct btAngleCompareFunc {
 
 inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector3>& originalPoints, btAlignedObjectArray<GrahamVector3>& hull, const btVector3& normalAxis)
 {
-	btVector3 axis0,axis1;
-	btPlaneSpace1(normalAxis,axis0,axis1);
-	
+	btVector3 axis0, axis1;
+	btPlaneSpace1(normalAxis, axis0, axis1);
 
-	if (originalPoints.size()<=1)
+	if (originalPoints.size() <= 1)
 	{
-		for (int i=0;i<originalPoints.size();i++)
+		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++)
+	for (int i = 0; i < originalPoints.size(); i++)
 	{
-//		const btVector3& left = originalPoints[i];
-//		const btVector3& right = originalPoints[0];
+		//		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);
+			originalPoints.swap(0, i);
 		}
 	}
 
 	//also precompute angles
 	originalPoints[0].m_angle = -1e30f;
-	for (int i=1;i<originalPoints.size();i++)
+	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);
-	    }
+		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);
+	originalPoints.quickSortInternal(comp, 1, originalPoints.size() - 1);
 
 	int i;
-	for (i = 0; i<2; 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++) 
+	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;
+		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 
+			else
 				hull.push_back(originalPoints[i]);
 		}
 
-	    if( hull.size() == 1 )
-	    {
-	      hull.push_back( originalPoints[i] );
-	    }
+		if (hull.size() == 1)
+		{
+			hull.push_back(originalPoints[i]);
+		}
 	}
 }
 
-#endif //GRAHAM_SCAN_2D_CONVEX_HULL_H
+#endif  //GRAHAM_SCAN_2D_CONVEX_HULL_H
diff --git a/extern/bullet2/src/LinearMath/btHashMap.h b/extern/bullet2/src/LinearMath/btHashMap.h
index af9727b7ada..1fca0fb73a5 100644
--- a/extern/bullet2/src/LinearMath/btHashMap.h
+++ b/extern/bullet2/src/LinearMath/btHashMap.h
@@ -13,82 +13,73 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_HASH_MAP_H
 #define BT_HASH_MAP_H
 
+#include <string>
 #include "btAlignedObjectArray.h"
 
 ///very basic hashable string implementation, compatible with btHashMap
 struct btHashString
 {
-	const char* m_string;
-	unsigned int	m_hash;
+	std::string m_string1;
+	unsigned int m_hash;
 
-	SIMD_FORCE_INLINE	unsigned int getHash()const
+	SIMD_FORCE_INLINE unsigned int getHash() const
 	{
 		return m_hash;
 	}
 
+	btHashString()
+	{
+		m_string1 = "";
+		m_hash = 0;
+	}
 	btHashString(const char* name)
-		:m_string(name)
+		: m_string1(name)
 	{
 		/* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
-		static const unsigned int  InitialFNV = 2166136261u;
+		static const unsigned int InitialFNV = 2166136261u;
 		static const unsigned int FNVMultiple = 16777619u;
 
 		/* Fowler / Noll / Vo (FNV) Hash */
 		unsigned int hash = InitialFNV;
-		
-		for(int i = 0; m_string[i]; i++)
+
+		for (int i = 0; m_string1.c_str()[i]; i++)
 		{
-			hash = hash ^ (m_string[i]);       /* xor  the low 8 bits */
-			hash = hash * FNVMultiple;  /* multiply by the magic number */
+			hash = hash ^ (m_string1.c_str()[i]); /* xor  the low 8 bits */
+			hash = hash * FNVMultiple;            /* multiply by the magic number */
 		}
 		m_hash = hash;
 	}
 
-	int portableStringCompare(const char* src,	const char* dst) const
-	{
-			int ret = 0 ;
-
-			while( ! (ret = *(unsigned char *)src - *(unsigned char *)dst) && *dst)
-					++src, ++dst;
-
-			if ( ret < 0 )
-					ret = -1 ;
-			else if ( ret > 0 )
-					ret = 1 ;
-
-			return( ret );
-	}
-
 	bool equals(const btHashString& other) const
 	{
-		return (m_string == other.m_string) ||
-			(0==portableStringCompare(m_string,other.m_string));
-
+		return (m_string1 == other.m_string1);
 	}
-
 };
 
-const int BT_HASH_NULL=0xffffffff;
-
+const int BT_HASH_NULL = 0xffffffff;
 
 class btHashInt
 {
-	int	m_uid;
+	int m_uid;
+
 public:
-	btHashInt(int uid)	:m_uid(uid)
+	btHashInt()
 	{
 	}
 
-	int	getUid1() const
+	btHashInt(int uid) : m_uid(uid)
+	{
+	}
+
+	int getUid1() const
 	{
 		return m_uid;
 	}
 
-	void	setUid1(int uid)
+	void setUid1(int uid)
 	{
 		m_uid = uid;
 	}
@@ -98,34 +89,35 @@ public:
 		return getUid1() == other.getUid1();
 	}
 	//to our success
-	SIMD_FORCE_INLINE	unsigned int getHash()const
+	SIMD_FORCE_INLINE unsigned int getHash() const
 	{
-		int key = m_uid;
+		unsigned int key = m_uid;
 		// Thomas Wang's hash
-		key += ~(key << 15);	key ^=  (key >> 10);	key +=  (key << 3);	key ^=  (key >> 6);	key += ~(key << 11);	key ^=  (key >> 16);
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
+
 		return key;
 	}
 };
 
-
-
 class btHashPtr
 {
-
-	union
-	{
-		const void*	m_pointer;
-		int	m_hashValues[2];
+	union {
+		const void* m_pointer;
+		unsigned int m_hashValues[2];
 	};
 
 public:
-
 	btHashPtr(const void* ptr)
-		:m_pointer(ptr)
+		: m_pointer(ptr)
 	{
 	}
 
-	const void*	getPointer() const
+	const void* getPointer() const
 	{
 		return m_pointer;
 	}
@@ -136,65 +128,68 @@ public:
 	}
 
 	//to our success
-	SIMD_FORCE_INLINE	unsigned int getHash()const
+	SIMD_FORCE_INLINE unsigned int getHash() const
 	{
-		const bool VOID_IS_8 = ((sizeof(void*)==8));
-		
-		int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0];
-	
+		const bool VOID_IS_8 = ((sizeof(void*) == 8));
+
+		unsigned int key = VOID_IS_8 ? m_hashValues[0] + m_hashValues[1] : m_hashValues[0];
 		// Thomas Wang's hash
-		key += ~(key << 15);	key ^=  (key >> 10);	key +=  (key << 3);	key ^=  (key >> 6);	key += ~(key << 11);	key ^=  (key >> 16);
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
 		return key;
 	}
-
-	
 };
 
-
 template <class Value>
 class btHashKeyPtr
 {
-        int     m_uid;
+	int m_uid;
+
 public:
+	btHashKeyPtr(int uid) : m_uid(uid)
+	{
+	}
 
-        btHashKeyPtr(int uid)    :m_uid(uid)
-        {
-        }
-
-        int     getUid1() const
-        {
-                return m_uid;
-        }
-
-        bool equals(const btHashKeyPtr<Value>& other) const
-        {
-                return getUid1() == other.getUid1();
-        }
-
-        //to our success
-        SIMD_FORCE_INLINE       unsigned int getHash()const
-        {
-                int key = m_uid;
-                // Thomas Wang's hash
-                key += ~(key << 15);	key ^=  (key >> 10);	key +=  (key << 3);	key ^=  (key >> 6);	key += ~(key << 11);	key ^=  (key >> 16);
-                return key;
-        }
-
-        
-};
+	int getUid1() const
+	{
+		return m_uid;
+	}
 
+	bool equals(const btHashKeyPtr<Value>& other) const
+	{
+		return getUid1() == other.getUid1();
+	}
+
+	//to our success
+	SIMD_FORCE_INLINE unsigned int getHash() const
+	{
+		unsigned int key = m_uid;
+		// Thomas Wang's hash
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
+		return key;
+	}
+};
 
 template <class Value>
 class btHashKey
 {
-	int	m_uid;
-public:
+	int m_uid;
 
-	btHashKey(int uid)	:m_uid(uid)
+public:
+	btHashKey(int uid) : m_uid(uid)
 	{
 	}
 
-	int	getUid1() const
+	int getUid1() const
 	{
 		return m_uid;
 	}
@@ -204,30 +199,33 @@ public:
 		return getUid1() == other.getUid1();
 	}
 	//to our success
-	SIMD_FORCE_INLINE	unsigned int getHash()const
+	SIMD_FORCE_INLINE unsigned int getHash() const
 	{
-		int key = m_uid;
+		unsigned int key = m_uid;
 		// Thomas Wang's hash
-		key += ~(key << 15);	key ^=  (key >> 10);	key +=  (key << 3);	key ^=  (key >> 6);	key += ~(key << 11);	key ^=  (key >> 16);
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
 		return key;
 	}
 };
 
-
 ///The btHashMap template class implements a generic and lightweight hashmap.
 ///A basic sample of how to use btHashMap is located in Demos\BasicDemo\main.cpp
 template <class Key, class Value>
 class btHashMap
 {
-
 protected:
-	btAlignedObjectArray<int>		m_hashTable;
-	btAlignedObjectArray<int>		m_next;
-	
-	btAlignedObjectArray<Value>		m_valueArray;
-	btAlignedObjectArray<Key>		m_keyArray;
+	btAlignedObjectArray<int> m_hashTable;
+	btAlignedObjectArray<int> m_next;
+
+	btAlignedObjectArray<Value> m_valueArray;
+	btAlignedObjectArray<Key> m_keyArray;
 
-	void	growTables(const Key& /*key*/)
+	void growTables(const Key& /*key*/)
 	{
 		int newCapacity = m_valueArray.capacity();
 
@@ -241,7 +239,7 @@ protected:
 
 			int i;
 
-			for (i= 0; i < newCapacity; ++i)
+			for (i = 0; i < newCapacity; ++i)
 			{
 				m_hashTable[i] = BT_HASH_NULL;
 			}
@@ -250,30 +248,28 @@ protected:
 				m_next[i] = BT_HASH_NULL;
 			}
 
-			for(i=0;i<curHashtableSize;i++)
+			for (i = 0; i < curHashtableSize; i++)
 			{
 				//const Value& value = m_valueArray[i];
 				//const Key& key = m_keyArray[i];
 
-				int	hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity()-1);	// New hash value with new mask
+				int hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity() - 1);  // New hash value with new mask
 				m_next[i] = m_hashTable[hashValue];
 				m_hashTable[hashValue] = i;
 			}
-
-
 		}
 	}
 
-	public:
-
-	void insert(const Key& key, const Value& value) {
-		int hash = key.getHash() & (m_valueArray.capacity()-1);
+public:
+	void insert(const Key& key, const Value& value)
+	{
+		int hash = key.getHash() & (m_valueArray.capacity() - 1);
 
 		//replace value if the key is already there
 		int index = findIndex(key);
 		if (index != BT_HASH_NULL)
 		{
-			m_valueArray[index]=value;
+			m_valueArray[index] = value;
 			return;
 		}
 
@@ -287,19 +283,19 @@ protected:
 		{
 			growTables(key);
 			//hash with new capacity
-			hash = key.getHash() & (m_valueArray.capacity()-1);
+			hash = key.getHash() & (m_valueArray.capacity() - 1);
 		}
 		m_next[count] = m_hashTable[hash];
 		m_hashTable[hash] = count;
 	}
 
-	void remove(const Key& key) {
-
-		int hash = key.getHash() & (m_valueArray.capacity()-1);
+	void remove(const Key& key)
+	{
+		int hash = key.getHash() & (m_valueArray.capacity() - 1);
 
 		int pairIndex = findIndex(key);
-		
-		if (pairIndex ==BT_HASH_NULL)
+
+		if (pairIndex == BT_HASH_NULL)
 		{
 			return;
 		}
@@ -340,7 +336,7 @@ protected:
 		}
 
 		// Remove the last pair from the hash table.
-		int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity()-1);
+		int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity() - 1);
 
 		index = m_hashTable[lastHash];
 		btAssert(index != BT_HASH_NULL);
@@ -372,10 +368,8 @@ protected:
 
 		m_valueArray.pop_back();
 		m_keyArray.pop_back();
-
 	}
 
-
 	int size() const
 	{
 		return m_valueArray.size();
@@ -384,26 +378,50 @@ protected:
 	const Value* getAtIndex(int index) const
 	{
 		btAssert(index < m_valueArray.size());
-
-		return &m_valueArray[index];
+		btAssert(index >= 0);
+		if (index >= 0 && index < m_valueArray.size())
+		{
+			return &m_valueArray[index];
+		}
+		return 0;
 	}
 
 	Value* getAtIndex(int index)
 	{
 		btAssert(index < m_valueArray.size());
+		btAssert(index >= 0);
+		if (index >= 0 && index < m_valueArray.size())
+		{
+			return &m_valueArray[index];
+		}
+		return 0;
+	}
 
-		return &m_valueArray[index];
+	Key getKeyAtIndex(int index)
+	{
+		btAssert(index < m_keyArray.size());
+		btAssert(index >= 0);
+		return m_keyArray[index];
+	}
+
+	const Key getKeyAtIndex(int index) const
+	{
+		btAssert(index < m_keyArray.size());
+		btAssert(index >= 0);
+		return m_keyArray[index];
 	}
 
-	Value* operator[](const Key& key) {
+	Value* operator[](const Key& key)
+	{
 		return find(key);
 	}
 
-	const Value* operator[](const Key& key) const {
+	const Value* operator[](const Key& key) const
+	{
 		return find(key);
 	}
 
-	const Value*	find(const Key& key) const
+	const Value* find(const Key& key) const
 	{
 		int index = findIndex(key);
 		if (index == BT_HASH_NULL)
@@ -413,7 +431,7 @@ protected:
 		return &m_valueArray[index];
 	}
 
-	Value*	find(const Key& key)
+	Value* find(const Key& key)
 	{
 		int index = findIndex(key);
 		if (index == BT_HASH_NULL)
@@ -423,10 +441,9 @@ protected:
 		return &m_valueArray[index];
 	}
 
-
-	int	findIndex(const Key& key) const
+	int findIndex(const Key& key) const
 	{
-		unsigned int hash = key.getHash() & (m_valueArray.capacity()-1);
+		unsigned int hash = key.getHash() & (m_valueArray.capacity() - 1);
 
 		if (hash >= (unsigned int)m_hashTable.size())
 		{
@@ -441,14 +458,13 @@ protected:
 		return index;
 	}
 
-	void	clear()
+	void clear()
 	{
 		m_hashTable.clear();
 		m_next.clear();
 		m_valueArray.clear();
 		m_keyArray.clear();
 	}
-
 };
 
-#endif //BT_HASH_MAP_H
+#endif  //BT_HASH_MAP_H
diff --git a/extern/bullet2/src/LinearMath/btIDebugDraw.h b/extern/bullet2/src/LinearMath/btIDebugDraw.h
index 58c9838c490..82ec19a69b2 100644
--- a/extern/bullet2/src/LinearMath/btIDebugDraw.h
+++ b/extern/bullet2/src/LinearMath/btIDebugDraw.h
@@ -13,86 +13,84 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_IDEBUG_DRAW__H
 #define BT_IDEBUG_DRAW__H
 
 #include "btVector3.h"
 #include "btTransform.h"
 
-
-
 ///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
 ///Typical use case: create a debug drawer object, and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer and call debugDrawWorld.
 ///A class that implements the btIDebugDraw interface has to implement the drawLine method at a minimum.
 ///For color arguments the X,Y,Z components refer to Red, Green and Blue each in the range [0..1]
-class	btIDebugDraw
+class btIDebugDraw
 {
-	public:
-
-	ATTRIBUTE_ALIGNED16(struct) DefaultColors
+public:
+	ATTRIBUTE_ALIGNED16(struct)
+	DefaultColors
 	{
-		btVector3	m_activeObject;
-		btVector3	m_deactivatedObject;
-		btVector3	m_wantsDeactivationObject;
-		btVector3	m_disabledDeactivationObject;
-		btVector3	m_disabledSimulationObject;
-		btVector3	m_aabb;
+		btVector3 m_activeObject;
+		btVector3 m_deactivatedObject;
+		btVector3 m_wantsDeactivationObject;
+		btVector3 m_disabledDeactivationObject;
+		btVector3 m_disabledSimulationObject;
+		btVector3 m_aabb;
 		btVector3 m_contactPoint;
-		
+
 		DefaultColors()
-		:	m_activeObject(1,1,1),
-			m_deactivatedObject(0,1,0),
-			m_wantsDeactivationObject(0,1,1),
-			m_disabledDeactivationObject(1,0,0),
-			m_disabledSimulationObject(1,1,0),
-			m_aabb(1,0,0),
-			m_contactPoint(1,1,0)
+			: m_activeObject(1, 1, 1),
+			  m_deactivatedObject(0, 1, 0),
+			  m_wantsDeactivationObject(0, 1, 1),
+			  m_disabledDeactivationObject(1, 0, 0),
+			  m_disabledSimulationObject(1, 1, 0),
+			  m_aabb(1, 0, 0),
+			  m_contactPoint(1, 1, 0)
 		{
 		}
 	};
 
-	
-	enum	DebugDrawModes
+	enum DebugDrawModes
 	{
-		DBG_NoDebug=0,
+		DBG_NoDebug = 0,
 		DBG_DrawWireframe = 1,
-		DBG_DrawAabb=2,
-		DBG_DrawFeaturesText=4,
-		DBG_DrawContactPoints=8,
-		DBG_NoDeactivation=16,
+		DBG_DrawAabb = 2,
+		DBG_DrawFeaturesText = 4,
+		DBG_DrawContactPoints = 8,
+		DBG_NoDeactivation = 16,
 		DBG_NoHelpText = 32,
-		DBG_DrawText=64,
+		DBG_DrawText = 64,
 		DBG_ProfileTimings = 128,
 		DBG_EnableSatComparison = 256,
 		DBG_DisableBulletLCP = 512,
 		DBG_EnableCCD = 1024,
 		DBG_DrawConstraints = (1 << 11),
 		DBG_DrawConstraintLimits = (1 << 12),
-		DBG_FastWireframe = (1<<13),
-		DBG_DrawNormals = (1<<14),
-		DBG_DrawFrames = (1<<15),
+		DBG_FastWireframe = (1 << 13),
+		DBG_DrawNormals = (1 << 14),
+		DBG_DrawFrames = (1 << 15),
 		DBG_MAX_DEBUG_DRAW_MODE
 	};
 
-	virtual ~btIDebugDraw() {};
+	virtual ~btIDebugDraw(){};
 
-	
-	virtual DefaultColors	getDefaultColors() const	{	DefaultColors colors;	return colors;	}
+	virtual DefaultColors getDefaultColors() const
+	{
+		DefaultColors colors;
+		return colors;
+	}
 	///the default implementation for setDefaultColors has no effect. A derived class can implement it and store the colors.
 	virtual void setDefaultColors(const DefaultColors& /*colors*/) {}
-	
-	virtual void	drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
-		
-	virtual void    drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor)
+
+	virtual void drawLine(const btVector3& from, const btVector3& to, const btVector3& color) = 0;
+
+	virtual void drawLine(const btVector3& from, const btVector3& to, const btVector3& fromColor, const btVector3& toColor)
 	{
-        (void) toColor;
-		drawLine (from, to, fromColor);
+		(void)toColor;
+		drawLine(from, to, fromColor);
 	}
 
-	virtual void	drawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
+	virtual void drawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
 	{
-		
 		btVector3 center = transform.getOrigin();
 		btVector3 up = transform.getBasis().getColumn(1);
 		btVector3 axis = transform.getBasis().getColumn(0);
@@ -101,103 +99,102 @@ class	btIDebugDraw
 		btScalar minPs = -SIMD_HALF_PI;
 		btScalar maxPs = SIMD_HALF_PI;
 		btScalar stepDegrees = 30.f;
-		drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees ,false);
-		drawSpherePatch(center, up, -axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees,false );
+		drawSpherePatch(center, up, axis, radius, minTh, maxTh, minPs, maxPs, color, stepDegrees, false);
+		drawSpherePatch(center, up, -axis, radius, minTh, maxTh, minPs, maxPs, color, stepDegrees, false);
 	}
-	
-	virtual void	drawSphere (const btVector3& p, btScalar radius, const btVector3& color)
+
+	virtual void drawSphere(const btVector3& p, btScalar radius, const btVector3& color)
 	{
 		btTransform tr;
 		tr.setIdentity();
 		tr.setOrigin(p);
-		drawSphere(radius,tr,color);
+		drawSphere(radius, tr, color);
 	}
-	
-	virtual	void	drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& /*n0*/,const btVector3& /*n1*/,const btVector3& /*n2*/,const btVector3& color, btScalar alpha)
+
+	virtual void drawTriangle(const btVector3& v0, const btVector3& v1, const btVector3& v2, const btVector3& /*n0*/, const btVector3& /*n1*/, const btVector3& /*n2*/, const btVector3& color, btScalar alpha)
 	{
-		drawTriangle(v0,v1,v2,color,alpha);
+		drawTriangle(v0, v1, v2, color, alpha);
 	}
-	virtual	void	drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& color, btScalar /*alpha*/)
+	virtual void drawTriangle(const btVector3& v0, const btVector3& v1, const btVector3& v2, const btVector3& color, btScalar /*alpha*/)
 	{
-		drawLine(v0,v1,color);
-		drawLine(v1,v2,color);
-		drawLine(v2,v0,color);
+		drawLine(v0, v1, color);
+		drawLine(v1, v2, color);
+		drawLine(v2, v0, color);
 	}
 
-	virtual void	drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)=0;
+	virtual void drawContactPoint(const btVector3& PointOnB, const btVector3& normalOnB, btScalar distance, int lifeTime, const btVector3& color) = 0;
 
-	virtual void	reportErrorWarning(const char* warningString) = 0;
+	virtual void reportErrorWarning(const char* warningString) = 0;
 
-	virtual void	draw3dText(const btVector3& location,const char* textString) = 0;
-	
-	virtual void	setDebugMode(int debugMode) =0;
-	
-	virtual int		getDebugMode() const = 0;
+	virtual void draw3dText(const btVector3& location, const char* textString) = 0;
 
-	virtual void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color)
-	{
+	virtual void setDebugMode(int debugMode) = 0;
 
-		btVector3 halfExtents = (to-from)* 0.5f;
-		btVector3 center = (to+from) *0.5f;
-		int i,j;
+	virtual int getDebugMode() const = 0;
 
-		btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
-		for (i=0;i<4;i++)
+	virtual void drawAabb(const btVector3& from, const btVector3& to, const btVector3& color)
+	{
+		btVector3 halfExtents = (to - from) * 0.5f;
+		btVector3 center = (to + from) * 0.5f;
+		int i, j;
+
+		btVector3 edgecoord(1.f, 1.f, 1.f), pa, pb;
+		for (i = 0; i < 4; i++)
 		{
-			for (j=0;j<3;j++)
+			for (j = 0; j < 3; j++)
 			{
-				pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],		
-					edgecoord[2]*halfExtents[2]);
-				pa+=center;
+				pa = btVector3(edgecoord[0] * halfExtents[0], edgecoord[1] * halfExtents[1],
+							   edgecoord[2] * halfExtents[2]);
+				pa += center;
 
-				int othercoord = j%3;
-				edgecoord[othercoord]*=-1.f;
-				pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],	
-					edgecoord[2]*halfExtents[2]);
-				pb+=center;
+				int othercoord = j % 3;
+				edgecoord[othercoord] *= -1.f;
+				pb = btVector3(edgecoord[0] * halfExtents[0], edgecoord[1] * halfExtents[1],
+							   edgecoord[2] * halfExtents[2]);
+				pb += center;
 
-				drawLine(pa,pb,color);
+				drawLine(pa, pb, color);
 			}
-			edgecoord = btVector3(-1.f,-1.f,-1.f);
-			if (i<3)
-				edgecoord[i]*=-1.f;
+			edgecoord = btVector3(-1.f, -1.f, -1.f);
+			if (i < 3)
+				edgecoord[i] *= -1.f;
 		}
 	}
 	virtual void drawTransform(const btTransform& transform, btScalar orthoLen)
 	{
 		btVector3 start = transform.getOrigin();
-		drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0));
-		drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0));
-		drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f));
+		drawLine(start, start + transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(btScalar(1.), btScalar(0.3), btScalar(0.3)));
+		drawLine(start, start + transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(btScalar(0.3), btScalar(1.), btScalar(0.3)));
+		drawLine(start, start + transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(btScalar(0.3), btScalar(0.3), btScalar(1.)));
 	}
 
-	virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle, 
-				const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f))
+	virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,
+						 const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f))
 	{
 		const btVector3& vx = axis;
 		btVector3 vy = normal.cross(axis);
 		btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
 		int nSteps = (int)btFabs((maxAngle - minAngle) / step);
-		if(!nSteps) nSteps = 1;
+		if (!nSteps) nSteps = 1;
 		btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle);
-		if(drawSect)
+		if (drawSect)
 		{
 			drawLine(center, prev, color);
 		}
-		for(int i = 1; i <= nSteps; i++)
+		for (int i = 1; i <= nSteps; i++)
 		{
 			btScalar angle = minAngle + (maxAngle - minAngle) * btScalar(i) / btScalar(nSteps);
 			btVector3 next = center + radiusA * vx * btCos(angle) + radiusB * vy * btSin(angle);
 			drawLine(prev, next, color);
 			prev = next;
 		}
-		if(drawSect)
+		if (drawSect)
 		{
 			drawLine(center, prev, color);
 		}
 	}
-	virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius, 
-		btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f),bool drawCenter = true)
+	virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius,
+								 btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f), bool drawCenter = true)
 	{
 		btVector3 vA[74];
 		btVector3 vB[74];
@@ -211,33 +208,33 @@ class	btIDebugDraw
 		btVector3 jv = kv.cross(iv);
 		bool drawN = false;
 		bool drawS = false;
-		if(minTh <= -SIMD_HALF_PI)
+		if (minTh <= -SIMD_HALF_PI)
 		{
 			minTh = -SIMD_HALF_PI + step;
 			drawN = true;
 		}
-		if(maxTh >= SIMD_HALF_PI)
+		if (maxTh >= SIMD_HALF_PI)
 		{
 			maxTh = SIMD_HALF_PI - step;
 			drawS = true;
 		}
-		if(minTh > maxTh)
+		if (minTh > maxTh)
 		{
 			minTh = -SIMD_HALF_PI + step;
-			maxTh =  SIMD_HALF_PI - step;
+			maxTh = SIMD_HALF_PI - step;
 			drawN = drawS = true;
 		}
 		int n_hor = (int)((maxTh - minTh) / step) + 1;
-		if(n_hor < 2) n_hor = 2;
+		if (n_hor < 2) n_hor = 2;
 		btScalar step_h = (maxTh - minTh) / btScalar(n_hor - 1);
 		bool isClosed = false;
-		if(minPs > maxPs)
+		if (minPs > maxPs)
 		{
 			minPs = -SIMD_PI + step;
-			maxPs =  SIMD_PI;
+			maxPs = SIMD_PI;
 			isClosed = true;
 		}
-		else if((maxPs - minPs) >= SIMD_PI * btScalar(2.f))
+		else if ((maxPs - minPs) >= SIMD_PI * btScalar(2.f))
 		{
 			isClosed = true;
 		}
@@ -246,63 +243,64 @@ class	btIDebugDraw
 			isClosed = false;
 		}
 		int n_vert = (int)((maxPs - minPs) / step) + 1;
-		if(n_vert < 2) n_vert = 2;
+		if (n_vert < 2) n_vert = 2;
 		btScalar step_v = (maxPs - minPs) / btScalar(n_vert - 1);
-		for(int i = 0; i < n_hor; i++)
+		for (int i = 0; i < n_hor; i++)
 		{
 			btScalar th = minTh + btScalar(i) * step_h;
 			btScalar sth = radius * btSin(th);
 			btScalar cth = radius * btCos(th);
-			for(int j = 0; j < n_vert; j++)
+			for (int j = 0; j < n_vert; j++)
 			{
 				btScalar psi = minPs + btScalar(j) * step_v;
 				btScalar sps = btSin(psi);
 				btScalar cps = btCos(psi);
 				pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv;
-				if(i)
+				if (i)
 				{
 					drawLine(pvA[j], pvB[j], color);
 				}
-				else if(drawS)
+				else if (drawS)
 				{
 					drawLine(spole, pvB[j], color);
 				}
-				if(j)
+				if (j)
 				{
-					drawLine(pvB[j-1], pvB[j], color);
+					drawLine(pvB[j - 1], pvB[j], color);
 				}
 				else
 				{
 					arcStart = pvB[j];
 				}
-				if((i == (n_hor - 1)) && drawN)
+				if ((i == (n_hor - 1)) && drawN)
 				{
 					drawLine(npole, pvB[j], color);
 				}
-				
+
 				if (drawCenter)
 				{
-					if(isClosed)
+					if (isClosed)
 					{
-						if(j == (n_vert-1))
+						if (j == (n_vert - 1))
 						{
 							drawLine(arcStart, pvB[j], color);
 						}
 					}
 					else
 					{
-						if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1))))
+						if (((!i) || (i == (n_hor - 1))) && ((!j) || (j == (n_vert - 1))))
 						{
 							drawLine(center, pvB[j], color);
 						}
 					}
 				}
 			}
-			pT = pvA; pvA = pvB; pvB = pT;
+			pT = pvA;
+			pvA = pvB;
+			pvB = pT;
 		}
 	}
-	
-  
+
 	virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color)
 	{
 		drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
@@ -338,31 +336,27 @@ class	btIDebugDraw
 	{
 		int stepDegrees = 30;
 
-		btVector3 capStart(0.f,0.f,0.f);
+		btVector3 capStart(0.f, 0.f, 0.f);
 		capStart[upAxis] = -halfHeight;
 
-		btVector3 capEnd(0.f,0.f,0.f);
+		btVector3 capEnd(0.f, 0.f, 0.f);
 		capEnd[upAxis] = halfHeight;
 
 		// Draw the ends
 		{
-
 			btTransform childTransform = transform;
 			childTransform.getOrigin() = transform * capStart;
 			{
 				btVector3 center = childTransform.getOrigin();
-				btVector3 up = childTransform.getBasis().getColumn((upAxis+1)%3);
+				btVector3 up = childTransform.getBasis().getColumn((upAxis + 1) % 3);
 				btVector3 axis = -childTransform.getBasis().getColumn(upAxis);
 				btScalar minTh = -SIMD_HALF_PI;
 				btScalar maxTh = SIMD_HALF_PI;
 				btScalar minPs = -SIMD_HALF_PI;
 				btScalar maxPs = SIMD_HALF_PI;
-				
-				drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees) ,false);
-			}
-
-
 
+				drawSpherePatch(center, up, axis, radius, minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees), false);
+			}
 		}
 
 		{
@@ -370,52 +364,51 @@ class	btIDebugDraw
 			childTransform.getOrigin() = transform * capEnd;
 			{
 				btVector3 center = childTransform.getOrigin();
-				btVector3 up = childTransform.getBasis().getColumn((upAxis+1)%3);
+				btVector3 up = childTransform.getBasis().getColumn((upAxis + 1) % 3);
 				btVector3 axis = childTransform.getBasis().getColumn(upAxis);
 				btScalar minTh = -SIMD_HALF_PI;
 				btScalar maxTh = SIMD_HALF_PI;
 				btScalar minPs = -SIMD_HALF_PI;
 				btScalar maxPs = SIMD_HALF_PI;
-				drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees) ,false);
+				drawSpherePatch(center, up, axis, radius, minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees), false);
 			}
 		}
 
 		// Draw some additional lines
 		btVector3 start = transform.getOrigin();
 
-		for (int i=0;i<360;i+=stepDegrees)
+		for (int i = 0; i < 360; i += stepDegrees)
 		{
-			capEnd[(upAxis+1)%3] = capStart[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
-			capEnd[(upAxis+2)%3] = capStart[(upAxis+2)%3]  = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
-			drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
+			capEnd[(upAxis + 1) % 3] = capStart[(upAxis + 1) % 3] = btSin(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
+			capEnd[(upAxis + 2) % 3] = capStart[(upAxis + 2) % 3] = btCos(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
+			drawLine(start + transform.getBasis() * capStart, start + transform.getBasis() * capEnd, color);
 		}
-		
 	}
 
 	virtual void drawCylinder(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color)
 	{
 		btVector3 start = transform.getOrigin();
-		btVector3	offsetHeight(0,0,0);
+		btVector3 offsetHeight(0, 0, 0);
 		offsetHeight[upAxis] = halfHeight;
-		int stepDegrees=30;
-		btVector3 capStart(0.f,0.f,0.f);
+		int stepDegrees = 30;
+		btVector3 capStart(0.f, 0.f, 0.f);
 		capStart[upAxis] = -halfHeight;
-		btVector3 capEnd(0.f,0.f,0.f);
+		btVector3 capEnd(0.f, 0.f, 0.f);
 		capEnd[upAxis] = halfHeight;
 
-		for (int i=0;i<360;i+=stepDegrees)
+		for (int i = 0; i < 360; i += stepDegrees)
 		{
-			capEnd[(upAxis+1)%3] = capStart[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
-			capEnd[(upAxis+2)%3] = capStart[(upAxis+2)%3]  = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
-			drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
+			capEnd[(upAxis + 1) % 3] = capStart[(upAxis + 1) % 3] = btSin(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
+			capEnd[(upAxis + 2) % 3] = capStart[(upAxis + 2) % 3] = btCos(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
+			drawLine(start + transform.getBasis() * capStart, start + transform.getBasis() * capEnd, color);
 		}
 		// Drawing top and bottom caps of the cylinder
-		btVector3 yaxis(0,0,0);
+		btVector3 yaxis(0, 0, 0);
 		yaxis[upAxis] = btScalar(1.0);
-		btVector3 xaxis(0,0,0);
-		xaxis[(upAxis+1)%3] = btScalar(1.0);
-		drawArc(start-transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
-		drawArc(start+transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
+		btVector3 xaxis(0, 0, 0);
+		xaxis[(upAxis + 1) % 3] = btScalar(1.0);
+		drawArc(start - transform.getBasis() * (offsetHeight), transform.getBasis() * yaxis, transform.getBasis() * xaxis, radius, radius, 0, SIMD_2_PI, color, false, btScalar(10.0));
+		drawArc(start + transform.getBasis() * (offsetHeight), transform.getBasis() * yaxis, transform.getBasis() * xaxis, radius, radius, 0, SIMD_2_PI, color, false, btScalar(10.0));
 	}
 
 	virtual void drawCone(btScalar radius, btScalar height, int upAxis, const btTransform& transform, const btVector3& color)
@@ -423,50 +416,53 @@ class	btIDebugDraw
 		int stepDegrees = 30;
 		btVector3 start = transform.getOrigin();
 
-		btVector3	offsetHeight(0,0,0);
+		btVector3 offsetHeight(0, 0, 0);
 		btScalar halfHeight = height * btScalar(0.5);
 		offsetHeight[upAxis] = halfHeight;
-		btVector3	offsetRadius(0,0,0);
-		offsetRadius[(upAxis+1)%3] = radius;
-		btVector3	offset2Radius(0,0,0);
-		offset2Radius[(upAxis+2)%3] = radius;
-
+		btVector3 offsetRadius(0, 0, 0);
+		offsetRadius[(upAxis + 1) % 3] = radius;
+		btVector3 offset2Radius(0, 0, 0);
+		offset2Radius[(upAxis + 2) % 3] = radius;
 
-		btVector3 capEnd(0.f,0.f,0.f);
+		btVector3 capEnd(0.f, 0.f, 0.f);
 		capEnd[upAxis] = -halfHeight;
 
-		for (int i=0;i<360;i+=stepDegrees)
+		for (int i = 0; i < 360; i += stepDegrees)
 		{
-			capEnd[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
-			capEnd[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
-			drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * capEnd, color);
+			capEnd[(upAxis + 1) % 3] = btSin(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
+			capEnd[(upAxis + 2) % 3] = btCos(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
+			drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * capEnd, color);
 		}
 
-		drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offsetRadius),color);
-		drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offsetRadius),color);
-		drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offset2Radius),color);
-		drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offset2Radius),color);
+		drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight + offsetRadius), color);
+		drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight - offsetRadius), color);
+		drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight + offset2Radius), color);
+		drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight - offset2Radius), color);
 
 		// Drawing the base of the cone
-		btVector3 yaxis(0,0,0);
+		btVector3 yaxis(0, 0, 0);
 		yaxis[upAxis] = btScalar(1.0);
-		btVector3 xaxis(0,0,0);
-		xaxis[(upAxis+1)%3] = btScalar(1.0);
-		drawArc(start-transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,10.0);
+		btVector3 xaxis(0, 0, 0);
+		xaxis[(upAxis + 1) % 3] = btScalar(1.0);
+		drawArc(start - transform.getBasis() * (offsetHeight), transform.getBasis() * yaxis, transform.getBasis() * xaxis, radius, radius, 0, SIMD_2_PI, color, false, 10.0);
 	}
 
 	virtual void drawPlane(const btVector3& planeNormal, btScalar planeConst, const btTransform& transform, const btVector3& color)
 	{
 		btVector3 planeOrigin = planeNormal * planeConst;
-		btVector3 vec0,vec1;
-		btPlaneSpace1(planeNormal,vec0,vec1);
+		btVector3 vec0, vec1;
+		btPlaneSpace1(planeNormal, vec0, vec1);
 		btScalar vecLen = 100.f;
-		btVector3 pt0 = planeOrigin + vec0*vecLen;
-		btVector3 pt1 = planeOrigin - vec0*vecLen;
-		btVector3 pt2 = planeOrigin + vec1*vecLen;
-		btVector3 pt3 = planeOrigin - vec1*vecLen;
-		drawLine(transform*pt0,transform*pt1,color);
-		drawLine(transform*pt2,transform*pt3,color);
+		btVector3 pt0 = planeOrigin + vec0 * vecLen;
+		btVector3 pt1 = planeOrigin - vec0 * vecLen;
+		btVector3 pt2 = planeOrigin + vec1 * vecLen;
+		btVector3 pt3 = planeOrigin - vec1 * vecLen;
+		drawLine(transform * pt0, transform * pt1, color);
+		drawLine(transform * pt2, transform * pt3, color);
+	}
+
+	virtual void clearLines()
+	{
 	}
 
 	virtual void flushLines()
@@ -474,6 +470,4 @@ class	btIDebugDraw
 	}
 };
 
-
-#endif //BT_IDEBUG_DRAW__H
-
+#endif  //BT_IDEBUG_DRAW__H
diff --git a/extern/bullet2/src/LinearMath/btImplicitQRSVD.h b/extern/bullet2/src/LinearMath/btImplicitQRSVD.h
new file mode 100644
index 00000000000..aaedc964f61
--- /dev/null
+++ b/extern/bullet2/src/LinearMath/btImplicitQRSVD.h
@@ -0,0 +1,916 @@
+/**
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ 
+ Copyright (c) 2016 Theodore Gast, Chuyuan Fu, Chenfanfu Jiang, Joseph Teran
+ 
+ Permission is hereby granted, free of charge, to any person obtaining a copy of
+ this software and associated documentation files (the "Software"), to deal in
+ the Software without restriction, including without limitation the rights to
+ use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
+ of the Software, and to permit persons to whom the Software is furnished to do
+ so, subject to the following conditions:
+ 
+ The above copyright notice and this permission notice shall be included in all
+ copies or substantial portions of the Software.
+ 
+ If the code is used in an article, the following paper shall be cited:
+ @techreport{qrsvd:2016,
+ title={Implicit-shifted Symmetric QR Singular Value Decomposition of 3x3 Matrices},
+ author={Gast, Theodore and Fu, Chuyuan and Jiang, Chenfanfu and Teran, Joseph},
+ year={2016},
+ institution={University of California Los Angeles}
+ }
+ 
+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ SOFTWARE.
+**/
+
+#ifndef btImplicitQRSVD_h
+#define btImplicitQRSVD_h
+#include <limits>
+#include "btMatrix3x3.h"
+class btMatrix2x2
+{
+public:
+    btScalar m_00, m_01, m_10, m_11;
+    btMatrix2x2(): m_00(0), m_10(0), m_01(0), m_11(0)
+    {
+    }
+    btMatrix2x2(const btMatrix2x2& other): m_00(other.m_00),m_01(other.m_01),m_10(other.m_10),m_11(other.m_11)
+    {}
+    btScalar& operator()(int i, int j)
+    {
+        if (i == 0 && j == 0)
+            return m_00;
+        if (i == 1 && j == 0)
+            return m_10;
+        if (i == 0 && j == 1)
+            return m_01;
+        if (i == 1 && j == 1)
+            return m_11;
+        btAssert(false);
+        return m_00;
+    }
+    const btScalar& operator()(int i, int j) const
+    {
+        if (i == 0 && j == 0)
+            return m_00;
+        if (i == 1 && j == 0)
+            return m_10;
+        if (i == 0 && j == 1)
+            return m_01;
+        if (i == 1 && j == 1)
+            return m_11;
+        btAssert(false);
+        return m_00;
+    }
+    void setIdentity()
+    {
+        m_00 = 1;
+        m_11 = 1;
+        m_01 = 0;
+        m_10 = 0;
+    }
+};
+
+static inline btScalar copySign(btScalar x, btScalar y) {
+    if ((x < 0 && y > 0) || (x > 0 && y < 0))
+        return -x;
+    return x;
+}
+
+/**
+ Class for givens rotation.
+ Row rotation G*A corresponds to something like
+ c -s  0
+ ( s  c  0 ) A
+ 0  0  1
+ Column rotation A G' corresponds to something like
+ c -s  0
+ A ( s  c  0 )
+ 0  0  1
+ 
+ c and s are always computed so that
+ ( c -s ) ( a )  =  ( * )
+ s  c     b       ( 0 )
+ 
+ Assume rowi<rowk.
+ */
+
+class GivensRotation {
+public:
+    int rowi;
+    int rowk;
+    btScalar c;
+    btScalar s;
+    
+    inline GivensRotation(int rowi_in, int rowk_in)
+    : rowi(rowi_in)
+    , rowk(rowk_in)
+    , c(1)
+    , s(0)
+    {
+    }
+    
+    inline GivensRotation(btScalar a, btScalar b, int rowi_in, int rowk_in)
+    : rowi(rowi_in)
+    , rowk(rowk_in)
+    {
+        compute(a, b);
+    }
+    
+    ~GivensRotation() {}
+    
+    inline void transposeInPlace()
+    {
+        s = -s;
+    }
+    
+    /**
+     Compute c and s from a and b so that
+     ( c -s ) ( a )  =  ( * )
+     s  c     b       ( 0 )
+     */
+    inline void compute(const btScalar a, const btScalar b)
+    {
+        btScalar d = a * a + b * b;
+        c = 1;
+        s = 0;
+        if (d > SIMD_EPSILON) {
+            btScalar sqrtd = btSqrt(d);
+            if (sqrtd>SIMD_EPSILON)
+            {
+              btScalar t = btScalar(1.0)/sqrtd;
+              c = a * t;
+              s = -b * t;
+            }
+        }
+    }
+    
+    /**
+     This function computes c and s so that
+     ( c -s ) ( a )  =  ( 0 )
+     s  c     b       ( * )
+     */
+    inline void computeUnconventional(const btScalar a, const btScalar b)
+    {
+        btScalar d = a * a + b * b;
+        c = 0;
+        s = 1;
+        if (d > SIMD_EPSILON) {
+            btScalar t = btScalar(1.0)/btSqrt(d);
+            s = a * t;
+            c = b * t;
+        }
+    }
+    /**
+     Fill the R with the entries of this rotation
+     */
+    inline void fill(const btMatrix3x3& R) const
+    {
+        btMatrix3x3& A = const_cast<btMatrix3x3&>(R);
+        A.setIdentity();
+        A[rowi][rowi] = c;
+        A[rowk][rowi] = -s;
+        A[rowi][rowk] = s;
+        A[rowk][rowk] = c;
+    }
+    
+    inline void fill(const btMatrix2x2& R) const
+    {
+        btMatrix2x2& A = const_cast<btMatrix2x2&>(R);
+        A(rowi,rowi) = c;
+        A(rowk,rowi) = -s;
+        A(rowi,rowk) = s;
+        A(rowk,rowk) = c;
+    }
+    
+    /**
+     This function does something like
+     c -s  0
+     ( s  c  0 ) A -> A
+     0  0  1
+     It only affects row i and row k of A.
+     */
+    inline void rowRotation(btMatrix3x3& A) const
+    {
+        for (int j = 0; j < 3; j++) {
+            btScalar tau1 = A[rowi][j];
+            btScalar tau2 = A[rowk][j];
+            A[rowi][j] = c * tau1 - s * tau2;
+            A[rowk][j] = s * tau1 + c * tau2;
+        }
+    }
+    inline void rowRotation(btMatrix2x2& A) const
+    {
+        for (int j = 0; j < 2; j++) {
+            btScalar tau1 = A(rowi,j);
+            btScalar tau2 = A(rowk,j);
+            A(rowi,j) = c * tau1 - s * tau2;
+            A(rowk,j) = s * tau1 + c * tau2;
+        }
+    }
+    
+    /**
+     This function does something like
+     c  s  0
+     A ( -s  c  0 )  -> A
+     0  0  1
+     It only affects column i and column k of A.
+     */
+    inline void columnRotation(btMatrix3x3& A) const
+    {
+        for (int j = 0; j < 3; j++) {
+            btScalar tau1 = A[j][rowi];
+            btScalar tau2 = A[j][rowk];
+            A[j][rowi] = c * tau1 - s * tau2;
+            A[j][rowk] = s * tau1 + c * tau2;
+        }
+    }
+    inline void columnRotation(btMatrix2x2& A) const
+    {
+        for (int j = 0; j < 2; j++) {
+            btScalar tau1 = A(j,rowi);
+            btScalar tau2 = A(j,rowk);
+            A(j,rowi) = c * tau1 - s * tau2;
+            A(j,rowk) = s * tau1 + c * tau2;
+        }
+    }
+    
+    /**
+     Multiply givens must be for same row and column
+     **/
+    inline void operator*=(const GivensRotation& A)
+    {
+        btScalar new_c = c * A.c - s * A.s;
+        btScalar new_s = s * A.c + c * A.s;
+        c = new_c;
+        s = new_s;
+    }
+    
+    /**
+     Multiply givens must be for same row and column
+     **/
+    inline GivensRotation operator*(const GivensRotation& A) const
+    {
+        GivensRotation r(*this);
+        r *= A;
+        return r;
+    }
+};
+
+/**
+ \brief zero chasing the 3X3 matrix to bidiagonal form
+ original form of H:   x x 0
+ x x x
+ 0 0 x
+ after zero chase:
+ x x 0
+ 0 x x
+ 0 0 x
+ */
+inline void zeroChase(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
+{
+    
+    /**
+     Reduce H to of form
+     x x +
+     0 x x
+     0 0 x
+     */
+    GivensRotation r1(H[0][0], H[1][0], 0, 1);
+    /**
+     Reduce H to of form
+     x x 0
+     0 x x
+     0 + x
+     Can calculate r2 without multiplying by r1 since both entries are in first two
+     rows thus no need to divide by sqrt(a^2+b^2)
+     */
+    GivensRotation r2(1, 2);
+    if (H[1][0] != 0)
+        r2.compute(H[0][0] * H[0][1] + H[1][0] * H[1][1], H[0][0] * H[0][2] + H[1][0] * H[1][2]);
+    else
+        r2.compute(H[0][1], H[0][2]);
+    
+    r1.rowRotation(H);
+    
+    /* GivensRotation<T> r2(H(0, 1), H(0, 2), 1, 2); */
+    r2.columnRotation(H);
+    r2.columnRotation(V);
+    
+    /**
+     Reduce H to of form
+     x x 0
+     0 x x
+     0 0 x
+     */
+    GivensRotation r3(H[1][1], H[2][1], 1, 2);
+    r3.rowRotation(H);
+    
+    // Save this till end for better cache coherency
+    // r1.rowRotation(u_transpose);
+    // r3.rowRotation(u_transpose);
+    r1.columnRotation(U);
+    r3.columnRotation(U);
+}
+
+/**
+ \brief make a 3X3 matrix to upper bidiagonal form
+ original form of H:   x x x
+ x x x
+ x x x
+ after zero chase:
+ x x 0
+ 0 x x
+ 0 0 x
+ */
+inline void makeUpperBidiag(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
+{
+    U.setIdentity();
+    V.setIdentity();
+    
+    /**
+     Reduce H to of form
+     x x x
+     x x x
+     0 x x
+     */
+    
+    GivensRotation r(H[1][0], H[2][0], 1, 2);
+    r.rowRotation(H);
+    // r.rowRotation(u_transpose);
+    r.columnRotation(U);
+    // zeroChase(H, u_transpose, V);
+    zeroChase(H, U, V);
+}
+
+/**
+ \brief make a 3X3 matrix to lambda shape
+ original form of H:   x x x
+ *                     x x x
+ *                     x x x
+ after :
+ *                     x 0 0
+ *                     x x 0
+ *                     x 0 x
+ */
+inline void makeLambdaShape(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
+{
+    U.setIdentity();
+    V.setIdentity();
+    
+    /**
+     Reduce H to of form
+     *                    x x 0
+     *                    x x x
+     *                    x x x
+     */
+    
+    GivensRotation r1(H[0][1], H[0][2], 1, 2);
+    r1.columnRotation(H);
+    r1.columnRotation(V);
+    
+    /**
+     Reduce H to of form
+     *                    x x 0
+     *                    x x 0
+     *                    x x x
+     */
+    
+    r1.computeUnconventional(H[1][2], H[2][2]);
+    r1.rowRotation(H);
+    r1.columnRotation(U);
+    
+    /**
+     Reduce H to of form
+     *                    x x 0
+     *                    x x 0
+     *                    x 0 x
+     */
+    
+    GivensRotation r2(H[2][0], H[2][1], 0, 1);
+    r2.columnRotation(H);
+    r2.columnRotation(V);
+    
+    /**
+     Reduce H to of form
+     *                    x 0 0
+     *                    x x 0
+     *                    x 0 x
+     */
+    r2.computeUnconventional(H[0][1], H[1][1]);
+    r2.rowRotation(H);
+    r2.columnRotation(U);
+}
+
+/**
+ \brief 2x2 polar decomposition.
+ \param[in] A matrix.
+ \param[out] R Robustly a rotation matrix.
+ \param[out] S_Sym Symmetric. Whole matrix is stored
+ 
+ Polar guarantees negative sign is on the small magnitude singular value.
+ S is guaranteed to be the closest one to identity.
+ R is guaranteed to be the closest rotation to A.
+ */
+inline void polarDecomposition(const btMatrix2x2& A,
+                   GivensRotation& R,
+                   const btMatrix2x2& S_Sym)
+{
+    btScalar a = (A(0, 0) + A(1, 1)),  b = (A(1, 0) - A(0, 1));
+    btScalar denominator = btSqrt(a*a+b*b);
+    R.c = (btScalar)1;
+    R.s = (btScalar)0;
+    if (denominator > SIMD_EPSILON) { 
+        /*
+         No need to use a tolerance here because x(0) and x(1) always have
+         smaller magnitude then denominator, therefore overflow never happens.
+         In Bullet, we use a tolerance anyway.
+         */
+        R.c = a / denominator;
+        R.s = -b / denominator;
+    }
+    btMatrix2x2& S = const_cast<btMatrix2x2&>(S_Sym);
+    S = A;
+    R.rowRotation(S);
+}
+
+inline void polarDecomposition(const btMatrix2x2& A,
+                   const btMatrix2x2& R,
+                   const btMatrix2x2& S_Sym)
+{
+    GivensRotation r(0, 1);
+    polarDecomposition(A, r, S_Sym);
+    r.fill(R);
+}
+
+/**
+ \brief 2x2 SVD (singular value decomposition) A=USV'
+ \param[in] A Input matrix.
+ \param[out] U Robustly a rotation matrix in Givens form
+ \param[out] Sigma matrix of singular values sorted with decreasing magnitude. The second one can be negative.
+ \param[out] V Robustly a rotation matrix in Givens form
+ */
+inline void singularValueDecomposition(
+                           const btMatrix2x2& A,
+                           GivensRotation& U,
+                           const btMatrix2x2& Sigma,
+                           GivensRotation& V,
+                           const btScalar tol = 64 * std::numeric_limits<btScalar>::epsilon())
+{
+    btMatrix2x2& sigma = const_cast<btMatrix2x2&>(Sigma);
+    sigma.setIdentity();
+    btMatrix2x2 S_Sym;
+    polarDecomposition(A, U, S_Sym);
+    btScalar cosine, sine;
+    btScalar x = S_Sym(0, 0);
+    btScalar y = S_Sym(0, 1);
+    btScalar z = S_Sym(1, 1);
+    if (y == 0) {
+        // S is already diagonal
+        cosine = 1;
+        sine = 0;
+        sigma(0,0) = x;
+        sigma(1,1) = z;
+    }
+    else {
+        btScalar tau = 0.5 * (x - z);
+        btScalar val = tau * tau + y * y;
+        if (val > SIMD_EPSILON)
+        {
+        btScalar w = btSqrt(val);
+        // w > y > 0
+        btScalar t;
+        if (tau > 0) {
+            // tau + w > w > y > 0 ==> division is safe
+            t = y / (tau + w);
+        }
+        else {
+            // tau - w < -w < -y < 0 ==> division is safe
+            t = y / (tau - w);
+        }
+        cosine = btScalar(1) / btSqrt(t * t + btScalar(1));
+        sine = -t * cosine;
+        /*
+         V = [cosine -sine; sine cosine]
+         Sigma = V'SV. Only compute the diagonals for efficiency.
+         Also utilize symmetry of S and don't form V yet.
+         */
+        btScalar c2 = cosine * cosine;
+        btScalar csy = 2 * cosine * sine * y;
+        btScalar s2 = sine * sine;
+        sigma(0,0) = c2 * x - csy + s2 * z;
+        sigma(1,1) = s2 * x + csy + c2 * z;
+      } else
+      	{
+      		cosine = 1;
+        sine = 0;
+        sigma(0,0) = x;
+        sigma(1,1) = z;
+      	}
+    }
+    
+    // Sorting
+    // Polar already guarantees negative sign is on the small magnitude singular value.
+    if (sigma(0,0) < sigma(1,1)) {
+        std::swap(sigma(0,0), sigma(1,1));
+        V.c = -sine;
+        V.s = cosine;
+    }
+    else {
+        V.c = cosine;
+        V.s = sine;
+    }
+    U *= V;
+}
+
+/**
+ \brief 2x2 SVD (singular value decomposition) A=USV'
+ \param[in] A Input matrix.
+ \param[out] U Robustly a rotation matrix.
+ \param[out] Sigma Vector of singular values sorted with decreasing magnitude. The second one can be negative.
+ \param[out] V Robustly a rotation matrix.
+ */
+inline void singularValueDecomposition(
+                           const btMatrix2x2& A,
+                           const btMatrix2x2& U,
+                           const btMatrix2x2& Sigma,
+                           const btMatrix2x2& V,
+                           const btScalar tol = 64 * std::numeric_limits<btScalar>::epsilon())
+{
+    GivensRotation gv(0, 1);
+    GivensRotation gu(0, 1);
+    singularValueDecomposition(A, gu, Sigma, gv);
+    
+    gu.fill(U);
+    gv.fill(V);
+}
+
+/**
+ \brief compute wilkinsonShift of the block
+ a1     b1
+ b1     a2
+ based on the wilkinsonShift formula
+ mu = c + d - sign (d) \ sqrt (d*d + b*b), where d = (a-c)/2
+ 
+ */
+inline btScalar wilkinsonShift(const btScalar a1, const btScalar b1, const btScalar a2)
+{
+	btScalar d = (btScalar)0.5 * (a1 - a2);
+	btScalar bs = b1 * b1;
+	btScalar val = d * d + bs;
+	if (val>SIMD_EPSILON)
+	{
+		btScalar denom = btFabs(d) + btSqrt(val);
+
+		btScalar mu = a2 - copySign(bs / (denom), d);
+		// T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs));
+		return mu;
+	}
+	return a2;
+}
+
+/**
+ \brief Helper function of 3X3 SVD for processing 2X2 SVD
+ */
+template <int t>
+inline void process(btMatrix3x3& B, btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V)
+{
+    int other = (t == 1) ? 0 : 2;
+    GivensRotation u(0, 1);
+    GivensRotation v(0, 1);
+    sigma[other] = B[other][other];
+    
+    btMatrix2x2 B_sub, sigma_sub;
+    if (t == 0)
+    {
+        B_sub.m_00 = B[0][0];
+        B_sub.m_10 = B[1][0];
+        B_sub.m_01 = B[0][1];
+        B_sub.m_11 = B[1][1];
+        sigma_sub.m_00 = sigma[0];
+        sigma_sub.m_11 = sigma[1];
+//        singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
+        singularValueDecomposition(B_sub, u, sigma_sub, v);
+        B[0][0] = B_sub.m_00;
+        B[1][0] = B_sub.m_10;
+        B[0][1] = B_sub.m_01;
+        B[1][1] = B_sub.m_11;
+        sigma[0] = sigma_sub.m_00;
+        sigma[1] = sigma_sub.m_11;
+    }
+    else
+    {
+        B_sub.m_00 = B[1][1];
+        B_sub.m_10 = B[2][1];
+        B_sub.m_01 = B[1][2];
+        B_sub.m_11 = B[2][2];
+        sigma_sub.m_00 = sigma[1];
+        sigma_sub.m_11 = sigma[2];
+        //        singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
+        singularValueDecomposition(B_sub, u, sigma_sub, v);
+        B[1][1] = B_sub.m_00;
+        B[2][1] = B_sub.m_10;
+        B[1][2] = B_sub.m_01;
+        B[2][2] = B_sub.m_11;
+        sigma[1] = sigma_sub.m_00;
+        sigma[2] = sigma_sub.m_11;
+    }
+    u.rowi += t;
+    u.rowk += t;
+    v.rowi += t;
+    v.rowk += t;
+    u.columnRotation(U);
+    v.columnRotation(V);
+}
+
+/**
+ \brief Helper function of 3X3 SVD for flipping signs due to flipping signs of sigma
+ */
+inline void flipSign(int i, btMatrix3x3& U, btVector3& sigma)
+{
+    sigma[i] = -sigma[i];
+    U[0][i] = -U[0][i];
+    U[1][i] = -U[1][i];
+    U[2][i] = -U[2][i];
+}
+
+inline void flipSign(int i, btMatrix3x3& U)
+{
+    U[0][i] = -U[0][i];
+    U[1][i] = -U[1][i];
+    U[2][i] = -U[2][i];
+}
+
+inline void swapCol(btMatrix3x3& A, int i, int j)
+{
+    for (int d = 0; d < 3; ++d)
+        std::swap(A[d][i], A[d][j]);
+}
+/**
+ \brief Helper function of 3X3 SVD for sorting singular values
+ */
+inline void sort(btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V, int t)
+{
+    if (t == 0)
+    {
+        // Case: sigma(0) > |sigma(1)| >= |sigma(2)|
+        if (btFabs(sigma[1]) >= btFabs(sigma[2])) {
+            if (sigma[1] < 0) {
+                flipSign(1, U, sigma);
+                flipSign(2, U, sigma);
+            }
+            return;
+        }
+        
+        //fix sign of sigma for both cases
+        if (sigma[2] < 0) {
+            flipSign(1, U, sigma);
+            flipSign(2, U, sigma);
+        }
+        
+        //swap sigma(1) and sigma(2) for both cases
+        std::swap(sigma[1], sigma[2]);
+        // swap the col 1 and col 2 for U,V
+        swapCol(U,1,2);
+        swapCol(V,1,2);
+        
+        // Case: |sigma(2)| >= sigma(0) > |simga(1)|
+        if (sigma[1] > sigma[0]) {
+            std::swap(sigma[0], sigma[1]);
+            swapCol(U,0,1);
+            swapCol(V,0,1);
+        }
+        
+        // Case: sigma(0) >= |sigma(2)| > |simga(1)|
+        else {
+            flipSign(2, U);
+            flipSign(2, V);
+        }
+    }
+    else if (t == 1)
+    {
+        // Case: |sigma(0)| >= sigma(1) > |sigma(2)|
+        if (btFabs(sigma[0]) >= sigma[1]) {
+            if (sigma[0] < 0) {
+                flipSign(0, U, sigma);
+                flipSign(2, U, sigma);
+            }
+            return;
+        }
+        
+        //swap sigma(0) and sigma(1) for both cases
+        std::swap(sigma[0], sigma[1]);
+        swapCol(U, 0, 1);
+        swapCol(V, 0, 1);
+        
+        // Case: sigma(1) > |sigma(2)| >= |sigma(0)|
+        if (btFabs(sigma[1]) < btFabs(sigma[2])) {
+            std::swap(sigma[1], sigma[2]);
+            swapCol(U, 1, 2);
+            swapCol(V, 1, 2);
+        }
+        
+        // Case: sigma(1) >= |sigma(0)| > |sigma(2)|
+        else {
+            flipSign(1, U);
+            flipSign(1, V);
+        }
+        
+        // fix sign for both cases
+        if (sigma[1] < 0) {
+            flipSign(1, U, sigma);
+            flipSign(2, U, sigma);
+        }
+    }
+}
+
+/**
+ \brief 3X3 SVD (singular value decomposition) A=USV'
+ \param[in] A Input matrix.
+ \param[out] U is a rotation matrix.
+ \param[out] sigma Diagonal matrix, sorted with decreasing magnitude. The third one can be negative.
+ \param[out] V is a rotation matrix.
+ */
+inline int singularValueDecomposition(const btMatrix3x3& A,
+                                     btMatrix3x3& U,
+                                     btVector3& sigma,
+                                     btMatrix3x3& V,
+                                     btScalar tol = 128*std::numeric_limits<btScalar>::epsilon())
+{
+//    using std::fabs;
+    btMatrix3x3 B = A;
+    U.setIdentity();
+    V.setIdentity();
+    
+    makeUpperBidiag(B, U, V);
+    
+    int count = 0;
+    btScalar mu = (btScalar)0;
+    GivensRotation r(0, 1);
+    
+    btScalar alpha_1 = B[0][0];
+    btScalar beta_1 = B[0][1];
+    btScalar alpha_2 = B[1][1];
+    btScalar alpha_3 = B[2][2];
+    btScalar beta_2 = B[1][2];
+    btScalar gamma_1 = alpha_1 * beta_1;
+    btScalar gamma_2 = alpha_2 * beta_2;
+    btScalar val = alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2;
+    if (val > SIMD_EPSILON)
+    {
+	    tol *= btMax((btScalar)0.5 * btSqrt(val), (btScalar)1);
+		}    
+    /**
+     Do implicit shift QR until A^T A is block diagonal
+     */
+    int max_count = 100;
+    
+    while (btFabs(beta_2) > tol && btFabs(beta_1) > tol
+           && btFabs(alpha_1) > tol && btFabs(alpha_2) > tol
+           && btFabs(alpha_3) > tol
+           && count < max_count) {
+        mu = wilkinsonShift(alpha_2 * alpha_2 + beta_1 * beta_1, gamma_2, alpha_3 * alpha_3 + beta_2 * beta_2);
+        
+        r.compute(alpha_1 * alpha_1 - mu, gamma_1);
+        r.columnRotation(B);
+        
+        r.columnRotation(V);
+        zeroChase(B, U, V);
+        
+        alpha_1 = B[0][0];
+        beta_1 = B[0][1];
+        alpha_2 = B[1][1];
+        alpha_3 = B[2][2];
+        beta_2 = B[1][2];
+        gamma_1 = alpha_1 * beta_1;
+        gamma_2 = alpha_2 * beta_2;
+        count++;
+    }
+    /**
+     Handle the cases of one of the alphas and betas being 0
+     Sorted by ease of handling and then frequency
+     of occurrence
+     
+     If B is of form
+     x x 0
+     0 x 0
+     0 0 x
+     */
+    if (btFabs(beta_2) <= tol) {
+        process<0>(B, U, sigma, V);
+        sort(U, sigma, V,0);
+    }
+    /**
+     If B is of form
+     x 0 0
+     0 x x
+     0 0 x
+     */
+    else if (btFabs(beta_1) <= tol) {
+        process<1>(B, U, sigma, V);
+        sort(U, sigma, V,1);
+    }
+    /**
+     If B is of form
+     x x 0
+     0 0 x
+     0 0 x
+     */
+    else if (btFabs(alpha_2) <= tol) {
+        /**
+         Reduce B to
+         x x 0
+         0 0 0
+         0 0 x
+         */
+        GivensRotation r1(1, 2);
+        r1.computeUnconventional(B[1][2], B[2][2]);
+        r1.rowRotation(B);
+        r1.columnRotation(U);
+        
+        process<0>(B, U, sigma, V);
+        sort(U, sigma, V, 0);
+    }
+    /**
+     If B is of form
+     x x 0
+     0 x x
+     0 0 0
+     */
+    else if (btFabs(alpha_3) <= tol) {
+        /**
+         Reduce B to
+         x x +
+         0 x 0
+         0 0 0
+         */
+        GivensRotation r1(1, 2);
+        r1.compute(B[1][1], B[1][2]);
+        r1.columnRotation(B);
+        r1.columnRotation(V);
+        /**
+         Reduce B to
+         x x 0
+         + x 0
+         0 0 0
+         */
+        GivensRotation r2(0, 2);
+        r2.compute(B[0][0], B[0][2]);
+        r2.columnRotation(B);
+        r2.columnRotation(V);
+        
+        process<0>(B, U, sigma, V);
+        sort(U, sigma, V, 0);
+    }
+    /**
+     If B is of form
+     0 x 0
+     0 x x
+     0 0 x
+     */
+    else if (btFabs(alpha_1) <= tol) {
+        /**
+         Reduce B to
+         0 0 +
+         0 x x
+         0 0 x
+         */
+        GivensRotation r1(0, 1);
+        r1.computeUnconventional(B[0][1], B[1][1]);
+        r1.rowRotation(B);
+        r1.columnRotation(U);
+        
+        /**
+         Reduce B to
+         0 0 0
+         0 x x
+         0 + x
+         */
+        GivensRotation r2(0, 2);
+        r2.computeUnconventional(B[0][2], B[2][2]);
+        r2.rowRotation(B);
+        r2.columnRotation(U);
+        
+        process<1>(B, U, sigma, V);
+        sort(U, sigma, V, 1);
+    }
+    
+    return count;
+}
+#endif /* btImplicitQRSVD_h */
diff --git a/extern/bullet2/src/LinearMath/btList.h b/extern/bullet2/src/LinearMath/btList.h
index eec80a7064b..b255938c30b 100644
--- a/extern/bullet2/src/LinearMath/btList.h
+++ b/extern/bullet2/src/LinearMath/btList.h
@@ -12,62 +12,62 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef BT_GEN_LIST_H
 #define BT_GEN_LIST_H
 
-class btGEN_Link {
+class btGEN_Link
+{
 public:
-    btGEN_Link() : m_next(0), m_prev(0) {}
-    btGEN_Link(btGEN_Link *next, btGEN_Link *prev) : m_next(next), m_prev(prev) {}
-    
-    btGEN_Link *getNext() const { return m_next; }  
-    btGEN_Link *getPrev() const { return m_prev; }  
-
-    bool isHead() const { return m_prev == 0; }
-    bool isTail() const { return m_next == 0; }
-
-    void insertBefore(btGEN_Link *link) {
-        m_next         = link;
-        m_prev         = link->m_prev;
-        m_next->m_prev = this;
-        m_prev->m_next = this;
-    } 
-
-    void insertAfter(btGEN_Link *link) {
-        m_next         = link->m_next;
-        m_prev         = link;
-        m_next->m_prev = this;
-        m_prev->m_next = this;
-    } 
-
-    void remove() { 
-        m_next->m_prev = m_prev; 
-        m_prev->m_next = m_next;
-    }
+	btGEN_Link() : m_next(0), m_prev(0) {}
+	btGEN_Link(btGEN_Link *next, btGEN_Link *prev) : m_next(next), m_prev(prev) {}
+
+	btGEN_Link *getNext() const { return m_next; }
+	btGEN_Link *getPrev() const { return m_prev; }
+
+	bool isHead() const { return m_prev == 0; }
+	bool isTail() const { return m_next == 0; }
+
+	void insertBefore(btGEN_Link *link)
+	{
+		m_next = link;
+		m_prev = link->m_prev;
+		m_next->m_prev = this;
+		m_prev->m_next = this;
+	}
+
+	void insertAfter(btGEN_Link *link)
+	{
+		m_next = link->m_next;
+		m_prev = link;
+		m_next->m_prev = this;
+		m_prev->m_next = this;
+	}
+
+	void remove()
+	{
+		m_next->m_prev = m_prev;
+		m_prev->m_next = m_next;
+	}
 
-private:  
-    btGEN_Link  *m_next;
-    btGEN_Link  *m_prev;
+private:
+	btGEN_Link *m_next;
+	btGEN_Link *m_prev;
 };
 
-class btGEN_List {
+class btGEN_List
+{
 public:
-    btGEN_List() : m_head(&m_tail, 0), m_tail(0, &m_head) {}
+	btGEN_List() : m_head(&m_tail, 0), m_tail(0, &m_head) {}
+
+	btGEN_Link *getHead() const { return m_head.getNext(); }
+	btGEN_Link *getTail() const { return m_tail.getPrev(); }
 
-    btGEN_Link *getHead() const { return m_head.getNext(); } 
-    btGEN_Link *getTail() const { return m_tail.getPrev(); } 
+	void addHead(btGEN_Link *link) { link->insertAfter(&m_head); }
+	void addTail(btGEN_Link *link) { link->insertBefore(&m_tail); }
 
-    void addHead(btGEN_Link *link) { link->insertAfter(&m_head); }
-    void addTail(btGEN_Link *link) { link->insertBefore(&m_tail); }
-    
 private:
-    btGEN_Link m_head;
-    btGEN_Link m_tail;
+	btGEN_Link m_head;
+	btGEN_Link m_tail;
 };
 
-#endif //BT_GEN_LIST_H
-
-
-
+#endif  //BT_GEN_LIST_H
diff --git a/extern/bullet2/src/LinearMath/btMatrix3x3.h b/extern/bullet2/src/LinearMath/btMatrix3x3.h
index 41dea694835..9c90fee1d2a 100644
--- a/extern/bullet2/src/LinearMath/btMatrix3x3.h
+++ b/extern/bullet2/src/LinearMath/btMatrix3x3.h
@@ -12,8 +12,7 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-#ifndef	BT_MATRIX3x3_H
+#ifndef BT_MATRIX3x3_H
 #define BT_MATRIX3x3_H
 
 #include "btVector3.h"
@@ -23,36 +22,38 @@ subject to the following restrictions:
 #ifdef BT_USE_SSE
 //const __m128 ATTRIBUTE_ALIGNED16(v2220) = {2.0f, 2.0f, 2.0f, 0.0f};
 //const __m128 ATTRIBUTE_ALIGNED16(vMPPP) = {-0.0f, +0.0f, +0.0f, +0.0f};
-#define vMPPP (_mm_set_ps (+0.0f, +0.0f, +0.0f, -0.0f))
+#define vMPPP (_mm_set_ps(+0.0f, +0.0f, +0.0f, -0.0f))
 #endif
 
 #if defined(BT_USE_SSE)
-#define v1000 (_mm_set_ps(0.0f,0.0f,0.0f,1.0f))
-#define v0100 (_mm_set_ps(0.0f,0.0f,1.0f,0.0f))
-#define v0010 (_mm_set_ps(0.0f,1.0f,0.0f,0.0f))
+#define v0000 (_mm_set_ps(0.0f, 0.0f, 0.0f, 0.0f))
+#define v1000 (_mm_set_ps(0.0f, 0.0f, 0.0f, 1.0f))
+#define v0100 (_mm_set_ps(0.0f, 0.0f, 1.0f, 0.0f))
+#define v0010 (_mm_set_ps(0.0f, 1.0f, 0.0f, 0.0f))
 #elif defined(BT_USE_NEON)
+const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0000) = {0.0f, 0.0f, 0.0f, 0.0f};
 const btSimdFloat4 ATTRIBUTE_ALIGNED16(v1000) = {1.0f, 0.0f, 0.0f, 0.0f};
 const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0100) = {0.0f, 1.0f, 0.0f, 0.0f};
 const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0010) = {0.0f, 0.0f, 1.0f, 0.0f};
 #endif
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btMatrix3x3Data	btMatrix3x3DoubleData 
+#define btMatrix3x3Data btMatrix3x3DoubleData
 #else
-#define btMatrix3x3Data	btMatrix3x3FloatData
-#endif //BT_USE_DOUBLE_PRECISION
-
+#define btMatrix3x3Data btMatrix3x3FloatData
+#endif  //BT_USE_DOUBLE_PRECISION
 
 /**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
 * Make sure to only include a pure orthogonal matrix without scaling. */
-ATTRIBUTE_ALIGNED16(class) btMatrix3x3 {
-
+ATTRIBUTE_ALIGNED16(class)
+btMatrix3x3
+{
 	///Data storage for the matrix, each vector is a row of the matrix
 	btVector3 m_el[3];
 
 public:
 	/** @brief No initializaion constructor */
-	btMatrix3x3 () {}
+	btMatrix3x3() {}
 
 	//		explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
 
@@ -67,27 +68,27 @@ public:
 	*/
 	/** @brief Constructor with row major formatting */
 	btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
-		const btScalar& yx, const btScalar& yy, const btScalar& yz,
-		const btScalar& zx, const btScalar& zy, const btScalar& zz)
-	{ 
-		setValue(xx, xy, xz, 
-			yx, yy, yz, 
-			zx, zy, zz);
+				const btScalar& yx, const btScalar& yy, const btScalar& yz,
+				const btScalar& zx, const btScalar& zy, const btScalar& zz)
+	{
+		setValue(xx, xy, xz,
+				 yx, yy, yz,
+				 zx, zy, zz);
 	}
 
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
-	SIMD_FORCE_INLINE btMatrix3x3 (const btSimdFloat4 v0, const btSimdFloat4 v1, const btSimdFloat4 v2 ) 
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
+	SIMD_FORCE_INLINE btMatrix3x3(const btSimdFloat4 v0, const btSimdFloat4 v1, const btSimdFloat4 v2)
 	{
-        m_el[0].mVec128 = v0;
-        m_el[1].mVec128 = v1;
-        m_el[2].mVec128 = v2;
+		m_el[0].mVec128 = v0;
+		m_el[1].mVec128 = v1;
+		m_el[2].mVec128 = v2;
 	}
 
-	SIMD_FORCE_INLINE btMatrix3x3 (const btVector3& v0, const btVector3& v1, const btVector3& v2 ) 
+	SIMD_FORCE_INLINE btMatrix3x3(const btVector3& v0, const btVector3& v1, const btVector3& v2)
 	{
-        m_el[0] = v0;
-        m_el[1] = v1;
-        m_el[2] = v2;
+		m_el[0] = v0;
+		m_el[1] = v1;
+		m_el[2] = v2;
 	}
 
 	// Copy constructor
@@ -99,25 +100,25 @@ public:
 	}
 
 	// Assignment Operator
-	SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& m) 
+	SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& m)
 	{
 		m_el[0].mVec128 = m.m_el[0].mVec128;
 		m_el[1].mVec128 = m.m_el[1].mVec128;
 		m_el[2].mVec128 = m.m_el[2].mVec128;
-		
+
 		return *this;
 	}
 
 #else
 
 	/** @brief Copy constructor */
-	SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
+	SIMD_FORCE_INLINE btMatrix3x3(const btMatrix3x3& other)
 	{
 		m_el[0] = other.m_el[0];
 		m_el[1] = other.m_el[1];
 		m_el[2] = other.m_el[2];
 	}
-    
+
 	/** @brief Assignment Operator */
 	SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
 	{
@@ -126,6 +127,13 @@ public:
 		m_el[2] = other.m_el[2];
 		return *this;
 	}
+    
+    SIMD_FORCE_INLINE btMatrix3x3(const btVector3& v0, const btVector3& v1, const btVector3& v2)
+    {
+        m_el[0] = v0;
+        m_el[1] = v1;
+        m_el[2] = v2;
+    }
 
 #endif
 
@@ -133,10 +141,9 @@ public:
 	*  @param i Column number 0 indexed */
 	SIMD_FORCE_INLINE btVector3 getColumn(int i) const
 	{
-		return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
+		return btVector3(m_el[0][i], m_el[1][i], m_el[2][i]);
 	}
 
-
 	/** @brief Get a row of the matrix as a vector 
 	*  @param i Row number 0 indexed */
 	SIMD_FORCE_INLINE const btVector3& getRow(int i) const
@@ -147,10 +154,10 @@ public:
 
 	/** @brief Get a mutable reference to a row of the matrix as a vector 
 	*  @param i Row number 0 indexed */
-	SIMD_FORCE_INLINE btVector3&  operator[](int i)
-	{ 
+	SIMD_FORCE_INLINE btVector3& operator[](int i)
+	{
 		btFullAssert(0 <= i && i < 3);
-		return m_el[i]; 
+		return m_el[i];
 	}
 
 	/** @brief Get a const reference to a row of the matrix as a vector 
@@ -158,32 +165,31 @@ public:
 	SIMD_FORCE_INLINE const btVector3& operator[](int i) const
 	{
 		btFullAssert(0 <= i && i < 3);
-		return m_el[i]; 
+		return m_el[i];
 	}
 
 	/** @brief Multiply by the target matrix on the right
 	*  @param m Rotation matrix to be applied 
 	* Equivilant to this = this * m */
-	btMatrix3x3& operator*=(const btMatrix3x3& m); 
+	btMatrix3x3& operator*=(const btMatrix3x3& m);
 
 	/** @brief Adds by the target matrix on the right
 	*  @param m matrix to be applied 
 	* Equivilant to this = this + m */
-	btMatrix3x3& operator+=(const btMatrix3x3& m); 
+	btMatrix3x3& operator+=(const btMatrix3x3& m);
 
 	/** @brief Substractss by the target matrix on the right
 	*  @param m matrix to be applied 
 	* Equivilant to this = this - m */
-	btMatrix3x3& operator-=(const btMatrix3x3& m); 
+	btMatrix3x3& operator-=(const btMatrix3x3& m);
 
 	/** @brief Set from the rotational part of a 4x4 OpenGL matrix
 	*  @param m A pointer to the beginning of the array of scalars*/
-	void setFromOpenGLSubMatrix(const btScalar *m)
+	void setFromOpenGLSubMatrix(const btScalar* m)
 	{
-		m_el[0].setValue(m[0],m[4],m[8]);
-		m_el[1].setValue(m[1],m[5],m[9]);
-		m_el[2].setValue(m[2],m[6],m[10]);
-
+		m_el[0].setValue(m[0], m[4], m[8]);
+		m_el[1].setValue(m[1], m[5], m[9]);
+		m_el[2].setValue(m[2], m[6], m[10]);
 	}
 	/** @brief Set the values of the matrix explicitly (row major)
 	*  @param xx Top left
@@ -195,93 +201,92 @@ public:
 	*  @param zx Bottom Left
 	*  @param zy Bottom Middle
 	*  @param zz Bottom Right*/
-	void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, 
-		const btScalar& yx, const btScalar& yy, const btScalar& yz, 
-		const btScalar& zx, const btScalar& zy, const btScalar& zz)
+	void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz,
+				  const btScalar& yx, const btScalar& yy, const btScalar& yz,
+				  const btScalar& zx, const btScalar& zy, const btScalar& zz)
 	{
-		m_el[0].setValue(xx,xy,xz);
-		m_el[1].setValue(yx,yy,yz);
-		m_el[2].setValue(zx,zy,zz);
+		m_el[0].setValue(xx, xy, xz);
+		m_el[1].setValue(yx, yy, yz);
+		m_el[2].setValue(zx, zy, zz);
 	}
 
 	/** @brief Set the matrix from a quaternion
-	*  @param q The Quaternion to match */  
-	void setRotation(const btQuaternion& q) 
+	*  @param q The Quaternion to match */
+	void setRotation(const btQuaternion& q)
 	{
 		btScalar d = q.length2();
 		btFullAssert(d != btScalar(0.0));
 		btScalar s = btScalar(2.0) / d;
-    
-    #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
-        __m128	vs, Q = q.get128();
+
+#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+		__m128 vs, Q = q.get128();
 		__m128i Qi = btCastfTo128i(Q);
-        __m128	Y, Z;
-        __m128	V1, V2, V3;
-        __m128	V11, V21, V31;
-        __m128	NQ = _mm_xor_ps(Q, btvMzeroMask);
+		__m128 Y, Z;
+		__m128 V1, V2, V3;
+		__m128 V11, V21, V31;
+		__m128 NQ = _mm_xor_ps(Q, btvMzeroMask);
 		__m128i NQi = btCastfTo128i(NQ);
-        
-        V1 = btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_SHUFFLE(1,0,2,3)));	// Y X Z W
-		V2 = _mm_shuffle_ps(NQ, Q, BT_SHUFFLE(0,0,1,3));     // -X -X  Y  W
-        V3 = btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_SHUFFLE(2,1,0,3)));	// Z Y X W
-        V1 = _mm_xor_ps(V1, vMPPP);	//	change the sign of the first element
-			
-        V11	= btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_SHUFFLE(1,1,0,3)));	// Y Y X W
-		V21 = _mm_unpackhi_ps(Q, Q);                    //  Z  Z  W  W
-		V31 = _mm_shuffle_ps(Q, NQ, BT_SHUFFLE(0,2,0,3));	//  X  Z -X -W
-
-		V2 = V2 * V1;	//
-		V1 = V1 * V11;	//
-		V3 = V3 * V31;	//
-
-        V11 = _mm_shuffle_ps(NQ, Q, BT_SHUFFLE(2,3,1,3));	//	-Z -W  Y  W
-		V11 = V11 * V21;	//
-        V21 = _mm_xor_ps(V21, vMPPP);	//	change the sign of the first element
-		V31 = _mm_shuffle_ps(Q, NQ, BT_SHUFFLE(3,3,1,3));	//	 W  W -Y -W
-        V31 = _mm_xor_ps(V31, vMPPP);	//	change the sign of the first element
-		Y = btCastiTo128f(_mm_shuffle_epi32 (NQi, BT_SHUFFLE(3,2,0,3)));	// -W -Z -X -W
-		Z = btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_SHUFFLE(1,0,1,3)));	//  Y  X  Y  W
+
+		V1 = btCastiTo128f(_mm_shuffle_epi32(Qi, BT_SHUFFLE(1, 0, 2, 3)));  // Y X Z W
+		V2 = _mm_shuffle_ps(NQ, Q, BT_SHUFFLE(0, 0, 1, 3));                 // -X -X  Y  W
+		V3 = btCastiTo128f(_mm_shuffle_epi32(Qi, BT_SHUFFLE(2, 1, 0, 3)));  // Z Y X W
+		V1 = _mm_xor_ps(V1, vMPPP);                                         //	change the sign of the first element
+
+		V11 = btCastiTo128f(_mm_shuffle_epi32(Qi, BT_SHUFFLE(1, 1, 0, 3)));  // Y Y X W
+		V21 = _mm_unpackhi_ps(Q, Q);                                         //  Z  Z  W  W
+		V31 = _mm_shuffle_ps(Q, NQ, BT_SHUFFLE(0, 2, 0, 3));                 //  X  Z -X -W
+
+		V2 = V2 * V1;   //
+		V1 = V1 * V11;  //
+		V3 = V3 * V31;  //
+
+		V11 = _mm_shuffle_ps(NQ, Q, BT_SHUFFLE(2, 3, 1, 3));                //	-Z -W  Y  W
+		V11 = V11 * V21;                                                    //
+		V21 = _mm_xor_ps(V21, vMPPP);                                       //	change the sign of the first element
+		V31 = _mm_shuffle_ps(Q, NQ, BT_SHUFFLE(3, 3, 1, 3));                //	 W  W -Y -W
+		V31 = _mm_xor_ps(V31, vMPPP);                                       //	change the sign of the first element
+		Y = btCastiTo128f(_mm_shuffle_epi32(NQi, BT_SHUFFLE(3, 2, 0, 3)));  // -W -Z -X -W
+		Z = btCastiTo128f(_mm_shuffle_epi32(Qi, BT_SHUFFLE(1, 0, 1, 3)));   //  Y  X  Y  W
 
 		vs = _mm_load_ss(&s);
 		V21 = V21 * Y;
 		V31 = V31 * Z;
 
 		V1 = V1 + V11;
-        V2 = V2 + V21;
-        V3 = V3 + V31;
-
-        vs = bt_splat3_ps(vs, 0);
-            //	s ready
-        V1 = V1 * vs;
-        V2 = V2 * vs;
-        V3 = V3 * vs;
-        
-        V1 = V1 + v1000;
-        V2 = V2 + v0100;
-        V3 = V3 + v0010;
-        
-        m_el[0] = V1; 
-        m_el[1] = V2;
-        m_el[2] = V3;
-    #else    
-		btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
-		btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
-		btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
-		btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
+		V2 = V2 + V21;
+		V3 = V3 + V31;
+
+		vs = bt_splat3_ps(vs, 0);
+		//	s ready
+		V1 = V1 * vs;
+		V2 = V2 * vs;
+		V3 = V3 * vs;
+
+		V1 = V1 + v1000;
+		V2 = V2 + v0100;
+		V3 = V3 + v0010;
+
+		m_el[0] = V1;
+		m_el[1] = V2;
+		m_el[2] = V3;
+#else
+		btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
+		btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
+		btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
+		btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
 		setValue(
-            btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
+			btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
 			xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
 			xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
-	#endif
-    }
-
+#endif
+	}
 
 	/** @brief Set the matrix from euler angles using YPR around YXZ respectively
 	*  @param yaw Yaw about Y axis
 	*  @param pitch Pitch about X axis
 	*  @param roll Roll about Z axis 
 	*/
-	void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 
+	void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
 	{
 		setEulerZYX(roll, pitch, yaw);
 	}
@@ -289,188 +294,217 @@ public:
 	/** @brief Set the matrix from euler angles YPR around ZYX axes
 	* @param eulerX Roll about X axis
 	* @param eulerY Pitch around Y axis
-	* @param eulerZ Yaw aboud Z axis
+	* @param eulerZ Yaw about Z axis
 	* 
 	* These angles are used to produce a rotation matrix. The euler
 	* angles are applied in ZYX order. I.e a vector is first rotated 
 	* about X then Y and then Z
 	**/
-	void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { 
+	void setEulerZYX(btScalar eulerX, btScalar eulerY, btScalar eulerZ)
+	{
 		///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
-		btScalar ci ( btCos(eulerX)); 
-		btScalar cj ( btCos(eulerY)); 
-		btScalar ch ( btCos(eulerZ)); 
-		btScalar si ( btSin(eulerX)); 
-		btScalar sj ( btSin(eulerY)); 
-		btScalar sh ( btSin(eulerZ)); 
-		btScalar cc = ci * ch; 
-		btScalar cs = ci * sh; 
-		btScalar sc = si * ch; 
+		btScalar ci(btCos(eulerX));
+		btScalar cj(btCos(eulerY));
+		btScalar ch(btCos(eulerZ));
+		btScalar si(btSin(eulerX));
+		btScalar sj(btSin(eulerY));
+		btScalar sh(btSin(eulerZ));
+		btScalar cc = ci * ch;
+		btScalar cs = ci * sh;
+		btScalar sc = si * ch;
 		btScalar ss = si * sh;
 
 		setValue(cj * ch, sj * sc - cs, sj * cc + ss,
-			cj * sh, sj * ss + cc, sj * cs - sc, 
-			-sj,      cj * si,      cj * ci);
+				 cj * sh, sj * ss + cc, sj * cs - sc,
+				 -sj, cj * si, cj * ci);
 	}
 
 	/**@brief Set the matrix to the identity */
 	void setIdentity()
-	{ 
-#if (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined(BT_USE_NEON)
-			m_el[0] = v1000; 
-			m_el[1] = v0100;
-			m_el[2] = v0010;
+	{
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
+		m_el[0] = v1000;
+		m_el[1] = v0100;
+		m_el[2] = v0010;
 #else
-		setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
-			btScalar(0.0), btScalar(1.0), btScalar(0.0), 
-			btScalar(0.0), btScalar(0.0), btScalar(1.0)); 
+		setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0),
+				 btScalar(0.0), btScalar(1.0), btScalar(0.0),
+				 btScalar(0.0), btScalar(0.0), btScalar(1.0));
 #endif
 	}
+    
+    /**@brief Set the matrix to the identity */
+    void setZero()
+    {
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
+        m_el[0] = v0000;
+        m_el[1] = v0000;
+        m_el[2] = v0000;
+#else
+        setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0),
+                 btScalar(0.0), btScalar(0.0), btScalar(0.0),
+                 btScalar(0.0), btScalar(0.0), btScalar(0.0));
+#endif
+    }
 
-	static const btMatrix3x3&	getIdentity()
+	static const btMatrix3x3& getIdentity()
 	{
-#if (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined(BT_USE_NEON)
-        static const btMatrix3x3 
-        identityMatrix(v1000, v0100, v0010);
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
+		static const btMatrix3x3
+			identityMatrix(v1000, v0100, v0010);
 #else
-		static const btMatrix3x3 
-        identityMatrix(
-            btScalar(1.0), btScalar(0.0), btScalar(0.0), 
-			btScalar(0.0), btScalar(1.0), btScalar(0.0), 
-			btScalar(0.0), btScalar(0.0), btScalar(1.0));
+		static const btMatrix3x3
+			identityMatrix(
+				btScalar(1.0), btScalar(0.0), btScalar(0.0),
+				btScalar(0.0), btScalar(1.0), btScalar(0.0),
+				btScalar(0.0), btScalar(0.0), btScalar(1.0));
 #endif
 		return identityMatrix;
 	}
 
 	/**@brief Fill the rotational part of an OpenGL matrix and clear the shear/perspective
 	* @param m The array to be filled */
-	void getOpenGLSubMatrix(btScalar *m) const 
+	void getOpenGLSubMatrix(btScalar * m) const
 	{
-#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
-        __m128 v0 = m_el[0].mVec128;
-        __m128 v1 = m_el[1].mVec128;
-        __m128 v2 = m_el[2].mVec128;    //  x2 y2 z2 w2
-        __m128 *vm = (__m128 *)m;
-        __m128 vT;
-        
-        v2 = _mm_and_ps(v2, btvFFF0fMask);  //  x2 y2 z2 0
-        
-        vT = _mm_unpackhi_ps(v0, v1);	//	z0 z1 * *
-        v0 = _mm_unpacklo_ps(v0, v1);	//	x0 x1 y0 y1
-
-        v1 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(2, 3, 1, 3) );	// y0 y1 y2 0
-        v0 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(0, 1, 0, 3) );	// x0 x1 x2 0
-        v2 = btCastdTo128f(_mm_move_sd(btCastfTo128d(v2), btCastfTo128d(vT)));	// z0 z1 z2 0
-
-        vm[0] = v0;
-        vm[1] = v1;
-        vm[2] = v2;
+#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+		__m128 v0 = m_el[0].mVec128;
+		__m128 v1 = m_el[1].mVec128;
+		__m128 v2 = m_el[2].mVec128;  //  x2 y2 z2 w2
+		__m128* vm = (__m128*)m;
+		__m128 vT;
+
+		v2 = _mm_and_ps(v2, btvFFF0fMask);  //  x2 y2 z2 0
+
+		vT = _mm_unpackhi_ps(v0, v1);  //	z0 z1 * *
+		v0 = _mm_unpacklo_ps(v0, v1);  //	x0 x1 y0 y1
+
+		v1 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(2, 3, 1, 3));                    // y0 y1 y2 0
+		v0 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(0, 1, 0, 3));                    // x0 x1 x2 0
+		v2 = btCastdTo128f(_mm_move_sd(btCastfTo128d(v2), btCastfTo128d(vT)));  // z0 z1 z2 0
+
+		vm[0] = v0;
+		vm[1] = v1;
+		vm[2] = v2;
 #elif defined(BT_USE_NEON)
-        // note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
-        static const uint32x2_t zMask = (const uint32x2_t) {static_cast<uint32_t>(-1), 0 };
-        float32x4_t *vm = (float32x4_t *)m;
-        float32x4x2_t top = vtrnq_f32( m_el[0].mVec128, m_el[1].mVec128 );  // {x0 x1 z0 z1}, {y0 y1 w0 w1}
-        float32x2x2_t bl = vtrn_f32( vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f) );       // {x2  0 }, {y2 0}
-        float32x4_t v0 = vcombine_f32( vget_low_f32(top.val[0]), bl.val[0] );
-        float32x4_t v1 = vcombine_f32( vget_low_f32(top.val[1]), bl.val[1] );
-        float32x2_t q = (float32x2_t) vand_u32( (uint32x2_t) vget_high_f32( m_el[2].mVec128), zMask );
-        float32x4_t v2 = vcombine_f32( vget_high_f32(top.val[0]), q );       // z0 z1 z2  0
-
-        vm[0] = v0;
-        vm[1] = v1;
-        vm[2] = v2;
+		// note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
+		static const uint32x2_t zMask = (const uint32x2_t){static_cast<uint32_t>(-1), 0};
+		float32x4_t* vm = (float32x4_t*)m;
+		float32x4x2_t top = vtrnq_f32(m_el[0].mVec128, m_el[1].mVec128);               // {x0 x1 z0 z1}, {y0 y1 w0 w1}
+		float32x2x2_t bl = vtrn_f32(vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f));  // {x2  0 }, {y2 0}
+		float32x4_t v0 = vcombine_f32(vget_low_f32(top.val[0]), bl.val[0]);
+		float32x4_t v1 = vcombine_f32(vget_low_f32(top.val[1]), bl.val[1]);
+		float32x2_t q = (float32x2_t)vand_u32((uint32x2_t)vget_high_f32(m_el[2].mVec128), zMask);
+		float32x4_t v2 = vcombine_f32(vget_high_f32(top.val[0]), q);  // z0 z1 z2  0
+
+		vm[0] = v0;
+		vm[1] = v1;
+		vm[2] = v2;
 #else
-		m[0]  = btScalar(m_el[0].x()); 
-		m[1]  = btScalar(m_el[1].x());
-		m[2]  = btScalar(m_el[2].x());
-		m[3]  = btScalar(0.0); 
-		m[4]  = btScalar(m_el[0].y());
-		m[5]  = btScalar(m_el[1].y());
-		m[6]  = btScalar(m_el[2].y());
-		m[7]  = btScalar(0.0); 
-		m[8]  = btScalar(m_el[0].z()); 
-		m[9]  = btScalar(m_el[1].z());
+		m[0] = btScalar(m_el[0].x());
+		m[1] = btScalar(m_el[1].x());
+		m[2] = btScalar(m_el[2].x());
+		m[3] = btScalar(0.0);
+		m[4] = btScalar(m_el[0].y());
+		m[5] = btScalar(m_el[1].y());
+		m[6] = btScalar(m_el[2].y());
+		m[7] = btScalar(0.0);
+		m[8] = btScalar(m_el[0].z());
+		m[9] = btScalar(m_el[1].z());
 		m[10] = btScalar(m_el[2].z());
-		m[11] = btScalar(0.0); 
+		m[11] = btScalar(0.0);
 #endif
 	}
 
 	/**@brief Get the matrix represented as a quaternion 
 	* @param q The quaternion which will be set */
-	void getRotation(btQuaternion& q) const
+	void getRotation(btQuaternion & q) const
 	{
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
-        btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
-        btScalar s, x;
-        
-        union {
-            btSimdFloat4 vec;
-            btScalar f[4];
-        } temp;
-        
-        if (trace > btScalar(0.0)) 
-        {
-            x = trace + btScalar(1.0);
-
-            temp.f[0]=m_el[2].y() - m_el[1].z();
-            temp.f[1]=m_el[0].z() - m_el[2].x();
-            temp.f[2]=m_el[1].x() - m_el[0].y();
-            temp.f[3]=x;
-            //temp.f[3]= s * btScalar(0.5);
-        } 
-        else 
-        {
-            int i, j, k;
-            if(m_el[0].x() < m_el[1].y()) 
-            { 
-                if( m_el[1].y() < m_el[2].z() )
-                    { i = 2; j = 0; k = 1; }
-                else
-                    { i = 1; j = 2; k = 0; }
-            }
-            else
-            {
-                if( m_el[0].x() < m_el[2].z())
-                    { i = 2; j = 0; k = 1; }
-                else
-                    { i = 0; j = 1; k = 2; }
-            }
-
-            x = m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0);
-
-            temp.f[3] = (m_el[k][j] - m_el[j][k]);
-            temp.f[j] = (m_el[j][i] + m_el[i][j]);
-            temp.f[k] = (m_el[k][i] + m_el[i][k]);
-            temp.f[i] = x;
-            //temp.f[i] = s * btScalar(0.5);
-        }
-
-        s = btSqrt(x);
-        q.set128(temp.vec);
-        s = btScalar(0.5) / s;
-
-        q *= s;
-#else    
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
+		btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
+		btScalar s, x;
+
+		union {
+			btSimdFloat4 vec;
+			btScalar f[4];
+		} temp;
+
+		if (trace > btScalar(0.0))
+		{
+			x = trace + btScalar(1.0);
+
+			temp.f[0] = m_el[2].y() - m_el[1].z();
+			temp.f[1] = m_el[0].z() - m_el[2].x();
+			temp.f[2] = m_el[1].x() - m_el[0].y();
+			temp.f[3] = x;
+			//temp.f[3]= s * btScalar(0.5);
+		}
+		else
+		{
+			int i, j, k;
+			if (m_el[0].x() < m_el[1].y())
+			{
+				if (m_el[1].y() < m_el[2].z())
+				{
+					i = 2;
+					j = 0;
+					k = 1;
+				}
+				else
+				{
+					i = 1;
+					j = 2;
+					k = 0;
+				}
+			}
+			else
+			{
+				if (m_el[0].x() < m_el[2].z())
+				{
+					i = 2;
+					j = 0;
+					k = 1;
+				}
+				else
+				{
+					i = 0;
+					j = 1;
+					k = 2;
+				}
+			}
+
+			x = m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0);
+
+			temp.f[3] = (m_el[k][j] - m_el[j][k]);
+			temp.f[j] = (m_el[j][i] + m_el[i][j]);
+			temp.f[k] = (m_el[k][i] + m_el[i][k]);
+			temp.f[i] = x;
+			//temp.f[i] = s * btScalar(0.5);
+		}
+
+		s = btSqrt(x);
+		q.set128(temp.vec);
+		s = btScalar(0.5) / s;
+
+		q *= s;
+#else
 		btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
 
 		btScalar temp[4];
 
-		if (trace > btScalar(0.0)) 
+		if (trace > btScalar(0.0))
 		{
 			btScalar s = btSqrt(trace + btScalar(1.0));
-			temp[3]=(s * btScalar(0.5));
+			temp[3] = (s * btScalar(0.5));
 			s = btScalar(0.5) / s;
 
-			temp[0]=((m_el[2].y() - m_el[1].z()) * s);
-			temp[1]=((m_el[0].z() - m_el[2].x()) * s);
-			temp[2]=((m_el[1].x() - m_el[0].y()) * s);
-		} 
-		else 
+			temp[0] = ((m_el[2].y() - m_el[1].z()) * s);
+			temp[1] = ((m_el[0].z() - m_el[2].x()) * s);
+			temp[2] = ((m_el[1].x() - m_el[0].y()) * s);
+		}
+		else
 		{
-			int i = m_el[0].x() < m_el[1].y() ? 
-				(m_el[1].y() < m_el[2].z() ? 2 : 1) :
-				(m_el[0].x() < m_el[2].z() ? 2 : 0); 
-			int j = (i + 1) % 3;  
+			int i = m_el[0].x() < m_el[1].y() ? (m_el[1].y() < m_el[2].z() ? 2 : 1) : (m_el[0].x() < m_el[2].z() ? 2 : 0);
+			int j = (i + 1) % 3;
 			int k = (i + 2) % 3;
 
 			btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
@@ -481,44 +515,42 @@ public:
 			temp[j] = (m_el[j][i] + m_el[i][j]) * s;
 			temp[k] = (m_el[k][i] + m_el[i][k]) * s;
 		}
-		q.setValue(temp[0],temp[1],temp[2],temp[3]);
+		q.setValue(temp[0], temp[1], temp[2], temp[3]);
 #endif
 	}
 
 	/**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
 	* @param yaw Yaw around Y axis
 	* @param pitch Pitch around X axis
-	* @param roll around Z axis */	
-	void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
+	* @param roll around Z axis */
+	void getEulerYPR(btScalar & yaw, btScalar & pitch, btScalar & roll) const
 	{
-
 		// first use the normal calculus
 		yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
 		pitch = btScalar(btAsin(-m_el[2].x()));
 		roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
 
 		// on pitch = +/-HalfPI
-		if (btFabs(pitch)==SIMD_HALF_PI)
+		if (btFabs(pitch) == SIMD_HALF_PI)
 		{
-			if (yaw>0)
-				yaw-=SIMD_PI;
+			if (yaw > 0)
+				yaw -= SIMD_PI;
 			else
-				yaw+=SIMD_PI;
+				yaw += SIMD_PI;
 
-			if (roll>0)
-				roll-=SIMD_PI;
+			if (roll > 0)
+				roll -= SIMD_PI;
 			else
-				roll+=SIMD_PI;
+				roll += SIMD_PI;
 		}
 	};
 
-
 	/**@brief Get the matrix represented as euler angles around ZYX
-	* @param yaw Yaw around X axis
+	* @param yaw Yaw around Z axis
 	* @param pitch Pitch around Y axis
 	* @param roll around X axis 
-	* @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/	
-	void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
+	* @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/
+	void getEulerZYX(btScalar & yaw, btScalar & pitch, btScalar & roll, unsigned int solution_number = 1) const
 	{
 		struct Euler
 		{
@@ -528,7 +560,7 @@ public:
 		};
 
 		Euler euler_out;
-		Euler euler_out2; //second solution
+		Euler euler_out2;  //second solution
 		//get the pointer to the raw data
 
 		// Check that pitch is not at a singularity
@@ -538,7 +570,7 @@ public:
 			euler_out2.yaw = 0;
 
 			// From difference of angles formula
-			btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
+			btScalar delta = btAtan2(m_el[0].x(), m_el[0].z());
 			if (m_el[2].x() > 0)  //gimbal locked up
 			{
 				euler_out.pitch = SIMD_PI / btScalar(2.0);
@@ -546,7 +578,7 @@ public:
 				euler_out.roll = euler_out.pitch + delta;
 				euler_out2.roll = euler_out.pitch + delta;
 			}
-			else // gimbal locked down
+			else  // gimbal locked down
 			{
 				euler_out.pitch = -SIMD_PI / btScalar(2.0);
 				euler_out2.pitch = -SIMD_PI / btScalar(2.0);
@@ -556,29 +588,29 @@ public:
 		}
 		else
 		{
-			euler_out.pitch = - btAsin(m_el[2].x());
+			euler_out.pitch = -btAsin(m_el[2].x());
 			euler_out2.pitch = SIMD_PI - euler_out.pitch;
 
-			euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), 
-				m_el[2].z()/btCos(euler_out.pitch));
-			euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), 
-				m_el[2].z()/btCos(euler_out2.pitch));
+			euler_out.roll = btAtan2(m_el[2].y() / btCos(euler_out.pitch),
+									 m_el[2].z() / btCos(euler_out.pitch));
+			euler_out2.roll = btAtan2(m_el[2].y() / btCos(euler_out2.pitch),
+									  m_el[2].z() / btCos(euler_out2.pitch));
 
-			euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), 
-				m_el[0].x()/btCos(euler_out.pitch));
-			euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), 
-				m_el[0].x()/btCos(euler_out2.pitch));
+			euler_out.yaw = btAtan2(m_el[1].x() / btCos(euler_out.pitch),
+									m_el[0].x() / btCos(euler_out.pitch));
+			euler_out2.yaw = btAtan2(m_el[1].x() / btCos(euler_out2.pitch),
+									 m_el[0].x() / btCos(euler_out2.pitch));
 		}
 
 		if (solution_number == 1)
-		{ 
-			yaw = euler_out.yaw; 
+		{
+			yaw = euler_out.yaw;
 			pitch = euler_out.pitch;
 			roll = euler_out.roll;
 		}
 		else
-		{ 
-			yaw = euler_out2.yaw; 
+		{
+			yaw = euler_out2.yaw;
 			pitch = euler_out2.pitch;
 			roll = euler_out2.roll;
 		}
@@ -589,18 +621,18 @@ public:
 
 	btMatrix3x3 scaled(const btVector3& s) const
 	{
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
 		return btMatrix3x3(m_el[0] * s, m_el[1] * s, m_el[2] * s);
-#else		
+#else
 		return btMatrix3x3(
-            m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
+			m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
 			m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
 			m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
 #endif
 	}
 
 	/**@brief Return the determinant of the matrix */
-	btScalar            determinant() const;
+	btScalar determinant() const;
 	/**@brief Return the adjoint of the matrix */
 	btMatrix3x3 adjoint() const;
 	/**@brief Return the matrix with all values non negative */
@@ -608,7 +640,7 @@ public:
 	/**@brief Return the transpose of the matrix */
 	btMatrix3x3 transpose() const;
 	/**@brief Return the inverse of the matrix */
-	btMatrix3x3 inverse() const; 
+	btMatrix3x3 inverse() const;
 
 	/// Solve A * x = b, where b is a column vector. This is more efficient
 	/// than computing the inverse in one-shot cases.
@@ -618,9 +650,9 @@ public:
 		btVector3 col1 = getColumn(0);
 		btVector3 col2 = getColumn(1);
 		btVector3 col3 = getColumn(2);
-		
+
 		btScalar det = btDot(col1, btCross(col2, col3));
-		if (btFabs(det)>SIMD_EPSILON)
+		if (btFabs(det) > SIMD_EPSILON)
 		{
 			det = 1.0f / det;
 		}
@@ -634,30 +666,54 @@ public:
 	btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
 	btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
 
-	SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const 
+	SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const
 	{
 		return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
 	}
-	SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const 
+	SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const
 	{
 		return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
 	}
-	SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const 
+	SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const
 	{
 		return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
 	}
 
+	///extractRotation is from "A robust method to extract the rotational part of deformations"
+	///See http://dl.acm.org/citation.cfm?doid=2994258.2994269
+	///decomposes a matrix A in a orthogonal matrix R and a
+	///symmetric matrix S:
+	///A = R*S.
+	///note that R can include both rotation and scaling.
+	SIMD_FORCE_INLINE void extractRotation(btQuaternion & q, btScalar tolerance = 1.0e-9, int maxIter = 100)
+	{
+		int iter = 0;
+		btScalar w;
+		const btMatrix3x3& A = *this;
+		for (iter = 0; iter < maxIter; iter++)
+		{
+			btMatrix3x3 R(q);
+			btVector3 omega = (R.getColumn(0).cross(A.getColumn(0)) + R.getColumn(1).cross(A.getColumn(1)) + R.getColumn(2).cross(A.getColumn(2))) * (btScalar(1.0) / btFabs(R.getColumn(0).dot(A.getColumn(0)) + R.getColumn(1).dot(A.getColumn(1)) + R.getColumn(2).dot(A.getColumn(2))) +
+																																					  tolerance);
+			w = omega.norm();
+			if (w < tolerance)
+				break;
+			q = btQuaternion(btVector3((btScalar(1.0) / w) * omega), w) *
+				q;
+			q.normalize();
+		}
+	}
 
 	/**@brief diagonalizes this matrix by the Jacobi method.
 	* @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
-	* coordinate system, i.e., old_this = rot * new_this * rot^T. 
+	* coordinate system, i.e., old_this = rot * new_this * rot^T.
 	* @param threshold See iteration
-	* @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied 
-	* by the sum of the absolute values of the diagonal, or when maxSteps have been executed. 
-	* 
-	* Note that this matrix is assumed to be symmetric. 
+	* @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
+	* by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
+	*
+	* Note that this matrix is assumed to be symmetric.
 	*/
-	void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
+	void diagonalize(btMatrix3x3 & rot, btScalar threshold, int maxSteps)
 	{
 		rot.setIdentity();
 		for (int step = maxSteps; step > 0; step--)
@@ -693,7 +749,7 @@ public:
 				step = 1;
 			}
 
-			// compute Jacobi rotation J which leads to a zero for element [p][q] 
+			// compute Jacobi rotation J which leads to a zero for element [p][q]
 			btScalar mpq = m_el[p][q];
 			btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
 			btScalar theta2 = theta * theta;
@@ -702,7 +758,7 @@ public:
 			if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
 			{
 				t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
-					: 1 / (theta - btSqrt(1 + theta2));
+								 : 1 / (theta - btSqrt(1 + theta2));
 				cos = 1 / btSqrt(1 + t * t);
 				sin = cos * t;
 			}
@@ -735,9 +791,6 @@ public:
 		}
 	}
 
-
-
-
 	/**@brief Calculate the matrix cofactor 
 	* @param r1 The first row to use for calculating the cofactor
 	* @param c1 The first column to use for calculating the cofactor
@@ -745,359 +798,354 @@ public:
 	* @param c1 The second column to use for calculating the cofactor
 	* See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
 	*/
-	btScalar cofac(int r1, int c1, int r2, int c2) const 
+	btScalar cofac(int r1, int c1, int r2, int c2) const
 	{
 		return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
 	}
 
-	void	serialize(struct	btMatrix3x3Data& dataOut) const;
+	void serialize(struct btMatrix3x3Data & dataOut) const;
 
-	void	serializeFloat(struct	btMatrix3x3FloatData& dataOut) const;
+	void serializeFloat(struct btMatrix3x3FloatData & dataOut) const;
 
-	void	deSerialize(const struct	btMatrix3x3Data& dataIn);
+	void deSerialize(const struct btMatrix3x3Data& dataIn);
 
-	void	deSerializeFloat(const struct	btMatrix3x3FloatData& dataIn);
-
-	void	deSerializeDouble(const struct	btMatrix3x3DoubleData& dataIn);
+	void deSerializeFloat(const struct btMatrix3x3FloatData& dataIn);
 
+	void deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn);
 };
 
-
-SIMD_FORCE_INLINE btMatrix3x3& 
+SIMD_FORCE_INLINE btMatrix3x3&
 btMatrix3x3::operator*=(const btMatrix3x3& m)
 {
-#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
-    __m128 rv00, rv01, rv02;
-    __m128 rv10, rv11, rv12;
-    __m128 rv20, rv21, rv22;
-    __m128 mv0, mv1, mv2;
-
-    rv02 = m_el[0].mVec128;
-    rv12 = m_el[1].mVec128;
-    rv22 = m_el[2].mVec128;
-
-    mv0 = _mm_and_ps(m[0].mVec128, btvFFF0fMask); 
-    mv1 = _mm_and_ps(m[1].mVec128, btvFFF0fMask); 
-    mv2 = _mm_and_ps(m[2].mVec128, btvFFF0fMask); 
-    
-    // rv0
-    rv00 = bt_splat_ps(rv02, 0);
-    rv01 = bt_splat_ps(rv02, 1);
-    rv02 = bt_splat_ps(rv02, 2);
-    
-    rv00 = _mm_mul_ps(rv00, mv0);
-    rv01 = _mm_mul_ps(rv01, mv1);
-    rv02 = _mm_mul_ps(rv02, mv2);
-    
-    // rv1
-    rv10 = bt_splat_ps(rv12, 0);
-    rv11 = bt_splat_ps(rv12, 1);
-    rv12 = bt_splat_ps(rv12, 2);
-    
-    rv10 = _mm_mul_ps(rv10, mv0);
-    rv11 = _mm_mul_ps(rv11, mv1);
-    rv12 = _mm_mul_ps(rv12, mv2);
-    
-    // rv2
-    rv20 = bt_splat_ps(rv22, 0);
-    rv21 = bt_splat_ps(rv22, 1);
-    rv22 = bt_splat_ps(rv22, 2);
-    
-    rv20 = _mm_mul_ps(rv20, mv0);
-    rv21 = _mm_mul_ps(rv21, mv1);
-    rv22 = _mm_mul_ps(rv22, mv2);
+#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+	__m128 rv00, rv01, rv02;
+	__m128 rv10, rv11, rv12;
+	__m128 rv20, rv21, rv22;
+	__m128 mv0, mv1, mv2;
+
+	rv02 = m_el[0].mVec128;
+	rv12 = m_el[1].mVec128;
+	rv22 = m_el[2].mVec128;
+
+	mv0 = _mm_and_ps(m[0].mVec128, btvFFF0fMask);
+	mv1 = _mm_and_ps(m[1].mVec128, btvFFF0fMask);
+	mv2 = _mm_and_ps(m[2].mVec128, btvFFF0fMask);
+
+	// rv0
+	rv00 = bt_splat_ps(rv02, 0);
+	rv01 = bt_splat_ps(rv02, 1);
+	rv02 = bt_splat_ps(rv02, 2);
+
+	rv00 = _mm_mul_ps(rv00, mv0);
+	rv01 = _mm_mul_ps(rv01, mv1);
+	rv02 = _mm_mul_ps(rv02, mv2);
+
+	// rv1
+	rv10 = bt_splat_ps(rv12, 0);
+	rv11 = bt_splat_ps(rv12, 1);
+	rv12 = bt_splat_ps(rv12, 2);
+
+	rv10 = _mm_mul_ps(rv10, mv0);
+	rv11 = _mm_mul_ps(rv11, mv1);
+	rv12 = _mm_mul_ps(rv12, mv2);
+
+	// rv2
+	rv20 = bt_splat_ps(rv22, 0);
+	rv21 = bt_splat_ps(rv22, 1);
+	rv22 = bt_splat_ps(rv22, 2);
+
+	rv20 = _mm_mul_ps(rv20, mv0);
+	rv21 = _mm_mul_ps(rv21, mv1);
+	rv22 = _mm_mul_ps(rv22, mv2);
+
+	rv00 = _mm_add_ps(rv00, rv01);
+	rv10 = _mm_add_ps(rv10, rv11);
+	rv20 = _mm_add_ps(rv20, rv21);
+
+	m_el[0].mVec128 = _mm_add_ps(rv00, rv02);
+	m_el[1].mVec128 = _mm_add_ps(rv10, rv12);
+	m_el[2].mVec128 = _mm_add_ps(rv20, rv22);
 
-    rv00 = _mm_add_ps(rv00, rv01);
-    rv10 = _mm_add_ps(rv10, rv11);
-    rv20 = _mm_add_ps(rv20, rv21);
+#elif defined(BT_USE_NEON)
 
-    m_el[0].mVec128 = _mm_add_ps(rv00, rv02);
-    m_el[1].mVec128 = _mm_add_ps(rv10, rv12);
-    m_el[2].mVec128 = _mm_add_ps(rv20, rv22);
+	float32x4_t rv0, rv1, rv2;
+	float32x4_t v0, v1, v2;
+	float32x4_t mv0, mv1, mv2;
 
-#elif defined(BT_USE_NEON)
+	v0 = m_el[0].mVec128;
+	v1 = m_el[1].mVec128;
+	v2 = m_el[2].mVec128;
 
-    float32x4_t rv0, rv1, rv2;
-    float32x4_t v0, v1, v2;
-    float32x4_t mv0, mv1, mv2;
+	mv0 = (float32x4_t)vandq_s32((int32x4_t)m[0].mVec128, btvFFF0Mask);
+	mv1 = (float32x4_t)vandq_s32((int32x4_t)m[1].mVec128, btvFFF0Mask);
+	mv2 = (float32x4_t)vandq_s32((int32x4_t)m[2].mVec128, btvFFF0Mask);
 
-    v0 = m_el[0].mVec128;
-    v1 = m_el[1].mVec128;
-    v2 = m_el[2].mVec128;
+	rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
+	rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
+	rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
 
-    mv0 = (float32x4_t) vandq_s32((int32x4_t)m[0].mVec128, btvFFF0Mask); 
-    mv1 = (float32x4_t) vandq_s32((int32x4_t)m[1].mVec128, btvFFF0Mask); 
-    mv2 = (float32x4_t) vandq_s32((int32x4_t)m[2].mVec128, btvFFF0Mask); 
-    
-    rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
-    rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
-    rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
-    
-    rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
-    rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
-    rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
-    
-    rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
-    rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
-    rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
-
-    m_el[0].mVec128 = rv0;
-    m_el[1].mVec128 = rv1;
-    m_el[2].mVec128 = rv2;
-#else    
+	rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
+	rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
+	rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
+
+	rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
+	rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
+	rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
+
+	m_el[0].mVec128 = rv0;
+	m_el[1].mVec128 = rv1;
+	m_el[2].mVec128 = rv2;
+#else
 	setValue(
-        m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
+		m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
 		m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
 		m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
 #endif
 	return *this;
 }
 
-SIMD_FORCE_INLINE btMatrix3x3& 
+SIMD_FORCE_INLINE btMatrix3x3&
 btMatrix3x3::operator+=(const btMatrix3x3& m)
 {
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
-    m_el[0].mVec128 = m_el[0].mVec128 + m.m_el[0].mVec128;
-    m_el[1].mVec128 = m_el[1].mVec128 + m.m_el[1].mVec128;
-    m_el[2].mVec128 = m_el[2].mVec128 + m.m_el[2].mVec128;
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
+	m_el[0].mVec128 = m_el[0].mVec128 + m.m_el[0].mVec128;
+	m_el[1].mVec128 = m_el[1].mVec128 + m.m_el[1].mVec128;
+	m_el[2].mVec128 = m_el[2].mVec128 + m.m_el[2].mVec128;
 #else
 	setValue(
-		m_el[0][0]+m.m_el[0][0], 
-		m_el[0][1]+m.m_el[0][1],
-		m_el[0][2]+m.m_el[0][2],
-		m_el[1][0]+m.m_el[1][0], 
-		m_el[1][1]+m.m_el[1][1],
-		m_el[1][2]+m.m_el[1][2],
-		m_el[2][0]+m.m_el[2][0], 
-		m_el[2][1]+m.m_el[2][1],
-		m_el[2][2]+m.m_el[2][2]);
+		m_el[0][0] + m.m_el[0][0],
+		m_el[0][1] + m.m_el[0][1],
+		m_el[0][2] + m.m_el[0][2],
+		m_el[1][0] + m.m_el[1][0],
+		m_el[1][1] + m.m_el[1][1],
+		m_el[1][2] + m.m_el[1][2],
+		m_el[2][0] + m.m_el[2][0],
+		m_el[2][1] + m.m_el[2][1],
+		m_el[2][2] + m.m_el[2][2]);
 #endif
 	return *this;
 }
 
 SIMD_FORCE_INLINE btMatrix3x3
-operator*(const btMatrix3x3& m, const btScalar & k)
+operator*(const btMatrix3x3& m, const btScalar& k)
 {
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
-    __m128 vk = bt_splat_ps(_mm_load_ss((float *)&k), 0x80);
-    return btMatrix3x3(
-                _mm_mul_ps(m[0].mVec128, vk), 
-                _mm_mul_ps(m[1].mVec128, vk), 
-                _mm_mul_ps(m[2].mVec128, vk)); 
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
+	__m128 vk = bt_splat_ps(_mm_load_ss((float*)&k), 0x80);
+	return btMatrix3x3(
+		_mm_mul_ps(m[0].mVec128, vk),
+		_mm_mul_ps(m[1].mVec128, vk),
+		_mm_mul_ps(m[2].mVec128, vk));
 #elif defined(BT_USE_NEON)
-    return btMatrix3x3(
-                vmulq_n_f32(m[0].mVec128, k),
-                vmulq_n_f32(m[1].mVec128, k),
-                vmulq_n_f32(m[2].mVec128, k)); 
+	return btMatrix3x3(
+		vmulq_n_f32(m[0].mVec128, k),
+		vmulq_n_f32(m[1].mVec128, k),
+		vmulq_n_f32(m[2].mVec128, k));
 #else
 	return btMatrix3x3(
-		m[0].x()*k,m[0].y()*k,m[0].z()*k,
-		m[1].x()*k,m[1].y()*k,m[1].z()*k,
-		m[2].x()*k,m[2].y()*k,m[2].z()*k);
+		m[0].x() * k, m[0].y() * k, m[0].z() * k,
+		m[1].x() * k, m[1].y() * k, m[1].z() * k,
+		m[2].x() * k, m[2].y() * k, m[2].z() * k);
 #endif
 }
 
-SIMD_FORCE_INLINE btMatrix3x3 
+SIMD_FORCE_INLINE btMatrix3x3
 operator+(const btMatrix3x3& m1, const btMatrix3x3& m2)
 {
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
 	return btMatrix3x3(
-        m1[0].mVec128 + m2[0].mVec128,
-        m1[1].mVec128 + m2[1].mVec128,
-        m1[2].mVec128 + m2[2].mVec128);
+		m1[0].mVec128 + m2[0].mVec128,
+		m1[1].mVec128 + m2[1].mVec128,
+		m1[2].mVec128 + m2[2].mVec128);
 #else
 	return btMatrix3x3(
-        m1[0][0]+m2[0][0], 
-        m1[0][1]+m2[0][1],
-        m1[0][2]+m2[0][2],
-        
-        m1[1][0]+m2[1][0], 
-        m1[1][1]+m2[1][1],
-        m1[1][2]+m2[1][2],
-        
-        m1[2][0]+m2[2][0], 
-        m1[2][1]+m2[2][1],
-        m1[2][2]+m2[2][2]);
-#endif    
+		m1[0][0] + m2[0][0],
+		m1[0][1] + m2[0][1],
+		m1[0][2] + m2[0][2],
+
+		m1[1][0] + m2[1][0],
+		m1[1][1] + m2[1][1],
+		m1[1][2] + m2[1][2],
+
+		m1[2][0] + m2[2][0],
+		m1[2][1] + m2[2][1],
+		m1[2][2] + m2[2][2]);
+#endif
 }
 
-SIMD_FORCE_INLINE btMatrix3x3 
+SIMD_FORCE_INLINE btMatrix3x3
 operator-(const btMatrix3x3& m1, const btMatrix3x3& m2)
 {
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
 	return btMatrix3x3(
-        m1[0].mVec128 - m2[0].mVec128,
-        m1[1].mVec128 - m2[1].mVec128,
-        m1[2].mVec128 - m2[2].mVec128);
+		m1[0].mVec128 - m2[0].mVec128,
+		m1[1].mVec128 - m2[1].mVec128,
+		m1[2].mVec128 - m2[2].mVec128);
 #else
 	return btMatrix3x3(
-        m1[0][0]-m2[0][0], 
-        m1[0][1]-m2[0][1],
-        m1[0][2]-m2[0][2],
-        
-        m1[1][0]-m2[1][0], 
-        m1[1][1]-m2[1][1],
-        m1[1][2]-m2[1][2],
-        
-        m1[2][0]-m2[2][0], 
-        m1[2][1]-m2[2][1],
-        m1[2][2]-m2[2][2]);
+		m1[0][0] - m2[0][0],
+		m1[0][1] - m2[0][1],
+		m1[0][2] - m2[0][2],
+
+		m1[1][0] - m2[1][0],
+		m1[1][1] - m2[1][1],
+		m1[1][2] - m2[1][2],
+
+		m1[2][0] - m2[2][0],
+		m1[2][1] - m2[2][1],
+		m1[2][2] - m2[2][2]);
 #endif
 }
 
-
-SIMD_FORCE_INLINE btMatrix3x3& 
+SIMD_FORCE_INLINE btMatrix3x3&
 btMatrix3x3::operator-=(const btMatrix3x3& m)
 {
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
-    m_el[0].mVec128 = m_el[0].mVec128 - m.m_el[0].mVec128;
-    m_el[1].mVec128 = m_el[1].mVec128 - m.m_el[1].mVec128;
-    m_el[2].mVec128 = m_el[2].mVec128 - m.m_el[2].mVec128;
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
+	m_el[0].mVec128 = m_el[0].mVec128 - m.m_el[0].mVec128;
+	m_el[1].mVec128 = m_el[1].mVec128 - m.m_el[1].mVec128;
+	m_el[2].mVec128 = m_el[2].mVec128 - m.m_el[2].mVec128;
 #else
 	setValue(
-	m_el[0][0]-m.m_el[0][0], 
-	m_el[0][1]-m.m_el[0][1],
-	m_el[0][2]-m.m_el[0][2],
-	m_el[1][0]-m.m_el[1][0], 
-	m_el[1][1]-m.m_el[1][1],
-	m_el[1][2]-m.m_el[1][2],
-	m_el[2][0]-m.m_el[2][0], 
-	m_el[2][1]-m.m_el[2][1],
-	m_el[2][2]-m.m_el[2][2]);
+		m_el[0][0] - m.m_el[0][0],
+		m_el[0][1] - m.m_el[0][1],
+		m_el[0][2] - m.m_el[0][2],
+		m_el[1][0] - m.m_el[1][0],
+		m_el[1][1] - m.m_el[1][1],
+		m_el[1][2] - m.m_el[1][2],
+		m_el[2][0] - m.m_el[2][0],
+		m_el[2][1] - m.m_el[2][1],
+		m_el[2][2] - m.m_el[2][2]);
 #endif
 	return *this;
 }
 
-
-SIMD_FORCE_INLINE btScalar 
+SIMD_FORCE_INLINE btScalar
 btMatrix3x3::determinant() const
-{ 
+{
 	return btTriple((*this)[0], (*this)[1], (*this)[2]);
 }
 
-
-SIMD_FORCE_INLINE btMatrix3x3 
+SIMD_FORCE_INLINE btMatrix3x3
 btMatrix3x3::absolute() const
 {
-#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
-    return btMatrix3x3(
-            _mm_and_ps(m_el[0].mVec128, btvAbsfMask),
-            _mm_and_ps(m_el[1].mVec128, btvAbsfMask),
-            _mm_and_ps(m_el[2].mVec128, btvAbsfMask));
+#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
+	return btMatrix3x3(
+		_mm_and_ps(m_el[0].mVec128, btvAbsfMask),
+		_mm_and_ps(m_el[1].mVec128, btvAbsfMask),
+		_mm_and_ps(m_el[2].mVec128, btvAbsfMask));
 #elif defined(BT_USE_NEON)
-    return btMatrix3x3(
-            (float32x4_t)vandq_s32((int32x4_t)m_el[0].mVec128, btv3AbsMask),
-            (float32x4_t)vandq_s32((int32x4_t)m_el[1].mVec128, btv3AbsMask),
-            (float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, btv3AbsMask));
-#else	
 	return btMatrix3x3(
-            btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
-            btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
-            btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
+		(float32x4_t)vandq_s32((int32x4_t)m_el[0].mVec128, btv3AbsMask),
+		(float32x4_t)vandq_s32((int32x4_t)m_el[1].mVec128, btv3AbsMask),
+		(float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, btv3AbsMask));
+#else
+	return btMatrix3x3(
+		btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
+		btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
+		btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
 #endif
 }
 
-SIMD_FORCE_INLINE btMatrix3x3 
-btMatrix3x3::transpose() const 
+SIMD_FORCE_INLINE btMatrix3x3
+btMatrix3x3::transpose() const
 {
-#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
-    __m128 v0 = m_el[0].mVec128;
-    __m128 v1 = m_el[1].mVec128;
-    __m128 v2 = m_el[2].mVec128;    //  x2 y2 z2 w2
-    __m128 vT;
-    
-    v2 = _mm_and_ps(v2, btvFFF0fMask);  //  x2 y2 z2 0
-    
-    vT = _mm_unpackhi_ps(v0, v1);	//	z0 z1 * *
-    v0 = _mm_unpacklo_ps(v0, v1);	//	x0 x1 y0 y1
+#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
+	__m128 v0 = m_el[0].mVec128;
+	__m128 v1 = m_el[1].mVec128;
+	__m128 v2 = m_el[2].mVec128;  //  x2 y2 z2 w2
+	__m128 vT;
+
+	v2 = _mm_and_ps(v2, btvFFF0fMask);  //  x2 y2 z2 0
 
-    v1 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(2, 3, 1, 3) );	// y0 y1 y2 0
-    v0 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(0, 1, 0, 3) );	// x0 x1 x2 0
-    v2 = btCastdTo128f(_mm_move_sd(btCastfTo128d(v2), btCastfTo128d(vT)));	// z0 z1 z2 0
+	vT = _mm_unpackhi_ps(v0, v1);  //	z0 z1 * *
+	v0 = _mm_unpacklo_ps(v0, v1);  //	x0 x1 y0 y1
 
+	v1 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(2, 3, 1, 3));                    // y0 y1 y2 0
+	v0 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(0, 1, 0, 3));                    // x0 x1 x2 0
+	v2 = btCastdTo128f(_mm_move_sd(btCastfTo128d(v2), btCastfTo128d(vT)));  // z0 z1 z2 0
 
-    return btMatrix3x3( v0, v1, v2 );
+	return btMatrix3x3(v0, v1, v2);
 #elif defined(BT_USE_NEON)
-    // note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
-    static const uint32x2_t zMask = (const uint32x2_t) {static_cast<uint32_t>(-1), 0 };
-    float32x4x2_t top = vtrnq_f32( m_el[0].mVec128, m_el[1].mVec128 );  // {x0 x1 z0 z1}, {y0 y1 w0 w1}
-    float32x2x2_t bl = vtrn_f32( vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f) );       // {x2  0 }, {y2 0}
-    float32x4_t v0 = vcombine_f32( vget_low_f32(top.val[0]), bl.val[0] );
-    float32x4_t v1 = vcombine_f32( vget_low_f32(top.val[1]), bl.val[1] );
-    float32x2_t q = (float32x2_t) vand_u32( (uint32x2_t) vget_high_f32( m_el[2].mVec128), zMask );
-    float32x4_t v2 = vcombine_f32( vget_high_f32(top.val[0]), q );       // z0 z1 z2  0
-    return btMatrix3x3( v0, v1, v2 ); 
+	// note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
+	static const uint32x2_t zMask = (const uint32x2_t){static_cast<uint32_t>(-1), 0};
+	float32x4x2_t top = vtrnq_f32(m_el[0].mVec128, m_el[1].mVec128);               // {x0 x1 z0 z1}, {y0 y1 w0 w1}
+	float32x2x2_t bl = vtrn_f32(vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f));  // {x2  0 }, {y2 0}
+	float32x4_t v0 = vcombine_f32(vget_low_f32(top.val[0]), bl.val[0]);
+	float32x4_t v1 = vcombine_f32(vget_low_f32(top.val[1]), bl.val[1]);
+	float32x2_t q = (float32x2_t)vand_u32((uint32x2_t)vget_high_f32(m_el[2].mVec128), zMask);
+	float32x4_t v2 = vcombine_f32(vget_high_f32(top.val[0]), q);  // z0 z1 z2  0
+	return btMatrix3x3(v0, v1, v2);
 #else
-	return btMatrix3x3( m_el[0].x(), m_el[1].x(), m_el[2].x(),
-                        m_el[0].y(), m_el[1].y(), m_el[2].y(),
-                        m_el[0].z(), m_el[1].z(), m_el[2].z());
+	return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
+					   m_el[0].y(), m_el[1].y(), m_el[2].y(),
+					   m_el[0].z(), m_el[1].z(), m_el[2].z());
 #endif
 }
 
-SIMD_FORCE_INLINE btMatrix3x3 
-btMatrix3x3::adjoint() const 
+SIMD_FORCE_INLINE btMatrix3x3
+btMatrix3x3::adjoint() const
 {
 	return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
-		cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
-		cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
+					   cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
+					   cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
 }
 
-SIMD_FORCE_INLINE btMatrix3x3 
+SIMD_FORCE_INLINE btMatrix3x3
 btMatrix3x3::inverse() const
 {
 	btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
 	btScalar det = (*this)[0].dot(co);
-	btFullAssert(det != btScalar(0.0));
+	//btFullAssert(det != btScalar(0.0));
+	btAssert(det != btScalar(0.0));
 	btScalar s = btScalar(1.0) / det;
 	return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
-		co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
-		co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
+					   co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
+					   co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
 }
 
-SIMD_FORCE_INLINE btMatrix3x3 
+SIMD_FORCE_INLINE btMatrix3x3
 btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
 {
-#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
-    // zeros w
-//    static const __m128i xyzMask = (const __m128i){ -1ULL, 0xffffffffULL };
-    __m128 row = m_el[0].mVec128;
-    __m128 m0 = _mm_and_ps( m.getRow(0).mVec128, btvFFF0fMask );
-    __m128 m1 = _mm_and_ps( m.getRow(1).mVec128, btvFFF0fMask);
-    __m128 m2 = _mm_and_ps( m.getRow(2).mVec128, btvFFF0fMask );
-    __m128 r0 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0));
-    __m128 r1 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0x55));
-    __m128 r2 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0xaa));
-    row = m_el[1].mVec128;
-    r0 = _mm_add_ps( r0, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0)));
-    r1 = _mm_add_ps( r1, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0x55)));
-    r2 = _mm_add_ps( r2, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0xaa)));
-    row = m_el[2].mVec128;
-    r0 = _mm_add_ps( r0, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0)));
-    r1 = _mm_add_ps( r1, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0x55)));
-    r2 = _mm_add_ps( r2, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0xaa)));
-    return btMatrix3x3( r0, r1, r2 );
+#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
+	// zeros w
+	//    static const __m128i xyzMask = (const __m128i){ -1ULL, 0xffffffffULL };
+	__m128 row = m_el[0].mVec128;
+	__m128 m0 = _mm_and_ps(m.getRow(0).mVec128, btvFFF0fMask);
+	__m128 m1 = _mm_and_ps(m.getRow(1).mVec128, btvFFF0fMask);
+	__m128 m2 = _mm_and_ps(m.getRow(2).mVec128, btvFFF0fMask);
+	__m128 r0 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0));
+	__m128 r1 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0x55));
+	__m128 r2 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0xaa));
+	row = m_el[1].mVec128;
+	r0 = _mm_add_ps(r0, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0)));
+	r1 = _mm_add_ps(r1, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0x55)));
+	r2 = _mm_add_ps(r2, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0xaa)));
+	row = m_el[2].mVec128;
+	r0 = _mm_add_ps(r0, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0)));
+	r1 = _mm_add_ps(r1, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0x55)));
+	r2 = _mm_add_ps(r2, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0xaa)));
+	return btMatrix3x3(r0, r1, r2);
 
 #elif defined BT_USE_NEON
-    // zeros w
-    static const uint32x4_t xyzMask = (const uint32x4_t){ static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), 0 };
-    float32x4_t m0 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(0).mVec128, xyzMask );
-    float32x4_t m1 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(1).mVec128, xyzMask );
-    float32x4_t m2 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(2).mVec128, xyzMask );
-    float32x4_t row = m_el[0].mVec128;
-    float32x4_t r0 = vmulq_lane_f32( m0, vget_low_f32(row), 0);
-    float32x4_t r1 = vmulq_lane_f32( m0, vget_low_f32(row), 1);
-    float32x4_t r2 = vmulq_lane_f32( m0, vget_high_f32(row), 0);
-    row = m_el[1].mVec128;
-    r0 = vmlaq_lane_f32( r0, m1, vget_low_f32(row), 0);
-    r1 = vmlaq_lane_f32( r1, m1, vget_low_f32(row), 1);
-    r2 = vmlaq_lane_f32( r2, m1, vget_high_f32(row), 0);
-    row = m_el[2].mVec128;
-    r0 = vmlaq_lane_f32( r0, m2, vget_low_f32(row), 0);
-    r1 = vmlaq_lane_f32( r1, m2, vget_low_f32(row), 1);
-    r2 = vmlaq_lane_f32( r2, m2, vget_high_f32(row), 0);
-    return btMatrix3x3( r0, r1, r2 );
+	// zeros w
+	static const uint32x4_t xyzMask = (const uint32x4_t){static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), 0};
+	float32x4_t m0 = (float32x4_t)vandq_u32((uint32x4_t)m.getRow(0).mVec128, xyzMask);
+	float32x4_t m1 = (float32x4_t)vandq_u32((uint32x4_t)m.getRow(1).mVec128, xyzMask);
+	float32x4_t m2 = (float32x4_t)vandq_u32((uint32x4_t)m.getRow(2).mVec128, xyzMask);
+	float32x4_t row = m_el[0].mVec128;
+	float32x4_t r0 = vmulq_lane_f32(m0, vget_low_f32(row), 0);
+	float32x4_t r1 = vmulq_lane_f32(m0, vget_low_f32(row), 1);
+	float32x4_t r2 = vmulq_lane_f32(m0, vget_high_f32(row), 0);
+	row = m_el[1].mVec128;
+	r0 = vmlaq_lane_f32(r0, m1, vget_low_f32(row), 0);
+	r1 = vmlaq_lane_f32(r1, m1, vget_low_f32(row), 1);
+	r2 = vmlaq_lane_f32(r2, m1, vget_high_f32(row), 0);
+	row = m_el[2].mVec128;
+	r0 = vmlaq_lane_f32(r0, m2, vget_low_f32(row), 0);
+	r1 = vmlaq_lane_f32(r1, m2, vget_low_f32(row), 1);
+	r2 = vmlaq_lane_f32(r2, m2, vget_high_f32(row), 0);
+	return btMatrix3x3(r0, r1, r2);
 #else
-    return btMatrix3x3(
+	return btMatrix3x3(
 		m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
 		m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
 		m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
@@ -1110,51 +1158,51 @@ btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
 #endif
 }
 
-SIMD_FORCE_INLINE btMatrix3x3 
+SIMD_FORCE_INLINE btMatrix3x3
 btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
 {
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
-    __m128 a0 = m_el[0].mVec128;
-    __m128 a1 = m_el[1].mVec128;
-    __m128 a2 = m_el[2].mVec128;
-    
-    btMatrix3x3 mT = m.transpose(); // we rely on transpose() zeroing w channel so that we don't have to do it here
-    __m128 mx = mT[0].mVec128;
-    __m128 my = mT[1].mVec128;
-    __m128 mz = mT[2].mVec128;
-    
-    __m128 r0 = _mm_mul_ps(mx, _mm_shuffle_ps(a0, a0, 0x00));
-    __m128 r1 = _mm_mul_ps(mx, _mm_shuffle_ps(a1, a1, 0x00));
-    __m128 r2 = _mm_mul_ps(mx, _mm_shuffle_ps(a2, a2, 0x00));
-    r0 = _mm_add_ps(r0, _mm_mul_ps(my, _mm_shuffle_ps(a0, a0, 0x55)));
-    r1 = _mm_add_ps(r1, _mm_mul_ps(my, _mm_shuffle_ps(a1, a1, 0x55)));
-    r2 = _mm_add_ps(r2, _mm_mul_ps(my, _mm_shuffle_ps(a2, a2, 0x55)));
-    r0 = _mm_add_ps(r0, _mm_mul_ps(mz, _mm_shuffle_ps(a0, a0, 0xaa)));
-    r1 = _mm_add_ps(r1, _mm_mul_ps(mz, _mm_shuffle_ps(a1, a1, 0xaa)));
-    r2 = _mm_add_ps(r2, _mm_mul_ps(mz, _mm_shuffle_ps(a2, a2, 0xaa)));
-    return btMatrix3x3( r0, r1, r2);
-            
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
+	__m128 a0 = m_el[0].mVec128;
+	__m128 a1 = m_el[1].mVec128;
+	__m128 a2 = m_el[2].mVec128;
+
+	btMatrix3x3 mT = m.transpose();  // we rely on transpose() zeroing w channel so that we don't have to do it here
+	__m128 mx = mT[0].mVec128;
+	__m128 my = mT[1].mVec128;
+	__m128 mz = mT[2].mVec128;
+
+	__m128 r0 = _mm_mul_ps(mx, _mm_shuffle_ps(a0, a0, 0x00));
+	__m128 r1 = _mm_mul_ps(mx, _mm_shuffle_ps(a1, a1, 0x00));
+	__m128 r2 = _mm_mul_ps(mx, _mm_shuffle_ps(a2, a2, 0x00));
+	r0 = _mm_add_ps(r0, _mm_mul_ps(my, _mm_shuffle_ps(a0, a0, 0x55)));
+	r1 = _mm_add_ps(r1, _mm_mul_ps(my, _mm_shuffle_ps(a1, a1, 0x55)));
+	r2 = _mm_add_ps(r2, _mm_mul_ps(my, _mm_shuffle_ps(a2, a2, 0x55)));
+	r0 = _mm_add_ps(r0, _mm_mul_ps(mz, _mm_shuffle_ps(a0, a0, 0xaa)));
+	r1 = _mm_add_ps(r1, _mm_mul_ps(mz, _mm_shuffle_ps(a1, a1, 0xaa)));
+	r2 = _mm_add_ps(r2, _mm_mul_ps(mz, _mm_shuffle_ps(a2, a2, 0xaa)));
+	return btMatrix3x3(r0, r1, r2);
+
 #elif defined BT_USE_NEON
-    float32x4_t a0 = m_el[0].mVec128;
-    float32x4_t a1 = m_el[1].mVec128;
-    float32x4_t a2 = m_el[2].mVec128;
-    
-    btMatrix3x3 mT = m.transpose(); // we rely on transpose() zeroing w channel so that we don't have to do it here
-    float32x4_t mx = mT[0].mVec128;
-    float32x4_t my = mT[1].mVec128;
-    float32x4_t mz = mT[2].mVec128;
-    
-    float32x4_t r0 = vmulq_lane_f32( mx, vget_low_f32(a0), 0);
-    float32x4_t r1 = vmulq_lane_f32( mx, vget_low_f32(a1), 0);
-    float32x4_t r2 = vmulq_lane_f32( mx, vget_low_f32(a2), 0);
-    r0 = vmlaq_lane_f32( r0, my, vget_low_f32(a0), 1);
-    r1 = vmlaq_lane_f32( r1, my, vget_low_f32(a1), 1);
-    r2 = vmlaq_lane_f32( r2, my, vget_low_f32(a2), 1);
-    r0 = vmlaq_lane_f32( r0, mz, vget_high_f32(a0), 0);
-    r1 = vmlaq_lane_f32( r1, mz, vget_high_f32(a1), 0);
-    r2 = vmlaq_lane_f32( r2, mz, vget_high_f32(a2), 0);
-    return btMatrix3x3( r0, r1, r2 );
-    
+	float32x4_t a0 = m_el[0].mVec128;
+	float32x4_t a1 = m_el[1].mVec128;
+	float32x4_t a2 = m_el[2].mVec128;
+
+	btMatrix3x3 mT = m.transpose();  // we rely on transpose() zeroing w channel so that we don't have to do it here
+	float32x4_t mx = mT[0].mVec128;
+	float32x4_t my = mT[1].mVec128;
+	float32x4_t mz = mT[2].mVec128;
+
+	float32x4_t r0 = vmulq_lane_f32(mx, vget_low_f32(a0), 0);
+	float32x4_t r1 = vmulq_lane_f32(mx, vget_low_f32(a1), 0);
+	float32x4_t r2 = vmulq_lane_f32(mx, vget_low_f32(a2), 0);
+	r0 = vmlaq_lane_f32(r0, my, vget_low_f32(a0), 1);
+	r1 = vmlaq_lane_f32(r1, my, vget_low_f32(a1), 1);
+	r2 = vmlaq_lane_f32(r2, my, vget_low_f32(a2), 1);
+	r0 = vmlaq_lane_f32(r0, mz, vget_high_f32(a0), 0);
+	r1 = vmlaq_lane_f32(r1, mz, vget_high_f32(a1), 0);
+	r2 = vmlaq_lane_f32(r2, mz, vget_high_f32(a2), 0);
+	return btMatrix3x3(r0, r1, r2);
+
 #else
 	return btMatrix3x3(
 		m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
@@ -1163,139 +1211,138 @@ btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
 #endif
 }
 
-SIMD_FORCE_INLINE btVector3 
-operator*(const btMatrix3x3& m, const btVector3& v) 
+SIMD_FORCE_INLINE btVector3
+operator*(const btMatrix3x3& m, const btVector3& v)
 {
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
-    return v.dot3(m[0], m[1], m[2]);
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
+	return v.dot3(m[0], m[1], m[2]);
 #else
 	return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
 #endif
 }
 
-
 SIMD_FORCE_INLINE btVector3
 operator*(const btVector3& v, const btMatrix3x3& m)
 {
-#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
+#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
 
-    const __m128 vv = v.mVec128;
+	const __m128 vv = v.mVec128;
 
-    __m128 c0 = bt_splat_ps( vv, 0);
-    __m128 c1 = bt_splat_ps( vv, 1);
-    __m128 c2 = bt_splat_ps( vv, 2);
+	__m128 c0 = bt_splat_ps(vv, 0);
+	__m128 c1 = bt_splat_ps(vv, 1);
+	__m128 c2 = bt_splat_ps(vv, 2);
 
-    c0 = _mm_mul_ps(c0, _mm_and_ps(m[0].mVec128, btvFFF0fMask) );
-    c1 = _mm_mul_ps(c1, _mm_and_ps(m[1].mVec128, btvFFF0fMask) );
-    c0 = _mm_add_ps(c0, c1);
-    c2 = _mm_mul_ps(c2, _mm_and_ps(m[2].mVec128, btvFFF0fMask) );
-    
-    return btVector3(_mm_add_ps(c0, c2));
+	c0 = _mm_mul_ps(c0, _mm_and_ps(m[0].mVec128, btvFFF0fMask));
+	c1 = _mm_mul_ps(c1, _mm_and_ps(m[1].mVec128, btvFFF0fMask));
+	c0 = _mm_add_ps(c0, c1);
+	c2 = _mm_mul_ps(c2, _mm_and_ps(m[2].mVec128, btvFFF0fMask));
+
+	return btVector3(_mm_add_ps(c0, c2));
 #elif defined(BT_USE_NEON)
-    const float32x4_t vv = v.mVec128;
-    const float32x2_t vlo = vget_low_f32(vv);
-    const float32x2_t vhi = vget_high_f32(vv);
+	const float32x4_t vv = v.mVec128;
+	const float32x2_t vlo = vget_low_f32(vv);
+	const float32x2_t vhi = vget_high_f32(vv);
 
-    float32x4_t c0, c1, c2;
+	float32x4_t c0, c1, c2;
 
-    c0 = (float32x4_t) vandq_s32((int32x4_t)m[0].mVec128, btvFFF0Mask);
-    c1 = (float32x4_t) vandq_s32((int32x4_t)m[1].mVec128, btvFFF0Mask);
-    c2 = (float32x4_t) vandq_s32((int32x4_t)m[2].mVec128, btvFFF0Mask);
+	c0 = (float32x4_t)vandq_s32((int32x4_t)m[0].mVec128, btvFFF0Mask);
+	c1 = (float32x4_t)vandq_s32((int32x4_t)m[1].mVec128, btvFFF0Mask);
+	c2 = (float32x4_t)vandq_s32((int32x4_t)m[2].mVec128, btvFFF0Mask);
 
-    c0 = vmulq_lane_f32(c0, vlo, 0);
-    c1 = vmulq_lane_f32(c1, vlo, 1);
-    c2 = vmulq_lane_f32(c2, vhi, 0);
-    c0 = vaddq_f32(c0, c1);
-    c0 = vaddq_f32(c0, c2);
-    
-    return btVector3(c0);
+	c0 = vmulq_lane_f32(c0, vlo, 0);
+	c1 = vmulq_lane_f32(c1, vlo, 1);
+	c2 = vmulq_lane_f32(c2, vhi, 0);
+	c0 = vaddq_f32(c0, c1);
+	c0 = vaddq_f32(c0, c2);
+
+	return btVector3(c0);
 #else
 	return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
 #endif
 }
 
-SIMD_FORCE_INLINE btMatrix3x3 
+SIMD_FORCE_INLINE btMatrix3x3
 operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
 {
-#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
+#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
 
-    __m128 m10 = m1[0].mVec128;  
-    __m128 m11 = m1[1].mVec128;
-    __m128 m12 = m1[2].mVec128;
-    
-    __m128 m2v = _mm_and_ps(m2[0].mVec128, btvFFF0fMask);
-    
-    __m128 c0 = bt_splat_ps( m10, 0);
-    __m128 c1 = bt_splat_ps( m11, 0);
-    __m128 c2 = bt_splat_ps( m12, 0);
-    
-    c0 = _mm_mul_ps(c0, m2v);
-    c1 = _mm_mul_ps(c1, m2v);
-    c2 = _mm_mul_ps(c2, m2v);
-    
-    m2v = _mm_and_ps(m2[1].mVec128, btvFFF0fMask);
-    
-    __m128 c0_1 = bt_splat_ps( m10, 1);
-    __m128 c1_1 = bt_splat_ps( m11, 1);
-    __m128 c2_1 = bt_splat_ps( m12, 1);
-    
-    c0_1 = _mm_mul_ps(c0_1, m2v);
-    c1_1 = _mm_mul_ps(c1_1, m2v);
-    c2_1 = _mm_mul_ps(c2_1, m2v);
-    
-    m2v = _mm_and_ps(m2[2].mVec128, btvFFF0fMask);
-    
-    c0 = _mm_add_ps(c0, c0_1);
-    c1 = _mm_add_ps(c1, c1_1);
-    c2 = _mm_add_ps(c2, c2_1);
-    
-    m10 = bt_splat_ps( m10, 2);
-    m11 = bt_splat_ps( m11, 2);
-    m12 = bt_splat_ps( m12, 2);
-    
-    m10 = _mm_mul_ps(m10, m2v);
-    m11 = _mm_mul_ps(m11, m2v);
-    m12 = _mm_mul_ps(m12, m2v);
-    
-    c0 = _mm_add_ps(c0, m10);
-    c1 = _mm_add_ps(c1, m11);
-    c2 = _mm_add_ps(c2, m12);
-    
-    return btMatrix3x3(c0, c1, c2);
+	__m128 m10 = m1[0].mVec128;
+	__m128 m11 = m1[1].mVec128;
+	__m128 m12 = m1[2].mVec128;
+
+	__m128 m2v = _mm_and_ps(m2[0].mVec128, btvFFF0fMask);
+
+	__m128 c0 = bt_splat_ps(m10, 0);
+	__m128 c1 = bt_splat_ps(m11, 0);
+	__m128 c2 = bt_splat_ps(m12, 0);
+
+	c0 = _mm_mul_ps(c0, m2v);
+	c1 = _mm_mul_ps(c1, m2v);
+	c2 = _mm_mul_ps(c2, m2v);
+
+	m2v = _mm_and_ps(m2[1].mVec128, btvFFF0fMask);
+
+	__m128 c0_1 = bt_splat_ps(m10, 1);
+	__m128 c1_1 = bt_splat_ps(m11, 1);
+	__m128 c2_1 = bt_splat_ps(m12, 1);
+
+	c0_1 = _mm_mul_ps(c0_1, m2v);
+	c1_1 = _mm_mul_ps(c1_1, m2v);
+	c2_1 = _mm_mul_ps(c2_1, m2v);
+
+	m2v = _mm_and_ps(m2[2].mVec128, btvFFF0fMask);
+
+	c0 = _mm_add_ps(c0, c0_1);
+	c1 = _mm_add_ps(c1, c1_1);
+	c2 = _mm_add_ps(c2, c2_1);
+
+	m10 = bt_splat_ps(m10, 2);
+	m11 = bt_splat_ps(m11, 2);
+	m12 = bt_splat_ps(m12, 2);
+
+	m10 = _mm_mul_ps(m10, m2v);
+	m11 = _mm_mul_ps(m11, m2v);
+	m12 = _mm_mul_ps(m12, m2v);
+
+	c0 = _mm_add_ps(c0, m10);
+	c1 = _mm_add_ps(c1, m11);
+	c2 = _mm_add_ps(c2, m12);
+
+	return btMatrix3x3(c0, c1, c2);
 
 #elif defined(BT_USE_NEON)
 
-    float32x4_t rv0, rv1, rv2;
-    float32x4_t v0, v1, v2;
-    float32x4_t mv0, mv1, mv2;
+	float32x4_t rv0, rv1, rv2;
+	float32x4_t v0, v1, v2;
+	float32x4_t mv0, mv1, mv2;
 
-    v0 = m1[0].mVec128;
-    v1 = m1[1].mVec128;
-    v2 = m1[2].mVec128;
+	v0 = m1[0].mVec128;
+	v1 = m1[1].mVec128;
+	v2 = m1[2].mVec128;
 
-    mv0 = (float32x4_t) vandq_s32((int32x4_t)m2[0].mVec128, btvFFF0Mask); 
-    mv1 = (float32x4_t) vandq_s32((int32x4_t)m2[1].mVec128, btvFFF0Mask); 
-    mv2 = (float32x4_t) vandq_s32((int32x4_t)m2[2].mVec128, btvFFF0Mask); 
-    
-    rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
-    rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
-    rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
-    
-    rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
-    rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
-    rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
-    
-    rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
-    rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
-    rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
+	mv0 = (float32x4_t)vandq_s32((int32x4_t)m2[0].mVec128, btvFFF0Mask);
+	mv1 = (float32x4_t)vandq_s32((int32x4_t)m2[1].mVec128, btvFFF0Mask);
+	mv2 = (float32x4_t)vandq_s32((int32x4_t)m2[2].mVec128, btvFFF0Mask);
+
+	rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
+	rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
+	rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
+
+	rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
+	rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
+	rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
+
+	rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
+	rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
+	rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
 
 	return btMatrix3x3(rv0, rv1, rv2);
-        
-#else	
+
+#else
 	return btMatrix3x3(
-		m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
-		m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
-		m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
+		m2.tdotx(m1[0]), m2.tdoty(m1[0]), m2.tdotz(m1[0]),
+		m2.tdotx(m1[1]), m2.tdoty(m1[1]), m2.tdotz(m1[1]),
+		m2.tdotx(m1[2]), m2.tdoty(m1[2]), m2.tdotz(m1[2]));
 #endif
 }
 
@@ -1318,71 +1365,67 @@ m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
 * It will test all elements are equal.  */
 SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
 {
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
 
-    __m128 c0, c1, c2;
+	__m128 c0, c1, c2;
 
-    c0 = _mm_cmpeq_ps(m1[0].mVec128, m2[0].mVec128);
-    c1 = _mm_cmpeq_ps(m1[1].mVec128, m2[1].mVec128);
-    c2 = _mm_cmpeq_ps(m1[2].mVec128, m2[2].mVec128);
-    
-    c0 = _mm_and_ps(c0, c1);
-    c0 = _mm_and_ps(c0, c2);
-
-    return (0x7 == _mm_movemask_ps((__m128)c0));
-#else 
-	return 
-    (   m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
-		m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
-		m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
+	c0 = _mm_cmpeq_ps(m1[0].mVec128, m2[0].mVec128);
+	c1 = _mm_cmpeq_ps(m1[1].mVec128, m2[1].mVec128);
+	c2 = _mm_cmpeq_ps(m1[2].mVec128, m2[2].mVec128);
+
+	c0 = _mm_and_ps(c0, c1);
+	c0 = _mm_and_ps(c0, c2);
+
+	int m = _mm_movemask_ps((__m128)c0);
+	return (0x7 == (m & 0x7));
+
+#else
+	return (m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
+			m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
+			m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2]);
 #endif
 }
 
 ///for serialization
-struct	btMatrix3x3FloatData
+struct btMatrix3x3FloatData
 {
 	btVector3FloatData m_el[3];
 };
 
 ///for serialization
-struct	btMatrix3x3DoubleData
+struct btMatrix3x3DoubleData
 {
 	btVector3DoubleData m_el[3];
 };
 
-
-	
-
-SIMD_FORCE_INLINE	void	btMatrix3x3::serialize(struct	btMatrix3x3Data& dataOut) const
+SIMD_FORCE_INLINE void btMatrix3x3::serialize(struct btMatrix3x3Data& dataOut) const
 {
-	for (int i=0;i<3;i++)
+	for (int i = 0; i < 3; i++)
 		m_el[i].serialize(dataOut.m_el[i]);
 }
 
-SIMD_FORCE_INLINE	void	btMatrix3x3::serializeFloat(struct	btMatrix3x3FloatData& dataOut) const
+SIMD_FORCE_INLINE void btMatrix3x3::serializeFloat(struct btMatrix3x3FloatData& dataOut) const
 {
-	for (int i=0;i<3;i++)
+	for (int i = 0; i < 3; i++)
 		m_el[i].serializeFloat(dataOut.m_el[i]);
 }
 
-
-SIMD_FORCE_INLINE	void	btMatrix3x3::deSerialize(const struct	btMatrix3x3Data& dataIn)
+SIMD_FORCE_INLINE void btMatrix3x3::deSerialize(const struct btMatrix3x3Data& dataIn)
 {
-	for (int i=0;i<3;i++)
+	for (int i = 0; i < 3; i++)
 		m_el[i].deSerialize(dataIn.m_el[i]);
 }
 
-SIMD_FORCE_INLINE	void	btMatrix3x3::deSerializeFloat(const struct	btMatrix3x3FloatData& dataIn)
+SIMD_FORCE_INLINE void btMatrix3x3::deSerializeFloat(const struct btMatrix3x3FloatData& dataIn)
 {
-	for (int i=0;i<3;i++)
+	for (int i = 0; i < 3; i++)
 		m_el[i].deSerializeFloat(dataIn.m_el[i]);
 }
 
-SIMD_FORCE_INLINE	void	btMatrix3x3::deSerializeDouble(const struct	btMatrix3x3DoubleData& dataIn)
+SIMD_FORCE_INLINE void btMatrix3x3::deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn)
 {
-	for (int i=0;i<3;i++)
+	for (int i = 0; i < 3; i++)
 		m_el[i].deSerializeDouble(dataIn.m_el[i]);
 }
 
-#endif //BT_MATRIX3x3_H
-
+#endif  //BT_MATRIX3x3_H
diff --git a/extern/bullet2/src/LinearMath/btMatrixX.h b/extern/bullet2/src/LinearMath/btMatrixX.h
index 42caed42eff..bb0f0dd259e 100644
--- a/extern/bullet2/src/LinearMath/btMatrixX.h
+++ b/extern/bullet2/src/LinearMath/btMatrixX.h
@@ -24,24 +24,23 @@ subject to the following restrictions:
 //#define BT_DEBUG_OSTREAM
 #ifdef BT_DEBUG_OSTREAM
 #include <iostream>
-#include <iomanip>      // std::setw
-#endif //BT_DEBUG_OSTREAM
+#include <iomanip>  // std::setw
+#endif              //BT_DEBUG_OSTREAM
 
 class btIntSortPredicate
 {
-	public:
-		bool operator() ( const int& a, const int& b ) const
-		{
-			 return a < b;
-		}
+public:
+	bool operator()(const int& a, const int& b) const
+	{
+		return a < b;
+	}
 };
 
-
 template <typename T>
 struct btVectorX
 {
-	btAlignedObjectArray<T>	m_storage;
-	
+	btAlignedObjectArray<T> m_storage;
+
 	btVectorX()
 	{
 	}
@@ -49,7 +48,7 @@ struct btVectorX
 	{
 		m_storage.resize(numRows);
 	}
-	
+
 	void resize(int rows)
 	{
 		m_storage.resize(rows);
@@ -66,13 +65,13 @@ struct btVectorX
 	{
 		return rows();
 	}
-	
+
 	T nrm2() const
 	{
 		T norm = T(0);
-		
+
 		int nn = rows();
-		
+
 		{
 			if (nn == 1)
 			{
@@ -82,11 +81,11 @@ struct btVectorX
 			{
 				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++)
+
+				for (int ix = 0; ix < nn; ix++)
 				{
 					if ((*this)[ix] != 0.0)
 					{
@@ -110,38 +109,36 @@ struct btVectorX
 			}
 		}
 		return norm;
-		
 	}
-	void	setZero()
+	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());
+			btSetZero(&m_storage[0], m_storage.size());
 		}
 	}
-	const T& operator[] (int index) const
+	const T& operator[](int index) const
 	{
 		return m_storage[index];
 	}
-	
-	T& operator[] (int 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>
@@ -151,8 +148,7 @@ struct btVectorX
  }
  */
 
-
-template <typename T> 
+template <typename T>
 struct btMatrixX
 {
 	int m_rows;
@@ -161,10 +157,10 @@ struct btMatrixX
 	int m_resizeOperations;
 	int m_setElemOperations;
 
-	btAlignedObjectArray<T>	m_storage;
-	mutable btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
+	btAlignedObjectArray<T> m_storage;
+	mutable btAlignedObjectArray<btAlignedObjectArray<int> > m_rowNonZeroElements1;
 
-	T* getBufferPointerWritable() 
+	T* getBufferPointerWritable()
 	{
 		return m_storage.size() ? &m_storage[0] : 0;
 	}
@@ -174,21 +170,21 @@ struct btMatrixX
 		return m_storage.size() ? &m_storage[0] : 0;
 	}
 	btMatrixX()
-		:m_rows(0),
-		m_cols(0),
-		m_operations(0),
-		m_resizeOperations(0),
-		m_setElemOperations(0)
+		: m_rows(0),
+		  m_cols(0),
+		  m_operations(0),
+		  m_resizeOperations(0),
+		  m_setElemOperations(0)
 	{
 	}
-	btMatrixX(int rows,int cols)
-		:m_rows(rows),
-		m_cols(cols),
-		m_operations(0),
-		m_resizeOperations(0),
-		m_setElemOperations(0)
+	btMatrixX(int rows, int cols)
+		: m_rows(rows),
+		  m_cols(cols),
+		  m_operations(0),
+		  m_resizeOperations(0),
+		  m_setElemOperations(0)
 	{
-		resize(rows,cols);
+		resize(rows, cols);
 	}
 	void resize(int rows, int cols)
 	{
@@ -197,7 +193,7 @@ struct btMatrixX
 		m_cols = cols;
 		{
 			BT_PROFILE("m_storage.resize");
-			m_storage.resize(rows*cols);
+			m_storage.resize(rows * cols);
 		}
 	}
 	int cols() const
@@ -215,108 +211,102 @@ struct btMatrixX
 	}
 	*/
 
-	void addElem(int row,int col, T val)
+	void addElem(int row, int col, T val)
 	{
 		if (val)
 		{
-			if (m_storage[col+row*m_cols]==0.f)
+			if (m_storage[col + row * m_cols] == 0.f)
 			{
-				setElem(row,col,val);
-			} else
+				setElem(row, col, val);
+			}
+			else
 			{
-				m_storage[row*m_cols+col] += val;
+				m_storage[row * m_cols + col] += val;
 			}
 		}
 	}
-	
-	
-	void setElem(int row,int col, T val)
+
+	void setElem(int row, int col, T val)
 	{
 		m_setElemOperations++;
-		m_storage[row*m_cols+col] = val;
+		m_storage[row * m_cols + col] = val;
 	}
-	
-	void mulElem(int row,int col, T val)
+
+	void mulElem(int row, int col, T val)
 	{
 		m_setElemOperations++;
 		//mul doesn't change sparsity info
 
-		m_storage[row*m_cols+col] *= val;
+		m_storage[row * m_cols + col] *= val;
 	}
-	
-	
-	
-	
+
 	void copyLowerToUpperTriangle()
 	{
-		int count=0;
-		for (int row=0;row<rows();row++)
+		int count = 0;
+		for (int row = 0; row < rows(); row++)
 		{
-			for (int col=0;col<row;col++)
+			for (int col = 0; col < row; col++)
 			{
-				setElem(col,row, (*this)(row,col));
+				setElem(col, row, (*this)(row, col));
 				count++;
-				
 			}
 		}
 		//printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows());
 	}
-	
-	const T& operator() (int row,int col) const
+
+	const T& operator()(int row, int col) const
 	{
-		return m_storage[col+row*m_cols];
+		return m_storage[col + row * m_cols];
 	}
 
-
 	void setZero()
 	{
 		{
 			BT_PROFILE("storage=0");
-			btSetZero(&m_storage[0],m_storage.size());
+			if (m_storage.size())
+			{
+				btSetZero(&m_storage[0], m_storage.size());
+			}
 			//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
 			//for (int i=0;i<m_storage.size();i++)
-	//			m_storage[i]=0;
+			//			m_storage[i]=0;
 		}
 	}
-	
+
 	void setIdentity()
 	{
 		btAssert(rows() == cols());
-		
+
 		setZero();
-		for (int row=0;row<rows();row++)
+		for (int row = 0; row < rows(); row++)
 		{
-			setElem(row,row,1);
+			setElem(row, row, 1);
 		}
 	}
 
-	
-
-	void	printMatrix(const char* msg)
+	void printMatrix(const char* msg) const
 	{
-		printf("%s ---------------------\n",msg);
-		for (int i=0;i<rows();i++)
+		printf("%s ---------------------\n", msg);
+		for (int i = 0; i < rows(); i++)
 		{
 			printf("\n");
-			for (int j=0;j<cols();j++)
+			for (int j = 0; j < cols(); j++)
 			{
-				printf("%2.1f\t",(*this)(i,j));
+				printf("%2.1f\t", (*this)(i, j));
 			}
 		}
 		printf("\n---------------------\n");
-
 	}
 
-
 	void rowComputeNonZeroElements() const
 	{
 		m_rowNonZeroElements1.resize(rows());
-		for (int i=0;i<rows();i++)
+		for (int i = 0; i < rows(); i++)
 		{
 			m_rowNonZeroElements1[i].resize(0);
-			for (int j=0;j<cols();j++)
+			for (int j = 0; j < cols(); j++)
 			{
-				if ((*this)(i,j)!=0.f)
+				if ((*this)(i, j) != 0.f)
 				{
 					m_rowNonZeroElements1[i].push_back(j);
 				}
@@ -326,54 +316,50 @@ struct btMatrixX
 	btMatrixX transpose() const
 	{
 		//transpose is optimized for sparse matrices
-		btMatrixX tr(m_cols,m_rows);
+		btMatrixX tr(m_cols, m_rows);
 		tr.setZero();
-		for (int i=0;i<m_cols;i++)
-			for (int j=0;j<m_rows;j++)
+		for (int i = 0; i < m_cols; i++)
+			for (int j = 0; j < m_rows; j++)
 			{
-				T v = (*this)(j,i);
+				T v = (*this)(j, i);
 				if (v)
 				{
-					tr.setElem(i,j,v);
+					tr.setElem(i, j, v);
 				}
 			}
 		return tr;
 	}
 
-
 	btMatrixX operator*(const btMatrixX& other)
 	{
 		//btMatrixX*btMatrixX implementation, brute force
 		btAssert(cols() == other.rows());
 
-		btMatrixX res(rows(),other.cols());
+		btMatrixX res(rows(), other.cols());
 		res.setZero();
-//		BT_PROFILE("btMatrixX mul");
-		for (int j=0; j < res.cols(); ++j)
+		//		BT_PROFILE("btMatrixX mul");
+		for (int i = 0; i < rows(); ++i)
 		{
 			{
-				for (int i=0; i < res.rows(); ++i)
+				for (int j = 0; j < other.cols(); ++j)
 				{
-					T dotProd=0;
-//					T dotProd2=0;
-					//int waste=0,waste2=0;
-
+					T dotProd = 0;
 					{
-//						bool useOtherCol = true;
 						{
-							for (int v=0;v<rows();v++)
+							int c = cols();
+
+							for (int k = 0; k < c; k++)
 							{
-								T w = (*this)(i,v);
-								if (other(v,j)!=0.f)
+								T w = (*this)(i, k);
+								if (other(k, j) != 0.f)
 								{
-									dotProd+=w*other(v,j);	
+									dotProd += w * other(k, j);
 								}
-						
 							}
 						}
 					}
 					if (dotProd)
-						res.setElem(i,j,dotProd);
+						res.setElem(i, j, dotProd);
 				}
 			}
 		}
@@ -381,174 +367,166 @@ struct btMatrixX
 	}
 
 	// this assumes the 4th and 8th rows of B and C are zero.
-	void multiplyAdd2_p8r (const btScalar *B, const btScalar *C,  int numRows,  int numRowsOther ,int row, int col)
+	void multiplyAdd2_p8r(const btScalar* B, const btScalar* C, int numRows, int numRowsOther, int row, int col)
 	{
-		const btScalar *bb = B;
-		for ( int i = 0;i<numRows;i++)
+		const btScalar* bb = B;
+		for (int i = 0; i < numRows; i++)
 		{
-			const btScalar *cc = C;
-			for ( int j = 0;j<numRowsOther;j++)
+			const btScalar* cc = C;
+			for (int j = 0; j < numRowsOther; j++)
 			{
 				btScalar sum;
-				sum  = bb[0]*cc[0];
-				sum += bb[1]*cc[1];
-				sum += bb[2]*cc[2];
-				sum += bb[4]*cc[4];
-				sum += bb[5]*cc[5];
-				sum += bb[6]*cc[6];
-				addElem(row+i,col+j,sum);
+				sum = bb[0] * cc[0];
+				sum += bb[1] * cc[1];
+				sum += bb[2] * cc[2];
+				sum += bb[4] * cc[4];
+				sum += bb[5] * cc[5];
+				sum += bb[6] * cc[6];
+				addElem(row + i, col + j, sum);
 				cc += 8;
 			}
 			bb += 8;
 		}
 	}
 
-	void multiply2_p8r (const btScalar *B, const btScalar *C,  int numRows,  int numRowsOther, int row, int col)
+	void multiply2_p8r(const btScalar* B, const btScalar* C, int numRows, int numRowsOther, int row, int col)
 	{
-		btAssert (numRows>0 && numRowsOther>0 && B && C);
-		const btScalar *bb = B;
-		for ( int i = 0;i<numRows;i++)
+		btAssert(numRows > 0 && numRowsOther > 0 && B && C);
+		const btScalar* bb = B;
+		for (int i = 0; i < numRows; i++)
 		{
-			const btScalar *cc = C;
-			for ( int j = 0;j<numRowsOther;j++)
+			const btScalar* cc = C;
+			for (int j = 0; j < numRowsOther; j++)
 			{
 				btScalar sum;
-				sum  = bb[0]*cc[0];
-				sum += bb[1]*cc[1];
-				sum += bb[2]*cc[2];
-				sum += bb[4]*cc[4];
-				sum += bb[5]*cc[5];
-				sum += bb[6]*cc[6];
-				setElem(row+i,col+j,sum);
+				sum = bb[0] * cc[0];
+				sum += bb[1] * cc[1];
+				sum += bb[2] * cc[2];
+				sum += bb[4] * cc[4];
+				sum += bb[5] * cc[5];
+				sum += bb[6] * cc[6];
+				setElem(row + i, col + j, sum);
 				cc += 8;
 			}
 			bb += 8;
 		}
 	}
-	
-	void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const T value)
+
+	void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const T value)
 	{
-		int numRows = rowend+1-rowstart;
-		int numCols = colend+1-colstart;
-		
-		for (int row=0;row<numRows;row++)
+		int numRows = rowend + 1 - rowstart;
+		int numCols = colend + 1 - colstart;
+
+		for (int row = 0; row < numRows; row++)
 		{
-			for (int col=0;col<numCols;col++)
+			for (int col = 0; col < numCols; col++)
 			{
-				setElem(rowstart+row,colstart+col,value);
+				setElem(rowstart + row, colstart + col, value);
 			}
 		}
 	}
-	
-	void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btMatrixX& block)
+
+	void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btMatrixX& block)
 	{
-		btAssert(rowend+1-rowstart == block.rows());
-		btAssert(colend+1-colstart == block.cols());
-		for (int row=0;row<block.rows();row++)
+		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++)
+			for (int col = 0; col < block.cols(); col++)
 			{
-				setElem(rowstart+row,colstart+col,block(row,col));
+				setElem(rowstart + row, colstart + col, block(row, col));
 			}
 		}
 	}
-	void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btVectorX<T>& block)
+	void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btVectorX<T>& block)
 	{
-		btAssert(rowend+1-rowstart == block.rows());
-		btAssert(colend+1-colstart == block.cols());
-		for (int row=0;row<block.rows();row++)
+		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++)
+			for (int col = 0; col < block.cols(); col++)
 			{
-				setElem(rowstart+row,colstart+col,block[row]);
+				setElem(rowstart + row, colstart + col, block[row]);
 			}
 		}
 	}
-	
-	
+
 	btMatrixX negative()
 	{
-		btMatrixX neg(rows(),cols());
-		for (int i=0;i<rows();i++)
-			for (int j=0;j<cols();j++)
+		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);
+				T v = (*this)(i, j);
+				neg.setElem(i, j, -v);
 			}
 		return neg;
 	}
-
 };
 
-
-
 typedef btMatrixX<float> btMatrixXf;
 typedef btVectorX<float> btVectorXf;
 
 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)
+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++)
 	{
-		
-		os << " [";
-		//printf("%s ---------------------\n",msg);
-		for (int i=0;i<mat.rows();i++)
+		for (int j = 0; j < mat.cols(); j++)
 		{
-			for (int j=0;j<mat.cols();j++)
-			{
-				os << std::setw(12) << mat(i,j);
-			}
-			if (i!=mat.rows()-1)
-				os << std::endl << "  ";
+			os << std::setw(12) << mat(i, j);
 		}
-		os << " ]";
-		//printf("\n---------------------\n");
-
-		return os;
+		if (i != mat.rows() - 1)
+			os << std::endl
+			   << "  ";
 	}
-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");
+	os << " ]";
+	//printf("\n---------------------\n");
 
-		return os;
+	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");
 
-#endif //BT_DEBUG_OSTREAM
+	return os;
+}
 
+#endif  //BT_DEBUG_OSTREAM
 
 inline void setElem(btMatrixXd& mat, int row, int col, double val)
 {
-	mat.setElem(row,col,val);
+	mat.setElem(row, col, val);
 }
 
 inline void setElem(btMatrixXf& mat, int row, int col, float val)
 {
-	mat.setElem(row,col,val);
+	mat.setElem(row, col, val);
 }
 
 #ifdef BT_USE_DOUBLE_PRECISION
-	#define btVectorXu btVectorXd
-	#define btMatrixXu btMatrixXd
+#define btVectorXu btVectorXd
+#define btMatrixXu btMatrixXd
 #else
-	#define btVectorXu btVectorXf
-	#define btMatrixXu btMatrixXf
-#endif //BT_USE_DOUBLE_PRECISION
-
-
+#define btVectorXu btVectorXf
+#define btMatrixXu btMatrixXf
+#endif  //BT_USE_DOUBLE_PRECISION
 
-#endif//BT_MATRIX_H_H
+#endif  //BT_MATRIX_H_H
diff --git a/extern/bullet2/src/LinearMath/btMinMax.h b/extern/bullet2/src/LinearMath/btMinMax.h
index 5b436e9ba4e..92fea0275ab 100644
--- a/extern/bullet2/src/LinearMath/btMinMax.h
+++ b/extern/bullet2/src/LinearMath/btMinMax.h
@@ -12,60 +12,58 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef BT_GEN_MINMAX_H
 #define BT_GEN_MINMAX_H
 
 #include "btScalar.h"
 
 template <class T>
-SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) 
+SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b)
 {
-  return a < b ? a : b ;
+	return a < b ? a : b;
 }
 
 template <class T>
-SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b) 
+SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b)
 {
-  return  a > b ? a : b;
+	return a > b ? a : b;
 }
 
 template <class T>
-SIMD_FORCE_INLINE const T& btClamped(const T& a, const T& lb, const T& ub) 
+SIMD_FORCE_INLINE const T& btClamped(const T& a, const T& lb, const T& ub)
 {
-	return a < lb ? lb : (ub < a ? ub : a); 
+	return a < lb ? lb : (ub < a ? ub : a);
 }
 
 template <class T>
-SIMD_FORCE_INLINE void btSetMin(T& a, const T& b) 
+SIMD_FORCE_INLINE void btSetMin(T& a, const T& b)
 {
-    if (b < a) 
+	if (b < a)
 	{
 		a = b;
 	}
 }
 
 template <class T>
-SIMD_FORCE_INLINE void btSetMax(T& a, const T& b) 
+SIMD_FORCE_INLINE void btSetMax(T& a, const T& b)
 {
-    if (a < b) 
+	if (a < b)
 	{
 		a = b;
 	}
 }
 
 template <class T>
-SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub) 
+SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub)
 {
-	if (a < lb) 
+	if (a < lb)
 	{
-		a = lb; 
+		a = lb;
 	}
-	else if (ub < a) 
+	else if (ub < a)
 	{
 		a = ub;
 	}
 }
 
-#endif //BT_GEN_MINMAX_H
+#endif  //BT_GEN_MINMAX_H
diff --git a/extern/bullet2/src/LinearMath/btModifiedGramSchmidt.h b/extern/bullet2/src/LinearMath/btModifiedGramSchmidt.h
new file mode 100644
index 00000000000..33bab8d650d
--- /dev/null
+++ b/extern/bullet2/src/LinearMath/btModifiedGramSchmidt.h
@@ -0,0 +1,83 @@
+//
+//  btModifiedGramSchmidt.h
+//  LinearMath
+//
+//  Created by Xuchen Han on 4/4/20.
+//
+
+#ifndef btModifiedGramSchmidt_h
+#define btModifiedGramSchmidt_h
+
+#include "btReducedVector.h"
+#include "btAlignedObjectArray.h"
+#include <iostream>
+#include <cmath>
+template<class TV>
+class btModifiedGramSchmidt
+{
+public:
+    btAlignedObjectArray<TV> m_in;
+    btAlignedObjectArray<TV> m_out;
+    
+    btModifiedGramSchmidt(const btAlignedObjectArray<TV>& vecs): m_in(vecs)
+    {
+        m_out.resize(0);
+    }
+    
+    void solve()
+    {
+        m_out.resize(m_in.size());
+        for (int i = 0; i < m_in.size(); ++i)
+        {
+//            printf("========= starting %d ==========\n", i);
+            TV v(m_in[i]);
+//            v.print();
+            for (int j = 0; j < i; ++j)
+            {
+                v = v - v.proj(m_out[j]);
+//                v.print();
+            }
+            v.normalize();
+            m_out[i] = v;
+//            v.print();
+        }
+    }
+    
+    void test()
+    {
+        std::cout << SIMD_EPSILON << std::endl;
+        printf("=======inputs=========\n");
+        for (int i = 0; i < m_out.size(); ++i)
+        {
+            m_in[i].print();
+        }
+        printf("=======output=========\n");
+        for (int i = 0; i < m_out.size(); ++i)
+        {
+            m_out[i].print();
+        }
+        btScalar eps = SIMD_EPSILON;
+        for (int i = 0; i < m_out.size(); ++i)
+        {
+            for (int j = 0; j < m_out.size(); ++j)
+            {
+                if (i == j)
+                {
+                    if (std::abs(1.0-m_out[i].dot(m_out[j])) > eps)// && std::abs(m_out[i].dot(m_out[j])) > eps)
+                    {
+                        printf("vec[%d] is not unit, norm squared = %f\n", i,m_out[i].dot(m_out[j]));
+                    }
+                }
+                else
+                {
+                    if (std::abs(m_out[i].dot(m_out[j])) > eps)
+                    {
+                        printf("vec[%d] and vec[%d] is not orthogonal, dot product = %f\n", i, j, m_out[i].dot(m_out[j]));
+                    }
+                }
+            }
+        }
+    }
+};
+template class btModifiedGramSchmidt<btReducedVector>;
+#endif /* btModifiedGramSchmidt_h */
diff --git a/extern/bullet2/src/LinearMath/btMotionState.h b/extern/bullet2/src/LinearMath/btMotionState.h
index 94318140902..ae6a51611d8 100644
--- a/extern/bullet2/src/LinearMath/btMotionState.h
+++ b/extern/bullet2/src/LinearMath/btMotionState.h
@@ -20,21 +20,17 @@ subject to the following restrictions:
 
 ///The btMotionState interface class allows the dynamics world to synchronize and interpolate the updated world transforms with graphics
 ///For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation)
-class	btMotionState
+class btMotionState
 {
-	public:
-		
-		virtual ~btMotionState()
-		{
-			
-		}
-		
-		virtual void	getWorldTransform(btTransform& worldTrans ) const =0;
-
-		//Bullet only calls the update of worldtransform for active objects
-		virtual void	setWorldTransform(const btTransform& worldTrans)=0;
-		
-	
+public:
+	virtual ~btMotionState()
+	{
+	}
+
+	virtual void getWorldTransform(btTransform& worldTrans) const = 0;
+
+	//Bullet only calls the update of worldtransform for active objects
+	virtual void setWorldTransform(const btTransform& worldTrans) = 0;
 };
 
-#endif //BT_MOTIONSTATE_H
+#endif  //BT_MOTIONSTATE_H
diff --git a/extern/bullet2/src/LinearMath/btPolarDecomposition.cpp b/extern/bullet2/src/LinearMath/btPolarDecomposition.cpp
index a4dca7fdd40..d9c72a8014a 100644
--- a/extern/bullet2/src/LinearMath/btPolarDecomposition.cpp
+++ b/extern/bullet2/src/LinearMath/btPolarDecomposition.cpp
@@ -3,97 +3,92 @@
 
 namespace
 {
-  btScalar abs_column_sum(const btMatrix3x3& a, int i)
-  {
-    return btFabs(a[0][i]) + btFabs(a[1][i]) + btFabs(a[2][i]);
-  }
-
-  btScalar abs_row_sum(const btMatrix3x3& a, int i)
-  {
-    return btFabs(a[i][0]) + btFabs(a[i][1]) + btFabs(a[i][2]);
-  }
-
-  btScalar p1_norm(const btMatrix3x3& a)
-  {
-    const btScalar sum0 = abs_column_sum(a,0);
-    const btScalar sum1 = abs_column_sum(a,1);
-    const btScalar sum2 = abs_column_sum(a,2);
-    return btMax(btMax(sum0, sum1), sum2);
-  }
-
-  btScalar pinf_norm(const btMatrix3x3& a)
-  {
-    const btScalar sum0 = abs_row_sum(a,0);
-    const btScalar sum1 = abs_row_sum(a,1);
-    const btScalar sum2 = abs_row_sum(a,2);
-    return btMax(btMax(sum0, sum1), sum2);
-  }
+btScalar abs_column_sum(const btMatrix3x3& a, int i)
+{
+	return btFabs(a[0][i]) + btFabs(a[1][i]) + btFabs(a[2][i]);
+}
+
+btScalar abs_row_sum(const btMatrix3x3& a, int i)
+{
+	return btFabs(a[i][0]) + btFabs(a[i][1]) + btFabs(a[i][2]);
+}
+
+btScalar p1_norm(const btMatrix3x3& a)
+{
+	const btScalar sum0 = abs_column_sum(a, 0);
+	const btScalar sum1 = abs_column_sum(a, 1);
+	const btScalar sum2 = abs_column_sum(a, 2);
+	return btMax(btMax(sum0, sum1), sum2);
 }
 
-const btScalar btPolarDecomposition::DEFAULT_TOLERANCE = btScalar(0.0001);
-const unsigned int btPolarDecomposition::DEFAULT_MAX_ITERATIONS = 16;
+btScalar pinf_norm(const btMatrix3x3& a)
+{
+	const btScalar sum0 = abs_row_sum(a, 0);
+	const btScalar sum1 = abs_row_sum(a, 1);
+	const btScalar sum2 = abs_row_sum(a, 2);
+	return btMax(btMax(sum0, sum1), sum2);
+}
+}  // namespace
 
 btPolarDecomposition::btPolarDecomposition(btScalar tolerance, unsigned int maxIterations)
-: m_tolerance(tolerance)
-, m_maxIterations(maxIterations)
+	: m_tolerance(tolerance), m_maxIterations(maxIterations)
 {
 }
 
 unsigned int btPolarDecomposition::decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const
 {
-  // Use the 'u' and 'h' matrices for intermediate calculations
-  u = a;
-  h = a.inverse();
-
-  for (unsigned int i = 0; i < m_maxIterations; ++i)
-  {
-    const btScalar h_1 = p1_norm(h);
-    const btScalar h_inf = pinf_norm(h);
-    const btScalar u_1 = p1_norm(u);
-    const btScalar u_inf = pinf_norm(u);
-
-    const btScalar h_norm = h_1 * h_inf;
-    const btScalar u_norm = u_1 * u_inf;
-
-    // The matrix is effectively singular so we cannot invert it
-    if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm))
-      break;
-
-    const btScalar gamma = btPow(h_norm / u_norm, 0.25f);
-    const btScalar inv_gamma = btScalar(1.0) / gamma;
-
-    // Determine the delta to 'u'
-    const btMatrix3x3 delta = (u * (gamma - btScalar(2.0)) + h.transpose() * inv_gamma) * btScalar(0.5);
-
-    // Update the matrices
-    u += delta;
-    h = u.inverse();
-
-    // Check for convergence
-    if (p1_norm(delta) <= m_tolerance * u_1)
-    {
-      h = u.transpose() * a;
-      h = (h + h.transpose()) * 0.5;
-      return i;
-    }
-  }
-
-  // The algorithm has failed to converge to the specified tolerance, but we
-  // want to make sure that the matrices returned are in the right form.
-  h = u.transpose() * a;
-  h = (h + h.transpose()) * 0.5;
-
-  return m_maxIterations;
+	// Use the 'u' and 'h' matrices for intermediate calculations
+	u = a;
+	h = a.inverse();
+
+	for (unsigned int i = 0; i < m_maxIterations; ++i)
+	{
+		const btScalar h_1 = p1_norm(h);
+		const btScalar h_inf = pinf_norm(h);
+		const btScalar u_1 = p1_norm(u);
+		const btScalar u_inf = pinf_norm(u);
+
+		const btScalar h_norm = h_1 * h_inf;
+		const btScalar u_norm = u_1 * u_inf;
+
+		// The matrix is effectively singular so we cannot invert it
+		if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm))
+			break;
+
+		const btScalar gamma = btPow(h_norm / u_norm, 0.25f);
+		const btScalar inv_gamma = btScalar(1.0) / gamma;
+
+		// Determine the delta to 'u'
+		const btMatrix3x3 delta = (u * (gamma - btScalar(2.0)) + h.transpose() * inv_gamma) * btScalar(0.5);
+
+		// Update the matrices
+		u += delta;
+		h = u.inverse();
+
+		// Check for convergence
+		if (p1_norm(delta) <= m_tolerance * u_1)
+		{
+			h = u.transpose() * a;
+			h = (h + h.transpose()) * 0.5;
+			return i;
+		}
+	}
+
+	// The algorithm has failed to converge to the specified tolerance, but we
+	// want to make sure that the matrices returned are in the right form.
+	h = u.transpose() * a;
+	h = (h + h.transpose()) * 0.5;
+
+	return m_maxIterations;
 }
 
 unsigned int btPolarDecomposition::maxIterations() const
 {
-  return m_maxIterations;
+	return m_maxIterations;
 }
 
 unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h)
 {
-  static btPolarDecomposition polar;
-  return polar.decompose(a, u, h);
+	static btPolarDecomposition polar;
+	return polar.decompose(a, u, h);
 }
-
diff --git a/extern/bullet2/src/LinearMath/btPolarDecomposition.h b/extern/bullet2/src/LinearMath/btPolarDecomposition.h
index 56156676415..bf29140a143 100644
--- a/extern/bullet2/src/LinearMath/btPolarDecomposition.h
+++ b/extern/bullet2/src/LinearMath/btPolarDecomposition.h
@@ -13,11 +13,8 @@
  */
 class btPolarDecomposition
 {
-  public:
-    static const btScalar DEFAULT_TOLERANCE;
-    static const unsigned int DEFAULT_MAX_ITERATIONS;
-
-    /**
+public:
+	/**
      * Creates an instance with optional parameters.
      *
      * @param tolerance     - the tolerance used to determine convergence of the
@@ -25,10 +22,10 @@ class btPolarDecomposition
      * @param maxIterations - the maximum number of iterations used to achieve
      *                        convergence
      */
-    btPolarDecomposition(btScalar tolerance = DEFAULT_TOLERANCE, 
-      unsigned int maxIterations = DEFAULT_MAX_ITERATIONS);
+	btPolarDecomposition(btScalar tolerance = btScalar(0.0001),
+						 unsigned int maxIterations = 16);
 
-    /**
+	/**
      * Decomposes a matrix into orthogonal and symmetric, positive-definite
      * parts. If the number of iterations returned by this function is equal to
      * the maximum number of iterations, the algorithm has failed to converge.
@@ -39,19 +36,19 @@ class btPolarDecomposition
      *
      * @return the number of iterations performed by the algorithm.
      */
-    unsigned int decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const; 
+	unsigned int decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const;
 
-    /**
+	/**
      * Returns the maximum number of iterations that this algorithm will perform
      * to achieve convergence.
      *
      * @return maximum number of iterations
      */
-    unsigned int maxIterations() const;
+	unsigned int maxIterations() const;
 
-  private:
-    btScalar m_tolerance;
-    unsigned int m_maxIterations;
+private:
+	btScalar m_tolerance;
+	unsigned int m_maxIterations;
 };
 
 /**
@@ -67,7 +64,6 @@ class btPolarDecomposition
  *
  * @return the number of iterations performed by the algorithm.
  */
-unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h); 
-
-#endif // POLARDECOMPOSITION_H
+unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h);
 
+#endif  // POLARDECOMPOSITION_H
diff --git a/extern/bullet2/src/LinearMath/btPoolAllocator.h b/extern/bullet2/src/LinearMath/btPoolAllocator.h
index ef2084537a2..4e7b49660a9 100644
--- a/extern/bullet2/src/LinearMath/btPoolAllocator.h
+++ b/extern/bullet2/src/LinearMath/btPoolAllocator.h
@@ -12,47 +12,48 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef _BT_POOL_ALLOCATOR_H
 #define _BT_POOL_ALLOCATOR_H
 
 #include "btScalar.h"
 #include "btAlignedAllocator.h"
+#include "btThreads.h"
 
 ///The btPoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately.
 class btPoolAllocator
 {
-	int				m_elemSize;
-	int				m_maxElements;
-	int				m_freeCount;
-	void*			m_firstFree;
-	unsigned char*	m_pool;
+	int m_elemSize;
+	int m_maxElements;
+	int m_freeCount;
+	void* m_firstFree;
+	unsigned char* m_pool;
+	btSpinMutex m_mutex;  // only used if BT_THREADSAFE
 
 public:
-
 	btPoolAllocator(int elemSize, int maxElements)
-		:m_elemSize(elemSize),
-		m_maxElements(maxElements)
+		: m_elemSize(elemSize),
+		  m_maxElements(maxElements)
 	{
-		m_pool = (unsigned char*) btAlignedAlloc( static_cast<unsigned int>(m_elemSize*m_maxElements),16);
+		m_pool = (unsigned char*)btAlignedAlloc(static_cast<unsigned int>(m_elemSize * m_maxElements), 16);
 
 		unsigned char* p = m_pool;
-        m_firstFree = p;
-        m_freeCount = m_maxElements;
-        int count = m_maxElements;
-        while (--count) {
-            *(void**)p = (p + m_elemSize);
-            p += m_elemSize;
-        }
-        *(void**)p = 0;
-    }
+		m_firstFree = p;
+		m_freeCount = m_maxElements;
+		int count = m_maxElements;
+		while (--count)
+		{
+			*(void**)p = (p + m_elemSize);
+			p += m_elemSize;
+		}
+		*(void**)p = 0;
+	}
 
 	~btPoolAllocator()
 	{
-		btAlignedFree( m_pool);
+		btAlignedFree(m_pool);
 	}
 
-	int	getFreeCount() const
+	int getFreeCount() const
 	{
 		return m_freeCount;
 	}
@@ -67,21 +68,27 @@ public:
 		return m_maxElements;
 	}
 
-	void*	allocate(int size)
+	void* allocate(int size)
 	{
 		// release mode fix
 		(void)size;
-		btAssert(!size || size<=m_elemSize);
-		btAssert(m_freeCount>0);
-        void* result = m_firstFree;
-        m_firstFree = *(void**)m_firstFree;
-        --m_freeCount;
-        return result;
+		btMutexLock(&m_mutex);
+		btAssert(!size || size <= m_elemSize);
+		//btAssert(m_freeCount>0);  // should return null if all full
+		void* result = m_firstFree;
+		if (NULL != m_firstFree)
+		{
+			m_firstFree = *(void**)m_firstFree;
+			--m_freeCount;
+		}
+		btMutexUnlock(&m_mutex);
+		return result;
 	}
 
 	bool validPtr(void* ptr)
 	{
-		if (ptr) {
+		if (ptr)
+		{
 			if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize))
 			{
 				return true;
@@ -90,32 +97,34 @@ public:
 		return false;
 	}
 
-	void	freeMemory(void* ptr)
+	void freeMemory(void* ptr)
 	{
-		 if (ptr) {
-            btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
-
-            *(void**)ptr = m_firstFree;
-            m_firstFree = ptr;
-            ++m_freeCount;
-        }
+		if (ptr)
+		{
+			btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
+
+			btMutexLock(&m_mutex);
+			*(void**)ptr = m_firstFree;
+			m_firstFree = ptr;
+			++m_freeCount;
+			btMutexUnlock(&m_mutex);
+		}
 	}
 
-	int	getElementSize() const
+	int getElementSize() const
 	{
 		return m_elemSize;
 	}
 
-	unsigned char*	getPoolAddress()
+	unsigned char* getPoolAddress()
 	{
 		return m_pool;
 	}
 
-	const unsigned char*	getPoolAddress() const
+	const unsigned char* getPoolAddress() const
 	{
 		return m_pool;
 	}
-
 };
 
-#endif //_BT_POOL_ALLOCATOR_H
+#endif  //_BT_POOL_ALLOCATOR_H
diff --git a/extern/bullet2/src/LinearMath/btQuadWord.h b/extern/bullet2/src/LinearMath/btQuadWord.h
index fcfb3be4447..ab2d3175ad6 100644
--- a/extern/bullet2/src/LinearMath/btQuadWord.h
+++ b/extern/bullet2/src/LinearMath/btQuadWord.h
@@ -12,18 +12,13 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_SIMD_QUADWORD_H
 #define BT_SIMD_QUADWORD_H
 
 #include "btScalar.h"
 #include "btMinMax.h"
 
-
-
-
-
-#if defined (__CELLOS_LV2) && defined (__SPU__)
+#if defined(__CELLOS_LV2) && defined(__SPU__)
 #include <altivec.h>
 #endif
 
@@ -31,51 +26,53 @@ subject to the following restrictions:
  * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
  */
 #ifndef USE_LIBSPE2
-ATTRIBUTE_ALIGNED16(class) btQuadWord
+ATTRIBUTE_ALIGNED16(class)
+btQuadWord
 #else
 class btQuadWord
 #endif
 {
 protected:
-
-#if defined (__SPU__) && defined (__CELLOS_LV2__)
+#if defined(__SPU__) && defined(__CELLOS_LV2__)
 	union {
 		vec_float4 mVec128;
-		btScalar	m_floats[4];
+		btScalar m_floats[4];
 	};
+
 public:
-	vec_float4	get128() const
+	vec_float4 get128() const
 	{
 		return mVec128;
 	}
+
 protected:
-#else //__CELLOS_LV2__ __SPU__
+#else  //__CELLOS_LV2__ __SPU__
 
-#if defined(BT_USE_SSE) || defined(BT_USE_NEON) 
+#if defined(BT_USE_SSE) || defined(BT_USE_NEON)
 	union {
 		btSimdFloat4 mVec128;
-		btScalar	m_floats[4];
+		btScalar m_floats[4];
 	};
+
 public:
-	SIMD_FORCE_INLINE	btSimdFloat4	get128() const
+	SIMD_FORCE_INLINE btSimdFloat4 get128() const
 	{
 		return mVec128;
 	}
-	SIMD_FORCE_INLINE	void	set128(btSimdFloat4 v128)
+	SIMD_FORCE_INLINE void set128(btSimdFloat4 v128)
 	{
 		mVec128 = v128;
 	}
 #else
-	btScalar	m_floats[4];
-#endif // BT_USE_SSE
+	btScalar m_floats[4];
+#endif  // BT_USE_SSE
 
-#endif //__CELLOS_LV2__ __SPU__
+#endif  //__CELLOS_LV2__ __SPU__
 
-	public:
-  
+public:
 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
 
-	// Set Vector 
+	// Set Vector
 	SIMD_FORCE_INLINE btQuadWord(const btSimdFloat4 vec)
 	{
 		mVec128 = vec;
@@ -88,157 +85,154 @@ public:
 	}
 
 	// Assignment Operator
-	SIMD_FORCE_INLINE btQuadWord& 
-	operator=(const btQuadWord& v) 
+	SIMD_FORCE_INLINE btQuadWord&
+	operator=(const btQuadWord& v)
 	{
 		mVec128 = v.mVec128;
-		
+
 		return *this;
 	}
-	
+
 #endif
 
-  /**@brief Return the x value */
-		SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
-  /**@brief Return the y value */
-		SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
-  /**@brief Return the z value */
-		SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
-  /**@brief Set the x value */
-		SIMD_FORCE_INLINE void	setX(btScalar _x) { m_floats[0] = _x;};
-  /**@brief Set the y value */
-		SIMD_FORCE_INLINE void	setY(btScalar _y) { m_floats[1] = _y;};
-  /**@brief Set the z value */
-		SIMD_FORCE_INLINE void	setZ(btScalar _z) { m_floats[2] = _z;};
-  /**@brief Set the w value */
-		SIMD_FORCE_INLINE void	setW(btScalar _w) { m_floats[3] = _w;};
-  /**@brief Return the x value */
-		SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
-  /**@brief Return the y value */
-		SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
-  /**@brief Return the z value */
-		SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
-  /**@brief Return the w value */
-		SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
-
-	//SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_floats[0])[i];	}      
+	/**@brief Return the x value */
+	SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
+	/**@brief Return the y value */
+	SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
+	/**@brief Return the z value */
+	SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
+	/**@brief Set the x value */
+	SIMD_FORCE_INLINE void setX(btScalar _x) { m_floats[0] = _x; };
+	/**@brief Set the y value */
+	SIMD_FORCE_INLINE void setY(btScalar _y) { m_floats[1] = _y; };
+	/**@brief Set the z value */
+	SIMD_FORCE_INLINE void setZ(btScalar _z) { m_floats[2] = _z; };
+	/**@brief Set the w value */
+	SIMD_FORCE_INLINE void setW(btScalar _w) { m_floats[3] = _w; };
+	/**@brief Return the x value */
+	SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
+	/**@brief Return the y value */
+	SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
+	/**@brief Return the z value */
+	SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
+	/**@brief Return the w value */
+	SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
+
+	//SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_floats[0])[i];	}
 	//SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
 	///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
-	SIMD_FORCE_INLINE	operator       btScalar *()       { return &m_floats[0]; }
-	SIMD_FORCE_INLINE	operator const btScalar *() const { return &m_floats[0]; }
+	SIMD_FORCE_INLINE operator btScalar*() { return &m_floats[0]; }
+	SIMD_FORCE_INLINE operator const btScalar*() const { return &m_floats[0]; }
 
-	SIMD_FORCE_INLINE	bool	operator==(const btQuadWord& other) const
+	SIMD_FORCE_INLINE bool operator==(const btQuadWord& other) const
 	{
 #ifdef BT_USE_SSE
-        return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
-#else 
-		return ((m_floats[3]==other.m_floats[3]) && 
-                (m_floats[2]==other.m_floats[2]) && 
-                (m_floats[1]==other.m_floats[1]) && 
-                (m_floats[0]==other.m_floats[0]));
+		return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
+#else
+		return ((m_floats[3] == other.m_floats[3]) &&
+				(m_floats[2] == other.m_floats[2]) &&
+				(m_floats[1] == other.m_floats[1]) &&
+				(m_floats[0] == other.m_floats[0]));
 #endif
 	}
 
-	SIMD_FORCE_INLINE	bool	operator!=(const btQuadWord& other) const
+	SIMD_FORCE_INLINE bool operator!=(const btQuadWord& other) const
 	{
 		return !(*this == other);
 	}
 
-  /**@brief Set x,y,z and zero w 
+	/**@brief Set x,y,z and zero w 
    * @param x Value of x
    * @param y Value of y
    * @param z Value of z
    */
-		SIMD_FORCE_INLINE void 	setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z)
-		{
-			m_floats[0]=_x;
-			m_floats[1]=_y;
-			m_floats[2]=_z;
-			m_floats[3] = 0.f;
-		}
+	SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z)
+	{
+		m_floats[0] = _x;
+		m_floats[1] = _y;
+		m_floats[2] = _z;
+		m_floats[3] = 0.f;
+	}
 
-/*		void getValue(btScalar *m) const 
+	/*		void getValue(btScalar *m) const 
 		{
 			m[0] = m_floats[0];
 			m[1] = m_floats[1];
 			m[2] = m_floats[2];
 		}
 */
-/**@brief Set the values 
+	/**@brief Set the values 
    * @param x Value of x
    * @param y Value of y
    * @param z Value of z
    * @param w Value of w
    */
-		SIMD_FORCE_INLINE void	setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
-		{
-			m_floats[0]=_x;
-			m_floats[1]=_y;
-			m_floats[2]=_z;
-			m_floats[3]=_w;
-		}
-  /**@brief No initialization constructor */
-		SIMD_FORCE_INLINE btQuadWord()
-		//	:m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.))
-		{
-		}
- 
-  /**@brief Three argument constructor (zeros w)
+	SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w)
+	{
+		m_floats[0] = _x;
+		m_floats[1] = _y;
+		m_floats[2] = _z;
+		m_floats[3] = _w;
+	}
+	/**@brief No initialization constructor */
+	SIMD_FORCE_INLINE btQuadWord()
+	//	:m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.))
+	{
+	}
+
+	/**@brief Three argument constructor (zeros w)
    * @param x Value of x
    * @param y Value of y
    * @param z Value of z
    */
-		SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z)		
-		{
-			m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
-		}
+	SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z)
+	{
+		m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
+	}
 
-/**@brief Initializing constructor
+	/**@brief Initializing constructor
    * @param x Value of x
    * @param y Value of y
    * @param z Value of z
    * @param w Value of w
    */
-		SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w) 
-		{
-			m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
-		}
+	SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w)
+	{
+		m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
+	}
 
-  /**@brief Set each element to the max of the current values and the values of another btQuadWord
+	/**@brief Set each element to the max of the current values and the values of another btQuadWord
    * @param other The other btQuadWord to compare with 
    */
-		SIMD_FORCE_INLINE void	setMax(const btQuadWord& other)
-		{
-        #ifdef BT_USE_SSE
-            mVec128 = _mm_max_ps(mVec128, other.mVec128);
-        #elif defined(BT_USE_NEON)
-            mVec128 = vmaxq_f32(mVec128, other.mVec128);
-        #else
-        	btSetMax(m_floats[0], other.m_floats[0]);
-			btSetMax(m_floats[1], other.m_floats[1]);
-			btSetMax(m_floats[2], other.m_floats[2]);
-			btSetMax(m_floats[3], other.m_floats[3]);
-		#endif
-        }
-  /**@brief Set each element to the min of the current values and the values of another btQuadWord
+	SIMD_FORCE_INLINE void setMax(const btQuadWord& other)
+	{
+#ifdef BT_USE_SSE
+		mVec128 = _mm_max_ps(mVec128, other.mVec128);
+#elif defined(BT_USE_NEON)
+		mVec128 = vmaxq_f32(mVec128, other.mVec128);
+#else
+		btSetMax(m_floats[0], other.m_floats[0]);
+		btSetMax(m_floats[1], other.m_floats[1]);
+		btSetMax(m_floats[2], other.m_floats[2]);
+		btSetMax(m_floats[3], other.m_floats[3]);
+#endif
+	}
+	/**@brief Set each element to the min of the current values and the values of another btQuadWord
    * @param other The other btQuadWord to compare with 
    */
-		SIMD_FORCE_INLINE void	setMin(const btQuadWord& other)
-		{
-        #ifdef BT_USE_SSE
-            mVec128 = _mm_min_ps(mVec128, other.mVec128);
-        #elif defined(BT_USE_NEON)
-            mVec128 = vminq_f32(mVec128, other.mVec128);
-        #else
-        	btSetMin(m_floats[0], other.m_floats[0]);
-			btSetMin(m_floats[1], other.m_floats[1]);
-			btSetMin(m_floats[2], other.m_floats[2]);
-			btSetMin(m_floats[3], other.m_floats[3]);
-		#endif
-        }
-
-
-
+	SIMD_FORCE_INLINE void setMin(const btQuadWord& other)
+	{
+#ifdef BT_USE_SSE
+		mVec128 = _mm_min_ps(mVec128, other.mVec128);
+#elif defined(BT_USE_NEON)
+		mVec128 = vminq_f32(mVec128, other.mVec128);
+#else
+		btSetMin(m_floats[0], other.m_floats[0]);
+		btSetMin(m_floats[1], other.m_floats[1]);
+		btSetMin(m_floats[2], other.m_floats[2]);
+		btSetMin(m_floats[3], other.m_floats[3]);
+#endif
+	}
 };
 
-#endif //BT_SIMD_QUADWORD_H
+#endif  //BT_SIMD_QUADWORD_H
diff --git a/extern/bullet2/src/LinearMath/btQuaternion.h b/extern/bullet2/src/LinearMath/btQuaternion.h
index ede76938404..53e8169b805 100644
--- a/extern/bullet2/src/LinearMath/btQuaternion.h
+++ b/extern/bullet2/src/LinearMath/btQuaternion.h
@@ -12,25 +12,19 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef BT_SIMD__QUATERNION_H_
 #define BT_SIMD__QUATERNION_H_
 
-
 #include "btVector3.h"
 #include "btQuadWord.h"
 
-
 #ifdef BT_USE_DOUBLE_PRECISION
 #define btQuaternionData btQuaternionDoubleData
 #define btQuaternionDataName "btQuaternionDoubleData"
 #else
 #define btQuaternionData btQuaternionFloatData
 #define btQuaternionDataName "btQuaternionFloatData"
-#endif //BT_USE_DOUBLE_PRECISION
-
-
+#endif  //BT_USE_DOUBLE_PRECISION
 
 #ifdef BT_USE_SSE
 
@@ -39,7 +33,7 @@ subject to the following restrictions:
 
 #endif
 
-#if defined(BT_USE_SSE) 
+#if defined(BT_USE_SSE)
 
 #define vQInv (_mm_set_ps(+0.0f, -0.0f, -0.0f, -0.0f))
 #define vPPPM (_mm_set_ps(-0.0f, +0.0f, +0.0f, +0.0f))
@@ -52,13 +46,14 @@ const btSimdFloat4 ATTRIBUTE_ALIGNED16(vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f};
 #endif
 
 /**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
-class btQuaternion : public btQuadWord {
+class btQuaternion : public btQuadWord
+{
 public:
-  /**@brief No initialization constructor */
+	/**@brief No initialization constructor */
 	btQuaternion() {}
 
-#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))|| defined(BT_USE_NEON) 
-	// Set Vector 
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
+	// Set Vector
 	SIMD_FORCE_INLINE btQuaternion(const btSimdFloat4 vec)
 	{
 		mVec128 = vec;
@@ -71,42 +66,43 @@ public:
 	}
 
 	// Assignment Operator
-	SIMD_FORCE_INLINE btQuaternion& 
-	operator=(const btQuaternion& v) 
+	SIMD_FORCE_INLINE btQuaternion&
+	operator=(const btQuaternion& v)
 	{
 		mVec128 = v.mVec128;
-		
+
 		return *this;
 	}
-	
+
 #endif
 
 	//		template <typename btScalar>
 	//		explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {}
-  /**@brief Constructor from scalars */
-	btQuaternion(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w) 
-		: btQuadWord(_x, _y, _z, _w) 
-	{}
-  /**@brief Axis angle Constructor
+	/**@brief Constructor from scalars */
+	btQuaternion(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w)
+		: btQuadWord(_x, _y, _z, _w)
+	{
+	}
+	/**@brief Axis angle Constructor
    * @param axis The axis which the rotation is around
    * @param angle The magnitude of the rotation around the angle (Radians) */
-	btQuaternion(const btVector3& _axis, const btScalar& _angle) 
-	{ 
-		setRotation(_axis, _angle); 
+	btQuaternion(const btVector3& _axis, const btScalar& _angle)
+	{
+		setRotation(_axis, _angle);
 	}
-  /**@brief Constructor from Euler angles
+	/**@brief Constructor from Euler angles
    * @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z
    * @param pitch Angle around X unless BT_EULER_DEFAULT_ZYX defined then Y
    * @param roll Angle around Z unless BT_EULER_DEFAULT_ZYX defined then X */
 	btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
-	{ 
+	{
 #ifndef BT_EULER_DEFAULT_ZYX
-		setEuler(yaw, pitch, roll); 
+		setEuler(yaw, pitch, roll);
 #else
-		setEulerZYX(yaw, pitch, roll); 
-#endif 
+		setEulerZYX(yaw, pitch, roll);
+#endif
 	}
-  /**@brief Set the rotation using axis angle notation 
+	/**@brief Set the rotation using axis angle notation 
    * @param axis The axis around which to rotate
    * @param angle The magnitude of the rotation in Radians */
 	void setRotation(const btVector3& axis, const btScalar& _angle)
@@ -114,18 +110,18 @@ public:
 		btScalar d = axis.length();
 		btAssert(d != btScalar(0.0));
 		btScalar s = btSin(_angle * btScalar(0.5)) / d;
-		setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
-			btCos(_angle * btScalar(0.5)));
+		setValue(axis.x() * s, axis.y() * s, axis.z() * s,
+				 btCos(_angle * btScalar(0.5)));
 	}
-  /**@brief Set the quaternion using Euler angles
+	/**@brief Set the quaternion using Euler angles
    * @param yaw Angle around Y
    * @param pitch Angle around X
    * @param roll Angle around Z */
 	void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
 	{
-		btScalar halfYaw = btScalar(yaw) * btScalar(0.5);  
-		btScalar halfPitch = btScalar(pitch) * btScalar(0.5);  
-		btScalar halfRoll = btScalar(roll) * btScalar(0.5);  
+		btScalar halfYaw = btScalar(yaw) * btScalar(0.5);
+		btScalar halfPitch = btScalar(pitch) * btScalar(0.5);
+		btScalar halfRoll = btScalar(roll) * btScalar(0.5);
 		btScalar cosYaw = btCos(halfYaw);
 		btScalar sinYaw = btSin(halfYaw);
 		btScalar cosPitch = btCos(halfPitch);
@@ -133,241 +129,291 @@ public:
 		btScalar cosRoll = btCos(halfRoll);
 		btScalar sinRoll = btSin(halfRoll);
 		setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
-			cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
-			sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
-			cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
+				 cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
+				 sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
+				 cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
 	}
-  /**@brief Set the quaternion using euler angles 
+	/**@brief Set the quaternion using euler angles 
    * @param yaw Angle around Z
    * @param pitch Angle around Y
    * @param roll Angle around X */
-	void setEulerZYX(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+	void setEulerZYX(const btScalar& yawZ, const btScalar& pitchY, const btScalar& rollX)
 	{
-		btScalar halfYaw = btScalar(yaw) * btScalar(0.5);  
-		btScalar halfPitch = btScalar(pitch) * btScalar(0.5);  
-		btScalar halfRoll = btScalar(roll) * btScalar(0.5);  
+		btScalar halfYaw = btScalar(yawZ) * btScalar(0.5);
+		btScalar halfPitch = btScalar(pitchY) * btScalar(0.5);
+		btScalar halfRoll = btScalar(rollX) * btScalar(0.5);
 		btScalar cosYaw = btCos(halfYaw);
 		btScalar sinYaw = btSin(halfYaw);
 		btScalar cosPitch = btCos(halfPitch);
 		btScalar sinPitch = btSin(halfPitch);
 		btScalar cosRoll = btCos(halfRoll);
 		btScalar sinRoll = btSin(halfRoll);
-		setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
-                         cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
-                         cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
-                         cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
+		setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,   //x
+				 cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,   //y
+				 cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,   //z
+				 cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);  //formerly yzx
+	}
+
+	/**@brief Get the euler angles from this quaternion
+	   * @param yaw Angle around Z
+	   * @param pitch Angle around Y
+	   * @param roll Angle around X */
+	void getEulerZYX(btScalar& yawZ, btScalar& pitchY, btScalar& rollX) const
+	{
+		btScalar squ;
+		btScalar sqx;
+		btScalar sqy;
+		btScalar sqz;
+		btScalar sarg;
+		sqx = m_floats[0] * m_floats[0];
+		sqy = m_floats[1] * m_floats[1];
+		sqz = m_floats[2] * m_floats[2];
+		squ = m_floats[3] * m_floats[3];
+		sarg = btScalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]);
+
+		// If the pitch angle is PI/2 or -PI/2, we can only compute
+		// the sum roll + yaw.  However, any combination that gives
+		// the right sum will produce the correct orientation, so we
+		// set rollX = 0 and compute yawZ.
+		if (sarg <= -btScalar(0.99999))
+		{
+			pitchY = btScalar(-0.5) * SIMD_PI;
+			rollX = 0;
+			yawZ = btScalar(2) * btAtan2(m_floats[0], -m_floats[1]);
+		}
+		else if (sarg >= btScalar(0.99999))
+		{
+			pitchY = btScalar(0.5) * SIMD_PI;
+			rollX = 0;
+			yawZ = btScalar(2) * btAtan2(-m_floats[0], m_floats[1]);
+		}
+		else
+		{
+			pitchY = btAsin(sarg);
+			rollX = btAtan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz);
+			yawZ = btAtan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz);
+		}
 	}
-  /**@brief Add two quaternions
+
+	/**@brief Add two quaternions
    * @param q The quaternion to add to this one */
-	SIMD_FORCE_INLINE	btQuaternion& operator+=(const btQuaternion& q)
+	SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q)
 	{
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		mVec128 = _mm_add_ps(mVec128, q.mVec128);
 #elif defined(BT_USE_NEON)
 		mVec128 = vaddq_f32(mVec128, q.mVec128);
-#else	
-		m_floats[0] += q.x(); 
-        m_floats[1] += q.y(); 
-        m_floats[2] += q.z(); 
-        m_floats[3] += q.m_floats[3];
+#else
+		m_floats[0] += q.x();
+		m_floats[1] += q.y();
+		m_floats[2] += q.z();
+		m_floats[3] += q.m_floats[3];
 #endif
 		return *this;
 	}
 
-  /**@brief Subtract out a quaternion
+	/**@brief Subtract out a quaternion
    * @param q The quaternion to subtract from this one */
-	btQuaternion& operator-=(const btQuaternion& q) 
+	btQuaternion& operator-=(const btQuaternion& q)
 	{
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		mVec128 = _mm_sub_ps(mVec128, q.mVec128);
 #elif defined(BT_USE_NEON)
 		mVec128 = vsubq_f32(mVec128, q.mVec128);
-#else	
-		m_floats[0] -= q.x(); 
-        m_floats[1] -= q.y(); 
-        m_floats[2] -= q.z(); 
-        m_floats[3] -= q.m_floats[3];
+#else
+		m_floats[0] -= q.x();
+		m_floats[1] -= q.y();
+		m_floats[2] -= q.z();
+		m_floats[3] -= q.m_floats[3];
 #endif
-        return *this;
+		return *this;
 	}
 
-  /**@brief Scale this quaternion
+	/**@brief Scale this quaternion
    * @param s The scalar to scale by */
 	btQuaternion& operator*=(const btScalar& s)
 	{
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
-		__m128	vs = _mm_load_ss(&s);	//	(S 0 0 0)
-		vs = bt_pshufd_ps(vs, 0);	//	(S S S S)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+		__m128 vs = _mm_load_ss(&s);  //	(S 0 0 0)
+		vs = bt_pshufd_ps(vs, 0);     //	(S S S S)
 		mVec128 = _mm_mul_ps(mVec128, vs);
 #elif defined(BT_USE_NEON)
 		mVec128 = vmulq_n_f32(mVec128, s);
 #else
-		m_floats[0] *= s; 
-        m_floats[1] *= s; 
-        m_floats[2] *= s; 
-        m_floats[3] *= s;
+		m_floats[0] *= s;
+		m_floats[1] *= s;
+		m_floats[2] *= s;
+		m_floats[3] *= s;
 #endif
 		return *this;
 	}
 
-  /**@brief Multiply this quaternion by q on the right
+	/**@brief Multiply this quaternion by q on the right
    * @param q The other quaternion 
    * Equivilant to this = this * q */
 	btQuaternion& operator*=(const btQuaternion& q)
 	{
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		__m128 vQ2 = q.get128();
-		
-		__m128 A1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(0,1,2,0));
-		__m128 B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0));
-		
+
+		__m128 A1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(0, 1, 2, 0));
+		__m128 B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3, 3, 3, 0));
+
 		A1 = A1 * B1;
-		
-		__m128 A2 = bt_pshufd_ps(mVec128, BT_SHUFFLE(1,2,0,1));
-		__m128 B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
-		
+
+		__m128 A2 = bt_pshufd_ps(mVec128, BT_SHUFFLE(1, 2, 0, 1));
+		__m128 B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2, 0, 1, 1));
+
 		A2 = A2 * B2;
-		
-		B1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(2,0,1,2));
-		B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
-		
-		B1 = B1 * B2;	//	A3 *= B3
-		
-		mVec128 = bt_splat_ps(mVec128, 3);	//	A0
-		mVec128 = mVec128 * vQ2;	//	A0 * B0
-		
-		A1 = A1 + A2;	//	AB12
-		mVec128 = mVec128 - B1;	//	AB03 = AB0 - AB3 
-		A1 = _mm_xor_ps(A1, vPPPM);	//	change sign of the last element
-		mVec128 = mVec128+ A1;	//	AB03 + AB12
-
-#elif defined(BT_USE_NEON)     
-
-        float32x4_t vQ1 = mVec128;
-        float32x4_t vQ2 = q.get128();
-        float32x4_t A0, A1, B1, A2, B2, A3, B3;
-        float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
-        
-        {
-        float32x2x2_t tmp;
-        tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) );       // {z x}, {w y}
-        vQ1zx = tmp.val[0];
-
-        tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) );       // {z x}, {w y}
-        vQ2zx = tmp.val[0];
-        }
-        vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1); 
-
-        vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
-
-        vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
-        vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
-
-        A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx);                    // X Y  z x 
-        B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W  W X 
-
-        A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
-        B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
-
-        A3 = vcombine_f32(vQ1zx, vQ1yz);        // Z X Y Z
-        B3 = vcombine_f32(vQ2yz, vQ2xz);        // Y Z x z
-
-        A1 = vmulq_f32(A1, B1);
-        A2 = vmulq_f32(A2, B2);
-        A3 = vmulq_f32(A3, B3);	//	A3 *= B3
-        A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); //	A0 * B0
-
-        A1 = vaddq_f32(A1, A2);	//	AB12 = AB1 + AB2
-        A0 = vsubq_f32(A0, A3);	//	AB03 = AB0 - AB3 
-        
-        //	change the sign of the last element
-        A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);	
-        A0 = vaddq_f32(A0, A1);	//	AB03 + AB12
-        
-        mVec128 = A0;
+
+		B1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(2, 0, 1, 2));
+		B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1, 2, 0, 2));
+
+		B1 = B1 * B2;  //	A3 *= B3
+
+		mVec128 = bt_splat_ps(mVec128, 3);  //	A0
+		mVec128 = mVec128 * vQ2;            //	A0 * B0
+
+		A1 = A1 + A2;                //	AB12
+		mVec128 = mVec128 - B1;      //	AB03 = AB0 - AB3
+		A1 = _mm_xor_ps(A1, vPPPM);  //	change sign of the last element
+		mVec128 = mVec128 + A1;      //	AB03 + AB12
+
+#elif defined(BT_USE_NEON)
+
+		float32x4_t vQ1 = mVec128;
+		float32x4_t vQ2 = q.get128();
+		float32x4_t A0, A1, B1, A2, B2, A3, B3;
+		float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
+
+		{
+			float32x2x2_t tmp;
+			tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1));  // {z x}, {w y}
+			vQ1zx = tmp.val[0];
+
+			tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2));  // {z x}, {w y}
+			vQ2zx = tmp.val[0];
+		}
+		vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
+
+		vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
+
+		vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
+		vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
+
+		A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx);                     // X Y  z x
+		B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx);  // W W  W X
+
+		A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
+		B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
+
+		A3 = vcombine_f32(vQ1zx, vQ1yz);  // Z X Y Z
+		B3 = vcombine_f32(vQ2yz, vQ2xz);  // Y Z x z
+
+		A1 = vmulq_f32(A1, B1);
+		A2 = vmulq_f32(A2, B2);
+		A3 = vmulq_f32(A3, B3);                           //	A3 *= B3
+		A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1);  //	A0 * B0
+
+		A1 = vaddq_f32(A1, A2);  //	AB12 = AB1 + AB2
+		A0 = vsubq_f32(A0, A3);  //	AB03 = AB0 - AB3
+
+		//	change the sign of the last element
+		A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
+		A0 = vaddq_f32(A0, A1);  //	AB03 + AB12
+
+		mVec128 = A0;
 #else
 		setValue(
-            m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
+			m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
 			m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
 			m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
 			m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
 #endif
 		return *this;
 	}
-  /**@brief Return the dot product between this quaternion and another
+	/**@brief Return the dot product between this quaternion and another
    * @param q The other quaternion */
 	btScalar dot(const btQuaternion& q) const
 	{
-#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
-		__m128	vd;
-		
+#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+		__m128 vd;
+
 		vd = _mm_mul_ps(mVec128, q.mVec128);
-		
-        __m128 t = _mm_movehl_ps(vd, vd);
+
+		__m128 t = _mm_movehl_ps(vd, vd);
 		vd = _mm_add_ps(vd, t);
 		t = _mm_shuffle_ps(vd, vd, 0x55);
 		vd = _mm_add_ss(vd, t);
-		
-        return _mm_cvtss_f32(vd);
+
+		return _mm_cvtss_f32(vd);
 #elif defined(BT_USE_NEON)
 		float32x4_t vd = vmulq_f32(mVec128, q.mVec128);
-		float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_high_f32(vd));  
+		float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_high_f32(vd));
 		x = vpadd_f32(x, x);
 		return vget_lane_f32(x, 0);
-#else    
-		return  m_floats[0] * q.x() + 
-                m_floats[1] * q.y() + 
-                m_floats[2] * q.z() + 
-                m_floats[3] * q.m_floats[3];
+#else
+		return m_floats[0] * q.x() +
+			   m_floats[1] * q.y() +
+			   m_floats[2] * q.z() +
+			   m_floats[3] * q.m_floats[3];
 #endif
 	}
 
-  /**@brief Return the length squared of the quaternion */
+	/**@brief Return the length squared of the quaternion */
 	btScalar length2() const
 	{
 		return dot(*this);
 	}
 
-  /**@brief Return the length of the quaternion */
+	/**@brief Return the length of the quaternion */
 	btScalar length() const
 	{
 		return btSqrt(length2());
 	}
-
-  /**@brief Normalize the quaternion 
+	btQuaternion& safeNormalize()
+	{
+		btScalar l2 = length2();
+		if (l2 > SIMD_EPSILON)
+		{
+			normalize();
+		}
+		return *this;
+	}
+	/**@brief Normalize the quaternion 
    * Such that x^2 + y^2 + z^2 +w^2 = 1 */
-	btQuaternion& normalize() 
+	btQuaternion& normalize()
 	{
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
-		__m128	vd;
-		
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+		__m128 vd;
+
 		vd = _mm_mul_ps(mVec128, mVec128);
-		
-        __m128 t = _mm_movehl_ps(vd, vd);
+
+		__m128 t = _mm_movehl_ps(vd, vd);
 		vd = _mm_add_ps(vd, t);
 		t = _mm_shuffle_ps(vd, vd, 0x55);
 		vd = _mm_add_ss(vd, t);
 
 		vd = _mm_sqrt_ss(vd);
 		vd = _mm_div_ss(vOnes, vd);
-        vd = bt_pshufd_ps(vd, 0); // splat
+		vd = bt_pshufd_ps(vd, 0);  // splat
 		mVec128 = _mm_mul_ps(mVec128, vd);
-    
+
 		return *this;
-#else    
+#else
 		return *this /= length();
 #endif
 	}
 
-  /**@brief Return a scaled version of this quaternion
+	/**@brief Return a scaled version of this quaternion
    * @param s The scale factor */
 	SIMD_FORCE_INLINE btQuaternion
 	operator*(const btScalar& s) const
 	{
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
-		__m128	vs = _mm_load_ss(&s);	//	(S 0 0 0)
-		vs = bt_pshufd_ps(vs, 0x00);	//	(S S S S)
-		
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+		__m128 vs = _mm_load_ss(&s);  //	(S 0 0 0)
+		vs = bt_pshufd_ps(vs, 0x00);  //	(S S S S)
+
 		return btQuaternion(_mm_mul_ps(mVec128, vs));
 #elif defined(BT_USE_NEON)
 		return btQuaternion(vmulq_n_f32(mVec128, s));
@@ -376,7 +422,7 @@ public:
 #endif
 	}
 
-  /**@brief Return an inversely scaled versionof this quaternion
+	/**@brief Return an inversely scaled versionof this quaternion
    * @param s The inverse scale factor */
 	btQuaternion operator/(const btScalar& s) const
 	{
@@ -384,174 +430,174 @@ public:
 		return *this * (btScalar(1.0) / s);
 	}
 
-  /**@brief Inversely scale this quaternion
+	/**@brief Inversely scale this quaternion
    * @param s The scale factor */
-	btQuaternion& operator/=(const btScalar& s) 
+	btQuaternion& operator/=(const btScalar& s)
 	{
 		btAssert(s != btScalar(0.0));
 		return *this *= btScalar(1.0) / s;
 	}
 
-  /**@brief Return a normalized version of this quaternion */
-	btQuaternion normalized() const 
+	/**@brief Return a normalized version of this quaternion */
+	btQuaternion normalized() const
 	{
 		return *this / length();
-	} 
+	}
 	/**@brief Return the ***half*** angle between this quaternion and the other
    * @param q The other quaternion */
-	btScalar angle(const btQuaternion& q) const 
+	btScalar angle(const btQuaternion& q) const
 	{
 		btScalar s = btSqrt(length2() * q.length2());
 		btAssert(s != btScalar(0.0));
 		return btAcos(dot(q) / s);
 	}
-	
+
 	/**@brief Return the angle between this quaternion and the other along the shortest path
 	* @param q The other quaternion */
-	btScalar angleShortestPath(const btQuaternion& q) const 
+	btScalar angleShortestPath(const btQuaternion& q) const
 	{
 		btScalar s = btSqrt(length2() * q.length2());
 		btAssert(s != btScalar(0.0));
-		if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
+		if (dot(q) < 0)  // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
 			return btAcos(dot(-q) / s) * btScalar(2.0);
-		else 
+		else
 			return btAcos(dot(q) / s) * btScalar(2.0);
 	}
 
-  /**@brief Return the angle of rotation represented by this quaternion */
-	btScalar getAngle() const 
+	/**@brief Return the angle [0, 2Pi] of rotation represented by this quaternion */
+	btScalar getAngle() const
 	{
 		btScalar s = btScalar(2.) * btAcos(m_floats[3]);
 		return s;
 	}
 
-	/**@brief Return the angle of rotation represented by this quaternion along the shortest path*/
-	btScalar getAngleShortestPath() const 
+	/**@brief Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path */
+	btScalar getAngleShortestPath() const
 	{
 		btScalar s;
-		if (dot(*this) < 0)
+		if (m_floats[3] >= 0)
 			s = btScalar(2.) * btAcos(m_floats[3]);
 		else
 			s = btScalar(2.) * btAcos(-m_floats[3]);
-
 		return s;
 	}
 
-
 	/**@brief Return the axis of the rotation represented by this quaternion */
 	btVector3 getAxis() const
 	{
-		btScalar s_squared = 1.f-m_floats[3]*m_floats[3];
-		
-		if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero
-			return btVector3(1.0, 0.0, 0.0);  // Arbitrary
-		btScalar s = 1.f/btSqrt(s_squared);
+		btScalar s_squared = 1.f - m_floats[3] * m_floats[3];
+
+		if (s_squared < btScalar(10.) * SIMD_EPSILON)  //Check for divide by zero
+			return btVector3(1.0, 0.0, 0.0);           // Arbitrary
+		btScalar s = 1.f / btSqrt(s_squared);
 		return btVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s);
 	}
 
 	/**@brief Return the inverse of this quaternion */
 	btQuaternion inverse() const
 	{
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		return btQuaternion(_mm_xor_ps(mVec128, vQInv));
 #elif defined(BT_USE_NEON)
-        return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)vQInv));
-#else	
+		return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)vQInv));
+#else
 		return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
 #endif
 	}
 
-  /**@brief Return the sum of this quaternion and the other 
+	/**@brief Return the sum of this quaternion and the other 
    * @param q2 The other quaternion */
 	SIMD_FORCE_INLINE btQuaternion
 	operator+(const btQuaternion& q2) const
 	{
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		return btQuaternion(_mm_add_ps(mVec128, q2.mVec128));
 #elif defined(BT_USE_NEON)
-        return btQuaternion(vaddq_f32(mVec128, q2.mVec128));
-#else	
+		return btQuaternion(vaddq_f32(mVec128, q2.mVec128));
+#else
 		const btQuaternion& q1 = *this;
 		return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
 #endif
 	}
 
-  /**@brief Return the difference between this quaternion and the other 
+	/**@brief Return the difference between this quaternion and the other 
    * @param q2 The other quaternion */
 	SIMD_FORCE_INLINE btQuaternion
 	operator-(const btQuaternion& q2) const
 	{
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		return btQuaternion(_mm_sub_ps(mVec128, q2.mVec128));
 #elif defined(BT_USE_NEON)
-        return btQuaternion(vsubq_f32(mVec128, q2.mVec128));
-#else	
+		return btQuaternion(vsubq_f32(mVec128, q2.mVec128));
+#else
 		const btQuaternion& q1 = *this;
 		return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
 #endif
 	}
 
-  /**@brief Return the negative of this quaternion 
+	/**@brief Return the negative of this quaternion 
    * This simply negates each element */
 	SIMD_FORCE_INLINE btQuaternion operator-() const
 	{
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		return btQuaternion(_mm_xor_ps(mVec128, btvMzeroMask));
 #elif defined(BT_USE_NEON)
-		return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)btvMzeroMask) );
-#else	
+		return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)btvMzeroMask));
+#else
 		const btQuaternion& q2 = *this;
-		return btQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_floats[3]);
+		return btQuaternion(-q2.x(), -q2.y(), -q2.z(), -q2.m_floats[3]);
 #endif
 	}
-  /**@todo document this and it's use */
-	SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const 
+	/**@todo document this and it's use */
+	SIMD_FORCE_INLINE btQuaternion farthest(const btQuaternion& qd) const
 	{
-		btQuaternion diff,sum;
+		btQuaternion diff, sum;
 		diff = *this - qd;
 		sum = *this + qd;
-		if( diff.dot(diff) > sum.dot(sum) )
+		if (diff.dot(diff) > sum.dot(sum))
 			return qd;
 		return (-qd);
 	}
 
 	/**@todo document this and it's use */
-	SIMD_FORCE_INLINE btQuaternion nearest( const btQuaternion& qd) const 
+	SIMD_FORCE_INLINE btQuaternion nearest(const btQuaternion& qd) const
 	{
-		btQuaternion diff,sum;
+		btQuaternion diff, sum;
 		diff = *this - qd;
 		sum = *this + qd;
-		if( diff.dot(diff) < sum.dot(sum) )
+		if (diff.dot(diff) < sum.dot(sum))
 			return qd;
 		return (-qd);
 	}
 
-
-  /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
+	/**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
    * @param q The other quaternion to interpolate with 
    * @param t The ratio between this and q to interpolate.  If t = 0 the result is this, if t=1 the result is q.
    * Slerp interpolates assuming constant velocity.  */
 	btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
 	{
-	  btScalar magnitude = btSqrt(length2() * q.length2()); 
-	  btAssert(magnitude > btScalar(0));
+		const btScalar magnitude = btSqrt(length2() * q.length2());
+		btAssert(magnitude > btScalar(0));
+
+		const btScalar product = dot(q) / magnitude;
+		const btScalar absproduct = btFabs(product);
 
-    btScalar product = dot(q) / magnitude;
-    if (btFabs(product) < btScalar(1))
+		if (absproduct < btScalar(1.0 - SIMD_EPSILON))
 		{
-      // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
-      const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1);
-
-      const btScalar theta = btAcos(sign * product);
-      const btScalar s1 = btSin(sign * t * theta);   
-      const btScalar d = btScalar(1.0) / btSin(theta);
-      const btScalar s0 = btSin((btScalar(1.0) - t) * theta);
-
-      return btQuaternion(
-          (m_floats[0] * s0 + q.x() * s1) * d,
-          (m_floats[1] * s0 + q.y() * s1) * d,
-          (m_floats[2] * s0 + q.z() * s1) * d,
-          (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
+			// Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
+			const btScalar theta = btAcos(absproduct);
+			const btScalar d = btSin(theta);
+			btAssert(d > btScalar(0));
+
+			const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1);
+			const btScalar s0 = btSin((btScalar(1.0) - t) * theta) / d;
+			const btScalar s1 = btSin(sign * t * theta) / d;
+
+			return btQuaternion(
+				(m_floats[0] * s0 + q.x() * s1),
+				(m_floats[1] * s0 + q.y() * s1),
+				(m_floats[2] * s0 + q.z() * s1),
+				(m_floats[3] * s0 + q.w() * s1));
 		}
 		else
 		{
@@ -559,312 +605,308 @@ public:
 		}
 	}
 
-	static const btQuaternion&	getIdentity()
+	static const btQuaternion& getIdentity()
 	{
-		static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.));
+		static const btQuaternion identityQuat(btScalar(0.), btScalar(0.), btScalar(0.), btScalar(1.));
 		return identityQuat;
 	}
 
 	SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
 
-	SIMD_FORCE_INLINE	void	serialize(struct	btQuaternionData& dataOut) const;
+	SIMD_FORCE_INLINE void serialize(struct btQuaternionData& dataOut) const;
 
-	SIMD_FORCE_INLINE	void	deSerialize(const struct	btQuaternionData& dataIn);
+	SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionFloatData& dataIn);
 
-	SIMD_FORCE_INLINE	void	serializeFloat(struct	btQuaternionFloatData& dataOut) const;
+	SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionDoubleData& dataIn);
 
-	SIMD_FORCE_INLINE	void	deSerializeFloat(const struct	btQuaternionFloatData& dataIn);
+	SIMD_FORCE_INLINE void serializeFloat(struct btQuaternionFloatData& dataOut) const;
 
-	SIMD_FORCE_INLINE	void	serializeDouble(struct	btQuaternionDoubleData& dataOut) const;
+	SIMD_FORCE_INLINE void deSerializeFloat(const struct btQuaternionFloatData& dataIn);
 
-	SIMD_FORCE_INLINE	void	deSerializeDouble(const struct	btQuaternionDoubleData& dataIn);
+	SIMD_FORCE_INLINE void serializeDouble(struct btQuaternionDoubleData& dataOut) const;
 
+	SIMD_FORCE_INLINE void deSerializeDouble(const struct btQuaternionDoubleData& dataIn);
 };
 
-
-
-
-
 /**@brief Return the product of two quaternions */
 SIMD_FORCE_INLINE btQuaternion
-operator*(const btQuaternion& q1, const btQuaternion& q2) 
+operator*(const btQuaternion& q1, const btQuaternion& q2)
 {
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 	__m128 vQ1 = q1.get128();
 	__m128 vQ2 = q2.get128();
 	__m128 A0, A1, B1, A2, B2;
-    
-	A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0,1,2,0)); // X Y  z x     //      vtrn
-	B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0)); // W W  W X     // vdup vext
+
+	A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0, 1, 2, 0));  // X Y  z x     //      vtrn
+	B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3, 3, 3, 0));  // W W  W X     // vdup vext
 
 	A1 = A1 * B1;
-	
-	A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1)); // Y Z  X Y     // vext 
-	B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1)); // z x  Y Y     // vtrn vdup
+
+	A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1, 2, 0, 1));  // Y Z  X Y     // vext
+	B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2, 0, 1, 1));  // z x  Y Y     // vtrn vdup
 
 	A2 = A2 * B2;
 
-	B1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2)); // z x Y Z      // vtrn vext
-	B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2)); // Y Z x z      // vext vtrn
-	
-	B1 = B1 * B2;	//	A3 *= B3
+	B1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2, 0, 1, 2));  // z x Y Z      // vtrn vext
+	B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1, 2, 0, 2));  // Y Z x z      // vext vtrn
+
+	B1 = B1 * B2;  //	A3 *= B3
+
+	A0 = bt_splat_ps(vQ1, 3);  //	A0
+	A0 = A0 * vQ2;             //	A0 * B0
+
+	A1 = A1 + A2;  //	AB12
+	A0 = A0 - B1;  //	AB03 = AB0 - AB3
 
-	A0 = bt_splat_ps(vQ1, 3);	//	A0
-	A0 = A0 * vQ2;	//	A0 * B0
+	A1 = _mm_xor_ps(A1, vPPPM);  //	change sign of the last element
+	A0 = A0 + A1;                //	AB03 + AB12
 
-	A1 = A1 + A2;	//	AB12
-	A0 =  A0 - B1;	//	AB03 = AB0 - AB3 
-	
-    A1 = _mm_xor_ps(A1, vPPPM);	//	change sign of the last element
-	A0 = A0 + A1;	//	AB03 + AB12
-	
 	return btQuaternion(A0);
 
-#elif defined(BT_USE_NEON)     
+#elif defined(BT_USE_NEON)
 
 	float32x4_t vQ1 = q1.get128();
 	float32x4_t vQ2 = q2.get128();
 	float32x4_t A0, A1, B1, A2, B2, A3, B3;
-    float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
-    
-    {
-    float32x2x2_t tmp;
-    tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) );       // {z x}, {w y}
-    vQ1zx = tmp.val[0];
+	float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
+
+	{
+		float32x2x2_t tmp;
+		tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1));  // {z x}, {w y}
+		vQ1zx = tmp.val[0];
 
-    tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) );       // {z x}, {w y}
-    vQ2zx = tmp.val[0];
-    }
-    vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1); 
+		tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2));  // {z x}, {w y}
+		vQ2zx = tmp.val[0];
+	}
+	vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
 
-    vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
+	vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
 
-    vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
-    vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
+	vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
+	vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
 
-    A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx);                    // X Y  z x 
-    B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W  W X 
+	A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx);                     // X Y  z x
+	B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx);  // W W  W X
 
 	A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
-    B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
+	B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
 
-    A3 = vcombine_f32(vQ1zx, vQ1yz);        // Z X Y Z
-    B3 = vcombine_f32(vQ2yz, vQ2xz);        // Y Z x z
+	A3 = vcombine_f32(vQ1zx, vQ1yz);  // Z X Y Z
+	B3 = vcombine_f32(vQ2yz, vQ2xz);  // Y Z x z
 
 	A1 = vmulq_f32(A1, B1);
 	A2 = vmulq_f32(A2, B2);
-	A3 = vmulq_f32(A3, B3);	//	A3 *= B3
-	A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); //	A0 * B0
-
-	A1 = vaddq_f32(A1, A2);	//	AB12 = AB1 + AB2
-	A0 = vsubq_f32(A0, A3);	//	AB03 = AB0 - AB3 
-	
-    //	change the sign of the last element
-    A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);	
-	A0 = vaddq_f32(A0, A1);	//	AB03 + AB12
-	
+	A3 = vmulq_f32(A3, B3);                           //	A3 *= B3
+	A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1);  //	A0 * B0
+
+	A1 = vaddq_f32(A1, A2);  //	AB12 = AB1 + AB2
+	A0 = vsubq_f32(A0, A3);  //	AB03 = AB0 - AB3
+
+	//	change the sign of the last element
+	A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
+	A0 = vaddq_f32(A0, A1);  //	AB03 + AB12
+
 	return btQuaternion(A0);
 
 #else
 	return btQuaternion(
-        q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
+		q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
 		q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
 		q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
-		q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 
+		q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
 #endif
 }
 
 SIMD_FORCE_INLINE btQuaternion
 operator*(const btQuaternion& q, const btVector3& w)
 {
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 	__m128 vQ1 = q.get128();
 	__m128 vQ2 = w.get128();
 	__m128 A1, B1, A2, B2, A3, B3;
-	
-	A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(3,3,3,0));
-	B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(0,1,2,0));
+
+	A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(3, 3, 3, 0));
+	B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(0, 1, 2, 0));
 
 	A1 = A1 * B1;
-	
-	A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1));
-	B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
+
+	A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1, 2, 0, 1));
+	B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2, 0, 1, 1));
 
 	A2 = A2 * B2;
 
-	A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2));
-	B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
-	
-	A3 = A3 * B3;	//	A3 *= B3
+	A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2, 0, 1, 2));
+	B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1, 2, 0, 2));
+
+	A3 = A3 * B3;  //	A3 *= B3
+
+	A1 = A1 + A2;                //	AB12
+	A1 = _mm_xor_ps(A1, vPPPM);  //	change sign of the last element
+	A1 = A1 - A3;                //	AB123 = AB12 - AB3
 
-	A1 = A1 + A2;	//	AB12
-	A1 = _mm_xor_ps(A1, vPPPM);	//	change sign of the last element
-    A1 = A1 - A3;	//	AB123 = AB12 - AB3 
-	
 	return btQuaternion(A1);
-    
-#elif defined(BT_USE_NEON)     
+
+#elif defined(BT_USE_NEON)
 
 	float32x4_t vQ1 = q.get128();
 	float32x4_t vQ2 = w.get128();
 	float32x4_t A1, B1, A2, B2, A3, B3;
-    float32x2_t vQ1wx, vQ2zx, vQ1yz, vQ2yz, vQ1zx, vQ2xz;
-    
-    vQ1wx = vext_f32(vget_high_f32(vQ1), vget_low_f32(vQ1), 1); 
-    {
-    float32x2x2_t tmp;
+	float32x2_t vQ1wx, vQ2zx, vQ1yz, vQ2yz, vQ1zx, vQ2xz;
 
-    tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) );       // {z x}, {w y}
-    vQ2zx = tmp.val[0];
+	vQ1wx = vext_f32(vget_high_f32(vQ1), vget_low_f32(vQ1), 1);
+	{
+		float32x2x2_t tmp;
+
+		tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2));  // {z x}, {w y}
+		vQ2zx = tmp.val[0];
 
-    tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) );       // {z x}, {w y}
-    vQ1zx = tmp.val[0];
-    }
+		tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1));  // {z x}, {w y}
+		vQ1zx = tmp.val[0];
+	}
 
-    vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
+	vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
 
-    vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
-    vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
+	vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
+	vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
 
-    A1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ1), 1), vQ1wx); // W W  W X 
-    B1 = vcombine_f32(vget_low_f32(vQ2), vQ2zx);                    // X Y  z x 
+	A1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ1), 1), vQ1wx);  // W W  W X
+	B1 = vcombine_f32(vget_low_f32(vQ2), vQ2zx);                     // X Y  z x
 
 	A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
-    B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
+	B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
 
-    A3 = vcombine_f32(vQ1zx, vQ1yz);        // Z X Y Z
-    B3 = vcombine_f32(vQ2yz, vQ2xz);        // Y Z x z
+	A3 = vcombine_f32(vQ1zx, vQ1yz);  // Z X Y Z
+	B3 = vcombine_f32(vQ2yz, vQ2xz);  // Y Z x z
 
 	A1 = vmulq_f32(A1, B1);
 	A2 = vmulq_f32(A2, B2);
-	A3 = vmulq_f32(A3, B3);	//	A3 *= B3
-
-	A1 = vaddq_f32(A1, A2);	//	AB12 = AB1 + AB2
-	
-    //	change the sign of the last element
-    A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);	
-	
-    A1 = vsubq_f32(A1, A3);	//	AB123 = AB12 - AB3
-	
+	A3 = vmulq_f32(A3, B3);  //	A3 *= B3
+
+	A1 = vaddq_f32(A1, A2);  //	AB12 = AB1 + AB2
+
+	//	change the sign of the last element
+	A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
+
+	A1 = vsubq_f32(A1, A3);  //	AB123 = AB12 - AB3
+
 	return btQuaternion(A1);
-    
+
 #else
-	return btQuaternion( 
-         q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
-		 q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
-		 q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
-		-q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 
+	return btQuaternion(
+		q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
+		q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
+		q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
+		-q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
 #endif
 }
 
 SIMD_FORCE_INLINE btQuaternion
 operator*(const btVector3& w, const btQuaternion& q)
 {
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 	__m128 vQ1 = w.get128();
 	__m128 vQ2 = q.get128();
 	__m128 A1, B1, A2, B2, A3, B3;
-	
-	A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0,1,2,0));  // X Y  z x
-	B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0));  // W W  W X 
+
+	A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0, 1, 2, 0));  // X Y  z x
+	B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3, 3, 3, 0));  // W W  W X
 
 	A1 = A1 * B1;
-	
-	A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1));
-	B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
 
-	A2 = A2 *B2;
+	A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1, 2, 0, 1));
+	B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2, 0, 1, 1));
+
+	A2 = A2 * B2;
+
+	A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2, 0, 1, 2));
+	B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1, 2, 0, 2));
+
+	A3 = A3 * B3;  //	A3 *= B3
 
-	A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2));
-	B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
-	
-	A3 = A3 * B3;	//	A3 *= B3
+	A1 = A1 + A2;                //	AB12
+	A1 = _mm_xor_ps(A1, vPPPM);  //	change sign of the last element
+	A1 = A1 - A3;                //	AB123 = AB12 - AB3
 
-	A1 = A1 + A2;	//	AB12
-	A1 = _mm_xor_ps(A1, vPPPM);	//	change sign of the last element
-	A1 = A1 - A3;	//	AB123 = AB12 - AB3 
-	
 	return btQuaternion(A1);
 
-#elif defined(BT_USE_NEON)     
+#elif defined(BT_USE_NEON)
 
 	float32x4_t vQ1 = w.get128();
 	float32x4_t vQ2 = q.get128();
-	float32x4_t  A1, B1, A2, B2, A3, B3;
-    float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
-    
-    {
-    float32x2x2_t tmp;
-   
-    tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) );       // {z x}, {w y}
-    vQ1zx = tmp.val[0];
+	float32x4_t A1, B1, A2, B2, A3, B3;
+	float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
+
+	{
+		float32x2x2_t tmp;
 
-    tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) );       // {z x}, {w y}
-    vQ2zx = tmp.val[0];
-    }
-    vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1); 
+		tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1));  // {z x}, {w y}
+		vQ1zx = tmp.val[0];
 
-    vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
+		tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2));  // {z x}, {w y}
+		vQ2zx = tmp.val[0];
+	}
+	vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
+
+	vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
 
-    vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
-    vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
+	vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
+	vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
 
-    A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx);                    // X Y  z x 
-    B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W  W X 
+	A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx);                     // X Y  z x
+	B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx);  // W W  W X
 
 	A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
-    B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
+	B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
 
-    A3 = vcombine_f32(vQ1zx, vQ1yz);        // Z X Y Z
-    B3 = vcombine_f32(vQ2yz, vQ2xz);        // Y Z x z
+	A3 = vcombine_f32(vQ1zx, vQ1yz);  // Z X Y Z
+	B3 = vcombine_f32(vQ2yz, vQ2xz);  // Y Z x z
 
 	A1 = vmulq_f32(A1, B1);
 	A2 = vmulq_f32(A2, B2);
-	A3 = vmulq_f32(A3, B3);	//	A3 *= B3
-
-	A1 = vaddq_f32(A1, A2);	//	AB12 = AB1 + AB2
-	
-    //	change the sign of the last element
-    A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);	
-	
-    A1 = vsubq_f32(A1, A3);	//	AB123 = AB12 - AB3
-	
+	A3 = vmulq_f32(A3, B3);  //	A3 *= B3
+
+	A1 = vaddq_f32(A1, A2);  //	AB12 = AB1 + AB2
+
+	//	change the sign of the last element
+	A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
+
+	A1 = vsubq_f32(A1, A3);  //	AB123 = AB12 - AB3
+
 	return btQuaternion(A1);
-    
+
 #else
-	return btQuaternion( 
-        +w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
+	return btQuaternion(
+		+w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
 		+w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
 		+w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
-		-w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); 
+		-w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
 #endif
 }
 
 /**@brief Calculate the dot product between two quaternions */
-SIMD_FORCE_INLINE btScalar 
-dot(const btQuaternion& q1, const btQuaternion& q2) 
-{ 
-	return q1.dot(q2); 
+SIMD_FORCE_INLINE btScalar
+dot(const btQuaternion& q1, const btQuaternion& q2)
+{
+	return q1.dot(q2);
 }
 
-
 /**@brief Return the length of a quaternion */
 SIMD_FORCE_INLINE btScalar
-length(const btQuaternion& q) 
-{ 
-	return q.length(); 
+length(const btQuaternion& q)
+{
+	return q.length();
 }
 
 /**@brief Return the angle between two quaternions*/
 SIMD_FORCE_INLINE btScalar
-btAngle(const btQuaternion& q1, const btQuaternion& q2) 
-{ 
-	return q1.angle(q2); 
+btAngle(const btQuaternion& q1, const btQuaternion& q2)
+{
+	return q1.angle(q2);
 }
 
 /**@brief Return the inverse of a quaternion*/
 SIMD_FORCE_INLINE btQuaternion
-inverse(const btQuaternion& q) 
+inverse(const btQuaternion& q)
 {
 	return q.inverse();
 }
@@ -875,109 +917,105 @@ inverse(const btQuaternion& q)
  * @param t The ration between q1 and q2.  t = 0 return q1, t=1 returns q2 
  * Slerp assumes constant velocity between positions. */
 SIMD_FORCE_INLINE btQuaternion
-slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) 
+slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t)
 {
 	return q1.slerp(q2, t);
 }
 
-SIMD_FORCE_INLINE btVector3 
-quatRotate(const btQuaternion& rotation, const btVector3& v) 
+SIMD_FORCE_INLINE btVector3
+quatRotate(const btQuaternion& rotation, const btVector3& v)
 {
 	btQuaternion q = rotation * v;
 	q *= rotation.inverse();
-#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 	return btVector3(_mm_and_ps(q.get128(), btvFFF0fMask));
 #elif defined(BT_USE_NEON)
-    return btVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), btvFFF0Mask));
-#else	
-	return btVector3(q.getX(),q.getY(),q.getZ());
+	return btVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), btvFFF0Mask));
+#else
+	return btVector3(q.getX(), q.getY(), q.getZ());
 #endif
 }
 
-SIMD_FORCE_INLINE btQuaternion 
-shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
+SIMD_FORCE_INLINE btQuaternion
+shortestArcQuat(const btVector3& v0, const btVector3& v1)  // Game Programming Gems 2.10. make sure v0,v1 are normalized
 {
 	btVector3 c = v0.cross(v1);
-	btScalar  d = v0.dot(v1);
+	btScalar d = v0.dot(v1);
 
 	if (d < -1.0 + SIMD_EPSILON)
 	{
-		btVector3 n,unused;
-		btPlaneSpace1(v0,n,unused);
-		return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
+		btVector3 n, unused;
+		btPlaneSpace1(v0, n, unused);
+		return btQuaternion(n.x(), n.y(), n.z(), 0.0f);  // just pick any vector that is orthogonal to v0
 	}
 
-	btScalar  s = btSqrt((1.0f + d) * 2.0f);
+	btScalar s = btSqrt((1.0f + d) * 2.0f);
 	btScalar rs = 1.0f / s;
 
-	return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
+	return btQuaternion(c.getX() * rs, c.getY() * rs, c.getZ() * rs, s * 0.5f);
 }
 
-SIMD_FORCE_INLINE btQuaternion 
-shortestArcQuatNormalize2(btVector3& v0,btVector3& v1)
+SIMD_FORCE_INLINE btQuaternion
+shortestArcQuatNormalize2(btVector3& v0, btVector3& v1)
 {
 	v0.normalize();
 	v1.normalize();
-	return shortestArcQuat(v0,v1);
+	return shortestArcQuat(v0, v1);
 }
 
-
-
-
-struct	btQuaternionFloatData
+struct btQuaternionFloatData
 {
-	float	m_floats[4];
+	float m_floats[4];
 };
 
-struct	btQuaternionDoubleData
+struct btQuaternionDoubleData
 {
-	double	m_floats[4];
-
+	double m_floats[4];
 };
 
-SIMD_FORCE_INLINE	void	btQuaternion::serializeFloat(struct	btQuaternionFloatData& dataOut) const
+SIMD_FORCE_INLINE void btQuaternion::serializeFloat(struct btQuaternionFloatData& dataOut) const
 {
 	///could also do a memcpy, check if it is worth it
-	for (int i=0;i<4;i++)
+	for (int i = 0; i < 4; i++)
 		dataOut.m_floats[i] = float(m_floats[i]);
 }
 
-SIMD_FORCE_INLINE void	btQuaternion::deSerializeFloat(const struct	btQuaternionFloatData& dataIn)
+SIMD_FORCE_INLINE void btQuaternion::deSerializeFloat(const struct btQuaternionFloatData& dataIn)
 {
-	for (int i=0;i<4;i++)
+	for (int i = 0; i < 4; i++)
 		m_floats[i] = btScalar(dataIn.m_floats[i]);
 }
 
-
-SIMD_FORCE_INLINE	void	btQuaternion::serializeDouble(struct	btQuaternionDoubleData& dataOut) const
+SIMD_FORCE_INLINE void btQuaternion::serializeDouble(struct btQuaternionDoubleData& dataOut) const
 {
 	///could also do a memcpy, check if it is worth it
-	for (int i=0;i<4;i++)
+	for (int i = 0; i < 4; i++)
 		dataOut.m_floats[i] = double(m_floats[i]);
 }
 
-SIMD_FORCE_INLINE void	btQuaternion::deSerializeDouble(const struct	btQuaternionDoubleData& dataIn)
+SIMD_FORCE_INLINE void btQuaternion::deSerializeDouble(const struct btQuaternionDoubleData& dataIn)
 {
-	for (int i=0;i<4;i++)
+	for (int i = 0; i < 4; i++)
 		m_floats[i] = btScalar(dataIn.m_floats[i]);
 }
 
-
-SIMD_FORCE_INLINE	void	btQuaternion::serialize(struct	btQuaternionData& dataOut) const
+SIMD_FORCE_INLINE void btQuaternion::serialize(struct btQuaternionData& dataOut) const
 {
 	///could also do a memcpy, check if it is worth it
-	for (int i=0;i<4;i++)
+	for (int i = 0; i < 4; i++)
 		dataOut.m_floats[i] = m_floats[i];
 }
 
-SIMD_FORCE_INLINE void	btQuaternion::deSerialize(const struct	btQuaternionData& dataIn)
+SIMD_FORCE_INLINE void btQuaternion::deSerialize(const struct btQuaternionFloatData& dataIn)
 {
-	for (int i=0;i<4;i++)
-		m_floats[i] = dataIn.m_floats[i];
+	for (int i = 0; i < 4; i++)
+		m_floats[i] = (btScalar)dataIn.m_floats[i];
 }
 
+SIMD_FORCE_INLINE void btQuaternion::deSerialize(const struct btQuaternionDoubleData& dataIn)
+{
+	for (int i = 0; i < 4; i++)
+		m_floats[i] = (btScalar)dataIn.m_floats[i];
+}
 
-#endif //BT_SIMD__QUATERNION_H_
-
-
-
+#endif  //BT_SIMD__QUATERNION_H_
diff --git a/extern/bullet2/src/LinearMath/btQuickprof.cpp b/extern/bullet2/src/LinearMath/btQuickprof.cpp
index d88d965a4cc..86fd1d78122 100644
--- a/extern/bullet2/src/LinearMath/btQuickprof.cpp
+++ b/extern/bullet2/src/LinearMath/btQuickprof.cpp
@@ -14,12 +14,7 @@
 // Ogre (www.ogre3d.org).
 
 #include "btQuickprof.h"
-
-#ifndef BT_NO_PROFILE
-
-
-static btClock gProfileClock;
-
+#include "btThreads.h"
 
 #ifdef __CELLOS_LV2__
 #include <sys/sys_time.h>
@@ -27,9 +22,13 @@ static btClock gProfileClock;
 #include <stdio.h>
 #endif
 
-#if defined (SUNOS) || defined (__SUNOS__)
+#if defined(SUNOS) || defined(__SUNOS__)
 #include <stdio.h>
 #endif
+#ifdef __APPLE__
+#include <mach/mach_time.h>
+#include <TargetConditionals.h>
+#endif
 
 #if defined(WIN32) || defined(_WIN32)
 
@@ -40,41 +39,46 @@ static btClock gProfileClock;
 #define NOIME
 
 #ifdef _XBOX
-	#include <Xtl.h>
-#else //_XBOX
-	#include <windows.h>
+#include <Xtl.h>
+#else  //_XBOX
+#include <windows.h>
 
-#if WINVER <0x0602
+#if WINVER < 0x0602
 #define GetTickCount64 GetTickCount
 #endif
 
-#endif //_XBOX
+#endif  //_XBOX
 
 #include <time.h>
 
-
-#else //_WIN32
+#else  //_WIN32
 #include <sys/time.h>
-#endif //_WIN32
 
-#define mymin(a,b) (a > b ? a : b)
+#ifdef BT_LINUX_REALTIME
+//required linking against rt (librt)
+#include <time.h>
+#endif  //BT_LINUX_REALTIME
+
+#endif  //_WIN32
+
+#define mymin(a, b) (a > b ? a : b)
 
 struct btClockData
 {
-
 #ifdef BT_USE_WINDOWS_TIMERS
 	LARGE_INTEGER mClockFrequency;
 	LONGLONG mStartTick;
-	LONGLONG mPrevElapsedTime;
 	LARGE_INTEGER mStartTime;
 #else
 #ifdef __CELLOS_LV2__
-	uint64_t	mStartTime;
+	uint64_t mStartTime;
 #else
+#ifdef __APPLE__
+	uint64_t mStartTimeNano;
+#endif
 	struct timeval mStartTime;
 #endif
-#endif //__CELLOS_LV2__
-
+#endif  //__CELLOS_LV2__
 };
 
 ///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling.
@@ -104,23 +108,24 @@ btClock& btClock::operator=(const btClock& other)
 	return *this;
 }
 
-
-	/// Resets the initial reference time.
+/// Resets the initial reference time.
 void btClock::reset()
 {
 #ifdef BT_USE_WINDOWS_TIMERS
 	QueryPerformanceCounter(&m_data->mStartTime);
 	m_data->mStartTick = GetTickCount64();
-	m_data->mPrevElapsedTime = 0;
 #else
 #ifdef __CELLOS_LV2__
 
-	typedef uint64_t  ClockSize;
+	typedef uint64_t ClockSize;
 	ClockSize newTime;
 	//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
-	SYS_TIMEBASE_GET( newTime );
+	SYS_TIMEBASE_GET(newTime);
 	m_data->mStartTime = newTime;
 #else
+#ifdef __APPLE__
+	m_data->mStartTimeNano = mach_absolute_time();
+#endif
 	gettimeofday(&m_data->mStartTime, 0);
 #endif
 #endif
@@ -128,121 +133,136 @@ void btClock::reset()
 
 /// Returns the time in ms since the last call to reset or since
 /// the btClock was created.
-unsigned long int btClock::getTimeMilliseconds()
+unsigned long long int btClock::getTimeMilliseconds()
 {
 #ifdef BT_USE_WINDOWS_TIMERS
 	LARGE_INTEGER currentTime;
 	QueryPerformanceCounter(&currentTime);
 	LONGLONG elapsedTime = currentTime.QuadPart -
-		m_data->mStartTime.QuadPart;
-		// Compute the number of millisecond ticks elapsed.
+						   m_data->mStartTime.QuadPart;
+	// Compute the number of millisecond ticks elapsed.
 	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
-		// bridge, aka south bridge.  See Microsoft KB274323.)
-		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 -
-				m_data->mPrevElapsedTime);
-			m_data->mStartTime.QuadPart += msecAdjustment;
-			elapsedTime -= msecAdjustment;
-
-			// Recompute the number of millisecond ticks elapsed.
-			msecTicks = (unsigned long)(1000 * elapsedTime /
-				m_data->mClockFrequency.QuadPart);
-		}
-
-		// Store the current elapsed time for adjustments next time.
-		m_data->mPrevElapsedTime = elapsedTime;
+											  m_data->mClockFrequency.QuadPart);
 
-		return msecTicks;
+	return msecTicks;
 #else
 
 #ifdef __CELLOS_LV2__
-		uint64_t freq=sys_time_get_timebase_frequency();
-		double dFreq=((double) freq) / 1000.0;
-		typedef uint64_t  ClockSize;
-		ClockSize newTime;
-		SYS_TIMEBASE_GET( newTime );
-		//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
-
-		return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq);
+	uint64_t freq = sys_time_get_timebase_frequency();
+	double dFreq = ((double)freq) / 1000.0;
+	typedef uint64_t ClockSize;
+	ClockSize newTime;
+	SYS_TIMEBASE_GET(newTime);
+	//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+
+	return (unsigned long int)((double(newTime - m_data->mStartTime)) / dFreq);
 #else
 
-		struct timeval currentTime;
-		gettimeofday(&currentTime, 0);
-		return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000 +
-			(currentTime.tv_usec - m_data->mStartTime.tv_usec) / 1000;
-#endif //__CELLOS_LV2__
+	struct timeval currentTime;
+	gettimeofday(&currentTime, 0);
+	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
-	/// the Clock was created.
-unsigned long int btClock::getTimeMicroseconds()
+/// Returns the time in us since the last call to reset or since
+/// the Clock was created.
+unsigned long long int btClock::getTimeMicroseconds()
 {
 #ifdef BT_USE_WINDOWS_TIMERS
-		LARGE_INTEGER currentTime;
-		QueryPerformanceCounter(&currentTime);
-		LONGLONG elapsedTime = currentTime.QuadPart -
-			m_data->mStartTime.QuadPart;
-
-		// Compute the number of millisecond ticks elapsed.
-		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
-		// bridge, aka south bridge.  See Microsoft KB274323.)
-		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 -
-				m_data->mPrevElapsedTime);
-			m_data->mStartTime.QuadPart += msecAdjustment;
-			elapsedTime -= msecAdjustment;
-		}
-
-		// Store the current elapsed time for adjustments next time.
-		m_data->mPrevElapsedTime = elapsedTime;
+	//see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx
+	LARGE_INTEGER currentTime, elapsedTime;
 
-		// Convert to microseconds.
-		unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime /
-			m_data->mClockFrequency.QuadPart);
+	QueryPerformanceCounter(&currentTime);
+	elapsedTime.QuadPart = currentTime.QuadPart -
+						   m_data->mStartTime.QuadPart;
+	elapsedTime.QuadPart *= 1000000;
+	elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart;
 
-		return usecTicks;
+	return (unsigned long long)elapsedTime.QuadPart;
 #else
 
 #ifdef __CELLOS_LV2__
-		uint64_t freq=sys_time_get_timebase_frequency();
-		double dFreq=((double) freq)/ 1000000.0;
-		typedef uint64_t  ClockSize;
-		ClockSize newTime;
-		//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
-		SYS_TIMEBASE_GET( newTime );
-
-		return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq);
+	uint64_t freq = sys_time_get_timebase_frequency();
+	double dFreq = ((double)freq) / 1000000.0;
+	typedef uint64_t ClockSize;
+	ClockSize newTime;
+	//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+	SYS_TIMEBASE_GET(newTime);
+
+	return (unsigned long int)((double(newTime - m_data->mStartTime)) / dFreq);
 #else
 
-		struct timeval currentTime;
-		gettimeofday(&currentTime, 0);
-		return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000000 +
-			(currentTime.tv_usec - m_data->mStartTime.tv_usec);
-#endif//__CELLOS_LV2__
+	struct timeval currentTime;
+	gettimeofday(&currentTime, 0);
+	return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000000 +
+		   (currentTime.tv_usec - m_data->mStartTime.tv_usec);
+#endif  //__CELLOS_LV2__
 #endif
 }
 
+unsigned long long int btClock::getTimeNanoseconds()
+{
+#ifdef BT_USE_WINDOWS_TIMERS
+	//see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx
+	LARGE_INTEGER currentTime, elapsedTime;
+
+	QueryPerformanceCounter(&currentTime);
+	elapsedTime.QuadPart = currentTime.QuadPart -
+						   m_data->mStartTime.QuadPart;
+	elapsedTime.QuadPart *= 1000000000;
+	elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart;
+
+	return (unsigned long long)elapsedTime.QuadPart;
+#else
+
+#ifdef __CELLOS_LV2__
+	uint64_t freq = sys_time_get_timebase_frequency();
+	double dFreq = ((double)freq) / 1e9;
+	typedef uint64_t ClockSize;
+	ClockSize newTime;
+	//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+	SYS_TIMEBASE_GET(newTime);
+
+	return (unsigned long int)((double(newTime - m_data->mStartTime)) / dFreq);
+#else
+#ifdef __APPLE__
+	uint64_t ticks = mach_absolute_time() - m_data->mStartTimeNano;
+	static long double conversion = 0.0L;
+	if (0.0L == conversion)
+	{
+		// attempt to get conversion to nanoseconds
+		mach_timebase_info_data_t info;
+		int err = mach_timebase_info(&info);
+		if (err)
+		{
+			btAssert(0);
+			conversion = 1.;
+		}
+		conversion = info.numer / info.denom;
+	}
+	return (ticks * conversion);
 
+#else  //__APPLE__
 
-/// Returns the time in s since the last call to reset or since 
+#ifdef BT_LINUX_REALTIME
+	timespec ts;
+	clock_gettime(CLOCK_REALTIME, &ts);
+	return 1000000000 * ts.tv_sec + ts.tv_nsec;
+#else
+	struct timeval currentTime;
+	gettimeofday(&currentTime, 0);
+	return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1e9 +
+		   (currentTime.tv_usec - m_data->mStartTime.tv_usec) * 1000;
+#endif  //BT_LINUX_REALTIME
+
+#endif  //__APPLE__
+#endif  //__CELLOS_LV2__
+#endif
+}
+
+/// Returns the time in s since the last call to reset or since
 /// the Clock was created.
 btScalar btClock::getTimeSeconds()
 {
@@ -250,22 +270,21 @@ btScalar btClock::getTimeSeconds()
 	return btScalar(getTimeMicroseconds()) * microseconds_to_seconds;
 }
 
+#ifndef BT_NO_PROFILE
 
+static btClock gProfileClock;
 
-inline void Profile_Get_Ticks(unsigned long int * ticks)
+inline void Profile_Get_Ticks(unsigned long int* ticks)
 {
-	*ticks = gProfileClock.getTimeMicroseconds();
+	*ticks = (unsigned long int)gProfileClock.getTimeMicroseconds();
 }
 
 inline float Profile_Get_Tick_Rate(void)
 {
-//	return 1000000.f;
+	//	return 1000000.f;
 	return 1000.f;
-
 }
 
-
-
 /***************************************************************************************************
 **
 ** CProfileNode
@@ -281,35 +300,32 @@ inline float Profile_Get_Tick_Rate(void)
  * The name is assumed to be a static pointer, only the pointer is stored and compared for     *
  * efficiency reasons.                                                                         *
  *=============================================================================================*/
-CProfileNode::CProfileNode( const char * name, CProfileNode * parent ) :
-	Name( name ),
-	TotalCalls( 0 ),
-	TotalTime( 0 ),
-	StartTime( 0 ),
-	RecursionCounter( 0 ),
-	Parent( parent ),
-	Child( NULL ),
-	Sibling( NULL ),
-	m_userPtr(0)
+CProfileNode::CProfileNode(const char* name, CProfileNode* parent) : Name(name),
+																	 TotalCalls(0),
+																	 TotalTime(0),
+																	 StartTime(0),
+																	 RecursionCounter(0),
+																	 Parent(parent),
+																	 Child(NULL),
+																	 Sibling(NULL),
+																	 m_userPtr(0)
 {
 	Reset();
 }
 
-
-void	CProfileNode::CleanupMemory()
+void CProfileNode::CleanupMemory()
 {
-	delete ( Child);
+	delete (Child);
 	Child = NULL;
-	delete ( Sibling);
+	delete (Sibling);
 	Sibling = NULL;
 }
 
-CProfileNode::~CProfileNode( void )
+CProfileNode::~CProfileNode(void)
 {
 	CleanupMemory();
 }
 
-
 /***********************************************************************************************
  * INPUT:                                                                                      *
  * name - static string pointer to the name of the node we are searching for                   *
@@ -318,12 +334,14 @@ CProfileNode::~CProfileNode( void )
  * All profile names are assumed to be static strings so this function uses pointer compares   *
  * to find the named node.                                                                     *
  *=============================================================================================*/
-CProfileNode * CProfileNode::Get_Sub_Node( const char * name )
+CProfileNode* CProfileNode::Get_Sub_Node(const char* name)
 {
 	// Try to find this sub node
-	CProfileNode * child = Child;
-	while ( child ) {
-		if ( child->Name == name ) {
+	CProfileNode* child = Child;
+	while (child)
+	{
+		if (child->Name == name)
+		{
 			return child;
 		}
 		child = child->Sibling;
@@ -331,114 +349,211 @@ CProfileNode * CProfileNode::Get_Sub_Node( const char * name )
 
 	// We didn't find it, so add it
 
-	CProfileNode * node = new CProfileNode( name, this );
+	CProfileNode* node = new CProfileNode(name, this);
 	node->Sibling = Child;
 	Child = node;
 	return node;
 }
 
-
-void	CProfileNode::Reset( void )
+void CProfileNode::Reset(void)
 {
 	TotalCalls = 0;
 	TotalTime = 0.0f;
 
-
-	if ( Child ) {
+	if (Child)
+	{
 		Child->Reset();
 	}
-	if ( Sibling ) {
+	if (Sibling)
+	{
 		Sibling->Reset();
 	}
 }
 
-
-void	CProfileNode::Call( void )
+void CProfileNode::Call(void)
 {
 	TotalCalls++;
-	if (RecursionCounter++ == 0) {
+	if (RecursionCounter++ == 0)
+	{
 		Profile_Get_Ticks(&StartTime);
 	}
 }
 
-
-bool	CProfileNode::Return( 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;
+
+		time -= StartTime;
 		TotalTime += (float)time / Profile_Get_Tick_Rate();
 	}
-	return ( RecursionCounter == 0 );
+	return (RecursionCounter == 0);
 }
 
-
 /***************************************************************************************************
 **
 ** CProfileIterator
 **
 ***************************************************************************************************/
-CProfileIterator::CProfileIterator( CProfileNode * start )
+CProfileIterator::CProfileIterator(CProfileNode* start)
 {
 	CurrentParent = start;
 	CurrentChild = CurrentParent->Get_Child();
 }
 
-
-void	CProfileIterator::First(void)
+void CProfileIterator::First(void)
 {
 	CurrentChild = CurrentParent->Get_Child();
 }
 
-
-void	CProfileIterator::Next(void)
+void CProfileIterator::Next(void)
 {
 	CurrentChild = CurrentChild->Get_Sibling();
 }
 
-
-bool	CProfileIterator::Is_Done(void)
+bool CProfileIterator::Is_Done(void)
 {
 	return CurrentChild == NULL;
 }
 
-
-void	CProfileIterator::Enter_Child( int index )
+void CProfileIterator::Enter_Child(int index)
 {
 	CurrentChild = CurrentParent->Get_Child();
-	while ( (CurrentChild != NULL) && (index != 0) ) {
+	while ((CurrentChild != NULL) && (index != 0))
+	{
 		index--;
 		CurrentChild = CurrentChild->Get_Sibling();
 	}
 
-	if ( CurrentChild != NULL ) {
+	if (CurrentChild != NULL)
+	{
 		CurrentParent = CurrentChild;
 		CurrentChild = CurrentParent->Get_Child();
 	}
 }
 
-
-void	CProfileIterator::Enter_Parent( void )
+void CProfileIterator::Enter_Parent(void)
 {
-	if ( CurrentParent->Get_Parent() != NULL ) {
+	if (CurrentParent->Get_Parent() != NULL)
+	{
 		CurrentParent = CurrentParent->Get_Parent();
 	}
 	CurrentChild = CurrentParent->Get_Child();
 }
 
-
 /***************************************************************************************************
 **
 ** CProfileManager
 **
 ***************************************************************************************************/
 
-CProfileNode	CProfileManager::Root( "Root", NULL );
-CProfileNode *	CProfileManager::CurrentNode = &CProfileManager::Root;
-int				CProfileManager::FrameCounter = 0;
-unsigned long int			CProfileManager::ResetTime = 0;
+CProfileNode gRoots[BT_QUICKPROF_MAX_THREAD_COUNT] = {
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
+	CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL)};
+
+CProfileNode* gCurrentNodes[BT_QUICKPROF_MAX_THREAD_COUNT] =
+	{
+		&gRoots[0],
+		&gRoots[1],
+		&gRoots[2],
+		&gRoots[3],
+		&gRoots[4],
+		&gRoots[5],
+		&gRoots[6],
+		&gRoots[7],
+		&gRoots[8],
+		&gRoots[9],
+		&gRoots[10],
+		&gRoots[11],
+		&gRoots[12],
+		&gRoots[13],
+		&gRoots[14],
+		&gRoots[15],
+		&gRoots[16],
+		&gRoots[17],
+		&gRoots[18],
+		&gRoots[19],
+		&gRoots[20],
+		&gRoots[21],
+		&gRoots[22],
+		&gRoots[23],
+		&gRoots[24],
+		&gRoots[25],
+		&gRoots[26],
+		&gRoots[27],
+		&gRoots[28],
+		&gRoots[29],
+		&gRoots[30],
+		&gRoots[31],
+		&gRoots[32],
+		&gRoots[33],
+		&gRoots[34],
+		&gRoots[35],
+		&gRoots[36],
+		&gRoots[37],
+		&gRoots[38],
+		&gRoots[39],
+		&gRoots[40],
+		&gRoots[41],
+		&gRoots[42],
+		&gRoots[43],
+		&gRoots[44],
+		&gRoots[45],
+		&gRoots[46],
+		&gRoots[47],
+		&gRoots[48],
+		&gRoots[49],
+		&gRoots[50],
+		&gRoots[51],
+		&gRoots[52],
+		&gRoots[53],
+		&gRoots[54],
+		&gRoots[55],
+		&gRoots[56],
+		&gRoots[57],
+		&gRoots[58],
+		&gRoots[59],
+		&gRoots[60],
+		&gRoots[61],
+		&gRoots[62],
+		&gRoots[63],
+};
+
+int CProfileManager::FrameCounter = 0;
+unsigned long int CProfileManager::ResetTime = 0;
 
+CProfileIterator* CProfileManager::Get_Iterator(void)
+{
+	int threadIndex = btQuickprofGetCurrentThreadIndex2();
+	if ((threadIndex < 0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT)
+		return 0;
+
+	return new CProfileIterator(&gRoots[threadIndex]);
+}
+
+void CProfileManager::CleanupMemory(void)
+{
+	for (int i = 0; i < BT_QUICKPROF_MAX_THREAD_COUNT; i++)
+	{
+		gRoots[i].CleanupMemory();
+	}
+}
 
 /***********************************************************************************************
  * CProfileManager::Start_Profile -- Begin a named profile                                    *
@@ -453,57 +568,66 @@ unsigned long int			CProfileManager::ResetTime = 0;
  * The string used is assumed to be a static string; pointer compares are used throughout      *
  * the profiling code for efficiency.                                                          *
  *=============================================================================================*/
-void	CProfileManager::Start_Profile( const char * name )
+void CProfileManager::Start_Profile(const char* name)
 {
-	if (name != CurrentNode->Get_Name()) {
-		CurrentNode = CurrentNode->Get_Sub_Node( name );
+	int threadIndex = btQuickprofGetCurrentThreadIndex2();
+	if ((threadIndex < 0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT)
+		return;
+
+	if (name != gCurrentNodes[threadIndex]->Get_Name())
+	{
+		gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Sub_Node(name);
 	}
 
-	CurrentNode->Call();
+	gCurrentNodes[threadIndex]->Call();
 }
 
-
 /***********************************************************************************************
  * CProfileManager::Stop_Profile -- Stop timing and record the results.                       *
  *=============================================================================================*/
-void	CProfileManager::Stop_Profile( void )
+void CProfileManager::Stop_Profile(void)
 {
+	int threadIndex = btQuickprofGetCurrentThreadIndex2();
+	if ((threadIndex < 0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT)
+		return;
+
 	// Return will indicate whether we should back up to our parent (we may
 	// be profiling a recursive function)
-	if (CurrentNode->Return()) {
-		CurrentNode = CurrentNode->Get_Parent();
+	if (gCurrentNodes[threadIndex]->Return())
+	{
+		gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Parent();
 	}
 }
 
-
 /***********************************************************************************************
  * CProfileManager::Reset -- Reset the contents of the profiling system                       *
  *                                                                                             *
  *    This resets everything except for the tree structure.  All of the timing data is reset.  *
  *=============================================================================================*/
-void	CProfileManager::Reset( void )
+void CProfileManager::Reset(void)
 {
 	gProfileClock.reset();
-	Root.Reset();
-    Root.Call();
+	int threadIndex = btQuickprofGetCurrentThreadIndex2();
+	if ((threadIndex < 0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT)
+		return;
+	gRoots[threadIndex].Reset();
+	gRoots[threadIndex].Call();
 	FrameCounter = 0;
 	Profile_Get_Ticks(&ResetTime);
 }
 
-
 /***********************************************************************************************
  * CProfileManager::Increment_Frame_Counter -- Increment the frame counter                    *
  *=============================================================================================*/
-void CProfileManager::Increment_Frame_Counter( void )
+void CProfileManager::Increment_Frame_Counter(void)
 {
 	FrameCounter++;
 }
 
-
 /***********************************************************************************************
  * CProfileManager::Get_Time_Since_Reset -- returns the elapsed time since last reset         *
  *=============================================================================================*/
-float CProfileManager::Get_Time_Since_Reset( void )
+float CProfileManager::Get_Time_Since_Reset(void)
 {
 	unsigned long int time;
 	Profile_Get_Ticks(&time);
@@ -513,34 +637,34 @@ float CProfileManager::Get_Time_Since_Reset( void )
 
 #include <stdio.h>
 
-void	CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spacing)
+void CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spacing)
 {
 	profileIterator->First();
 	if (profileIterator->Is_Done())
 		return;
 
-	float accumulated_time=0,parent_time = profileIterator->Is_Root() ? CProfileManager::Get_Time_Since_Reset() : profileIterator->Get_Current_Parent_Total_Time();
+	float accumulated_time = 0, parent_time = profileIterator->Is_Root() ? CProfileManager::Get_Time_Since_Reset() : profileIterator->Get_Current_Parent_Total_Time();
 	int i;
 	int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset();
-	for (i=0;i<spacing;i++)	printf(".");
+	for (i = 0; i < spacing; i++) printf(".");
 	printf("----------------------------------\n");
-	for (i=0;i<spacing;i++)	printf(".");
-	printf("Profiling: %s (total running time: %.3f ms) ---\n",	profileIterator->Get_Current_Parent_Name(), parent_time );
+	for (i = 0; i < spacing; i++) printf(".");
+	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())
+	for (i = 0; !profileIterator->Is_Done(); i++, profileIterator->Next())
 	{
 		numChildren++;
 		float current_total_time = profileIterator->Get_Current_Total_Time();
 		accumulated_time += current_total_time;
 		float fraction = parent_time > SIMD_EPSILON ? (current_total_time / parent_time) * 100 : 0.f;
 		{
-			int i;	for (i=0;i<spacing;i++)	printf(".");
+			int i;
+			for (i = 0; i < spacing; i++) printf(".");
 		}
-		printf("%d -- %s (%.2f %%) :: %.3f ms / frame (%d calls)\n",i, profileIterator->Get_Current_Name(), fraction,(current_total_time / (double)frames_since_reset),profileIterator->Get_Current_Total_Calls());
+		printf("%d -- %s (%.2f %%) :: %.3f ms / frame (%d calls)\n", i, profileIterator->Get_Current_Name(), fraction, (current_total_time / (double)frames_since_reset), profileIterator->Get_Current_Total_Calls());
 		totalTime += current_total_time;
 		//recurse into children
 	}
@@ -549,30 +673,130 @@ void	CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spaci
 	{
 		//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 < 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++)
+	for (i = 0; i < numChildren; i++)
 	{
 		profileIterator->Enter_Child(i);
-		dumpRecursive(profileIterator,spacing+3);
+		dumpRecursive(profileIterator, spacing + 3);
 		profileIterator->Enter_Parent();
 	}
 }
 
-
-
-void	CProfileManager::dumpAll()
+void CProfileManager::dumpAll()
 {
 	CProfileIterator* profileIterator = 0;
 	profileIterator = CProfileManager::Get_Iterator();
 
-	dumpRecursive(profileIterator,0);
+	dumpRecursive(profileIterator, 0);
 
 	CProfileManager::Release_Iterator(profileIterator);
 }
 
 
+void btEnterProfileZoneDefault(const char* name)
+{
+}
+void btLeaveProfileZoneDefault()
+{
+}
 
+#else
+void btEnterProfileZoneDefault(const char* name)
+{
+}
+void btLeaveProfileZoneDefault()
+{
+}
+#endif  //BT_NO_PROFILE
+
+
+// clang-format off
+#if defined(_WIN32) && (defined(__MINGW32__) || defined(__MINGW64__))
+  #define BT_HAVE_TLS 1
+#elif __APPLE__ && !TARGET_OS_IPHONE
+  // TODO: Modern versions of iOS support TLS now with updated version checking.
+  #define BT_HAVE_TLS 1
+#elif __linux__
+  #define BT_HAVE_TLS 1
+#endif
+
+// __thread is broken on Andorid clang until r12b. See
+// https://github.com/android-ndk/ndk/issues/8
+#if defined(__ANDROID__) && defined(__clang__)
+  #if __has_include(<android/ndk-version.h>)
+    #include <android/ndk-version.h>
+  #endif  // __has_include(<android/ndk-version.h>)
+  #if defined(__NDK_MAJOR__) && \
+    ((__NDK_MAJOR__ < 12) || ((__NDK_MAJOR__ == 12) && (__NDK_MINOR__ < 1)))
+    #undef BT_HAVE_TLS
+  #endif
+#endif  // defined(__ANDROID__) && defined(__clang__)
+// clang-format on
+
+unsigned int btQuickprofGetCurrentThreadIndex2()
+{
+	const unsigned int kNullIndex = ~0U;
+
+#if BT_THREADSAFE
+	return btGetCurrentThreadIndex();
+#else
+#if defined(BT_HAVE_TLS)
+	static __thread unsigned int sThreadIndex = kNullIndex;
+#elif defined(_WIN32)
+	__declspec(thread) static unsigned int sThreadIndex = kNullIndex;
+#else
+	unsigned int sThreadIndex = 0;
+	return -1;
+#endif
+
+	static int gThreadCounter = 0;
+
+	if (sThreadIndex == kNullIndex)
+	{
+		sThreadIndex = gThreadCounter++;
+	}
+	return sThreadIndex;
+#endif  //BT_THREADSAFE
+}
+
+static btEnterProfileZoneFunc* bts_enterFunc = btEnterProfileZoneDefault;
+static btLeaveProfileZoneFunc* bts_leaveFunc = btLeaveProfileZoneDefault;
+
+void btEnterProfileZone(const char* name)
+{
+	(bts_enterFunc)(name);
+}
+void btLeaveProfileZone()
+{
+	(bts_leaveFunc)();
+}
+
+btEnterProfileZoneFunc* btGetCurrentEnterProfileZoneFunc()
+{
+	return bts_enterFunc;
+}
+btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc()
+{
+	return bts_leaveFunc;
+}
+
+void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc)
+{
+	bts_enterFunc = enterFunc;
+}
+void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc)
+{
+	bts_leaveFunc = leaveFunc;
+}
 
-#endif //BT_NO_PROFILE
+CProfileSample::CProfileSample(const char* name)
+{
+	btEnterProfileZone(name);
+}
+
+CProfileSample::~CProfileSample(void)
+{
+	btLeaveProfileZone();
+}
diff --git a/extern/bullet2/src/LinearMath/btQuickprof.h b/extern/bullet2/src/LinearMath/btQuickprof.h
index 362f62d6d40..990d401d507 100644
--- a/extern/bullet2/src/LinearMath/btQuickprof.h
+++ b/extern/bullet2/src/LinearMath/btQuickprof.h
@@ -7,26 +7,13 @@
 **
 ***************************************************************************************************/
 
-// 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).
 
-
-
 #ifndef BT_QUICK_PROF_H
 #define BT_QUICK_PROF_H
 
-//To disable built-in profiling, please comment out next line
-//#define BT_NO_PROFILE 1
-#ifndef BT_NO_PROFILE
-#include <stdio.h>//@todo remove this, backwards compatibility
 #include "btScalar.h"
-#include "btAlignedAllocator.h"
-#include <new>
-
-
-
-
-
 #define USE_BT_CLOCK 1
 
 #ifdef USE_BT_CLOCK
@@ -45,62 +32,88 @@ public:
 	/// Resets the initial reference time.
 	void reset();
 
-	/// 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 getTimeMilliseconds();
+	unsigned long long int getTimeMilliseconds();
 
-	/// 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 getTimeMicroseconds();
-	
-	/// Returns the time in s since the last call to reset or since 
+	unsigned long long int getTimeMicroseconds();
+
+	unsigned long long int getTimeNanoseconds();
+
+	/// Returns the time in s since the last call to reset or since
 	/// the Clock was created.
 	btScalar getTimeSeconds();
-	
+
 private:
 	struct btClockData* m_data;
 };
 
-#endif //USE_BT_CLOCK
+#endif  //USE_BT_CLOCK
 
+typedef void(btEnterProfileZoneFunc)(const char* msg);
+typedef void(btLeaveProfileZoneFunc)();
 
+btEnterProfileZoneFunc* btGetCurrentEnterProfileZoneFunc();
+btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc();
 
+void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc);
+void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc);
 
-///A node in the Profile Hierarchy Tree
-class	CProfileNode {
+#ifndef BT_ENABLE_PROFILE
+#define BT_NO_PROFILE 1
+#endif  //BT_NO_PROFILE
 
-public:
-	CProfileNode( const char * name, CProfileNode * parent );
-	~CProfileNode( void );
+const unsigned int BT_QUICKPROF_MAX_THREAD_COUNT = 64;
+
+//btQuickprofGetCurrentThreadIndex will return -1 if thread index cannot be determined,
+//otherwise returns thread index in range [0..maxThreads]
+unsigned int btQuickprofGetCurrentThreadIndex2();
 
-	CProfileNode * Get_Sub_Node( const char * name );
+#ifndef BT_NO_PROFILE
 
-	CProfileNode * Get_Parent( void )		{ return Parent; }
-	CProfileNode * Get_Sibling( void )		{ return Sibling; }
-	CProfileNode * Get_Child( void )			{ return Child; }
 
-	void				CleanupMemory();
-	void				Reset( void );
-	void				Call( void );
-	bool				Return( void );
+#include <stdio.h>  //@todo remove this, backwards compatibility
 
-	const char *	Get_Name( void )				{ return Name; }
-	int				Get_Total_Calls( void )		{ return TotalCalls; }
-	float				Get_Total_Time( void )		{ return TotalTime; }
-	void*			GetUserPointer() const {return m_userPtr;}
-	void			SetUserPointer(void* ptr) { m_userPtr = ptr;}
-protected:
+#include "btAlignedAllocator.h"
+#include <new>
+
+///A node in the Profile Hierarchy Tree
+class CProfileNode
+{
+public:
+	CProfileNode(const char* name, CProfileNode* parent);
+	~CProfileNode(void);
+
+	CProfileNode* Get_Sub_Node(const char* name);
+
+	CProfileNode* Get_Parent(void) { return Parent; }
+	CProfileNode* Get_Sibling(void) { return Sibling; }
+	CProfileNode* Get_Child(void) { return Child; }
+
+	void CleanupMemory();
+	void Reset(void);
+	void Call(void);
+	bool Return(void);
 
-	const char *	Name;
-	int				TotalCalls;
-	float				TotalTime;
-	unsigned long int			StartTime;
-	int				RecursionCounter;
+	const char* Get_Name(void) { return Name; }
+	int Get_Total_Calls(void) { return TotalCalls; }
+	float Get_Total_Time(void) { return TotalTime; }
+	void* GetUserPointer() const { return m_userPtr; }
+	void SetUserPointer(void* ptr) { m_userPtr = ptr; }
 
-	CProfileNode *	Parent;
-	CProfileNode *	Child;
-	CProfileNode *	Sibling;
-	void*	m_userPtr;
+protected:
+	const char* Name;
+	int TotalCalls;
+	float TotalTime;
+	unsigned long int StartTime;
+	int RecursionCounter;
+
+	CProfileNode* Parent;
+	CProfileNode* Child;
+	CProfileNode* Sibling;
+	void* m_userPtr;
 };
 
 ///An iterator to navigate through the tree
@@ -108,101 +121,80 @@ class CProfileIterator
 {
 public:
 	// Access all the children of the current parent
-	void				First(void);
-	void				Next(void);
-	bool				Is_Done(void);
-	bool                Is_Root(void) { return (CurrentParent->Get_Parent() == 0); }
+	void First(void);
+	void Next(void);
+	bool Is_Done(void);
+	bool Is_Root(void) { return (CurrentParent->Get_Parent() == 0); }
 
-	void				Enter_Child( int index );		// Make the given child the new parent
-	void				Enter_Largest_Child( void );	// Make the largest child the new parent
-	void				Enter_Parent( void );			// Make the current parent's parent the new parent
+	void Enter_Child(int index);     // Make the given child the new parent
+	void Enter_Largest_Child(void);  // Make the largest child the new parent
+	void Enter_Parent(void);         // Make the current parent's parent the new parent
 
 	// Access the current child
-	const char *	Get_Current_Name( void )			{ return CurrentChild->Get_Name(); }
-	int				Get_Current_Total_Calls( void )	{ return CurrentChild->Get_Total_Calls(); }
-	float				Get_Current_Total_Time( void )	{ return CurrentChild->Get_Total_Time(); }
+	const char* Get_Current_Name(void) { return CurrentChild->Get_Name(); }
+	int Get_Current_Total_Calls(void) { return CurrentChild->Get_Total_Calls(); }
+	float Get_Current_Total_Time(void) { return CurrentChild->Get_Total_Time(); }
 
-	void*	Get_Current_UserPointer( void )			{ return CurrentChild->GetUserPointer(); }
-	void	Set_Current_UserPointer(void* ptr) {CurrentChild->SetUserPointer(ptr);}
+	void* Get_Current_UserPointer(void) { return CurrentChild->GetUserPointer(); }
+	void Set_Current_UserPointer(void* ptr) { CurrentChild->SetUserPointer(ptr); }
 	// Access the current parent
-	const char *	Get_Current_Parent_Name( void )			{ return CurrentParent->Get_Name(); }
-	int				Get_Current_Parent_Total_Calls( void )	{ return CurrentParent->Get_Total_Calls(); }
-	float				Get_Current_Parent_Total_Time( void )	{ return CurrentParent->Get_Total_Time(); }
-
-	
+	const char* Get_Current_Parent_Name(void) { return CurrentParent->Get_Name(); }
+	int Get_Current_Parent_Total_Calls(void) { return CurrentParent->Get_Total_Calls(); }
+	float Get_Current_Parent_Total_Time(void) { return CurrentParent->Get_Total_Time(); }
 
 protected:
+	CProfileNode* CurrentParent;
+	CProfileNode* CurrentChild;
 
-	CProfileNode *	CurrentParent;
-	CProfileNode *	CurrentChild;
-	
-
-	CProfileIterator( CProfileNode * start );
-	friend	class		CProfileManager;
+	CProfileIterator(CProfileNode* start);
+	friend class CProfileManager;
 };
 
-
 ///The Manager for the Profile system
-class	CProfileManager {
+class CProfileManager
+{
 public:
-	static	void						Start_Profile( const char * name );
-	static	void						Stop_Profile( void );
+	static void Start_Profile(const char* name);
+	static void Stop_Profile(void);
 
-	static	void						CleanupMemory(void)
-	{
-		Root.CleanupMemory();
-	}
+	static void CleanupMemory(void);
+	//	{
+	//		Root.CleanupMemory();
+	//	}
 
-	static	void						Reset( void );
-	static	void						Increment_Frame_Counter( void );
-	static	int						Get_Frame_Count_Since_Reset( void )		{ return FrameCounter; }
-	static	float						Get_Time_Since_Reset( void );
+	static void Reset(void);
+	static void Increment_Frame_Counter(void);
+	static int Get_Frame_Count_Since_Reset(void) { return FrameCounter; }
+	static float Get_Time_Since_Reset(void);
 
-	static	CProfileIterator *	Get_Iterator( void )	
-	{ 
-		
-		return new CProfileIterator( &Root ); 
-	}
-	static	void						Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); }
+	static CProfileIterator* Get_Iterator(void);
+	//	{
+	//
+	//		return new CProfileIterator( &Root );
+	//	}
+	static void Release_Iterator(CProfileIterator* iterator) { delete (iterator); }
 
-	static void	dumpRecursive(CProfileIterator* profileIterator, int spacing);
+	static void dumpRecursive(CProfileIterator* profileIterator, int spacing);
 
-	static void	dumpAll();
+	static void dumpAll();
 
 private:
-	static	CProfileNode			Root;
-	static	CProfileNode *			CurrentNode;
-	static	int						FrameCounter;
-	static	unsigned long int					ResetTime;
+	static int FrameCounter;
+	static unsigned long int ResetTime;
 };
 
+#endif  //#ifndef BT_NO_PROFILE
 
 ///ProfileSampleClass is a simple way to profile a function's scope
 ///Use the BT_PROFILE macro at the start of scope to time
-class	CProfileSample {
+class CProfileSample
+{
 public:
-	CProfileSample( const char * name )
-	{ 
-		CProfileManager::Start_Profile( name ); 
-	}
-
-	~CProfileSample( void )					
-	{ 
-		CProfileManager::Stop_Profile(); 
-	}
-};
-
-
-#define	BT_PROFILE( name )			CProfileSample __profile( name )
-
-#else
+	CProfileSample(const char* name);
 
-#define	BT_PROFILE( name )
-
-#endif //#ifndef BT_NO_PROFILE
-
-
-
-#endif //BT_QUICK_PROF_H
+	~CProfileSample(void);
+};
 
+#define BT_PROFILE(name) CProfileSample __profile(name)
 
+#endif  //BT_QUICK_PROF_H
diff --git a/extern/bullet2/src/LinearMath/btRandom.h b/extern/bullet2/src/LinearMath/btRandom.h
index 4cbfc6bfe9f..e659af86055 100644
--- a/extern/bullet2/src/LinearMath/btRandom.h
+++ b/extern/bullet2/src/LinearMath/btRandom.h
@@ -12,8 +12,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef BT_GEN_RANDOM_H
 #define BT_GEN_RANDOM_H
 
@@ -24,8 +22,8 @@ subject to the following restrictions:
 
 #define GEN_RAND_MAX UINT_MAX
 
-SIMD_FORCE_INLINE void         GEN_srand(unsigned int seed) { init_genrand(seed); }
-SIMD_FORCE_INLINE unsigned int GEN_rand()                   { return genrand_int32(); }
+SIMD_FORCE_INLINE void GEN_srand(unsigned int seed) { init_genrand(seed); }
+SIMD_FORCE_INLINE unsigned int GEN_rand() { return genrand_int32(); }
 
 #else
 
@@ -33,10 +31,9 @@ SIMD_FORCE_INLINE unsigned int GEN_rand()                   { return genrand_int
 
 #define GEN_RAND_MAX RAND_MAX
 
-SIMD_FORCE_INLINE void         GEN_srand(unsigned int seed) { srand(seed); } 
-SIMD_FORCE_INLINE unsigned int GEN_rand()                   { return rand(); }
+SIMD_FORCE_INLINE void GEN_srand(unsigned int seed) { srand(seed); }
+SIMD_FORCE_INLINE unsigned int GEN_rand() { return rand(); }
 
 #endif
 
-#endif //BT_GEN_RANDOM_H
-
+#endif  //BT_GEN_RANDOM_H
diff --git a/extern/bullet2/src/LinearMath/btReducedVector.cpp b/extern/bullet2/src/LinearMath/btReducedVector.cpp
new file mode 100644
index 00000000000..1539584e7ec
--- /dev/null
+++ b/extern/bullet2/src/LinearMath/btReducedVector.cpp
@@ -0,0 +1,170 @@
+//
+//  btReducedVector.cpp
+//  LinearMath
+//
+//  Created by Xuchen Han on 4/4/20.
+//
+#include <stdio.h>
+#include "btReducedVector.h"
+#include <cmath>
+
+// returns the projection of this onto other
+btReducedVector btReducedVector::proj(const btReducedVector& other) const
+{
+    btReducedVector ret(m_sz);
+    btScalar other_length2 = other.length2();
+    if (other_length2 < SIMD_EPSILON)
+    {
+        return ret;
+    }
+    return other*(this->dot(other))/other_length2;
+}
+
+void btReducedVector::normalize()
+{
+    if (this->length2() < SIMD_EPSILON)
+    {
+        m_indices.clear();
+        m_vecs.clear();
+        return;
+    }
+    *this /= std::sqrt(this->length2());
+}
+
+bool btReducedVector::testAdd() const
+{
+    int sz = 5;
+    btAlignedObjectArray<int> id1;
+    id1.push_back(1);
+    id1.push_back(3);
+    btAlignedObjectArray<btVector3> v1;
+    v1.push_back(btVector3(1,0,1));
+    v1.push_back(btVector3(3,1,5));
+    btAlignedObjectArray<int> id2;
+    id2.push_back(2);
+    id2.push_back(3);
+    id2.push_back(5);
+    btAlignedObjectArray<btVector3> v2;
+    v2.push_back(btVector3(2,3,1));
+    v2.push_back(btVector3(3,4,9));
+    v2.push_back(btVector3(0,4,0));
+    btAlignedObjectArray<int> id3;
+    id3.push_back(1);
+    id3.push_back(2);
+    id3.push_back(3);
+    id3.push_back(5);
+    btAlignedObjectArray<btVector3> v3;
+    v3.push_back(btVector3(1,0,1));
+    v3.push_back(btVector3(2,3,1));
+    v3.push_back(btVector3(6,5,14));
+    v3.push_back(btVector3(0,4,0));
+    btReducedVector rv1(sz, id1, v1);
+    btReducedVector rv2(sz, id2, v2);
+    btReducedVector ans(sz, id3, v3);
+    bool ret = ((ans == rv1+rv2) && (ans == rv2+rv1));
+    if (!ret)
+        printf("btReducedVector testAdd failed\n");
+    return ret;
+}
+
+bool btReducedVector::testMinus() const
+{
+    int sz = 5;
+    btAlignedObjectArray<int> id1;
+    id1.push_back(1);
+    id1.push_back(3);
+    btAlignedObjectArray<btVector3> v1;
+    v1.push_back(btVector3(1,0,1));
+    v1.push_back(btVector3(3,1,5));
+    btAlignedObjectArray<int> id2;
+    id2.push_back(2);
+    id2.push_back(3);
+    id2.push_back(5);
+    btAlignedObjectArray<btVector3> v2;
+    v2.push_back(btVector3(2,3,1));
+    v2.push_back(btVector3(3,4,9));
+    v2.push_back(btVector3(0,4,0));
+    btAlignedObjectArray<int> id3;
+    id3.push_back(1);
+    id3.push_back(2);
+    id3.push_back(3);
+    id3.push_back(5);
+    btAlignedObjectArray<btVector3> v3;
+    v3.push_back(btVector3(-1,-0,-1));
+    v3.push_back(btVector3(2,3,1));
+    v3.push_back(btVector3(0,3,4));
+    v3.push_back(btVector3(0,4,0));
+    btReducedVector rv1(sz, id1, v1);
+    btReducedVector rv2(sz, id2, v2);
+    btReducedVector ans(sz, id3, v3);
+    bool ret = (ans == rv2-rv1);
+    if (!ret)
+        printf("btReducedVector testMinus failed\n");
+    return ret;
+}
+
+bool btReducedVector::testDot() const
+{
+    int sz = 5;
+    btAlignedObjectArray<int> id1;
+    id1.push_back(1);
+    id1.push_back(3);
+    btAlignedObjectArray<btVector3> v1;
+    v1.push_back(btVector3(1,0,1));
+    v1.push_back(btVector3(3,1,5));
+    btAlignedObjectArray<int> id2;
+    id2.push_back(2);
+    id2.push_back(3);
+    id2.push_back(5);
+    btAlignedObjectArray<btVector3> v2;
+    v2.push_back(btVector3(2,3,1));
+    v2.push_back(btVector3(3,4,9));
+    v2.push_back(btVector3(0,4,0));
+    btReducedVector rv1(sz, id1, v1);
+    btReducedVector rv2(sz, id2, v2);
+    btScalar ans = 58;
+    bool ret = (ans == rv2.dot(rv1) && ans == rv1.dot(rv2));
+    ans = 14+16+9+16+81;
+    ret &= (ans==rv2.dot(rv2));
+    
+    if (!ret)
+        printf("btReducedVector testDot failed\n");
+    return ret;
+}
+
+bool btReducedVector::testMultiply() const
+{
+    int sz = 5;
+    btAlignedObjectArray<int> id1;
+    id1.push_back(1);
+    id1.push_back(3);
+    btAlignedObjectArray<btVector3> v1;
+    v1.push_back(btVector3(1,0,1));
+    v1.push_back(btVector3(3,1,5));
+    btScalar s = 2;
+    btReducedVector rv1(sz, id1, v1);
+    btAlignedObjectArray<int> id2;
+    id2.push_back(1);
+    id2.push_back(3);
+    btAlignedObjectArray<btVector3> v2;
+    v2.push_back(btVector3(2,0,2));
+    v2.push_back(btVector3(6,2,10));
+    btReducedVector ans(sz, id2, v2);
+    bool ret = (ans == rv1*s);
+    if (!ret)
+        printf("btReducedVector testMultiply failed\n");
+    return ret;
+}
+
+void btReducedVector::test() const
+{
+    bool ans = testAdd() && testMinus() && testDot() && testMultiply();
+    if (ans)
+    {
+        printf("All tests passed\n");
+    }
+    else
+    {
+        printf("Tests failed\n");
+    }
+}
diff --git a/extern/bullet2/src/LinearMath/btReducedVector.h b/extern/bullet2/src/LinearMath/btReducedVector.h
new file mode 100644
index 00000000000..83b5e581e5e
--- /dev/null
+++ b/extern/bullet2/src/LinearMath/btReducedVector.h
@@ -0,0 +1,320 @@
+//
+//  btReducedVectors.h
+//  BulletLinearMath
+//
+//  Created by Xuchen Han on 4/4/20.
+//
+#ifndef btReducedVectors_h
+#define btReducedVectors_h
+#include "btVector3.h"
+#include "btMatrix3x3.h"
+#include "btAlignedObjectArray.h"
+#include <stdio.h>
+#include <vector>
+#include <algorithm>
+struct TwoInts
+{
+    int a,b;
+};
+inline bool operator<(const TwoInts& A, const TwoInts& B)
+{
+    return A.b < B.b;
+}
+
+
+// A helper vector type used for CG projections
+class btReducedVector
+{
+public:
+    btAlignedObjectArray<int> m_indices;
+    btAlignedObjectArray<btVector3> m_vecs;
+    int m_sz; // all m_indices value < m_sz
+public:
+	btReducedVector():m_sz(0)
+	{
+		m_indices.resize(0);
+		m_vecs.resize(0);
+        m_indices.clear();
+        m_vecs.clear();
+	}
+	
+    btReducedVector(int sz): m_sz(sz)
+    {
+        m_indices.resize(0);
+        m_vecs.resize(0);
+        m_indices.clear();
+        m_vecs.clear();
+    }
+    
+    btReducedVector(int sz, const btAlignedObjectArray<int>& indices, const btAlignedObjectArray<btVector3>& vecs): m_sz(sz), m_indices(indices), m_vecs(vecs)
+    {
+    }
+    
+    void simplify()
+    {
+        btAlignedObjectArray<int> old_indices(m_indices);
+        btAlignedObjectArray<btVector3> old_vecs(m_vecs);
+        m_indices.resize(0);
+        m_vecs.resize(0);
+        m_indices.clear();
+        m_vecs.clear();
+        for (int i = 0; i < old_indices.size(); ++i)
+        {
+            if (old_vecs[i].length2() > SIMD_EPSILON)
+            {
+                m_indices.push_back(old_indices[i]);
+                m_vecs.push_back(old_vecs[i]);
+            }
+        }
+    }
+    
+    btReducedVector operator+(const btReducedVector& other)
+    {
+		btReducedVector ret(m_sz);
+		int i=0, j=0;
+		while (i < m_indices.size() && j < other.m_indices.size())
+		{
+			if (m_indices[i] < other.m_indices[j])
+			{
+				ret.m_indices.push_back(m_indices[i]);
+				ret.m_vecs.push_back(m_vecs[i]);
+				++i;
+			}
+			else if (m_indices[i] > other.m_indices[j])
+			{
+				ret.m_indices.push_back(other.m_indices[j]);
+				ret.m_vecs.push_back(other.m_vecs[j]);
+				++j;
+			}
+			else
+			{
+				ret.m_indices.push_back(other.m_indices[j]);
+				ret.m_vecs.push_back(m_vecs[i] + other.m_vecs[j]);
+				++i; ++j;
+			}
+		}
+		while (i < m_indices.size())
+		{
+			ret.m_indices.push_back(m_indices[i]);
+			ret.m_vecs.push_back(m_vecs[i]);
+			++i;
+		}
+		while (j < other.m_indices.size())
+		{
+			ret.m_indices.push_back(other.m_indices[j]);
+			ret.m_vecs.push_back(other.m_vecs[j]);
+			++j;
+		}
+        ret.simplify();
+        return ret;
+    }
+
+    btReducedVector operator-()
+    {
+        btReducedVector ret(m_sz);
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            ret.m_indices.push_back(m_indices[i]);
+            ret.m_vecs.push_back(-m_vecs[i]);
+        }
+        ret.simplify();
+        return ret;
+    }
+    
+    btReducedVector operator-(const btReducedVector& other)
+    {
+		btReducedVector ret(m_sz);
+		int i=0, j=0;
+		while (i < m_indices.size() && j < other.m_indices.size())
+		{
+			if (m_indices[i] < other.m_indices[j])
+			{
+				ret.m_indices.push_back(m_indices[i]);
+				ret.m_vecs.push_back(m_vecs[i]);
+				++i;
+			}
+			else if (m_indices[i] > other.m_indices[j])
+			{
+				ret.m_indices.push_back(other.m_indices[j]);
+				ret.m_vecs.push_back(-other.m_vecs[j]);
+				++j;
+			}
+			else
+			{
+				ret.m_indices.push_back(other.m_indices[j]);
+				ret.m_vecs.push_back(m_vecs[i] - other.m_vecs[j]);
+				++i; ++j;
+			}
+		}
+		while (i < m_indices.size())
+		{
+			ret.m_indices.push_back(m_indices[i]);
+			ret.m_vecs.push_back(m_vecs[i]);
+			++i;
+		}
+		while (j < other.m_indices.size())
+		{
+			ret.m_indices.push_back(other.m_indices[j]);
+			ret.m_vecs.push_back(-other.m_vecs[j]);
+			++j;
+		}
+        ret.simplify();
+		return ret;
+    }
+    
+    bool operator==(const btReducedVector& other) const
+    {
+        if (m_sz != other.m_sz)
+            return false;
+        if (m_indices.size() != other.m_indices.size())
+            return false;
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            if (m_indices[i] != other.m_indices[i] || m_vecs[i] != other.m_vecs[i])
+            {
+                return false;
+            }
+        }
+        return true;
+    }
+    
+    bool operator!=(const btReducedVector& other) const
+    {
+        return !(*this == other);
+    }
+	
+	btReducedVector& operator=(const btReducedVector& other)
+	{
+		if (this == &other)
+		{
+			return *this;
+		}
+        m_sz = other.m_sz;
+		m_indices.copyFromArray(other.m_indices);
+		m_vecs.copyFromArray(other.m_vecs);
+		return *this;
+	}
+    
+    btScalar dot(const btReducedVector& other) const
+    {
+        btScalar ret = 0;
+        int j = 0;
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            while (j < other.m_indices.size() && other.m_indices[j] < m_indices[i])
+            {
+                ++j;
+            }
+            if (j < other.m_indices.size() && other.m_indices[j] == m_indices[i])
+            {
+                ret += m_vecs[i].dot(other.m_vecs[j]);
+//                ++j;
+            }
+        }
+        return ret;
+    }
+    
+    btScalar dot(const btAlignedObjectArray<btVector3>& other) const
+    {
+        btScalar ret = 0;
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            ret += m_vecs[i].dot(other[m_indices[i]]);
+        }
+        return ret;
+    }
+    
+    btScalar length2() const
+    {
+        return this->dot(*this);
+    }
+	
+	void normalize();
+    
+    // returns the projection of this onto other
+    btReducedVector proj(const btReducedVector& other) const;
+    
+    bool testAdd() const;
+    
+    bool testMinus() const;
+    
+    bool testDot() const;
+    
+    bool testMultiply() const;
+    
+    void test() const;
+    
+    void print() const
+    {
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            printf("%d: (%f, %f, %f)/", m_indices[i], m_vecs[i][0],m_vecs[i][1],m_vecs[i][2]);
+        }
+        printf("\n");
+    }
+    
+    
+    void sort()
+    {
+        std::vector<TwoInts> tuples;
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            TwoInts ti;
+            ti.a = i;
+            ti.b = m_indices[i];
+            tuples.push_back(ti);
+        }
+        std::sort(tuples.begin(), tuples.end());
+        btAlignedObjectArray<int> new_indices;
+        btAlignedObjectArray<btVector3> new_vecs;
+        for (int i = 0; i < tuples.size(); ++i)
+        {
+            new_indices.push_back(tuples[i].b);
+            new_vecs.push_back(m_vecs[tuples[i].a]);
+        }
+        m_indices = new_indices;
+        m_vecs = new_vecs;
+    }
+};
+
+SIMD_FORCE_INLINE btReducedVector operator*(const btReducedVector& v, btScalar s)
+{
+    btReducedVector ret(v.m_sz);
+    for (int i = 0; i < v.m_indices.size(); ++i)
+    {
+        ret.m_indices.push_back(v.m_indices[i]);
+        ret.m_vecs.push_back(s*v.m_vecs[i]);
+    }
+    ret.simplify();
+    return ret;
+}
+
+SIMD_FORCE_INLINE btReducedVector operator*(btScalar s, const btReducedVector& v)
+{
+    return v*s;
+}
+
+SIMD_FORCE_INLINE btReducedVector operator/(const btReducedVector& v, btScalar s)
+{
+	return v * (1.0/s);
+}
+
+SIMD_FORCE_INLINE btReducedVector& operator/=(btReducedVector& v, btScalar s)
+{
+	v = v/s;
+	return v;
+}
+
+SIMD_FORCE_INLINE btReducedVector& operator+=(btReducedVector& v1, const btReducedVector& v2)
+{
+	v1 = v1+v2;
+	return v1;
+}
+
+SIMD_FORCE_INLINE btReducedVector& operator-=(btReducedVector& v1, const btReducedVector& v2)
+{
+	v1 = v1-v2;
+	return v1;
+}
+
+#endif /* btReducedVectors_h */
diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h
index 898669f86e2..86d94e89749 100644
--- a/extern/bullet2/src/LinearMath/btScalar.h
+++ b/extern/bullet2/src/LinearMath/btScalar.h
@@ -12,61 +12,93 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef BT_SCALAR_H
 #define BT_SCALAR_H
-#if defined(_MSC_VER) && defined(__clang__) /* clang supplies it's own overloads already */
-#define BT_NO_SIMD_OPERATOR_OVERLOADS
-#endif
 
 #ifdef BT_MANAGED_CODE
 //Aligned data types not supported in managed code
 #pragma unmanaged
 #endif
 
-
 #include <math.h>
-#include <stdlib.h>//size_t for MSVC 6.0
+#include <stdlib.h>  //size_t for MSVC 6.0
 #include <float.h>
 
 /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
-#define BT_BULLET_VERSION 284
+#define BT_BULLET_VERSION 289
 
-inline int	btGetVersion()
+inline int btGetVersion()
 {
 	return BT_BULLET_VERSION;
 }
 
-#if defined(DEBUG) || defined (_DEBUG)
-#define BT_DEBUG
+inline int btIsDoublePrecision()
+{
+  #ifdef BT_USE_DOUBLE_PRECISION
+  return true;
+  #else
+  return false;
+  #endif
+}
+
+
+// The following macro "BT_NOT_EMPTY_FILE" can be put into a file
+// in order suppress the MS Visual C++ Linker warning 4221
+//
+// warning LNK4221: no public symbols found; archive member will be inaccessible
+//
+// This warning occurs on PC and XBOX when a file compiles out completely
+// has no externally visible symbols which may be dependant on configuration
+// #defines and options.
+//
+// see more https://stackoverflow.com/questions/1822887/what-is-the-best-way-to-eliminate-ms-visual-c-linker-warning-warning-lnk422
+
+#if defined(_MSC_VER)
+#define BT_NOT_EMPTY_FILE_CAT_II(p, res) res
+#define BT_NOT_EMPTY_FILE_CAT_I(a, b) BT_NOT_EMPTY_FILE_CAT_II(~, a##b)
+#define BT_NOT_EMPTY_FILE_CAT(a, b) BT_NOT_EMPTY_FILE_CAT_I(a, b)
+#define BT_NOT_EMPTY_FILE                                      \
+	namespace                                                  \
+	{                                                          \
+	char BT_NOT_EMPTY_FILE_CAT(NoEmptyFileDummy, __COUNTER__); \
+	}
+#else
+#define BT_NOT_EMPTY_FILE
 #endif
 
+// clang and most formatting tools don't support indentation of preprocessor guards, so turn it off
+// clang-format off
+#if defined(DEBUG) || defined (_DEBUG)
+	#define BT_DEBUG
+#endif
 
 #ifdef _WIN32
-
-		#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
-
-			#define SIMD_FORCE_INLINE inline
-			#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
+	#if  defined(__GNUC__)	// it should handle both MINGW and CYGWIN
+        	#define SIMD_FORCE_INLINE        __inline__ __attribute__((always_inline))
+        	#define ATTRIBUTE_ALIGNED16(a)   a __attribute__((aligned(16)))
+        	#define ATTRIBUTE_ALIGNED64(a)   a __attribute__((aligned(64)))
+        	#define ATTRIBUTE_ALIGNED128(a)  a __attribute__((aligned(128)))
+    	#elif ( defined(_MSC_VER) && _MSC_VER < 1300 )
+		#define SIMD_FORCE_INLINE inline
+		#define ATTRIBUTE_ALIGNED16(a) a
+		#define ATTRIBUTE_ALIGNED64(a) a
+		#define ATTRIBUTE_ALIGNED128(a) a
+	#elif defined(_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//__MINGW32__
+		//#define BT_HAS_ALIGNED_ALLOCATOR
+		#pragma warning(disable : 4324) // disable padding warning
 //			#pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
-//			#pragma warning(disable:4996) //Turn off warnings about deprecated C routines
+		#pragma warning(disable:4996) //Turn off warnings about deprecated C routines
 //			#pragma warning(disable:4786) // Disable the "debug name too long" warning
 
-			#define SIMD_FORCE_INLINE __forceinline
-			#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
-			#define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
-			#define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
+		#define SIMD_FORCE_INLINE __forceinline
+		#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
+		#define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
+		#define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
 		#ifdef _XBOX
 			#define BT_USE_VMX128
 
@@ -78,15 +110,20 @@ inline int	btGetVersion()
 #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))
+
+#ifdef __clang__
+#define __BT_DISABLE_SSE__
+#endif
+#ifndef __BT_DISABLE_SSE__
 			#if _MSC_VER>1400
 				#define BT_USE_SIMD_VECTOR3
 			#endif
-
 			#define BT_USE_SSE
+#endif//__BT_DISABLE_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 //disable this cause blender targets sse2 
+			#define BT_ALLOW_SSE4
 #endif //(_MSC_FULL_VER >= 160040219)
 
 			//BT_USE_SSE_IN_API is disabled under Windows by default, because 
@@ -102,28 +139,28 @@ inline int	btGetVersion()
 
 		#endif//_XBOX
 
-		#endif //__MINGW32__
-
-#ifdef BT_DEBUG
-	#if defined(_MSC_VER) && !defined(__clang__)
-		#include <stdio.h>
-		#define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);__debugbreak();	}}
-	#else//_MSC_VER
-		#include <assert.h>
-		#define btAssert assert
-	#endif//_MSC_VER
-#else
+	#endif //__MINGW32__
+
+	#ifdef BT_DEBUG
+		#ifdef _MSC_VER
+			#include <stdio.h>
+			#define btAssert(x) { if(!(x)){printf("Assert " __FILE__ ":%u (%s)\n", __LINE__, #x);__debugbreak();	}}
+		#else//_MSC_VER
+			#include <assert.h>
+			#define btAssert assert
+		#endif//_MSC_VER
+	#else
 		#define btAssert(x)
-#endif
+	#endif
 		//btFullAssert is optional, slows down a lot
 		#define btFullAssert(x)
 
 		#define btLikely(_c)  _c
 		#define btUnlikely(_c) _c
 
-#else
+#else//_WIN32
 	
-#if defined	(__CELLOS_LV2__)
+	#if defined	(__CELLOS_LV2__)
 		#define SIMD_FORCE_INLINE inline __attribute__((always_inline))
 		#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
 		#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
@@ -131,411 +168,431 @@ inline int	btGetVersion()
 		#ifndef assert
 		#include <assert.h>
 		#endif
-#ifdef BT_DEBUG
-#ifdef __SPU__
-#include <spu_printf.h>
-#define printf spu_printf
-	#define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}}
-#else
-	#define btAssert assert
-#endif
+		#ifdef BT_DEBUG
+			#ifdef __SPU__
+				#include <spu_printf.h>
+				#define printf spu_printf
+				#define btAssert(x) {if(!(x)){printf("Assert " __FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}}
+			#else
+				#define btAssert assert
+			#endif
 	
-#else
-		#define btAssert(x)
-#endif
+		#else//BT_DEBUG
+				#define btAssert(x)
+		#endif//BT_DEBUG
 		//btFullAssert is optional, slows down a lot
 		#define btFullAssert(x)
 
 		#define btLikely(_c)  _c
 		#define btUnlikely(_c) _c
 
-#else
+	#else//defined	(__CELLOS_LV2__)
 
-#ifdef USE_LIBSPE2
+		#ifdef USE_LIBSPE2
 
-		#define SIMD_FORCE_INLINE __inline
-		#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
-		#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
-		#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
-		#ifndef assert
-		#include <assert.h>
-		#endif
-#ifdef BT_DEBUG
-		#define btAssert assert
-#else
-		#define btAssert(x)
-#endif
-		//btFullAssert is optional, slows down a lot
-		#define btFullAssert(x)
+			#define SIMD_FORCE_INLINE __inline
+			#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+			#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
+			#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+			#ifndef assert
+			#include <assert.h>
+			#endif
+	#ifdef BT_DEBUG
+			#define btAssert assert
+	#else
+			#define btAssert(x)
+	#endif
+			//btFullAssert is optional, slows down a lot
+			#define btFullAssert(x)
 
 
-		#define btLikely(_c)   __builtin_expect((_c), 1)
-		#define btUnlikely(_c) __builtin_expect((_c), 0)
+			#define btLikely(_c)   __builtin_expect((_c), 1)
+			#define btUnlikely(_c) __builtin_expect((_c), 0)
 		
 
-#else
+		#else//USE_LIBSPE2
 	//non-windows systems
 
-#if (defined (__APPLE__) && (!defined (BT_USE_DOUBLE_PRECISION)))
-    #if defined (__i386__) || defined (__x86_64__)
-		#define BT_USE_SIMD_VECTOR3
-		#define BT_USE_SSE
-		//BT_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries
-		//if apps run into issues, we will disable the next line
-		#define BT_USE_SSE_IN_API
-        #ifdef BT_USE_SSE
-            // include appropriate SSE level
-            #if defined (__SSE4_1__)
-                #include <smmintrin.h>
-            #elif defined (__SSSE3__)
-                #include <tmmintrin.h>
-            #elif defined (__SSE3__)
-                #include <pmmintrin.h>
-            #else
-                #include <emmintrin.h>
-            #endif
-        #endif //BT_USE_SSE
-    #elif defined( __ARM_NEON__ )
-        #ifdef __clang__
-            #define BT_USE_NEON 1
-			#define BT_USE_SIMD_VECTOR3
+			#if (defined (__APPLE__) && (!defined (BT_USE_DOUBLE_PRECISION)))
+				#if defined (__i386__) || defined (__x86_64__)
+					#define BT_USE_SIMD_VECTOR3
+					#define BT_USE_SSE
+					//BT_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries
+					//if apps run into issues, we will disable the next line
+					#define BT_USE_SSE_IN_API
+					#ifdef BT_USE_SSE
+						// include appropriate SSE level
+						#if defined (__SSE4_1__)
+							#include <smmintrin.h>
+						#elif defined (__SSSE3__)
+							#include <tmmintrin.h>
+						#elif defined (__SSE3__)
+							#include <pmmintrin.h>
+						#else
+							#include <emmintrin.h>
+						#endif
+					#endif //BT_USE_SSE
+				#elif defined( __ARM_NEON__ )
+					#ifdef __clang__
+						#define BT_USE_NEON 1
+						#define BT_USE_SIMD_VECTOR3
 		
-            #if defined BT_USE_NEON && defined (__clang__)
-                #include <arm_neon.h>
-            #endif//BT_USE_NEON
-       #endif //__clang__
-    #endif//__arm__
-
-	#define SIMD_FORCE_INLINE inline __attribute__ ((always_inline))
-///@todo: check out alignment methods for other platforms/compilers
-	#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
-	#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
-	#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
-	#ifndef assert
-	#include <assert.h>
-	#endif
-
-	#if defined(DEBUG) || defined (_DEBUG)
-	 #if defined (__i386__) || defined (__x86_64__)
-	#include <stdio.h>
-	 #define btAssert(x)\
-	{\
-	if(!(x))\
-	{\
-		printf("Assert %s in line %d, file %s\n",#x, __LINE__, __FILE__);\
-		asm volatile ("int3");\
-	}\
-	}
-	#else//defined (__i386__) || defined (__x86_64__)
-		#define btAssert assert
-	#endif//defined (__i386__) || defined (__x86_64__)
-	#else//defined(DEBUG) || defined (_DEBUG)
-		#define btAssert(x)
-	#endif//defined(DEBUG) || defined (_DEBUG)
-
-	//btFullAssert is optional, slows down a lot
-	#define btFullAssert(x)
-	#define btLikely(_c)  _c
-	#define btUnlikely(_c) _c
-
-#else
-
-		#define SIMD_FORCE_INLINE inline
-		///@todo: check out alignment methods for other platforms/compilers
-		///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
-		///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
-		///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
-		#define ATTRIBUTE_ALIGNED16(a) a
-		#define ATTRIBUTE_ALIGNED64(a) a
-		#define ATTRIBUTE_ALIGNED128(a) a
-		#ifndef assert
-		#include <assert.h>
-		#endif
-
-#if defined(DEBUG) || defined (_DEBUG)
-		#define btAssert assert
-#else
-		#define btAssert(x)
-#endif
-
-		//btFullAssert is optional, slows down a lot
-		#define btFullAssert(x)
-		#define btLikely(_c)  _c
-		#define btUnlikely(_c) _c
-#endif //__APPLE__ 
-
-#endif // LIBSPE2
-
-#endif	//__CELLOS_LV2__
-#endif
+						#if defined BT_USE_NEON && defined (__clang__)
+							#include <arm_neon.h>
+						#endif//BT_USE_NEON
+				   #endif //__clang__
+				#endif//__arm__
+
+				#define SIMD_FORCE_INLINE inline __attribute__ ((always_inline))
+			///@todo: check out alignment methods for other platforms/compilers
+				#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+				#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
+				#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+				#ifndef assert
+				#include <assert.h>
+				#endif
+
+				#if defined(DEBUG) || defined (_DEBUG)
+				 #if defined (__i386__) || defined (__x86_64__)
+				#include <stdio.h>
+				 #define btAssert(x)\
+				{\
+				if(!(x))\
+				{\
+					printf("Assert %s in line %d, file %s\n",#x, __LINE__, __FILE__);\
+					asm volatile ("int3");\
+				}\
+				}
+				#else//defined (__i386__) || defined (__x86_64__)
+					#define btAssert assert
+				#endif//defined (__i386__) || defined (__x86_64__)
+				#else//defined(DEBUG) || defined (_DEBUG)
+					#define btAssert(x)
+				#endif//defined(DEBUG) || defined (_DEBUG)
+
+				//btFullAssert is optional, slows down a lot
+				#define btFullAssert(x)
+				#define btLikely(_c)  _c
+				#define btUnlikely(_c) _c
+
+			#else//__APPLE__
+
+				#define SIMD_FORCE_INLINE inline
+				///@todo: check out alignment methods for other platforms/compilers
+				///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+				///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
+				///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+				#define ATTRIBUTE_ALIGNED16(a) a
+				#define ATTRIBUTE_ALIGNED64(a) a
+				#define ATTRIBUTE_ALIGNED128(a) a
+				#ifndef assert
+				#include <assert.h>
+				#endif
+
+				#if defined(DEBUG) || defined (_DEBUG)
+					#define btAssert assert
+				#else
+					#define btAssert(x)
+				#endif
+
+				//btFullAssert is optional, slows down a lot
+				#define btFullAssert(x)
+				#define btLikely(_c)  _c
+				#define btUnlikely(_c) _c
+			#endif //__APPLE__ 
+		#endif // LIBSPE2
+	#endif	//__CELLOS_LV2__
+#endif//_WIN32
 
 
 ///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
 #if defined(BT_USE_DOUBLE_PRECISION)
-
-typedef double btScalar;
-//this number could be bigger in double precision
-#define BT_LARGE_FLOAT 1e30
+	typedef double btScalar;
+	//this number could be bigger in double precision
+	#define BT_LARGE_FLOAT 1e30
 #else
-
-typedef float btScalar;
-//keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
-#define BT_LARGE_FLOAT 1e18f
+	typedef float btScalar;
+	//keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
+	#define BT_LARGE_FLOAT 1e18f
 #endif
 
 #ifdef BT_USE_SSE
-typedef __m128 btSimdFloat4;
-#endif//BT_USE_SSE
+	typedef __m128 btSimdFloat4;
+#endif  //BT_USE_SSE
 
-#if defined (BT_USE_SSE)
-//#if defined BT_USE_SSE_IN_API && defined (BT_USE_SSE)
-#ifdef _WIN32
+#if defined(BT_USE_SSE)
+	//#if defined BT_USE_SSE_IN_API && defined (BT_USE_SSE)
+	#ifdef _WIN32
 
-#ifndef BT_NAN
-static int btNanMask = 0x7F800001;
-#define BT_NAN (*(float*)&btNanMask)
-#endif
-
-#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)
-#ifndef BT_NO_SIMD_OPERATOR_OVERLOADS
-inline __m128 operator + (const __m128 A, const __m128 B)
-{
-    return _mm_add_ps(A, B);
-}
+		#ifndef BT_NAN
+			static int btNanMask = 0x7F800001;
+			#define BT_NAN (*(float *)&btNanMask)
+		#endif
 
-inline __m128 operator - (const __m128 A, const __m128 B)
-{
-    return _mm_sub_ps(A, B);
-}
+		#ifndef BT_INFINITY
+			static int btInfinityMask = 0x7F800000;
+			#define BT_INFINITY (*(float *)&btInfinityMask)
+			inline int btGetInfinityMask()  //suppress stupid compiler warning
+			{
+				return btInfinityMask;
+			}
+		#endif
 
-inline __m128 operator * (const __m128 A, const __m128 B)
-{
-    return _mm_mul_ps(A, B);
-}
-#endif //BT_NO_SIMD_OPERATOR_OVERLOADS
 
-#define btCastfTo128i(a) (_mm_castps_si128(a))
-#define btCastfTo128d(a) (_mm_castps_pd(a))
-#define btCastiTo128f(a) (_mm_castsi128_ps(a))
-#define btCastdTo128f(a) (_mm_castpd_ps(a))
-#define btCastdTo128i(a) (_mm_castpd_si128(a))
-#define btAssign128(r0,r1,r2,r3) _mm_setr_ps(r0,r1,r2,r3)
 
-#else//_WIN32
+	//use this, in case there are clashes (such as xnamath.h)
+	#ifndef BT_NO_SIMD_OPERATOR_OVERLOADS
+	inline __m128 operator+(const __m128 A, const __m128 B)
+	{
+		return _mm_add_ps(A, B);
+	}
 
-#define btCastfTo128i(a) ((__m128i)(a))
-#define btCastfTo128d(a) ((__m128d)(a))
-#define btCastiTo128f(a)  ((__m128) (a))
-#define btCastdTo128f(a) ((__m128) (a))
-#define btCastdTo128i(a) ((__m128i)(a))
-#define btAssign128(r0,r1,r2,r3) (__m128){r0,r1,r2,r3}
-#define BT_INFINITY INFINITY
-#define BT_NAN NAN
-#endif//_WIN32
-#else
+	inline __m128 operator-(const __m128 A, const __m128 B)
+	{
+		return _mm_sub_ps(A, B);
+	}
 
-#ifdef BT_USE_NEON
+	inline __m128 operator*(const __m128 A, const __m128 B)
+	{
+		return _mm_mul_ps(A, B);
+	}
+	#endif  //BT_NO_SIMD_OPERATOR_OVERLOADS
+
+	#define btCastfTo128i(a) (_mm_castps_si128(a))
+	#define btCastfTo128d(a) (_mm_castps_pd(a))
+	#define btCastiTo128f(a) (_mm_castsi128_ps(a))
+	#define btCastdTo128f(a) (_mm_castpd_ps(a))
+	#define btCastdTo128i(a) (_mm_castpd_si128(a))
+	#define btAssign128(r0, r1, r2, r3) _mm_setr_ps(r0, r1, r2, r3)
+
+	#else  //_WIN32
+
+		#define btCastfTo128i(a) ((__m128i)(a))
+		#define btCastfTo128d(a) ((__m128d)(a))
+		#define btCastiTo128f(a) ((__m128)(a))
+		#define btCastdTo128f(a) ((__m128)(a))
+		#define btCastdTo128i(a) ((__m128i)(a))
+		#define btAssign128(r0, r1, r2, r3) \
+			(__m128) { r0, r1, r2, r3 }
+		#define BT_INFINITY INFINITY
+		#define BT_NAN NAN
+	#endif  //_WIN32
+#else//BT_USE_SSE
+
+	#ifdef BT_USE_NEON
 	#include <arm_neon.h>
 
 	typedef float32x4_t btSimdFloat4;
 	#define BT_INFINITY INFINITY
 	#define BT_NAN NAN
-	#define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3}
-#else//BT_USE_NEON
+	#define btAssign128(r0, r1, r2, r3) \
+		(float32x4_t) { r0, r1, r2, r3 }
+	#else  //BT_USE_NEON
 
 	#ifndef BT_INFINITY
-		struct btInfMaskConverter
-		{
-		        union {
-		                float mask;
-		                int intmask;
-		        };
-		        btInfMaskConverter(int mask=0x7F800000)
-		        :intmask(mask)
-		        {
-		        }
+	struct btInfMaskConverter
+	{
+		union {
+			float mask;
+			int intmask;
 		};
-		static btInfMaskConverter btInfinityMask = 0x7F800000;
-		#define BT_INFINITY (btInfinityMask.mask)
-		inline int btGetInfinityMask()//suppress stupid compiler warning
+		btInfMaskConverter(int _mask = 0x7F800000)
+			: intmask(_mask)
 		{
-		        return btInfinityMask.intmask;
 		}
+	};
+	static btInfMaskConverter btInfinityMask = 0x7F800000;
+	#define BT_INFINITY (btInfinityMask.mask)
+	inline int btGetInfinityMask()  //suppress stupid compiler warning
+	{
+		return btInfinityMask.intmask;
+	}
 	#endif
-#endif//BT_USE_NEON
+	#endif  //BT_USE_NEON
 
-#endif //BT_USE_SSE
+#endif  //BT_USE_SSE
 
 #ifdef BT_USE_NEON
-#include <arm_neon.h>
-
-typedef float32x4_t btSimdFloat4;
-#define BT_INFINITY INFINITY
-#define BT_NAN NAN
-#define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3}
-#endif
-
-
-
-
-
-#define BT_DECLARE_ALIGNED_ALLOCATOR() \
-   SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes)   { return btAlignedAlloc(sizeInBytes,16); }   \
-   SIMD_FORCE_INLINE void  operator delete(void* ptr)         { btAlignedFree(ptr); }   \
-   SIMD_FORCE_INLINE void* operator new(size_t, void* ptr)   { return ptr; }   \
-   SIMD_FORCE_INLINE void  operator delete(void*, void*)      { }   \
-   SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes)   { return btAlignedAlloc(sizeInBytes,16); }   \
-   SIMD_FORCE_INLINE void  operator delete[](void* ptr)         { btAlignedFree(ptr); }   \
-   SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr)   { return ptr; }   \
-   SIMD_FORCE_INLINE void  operator delete[](void*, void*)      { }   \
+	#include <arm_neon.h>
 
+	typedef float32x4_t btSimdFloat4;
+	#define BT_INFINITY INFINITY
+	#define BT_NAN NAN
+	#define btAssign128(r0, r1, r2, r3) \
+		(float32x4_t) { r0, r1, r2, r3 }
+#endif//BT_USE_NEON
 
+#define BT_DECLARE_ALIGNED_ALLOCATOR()                                                                     \
+	SIMD_FORCE_INLINE void *operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes, 16); }   \
+	SIMD_FORCE_INLINE void operator delete(void *ptr) { btAlignedFree(ptr); }                              \
+	SIMD_FORCE_INLINE void *operator new(size_t, void *ptr) { return ptr; }                                \
+	SIMD_FORCE_INLINE void operator delete(void *, void *) {}                                              \
+	SIMD_FORCE_INLINE void *operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes, 16); } \
+	SIMD_FORCE_INLINE void operator delete[](void *ptr) { btAlignedFree(ptr); }                            \
+	SIMD_FORCE_INLINE void *operator new[](size_t, void *ptr) { return ptr; }                              \
+	SIMD_FORCE_INLINE void operator delete[](void *, void *) {}
 
 #if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS)
-		
-SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); }
-SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }
-SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); }
-SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
-SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
-SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { if (x<btScalar(-1))	x=btScalar(-1); if (x>btScalar(1))	x=btScalar(1); return acos(x); }
-SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { if (x<btScalar(-1))	x=btScalar(-1); if (x>btScalar(1))	x=btScalar(1); return asin(x); }
-SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
-SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
-SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
-SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
-SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
-SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); }
 
-#else
-		
-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;
-#endif
-#else
-	return sqrtf(y); 
-#endif
-}
-SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }
-SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
-SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
-SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
-SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { 
-	if (x<btScalar(-1))	
-		x=btScalar(-1); 
-	if (x>btScalar(1))	
-		x=btScalar(1);
-	return acosf(x); 
-}
-SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { 
-	if (x<btScalar(-1))	
-		x=btScalar(-1); 
-	if (x>btScalar(1))	
-		x=btScalar(1);
-	return asinf(x); 
-}
-SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
-SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
-SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
-SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
-SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
-SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
-	
-#endif
+	SIMD_FORCE_INLINE btScalar btSqrt(btScalar x)
+	{
+		return sqrt(x);
+	}
+	SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }
+	SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); }
+	SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
+	SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
+	SIMD_FORCE_INLINE btScalar btAcos(btScalar x)
+	{
+		if (x < btScalar(-1)) x = btScalar(-1);
+		if (x > btScalar(1)) x = btScalar(1);
+		return acos(x);
+	}
+	SIMD_FORCE_INLINE btScalar btAsin(btScalar x)
+	{
+		if (x < btScalar(-1)) x = btScalar(-1);
+		if (x > btScalar(1)) x = btScalar(1);
+		return asin(x);
+	}
+	SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
+	SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
+	SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
+	SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
+	SIMD_FORCE_INLINE btScalar btPow(btScalar x, btScalar y) { return pow(x, y); }
+	SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y) { return fmod(x, y); }
+
+#else//BT_USE_DOUBLE_PRECISION
 
-#define SIMD_PI           btScalar(3.1415926535897932384626433832795029)
-#define SIMD_2_PI         (btScalar(2.0) * SIMD_PI)
-#define SIMD_HALF_PI      (SIMD_PI * btScalar(0.5))
+	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;
+	#endif
+	#else
+		return sqrtf(y);
+	#endif
+	}
+	SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }
+	SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
+	SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
+	SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
+	SIMD_FORCE_INLINE btScalar btAcos(btScalar x)
+	{
+		if (x < btScalar(-1))
+			x = btScalar(-1);
+		if (x > btScalar(1))
+			x = btScalar(1);
+		return acosf(x);
+	}
+	SIMD_FORCE_INLINE btScalar btAsin(btScalar x)
+	{
+		if (x < btScalar(-1))
+			x = btScalar(-1);
+		if (x > btScalar(1))
+			x = btScalar(1);
+		return asinf(x);
+	}
+	SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
+	SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
+	SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
+	SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
+	SIMD_FORCE_INLINE btScalar btPow(btScalar x, btScalar y) { return powf(x, y); }
+	SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y) { return fmodf(x, y); }
+
+#endif//BT_USE_DOUBLE_PRECISION
+
+#define SIMD_PI btScalar(3.1415926535897932384626433832795029)
+#define SIMD_2_PI (btScalar(2.0) * SIMD_PI)
+#define SIMD_HALF_PI (SIMD_PI * btScalar(0.5))
 #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
-#define SIMD_DEGS_PER_RAD  (btScalar(360.0) / SIMD_2_PI)
+#define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI)
 #define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
-
-#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))		/* reciprocal square root */
-#define btRecip(x) (btScalar(1.0)/btScalar(x))
+#define btRecipSqrt(x) ((btScalar)(btScalar(1.0) / btSqrt(btScalar(x)))) /* reciprocal square root */
+#define btRecip(x) (btScalar(1.0) / btScalar(x))
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define SIMD_EPSILON      DBL_EPSILON
-#define SIMD_INFINITY     DBL_MAX
-#define BT_ONE			1.0
-#define BT_ZERO			0.0
-#define BT_TWO			2.0
-#define BT_HALF			0.5
+	#define SIMD_EPSILON DBL_EPSILON
+	#define SIMD_INFINITY DBL_MAX
+	#define BT_ONE 1.0
+	#define BT_ZERO 0.0
+	#define BT_TWO 2.0
+	#define BT_HALF 0.5
 #else
-#define SIMD_EPSILON      FLT_EPSILON
-#define SIMD_INFINITY     FLT_MAX
-#define BT_ONE			1.0f
-#define BT_ZERO			0.0f
-#define BT_TWO			2.0f
-#define BT_HALF			0.5f
+	#define SIMD_EPSILON FLT_EPSILON
+	#define SIMD_INFINITY FLT_MAX
+	#define BT_ONE 1.0f
+	#define BT_ZERO 0.0f
+	#define BT_TWO 2.0f
+	#define BT_HALF 0.5f
 #endif
 
-SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) 
+// clang-format on
+
+SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x)
 {
 	btScalar coeff_1 = SIMD_PI / 4.0f;
 	btScalar coeff_2 = 3.0f * coeff_1;
 	btScalar abs_y = btFabs(y);
 	btScalar angle;
-	if (x >= 0.0f) {
+	if (x >= 0.0f)
+	{
 		btScalar r = (x - abs_y) / (x + abs_y);
 		angle = coeff_1 - coeff_1 * r;
-	} else {
+	}
+	else
+	{
 		btScalar r = (x + abs_y) / (abs_y - x);
 		angle = coeff_2 - coeff_1 * r;
 	}
 	return (y < 0.0f) ? -angle : angle;
 }
 
-SIMD_FORCE_INLINE bool      btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
+SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
 
-SIMD_FORCE_INLINE bool	btEqual(btScalar a, btScalar eps) {
+SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps)
+{
 	return (((a) <= eps) && !((a) < -eps));
 }
-SIMD_FORCE_INLINE bool	btGreaterEqual (btScalar a, btScalar eps) {
+SIMD_FORCE_INLINE bool btGreaterEqual(btScalar a, btScalar eps)
+{
 	return (!((a) <= eps));
 }
 
-
-SIMD_FORCE_INLINE int       btIsNegative(btScalar x) {
-    return x < btScalar(0.0) ? 1 : 0;
+SIMD_FORCE_INLINE int btIsNegative(btScalar x)
+{
+	return x < btScalar(0.0) ? 1 : 0;
 }
 
 SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; }
 SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; }
 
-#define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
+#define BT_DECLARE_HANDLE(name) \
+	typedef struct name##__     \
+	{                           \
+		int unused;             \
+	} * name
 
 #ifndef btFsel
 SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c)
@@ -543,60 +600,57 @@ SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c)
 	return a >= 0 ? b : c;
 }
 #endif
-#define btFsels(a,b,c) (btScalar)btFsel(a,b,c)
-
+#define btFsels(a, b, c) (btScalar) btFsel(a, b, c)
 
 SIMD_FORCE_INLINE bool btMachineIsLittleEndian()
 {
-   long int i = 1;
-   const char *p = (const char *) &i;
-   if (p[0] == 1)  // Lowest address contains the least significant byte
-	   return true;
-   else
-	   return false;
+	long int i = 1;
+	const char *p = (const char *)&i;
+	if (p[0] == 1)  // Lowest address contains the least significant byte
+		return true;
+	else
+		return false;
 }
 
-
-
 ///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
 ///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
-SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) 
+SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
 {
-    // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
-    // Rely on positive value or'ed with its negative having sign bit on
-    // and zero value or'ed with its negative (which is still zero) having sign bit off 
-    // Use arithmetic shift right, shifting the sign bit through all 32 bits
-    unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
-    unsigned testEqz = ~testNz;
-    return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); 
+	// Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
+	// Rely on positive value or'ed with its negative having sign bit on
+	// and zero value or'ed with its negative (which is still zero) having sign bit off
+	// Use arithmetic shift right, shifting the sign bit through all 32 bits
+	unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
+	unsigned testEqz = ~testNz;
+	return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
 }
 SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
 {
-    unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
-    unsigned testEqz = ~testNz; 
-    return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
+	unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
+	unsigned testEqz = ~testNz;
+	return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
 }
 SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
 {
 #ifdef BT_HAVE_NATIVE_FSEL
-    return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
+	return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
 #else
-    return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; 
+	return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
 #endif
 }
 
-template<typename T> SIMD_FORCE_INLINE void btSwap(T& a, T& b)
+template <typename T>
+SIMD_FORCE_INLINE void btSwap(T &a, T &b)
 {
 	T tmp = a;
 	a = b;
 	b = tmp;
 }
 
-
 //PCK: endian swapping functions
 SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val)
 {
-	return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8)  | ((val & 0x000000ff) << 24));
+	return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
 }
 
 SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val)
@@ -611,127 +665,127 @@ SIMD_FORCE_INLINE unsigned btSwapEndian(int val)
 
 SIMD_FORCE_INLINE unsigned short btSwapEndian(short val)
 {
-	return btSwapEndian((unsigned short) val);
+	return btSwapEndian((unsigned short)val);
 }
 
 ///btSwapFloat uses using char pointers to swap the endianness
 ////btSwapFloat/btSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
-///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. 
-///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. 
-///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you. 
+///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754.
+///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception.
+///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you.
 ///so instead of returning a float/double, we return integer/long long integer
-SIMD_FORCE_INLINE unsigned int  btSwapEndianFloat(float d)
+SIMD_FORCE_INLINE unsigned int btSwapEndianFloat(float d)
 {
-    unsigned int a = 0;
-    unsigned char *dst = (unsigned char *)&a;
-    unsigned char *src = (unsigned char *)&d;
-
-    dst[0] = src[3];
-    dst[1] = src[2];
-    dst[2] = src[1];
-    dst[3] = src[0];
-    return a;
+	unsigned int a = 0;
+	unsigned char *dst = (unsigned char *)&a;
+	unsigned char *src = (unsigned char *)&d;
+
+	dst[0] = src[3];
+	dst[1] = src[2];
+	dst[2] = src[1];
+	dst[3] = src[0];
+	return a;
 }
 
 // unswap using char pointers
-SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a) 
+SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a)
 {
-    float d = 0.0f;
-    unsigned char *src = (unsigned char *)&a;
-    unsigned char *dst = (unsigned char *)&d;
+	float d = 0.0f;
+	unsigned char *src = (unsigned char *)&a;
+	unsigned char *dst = (unsigned char *)&d;
 
-    dst[0] = src[3];
-    dst[1] = src[2];
-    dst[2] = src[1];
-    dst[3] = src[0];
+	dst[0] = src[3];
+	dst[1] = src[2];
+	dst[2] = src[1];
+	dst[3] = src[0];
 
-    return d;
+	return d;
 }
 
-
 // swap using char pointers
-SIMD_FORCE_INLINE void  btSwapEndianDouble(double d, unsigned char* dst)
+SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char *dst)
 {
-    unsigned char *src = (unsigned char *)&d;
-
-    dst[0] = src[7];
-    dst[1] = src[6];
-    dst[2] = src[5];
-    dst[3] = src[4];
-    dst[4] = src[3];
-    dst[5] = src[2];
-    dst[6] = src[1];
-    dst[7] = src[0];
-
+	unsigned char *src = (unsigned char *)&d;
+
+	dst[0] = src[7];
+	dst[1] = src[6];
+	dst[2] = src[5];
+	dst[3] = src[4];
+	dst[4] = src[3];
+	dst[5] = src[2];
+	dst[6] = src[1];
+	dst[7] = src[0];
 }
 
 // unswap using char pointers
-SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src) 
+SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src)
 {
-    double d = 0.0;
-    unsigned char *dst = (unsigned char *)&d;
-
-    dst[0] = src[7];
-    dst[1] = src[6];
-    dst[2] = src[5];
-    dst[3] = src[4];
-    dst[4] = src[3];
-    dst[5] = src[2];
-    dst[6] = src[1];
-    dst[7] = src[0];
+	double d = 0.0;
+	unsigned char *dst = (unsigned char *)&d;
+
+	dst[0] = src[7];
+	dst[1] = src[6];
+	dst[2] = src[5];
+	dst[3] = src[4];
+	dst[4] = src[3];
+	dst[5] = src[2];
+	dst[6] = src[1];
+	dst[7] = src[0];
 
 	return d;
 }
 
-template<typename T>
-SIMD_FORCE_INLINE void btSetZero(T* a, int n)
+template <typename T>
+SIMD_FORCE_INLINE void btSetZero(T *a, int n)
 {
-  T* acurr = a;
-  size_t ncurr = n;
-  while (ncurr > 0) 
-  {
-    *(acurr++) = 0;
-    --ncurr;
-  }
+	T *acurr = a;
+	size_t ncurr = n;
+	while (ncurr > 0)
+	{
+		*(acurr++) = 0;
+		--ncurr;
+	}
 }
 
-
 SIMD_FORCE_INLINE btScalar btLargeDot(const btScalar *a, const btScalar *b, int n)
-{  
-  btScalar p0,q0,m0,p1,q1,m1,sum;
-  sum = 0;
-  n -= 2;
-  while (n >= 0) {
-    p0 = a[0]; q0 = b[0];
-    m0 = p0 * q0;
-    p1 = a[1]; q1 = b[1];
-    m1 = p1 * q1;
-    sum += m0;
-    sum += m1;
-    a += 2;
-    b += 2;
-    n -= 2;
-  }
-  n += 2;
-  while (n > 0) {
-    sum += (*a) * (*b);
-    a++;
-    b++;
-    n--;
-  }
-  return sum;
+{
+	btScalar p0, q0, m0, p1, q1, m1, sum;
+	sum = 0;
+	n -= 2;
+	while (n >= 0)
+	{
+		p0 = a[0];
+		q0 = b[0];
+		m0 = p0 * q0;
+		p1 = a[1];
+		q1 = b[1];
+		m1 = p1 * q1;
+		sum += m0;
+		sum += m1;
+		a += 2;
+		b += 2;
+		n -= 2;
+	}
+	n += 2;
+	while (n > 0)
+	{
+		sum += (*a) * (*b);
+		a++;
+		b++;
+		n--;
+	}
+	return sum;
 }
 
-
 // returns normalized value in range [-SIMD_PI, SIMD_PI]
-SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) 
+SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
 {
 	angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
-	if(angleInRadians < -SIMD_PI)
+	if (angleInRadians < -SIMD_PI)
 	{
 		return angleInRadians + SIMD_2_PI;
 	}
-	else if(angleInRadians > SIMD_PI)
+	else if (angleInRadians > SIMD_PI)
 	{
 		return angleInRadians - SIMD_2_PI;
 	}
@@ -741,45 +795,38 @@ SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
 	}
 }
 
-
-
 ///rudimentary class to provide type info
 struct btTypedObject
 {
 	btTypedObject(int objectType)
-		:m_objectType(objectType)
+		: m_objectType(objectType)
 	{
 	}
-	int	m_objectType;
+	int m_objectType;
 	inline int getObjectType() const
 	{
 		return m_objectType;
 	}
 };
 
-
-  
 ///align a pointer to the provided alignment, upwards
-template <typename T>T* btAlignPointer(T* unalignedPtr, size_t alignment)
+template <typename T>
+T *btAlignPointer(T *unalignedPtr, size_t alignment)
 {
-		
 	struct btConvertPointerSizeT
 	{
-		union 
-		{
-				T* ptr;
-				size_t integer;
+		union {
+			T *ptr;
+			size_t integer;
 		};
 	};
-    btConvertPointerSizeT converter;
-    
-    
+	btConvertPointerSizeT converter;
+
 	const size_t bit_mask = ~(alignment - 1);
-    converter.ptr = unalignedPtr;
-	converter.integer += alignment-1;
+	converter.ptr = unalignedPtr;
+	converter.integer += alignment - 1;
 	converter.integer &= bit_mask;
 	return converter.ptr;
 }
 
-
-#endif //BT_SCALAR_H
+#endif  //BT_SCALAR_H
diff --git a/extern/bullet2/src/LinearMath/btSerializer.cpp b/extern/bullet2/src/LinearMath/btSerializer.cpp
index 8fdcfb14211..068836f2c4e 100644
--- a/extern/bullet2/src/LinearMath/btSerializer.cpp
+++ b/extern/bullet2/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(123),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(-74),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),
@@ -72,1106 +72,621 @@ char(105),char(115),char(116),char(97),char(110),char(99),char(101),char(84),cha
 char(101),char(114),char(111),char(65),char(114),char(101),char(97),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(101),char(120),char(116),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(83),
 char(105),char(122),char(101),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(97),char(108),char(117),char(101),char(115),char(0),char(109),char(95),char(110),char(117),
-char(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(103),char(105),char(109),char(112),char(97),char(99),char(116),char(83),char(117),char(98),char(84),char(121),
-char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),
-char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),
-char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),
-char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(112),char(97),
-char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0),char(42),char(109),char(95),char(98),char(114),char(111),char(97),char(100),char(112),char(104),
-char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0),char(42),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(0),char(42),char(109),char(95),char(114),char(111),char(111),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(0),char(109),char(95),char(119),char(111),char(114),char(108),char(100),char(84),char(114),char(97),
-char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),
-char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),
-char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101),
-char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),
-char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),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(105),char(115),char(111),char(116),char(114),char(111),char(112),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(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(114),char(111),char(99),char(101),char(115),char(115),char(105),char(110),char(103),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(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116),
-char(105),char(111),char(110),char(84),char(105),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),
-char(95),char(114),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(0),char(109),char(95),char(114),
-char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99),
-char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(99),char(100),char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114),
-char(101),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(99),char(100),char(77),char(111),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(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116),
-char(114),char(111),char(112),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(99),char(111),char(108),char(108),
-char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84),
-char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95),
-char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105),
-char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67),
-char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73),
-char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),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(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110),
-char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0),
-char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),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(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(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70),
-char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101),
-char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),
-char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0),
-char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101),
-char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),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(68),char(97),char(109),char(112),char(105),char(110),char(103),
-char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),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(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76),
-char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),
-char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),
-char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),
-char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),
-char(108),char(97),char(114),char(68),char(97),char(109),char(112),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(108),
-char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),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(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),
-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(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),
-char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116),
-char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65),
-char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101),
-char(0),char(109),char(95),char(117),char(115),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(84),char(121),char(112),
-char(101),char(0),char(109),char(95),char(117),char(115),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(73),char(100),
-char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101),char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97),
-char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68),
-char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108),
-char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116),char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100),
-char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111),char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109),
-char(83),char(111),char(108),char(118),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(98),
-char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112),char(117),char(108),char(115),char(101),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(105),char(115),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(0),char(112),char(97),char(100),char(100),
-char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(121),char(112),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(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109),
-char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101),
-char(0),char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102),
-char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),
-char(97),char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108),
-char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),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(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114),
-char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),
-char(0),char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105),
-char(116),char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116),
-char(111),char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111),
-char(114),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(115),char(119),
-char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),
-char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109),char(95),char(100),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(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),
-char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),
-char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),
-char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),
-char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),char(114),char(101),char(110),
-char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),char(115),char(101),char(116),
-char(70),char(111),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(114),char(97),char(109),char(101),char(0),char(109),
-char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(69),char(110),
-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(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(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(76),char(105),char(109),char(105),char(116),char(101),char(100),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(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),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(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(76),char(105),char(109),char(105),char(116),char(101),char(100),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(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),
-char(76),char(105),char(109),char(105),char(116),char(101),char(100),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(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(76),char(111),
+char(99),char(97),char(108),char(80),char(111),char(105),char(110),char(116),char(65),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),
+char(67),char(97),char(99),char(104),char(101),char(76),char(111),char(99),char(97),char(108),char(80),char(111),char(105),char(110),char(116),char(66),char(91),char(52),char(93),char(0),
+char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),
+char(87),char(111),char(114),char(108),char(100),char(79),char(110),char(65),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),
+char(97),char(99),char(104),char(101),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(79),char(110),char(66),
+char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(78),char(111),char(114),char(109),
+char(97),char(108),char(87),char(111),char(114),char(108),char(100),char(79),char(110),char(66),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),
+char(116),char(67),char(97),char(99),char(104),char(101),char(76),char(97),char(116),char(101),char(114),char(97),char(108),char(70),char(114),char(105),char(99),char(116),char(105),char(111),
+char(110),char(68),char(105),char(114),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),
+char(101),char(76),char(97),char(116),char(101),char(114),char(97),char(108),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(68),char(105),char(114),char(50),
+char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(68),char(105),char(115),char(116),
+char(97),char(110),char(99),char(101),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),
+char(65),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(91),char(52),char(93),char(0),char(109),char(95),
+char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(80),char(114),char(101),char(118),char(82),char(72),char(83),char(91),char(52),char(93),
+char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),
+char(100),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),
+char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),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(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),
+char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(83),char(112),char(105),char(110),char(110),char(105),char(110),char(103),
+char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),
+char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(82),char(101),char(115),char(116),char(105),char(116),char(117),char(116),
+char(105),char(111),char(110),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(80),
+char(97),char(114),char(116),char(73),char(100),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),
+char(104),char(101),char(80),char(97),char(114),char(116),char(73),char(100),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),
+char(67),char(97),char(99),char(104),char(101),char(73),char(110),char(100),char(101),char(120),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),
+char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(73),char(110),char(100),char(101),char(120),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),
+char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(111),char(105),char(110),
+char(116),char(70),char(108),char(97),char(103),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),
+char(104),char(101),char(65),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(76),char(97),char(116),char(101),
+char(114),char(97),char(108),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),
+char(65),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(76),char(97),char(116),char(101),char(114),char(97),
+char(108),char(50),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),
+char(110),char(116),char(97),char(99),char(116),char(77),char(111),char(116),char(105),char(111),char(110),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),
+char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(77),char(111),char(116),char(105),char(111),
+char(110),char(50),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),
+char(110),char(116),char(97),char(99),char(116),char(67),char(70),char(77),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),
+char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),
+char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),
+char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(69),char(82),char(80),char(91),char(52),char(93),char(0),char(109),
+char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(67),
+char(111),char(110),char(116),char(97),char(99),char(116),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),
+char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(67),char(70),
+char(77),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(76),char(105),char(102),
+char(101),char(84),char(105),char(109),char(101),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(97),char(99),char(104),char(101),char(100),
+char(80),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),
+char(65),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(66),char(0),char(109),char(95),char(105),
+char(110),char(100),char(101),char(120),char(49),char(97),char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101),char(0),
+char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(99),char(116),char(66),char(114),char(101),char(97),char(107),char(105),char(110),char(103),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(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(114),char(111),char(99),
+char(101),char(115),char(115),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(42),char(109),char(95),char(98),
+char(111),char(100),char(121),char(48),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(49),char(0),char(109),char(95),char(103),char(105),char(109),char(112),
+char(97),char(99),char(116),char(83),char(117),char(98),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),
+char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),
+char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98),char(108),char(101),
+char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),
+char(110),char(116),char(115),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0),char(42),char(109),
+char(95),char(98),char(114),char(111),char(97),char(100),char(112),char(104),char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0),char(42),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(0),char(42),char(109),char(95),char(114),
+char(111),char(111),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(0),char(109),char(95),
+char(119),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),char(110),char(116),
+char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),
+char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),
+char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105),char(110),char(116),
+char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),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(105),char(115),char(111),char(116),char(114),char(111),char(112),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(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),
+char(111),char(110),char(84),char(105),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),
+char(114),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(0),char(109),char(95),char(99),char(111),
+char(110),char(116),char(97),char(99),char(116),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),
+char(99),char(116),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(114),char(101),char(115),char(116),char(105),char(116),
+char(117),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99),char(116),char(105),char(111),char(110),char(0),
+char(109),char(95),char(99),char(99),char(100),char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114),char(101),char(82),char(97),char(100),char(105),
+char(117),char(115),char(0),char(109),char(95),char(99),char(99),char(100),char(77),char(111),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(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),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(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),
+char(70),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84),char(97),char(103),char(49),char(0),char(109),
+char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95),char(97),char(99),char(116),char(105),char(118),
+char(97),char(116),char(105),char(111),char(110),char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(110),
+char(97),char(108),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67),char(111),char(108),char(108),char(105),char(100),
+char(101),char(87),char(105),char(116),char(104),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(70),char(105),char(108),
+char(116),char(101),char(114),char(71),char(114),char(111),char(117),char(112),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(70),char(105),char(108),char(116),char(101),char(114),char(77),char(97),char(115),char(107),char(0),char(109),char(95),char(117),char(110),char(105),char(113),char(117),char(101),char(73),
+char(100),char(0),char(109),char(95),char(116),char(97),char(117),char(0),char(109),char(95),char(100),char(97),char(109),char(112),char(105),char(110),char(103),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(97),char(114),char(116),char(105),char(99),char(117),char(108),
+char(97),char(116),char(101),char(100),char(87),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(115),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(0),
+char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),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(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),
+char(114),char(116),char(105),char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0),char(109),char(95),char(108),char(105),
+char(110),char(101),char(97),char(114),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(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(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70),char(97),char(99),char(116),char(111),
+char(114),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101),char(108),char(101),char(114),char(97),
+char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(76),char(111),char(99),
+char(97),char(108),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(116),char(111),
+char(116),char(97),char(108),char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101),char(114),char(115),char(101),char(77),
+char(97),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),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(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),
+char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),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(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76),char(105),char(110),char(101),char(97),
+char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),
+char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),
+char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),char(0),
+char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(68),
+char(97),char(109),char(112),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(108),char(105),char(110),char(101),char(97),
+char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),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(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),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(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),
+char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),
+char(116),char(82),char(111),char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65),char(0),char(42),char(109),char(95),
+char(114),char(98),char(66),char(0),char(109),char(95),char(117),char(115),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(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),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(73),char(100),char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101),char(101),char(100),char(98),char(97),char(99),char(107),char(0),
+char(109),char(95),char(97),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(100),
+char(98),char(103),char(68),char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(100),char(105),char(115),char(97),char(98),char(108),char(101),
+char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116),char(119),char(101),char(101),char(110),char(76),char(105),char(110),
+char(107),char(101),char(100),char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111),char(118),char(101),char(114),char(114),char(105),char(100),char(101),
+char(78),char(117),char(109),char(83),char(111),char(108),char(118),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(98),char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112),char(117),char(108),char(115),char(101),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(105),char(115),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(0),char(112),
+char(97),char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(121),char(112),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(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),
+char(65),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),
+char(97),char(109),char(101),char(0),char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),
+char(82),char(101),char(102),char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),
+char(103),char(117),char(108),char(97),char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),
+char(103),char(117),char(108),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),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(109),char(97),char(120),char(77),char(111),
+char(116),char(111),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),
+char(109),char(105),char(116),char(0),char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),
+char(105),char(109),char(105),char(116),char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),
+char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),
+char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),
+char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),
+char(112),char(97),char(110),char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109),char(95),char(108),
+char(105),char(110),char(101),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),
+char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),
+char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),
+char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(117),char(115),char(101),
+char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),
+char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),char(115),char(101),char(116),char(70),char(111),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(114),char(97),char(109),char(101),char(0),char(109),char(95),char(54),char(100),char(111),char(102),char(68),char(97),
+char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(69),char(110),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(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(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(76),char(105),char(109),char(105),char(116),char(101),char(100),
+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(112),char(114),char(105),char(110),char(103),char(68),char(97),
+char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),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(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(76),char(105),char(109),char(105),char(116),char(101),char(100),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(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),
+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(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(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(109),char(95),char(122),char(101),char(114),char(111),char(82),char(111),char(116),char(80),char(97),
-char(114),char(101),char(110),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(67),
-char(111),char(109),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102),char(102),char(115),char(101),char(116),char(0),char(109),char(95),
-char(116),char(104),char(105),char(115),char(80),char(105),char(118),char(111),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102),
-char(102),char(115),char(101),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(84),char(111),char(112),char(91),
-char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(66),char(111),char(116),char(116),char(111),char(109),
-char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(106),
-char(111),char(105),char(110),char(116),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(67),char(111),char(108),char(108),
-char(105),char(100),char(101),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),
-char(95),char(108),char(105),char(110),char(107),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(73),char(110),
-char(100),char(101),char(120),char(0),char(109),char(95),char(100),char(111),char(102),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(112),char(111),char(115),
-char(86),char(97),char(114),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(80),char(111),char(115),char(91),
-char(55),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(86),char(101),char(108),char(91),char(54),char(93),char(0),char(109),char(95),char(106),
-char(111),char(105),char(110),char(116),char(84),char(111),char(114),char(113),char(117),char(101),char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(98),char(97),char(115),
-char(101),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(67),char(111),char(108),char(108),char(105),char(100),char(101),
-char(114),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),
-char(114),char(109),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(98),
-char(97),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(84),char(89),char(80),char(69),char(95),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(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),
-char(111),char(110),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(116),char(101),char(114),
-char(110),char(105),char(111),char(110),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(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(109),
+char(95),char(122),char(101),char(114),char(111),char(82),char(111),char(116),char(80),char(97),char(114),char(101),char(110),char(116),char(84),char(111),char(84),char(104),char(105),char(115),
+char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(67),char(111),char(109),char(84),char(111),char(84),char(104),char(105),char(115),char(80),char(105),
+char(118),char(111),char(116),char(79),char(102),char(102),char(115),char(101),char(116),char(0),char(109),char(95),char(116),char(104),char(105),char(115),char(80),char(105),char(118),char(111),
+char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102),char(102),char(115),char(101),char(116),char(0),char(109),char(95),char(106),
+char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(84),char(111),char(112),char(91),char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),
+char(110),char(116),char(65),char(120),char(105),char(115),char(66),char(111),char(116),char(116),char(111),char(109),char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),
+char(110),char(107),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(97),char(98),char(115),char(70),char(114),char(97),char(109),char(101),
+char(84),char(111),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(84),char(111),char(112),char(0),char(109),char(95),char(97),char(98),char(115),
+char(70),char(114),char(97),char(109),char(101),char(84),char(111),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(66),char(111),char(116),char(116),
+char(111),char(109),char(0),char(109),char(95),char(97),char(98),char(115),char(70),char(114),char(97),char(109),char(101),char(76),char(111),char(99),char(86),char(101),char(108),char(111),
+char(99),char(105),char(116),char(121),char(84),char(111),char(112),char(0),char(109),char(95),char(97),char(98),char(115),char(70),char(114),char(97),char(109),char(101),char(76),char(111),
+char(99),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(66),char(111),char(116),char(116),char(111),char(109),char(0),char(109),char(95),char(108),char(105),
+char(110),char(107),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(73),char(110),char(100),char(101),char(120),
+char(0),char(109),char(95),char(100),char(111),char(102),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(112),char(111),char(115),char(86),char(97),char(114),
+char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(80),char(111),char(115),char(91),char(55),char(93),char(0),
+char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(86),char(101),char(108),char(91),char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),
+char(116),char(84),char(111),char(114),char(113),char(117),char(101),char(91),char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(68),char(97),
+char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(70),char(114),char(105),char(99),char(116),char(105),char(111),
+char(110),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),
+char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),
+char(106),char(111),char(105),char(110),char(116),char(77),char(97),char(120),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(106),char(111),char(105),char(110),
+char(116),char(77),char(97),char(120),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),
+char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(78),char(97),char(109),char(101),char(0),char(42),char(109),
+char(95),char(108),char(105),char(110),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(0),char(42),char(109),char(95),char(112),char(97),char(100),
+char(100),char(105),char(110),char(103),char(80),char(116),char(114),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87),char(111),char(114),char(108),char(100),char(80),
+char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87),char(111),char(114),char(108),char(100),char(79),
+char(114),char(105),char(101),char(110),char(116),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(76),char(105),char(110),
+char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(65),char(110),
+char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(98),char(97),char(115),char(101),
+char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(42),
+char(109),char(95),char(98),char(97),char(115),char(101),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(67),char(111),
+char(108),char(108),char(105),char(100),char(101),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(79),char(98),char(106),char(68),char(97),char(116),char(97),char(0),
+char(42),char(109),char(95),char(109),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(0),
+char(84),char(89),char(80),char(69),char(99),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(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),char(111),char(110),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(116),char(101),char(114),char(110),char(105),char(111),char(110),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(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(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(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(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(80),char(101),char(114),char(115),char(105),char(115),char(116),char(101),char(110),char(116),char(77),char(97),char(110),char(105),char(102),char(111),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(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(80),char(101),char(114),char(115),char(105),char(115),char(116),char(101),char(110),char(116),char(77),char(97),char(110),char(105),char(102),char(111),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(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(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(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(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(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(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(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(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(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(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(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(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),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(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),
-char(110),char(107),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(117),char(108),char(116),char(105),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(77),char(117),char(108),char(116),
-char(105),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(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(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(98),char(116),char(77),char(117),char(108),char(116),
+char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),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(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),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(117),char(108),char(116),char(105),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(77),char(117),char(108),char(116),char(105),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(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),
+char(105),char(110),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),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(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(67),char(111),char(108),char(108),
+char(105),char(100),char(101),char(114),char(68),char(111),char(117),char(98),char(108),char(101),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(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(-76),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(-68),char(2),char(108),char(1),char(-68),char(0),char(100),char(0),char(0),char(0),char(83),char(84),char(82),char(67),char(84),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(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0),
-char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0),
-char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(21),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(22),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(23),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(24),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(25),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(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),
-char(26),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(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0),
-char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),char(27),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(28),char(0),char(5),char(0),char(27),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(29),char(0),char(5),char(0),char(27),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(30),char(0),char(2),char(0),
-char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0),
-char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0),
-char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),
-char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0),
-char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0),
-char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),char(36),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(38),char(0),char(7),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),
-char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),
-char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0),
-char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0),
-char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(43),char(0),char(3),char(0),char(29),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(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0),
-char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),
-char(46),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(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(46),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(47),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),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(48),char(0),char(5),char(0),char(29),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(49),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),
-char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(104),char(0),char(20),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(50),char(0),char(25),char(0),char(9),char(0),char(101),char(0),
-char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(19),char(0),char(104),char(0),char(19),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(51),char(0),char(2),char(0),
-char(52),char(0),char(124),char(0),char(14),char(0),char(125),char(0),char(53),char(0),char(2),char(0),char(54),char(0),char(124),char(0),char(13),char(0),char(125),char(0),
-char(55),char(0),char(21),char(0),char(50),char(0),char(126),char(0),char(17),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(56),char(0),char(22),char(0),char(49),char(0),char(126),char(0),char(18),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(57),char(0),char(2),char(0),char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-108),char(0),
-char(55),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(13),char(0),char(60),char(0),char(-108),char(0),char(60),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(61),char(0),char(14),char(0),char(56),char(0),char(-108),char(0),char(56),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(62),char(0),char(3),char(0),char(59),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0),
-char(63),char(0),char(3),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(64),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(65),char(0),char(13),char(0),char(59),char(0),char(-95),char(0),
-char(20),char(0),char(-92),char(0),char(20),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(66),char(0),char(13),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),
-char(19),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(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(16),char(6),char(-24),char(1),char(72),char(3),
+char(16),char(1),char(64),char(0),char(68),char(0),char(-96),char(0),char(88),char(0),char(-64),char(0),char(104),char(0),char(-8),char(1),char(-72),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(-76),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(-68),char(1),char(112),char(3),char(-56),char(1),char(-68),char(0),char(100),char(0),char(28),char(1),char(-12),char(1),char(0),char(0),
+char(83),char(84),char(82),char(67),char(88),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(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),
+char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0),char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),
+char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0),char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),
+char(14),char(0),char(11),char(0),char(21),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(22),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(23),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(24),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(25),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(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),
+char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(26),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(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),
+char(27),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(28),char(0),char(5),char(0),
+char(27),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(29),char(0),char(5),char(0),char(27),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(30),char(0),char(2),char(0),char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),
+char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0),char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),
+char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0),char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),
+char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),
+char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0),char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),
+char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0),char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),
+char(36),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(38),char(0),char(7),char(0),
+char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),
+char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),
+char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0),char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),
+char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0),char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),
+char(43),char(0),char(3),char(0),char(29),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(3),char(0),
+char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0),char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),
+char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(46),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(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),
+char(46),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(47),char(0),char(39),char(0),char(14),char(0),char(96),char(0),char(14),char(0),char(97),char(0),
+char(14),char(0),char(98),char(0),char(14),char(0),char(99),char(0),char(14),char(0),char(100),char(0),char(14),char(0),char(101),char(0),char(14),char(0),char(102),char(0),
+char(8),char(0),char(103),char(0),char(8),char(0),char(104),char(0),char(8),char(0),char(105),char(0),char(8),char(0),char(106),char(0),char(8),char(0),char(107),char(0),
+char(8),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(4),char(0),char(110),char(0),char(4),char(0),char(111),char(0),char(4),char(0),char(112),char(0),
+char(4),char(0),char(113),char(0),char(4),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0),char(8),char(0),char(117),char(0),
+char(8),char(0),char(118),char(0),char(8),char(0),char(119),char(0),char(8),char(0),char(120),char(0),char(8),char(0),char(121),char(0),char(8),char(0),char(122),char(0),
+char(8),char(0),char(123),char(0),char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0),char(4),char(0),char(126),char(0),char(4),char(0),char(127),char(0),
+char(4),char(0),char(-128),char(0),char(4),char(0),char(-127),char(0),char(8),char(0),char(-126),char(0),char(8),char(0),char(-125),char(0),char(4),char(0),char(44),char(0),
+char(48),char(0),char(-124),char(0),char(48),char(0),char(-123),char(0),char(49),char(0),char(39),char(0),char(13),char(0),char(96),char(0),char(13),char(0),char(97),char(0),
+char(13),char(0),char(98),char(0),char(13),char(0),char(99),char(0),char(13),char(0),char(100),char(0),char(13),char(0),char(101),char(0),char(13),char(0),char(102),char(0),
+char(7),char(0),char(103),char(0),char(7),char(0),char(104),char(0),char(7),char(0),char(105),char(0),char(7),char(0),char(106),char(0),char(7),char(0),char(107),char(0),
+char(7),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(4),char(0),char(110),char(0),char(4),char(0),char(111),char(0),char(4),char(0),char(112),char(0),
+char(4),char(0),char(113),char(0),char(4),char(0),char(114),char(0),char(7),char(0),char(115),char(0),char(7),char(0),char(116),char(0),char(7),char(0),char(117),char(0),
+char(7),char(0),char(118),char(0),char(7),char(0),char(119),char(0),char(7),char(0),char(120),char(0),char(7),char(0),char(121),char(0),char(7),char(0),char(122),char(0),
+char(7),char(0),char(123),char(0),char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0),char(4),char(0),char(126),char(0),char(4),char(0),char(127),char(0),
+char(4),char(0),char(-128),char(0),char(4),char(0),char(-127),char(0),char(7),char(0),char(-126),char(0),char(7),char(0),char(-125),char(0),char(4),char(0),char(44),char(0),
+char(50),char(0),char(-124),char(0),char(50),char(0),char(-123),char(0),char(51),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),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(-122),char(0),char(52),char(0),char(5),char(0),char(29),char(0),char(47),char(0),
+char(13),char(0),char(-121),char(0),char(14),char(0),char(-120),char(0),char(4),char(0),char(-119),char(0),char(0),char(0),char(-118),char(0),char(48),char(0),char(29),char(0),
+char(9),char(0),char(-117),char(0),char(9),char(0),char(-116),char(0),char(27),char(0),char(-115),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(-114),char(0),
+char(20),char(0),char(-113),char(0),char(14),char(0),char(-112),char(0),char(14),char(0),char(-111),char(0),char(14),char(0),char(-110),char(0),char(8),char(0),char(-125),char(0),
+char(8),char(0),char(-109),char(0),char(8),char(0),char(-108),char(0),char(8),char(0),char(-107),char(0),char(8),char(0),char(-106),char(0),char(8),char(0),char(-105),char(0),
+char(8),char(0),char(-104),char(0),char(8),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(4),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(4),char(0),char(-96),char(0),char(4),char(0),char(-95),char(0),
+char(4),char(0),char(-94),char(0),char(4),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(4),char(0),char(-91),char(0),char(50),char(0),char(29),char(0),
+char(9),char(0),char(-117),char(0),char(9),char(0),char(-116),char(0),char(27),char(0),char(-115),char(0),char(0),char(0),char(35),char(0),char(19),char(0),char(-114),char(0),
+char(19),char(0),char(-113),char(0),char(13),char(0),char(-112),char(0),char(13),char(0),char(-111),char(0),char(13),char(0),char(-110),char(0),char(7),char(0),char(-125),char(0),
+char(7),char(0),char(-109),char(0),char(7),char(0),char(-108),char(0),char(7),char(0),char(-107),char(0),char(7),char(0),char(-106),char(0),char(7),char(0),char(-105),char(0),
+char(7),char(0),char(-104),char(0),char(7),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(4),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(4),char(0),char(-96),char(0),char(4),char(0),char(-95),char(0),
+char(4),char(0),char(-94),char(0),char(4),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(4),char(0),char(-91),char(0),char(53),char(0),char(23),char(0),
+char(8),char(0),char(-90),char(0),char(8),char(0),char(-89),char(0),char(8),char(0),char(-108),char(0),char(8),char(0),char(-88),char(0),char(8),char(0),char(-104),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(8),char(0),char(-80),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(-76),char(0),char(4),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0),char(4),char(0),char(-73),char(0),
+char(4),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(22),char(0),char(7),char(0),char(-90),char(0),
+char(7),char(0),char(-89),char(0),char(7),char(0),char(-108),char(0),char(7),char(0),char(-88),char(0),char(7),char(0),char(-104),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(67),char(0),char(14),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),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(68),char(0),char(10),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),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(69),char(0),char(11),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),
-char(19),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(70),char(0),char(9),char(0),
-char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),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(71),char(0),char(9),char(0),
-char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),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(72),char(0),char(5),char(0),
-char(70),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(73),char(0),char(5),char(0),char(71),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(74),char(0),char(41),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),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(-50),char(0),char(0),char(0),char(-49),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(-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(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),
-char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-95),char(0),
-char(20),char(0),char(-92),char(0),char(20),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(-50),char(0),char(0),char(0),char(-49),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(-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(14),char(0),char(-39),char(0),char(14),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),
-char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0),
-char(76),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),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(77),char(0),char(9),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),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(78),char(0),char(5),char(0),char(58),char(0),char(-95),char(0),char(13),char(0),char(-31),char(0),char(13),char(0),char(-30),char(0),char(7),char(0),char(-29),char(0),
-char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-31),char(0),char(14),char(0),char(-30),char(0),
-char(8),char(0),char(-29),char(0),char(52),char(0),char(22),char(0),char(8),char(0),char(-28),char(0),char(8),char(0),char(-76),char(0),char(8),char(0),char(111),char(0),
-char(8),char(0),char(-27),char(0),char(8),char(0),char(113),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(8),char(0),char(-18),char(0),char(8),char(0),char(-17),char(0),char(8),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),
-char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(22),char(0),
-char(7),char(0),char(-28),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-27),char(0),char(7),char(0),char(113),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(7),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0),
-char(7),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),
-char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(80),char(0),char(4),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(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0),char(80),char(0),char(-7),char(0),char(13),char(0),char(-6),char(0),
-char(13),char(0),char(-5),char(0),char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0),char(13),char(0),char(-2),char(0),char(7),char(0),char(-120),char(0),
-char(7),char(0),char(-1),char(0),char(4),char(0),char(0),char(1),char(4),char(0),char(53),char(0),char(82),char(0),char(4),char(0),char(80),char(0),char(-7),char(0),
-char(4),char(0),char(1),char(1),char(7),char(0),char(2),char(1),char(4),char(0),char(3),char(1),char(83),char(0),char(4),char(0),char(13),char(0),char(-2),char(0),
-char(80),char(0),char(-7),char(0),char(4),char(0),char(4),char(1),char(7),char(0),char(5),char(1),char(84),char(0),char(7),char(0),char(13),char(0),char(6),char(1),
-char(80),char(0),char(-7),char(0),char(4),char(0),char(7),char(1),char(7),char(0),char(8),char(1),char(7),char(0),char(9),char(1),char(7),char(0),char(10),char(1),
-char(4),char(0),char(53),char(0),char(85),char(0),char(6),char(0),char(17),char(0),char(11),char(1),char(13),char(0),char(9),char(1),char(13),char(0),char(12),char(1),
-char(60),char(0),char(13),char(1),char(4),char(0),char(14),char(1),char(7),char(0),char(10),char(1),char(86),char(0),char(26),char(0),char(4),char(0),char(15),char(1),
-char(7),char(0),char(16),char(1),char(7),char(0),char(-76),char(0),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(7),char(0),char(32),char(1),char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1),
-char(4),char(0),char(35),char(1),char(4),char(0),char(36),char(1),char(4),char(0),char(37),char(1),char(4),char(0),char(38),char(1),char(4),char(0),char(118),char(0),
-char(87),char(0),char(12),char(0),char(17),char(0),char(39),char(1),char(17),char(0),char(40),char(1),char(17),char(0),char(41),char(1),char(13),char(0),char(42),char(1),
-char(13),char(0),char(43),char(1),char(7),char(0),char(44),char(1),char(4),char(0),char(45),char(1),char(4),char(0),char(46),char(1),char(4),char(0),char(47),char(1),
-char(4),char(0),char(48),char(1),char(7),char(0),char(8),char(1),char(4),char(0),char(53),char(0),char(88),char(0),char(27),char(0),char(19),char(0),char(49),char(1),
-char(17),char(0),char(50),char(1),char(17),char(0),char(51),char(1),char(13),char(0),char(42),char(1),char(13),char(0),char(52),char(1),char(13),char(0),char(53),char(1),
-char(13),char(0),char(54),char(1),char(13),char(0),char(55),char(1),char(13),char(0),char(56),char(1),char(4),char(0),char(57),char(1),char(7),char(0),char(58),char(1),
-char(4),char(0),char(59),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(7),char(0),char(62),char(1),char(7),char(0),char(63),char(1),
-char(4),char(0),char(64),char(1),char(4),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(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1),char(4),char(0),char(72),char(1),char(4),char(0),char(73),char(1),
-char(4),char(0),char(74),char(1),char(89),char(0),char(12),char(0),char(9),char(0),char(75),char(1),char(9),char(0),char(76),char(1),char(13),char(0),char(77),char(1),
-char(7),char(0),char(78),char(1),char(7),char(0),char(-24),char(0),char(7),char(0),char(79),char(1),char(4),char(0),char(80),char(1),char(13),char(0),char(81),char(1),
-char(4),char(0),char(82),char(1),char(4),char(0),char(83),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(53),char(0),char(90),char(0),char(19),char(0),
-char(50),char(0),char(126),char(0),char(87),char(0),char(85),char(1),char(80),char(0),char(86),char(1),char(81),char(0),char(87),char(1),char(82),char(0),char(88),char(1),
-char(83),char(0),char(89),char(1),char(84),char(0),char(90),char(1),char(85),char(0),char(91),char(1),char(88),char(0),char(92),char(1),char(89),char(0),char(93),char(1),
-char(4),char(0),char(94),char(1),char(4),char(0),char(60),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(4),char(0),char(98),char(1),char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1),char(86),char(0),char(101),char(1),char(91),char(0),char(17),char(0),
-char(16),char(0),char(102),char(1),char(14),char(0),char(103),char(1),char(14),char(0),char(104),char(1),char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1),
-char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1),char(49),char(0),char(109),char(1),char(14),char(0),char(110),char(1),char(8),char(0),char(111),char(1),
-char(4),char(0),char(112),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(113),char(1),char(4),char(0),char(114),char(1),char(8),char(0),char(115),char(1),
-char(8),char(0),char(116),char(1),char(8),char(0),char(117),char(1),char(92),char(0),char(17),char(0),char(15),char(0),char(102),char(1),char(13),char(0),char(103),char(1),
-char(13),char(0),char(104),char(1),char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1),char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1),
-char(50),char(0),char(109),char(1),char(13),char(0),char(110),char(1),char(4),char(0),char(113),char(1),char(7),char(0),char(111),char(1),char(4),char(0),char(112),char(1),
-char(4),char(0),char(84),char(1),char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1),char(7),char(0),char(117),char(1),char(4),char(0),char(114),char(1),
-char(93),char(0),char(8),char(0),char(0),char(0),char(118),char(1),char(91),char(0),char(88),char(1),char(49),char(0),char(119),char(1),char(20),char(0),char(120),char(1),
-char(14),char(0),char(121),char(1),char(4),char(0),char(95),char(1),char(8),char(0),char(122),char(1),char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0),
-char(0),char(0),char(118),char(1),char(92),char(0),char(88),char(1),char(50),char(0),char(119),char(1),char(19),char(0),char(120),char(1),char(13),char(0),char(121),char(1),
-char(7),char(0),char(122),char(1),char(4),char(0),char(95),char(1),};
+char(7),char(0),char(-81),char(0),char(7),char(0),char(-80),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(-76),char(0),char(4),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0),char(4),char(0),char(-73),char(0),char(4),char(0),char(-72),char(0),
+char(4),char(0),char(-71),char(0),char(55),char(0),char(2),char(0),char(53),char(0),char(-70),char(0),char(14),char(0),char(-69),char(0),char(56),char(0),char(2),char(0),
+char(54),char(0),char(-70),char(0),char(13),char(0),char(-69),char(0),char(57),char(0),char(21),char(0),char(50),char(0),char(-68),char(0),char(17),char(0),char(-67),char(0),
+char(13),char(0),char(-66),char(0),char(13),char(0),char(-65),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0),char(13),char(0),char(-69),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(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(4),char(0),char(-49),char(0),char(58),char(0),char(22),char(0),
+char(48),char(0),char(-68),char(0),char(18),char(0),char(-67),char(0),char(14),char(0),char(-66),char(0),char(14),char(0),char(-65),char(0),char(14),char(0),char(-64),char(0),
+char(14),char(0),char(-63),char(0),char(14),char(0),char(-69),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(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(4),char(0),char(-49),char(0),char(0),char(0),char(37),char(0),char(59),char(0),char(2),char(0),char(4),char(0),char(-48),char(0),char(4),char(0),char(-47),char(0),
+char(60),char(0),char(13),char(0),char(57),char(0),char(-46),char(0),char(57),char(0),char(-45),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-127),char(0),
+char(4),char(0),char(-44),char(0),char(4),char(0),char(-43),char(0),char(4),char(0),char(-42),char(0),char(7),char(0),char(-41),char(0),char(7),char(0),char(-40),char(0),
+char(4),char(0),char(-39),char(0),char(4),char(0),char(-38),char(0),char(7),char(0),char(-37),char(0),char(4),char(0),char(-36),char(0),char(61),char(0),char(13),char(0),
+char(62),char(0),char(-46),char(0),char(62),char(0),char(-45),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-127),char(0),char(4),char(0),char(-44),char(0),
+char(4),char(0),char(-43),char(0),char(4),char(0),char(-42),char(0),char(7),char(0),char(-41),char(0),char(7),char(0),char(-40),char(0),char(4),char(0),char(-39),char(0),
+char(4),char(0),char(-38),char(0),char(7),char(0),char(-37),char(0),char(4),char(0),char(-36),char(0),char(63),char(0),char(14),char(0),char(58),char(0),char(-46),char(0),
+char(58),char(0),char(-45),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-127),char(0),char(4),char(0),char(-44),char(0),char(4),char(0),char(-43),char(0),
+char(4),char(0),char(-42),char(0),char(8),char(0),char(-41),char(0),char(8),char(0),char(-40),char(0),char(4),char(0),char(-39),char(0),char(4),char(0),char(-38),char(0),
+char(8),char(0),char(-37),char(0),char(4),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),char(64),char(0),char(3),char(0),char(61),char(0),char(-34),char(0),
+char(13),char(0),char(-33),char(0),char(13),char(0),char(-32),char(0),char(65),char(0),char(3),char(0),char(63),char(0),char(-34),char(0),char(14),char(0),char(-33),char(0),
+char(14),char(0),char(-32),char(0),char(66),char(0),char(3),char(0),char(61),char(0),char(-34),char(0),char(14),char(0),char(-33),char(0),char(14),char(0),char(-32),char(0),
+char(67),char(0),char(13),char(0),char(61),char(0),char(-34),char(0),char(20),char(0),char(-31),char(0),char(20),char(0),char(-30),char(0),char(4),char(0),char(-29),char(0),
+char(4),char(0),char(-28),char(0),char(4),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(68),char(0),char(13),char(0),
+char(61),char(0),char(-34),char(0),char(19),char(0),char(-31),char(0),char(19),char(0),char(-30),char(0),char(4),char(0),char(-29),char(0),char(4),char(0),char(-28),char(0),
+char(4),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(69),char(0),char(14),char(0),char(63),char(0),char(-34),char(0),
+char(20),char(0),char(-31),char(0),char(20),char(0),char(-30),char(0),char(4),char(0),char(-29),char(0),char(4),char(0),char(-28),char(0),char(4),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(0),char(0),char(-19),char(0),char(70),char(0),char(10),char(0),char(63),char(0),char(-34),char(0),
+char(20),char(0),char(-31),char(0),char(20),char(0),char(-30),char(0),char(8),char(0),char(-18),char(0),char(8),char(0),char(-17),char(0),char(8),char(0),char(-16),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(-89),char(0),char(71),char(0),char(11),char(0),
+char(61),char(0),char(-34),char(0),char(19),char(0),char(-31),char(0),char(19),char(0),char(-30),char(0),char(7),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0),
+char(7),char(0),char(-16),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(-89),char(0),
+char(0),char(0),char(21),char(0),char(72),char(0),char(9),char(0),char(61),char(0),char(-34),char(0),char(19),char(0),char(-31),char(0),char(19),char(0),char(-30),char(0),
+char(13),char(0),char(-15),char(0),char(13),char(0),char(-14),char(0),char(13),char(0),char(-13),char(0),char(13),char(0),char(-12),char(0),char(4),char(0),char(-11),char(0),
+char(4),char(0),char(-10),char(0),char(73),char(0),char(9),char(0),char(63),char(0),char(-34),char(0),char(20),char(0),char(-31),char(0),char(20),char(0),char(-30),char(0),
+char(14),char(0),char(-15),char(0),char(14),char(0),char(-14),char(0),char(14),char(0),char(-13),char(0),char(14),char(0),char(-12),char(0),char(4),char(0),char(-11),char(0),
+char(4),char(0),char(-10),char(0),char(74),char(0),char(5),char(0),char(72),char(0),char(-9),char(0),char(4),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(75),char(0),char(5),char(0),char(73),char(0),char(-9),char(0),char(4),char(0),char(-8),char(0),
+char(8),char(0),char(-7),char(0),char(8),char(0),char(-6),char(0),char(8),char(0),char(-5),char(0),char(76),char(0),char(41),char(0),char(61),char(0),char(-34),char(0),
+char(19),char(0),char(-31),char(0),char(19),char(0),char(-30),char(0),char(13),char(0),char(-15),char(0),char(13),char(0),char(-14),char(0),char(13),char(0),char(-4),char(0),
+char(13),char(0),char(-3),char(0),char(13),char(0),char(-2),char(0),char(13),char(0),char(-1),char(0),char(13),char(0),char(0),char(1),char(13),char(0),char(1),char(1),
+char(13),char(0),char(2),char(1),char(13),char(0),char(3),char(1),char(13),char(0),char(4),char(1),char(13),char(0),char(5),char(1),char(13),char(0),char(6),char(1),
+char(0),char(0),char(7),char(1),char(0),char(0),char(8),char(1),char(0),char(0),char(9),char(1),char(0),char(0),char(10),char(1),char(0),char(0),char(11),char(1),
+char(0),char(0),char(-19),char(0),char(13),char(0),char(-13),char(0),char(13),char(0),char(-12),char(0),char(13),char(0),char(12),char(1),char(13),char(0),char(13),char(1),
+char(13),char(0),char(14),char(1),char(13),char(0),char(15),char(1),char(13),char(0),char(16),char(1),char(13),char(0),char(17),char(1),char(13),char(0),char(18),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(0),char(0),char(23),char(1),
+char(0),char(0),char(24),char(1),char(0),char(0),char(25),char(1),char(0),char(0),char(26),char(1),char(0),char(0),char(27),char(1),char(4),char(0),char(28),char(1),
+char(77),char(0),char(41),char(0),char(63),char(0),char(-34),char(0),char(20),char(0),char(-31),char(0),char(20),char(0),char(-30),char(0),char(14),char(0),char(-15),char(0),
+char(14),char(0),char(-14),char(0),char(14),char(0),char(-4),char(0),char(14),char(0),char(-3),char(0),char(14),char(0),char(-2),char(0),char(14),char(0),char(-1),char(0),
+char(14),char(0),char(0),char(1),char(14),char(0),char(1),char(1),char(14),char(0),char(2),char(1),char(14),char(0),char(3),char(1),char(14),char(0),char(4),char(1),
+char(14),char(0),char(5),char(1),char(14),char(0),char(6),char(1),char(0),char(0),char(7),char(1),char(0),char(0),char(8),char(1),char(0),char(0),char(9),char(1),
+char(0),char(0),char(10),char(1),char(0),char(0),char(11),char(1),char(0),char(0),char(-19),char(0),char(14),char(0),char(-13),char(0),char(14),char(0),char(-12),char(0),
+char(14),char(0),char(12),char(1),char(14),char(0),char(13),char(1),char(14),char(0),char(14),char(1),char(14),char(0),char(15),char(1),char(14),char(0),char(16),char(1),
+char(14),char(0),char(17),char(1),char(14),char(0),char(18),char(1),char(14),char(0),char(19),char(1),char(14),char(0),char(20),char(1),char(14),char(0),char(21),char(1),
+char(14),char(0),char(22),char(1),char(0),char(0),char(23),char(1),char(0),char(0),char(24),char(1),char(0),char(0),char(25),char(1),char(0),char(0),char(26),char(1),
+char(0),char(0),char(27),char(1),char(4),char(0),char(28),char(1),char(78),char(0),char(9),char(0),char(61),char(0),char(-34),char(0),char(19),char(0),char(-31),char(0),
+char(19),char(0),char(-30),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(4),char(0),char(-11),char(0),char(4),char(0),char(-10),char(0),char(79),char(0),char(9),char(0),char(63),char(0),char(-34),char(0),char(20),char(0),char(-31),char(0),
+char(20),char(0),char(-30),char(0),char(8),char(0),char(-15),char(0),char(8),char(0),char(-14),char(0),char(8),char(0),char(-13),char(0),char(8),char(0),char(-12),char(0),
+char(4),char(0),char(-11),char(0),char(4),char(0),char(-10),char(0),char(80),char(0),char(5),char(0),char(60),char(0),char(-34),char(0),char(13),char(0),char(29),char(1),
+char(13),char(0),char(30),char(1),char(7),char(0),char(31),char(1),char(0),char(0),char(37),char(0),char(81),char(0),char(4),char(0),char(63),char(0),char(-34),char(0),
+char(14),char(0),char(29),char(1),char(14),char(0),char(30),char(1),char(8),char(0),char(31),char(1),char(82),char(0),char(4),char(0),char(7),char(0),char(32),char(1),
+char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1),char(4),char(0),char(79),char(0),char(83),char(0),char(10),char(0),char(82),char(0),char(35),char(1),
+char(13),char(0),char(36),char(1),char(13),char(0),char(37),char(1),char(13),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(-58),char(0),char(7),char(0),char(41),char(1),char(4),char(0),char(42),char(1),char(4),char(0),char(53),char(0),char(84),char(0),char(4),char(0),
+char(82),char(0),char(35),char(1),char(4),char(0),char(43),char(1),char(7),char(0),char(44),char(1),char(4),char(0),char(45),char(1),char(85),char(0),char(4),char(0),
+char(13),char(0),char(40),char(1),char(82),char(0),char(35),char(1),char(4),char(0),char(46),char(1),char(7),char(0),char(47),char(1),char(86),char(0),char(7),char(0),
+char(13),char(0),char(48),char(1),char(82),char(0),char(35),char(1),char(4),char(0),char(49),char(1),char(7),char(0),char(50),char(1),char(7),char(0),char(51),char(1),
+char(7),char(0),char(52),char(1),char(4),char(0),char(53),char(0),char(87),char(0),char(6),char(0),char(17),char(0),char(53),char(1),char(13),char(0),char(51),char(1),
+char(13),char(0),char(54),char(1),char(62),char(0),char(55),char(1),char(4),char(0),char(56),char(1),char(7),char(0),char(52),char(1),char(88),char(0),char(26),char(0),
+char(4),char(0),char(57),char(1),char(7),char(0),char(58),char(1),char(7),char(0),char(-89),char(0),char(7),char(0),char(59),char(1),char(7),char(0),char(60),char(1),
+char(7),char(0),char(61),char(1),char(7),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(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),
+char(7),char(0),char(71),char(1),char(7),char(0),char(72),char(1),char(7),char(0),char(73),char(1),char(7),char(0),char(74),char(1),char(7),char(0),char(75),char(1),
+char(7),char(0),char(76),char(1),char(4),char(0),char(77),char(1),char(4),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(-99),char(0),char(89),char(0),char(12),char(0),char(17),char(0),char(81),char(1),char(17),char(0),char(82),char(1),char(17),char(0),char(83),char(1),
+char(13),char(0),char(84),char(1),char(13),char(0),char(85),char(1),char(7),char(0),char(86),char(1),char(4),char(0),char(87),char(1),char(4),char(0),char(88),char(1),
+char(4),char(0),char(89),char(1),char(4),char(0),char(90),char(1),char(7),char(0),char(50),char(1),char(4),char(0),char(53),char(0),char(90),char(0),char(27),char(0),
+char(19),char(0),char(91),char(1),char(17),char(0),char(92),char(1),char(17),char(0),char(93),char(1),char(13),char(0),char(84),char(1),char(13),char(0),char(94),char(1),
+char(13),char(0),char(95),char(1),char(13),char(0),char(96),char(1),char(13),char(0),char(97),char(1),char(13),char(0),char(98),char(1),char(4),char(0),char(99),char(1),
+char(7),char(0),char(100),char(1),char(4),char(0),char(101),char(1),char(4),char(0),char(102),char(1),char(4),char(0),char(103),char(1),char(7),char(0),char(104),char(1),
+char(7),char(0),char(105),char(1),char(4),char(0),char(106),char(1),char(4),char(0),char(107),char(1),char(7),char(0),char(108),char(1),char(7),char(0),char(109),char(1),
+char(7),char(0),char(110),char(1),char(7),char(0),char(111),char(1),char(7),char(0),char(112),char(1),char(7),char(0),char(113),char(1),char(4),char(0),char(114),char(1),
+char(4),char(0),char(115),char(1),char(4),char(0),char(116),char(1),char(91),char(0),char(12),char(0),char(9),char(0),char(117),char(1),char(9),char(0),char(118),char(1),
+char(13),char(0),char(119),char(1),char(7),char(0),char(120),char(1),char(7),char(0),char(-85),char(0),char(7),char(0),char(121),char(1),char(4),char(0),char(122),char(1),
+char(13),char(0),char(123),char(1),char(4),char(0),char(124),char(1),char(4),char(0),char(125),char(1),char(4),char(0),char(126),char(1),char(4),char(0),char(53),char(0),
+char(92),char(0),char(19),char(0),char(50),char(0),char(-68),char(0),char(89),char(0),char(127),char(1),char(82),char(0),char(-128),char(1),char(83),char(0),char(-127),char(1),
+char(84),char(0),char(-126),char(1),char(85),char(0),char(-125),char(1),char(86),char(0),char(-124),char(1),char(87),char(0),char(-123),char(1),char(90),char(0),char(-122),char(1),
+char(91),char(0),char(-121),char(1),char(4),char(0),char(-120),char(1),char(4),char(0),char(102),char(1),char(4),char(0),char(-119),char(1),char(4),char(0),char(-118),char(1),
+char(4),char(0),char(-117),char(1),char(4),char(0),char(-116),char(1),char(4),char(0),char(-115),char(1),char(4),char(0),char(-114),char(1),char(88),char(0),char(-113),char(1),
+char(93),char(0),char(28),char(0),char(16),char(0),char(-112),char(1),char(14),char(0),char(-111),char(1),char(14),char(0),char(-110),char(1),char(14),char(0),char(-109),char(1),
+char(14),char(0),char(-108),char(1),char(14),char(0),char(-107),char(1),char(14),char(0),char(-106),char(1),char(14),char(0),char(-105),char(1),char(14),char(0),char(-104),char(1),
+char(14),char(0),char(-103),char(1),char(8),char(0),char(-102),char(1),char(4),char(0),char(-101),char(1),char(4),char(0),char(126),char(1),char(4),char(0),char(-100),char(1),
+char(4),char(0),char(-99),char(1),char(8),char(0),char(-98),char(1),char(8),char(0),char(-97),char(1),char(8),char(0),char(-96),char(1),char(8),char(0),char(-95),char(1),
+char(8),char(0),char(-94),char(1),char(8),char(0),char(-93),char(1),char(8),char(0),char(-92),char(1),char(8),char(0),char(-91),char(1),char(8),char(0),char(-90),char(1),
+char(0),char(0),char(-89),char(1),char(0),char(0),char(-88),char(1),char(48),char(0),char(-87),char(1),char(0),char(0),char(-86),char(1),char(94),char(0),char(28),char(0),
+char(15),char(0),char(-112),char(1),char(13),char(0),char(-111),char(1),char(13),char(0),char(-110),char(1),char(13),char(0),char(-109),char(1),char(13),char(0),char(-108),char(1),
+char(13),char(0),char(-107),char(1),char(13),char(0),char(-106),char(1),char(13),char(0),char(-105),char(1),char(13),char(0),char(-104),char(1),char(13),char(0),char(-103),char(1),
+char(4),char(0),char(-100),char(1),char(7),char(0),char(-102),char(1),char(4),char(0),char(-101),char(1),char(4),char(0),char(126),char(1),char(7),char(0),char(-98),char(1),
+char(7),char(0),char(-97),char(1),char(7),char(0),char(-96),char(1),char(4),char(0),char(-99),char(1),char(7),char(0),char(-95),char(1),char(7),char(0),char(-94),char(1),
+char(7),char(0),char(-93),char(1),char(7),char(0),char(-92),char(1),char(7),char(0),char(-91),char(1),char(7),char(0),char(-90),char(1),char(0),char(0),char(-89),char(1),
+char(0),char(0),char(-88),char(1),char(50),char(0),char(-87),char(1),char(0),char(0),char(-86),char(1),char(95),char(0),char(11),char(0),char(14),char(0),char(-85),char(1),
+char(16),char(0),char(-84),char(1),char(14),char(0),char(-83),char(1),char(14),char(0),char(-82),char(1),char(14),char(0),char(-81),char(1),char(8),char(0),char(-80),char(1),
+char(4),char(0),char(-119),char(1),char(0),char(0),char(37),char(0),char(0),char(0),char(-79),char(1),char(93),char(0),char(-126),char(1),char(48),char(0),char(-78),char(1),
+char(96),char(0),char(10),char(0),char(13),char(0),char(-85),char(1),char(15),char(0),char(-84),char(1),char(13),char(0),char(-83),char(1),char(13),char(0),char(-82),char(1),
+char(13),char(0),char(-81),char(1),char(7),char(0),char(-80),char(1),char(4),char(0),char(-119),char(1),char(0),char(0),char(-79),char(1),char(94),char(0),char(-126),char(1),
+char(50),char(0),char(-78),char(1),char(97),char(0),char(4),char(0),char(50),char(0),char(-77),char(1),char(96),char(0),char(-76),char(1),char(4),char(0),char(-75),char(1),
+char(0),char(0),char(37),char(0),char(98),char(0),char(4),char(0),char(48),char(0),char(-77),char(1),char(95),char(0),char(-76),char(1),char(4),char(0),char(-75),char(1),
+char(0),char(0),char(37),char(0),};
 int sBulletDNAlen= sizeof(sBulletDNAstr);
-
-char sBulletDNAstr64[]= {
-char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(123),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),
-char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(115),char(0),char(42),char(102),char(105),char(114),char(115),char(116),char(0),char(42),char(108),char(97),char(115),
-char(116),char(0),char(109),char(95),char(102),char(108),char(111),char(97),char(116),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(108),char(91),char(51),
-char(93),char(0),char(109),char(95),char(98),char(97),char(115),char(105),char(115),char(0),char(109),char(95),char(111),char(114),char(105),char(103),char(105),char(110),char(0),char(109),
-char(95),char(114),char(111),char(111),char(116),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98),
-char(116),char(114),char(101),char(101),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),
-char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(91),char(51),char(93),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),
-char(101),char(100),char(65),char(97),char(98),char(98),char(77),char(97),char(120),char(91),char(51),char(93),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77),
-char(105),char(110),char(79),char(114),char(103),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77),char(97),char(120),char(79),char(114),char(103),char(0),char(109),
-char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98),char(80),char(97),
-char(114),char(116),char(0),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),
-char(95),char(112),char(97),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101),
-char(120),char(79),char(114),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(98),
-char(118),char(104),char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(0),char(109),char(95),char(98),char(118),char(104),char(65),char(97),char(98),char(98),char(77),
-char(97),char(120),char(0),char(109),char(95),char(98),char(118),char(104),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110),
-char(0),char(109),char(95),char(99),char(117),char(114),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(117),char(115),
-char(101),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(110),char(117),char(109),char(67),
-char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(76),char(101),char(97),char(102),char(78),char(111),char(100),char(101),char(115),char(0),char(109),
-char(95),char(110),char(117),char(109),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117),
-char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(99),char(111),char(110),char(116),char(105),char(103),char(117),char(111),
-char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),
-char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116),
-char(114),char(0),char(42),char(109),char(95),char(115),char(117),char(98),char(84),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(80),char(116),char(114),char(0),
-char(109),char(95),char(116),char(114),char(97),char(118),char(101),char(114),char(115),char(97),char(108),char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(110),char(117),
-char(109),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(72),char(101),char(97),char(100),char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(110),
-char(97),char(109),char(101),char(0),char(109),char(95),char(115),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(112),char(97),
-char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),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(68),char(97),char(116),char(97),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(83),char(99),char(97),
-char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(108),char(97),char(110),char(101),char(78),char(111),char(114),char(109),char(97),char(108),char(0),char(109),
-char(95),char(112),char(108),char(97),char(110),char(101),char(67),char(111),char(110),char(115),char(116),char(97),char(110),char(116),char(0),char(109),char(95),char(105),char(109),char(112),
-char(108),char(105),char(99),char(105),char(116),char(83),char(104),char(97),char(112),char(101),char(68),char(105),char(109),char(101),char(110),char(115),char(105),char(111),char(110),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(77),char(97),char(114),char(103),char(105),char(110),char(0),char(109),
-char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(111),char(115),char(0),char(109),char(95),char(114),char(97),char(100),
-char(105),char(117),char(115),char(0),char(109),char(95),char(99),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(42),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(80),char(111),
-char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),char(108),char(111),char(99),
-char(97),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(83),char(105),char(122),char(101),char(0),
-char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(0),char(109),char(95),char(112),char(97),char(100),char(91),char(50),char(93),char(0),char(109),char(95),char(118),
-char(97),char(108),char(117),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(112),char(97),char(100),char(0),char(42),char(109),char(95),char(118),char(101),
-char(114),char(116),char(105),char(99),char(101),char(115),char(51),char(102),char(0),char(42),char(109),char(95),char(118),char(101),char(114),char(116),char(105),char(99),char(101),char(115),
-char(51),char(100),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(51),char(50),char(0),char(42),char(109),char(95),char(51),
-char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(42),char(109),char(95),char(51),char(105),char(110),char(100),char(105),char(99),char(101),
-char(115),char(56),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(109),char(95),char(110),char(117),
-char(109),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(101),char(114),char(116),
-char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(115),char(80),char(116),char(114),
-char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(101),char(115),char(104),
-char(80),char(97),char(114),char(116),char(115),char(0),char(109),char(95),char(109),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(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(70),char(108),char(111),char(97),char(116),char(66),
-char(118),char(104),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(68),char(111),char(117),char(98),char(108),
-char(101),char(66),char(118),char(104),char(0),char(42),char(109),char(95),char(116),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(0),char(109),char(95),char(112),char(97),char(100),char(51),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(114),char(105),char(109),
-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(109),char(95),char(116),char(114),char(97),char(110),char(115),
-char(102),char(111),char(114),char(109),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(0),char(109),
-char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),
-char(105),char(108),char(100),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),
-char(97),char(112),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(104),char(105),char(108),char(100),char(83),char(104),char(97),
-char(112),char(101),char(115),char(0),char(109),char(95),char(117),char(112),char(65),char(120),char(105),char(115),char(0),char(109),char(95),char(117),char(112),char(73),char(110),char(100),
-char(101),char(120),char(0),char(109),char(95),char(102),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(48),char(86),
-char(49),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(49),char(86),char(50),char(65),char(110),char(103),
-char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(50),char(86),char(48),char(65),char(110),char(103),char(108),char(101),char(0),char(42),
-char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(110),char(101),
-char(120),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(65),char(114),char(114),char(97),char(121),char(80),
-char(116),char(114),char(0),char(42),char(109),char(95),char(107),char(101),char(121),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),
-char(99),char(111),char(110),char(118),char(101),char(120),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(112),char(108),char(97),char(110),
-char(97),char(114),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(101),char(113),char(117),char(97),char(108),char(86),char(101),char(114),
-char(116),char(101),char(120),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(101),char(100),char(103),char(101),char(68),
-char(105),char(115),char(116),char(97),char(110),char(99),char(101),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(122),
-char(101),char(114),char(111),char(65),char(114),char(101),char(97),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(101),char(120),char(116),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(83),
-char(105),char(122),char(101),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(97),char(108),char(117),char(101),char(115),char(0),char(109),char(95),char(110),char(117),
-char(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(103),char(105),char(109),char(112),char(97),char(99),char(116),char(83),char(117),char(98),char(84),char(121),
-char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),
-char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),
-char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),
-char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(112),char(97),
-char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0),char(42),char(109),char(95),char(98),char(114),char(111),char(97),char(100),char(112),char(104),
-char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0),char(42),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(0),char(42),char(109),char(95),char(114),char(111),char(111),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(0),char(109),char(95),char(119),char(111),char(114),char(108),char(100),char(84),char(114),char(97),
-char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),
-char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),
-char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101),
-char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),
-char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),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(105),char(115),char(111),char(116),char(114),char(111),char(112),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(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(114),char(111),char(99),char(101),char(115),char(115),char(105),char(110),char(103),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(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116),
-char(105),char(111),char(110),char(84),char(105),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),
-char(95),char(114),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(0),char(109),char(95),char(114),
-char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99),
-char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(99),char(100),char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114),
-char(101),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(99),char(100),char(77),char(111),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(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116),
-char(114),char(111),char(112),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(99),char(111),char(108),char(108),
-char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84),
-char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95),
-char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105),
-char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67),
-char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73),
-char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),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(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110),
-char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0),
-char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),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(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(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70),
-char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101),
-char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),
-char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0),
-char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101),
-char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),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(68),char(97),char(109),char(112),char(105),char(110),char(103),
-char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),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(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76),
-char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),
-char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),
-char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),
-char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),
-char(108),char(97),char(114),char(68),char(97),char(109),char(112),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(108),
-char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),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(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),
-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(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),
-char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116),
-char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65),
-char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101),
-char(0),char(109),char(95),char(117),char(115),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(84),char(121),char(112),
-char(101),char(0),char(109),char(95),char(117),char(115),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(73),char(100),
-char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101),char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97),
-char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68),
-char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108),
-char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116),char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100),
-char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111),char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109),
-char(83),char(111),char(108),char(118),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(98),
-char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112),char(117),char(108),char(115),char(101),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(105),char(115),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(0),char(112),char(97),char(100),char(100),
-char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(121),char(112),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(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109),
-char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101),
-char(0),char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102),
-char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),
-char(97),char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108),
-char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),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(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114),
-char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),
-char(0),char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105),
-char(116),char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116),
-char(111),char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111),
-char(114),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(115),char(119),
-char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),
-char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109),char(95),char(100),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(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),
-char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),
-char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),
-char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),
-char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),char(114),char(101),char(110),
-char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),char(115),char(101),char(116),
-char(70),char(111),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(114),char(97),char(109),char(101),char(0),char(109),
-char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(69),char(110),
-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(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(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(76),char(105),char(109),char(105),char(116),char(101),char(100),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(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),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(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(76),char(105),char(109),char(105),char(116),char(101),char(100),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(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),
-char(76),char(105),char(109),char(105),char(116),char(101),char(100),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(109),char(95),char(122),char(101),char(114),char(111),char(82),char(111),char(116),char(80),char(97),
-char(114),char(101),char(110),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(67),
-char(111),char(109),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102),char(102),char(115),char(101),char(116),char(0),char(109),char(95),
-char(116),char(104),char(105),char(115),char(80),char(105),char(118),char(111),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102),
-char(102),char(115),char(101),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(84),char(111),char(112),char(91),
-char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(66),char(111),char(116),char(116),char(111),char(109),
-char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(106),
-char(111),char(105),char(110),char(116),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(67),char(111),char(108),char(108),
-char(105),char(100),char(101),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),
-char(95),char(108),char(105),char(110),char(107),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(73),char(110),
-char(100),char(101),char(120),char(0),char(109),char(95),char(100),char(111),char(102),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(112),char(111),char(115),
-char(86),char(97),char(114),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(80),char(111),char(115),char(91),
-char(55),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(86),char(101),char(108),char(91),char(54),char(93),char(0),char(109),char(95),char(106),
-char(111),char(105),char(110),char(116),char(84),char(111),char(114),char(113),char(117),char(101),char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(98),char(97),char(115),
-char(101),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(67),char(111),char(108),char(108),char(105),char(100),char(101),
-char(114),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),
-char(114),char(109),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(98),
-char(97),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(84),char(89),char(80),char(69),char(95),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(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),
-char(111),char(110),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(116),char(101),char(114),
-char(110),char(105),char(111),char(110),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(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(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),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(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),
-char(110),char(107),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(117),char(108),char(116),char(105),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(77),char(117),char(108),char(116),
-char(105),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(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(-112),char(2),char(-64),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(-56),char(2),char(120),char(1),char(-56),char(0),char(112),char(0),char(0),char(0),char(83),char(84),char(82),char(67),char(84),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(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0),
-char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0),
-char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(21),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(22),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(23),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(24),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(25),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(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),
-char(26),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(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0),
-char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),char(27),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(28),char(0),char(5),char(0),char(27),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(29),char(0),char(5),char(0),char(27),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(30),char(0),char(2),char(0),
-char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0),
-char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0),
-char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),
-char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0),
-char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0),
-char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),char(36),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(38),char(0),char(7),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),
-char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),
-char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0),
-char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0),
-char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(43),char(0),char(3),char(0),char(29),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(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0),
-char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),
-char(46),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(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(46),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(47),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),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(48),char(0),char(5),char(0),char(29),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(49),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),
-char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(104),char(0),char(20),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(50),char(0),char(25),char(0),char(9),char(0),char(101),char(0),
-char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(19),char(0),char(104),char(0),char(19),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(51),char(0),char(2),char(0),
-char(52),char(0),char(124),char(0),char(14),char(0),char(125),char(0),char(53),char(0),char(2),char(0),char(54),char(0),char(124),char(0),char(13),char(0),char(125),char(0),
-char(55),char(0),char(21),char(0),char(50),char(0),char(126),char(0),char(17),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(56),char(0),char(22),char(0),char(49),char(0),char(126),char(0),char(18),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(57),char(0),char(2),char(0),char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-108),char(0),
-char(55),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(13),char(0),char(60),char(0),char(-108),char(0),char(60),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(61),char(0),char(14),char(0),char(56),char(0),char(-108),char(0),char(56),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(62),char(0),char(3),char(0),char(59),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0),
-char(63),char(0),char(3),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(64),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(65),char(0),char(13),char(0),char(59),char(0),char(-95),char(0),
-char(20),char(0),char(-92),char(0),char(20),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(66),char(0),char(13),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),
-char(19),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(67),char(0),char(14),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),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(68),char(0),char(10),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),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(69),char(0),char(11),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),
-char(19),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(70),char(0),char(9),char(0),
-char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),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(71),char(0),char(9),char(0),
-char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),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(72),char(0),char(5),char(0),
-char(70),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(73),char(0),char(5),char(0),char(71),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(74),char(0),char(41),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),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(-50),char(0),char(0),char(0),char(-49),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(-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(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),
-char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-95),char(0),
-char(20),char(0),char(-92),char(0),char(20),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(-50),char(0),char(0),char(0),char(-49),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(-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(14),char(0),char(-39),char(0),char(14),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),
-char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0),
-char(76),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),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(77),char(0),char(9),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),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(78),char(0),char(5),char(0),char(58),char(0),char(-95),char(0),char(13),char(0),char(-31),char(0),char(13),char(0),char(-30),char(0),char(7),char(0),char(-29),char(0),
-char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-31),char(0),char(14),char(0),char(-30),char(0),
-char(8),char(0),char(-29),char(0),char(52),char(0),char(22),char(0),char(8),char(0),char(-28),char(0),char(8),char(0),char(-76),char(0),char(8),char(0),char(111),char(0),
-char(8),char(0),char(-27),char(0),char(8),char(0),char(113),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(8),char(0),char(-18),char(0),char(8),char(0),char(-17),char(0),char(8),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),
-char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(22),char(0),
-char(7),char(0),char(-28),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-27),char(0),char(7),char(0),char(113),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(7),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0),
-char(7),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),
-char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(80),char(0),char(4),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(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0),char(80),char(0),char(-7),char(0),char(13),char(0),char(-6),char(0),
-char(13),char(0),char(-5),char(0),char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0),char(13),char(0),char(-2),char(0),char(7),char(0),char(-120),char(0),
-char(7),char(0),char(-1),char(0),char(4),char(0),char(0),char(1),char(4),char(0),char(53),char(0),char(82),char(0),char(4),char(0),char(80),char(0),char(-7),char(0),
-char(4),char(0),char(1),char(1),char(7),char(0),char(2),char(1),char(4),char(0),char(3),char(1),char(83),char(0),char(4),char(0),char(13),char(0),char(-2),char(0),
-char(80),char(0),char(-7),char(0),char(4),char(0),char(4),char(1),char(7),char(0),char(5),char(1),char(84),char(0),char(7),char(0),char(13),char(0),char(6),char(1),
-char(80),char(0),char(-7),char(0),char(4),char(0),char(7),char(1),char(7),char(0),char(8),char(1),char(7),char(0),char(9),char(1),char(7),char(0),char(10),char(1),
-char(4),char(0),char(53),char(0),char(85),char(0),char(6),char(0),char(17),char(0),char(11),char(1),char(13),char(0),char(9),char(1),char(13),char(0),char(12),char(1),
-char(60),char(0),char(13),char(1),char(4),char(0),char(14),char(1),char(7),char(0),char(10),char(1),char(86),char(0),char(26),char(0),char(4),char(0),char(15),char(1),
-char(7),char(0),char(16),char(1),char(7),char(0),char(-76),char(0),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(7),char(0),char(32),char(1),char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1),
-char(4),char(0),char(35),char(1),char(4),char(0),char(36),char(1),char(4),char(0),char(37),char(1),char(4),char(0),char(38),char(1),char(4),char(0),char(118),char(0),
-char(87),char(0),char(12),char(0),char(17),char(0),char(39),char(1),char(17),char(0),char(40),char(1),char(17),char(0),char(41),char(1),char(13),char(0),char(42),char(1),
-char(13),char(0),char(43),char(1),char(7),char(0),char(44),char(1),char(4),char(0),char(45),char(1),char(4),char(0),char(46),char(1),char(4),char(0),char(47),char(1),
-char(4),char(0),char(48),char(1),char(7),char(0),char(8),char(1),char(4),char(0),char(53),char(0),char(88),char(0),char(27),char(0),char(19),char(0),char(49),char(1),
-char(17),char(0),char(50),char(1),char(17),char(0),char(51),char(1),char(13),char(0),char(42),char(1),char(13),char(0),char(52),char(1),char(13),char(0),char(53),char(1),
-char(13),char(0),char(54),char(1),char(13),char(0),char(55),char(1),char(13),char(0),char(56),char(1),char(4),char(0),char(57),char(1),char(7),char(0),char(58),char(1),
-char(4),char(0),char(59),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(7),char(0),char(62),char(1),char(7),char(0),char(63),char(1),
-char(4),char(0),char(64),char(1),char(4),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(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1),char(4),char(0),char(72),char(1),char(4),char(0),char(73),char(1),
-char(4),char(0),char(74),char(1),char(89),char(0),char(12),char(0),char(9),char(0),char(75),char(1),char(9),char(0),char(76),char(1),char(13),char(0),char(77),char(1),
-char(7),char(0),char(78),char(1),char(7),char(0),char(-24),char(0),char(7),char(0),char(79),char(1),char(4),char(0),char(80),char(1),char(13),char(0),char(81),char(1),
-char(4),char(0),char(82),char(1),char(4),char(0),char(83),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(53),char(0),char(90),char(0),char(19),char(0),
-char(50),char(0),char(126),char(0),char(87),char(0),char(85),char(1),char(80),char(0),char(86),char(1),char(81),char(0),char(87),char(1),char(82),char(0),char(88),char(1),
-char(83),char(0),char(89),char(1),char(84),char(0),char(90),char(1),char(85),char(0),char(91),char(1),char(88),char(0),char(92),char(1),char(89),char(0),char(93),char(1),
-char(4),char(0),char(94),char(1),char(4),char(0),char(60),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(4),char(0),char(98),char(1),char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1),char(86),char(0),char(101),char(1),char(91),char(0),char(17),char(0),
-char(16),char(0),char(102),char(1),char(14),char(0),char(103),char(1),char(14),char(0),char(104),char(1),char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1),
-char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1),char(49),char(0),char(109),char(1),char(14),char(0),char(110),char(1),char(8),char(0),char(111),char(1),
-char(4),char(0),char(112),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(113),char(1),char(4),char(0),char(114),char(1),char(8),char(0),char(115),char(1),
-char(8),char(0),char(116),char(1),char(8),char(0),char(117),char(1),char(92),char(0),char(17),char(0),char(15),char(0),char(102),char(1),char(13),char(0),char(103),char(1),
-char(13),char(0),char(104),char(1),char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1),char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1),
-char(50),char(0),char(109),char(1),char(13),char(0),char(110),char(1),char(4),char(0),char(113),char(1),char(7),char(0),char(111),char(1),char(4),char(0),char(112),char(1),
-char(4),char(0),char(84),char(1),char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1),char(7),char(0),char(117),char(1),char(4),char(0),char(114),char(1),
-char(93),char(0),char(8),char(0),char(0),char(0),char(118),char(1),char(91),char(0),char(88),char(1),char(49),char(0),char(119),char(1),char(20),char(0),char(120),char(1),
-char(14),char(0),char(121),char(1),char(4),char(0),char(95),char(1),char(8),char(0),char(122),char(1),char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0),
-char(0),char(0),char(118),char(1),char(92),char(0),char(88),char(1),char(50),char(0),char(119),char(1),char(19),char(0),char(120),char(1),char(13),char(0),char(121),char(1),
-char(7),char(0),char(122),char(1),char(4),char(0),char(95),char(1),};
-int sBulletDNAlen64= sizeof(sBulletDNAstr64);
diff --git a/extern/bullet2/src/LinearMath/btSerializer.h b/extern/bullet2/src/LinearMath/btSerializer.h
index 033895b1e5e..2ee712047f5 100644
--- a/extern/bullet2/src/LinearMath/btSerializer.h
+++ b/extern/bullet2/src/LinearMath/btSerializer.h
@@ -16,126 +16,117 @@ subject to the following restrictions:
 #ifndef BT_SERIALIZER_H
 #define BT_SERIALIZER_H
 
-#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE
+#include "btScalar.h"  // has definitions like SIMD_FORCE_INLINE
 #include "btHashMap.h"
 
-#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__)
+#if !defined(__CELLOS_LV2__) && !defined(__MWERKS__)
 #include <memory.h>
 #endif
 #include <string.h>
 
-
-
-///only the 32bit versions for now
 extern char sBulletDNAstr[];
 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)
-		return(0);
+	if (!str)
+		return (0);
 	int len = 0;
 
 	while (*str != 0)
 	{
-        str++;
-        len++;
-    }
+		str++;
+		len++;
+	}
 
-    return len;
+	return len;
 }
 
-
 class btChunk
 {
 public:
-	int		m_chunkCode;
-	int		m_length;
-	void	*m_oldPtr;
-	int		m_dna_nr;
-	int		m_number;
+	int m_chunkCode;
+	int m_length;
+	void* m_oldPtr;
+	int m_dna_nr;
+	int m_number;
 };
 
-enum	btSerializationFlags
+enum btSerializationFlags
 {
 	BT_SERIALIZE_NO_BVH = 1,
 	BT_SERIALIZE_NO_TRIANGLEINFOMAP = 2,
-	BT_SERIALIZE_NO_DUPLICATE_ASSERT = 4
+	BT_SERIALIZE_NO_DUPLICATE_ASSERT = 4,
+	BT_SERIALIZE_CONTACT_MANIFOLDS = 8,
 };
 
-class	btSerializer
+class btSerializer
 {
-
 public:
-
 	virtual ~btSerializer() {}
 
-	virtual	const unsigned char*		getBufferPointer() const = 0;
+	virtual const unsigned char* getBufferPointer() const = 0;
 
-	virtual	int		getCurrentBufferSize() const = 0;
+	virtual int getCurrentBufferSize() const = 0;
 
-	virtual	btChunk*	allocate(size_t size, int numElements) = 0;
+	virtual btChunk* allocate(size_t size, int numElements) = 0;
 
-	virtual	void	finalizeChunk(btChunk* chunk, const char* structType, int chunkCode,void* oldPtr)= 0;
+	virtual void finalizeChunk(btChunk* chunk, const char* structType, int chunkCode, void* oldPtr) = 0;
 
-	virtual	 void*	findPointer(void* oldPtr)  = 0;
+	virtual void* findPointer(void* oldPtr) = 0;
 
-	virtual	void*	getUniquePointer(void*oldPtr) = 0;
+	virtual void* getUniquePointer(void* oldPtr) = 0;
 
-	virtual	void	startSerialization() = 0;
+	virtual void startSerialization() = 0;
 
-	virtual	void	finishSerialization() = 0;
+	virtual void finishSerialization() = 0;
 
-	virtual	const char*	findNameForPointer(const void* ptr) const = 0;
+	virtual const char* findNameForPointer(const void* ptr) const = 0;
 
-	virtual	void	registerNameForPointer(const void* ptr, const char* name) = 0;
+	virtual void registerNameForPointer(const void* ptr, const char* name) = 0;
 
-	virtual void	serializeName(const char* ptr) = 0;
+	virtual void serializeName(const char* ptr) = 0;
 
-	virtual int		getSerializationFlags() const = 0;
+	virtual int getSerializationFlags() const = 0;
 
-	virtual void	setSerializationFlags(int flags) = 0;
+	virtual void setSerializationFlags(int flags) = 0;
 
 	virtual int getNumChunks() const = 0;
 
 	virtual const btChunk* getChunk(int chunkIndex) const = 0;
-
 };
 
-
-
 #define BT_HEADER_LENGTH 12
-#if defined(__sgi) || defined (__sparc) || defined (__sparc__) || defined (__PPC__) || defined (__ppc__) || defined (__BIG_ENDIAN__)
-#	define BT_MAKE_ID(a,b,c,d) ( (int)(a)<<24 | (int)(b)<<16 | (c)<<8 | (d) )
+#if defined(__sgi) || defined(__sparc) || defined(__sparc__) || defined(__PPC__) || defined(__ppc__) || defined(__BIG_ENDIAN__)
+#define BT_MAKE_ID(a, b, c, d) ((int)(a) << 24 | (int)(b) << 16 | (c) << 8 | (d))
 #else
-#	define BT_MAKE_ID(a,b,c,d) ( (int)(d)<<24 | (int)(c)<<16 | (b)<<8 | (a) )
+#define BT_MAKE_ID(a, b, c, d) ((int)(d) << 24 | (int)(c) << 16 | (b) << 8 | (a))
 #endif
 
-
-#define BT_MULTIBODY_CODE       BT_MAKE_ID('M','B','D','Y')
-#define BT_SOFTBODY_CODE		BT_MAKE_ID('S','B','D','Y')
-#define BT_COLLISIONOBJECT_CODE BT_MAKE_ID('C','O','B','J')
-#define BT_RIGIDBODY_CODE		BT_MAKE_ID('R','B','D','Y')
-#define BT_CONSTRAINT_CODE		BT_MAKE_ID('C','O','N','S')
-#define BT_BOXSHAPE_CODE		BT_MAKE_ID('B','O','X','S')
-#define BT_QUANTIZED_BVH_CODE	BT_MAKE_ID('Q','B','V','H')
-#define BT_TRIANLGE_INFO_MAP	BT_MAKE_ID('T','M','A','P')
-#define BT_SHAPE_CODE			BT_MAKE_ID('S','H','A','P')
-#define BT_ARRAY_CODE			BT_MAKE_ID('A','R','A','Y')
-#define BT_SBMATERIAL_CODE		BT_MAKE_ID('S','B','M','T')
-#define BT_SBNODE_CODE			BT_MAKE_ID('S','B','N','D')
-#define BT_DYNAMICSWORLD_CODE	BT_MAKE_ID('D','W','L','D')
-#define BT_DNA_CODE				BT_MAKE_ID('D','N','A','1')
-
-
-struct	btPointerUid
+#define BT_MULTIBODY_CODE BT_MAKE_ID('M', 'B', 'D', 'Y')
+#define BT_MB_LINKCOLLIDER_CODE BT_MAKE_ID('M', 'B', 'L', 'C')
+#define BT_SOFTBODY_CODE BT_MAKE_ID('S', 'B', 'D', 'Y')
+#define BT_COLLISIONOBJECT_CODE BT_MAKE_ID('C', 'O', 'B', 'J')
+#define BT_RIGIDBODY_CODE BT_MAKE_ID('R', 'B', 'D', 'Y')
+#define BT_CONSTRAINT_CODE BT_MAKE_ID('C', 'O', 'N', 'S')
+#define BT_BOXSHAPE_CODE BT_MAKE_ID('B', 'O', 'X', 'S')
+#define BT_QUANTIZED_BVH_CODE BT_MAKE_ID('Q', 'B', 'V', 'H')
+#define BT_TRIANLGE_INFO_MAP BT_MAKE_ID('T', 'M', 'A', 'P')
+#define BT_SHAPE_CODE BT_MAKE_ID('S', 'H', 'A', 'P')
+#define BT_ARRAY_CODE BT_MAKE_ID('A', 'R', 'A', 'Y')
+#define BT_SBMATERIAL_CODE BT_MAKE_ID('S', 'B', 'M', 'T')
+#define BT_SBNODE_CODE BT_MAKE_ID('S', 'B', 'N', 'D')
+#define BT_DYNAMICSWORLD_CODE BT_MAKE_ID('D', 'W', 'L', 'D')
+#define BT_CONTACTMANIFOLD_CODE BT_MAKE_ID('C', 'O', 'N', 'T')
+#define BT_DNA_CODE BT_MAKE_ID('D', 'N', 'A', '1')
+
+struct btPointerUid
 {
-	union
-	{
-		void*	m_ptr;
-		int		m_uniqueIds[2];
+	union {
+		void* m_ptr;
+		int m_uniqueIds[2];
 	};
 };
 
@@ -144,8 +135,8 @@ struct btBulletSerializedArrays
 	btBulletSerializedArrays()
 	{
 	}
-	btAlignedObjectArray<struct btQuantizedBvhDoubleData*>	m_bvhsDouble;
-	btAlignedObjectArray<struct btQuantizedBvhFloatData*>	m_bvhsFloat;
+	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;
@@ -155,51 +146,42 @@ struct btBulletSerializedArrays
 	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 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
+class btDefaultSerializer : public btSerializer
 {
-
 protected:
+	btAlignedObjectArray<char*> mTypes;
+	btAlignedObjectArray<short*> mStructs;
+	btAlignedObjectArray<short> mTlens;
+	btHashMap<btHashInt, int> mStructReverse;
+	btHashMap<btHashString, int> mTypeLookup;
 
-	btAlignedObjectArray<char*>			mTypes;
-	btAlignedObjectArray<short*>			mStructs;
-	btAlignedObjectArray<short>			mTlens;
-	btHashMap<btHashInt, int>			mStructReverse;
-	btHashMap<btHashString,int>	mTypeLookup;
-	
-
-
-	btHashMap<btHashPtr,void*>	m_chunkP;
-
-	btHashMap<btHashPtr,const char*>	m_nameMap;
+	btHashMap<btHashPtr, void*> m_chunkP;
 
-	btHashMap<btHashPtr,btPointerUid>	m_uniquePointers;
-	int	m_uniqueIdGenerator;
+	btHashMap<btHashPtr, const char*> m_nameMap;
 
-	int					m_totalSize;
-	unsigned char*		m_buffer;
-	bool                m_ownsBuffer;
-	int					m_currentSize;
-	void*				m_dna;
-	int					m_dnaLength;
+	btHashMap<btHashPtr, btPointerUid> m_uniquePointers;
+	int m_uniqueIdGenerator;
 
-	int					m_serializationFlags;
+	int m_totalSize;
+	unsigned char* m_buffer;
+	bool m_ownsBuffer;
+	int m_currentSize;
+	void* m_dna;
+	int m_dnaLength;
 
+	int m_serializationFlags;
 
-	btAlignedObjectArray<btChunk*>	m_chunkPtrs;
+	btAlignedObjectArray<btChunk*> m_chunkPtrs;
 
 protected:
-
-	
-	virtual	void*	findPointer(void* oldPtr)
+	virtual void* findPointer(void* oldPtr)
 	{
 		void** ptr = m_chunkP.find(oldPtr);
 		if (ptr && *ptr)
@@ -207,48 +189,43 @@ protected:
 		return 0;
 	}
 
+	virtual void writeDNA()
+	{
+		btChunk* dnaChunk = allocate(m_dnaLength, 1);
+		memcpy(dnaChunk->m_oldPtr, m_dna, m_dnaLength);
+		finalizeChunk(dnaChunk, "DNA1", BT_DNA_CODE, m_dna);
+	}
 
+	int getReverseType(const char* type) const
+	{
+		btHashString key(type);
+		const int* valuePtr = mTypeLookup.find(key);
+		if (valuePtr)
+			return *valuePtr;
 
+		return -1;
+	}
 
+	void initDNA(const char* bdnaOrg, int dnalen)
+	{
+		///was already initialized
+		if (m_dna)
+			return;
 
-		virtual void	writeDNA()
-		{
-			btChunk* dnaChunk = allocate(m_dnaLength,1);
-			memcpy(dnaChunk->m_oldPtr,m_dna,m_dnaLength);
-			finalizeChunk(dnaChunk,"DNA1",BT_DNA_CODE, m_dna);
-		}
-
-		int getReverseType(const char *type) const
-		{
-
-			btHashString key(type);
-			const int* valuePtr = mTypeLookup.find(key);
-			if (valuePtr)
-				return *valuePtr;
-
-			return -1;
-		}
-
-		void initDNA(const char* bdnaOrg,int dnalen)
-		{
-			///was already initialized
-			if (m_dna)
-				return;
-
-			int littleEndian= 1;
-			littleEndian= ((char*)&littleEndian)[0];
-
+		int littleEndian = 1;
+		littleEndian = ((char*)&littleEndian)[0];
 
-			m_dna = btAlignedAlloc(dnalen,16);
-			memcpy(m_dna,bdnaOrg,dnalen);
-			m_dnaLength = dnalen;
+		m_dna = btAlignedAlloc(dnalen, 16);
+		memcpy(m_dna, bdnaOrg, dnalen);
+		m_dnaLength = dnalen;
 
-			int *intPtr=0;
-			short *shtPtr=0;
-			char *cp = 0;int dataLen =0;
-			intPtr = (int*)m_dna;
+		int* intPtr = 0;
+		short* shtPtr = 0;
+		char* cp = 0;
+		int dataLen = 0;
+		intPtr = (int*)m_dna;
 
-			/*
+		/*
 				SDNA (4 bytes) (magic number)
 				NAME (4 bytes)
 				<nr> (4 bytes) amount of names (int)
@@ -256,81 +233,81 @@ protected:
 				<string>
 			*/
 
-			if (strncmp((const char*)m_dna, "SDNA", 4)==0)
-			{
-				// skip ++ NAME
-				intPtr++; intPtr++;
-			}
-
-			// Parse names
-			if (!littleEndian)
-				*intPtr = btSwapEndian(*intPtr);
+		if (strncmp((const char*)m_dna, "SDNA", 4) == 0)
+		{
+			// skip ++ NAME
+			intPtr++;
+			intPtr++;
+		}
 
-			dataLen = *intPtr;
+		// Parse names
+		if (!littleEndian)
+			*intPtr = btSwapEndian(*intPtr);
 
-			intPtr++;
+		dataLen = *intPtr;
 
-			cp = (char*)intPtr;
-			int i;
-			for ( i=0; i<dataLen; i++)
-			{
+		intPtr++;
 
-				while (*cp)cp++;
-				cp++;
-			}
-			cp = btAlignPointer(cp,4);
+		cp = (char*)intPtr;
+		int i;
+		for (i = 0; i < dataLen; i++)
+		{
+			while (*cp) cp++;
+			cp++;
+		}
+		cp = btAlignPointer(cp, 4);
 
-			/*
+		/*
 				TYPE (4 bytes)
 				<nr> amount of types (int)
 				<string>
 				<string>
 			*/
 
-			intPtr = (int*)cp;
-			btAssert(strncmp(cp, "TYPE", 4)==0); intPtr++;
+		intPtr = (int*)cp;
+		btAssert(strncmp(cp, "TYPE", 4) == 0);
+		intPtr++;
 
-			if (!littleEndian)
-				*intPtr =  btSwapEndian(*intPtr);
-
-			dataLen = *intPtr;
-			intPtr++;
+		if (!littleEndian)
+			*intPtr = btSwapEndian(*intPtr);
 
+		dataLen = *intPtr;
+		intPtr++;
 
-			cp = (char*)intPtr;
-			for (i=0; i<dataLen; i++)
-			{
-				mTypes.push_back(cp);
-				while (*cp)cp++;
-				cp++;
-			}
-
-			cp = btAlignPointer(cp,4);
+		cp = (char*)intPtr;
+		for (i = 0; i < dataLen; i++)
+		{
+			mTypes.push_back(cp);
+			while (*cp) cp++;
+			cp++;
+		}
 
+		cp = btAlignPointer(cp, 4);
 
-			/*
+		/*
 				TLEN (4 bytes)
 				<len> (short) the lengths of types
 				<len>
 			*/
 
-			// Parse type lens
-			intPtr = (int*)cp;
-			btAssert(strncmp(cp, "TLEN", 4)==0); intPtr++;
+		// Parse type lens
+		intPtr = (int*)cp;
+		btAssert(strncmp(cp, "TLEN", 4) == 0);
+		intPtr++;
 
-			dataLen = (int)mTypes.size();
+		dataLen = (int)mTypes.size();
 
-			shtPtr = (short*)intPtr;
-			for (i=0; i<dataLen; i++, shtPtr++)
-			{
-				if (!littleEndian)
-					shtPtr[0] = btSwapEndian(shtPtr[0]);
-				mTlens.push_back(shtPtr[0]);
-			}
+		shtPtr = (short*)intPtr;
+		for (i = 0; i < dataLen; i++, shtPtr++)
+		{
+			if (!littleEndian)
+				shtPtr[0] = btSwapEndian(shtPtr[0]);
+			mTlens.push_back(shtPtr[0]);
+		}
 
-			if (dataLen & 1) shtPtr++;
+		if (dataLen & 1) shtPtr++;
 
-			/*
+		/*
 				STRC (4 bytes)
 				<nr> amount of structs (int)
 				<typenr>
@@ -341,362 +318,372 @@ protected:
 				<namenr>
 			*/
 
-			intPtr = (int*)shtPtr;
-			cp = (char*)intPtr;
-			btAssert(strncmp(cp, "STRC", 4)==0); intPtr++;
+		intPtr = (int*)shtPtr;
+		cp = (char*)intPtr;
+		btAssert(strncmp(cp, "STRC", 4) == 0);
+		intPtr++;
 
-			if (!littleEndian)
-				*intPtr = btSwapEndian(*intPtr);
-			dataLen = *intPtr ;
-			intPtr++;
+		if (!littleEndian)
+			*intPtr = btSwapEndian(*intPtr);
+		dataLen = *intPtr;
+		intPtr++;
 
+		shtPtr = (short*)intPtr;
+		for (i = 0; i < dataLen; i++)
+		{
+			mStructs.push_back(shtPtr);
 
-			shtPtr = (short*)intPtr;
-			for (i=0; i<dataLen; i++)
+			if (!littleEndian)
 			{
-				mStructs.push_back (shtPtr);
-
-				if (!littleEndian)
-				{
-					shtPtr[0]= btSwapEndian(shtPtr[0]);
-					shtPtr[1]= btSwapEndian(shtPtr[1]);
-
-					int len = shtPtr[1];
-					shtPtr+= 2;
+				shtPtr[0] = btSwapEndian(shtPtr[0]);
+				shtPtr[1] = btSwapEndian(shtPtr[1]);
 
-					for (int a=0; a<len; a++, shtPtr+=2)
-					{
-							shtPtr[0]= btSwapEndian(shtPtr[0]);
-							shtPtr[1]= btSwapEndian(shtPtr[1]);
-					}
+				int len = shtPtr[1];
+				shtPtr += 2;
 
-				} else
+				for (int a = 0; a < len; a++, shtPtr += 2)
 				{
-					shtPtr+= (2*shtPtr[1])+2;
+					shtPtr[0] = btSwapEndian(shtPtr[0]);
+					shtPtr[1] = btSwapEndian(shtPtr[1]);
 				}
 			}
-
-			// build reverse lookups
-			for (i=0; i<(int)mStructs.size(); i++)
+			else
 			{
-				short *strc = mStructs.at(i);
-				mStructReverse.insert(strc[0], i);
-				mTypeLookup.insert(btHashString(mTypes[strc[0]]),i);
+				shtPtr += (2 * shtPtr[1]) + 2;
 			}
 		}
 
-public:
-
-	btHashMap<btHashPtr,void*> m_skipPointers;
-
+		// build reverse lookups
+		for (i = 0; i < (int)mStructs.size(); i++)
+		{
+			short* strc = mStructs.at(i);
+			mStructReverse.insert(strc[0], i);
+			mTypeLookup.insert(btHashString(mTypes[strc[0]]), i);
+		}
+	}
 
-		btDefaultSerializer(int totalSize=0, unsigned char*	buffer=0)
-			:m_totalSize(totalSize),
-			m_currentSize(0),
-			m_dna(0),
-			m_dnaLength(0),
-			m_serializationFlags(0)
+public:
+	btHashMap<btHashPtr, void*> m_skipPointers;
+
+	btDefaultSerializer(int totalSize = 0, unsigned char* buffer = 0)
+		: m_uniqueIdGenerator(0),
+		  m_totalSize(totalSize),
+		  m_currentSize(0),
+		  m_dna(0),
+		  m_dnaLength(0),
+		  m_serializationFlags(0)
+	{
+		if (buffer == 0)
+		{
+			m_buffer = m_totalSize ? (unsigned char*)btAlignedAlloc(totalSize, 16) : 0;
+			m_ownsBuffer = true;
+		}
+		else
 		{
-		    if (buffer==0)
-            {
-                m_buffer = m_totalSize?(unsigned char*)btAlignedAlloc(totalSize,16):0;
-                m_ownsBuffer = true;
-            } else
-            {
-                m_buffer = buffer;
-                m_ownsBuffer = false;
-            }
-
-			const bool VOID_IS_8 = ((sizeof(void*)==8));
+			m_buffer = buffer;
+			m_ownsBuffer = false;
+		}
+
+		const bool VOID_IS_8 = ((sizeof(void*) == 8));
 
 #ifdef BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
-			if (VOID_IS_8)
-			{
+		if (VOID_IS_8)
+		{
 #if _WIN64
-				initDNA((const char*)sBulletDNAstr64,sBulletDNAlen64);
+			initDNA((const char*)sBulletDNAstr64, sBulletDNAlen64);
 #else
-				btAssert(0);
+			btAssert(0);
 #endif
-			} else
-			{
+		}
+		else
+		{
 #ifndef _WIN64
-				initDNA((const char*)sBulletDNAstr,sBulletDNAlen);
+			initDNA((const char*)sBulletDNAstr, sBulletDNAlen);
 #else
-				btAssert(0);
+			btAssert(0);
 #endif
-			}
-
-#else //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
-			if (VOID_IS_8)
-			{
-				initDNA((const char*)sBulletDNAstr64,sBulletDNAlen64);
-			} else
-			{
-				initDNA((const char*)sBulletDNAstr,sBulletDNAlen);
-			}
-#endif //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
-
 		}
 
-		virtual ~btDefaultSerializer()
+#else   //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
+		if (VOID_IS_8)
 		{
-			if (m_buffer && m_ownsBuffer)
-				btAlignedFree(m_buffer);
-			if (m_dna)
-				btAlignedFree(m_dna);
+			initDNA((const char*)sBulletDNAstr64, sBulletDNAlen64);
 		}
-
-		void	insertHeader()
+		else
 		{
-			writeHeader(m_buffer);
-			m_currentSize += BT_HEADER_LENGTH;
+			initDNA((const char*)sBulletDNAstr, sBulletDNAlen);
 		}
+#endif  //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
+	}
 
-		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
+	virtual ~btDefaultSerializer()
+	{
+		if (m_buffer && m_ownsBuffer)
+			btAlignedFree(m_buffer);
+		if (m_dna)
+			btAlignedFree(m_dna);
+	}
 
-			int littleEndian= 1;
-			littleEndian= ((char*)&littleEndian)[0];
+	static int getMemoryDnaSizeInBytes()
+	{
+		const bool VOID_IS_8 = ((sizeof(void*) == 8));
 
-			if (sizeof(void*)==8)
-			{
-				buffer[7] = '-';
-			} else
-			{
-				buffer[7] = '_';
-			}
+		if (VOID_IS_8)
+		{
+			return sBulletDNAlen64;
+		}
+		return sBulletDNAlen;
+	}
+	static const char* getMemoryDna()
+	{
+		const bool VOID_IS_8 = ((sizeof(void*) == 8));
+		if (VOID_IS_8)
+		{
+			return (const char*)sBulletDNAstr64;
+		}
+		return (const char*)sBulletDNAstr;
+	}
 
-			if (littleEndian)
-			{
-				buffer[8]='v';
-			} else
-			{
-				buffer[8]='V';
-			}
+	void insertHeader()
+	{
+		writeHeader(m_buffer);
+		m_currentSize += BT_HEADER_LENGTH;
+	}
 
+	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
 
-			buffer[9] = '2';
-			buffer[10] = '8';
-			buffer[11] = '4';
+		int littleEndian = 1;
+		littleEndian = ((char*)&littleEndian)[0];
 
+		if (sizeof(void*) == 8)
+		{
+			buffer[7] = '-';
 		}
-
-		virtual	void	startSerialization()
+		else
 		{
-			m_uniqueIdGenerator= 1;
-			if (m_totalSize)
-			{
-				unsigned char* buffer = internalAlloc(BT_HEADER_LENGTH);
-				writeHeader(buffer);
-			}
-
+			buffer[7] = '_';
 		}
 
-		virtual	void	finishSerialization()
+		if (littleEndian)
 		{
-			writeDNA();
-
-			//if we didn't pre-allocate a buffer, we need to create a contiguous buffer now
-			int mysize = 0;
-			if (!m_totalSize)
-			{
-				if (m_buffer)
-					btAlignedFree(m_buffer);
-
-				m_currentSize += BT_HEADER_LENGTH;
-				m_buffer = (unsigned char*)btAlignedAlloc(m_currentSize,16);
+			buffer[8] = 'v';
+		}
+		else
+		{
+			buffer[8] = 'V';
+		}
 
-				unsigned char* currentPtr = m_buffer;
-				writeHeader(m_buffer);
-				currentPtr += BT_HEADER_LENGTH;
-				mysize+=BT_HEADER_LENGTH;
-				for (int i=0;i<	m_chunkPtrs.size();i++)
-				{
-					int curLength = sizeof(btChunk)+m_chunkPtrs[i]->m_length;
-					memcpy(currentPtr,m_chunkPtrs[i], curLength);
-					btAlignedFree(m_chunkPtrs[i]);
-					currentPtr+=curLength;
-					mysize+=curLength;
-				}
-			}
+		buffer[9] = '2';
+		buffer[10] = '8';
+		buffer[11] = '9';
+	}
 
-			mTypes.clear();
-			mStructs.clear();
-			mTlens.clear();
-			mStructReverse.clear();
-			mTypeLookup.clear();
-			m_skipPointers.clear();
-			m_chunkP.clear();
-			m_nameMap.clear();
-			m_uniquePointers.clear();
-			m_chunkPtrs.clear();
+	virtual void startSerialization()
+	{
+		m_uniqueIdGenerator = 1;
+		if (m_totalSize)
+		{
+			unsigned char* buffer = internalAlloc(BT_HEADER_LENGTH);
+			writeHeader(buffer);
 		}
+	}
 
-		virtual	void*	getUniquePointer(void*oldPtr)
+	virtual void finishSerialization()
+	{
+		writeDNA();
+
+		//if we didn't pre-allocate a buffer, we need to create a contiguous buffer now
+		int mysize = 0;
+		if (!m_totalSize)
 		{
-			if (!oldPtr)
-				return 0;
+			if (m_buffer)
+				btAlignedFree(m_buffer);
 
-			btPointerUid* uptr = (btPointerUid*)m_uniquePointers.find(oldPtr);
-			if (uptr)
-			{
-				return uptr->m_ptr;
-			}
+			m_currentSize += BT_HEADER_LENGTH;
+			m_buffer = (unsigned char*)btAlignedAlloc(m_currentSize, 16);
 
-			void** ptr2 = m_skipPointers[oldPtr];
-            if (ptr2)
+			unsigned char* currentPtr = m_buffer;
+			writeHeader(m_buffer);
+			currentPtr += BT_HEADER_LENGTH;
+			mysize += BT_HEADER_LENGTH;
+			for (int i = 0; i < m_chunkPtrs.size(); i++)
 			{
-				return 0;
+				int curLength = sizeof(btChunk) + m_chunkPtrs[i]->m_length;
+				memcpy(currentPtr, m_chunkPtrs[i], curLength);
+				btAlignedFree(m_chunkPtrs[i]);
+				currentPtr += curLength;
+				mysize += curLength;
 			}
+		}
 
-			m_uniqueIdGenerator++;
-
-			btPointerUid uid;
-			uid.m_uniqueIds[0] = m_uniqueIdGenerator;
-			uid.m_uniqueIds[1] = m_uniqueIdGenerator;
-			m_uniquePointers.insert(oldPtr,uid);
-			return uid.m_ptr;
+		mTypes.clear();
+		mStructs.clear();
+		mTlens.clear();
+		mStructReverse.clear();
+		mTypeLookup.clear();
+		m_skipPointers.clear();
+		m_chunkP.clear();
+		m_nameMap.clear();
+		m_uniquePointers.clear();
+		m_chunkPtrs.clear();
+	}
 
-		}
+	virtual void* getUniquePointer(void* oldPtr)
+	{
+		btAssert(m_uniqueIdGenerator >= 0);
+		if (!oldPtr)
+			return 0;
 
-		virtual	const unsigned char*		getBufferPointer() const
+		btPointerUid* uptr = (btPointerUid*)m_uniquePointers.find(oldPtr);
+		if (uptr)
 		{
-			return m_buffer;
+			return uptr->m_ptr;
 		}
 
-		virtual	int					getCurrentBufferSize() const
+		void** ptr2 = m_skipPointers[oldPtr];
+		if (ptr2)
 		{
-			return	m_currentSize;
+			return 0;
 		}
 
-		virtual	void	finalizeChunk(btChunk* chunk, const char* structType, int chunkCode,void* oldPtr)
-		{
-			if (!(m_serializationFlags&BT_SERIALIZE_NO_DUPLICATE_ASSERT))
-			{
-				btAssert(!findPointer(oldPtr));
-			}
+		m_uniqueIdGenerator++;
 
-			chunk->m_dna_nr = getReverseType(structType);
-
-			chunk->m_chunkCode = chunkCode;
+		btPointerUid uid;
+		uid.m_uniqueIds[0] = m_uniqueIdGenerator;
+		uid.m_uniqueIds[1] = m_uniqueIdGenerator;
+		m_uniquePointers.insert(oldPtr, uid);
+		return uid.m_ptr;
+	}
 
-			void* uniquePtr = getUniquePointer(oldPtr);
+	virtual const unsigned char* getBufferPointer() const
+	{
+		return m_buffer;
+	}
 
-			m_chunkP.insert(oldPtr,uniquePtr);//chunk->m_oldPtr);
-			chunk->m_oldPtr = uniquePtr;//oldPtr;
+	virtual int getCurrentBufferSize() const
+	{
+		return m_currentSize;
+	}
 
+	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);
 
-		virtual unsigned char* internalAlloc(size_t size)
-		{
-			unsigned char* ptr = 0;
+		chunk->m_chunkCode = chunkCode;
 
-			if (m_totalSize)
-			{
-				ptr = m_buffer+m_currentSize;
-				m_currentSize += int(size);
-				btAssert(m_currentSize<m_totalSize);
-			} else
-			{
-				ptr = (unsigned char*)btAlignedAlloc(size,16);
-				m_currentSize += int(size);
-			}
-			return ptr;
-		}
+		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;
 
-		virtual	btChunk*	allocate(size_t size, int numElements)
+		if (m_totalSize)
 		{
+			ptr = m_buffer + m_currentSize;
+			m_currentSize += int(size);
+			btAssert(m_currentSize < m_totalSize);
+		}
+		else
+		{
+			ptr = (unsigned char*)btAlignedAlloc(size, 16);
+			m_currentSize += int(size);
+		}
+		return ptr;
+	}
 
-			unsigned char* ptr = internalAlloc(int(size)*numElements+sizeof(btChunk));
-
-			unsigned char* data = ptr + sizeof(btChunk);
+	virtual btChunk* allocate(size_t size, int numElements)
+	{
+		unsigned char* ptr = internalAlloc(int(size) * numElements + sizeof(btChunk));
 
-			btChunk* chunk = (btChunk*)ptr;
-			chunk->m_chunkCode = 0;
-			chunk->m_oldPtr = data;
-			chunk->m_length = int(size)*numElements;
-			chunk->m_number = numElements;
+		unsigned char* data = ptr + sizeof(btChunk);
 
-			m_chunkPtrs.push_back(chunk);
+		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;
-		}
+		return chunk;
+	}
 
-		virtual	const char*	findNameForPointer(const void* ptr) const
-		{
-			const char*const * namePtr = m_nameMap.find(ptr);
-			if (namePtr && *namePtr)
-				return *namePtr;
-			return 0;
+	virtual const char* findNameForPointer(const void* ptr) const
+	{
+		const char* const* namePtr = m_nameMap.find(ptr);
+		if (namePtr && *namePtr)
+			return *namePtr;
+		return 0;
+	}
 
-		}
+	virtual void registerNameForPointer(const void* ptr, const char* name)
+	{
+		m_nameMap.insert(ptr, name);
+	}
 
-		virtual	void	registerNameForPointer(const void* ptr, const char* name)
+	virtual void serializeName(const char* name)
+	{
+		if (name)
 		{
-			m_nameMap.insert(ptr,name);
-		}
+			//don't serialize name twice
+			if (findPointer((void*)name))
+				return;
 
-		virtual void	serializeName(const char* name)
-		{
-			if (name)
+			int len = btStrLen(name);
+			if (len)
 			{
-				//don't serialize name twice
-				if (findPointer((void*)name))
-					return;
-
-				int len = btStrLen(name);
-				if (len)
+				int newLen = len + 1;
+				int padding = ((newLen + 3) & ~3) - newLen;
+				newLen += padding;
+
+				//serialize name string now
+				btChunk* chunk = allocate(sizeof(char), newLen);
+				char* destinationName = (char*)chunk->m_oldPtr;
+				for (int i = 0; i < len; i++)
 				{
-
-					int newLen = len+1;
-					int padding = ((newLen+3)&~3)-newLen;
-					newLen += padding;
-
-					//serialize name string now
-					btChunk* chunk = allocate(sizeof(char),newLen);
-					char* destinationName = (char*)chunk->m_oldPtr;
-					for (int i=0;i<len;i++)
-					{
-						destinationName[i] = name[i];
-					}
-					destinationName[len] = 0;
-					finalizeChunk(chunk,"char",BT_ARRAY_CODE,(void*)name);
+					destinationName[i] = name[i];
 				}
+				destinationName[len] = 0;
+				finalizeChunk(chunk, "char", BT_ARRAY_CODE, (void*)name);
 			}
 		}
+	}
 
-		virtual int		getSerializationFlags() const
-		{
-			return m_serializationFlags;
-		}
+	virtual int getSerializationFlags() const
+	{
+		return m_serializationFlags;
+	}
 
-		virtual void	setSerializationFlags(int flags)
-		{
-			m_serializationFlags = flags;
-		}
-		int getNumChunks() const
-		{
-			return m_chunkPtrs.size();
-		}
+	virtual void setSerializationFlags(int flags)
+	{
+		m_serializationFlags = flags;
+	}
+	int getNumChunks() const
+	{
+		return m_chunkPtrs.size();
+	}
 
-		const btChunk* getChunk(int chunkIndex) const
-		{
-			return m_chunkPtrs[chunkIndex];
-		}
+	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.
@@ -706,181 +693,177 @@ public:
 
 struct btInMemorySerializer : public btDefaultSerializer
 {
-    btHashMap<btHashPtr,btChunk*> m_uid2ChunkPtr;
-    btHashMap<btHashPtr,void*> m_orgPtr2UniqueDataPtr;
-    btHashMap<btHashString,const void*> m_names2Ptr;
-    
-
-    btBulletSerializedArrays    m_arrays;
-
-    btInMemorySerializer(int totalSize=0, unsigned char*	buffer=0)
-    :btDefaultSerializer(totalSize,buffer)
-    {
-        
-    }
-
-    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;
-    }
+	btHashMap<btHashPtr, btChunk*> m_uid2ChunkPtr;
+	btHashMap<btHashPtr, void*> m_orgPtr2UniqueDataPtr;
+	btHashMap<btHashString, const void*> m_names2Ptr;
 
-    virtual void finalizeChunk(btChunk* chunk, const char* structType, int chunkCode,void* oldPtr)
-    {
-        if (!(m_serializationFlags&BT_SERIALIZE_NO_DUPLICATE_ASSERT))
-        {
-            btAssert(!findPointer(oldPtr));
-        }
+	btBulletSerializedArrays m_arrays;
 
-        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;
+	btInMemorySerializer(int totalSize = 0, unsigned char* buffer = 0)
+		: btDefaultSerializer(totalSize, buffer)
+	{
+	}
 
-        void* uid = findPointer(oldPtr);
-        m_uid2ChunkPtr.insert(uid,chunk);
+	virtual void startSerialization()
+	{
+		m_uid2ChunkPtr.clear();
+		//todo: m_arrays.clear();
+		btDefaultSerializer::startSerialization();
+	}
 
-        switch (chunk->m_chunkCode)
+	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)
 			{
-			case BT_SOFTBODY_CODE:
+				return oldPtr;
+			}
+			else
 			{
-	#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;
+				void** ptr2 = m_skipPointers[oldPtr];
+				if (ptr2)
+				{
+					return 0;
 				}
-			case BT_COLLISIONOBJECT_CODE:
+				else
 				{
-	#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;
+					//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;
-				};
+			{
+#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;
-				}
+			{
+#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;
-				}
+			{
+#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;
-				}
+			{
+				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;
-				}
+			{
+				break;
+			}
 			default:
-				{
-				}
-			};
-    }
-
-    int getNumChunks() const
-    {
-        return m_uid2ChunkPtr.size();
-    }
+			{
+			}
+		};
+	}
 
-    const btChunk* getChunk(int chunkIndex) const
-    {
-        return *m_uid2ChunkPtr.getAtIndex(chunkIndex);
-    }
+	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
+#endif  //ENABLE_INMEMORY_SERIALIZER
 
+#endif  //BT_SERIALIZER_H
diff --git a/extern/bullet2/src/LinearMath/btSerializer64.cpp b/extern/bullet2/src/LinearMath/btSerializer64.cpp
new file mode 100644
index 00000000000..6c4bc7031fd
--- /dev/null
+++ b/extern/bullet2/src/LinearMath/btSerializer64.cpp
@@ -0,0 +1,692 @@
+char sBulletDNAstr64[]= {
+char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(-74),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),
+char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(115),char(0),char(42),char(102),char(105),char(114),char(115),char(116),char(0),char(42),char(108),char(97),char(115),
+char(116),char(0),char(109),char(95),char(102),char(108),char(111),char(97),char(116),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(108),char(91),char(51),
+char(93),char(0),char(109),char(95),char(98),char(97),char(115),char(105),char(115),char(0),char(109),char(95),char(111),char(114),char(105),char(103),char(105),char(110),char(0),char(109),
+char(95),char(114),char(111),char(111),char(116),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98),
+char(116),char(114),char(101),char(101),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),
+char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(91),char(51),char(93),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),
+char(101),char(100),char(65),char(97),char(98),char(98),char(77),char(97),char(120),char(91),char(51),char(93),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77),
+char(105),char(110),char(79),char(114),char(103),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77),char(97),char(120),char(79),char(114),char(103),char(0),char(109),
+char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98),char(80),char(97),
+char(114),char(116),char(0),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),
+char(95),char(112),char(97),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101),
+char(120),char(79),char(114),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(98),
+char(118),char(104),char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(0),char(109),char(95),char(98),char(118),char(104),char(65),char(97),char(98),char(98),char(77),
+char(97),char(120),char(0),char(109),char(95),char(98),char(118),char(104),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110),
+char(0),char(109),char(95),char(99),char(117),char(114),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(117),char(115),
+char(101),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(110),char(117),char(109),char(67),
+char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(76),char(101),char(97),char(102),char(78),char(111),char(100),char(101),char(115),char(0),char(109),
+char(95),char(110),char(117),char(109),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117),
+char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(99),char(111),char(110),char(116),char(105),char(103),char(117),char(111),
+char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),
+char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116),
+char(114),char(0),char(42),char(109),char(95),char(115),char(117),char(98),char(84),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(80),char(116),char(114),char(0),
+char(109),char(95),char(116),char(114),char(97),char(118),char(101),char(114),char(115),char(97),char(108),char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(110),char(117),
+char(109),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(72),char(101),char(97),char(100),char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(110),
+char(97),char(109),char(101),char(0),char(109),char(95),char(115),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(112),char(97),
+char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),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(68),char(97),char(116),char(97),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(83),char(99),char(97),
+char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(108),char(97),char(110),char(101),char(78),char(111),char(114),char(109),char(97),char(108),char(0),char(109),
+char(95),char(112),char(108),char(97),char(110),char(101),char(67),char(111),char(110),char(115),char(116),char(97),char(110),char(116),char(0),char(109),char(95),char(105),char(109),char(112),
+char(108),char(105),char(99),char(105),char(116),char(83),char(104),char(97),char(112),char(101),char(68),char(105),char(109),char(101),char(110),char(115),char(105),char(111),char(110),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(77),char(97),char(114),char(103),char(105),char(110),char(0),char(109),
+char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(111),char(115),char(0),char(109),char(95),char(114),char(97),char(100),
+char(105),char(117),char(115),char(0),char(109),char(95),char(99),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(42),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(80),char(111),
+char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),char(108),char(111),char(99),
+char(97),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(83),char(105),char(122),char(101),char(0),
+char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(0),char(109),char(95),char(112),char(97),char(100),char(91),char(50),char(93),char(0),char(109),char(95),char(118),
+char(97),char(108),char(117),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(112),char(97),char(100),char(0),char(42),char(109),char(95),char(118),char(101),
+char(114),char(116),char(105),char(99),char(101),char(115),char(51),char(102),char(0),char(42),char(109),char(95),char(118),char(101),char(114),char(116),char(105),char(99),char(101),char(115),
+char(51),char(100),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(51),char(50),char(0),char(42),char(109),char(95),char(51),
+char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(42),char(109),char(95),char(51),char(105),char(110),char(100),char(105),char(99),char(101),
+char(115),char(56),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(109),char(95),char(110),char(117),
+char(109),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(101),char(114),char(116),
+char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(115),char(80),char(116),char(114),
+char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(101),char(115),char(104),
+char(80),char(97),char(114),char(116),char(115),char(0),char(109),char(95),char(109),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(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(70),char(108),char(111),char(97),char(116),char(66),
+char(118),char(104),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(68),char(111),char(117),char(98),char(108),
+char(101),char(66),char(118),char(104),char(0),char(42),char(109),char(95),char(116),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(0),char(109),char(95),char(112),char(97),char(100),char(51),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(114),char(105),char(109),
+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(109),char(95),char(116),char(114),char(97),char(110),char(115),
+char(102),char(111),char(114),char(109),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(0),char(109),
+char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),
+char(105),char(108),char(100),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),
+char(97),char(112),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(104),char(105),char(108),char(100),char(83),char(104),char(97),
+char(112),char(101),char(115),char(0),char(109),char(95),char(117),char(112),char(65),char(120),char(105),char(115),char(0),char(109),char(95),char(117),char(112),char(73),char(110),char(100),
+char(101),char(120),char(0),char(109),char(95),char(102),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(48),char(86),
+char(49),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(49),char(86),char(50),char(65),char(110),char(103),
+char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(50),char(86),char(48),char(65),char(110),char(103),char(108),char(101),char(0),char(42),
+char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(110),char(101),
+char(120),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(65),char(114),char(114),char(97),char(121),char(80),
+char(116),char(114),char(0),char(42),char(109),char(95),char(107),char(101),char(121),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),
+char(99),char(111),char(110),char(118),char(101),char(120),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(112),char(108),char(97),char(110),
+char(97),char(114),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(101),char(113),char(117),char(97),char(108),char(86),char(101),char(114),
+char(116),char(101),char(120),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(101),char(100),char(103),char(101),char(68),
+char(105),char(115),char(116),char(97),char(110),char(99),char(101),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(122),
+char(101),char(114),char(111),char(65),char(114),char(101),char(97),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(101),char(120),char(116),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(83),
+char(105),char(122),char(101),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(97),char(108),char(117),char(101),char(115),char(0),char(109),char(95),char(110),char(117),
+char(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(76),char(111),
+char(99),char(97),char(108),char(80),char(111),char(105),char(110),char(116),char(65),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),
+char(67),char(97),char(99),char(104),char(101),char(76),char(111),char(99),char(97),char(108),char(80),char(111),char(105),char(110),char(116),char(66),char(91),char(52),char(93),char(0),
+char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),
+char(87),char(111),char(114),char(108),char(100),char(79),char(110),char(65),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),
+char(97),char(99),char(104),char(101),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(79),char(110),char(66),
+char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(78),char(111),char(114),char(109),
+char(97),char(108),char(87),char(111),char(114),char(108),char(100),char(79),char(110),char(66),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),
+char(116),char(67),char(97),char(99),char(104),char(101),char(76),char(97),char(116),char(101),char(114),char(97),char(108),char(70),char(114),char(105),char(99),char(116),char(105),char(111),
+char(110),char(68),char(105),char(114),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),
+char(101),char(76),char(97),char(116),char(101),char(114),char(97),char(108),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(68),char(105),char(114),char(50),
+char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(68),char(105),char(115),char(116),
+char(97),char(110),char(99),char(101),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),
+char(65),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(91),char(52),char(93),char(0),char(109),char(95),
+char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(80),char(114),char(101),char(118),char(82),char(72),char(83),char(91),char(52),char(93),
+char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),
+char(100),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),
+char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),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(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),
+char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(83),char(112),char(105),char(110),char(110),char(105),char(110),char(103),
+char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),
+char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(82),char(101),char(115),char(116),char(105),char(116),char(117),char(116),
+char(105),char(111),char(110),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(80),
+char(97),char(114),char(116),char(73),char(100),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),
+char(104),char(101),char(80),char(97),char(114),char(116),char(73),char(100),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),
+char(67),char(97),char(99),char(104),char(101),char(73),char(110),char(100),char(101),char(120),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),
+char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(73),char(110),char(100),char(101),char(120),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),
+char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(111),char(105),char(110),
+char(116),char(70),char(108),char(97),char(103),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),
+char(104),char(101),char(65),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(76),char(97),char(116),char(101),
+char(114),char(97),char(108),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),
+char(65),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(76),char(97),char(116),char(101),char(114),char(97),
+char(108),char(50),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),
+char(110),char(116),char(97),char(99),char(116),char(77),char(111),char(116),char(105),char(111),char(110),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),
+char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(77),char(111),char(116),char(105),char(111),
+char(110),char(50),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),
+char(110),char(116),char(97),char(99),char(116),char(67),char(70),char(77),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),
+char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),
+char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),
+char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(69),char(82),char(80),char(91),char(52),char(93),char(0),char(109),
+char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(67),
+char(111),char(110),char(116),char(97),char(99),char(116),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),
+char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(67),char(70),
+char(77),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(76),char(105),char(102),
+char(101),char(84),char(105),char(109),char(101),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(97),char(99),char(104),char(101),char(100),
+char(80),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),
+char(65),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(66),char(0),char(109),char(95),char(105),
+char(110),char(100),char(101),char(120),char(49),char(97),char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101),char(0),
+char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(99),char(116),char(66),char(114),char(101),char(97),char(107),char(105),char(110),char(103),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(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(114),char(111),char(99),
+char(101),char(115),char(115),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(42),char(109),char(95),char(98),
+char(111),char(100),char(121),char(48),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(49),char(0),char(109),char(95),char(103),char(105),char(109),char(112),
+char(97),char(99),char(116),char(83),char(117),char(98),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),
+char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),
+char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98),char(108),char(101),
+char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),
+char(110),char(116),char(115),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0),char(42),char(109),
+char(95),char(98),char(114),char(111),char(97),char(100),char(112),char(104),char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0),char(42),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(0),char(42),char(109),char(95),char(114),
+char(111),char(111),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(0),char(109),char(95),
+char(119),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),char(110),char(116),
+char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),
+char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),
+char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105),char(110),char(116),
+char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),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(105),char(115),char(111),char(116),char(114),char(111),char(112),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(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),
+char(111),char(110),char(84),char(105),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),
+char(114),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(0),char(109),char(95),char(99),char(111),
+char(110),char(116),char(97),char(99),char(116),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),
+char(99),char(116),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(114),char(101),char(115),char(116),char(105),char(116),
+char(117),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99),char(116),char(105),char(111),char(110),char(0),
+char(109),char(95),char(99),char(99),char(100),char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114),char(101),char(82),char(97),char(100),char(105),
+char(117),char(115),char(0),char(109),char(95),char(99),char(99),char(100),char(77),char(111),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(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),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(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),
+char(70),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84),char(97),char(103),char(49),char(0),char(109),
+char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95),char(97),char(99),char(116),char(105),char(118),
+char(97),char(116),char(105),char(111),char(110),char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(110),
+char(97),char(108),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67),char(111),char(108),char(108),char(105),char(100),
+char(101),char(87),char(105),char(116),char(104),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(70),char(105),char(108),
+char(116),char(101),char(114),char(71),char(114),char(111),char(117),char(112),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(70),char(105),char(108),char(116),char(101),char(114),char(77),char(97),char(115),char(107),char(0),char(109),char(95),char(117),char(110),char(105),char(113),char(117),char(101),char(73),
+char(100),char(0),char(109),char(95),char(116),char(97),char(117),char(0),char(109),char(95),char(100),char(97),char(109),char(112),char(105),char(110),char(103),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(97),char(114),char(116),char(105),char(99),char(117),char(108),
+char(97),char(116),char(101),char(100),char(87),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(115),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(0),
+char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),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(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),
+char(114),char(116),char(105),char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0),char(109),char(95),char(108),char(105),
+char(110),char(101),char(97),char(114),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(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(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70),char(97),char(99),char(116),char(111),
+char(114),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101),char(108),char(101),char(114),char(97),
+char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(76),char(111),char(99),
+char(97),char(108),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(116),char(111),
+char(116),char(97),char(108),char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101),char(114),char(115),char(101),char(77),
+char(97),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),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(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),
+char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),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(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76),char(105),char(110),char(101),char(97),
+char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),
+char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),
+char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),char(0),
+char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(68),
+char(97),char(109),char(112),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(108),char(105),char(110),char(101),char(97),
+char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),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(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),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(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),
+char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),
+char(116),char(82),char(111),char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65),char(0),char(42),char(109),char(95),
+char(114),char(98),char(66),char(0),char(109),char(95),char(117),char(115),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(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),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(73),char(100),char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101),char(101),char(100),char(98),char(97),char(99),char(107),char(0),
+char(109),char(95),char(97),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(100),
+char(98),char(103),char(68),char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(100),char(105),char(115),char(97),char(98),char(108),char(101),
+char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116),char(119),char(101),char(101),char(110),char(76),char(105),char(110),
+char(107),char(101),char(100),char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111),char(118),char(101),char(114),char(114),char(105),char(100),char(101),
+char(78),char(117),char(109),char(83),char(111),char(108),char(118),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(98),char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112),char(117),char(108),char(115),char(101),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(105),char(115),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(0),char(112),
+char(97),char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(121),char(112),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(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),
+char(65),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),
+char(97),char(109),char(101),char(0),char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),
+char(82),char(101),char(102),char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),
+char(103),char(117),char(108),char(97),char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),
+char(103),char(117),char(108),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),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(109),char(97),char(120),char(77),char(111),
+char(116),char(111),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),
+char(109),char(105),char(116),char(0),char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),
+char(105),char(109),char(105),char(116),char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),
+char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),
+char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),
+char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),
+char(112),char(97),char(110),char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109),char(95),char(108),
+char(105),char(110),char(101),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),
+char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),
+char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),
+char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(117),char(115),char(101),
+char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),
+char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),char(115),char(101),char(116),char(70),char(111),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(114),char(97),char(109),char(101),char(0),char(109),char(95),char(54),char(100),char(111),char(102),char(68),char(97),
+char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(69),char(110),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(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(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(76),char(105),char(109),char(105),char(116),char(101),char(100),
+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(112),char(114),char(105),char(110),char(103),char(68),char(97),
+char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),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(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(76),char(105),char(109),char(105),char(116),char(101),char(100),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(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),
+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(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(109),
+char(95),char(122),char(101),char(114),char(111),char(82),char(111),char(116),char(80),char(97),char(114),char(101),char(110),char(116),char(84),char(111),char(84),char(104),char(105),char(115),
+char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(67),char(111),char(109),char(84),char(111),char(84),char(104),char(105),char(115),char(80),char(105),
+char(118),char(111),char(116),char(79),char(102),char(102),char(115),char(101),char(116),char(0),char(109),char(95),char(116),char(104),char(105),char(115),char(80),char(105),char(118),char(111),
+char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102),char(102),char(115),char(101),char(116),char(0),char(109),char(95),char(106),
+char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(84),char(111),char(112),char(91),char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),
+char(110),char(116),char(65),char(120),char(105),char(115),char(66),char(111),char(116),char(116),char(111),char(109),char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),
+char(110),char(107),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(97),char(98),char(115),char(70),char(114),char(97),char(109),char(101),
+char(84),char(111),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(84),char(111),char(112),char(0),char(109),char(95),char(97),char(98),char(115),
+char(70),char(114),char(97),char(109),char(101),char(84),char(111),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(66),char(111),char(116),char(116),
+char(111),char(109),char(0),char(109),char(95),char(97),char(98),char(115),char(70),char(114),char(97),char(109),char(101),char(76),char(111),char(99),char(86),char(101),char(108),char(111),
+char(99),char(105),char(116),char(121),char(84),char(111),char(112),char(0),char(109),char(95),char(97),char(98),char(115),char(70),char(114),char(97),char(109),char(101),char(76),char(111),
+char(99),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(66),char(111),char(116),char(116),char(111),char(109),char(0),char(109),char(95),char(108),char(105),
+char(110),char(107),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(73),char(110),char(100),char(101),char(120),
+char(0),char(109),char(95),char(100),char(111),char(102),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(112),char(111),char(115),char(86),char(97),char(114),
+char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(80),char(111),char(115),char(91),char(55),char(93),char(0),
+char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(86),char(101),char(108),char(91),char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),
+char(116),char(84),char(111),char(114),char(113),char(117),char(101),char(91),char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(68),char(97),
+char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(70),char(114),char(105),char(99),char(116),char(105),char(111),
+char(110),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),
+char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),
+char(106),char(111),char(105),char(110),char(116),char(77),char(97),char(120),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(106),char(111),char(105),char(110),
+char(116),char(77),char(97),char(120),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),
+char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(78),char(97),char(109),char(101),char(0),char(42),char(109),
+char(95),char(108),char(105),char(110),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(0),char(42),char(109),char(95),char(112),char(97),char(100),
+char(100),char(105),char(110),char(103),char(80),char(116),char(114),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87),char(111),char(114),char(108),char(100),char(80),
+char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87),char(111),char(114),char(108),char(100),char(79),
+char(114),char(105),char(101),char(110),char(116),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(76),char(105),char(110),
+char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(65),char(110),
+char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(98),char(97),char(115),char(101),
+char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(42),
+char(109),char(95),char(98),char(97),char(115),char(101),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(67),char(111),
+char(108),char(108),char(105),char(100),char(101),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(79),char(98),char(106),char(68),char(97),char(116),char(97),char(0),
+char(42),char(109),char(95),char(109),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(0),
+char(84),char(89),char(80),char(69),char(99),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(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),char(111),char(110),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(116),char(101),char(114),char(110),char(105),char(111),char(110),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(80),char(101),char(114),char(115),char(105),char(115),char(116),char(101),char(110),char(116),char(77),char(97),char(110),char(105),char(102),char(111),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(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(80),char(101),char(114),char(115),char(105),char(115),char(116),char(101),char(110),char(116),char(77),char(97),char(110),char(105),char(102),char(111),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(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(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(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(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(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(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(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(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(98),char(116),char(77),char(117),char(108),char(116),
+char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),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(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),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(117),char(108),char(116),char(105),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(77),char(117),char(108),char(116),char(105),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(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),
+char(105),char(110),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),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(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(67),char(111),char(108),char(108),
+char(105),char(100),char(101),char(114),char(68),char(111),char(117),char(98),char(108),char(101),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(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(24),char(6),char(-8),char(1),char(80),char(3),
+char(32),char(1),char(72),char(0),char(80),char(0),char(-96),char(0),char(88),char(0),char(-64),char(0),char(104),char(0),char(8),char(2),char(-56),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(-112),char(2),char(-64),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(-16),char(1),char(-128),char(3),char(-40),char(1),char(-56),char(0),char(112),char(0),char(48),char(1),char(8),char(2),char(0),char(0),
+char(83),char(84),char(82),char(67),char(88),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(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),
+char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0),char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),
+char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0),char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),
+char(14),char(0),char(11),char(0),char(21),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(22),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(23),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(24),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(25),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(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),
+char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(26),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(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),
+char(27),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(28),char(0),char(5),char(0),
+char(27),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(29),char(0),char(5),char(0),char(27),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(30),char(0),char(2),char(0),char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),
+char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0),char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),
+char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0),char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),
+char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),
+char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0),char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),
+char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0),char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),
+char(36),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(38),char(0),char(7),char(0),
+char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),
+char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),
+char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0),char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),
+char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0),char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),
+char(43),char(0),char(3),char(0),char(29),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(3),char(0),
+char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0),char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),
+char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(46),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(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),
+char(46),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(47),char(0),char(39),char(0),char(14),char(0),char(96),char(0),char(14),char(0),char(97),char(0),
+char(14),char(0),char(98),char(0),char(14),char(0),char(99),char(0),char(14),char(0),char(100),char(0),char(14),char(0),char(101),char(0),char(14),char(0),char(102),char(0),
+char(8),char(0),char(103),char(0),char(8),char(0),char(104),char(0),char(8),char(0),char(105),char(0),char(8),char(0),char(106),char(0),char(8),char(0),char(107),char(0),
+char(8),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(4),char(0),char(110),char(0),char(4),char(0),char(111),char(0),char(4),char(0),char(112),char(0),
+char(4),char(0),char(113),char(0),char(4),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0),char(8),char(0),char(117),char(0),
+char(8),char(0),char(118),char(0),char(8),char(0),char(119),char(0),char(8),char(0),char(120),char(0),char(8),char(0),char(121),char(0),char(8),char(0),char(122),char(0),
+char(8),char(0),char(123),char(0),char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0),char(4),char(0),char(126),char(0),char(4),char(0),char(127),char(0),
+char(4),char(0),char(-128),char(0),char(4),char(0),char(-127),char(0),char(8),char(0),char(-126),char(0),char(8),char(0),char(-125),char(0),char(4),char(0),char(44),char(0),
+char(48),char(0),char(-124),char(0),char(48),char(0),char(-123),char(0),char(49),char(0),char(39),char(0),char(13),char(0),char(96),char(0),char(13),char(0),char(97),char(0),
+char(13),char(0),char(98),char(0),char(13),char(0),char(99),char(0),char(13),char(0),char(100),char(0),char(13),char(0),char(101),char(0),char(13),char(0),char(102),char(0),
+char(7),char(0),char(103),char(0),char(7),char(0),char(104),char(0),char(7),char(0),char(105),char(0),char(7),char(0),char(106),char(0),char(7),char(0),char(107),char(0),
+char(7),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(4),char(0),char(110),char(0),char(4),char(0),char(111),char(0),char(4),char(0),char(112),char(0),
+char(4),char(0),char(113),char(0),char(4),char(0),char(114),char(0),char(7),char(0),char(115),char(0),char(7),char(0),char(116),char(0),char(7),char(0),char(117),char(0),
+char(7),char(0),char(118),char(0),char(7),char(0),char(119),char(0),char(7),char(0),char(120),char(0),char(7),char(0),char(121),char(0),char(7),char(0),char(122),char(0),
+char(7),char(0),char(123),char(0),char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0),char(4),char(0),char(126),char(0),char(4),char(0),char(127),char(0),
+char(4),char(0),char(-128),char(0),char(4),char(0),char(-127),char(0),char(7),char(0),char(-126),char(0),char(7),char(0),char(-125),char(0),char(4),char(0),char(44),char(0),
+char(50),char(0),char(-124),char(0),char(50),char(0),char(-123),char(0),char(51),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),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(-122),char(0),char(52),char(0),char(5),char(0),char(29),char(0),char(47),char(0),
+char(13),char(0),char(-121),char(0),char(14),char(0),char(-120),char(0),char(4),char(0),char(-119),char(0),char(0),char(0),char(-118),char(0),char(48),char(0),char(29),char(0),
+char(9),char(0),char(-117),char(0),char(9),char(0),char(-116),char(0),char(27),char(0),char(-115),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(-114),char(0),
+char(20),char(0),char(-113),char(0),char(14),char(0),char(-112),char(0),char(14),char(0),char(-111),char(0),char(14),char(0),char(-110),char(0),char(8),char(0),char(-125),char(0),
+char(8),char(0),char(-109),char(0),char(8),char(0),char(-108),char(0),char(8),char(0),char(-107),char(0),char(8),char(0),char(-106),char(0),char(8),char(0),char(-105),char(0),
+char(8),char(0),char(-104),char(0),char(8),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(4),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(4),char(0),char(-96),char(0),char(4),char(0),char(-95),char(0),
+char(4),char(0),char(-94),char(0),char(4),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(4),char(0),char(-91),char(0),char(50),char(0),char(29),char(0),
+char(9),char(0),char(-117),char(0),char(9),char(0),char(-116),char(0),char(27),char(0),char(-115),char(0),char(0),char(0),char(35),char(0),char(19),char(0),char(-114),char(0),
+char(19),char(0),char(-113),char(0),char(13),char(0),char(-112),char(0),char(13),char(0),char(-111),char(0),char(13),char(0),char(-110),char(0),char(7),char(0),char(-125),char(0),
+char(7),char(0),char(-109),char(0),char(7),char(0),char(-108),char(0),char(7),char(0),char(-107),char(0),char(7),char(0),char(-106),char(0),char(7),char(0),char(-105),char(0),
+char(7),char(0),char(-104),char(0),char(7),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(4),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(4),char(0),char(-96),char(0),char(4),char(0),char(-95),char(0),
+char(4),char(0),char(-94),char(0),char(4),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(4),char(0),char(-91),char(0),char(53),char(0),char(23),char(0),
+char(8),char(0),char(-90),char(0),char(8),char(0),char(-89),char(0),char(8),char(0),char(-108),char(0),char(8),char(0),char(-88),char(0),char(8),char(0),char(-104),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(8),char(0),char(-80),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(-76),char(0),char(4),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0),char(4),char(0),char(-73),char(0),
+char(4),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(22),char(0),char(7),char(0),char(-90),char(0),
+char(7),char(0),char(-89),char(0),char(7),char(0),char(-108),char(0),char(7),char(0),char(-88),char(0),char(7),char(0),char(-104),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(7),char(0),char(-80),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(-76),char(0),char(4),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0),char(4),char(0),char(-73),char(0),char(4),char(0),char(-72),char(0),
+char(4),char(0),char(-71),char(0),char(55),char(0),char(2),char(0),char(53),char(0),char(-70),char(0),char(14),char(0),char(-69),char(0),char(56),char(0),char(2),char(0),
+char(54),char(0),char(-70),char(0),char(13),char(0),char(-69),char(0),char(57),char(0),char(21),char(0),char(50),char(0),char(-68),char(0),char(17),char(0),char(-67),char(0),
+char(13),char(0),char(-66),char(0),char(13),char(0),char(-65),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0),char(13),char(0),char(-69),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(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(4),char(0),char(-49),char(0),char(58),char(0),char(22),char(0),
+char(48),char(0),char(-68),char(0),char(18),char(0),char(-67),char(0),char(14),char(0),char(-66),char(0),char(14),char(0),char(-65),char(0),char(14),char(0),char(-64),char(0),
+char(14),char(0),char(-63),char(0),char(14),char(0),char(-69),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(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(4),char(0),char(-49),char(0),char(0),char(0),char(37),char(0),char(59),char(0),char(2),char(0),char(4),char(0),char(-48),char(0),char(4),char(0),char(-47),char(0),
+char(60),char(0),char(13),char(0),char(57),char(0),char(-46),char(0),char(57),char(0),char(-45),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-127),char(0),
+char(4),char(0),char(-44),char(0),char(4),char(0),char(-43),char(0),char(4),char(0),char(-42),char(0),char(7),char(0),char(-41),char(0),char(7),char(0),char(-40),char(0),
+char(4),char(0),char(-39),char(0),char(4),char(0),char(-38),char(0),char(7),char(0),char(-37),char(0),char(4),char(0),char(-36),char(0),char(61),char(0),char(13),char(0),
+char(62),char(0),char(-46),char(0),char(62),char(0),char(-45),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-127),char(0),char(4),char(0),char(-44),char(0),
+char(4),char(0),char(-43),char(0),char(4),char(0),char(-42),char(0),char(7),char(0),char(-41),char(0),char(7),char(0),char(-40),char(0),char(4),char(0),char(-39),char(0),
+char(4),char(0),char(-38),char(0),char(7),char(0),char(-37),char(0),char(4),char(0),char(-36),char(0),char(63),char(0),char(14),char(0),char(58),char(0),char(-46),char(0),
+char(58),char(0),char(-45),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-127),char(0),char(4),char(0),char(-44),char(0),char(4),char(0),char(-43),char(0),
+char(4),char(0),char(-42),char(0),char(8),char(0),char(-41),char(0),char(8),char(0),char(-40),char(0),char(4),char(0),char(-39),char(0),char(4),char(0),char(-38),char(0),
+char(8),char(0),char(-37),char(0),char(4),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),char(64),char(0),char(3),char(0),char(61),char(0),char(-34),char(0),
+char(13),char(0),char(-33),char(0),char(13),char(0),char(-32),char(0),char(65),char(0),char(3),char(0),char(63),char(0),char(-34),char(0),char(14),char(0),char(-33),char(0),
+char(14),char(0),char(-32),char(0),char(66),char(0),char(3),char(0),char(61),char(0),char(-34),char(0),char(14),char(0),char(-33),char(0),char(14),char(0),char(-32),char(0),
+char(67),char(0),char(13),char(0),char(61),char(0),char(-34),char(0),char(20),char(0),char(-31),char(0),char(20),char(0),char(-30),char(0),char(4),char(0),char(-29),char(0),
+char(4),char(0),char(-28),char(0),char(4),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(68),char(0),char(13),char(0),
+char(61),char(0),char(-34),char(0),char(19),char(0),char(-31),char(0),char(19),char(0),char(-30),char(0),char(4),char(0),char(-29),char(0),char(4),char(0),char(-28),char(0),
+char(4),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(69),char(0),char(14),char(0),char(63),char(0),char(-34),char(0),
+char(20),char(0),char(-31),char(0),char(20),char(0),char(-30),char(0),char(4),char(0),char(-29),char(0),char(4),char(0),char(-28),char(0),char(4),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(0),char(0),char(-19),char(0),char(70),char(0),char(10),char(0),char(63),char(0),char(-34),char(0),
+char(20),char(0),char(-31),char(0),char(20),char(0),char(-30),char(0),char(8),char(0),char(-18),char(0),char(8),char(0),char(-17),char(0),char(8),char(0),char(-16),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(-89),char(0),char(71),char(0),char(11),char(0),
+char(61),char(0),char(-34),char(0),char(19),char(0),char(-31),char(0),char(19),char(0),char(-30),char(0),char(7),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0),
+char(7),char(0),char(-16),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(-89),char(0),
+char(0),char(0),char(21),char(0),char(72),char(0),char(9),char(0),char(61),char(0),char(-34),char(0),char(19),char(0),char(-31),char(0),char(19),char(0),char(-30),char(0),
+char(13),char(0),char(-15),char(0),char(13),char(0),char(-14),char(0),char(13),char(0),char(-13),char(0),char(13),char(0),char(-12),char(0),char(4),char(0),char(-11),char(0),
+char(4),char(0),char(-10),char(0),char(73),char(0),char(9),char(0),char(63),char(0),char(-34),char(0),char(20),char(0),char(-31),char(0),char(20),char(0),char(-30),char(0),
+char(14),char(0),char(-15),char(0),char(14),char(0),char(-14),char(0),char(14),char(0),char(-13),char(0),char(14),char(0),char(-12),char(0),char(4),char(0),char(-11),char(0),
+char(4),char(0),char(-10),char(0),char(74),char(0),char(5),char(0),char(72),char(0),char(-9),char(0),char(4),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(75),char(0),char(5),char(0),char(73),char(0),char(-9),char(0),char(4),char(0),char(-8),char(0),
+char(8),char(0),char(-7),char(0),char(8),char(0),char(-6),char(0),char(8),char(0),char(-5),char(0),char(76),char(0),char(41),char(0),char(61),char(0),char(-34),char(0),
+char(19),char(0),char(-31),char(0),char(19),char(0),char(-30),char(0),char(13),char(0),char(-15),char(0),char(13),char(0),char(-14),char(0),char(13),char(0),char(-4),char(0),
+char(13),char(0),char(-3),char(0),char(13),char(0),char(-2),char(0),char(13),char(0),char(-1),char(0),char(13),char(0),char(0),char(1),char(13),char(0),char(1),char(1),
+char(13),char(0),char(2),char(1),char(13),char(0),char(3),char(1),char(13),char(0),char(4),char(1),char(13),char(0),char(5),char(1),char(13),char(0),char(6),char(1),
+char(0),char(0),char(7),char(1),char(0),char(0),char(8),char(1),char(0),char(0),char(9),char(1),char(0),char(0),char(10),char(1),char(0),char(0),char(11),char(1),
+char(0),char(0),char(-19),char(0),char(13),char(0),char(-13),char(0),char(13),char(0),char(-12),char(0),char(13),char(0),char(12),char(1),char(13),char(0),char(13),char(1),
+char(13),char(0),char(14),char(1),char(13),char(0),char(15),char(1),char(13),char(0),char(16),char(1),char(13),char(0),char(17),char(1),char(13),char(0),char(18),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(0),char(0),char(23),char(1),
+char(0),char(0),char(24),char(1),char(0),char(0),char(25),char(1),char(0),char(0),char(26),char(1),char(0),char(0),char(27),char(1),char(4),char(0),char(28),char(1),
+char(77),char(0),char(41),char(0),char(63),char(0),char(-34),char(0),char(20),char(0),char(-31),char(0),char(20),char(0),char(-30),char(0),char(14),char(0),char(-15),char(0),
+char(14),char(0),char(-14),char(0),char(14),char(0),char(-4),char(0),char(14),char(0),char(-3),char(0),char(14),char(0),char(-2),char(0),char(14),char(0),char(-1),char(0),
+char(14),char(0),char(0),char(1),char(14),char(0),char(1),char(1),char(14),char(0),char(2),char(1),char(14),char(0),char(3),char(1),char(14),char(0),char(4),char(1),
+char(14),char(0),char(5),char(1),char(14),char(0),char(6),char(1),char(0),char(0),char(7),char(1),char(0),char(0),char(8),char(1),char(0),char(0),char(9),char(1),
+char(0),char(0),char(10),char(1),char(0),char(0),char(11),char(1),char(0),char(0),char(-19),char(0),char(14),char(0),char(-13),char(0),char(14),char(0),char(-12),char(0),
+char(14),char(0),char(12),char(1),char(14),char(0),char(13),char(1),char(14),char(0),char(14),char(1),char(14),char(0),char(15),char(1),char(14),char(0),char(16),char(1),
+char(14),char(0),char(17),char(1),char(14),char(0),char(18),char(1),char(14),char(0),char(19),char(1),char(14),char(0),char(20),char(1),char(14),char(0),char(21),char(1),
+char(14),char(0),char(22),char(1),char(0),char(0),char(23),char(1),char(0),char(0),char(24),char(1),char(0),char(0),char(25),char(1),char(0),char(0),char(26),char(1),
+char(0),char(0),char(27),char(1),char(4),char(0),char(28),char(1),char(78),char(0),char(9),char(0),char(61),char(0),char(-34),char(0),char(19),char(0),char(-31),char(0),
+char(19),char(0),char(-30),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(4),char(0),char(-11),char(0),char(4),char(0),char(-10),char(0),char(79),char(0),char(9),char(0),char(63),char(0),char(-34),char(0),char(20),char(0),char(-31),char(0),
+char(20),char(0),char(-30),char(0),char(8),char(0),char(-15),char(0),char(8),char(0),char(-14),char(0),char(8),char(0),char(-13),char(0),char(8),char(0),char(-12),char(0),
+char(4),char(0),char(-11),char(0),char(4),char(0),char(-10),char(0),char(80),char(0),char(5),char(0),char(60),char(0),char(-34),char(0),char(13),char(0),char(29),char(1),
+char(13),char(0),char(30),char(1),char(7),char(0),char(31),char(1),char(0),char(0),char(37),char(0),char(81),char(0),char(4),char(0),char(63),char(0),char(-34),char(0),
+char(14),char(0),char(29),char(1),char(14),char(0),char(30),char(1),char(8),char(0),char(31),char(1),char(82),char(0),char(4),char(0),char(7),char(0),char(32),char(1),
+char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1),char(4),char(0),char(79),char(0),char(83),char(0),char(10),char(0),char(82),char(0),char(35),char(1),
+char(13),char(0),char(36),char(1),char(13),char(0),char(37),char(1),char(13),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(-58),char(0),char(7),char(0),char(41),char(1),char(4),char(0),char(42),char(1),char(4),char(0),char(53),char(0),char(84),char(0),char(4),char(0),
+char(82),char(0),char(35),char(1),char(4),char(0),char(43),char(1),char(7),char(0),char(44),char(1),char(4),char(0),char(45),char(1),char(85),char(0),char(4),char(0),
+char(13),char(0),char(40),char(1),char(82),char(0),char(35),char(1),char(4),char(0),char(46),char(1),char(7),char(0),char(47),char(1),char(86),char(0),char(7),char(0),
+char(13),char(0),char(48),char(1),char(82),char(0),char(35),char(1),char(4),char(0),char(49),char(1),char(7),char(0),char(50),char(1),char(7),char(0),char(51),char(1),
+char(7),char(0),char(52),char(1),char(4),char(0),char(53),char(0),char(87),char(0),char(6),char(0),char(17),char(0),char(53),char(1),char(13),char(0),char(51),char(1),
+char(13),char(0),char(54),char(1),char(62),char(0),char(55),char(1),char(4),char(0),char(56),char(1),char(7),char(0),char(52),char(1),char(88),char(0),char(26),char(0),
+char(4),char(0),char(57),char(1),char(7),char(0),char(58),char(1),char(7),char(0),char(-89),char(0),char(7),char(0),char(59),char(1),char(7),char(0),char(60),char(1),
+char(7),char(0),char(61),char(1),char(7),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(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),
+char(7),char(0),char(71),char(1),char(7),char(0),char(72),char(1),char(7),char(0),char(73),char(1),char(7),char(0),char(74),char(1),char(7),char(0),char(75),char(1),
+char(7),char(0),char(76),char(1),char(4),char(0),char(77),char(1),char(4),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(-99),char(0),char(89),char(0),char(12),char(0),char(17),char(0),char(81),char(1),char(17),char(0),char(82),char(1),char(17),char(0),char(83),char(1),
+char(13),char(0),char(84),char(1),char(13),char(0),char(85),char(1),char(7),char(0),char(86),char(1),char(4),char(0),char(87),char(1),char(4),char(0),char(88),char(1),
+char(4),char(0),char(89),char(1),char(4),char(0),char(90),char(1),char(7),char(0),char(50),char(1),char(4),char(0),char(53),char(0),char(90),char(0),char(27),char(0),
+char(19),char(0),char(91),char(1),char(17),char(0),char(92),char(1),char(17),char(0),char(93),char(1),char(13),char(0),char(84),char(1),char(13),char(0),char(94),char(1),
+char(13),char(0),char(95),char(1),char(13),char(0),char(96),char(1),char(13),char(0),char(97),char(1),char(13),char(0),char(98),char(1),char(4),char(0),char(99),char(1),
+char(7),char(0),char(100),char(1),char(4),char(0),char(101),char(1),char(4),char(0),char(102),char(1),char(4),char(0),char(103),char(1),char(7),char(0),char(104),char(1),
+char(7),char(0),char(105),char(1),char(4),char(0),char(106),char(1),char(4),char(0),char(107),char(1),char(7),char(0),char(108),char(1),char(7),char(0),char(109),char(1),
+char(7),char(0),char(110),char(1),char(7),char(0),char(111),char(1),char(7),char(0),char(112),char(1),char(7),char(0),char(113),char(1),char(4),char(0),char(114),char(1),
+char(4),char(0),char(115),char(1),char(4),char(0),char(116),char(1),char(91),char(0),char(12),char(0),char(9),char(0),char(117),char(1),char(9),char(0),char(118),char(1),
+char(13),char(0),char(119),char(1),char(7),char(0),char(120),char(1),char(7),char(0),char(-85),char(0),char(7),char(0),char(121),char(1),char(4),char(0),char(122),char(1),
+char(13),char(0),char(123),char(1),char(4),char(0),char(124),char(1),char(4),char(0),char(125),char(1),char(4),char(0),char(126),char(1),char(4),char(0),char(53),char(0),
+char(92),char(0),char(19),char(0),char(50),char(0),char(-68),char(0),char(89),char(0),char(127),char(1),char(82),char(0),char(-128),char(1),char(83),char(0),char(-127),char(1),
+char(84),char(0),char(-126),char(1),char(85),char(0),char(-125),char(1),char(86),char(0),char(-124),char(1),char(87),char(0),char(-123),char(1),char(90),char(0),char(-122),char(1),
+char(91),char(0),char(-121),char(1),char(4),char(0),char(-120),char(1),char(4),char(0),char(102),char(1),char(4),char(0),char(-119),char(1),char(4),char(0),char(-118),char(1),
+char(4),char(0),char(-117),char(1),char(4),char(0),char(-116),char(1),char(4),char(0),char(-115),char(1),char(4),char(0),char(-114),char(1),char(88),char(0),char(-113),char(1),
+char(93),char(0),char(28),char(0),char(16),char(0),char(-112),char(1),char(14),char(0),char(-111),char(1),char(14),char(0),char(-110),char(1),char(14),char(0),char(-109),char(1),
+char(14),char(0),char(-108),char(1),char(14),char(0),char(-107),char(1),char(14),char(0),char(-106),char(1),char(14),char(0),char(-105),char(1),char(14),char(0),char(-104),char(1),
+char(14),char(0),char(-103),char(1),char(8),char(0),char(-102),char(1),char(4),char(0),char(-101),char(1),char(4),char(0),char(126),char(1),char(4),char(0),char(-100),char(1),
+char(4),char(0),char(-99),char(1),char(8),char(0),char(-98),char(1),char(8),char(0),char(-97),char(1),char(8),char(0),char(-96),char(1),char(8),char(0),char(-95),char(1),
+char(8),char(0),char(-94),char(1),char(8),char(0),char(-93),char(1),char(8),char(0),char(-92),char(1),char(8),char(0),char(-91),char(1),char(8),char(0),char(-90),char(1),
+char(0),char(0),char(-89),char(1),char(0),char(0),char(-88),char(1),char(48),char(0),char(-87),char(1),char(0),char(0),char(-86),char(1),char(94),char(0),char(28),char(0),
+char(15),char(0),char(-112),char(1),char(13),char(0),char(-111),char(1),char(13),char(0),char(-110),char(1),char(13),char(0),char(-109),char(1),char(13),char(0),char(-108),char(1),
+char(13),char(0),char(-107),char(1),char(13),char(0),char(-106),char(1),char(13),char(0),char(-105),char(1),char(13),char(0),char(-104),char(1),char(13),char(0),char(-103),char(1),
+char(4),char(0),char(-100),char(1),char(7),char(0),char(-102),char(1),char(4),char(0),char(-101),char(1),char(4),char(0),char(126),char(1),char(7),char(0),char(-98),char(1),
+char(7),char(0),char(-97),char(1),char(7),char(0),char(-96),char(1),char(4),char(0),char(-99),char(1),char(7),char(0),char(-95),char(1),char(7),char(0),char(-94),char(1),
+char(7),char(0),char(-93),char(1),char(7),char(0),char(-92),char(1),char(7),char(0),char(-91),char(1),char(7),char(0),char(-90),char(1),char(0),char(0),char(-89),char(1),
+char(0),char(0),char(-88),char(1),char(50),char(0),char(-87),char(1),char(0),char(0),char(-86),char(1),char(95),char(0),char(11),char(0),char(14),char(0),char(-85),char(1),
+char(16),char(0),char(-84),char(1),char(14),char(0),char(-83),char(1),char(14),char(0),char(-82),char(1),char(14),char(0),char(-81),char(1),char(8),char(0),char(-80),char(1),
+char(4),char(0),char(-119),char(1),char(0),char(0),char(37),char(0),char(0),char(0),char(-79),char(1),char(93),char(0),char(-126),char(1),char(48),char(0),char(-78),char(1),
+char(96),char(0),char(10),char(0),char(13),char(0),char(-85),char(1),char(15),char(0),char(-84),char(1),char(13),char(0),char(-83),char(1),char(13),char(0),char(-82),char(1),
+char(13),char(0),char(-81),char(1),char(7),char(0),char(-80),char(1),char(4),char(0),char(-119),char(1),char(0),char(0),char(-79),char(1),char(94),char(0),char(-126),char(1),
+char(50),char(0),char(-78),char(1),char(97),char(0),char(4),char(0),char(50),char(0),char(-77),char(1),char(96),char(0),char(-76),char(1),char(4),char(0),char(-75),char(1),
+char(0),char(0),char(37),char(0),char(98),char(0),char(4),char(0),char(48),char(0),char(-77),char(1),char(95),char(0),char(-76),char(1),char(4),char(0),char(-75),char(1),
+char(0),char(0),char(37),char(0),};
+int sBulletDNAlen64= sizeof(sBulletDNAstr64);
diff --git a/extern/bullet2/src/LinearMath/btSpatialAlgebra.h b/extern/bullet2/src/LinearMath/btSpatialAlgebra.h
index 8e59658bca7..6ad67a10812 100644
--- a/extern/bullet2/src/LinearMath/btSpatialAlgebra.h
+++ b/extern/bullet2/src/LinearMath/btSpatialAlgebra.h
@@ -12,18 +12,17 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-///These spatial algebra classes are used for btMultiBody, 
+///These spatial algebra classes are used for btMultiBody,
 ///see BulletDynamics/Featherstone
 
 #ifndef BT_SPATIAL_ALGEBRA_H
 #define BT_SPATIAL_ALGEBRA_H
 
-
 #include "btMatrix3x3.h"
 
 struct btSpatialForceVector
-{	
-	btVector3 m_topVec, m_bottomVec;	
+{
+	btVector3 m_topVec, m_bottomVec;
 	//
 	btSpatialForceVector() { setZero(); }
 	btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {}
@@ -32,21 +31,34 @@ struct btSpatialForceVector
 		setValue(ax, ay, az, lx, ly, lz);
 	}
 	//
-	void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; }
+	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);
+		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 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;			
+		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; }
+	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; }
@@ -54,14 +66,28 @@ struct btSpatialForceVector
 	void addAngular(const btVector3 &angular) { m_bottomVec += angular; }
 	void addLinear(const btVector3 &linear) { m_topVec += linear; }
 	//
-	void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
+	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;
+	}
+	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; }
 };
 
@@ -70,23 +96,36 @@ struct btSpatialMotionVector
 	btVector3 m_topVec, m_bottomVec;
 	//
 	btSpatialMotionVector() { setZero(); }
-	btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {}		
+	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 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);
+		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 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;			
+		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; }
+	//
+	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; }
@@ -94,20 +133,24 @@ struct btSpatialMotionVector
 	void addAngular(const btVector3 &angular) { m_topVec += angular; }
 	void addLinear(const btVector3 &linear) { m_bottomVec += linear; }
 	//
-	void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
+	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>
+	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>
+	template <typename SpatialVectorType>
 	SpatialVectorType cross(const SpatialVectorType &b) const
 	{
 		SpatialVectorType out;
@@ -116,21 +159,36 @@ struct btSpatialMotionVector
 		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); }
+	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); }			
+	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)
 	{
@@ -146,17 +204,22 @@ struct btSymmetricSpatialDyad
 		m_bottomLeftMat += bottomLeftMat;
 	}
 	//
-	void setIdentity() { m_topLeftMat.setIdentity(); m_topRightMat.setIdentity(); m_bottomLeftMat.setIdentity();  }
+	void setIdentity()
+	{
+		m_topLeftMat.setIdentity();
+		m_topRightMat.setIdentity();
+		m_bottomLeftMat.setIdentity();
+	}
 	//
-	btSymmetricSpatialDyad & operator -= (const btSymmetricSpatialDyad &mat)
+	btSymmetricSpatialDyad &operator-=(const btSymmetricSpatialDyad &mat)
 	{
 		m_topLeftMat -= mat.m_topLeftMat;
 		m_topRightMat -= mat.m_topRightMat;
 		m_bottomLeftMat -= mat.m_bottomLeftMat;
-		return *this; 
+		return *this;
 	}
 	//
-	btSpatialForceVector operator * (const btSpatialMotionVector &vec)
+	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);
 	}
@@ -164,7 +227,7 @@ struct btSymmetricSpatialDyad
 
 struct btSpatialTransformationMatrix
 {
-	btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat;
+	btMatrix3x3 m_rotMat;  //btMatrix3x3 m_trnCrossMat;
 	btVector3 m_trnVec;
 	//
 	enum eOutputOperation
@@ -174,128 +237,124 @@ struct btSpatialTransformationMatrix
 		Subtract = 2
 	};
 	//
-	template<typename SpatialVectorType>
-	void transform(	const SpatialVectorType &inVec,
-                      SpatialVectorType &outVec,
-					eOutputOperation outOp = None)
+	template <typename SpatialVectorType>
+	void transform(const SpatialVectorType &inVec,
+				   SpatialVectorType &outVec,
+				   eOutputOperation outOp = None)
 	{
-		if(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)
+		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)
+		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)
+	template <typename SpatialVectorType>
+	void transformRotationOnly(const SpatialVectorType &inVec,
+							   SpatialVectorType &outVec,
+							   eOutputOperation outOp = None)
 	{
-		if(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)
+		else if (outOp == Add)
 		{
 			outVec.m_topVec += m_rotMat * inVec.m_topVec;
 			outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec;
 		}
-		else if(outOp == Subtract)
+		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)
+	template <typename SpatialVectorType>
+	void transformInverse(const SpatialVectorType &inVec,
+						  SpatialVectorType &outVec,
+						  eOutputOperation outOp = None)
 	{
-		if(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)
+		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)
+		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)
+	template <typename SpatialVectorType>
+	void transformInverseRotationOnly(const SpatialVectorType &inVec,
+									  SpatialVectorType &outVec,
+									  eOutputOperation outOp = None)
 	{
-		if(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)
+		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)
+		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)
+	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);
+		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)
+		if (outOp == None)
 		{
-			outMat.m_topLeftMat = m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
+			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)
+		else if (outOp == Add)
 		{
-			outMat.m_topLeftMat += m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
+			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)
+		else if (outOp == Subtract)
 		{
-			outMat.m_topLeftMat -= m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
+			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)
+	template <typename SpatialVectorType>
+	SpatialVectorType operator*(const SpatialVectorType &vec)
 	{
 		SpatialVectorType out;
 		transform(vec, out);
@@ -303,7 +362,7 @@ struct btSpatialTransformationMatrix
 	}
 };
 
-template<typename SpatialVectorType>
+template <typename SpatialVectorType>
 void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
 {
 	//output op maybe?
@@ -314,7 +373,7 @@ void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVecto
 	//maybe simple a*spatTranspose(a) would be nicer?
 }
 
-template<typename SpatialVectorType>
+template <typename SpatialVectorType>
 btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b)
 {
 	btSymmetricSpatialDyad out;
@@ -327,5 +386,4 @@ btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a,
 	//maybe simple a*spatTranspose(a) would be nicer?
 }
 
-#endif //BT_SPATIAL_ALGEBRA_H
-
+#endif  //BT_SPATIAL_ALGEBRA_H
diff --git a/extern/bullet2/src/LinearMath/btStackAlloc.h b/extern/bullet2/src/LinearMath/btStackAlloc.h
index 397b084877f..3fc20849765 100644
--- a/extern/bullet2/src/LinearMath/btStackAlloc.h
+++ b/extern/bullet2/src/LinearMath/btStackAlloc.h
@@ -20,97 +20,99 @@ Nov.2006
 #ifndef BT_STACK_ALLOC
 #define BT_STACK_ALLOC
 
-#include "btScalar.h" //for btAssert
+#include "btScalar.h"  //for btAssert
 #include "btAlignedAllocator.h"
 
 ///The btBlock class is an internal structure for the btStackAlloc memory allocator.
 struct btBlock
 {
-	btBlock*			previous;
-	unsigned char*		address;
+	btBlock* previous;
+	unsigned char* address;
 };
 
 ///The StackAlloc class provides some fast stack-based memory allocator (LIFO last-in first-out)
 class btStackAlloc
 {
 public:
+	btStackAlloc(unsigned int size)
+	{
+		ctor();
+		create(size);
+	}
+	~btStackAlloc() { destroy(); }
 
-	btStackAlloc(unsigned int size)	{ ctor();create(size); }
-	~btStackAlloc()		{ destroy(); }
-	
-	inline void		create(unsigned int size)
+	inline void create(unsigned int size)
 	{
 		destroy();
-		data		=  (unsigned char*) btAlignedAlloc(size,16);
-		totalsize	=	size;
+		data = (unsigned char*)btAlignedAlloc(size, 16);
+		totalsize = size;
 	}
-	inline void		destroy()
+	inline void destroy()
 	{
-		btAssert(usedsize==0);
+		btAssert(usedsize == 0);
 		//Raise(L"StackAlloc is still in use");
 
-		if(usedsize==0)
+		if (usedsize == 0)
 		{
-			if(!ischild && data)		
+			if (!ischild && data)
 				btAlignedFree(data);
 
-			data				=	0;
-			usedsize			=	0;
+			data = 0;
+			usedsize = 0;
 		}
-		
 	}
 
-	int	getAvailableMemory() const
+	int getAvailableMemory() const
 	{
 		return static_cast<int>(totalsize - usedsize);
 	}
 
-	unsigned char*			allocate(unsigned int size)
+	unsigned char* allocate(unsigned int size)
 	{
-		const unsigned int	nus(usedsize+size);
-		if(nus<totalsize)
+		const unsigned int nus(usedsize + size);
+		if (nus < totalsize)
 		{
-			usedsize=nus;
-			return(data+(usedsize-size));
+			usedsize = nus;
+			return (data + (usedsize - size));
 		}
 		btAssert(0);
 		//&& (L"Not enough memory"));
-		
-		return(0);
+
+		return (0);
 	}
-	SIMD_FORCE_INLINE btBlock*		beginBlock()
+	SIMD_FORCE_INLINE btBlock* beginBlock()
 	{
-		btBlock*	pb = (btBlock*)allocate(sizeof(btBlock));
-		pb->previous	=	current;
-		pb->address		=	data+usedsize;
-		current			=	pb;
-		return(pb);
+		btBlock* pb = (btBlock*)allocate(sizeof(btBlock));
+		pb->previous = current;
+		pb->address = data + usedsize;
+		current = pb;
+		return (pb);
 	}
-	SIMD_FORCE_INLINE void		endBlock(btBlock* block)
+	SIMD_FORCE_INLINE void endBlock(btBlock* block)
 	{
-		btAssert(block==current);
+		btAssert(block == current);
 		//Raise(L"Unmatched blocks");
-		if(block==current)
+		if (block == current)
 		{
-			current		=	block->previous;
-			usedsize	=	(unsigned int)((block->address-data)-sizeof(btBlock));
+			current = block->previous;
+			usedsize = (unsigned int)((block->address - data) - sizeof(btBlock));
 		}
 	}
 
 private:
-	void		ctor()
+	void ctor()
 	{
-		data		=	0;
-		totalsize	=	0;
-		usedsize	=	0;
-		current		=	0;
-		ischild		=	false;
+		data = 0;
+		totalsize = 0;
+		usedsize = 0;
+		current = 0;
+		ischild = false;
 	}
-	unsigned char*		data;
-	unsigned int		totalsize;
-	unsigned int		usedsize;
-	btBlock*	current;
-	bool		ischild;
+	unsigned char* data;
+	unsigned int totalsize;
+	unsigned int usedsize;
+	btBlock* current;
+	bool ischild;
 };
 
-#endif //BT_STACK_ALLOC
+#endif  //BT_STACK_ALLOC
diff --git a/extern/bullet2/src/LinearMath/btThreads.cpp b/extern/bullet2/src/LinearMath/btThreads.cpp
new file mode 100644
index 00000000000..69a86799fad
--- /dev/null
+++ b/extern/bullet2/src/LinearMath/btThreads.cpp
@@ -0,0 +1,792 @@
+/*
+Copyright (c) 2003-2014 Erwin Coumans  http://bullet.googlecode.com
+
+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 "btThreads.h"
+#include "btQuickprof.h"
+#include <algorithm>  // for min and max
+
+#if BT_USE_OPENMP && BT_THREADSAFE
+
+#include <omp.h>
+
+#endif  // #if BT_USE_OPENMP && BT_THREADSAFE
+
+#if BT_USE_PPL && BT_THREADSAFE
+
+// use Microsoft Parallel Patterns Library (installed with Visual Studio 2010 and later)
+#include <ppl.h>  // if you get a compile error here, check whether your version of Visual Studio includes PPL
+// Visual Studio 2010 and later should come with it
+#include <concrtrm.h>  // for GetProcessorCount()
+
+#endif  // #if BT_USE_PPL && BT_THREADSAFE
+
+#if BT_USE_TBB && BT_THREADSAFE
+
+// use Intel Threading Building Blocks for thread management
+#define __TBB_NO_IMPLICIT_LINKAGE 1
+#include <tbb/tbb.h>
+#include <tbb/task_scheduler_init.h>
+#include <tbb/parallel_for.h>
+#include <tbb/blocked_range.h>
+
+#endif  // #if BT_USE_TBB && BT_THREADSAFE
+
+#if BT_THREADSAFE
+//
+// Lightweight spin-mutex based on atomics
+// Using ordinary system-provided mutexes like Windows critical sections was noticeably slower
+// presumably because when it fails to lock at first it would sleep the thread and trigger costly
+// context switching.
+//
+
+#if __cplusplus >= 201103L
+
+// for anything claiming full C++11 compliance, use C++11 atomics
+// on GCC or Clang you need to compile with -std=c++11
+#define USE_CPP11_ATOMICS 1
+
+#elif defined(_MSC_VER)
+
+// on MSVC, use intrinsics instead
+#define USE_MSVC_INTRINSICS 1
+
+#elif defined(__GNUC__) && (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 7))
+
+// available since GCC 4.7 and some versions of clang
+// todo: check for clang
+#define USE_GCC_BUILTIN_ATOMICS 1
+
+#elif defined(__GNUC__) && (__GNUC__ == 4 && __GNUC_MINOR__ >= 1)
+
+// available since GCC 4.1
+#define USE_GCC_BUILTIN_ATOMICS_OLD 1
+
+#endif
+
+#if USE_CPP11_ATOMICS
+
+#include <atomic>
+#include <thread>
+
+#define THREAD_LOCAL_STATIC thread_local static
+
+bool btSpinMutex::tryLock()
+{
+	std::atomic<int>* aDest = reinterpret_cast<std::atomic<int>*>(&mLock);
+	int expected = 0;
+	return std::atomic_compare_exchange_weak_explicit(aDest, &expected, int(1), std::memory_order_acq_rel, std::memory_order_acquire);
+}
+
+void btSpinMutex::lock()
+{
+	// note: this lock does not sleep the thread.
+	while (!tryLock())
+	{
+		// spin
+	}
+}
+
+void btSpinMutex::unlock()
+{
+	std::atomic<int>* aDest = reinterpret_cast<std::atomic<int>*>(&mLock);
+	std::atomic_store_explicit(aDest, int(0), std::memory_order_release);
+}
+
+#elif USE_MSVC_INTRINSICS
+
+#define WIN32_LEAN_AND_MEAN
+
+#include <windows.h>
+#include <intrin.h>
+
+#define THREAD_LOCAL_STATIC __declspec(thread) static
+
+bool btSpinMutex::tryLock()
+{
+	volatile long* aDest = reinterpret_cast<long*>(&mLock);
+	return (0 == _InterlockedCompareExchange(aDest, 1, 0));
+}
+
+void btSpinMutex::lock()
+{
+	// note: this lock does not sleep the thread
+	while (!tryLock())
+	{
+		// spin
+	}
+}
+
+void btSpinMutex::unlock()
+{
+	volatile long* aDest = reinterpret_cast<long*>(&mLock);
+	_InterlockedExchange(aDest, 0);
+}
+
+#elif USE_GCC_BUILTIN_ATOMICS
+
+#define THREAD_LOCAL_STATIC static __thread
+
+bool btSpinMutex::tryLock()
+{
+	int expected = 0;
+	bool weak = false;
+	const int memOrderSuccess = __ATOMIC_ACQ_REL;
+	const int memOrderFail = __ATOMIC_ACQUIRE;
+	return __atomic_compare_exchange_n(&mLock, &expected, int(1), weak, memOrderSuccess, memOrderFail);
+}
+
+void btSpinMutex::lock()
+{
+	// note: this lock does not sleep the thread
+	while (!tryLock())
+	{
+		// spin
+	}
+}
+
+void btSpinMutex::unlock()
+{
+	__atomic_store_n(&mLock, int(0), __ATOMIC_RELEASE);
+}
+
+#elif USE_GCC_BUILTIN_ATOMICS_OLD
+
+#define THREAD_LOCAL_STATIC static __thread
+
+bool btSpinMutex::tryLock()
+{
+	return __sync_bool_compare_and_swap(&mLock, int(0), int(1));
+}
+
+void btSpinMutex::lock()
+{
+	// note: this lock does not sleep the thread
+	while (!tryLock())
+	{
+		// spin
+	}
+}
+
+void btSpinMutex::unlock()
+{
+	// write 0
+	__sync_fetch_and_and(&mLock, int(0));
+}
+
+#else  //#elif USE_MSVC_INTRINSICS
+
+#error "no threading primitives defined -- unknown platform"
+
+#endif  //#else //#elif USE_MSVC_INTRINSICS
+
+#else  //#if BT_THREADSAFE
+
+// These should not be called ever
+void btSpinMutex::lock()
+{
+	btAssert(!"unimplemented btSpinMutex::lock() called");
+}
+
+void btSpinMutex::unlock()
+{
+	btAssert(!"unimplemented btSpinMutex::unlock() called");
+}
+
+bool btSpinMutex::tryLock()
+{
+	btAssert(!"unimplemented btSpinMutex::tryLock() called");
+	return true;
+}
+
+#define THREAD_LOCAL_STATIC static
+
+#endif  // #else //#if BT_THREADSAFE
+
+struct ThreadsafeCounter
+{
+	unsigned int mCounter;
+	btSpinMutex mMutex;
+
+	ThreadsafeCounter()
+	{
+		mCounter = 0;
+		--mCounter;  // first count should come back 0
+	}
+
+	unsigned int getNext()
+	{
+		// no need to optimize this with atomics, it is only called ONCE per thread!
+		mMutex.lock();
+		mCounter++;
+		if (mCounter >= BT_MAX_THREAD_COUNT)
+		{
+			btAssert(!"thread counter exceeded");
+			// wrap back to the first worker index
+			mCounter = 1;
+		}
+		unsigned int val = mCounter;
+		mMutex.unlock();
+		return val;
+	}
+};
+
+static btITaskScheduler* gBtTaskScheduler=0;
+static int gThreadsRunningCounter = 0;  // useful for detecting if we are trying to do nested parallel-for calls
+static btSpinMutex gThreadsRunningCounterMutex;
+static ThreadsafeCounter gThreadCounter;
+
+//
+// BT_DETECT_BAD_THREAD_INDEX tries to detect when there are multiple threads assigned the same thread index.
+//
+// BT_DETECT_BAD_THREAD_INDEX is a developer option to test if
+// certain assumptions about how the task scheduler manages its threads
+// holds true.
+// The main assumption is:
+//   - when the threadpool is resized, the task scheduler either
+//      1. destroys all worker threads and creates all new ones in the correct number, OR
+//      2. never destroys a worker thread
+//
+// We make that assumption because we can't easily enumerate the worker threads of a task scheduler
+// to assign nice sequential thread-indexes. We also do not get notified if a worker thread is destroyed,
+// so we can't tell when a thread-index is no longer being used.
+// We allocate thread-indexes as needed with a sequential global thread counter.
+//
+// Our simple thread-counting scheme falls apart if the task scheduler destroys some threads but
+// continues to re-use other threads and the application repeatedly resizes the thread pool of the
+// task scheduler.
+// In order to prevent the thread-counter from exceeding the global max (BT_MAX_THREAD_COUNT), we
+// wrap the thread counter back to 1. This should only happen if the worker threads have all been
+// destroyed and re-created.
+//
+// BT_DETECT_BAD_THREAD_INDEX only works for Win32 right now,
+// but could be adapted to work with pthreads
+#define BT_DETECT_BAD_THREAD_INDEX 0
+
+#if BT_DETECT_BAD_THREAD_INDEX
+
+typedef DWORD ThreadId_t;
+const static ThreadId_t kInvalidThreadId = 0;
+ThreadId_t gDebugThreadIds[BT_MAX_THREAD_COUNT];
+
+static ThreadId_t getDebugThreadId()
+{
+	return GetCurrentThreadId();
+}
+
+#endif  // #if BT_DETECT_BAD_THREAD_INDEX
+
+// return a unique index per thread, main thread is 0, worker threads are in [1, BT_MAX_THREAD_COUNT)
+unsigned int btGetCurrentThreadIndex()
+{
+	const unsigned int kNullIndex = ~0U;
+	THREAD_LOCAL_STATIC unsigned int sThreadIndex = kNullIndex;
+	if (sThreadIndex == kNullIndex)
+	{
+		sThreadIndex = gThreadCounter.getNext();
+		btAssert(sThreadIndex < BT_MAX_THREAD_COUNT);
+	}
+#if BT_DETECT_BAD_THREAD_INDEX
+	if (gBtTaskScheduler && sThreadIndex > 0)
+	{
+		ThreadId_t tid = getDebugThreadId();
+		// if not set
+		if (gDebugThreadIds[sThreadIndex] == kInvalidThreadId)
+		{
+			// set it
+			gDebugThreadIds[sThreadIndex] = tid;
+		}
+		else
+		{
+			if (gDebugThreadIds[sThreadIndex] != tid)
+			{
+				// this could indicate the task scheduler is breaking our assumptions about
+				// how threads are managed when threadpool is resized
+				btAssert(!"there are 2 or more threads with the same thread-index!");
+				__debugbreak();
+			}
+		}
+	}
+#endif  // #if BT_DETECT_BAD_THREAD_INDEX
+	return sThreadIndex;
+}
+
+bool btIsMainThread()
+{
+	return btGetCurrentThreadIndex() == 0;
+}
+
+void btResetThreadIndexCounter()
+{
+	// for when all current worker threads are destroyed
+	btAssert(btIsMainThread());
+	gThreadCounter.mCounter = 0;
+}
+
+btITaskScheduler::btITaskScheduler(const char* name)
+{
+	m_name = name;
+	m_savedThreadCounter = 0;
+	m_isActive = false;
+}
+
+void btITaskScheduler::activate()
+{
+	// gThreadCounter is used to assign a thread-index to each worker thread in a task scheduler.
+	// The main thread is always thread-index 0, and worker threads are numbered from 1 to 63 (BT_MAX_THREAD_COUNT-1)
+	// The thread-indexes need to be unique amongst the threads that can be running simultaneously.
+	// Since only one task scheduler can be used at a time, it is OK for a pair of threads that belong to different
+	// task schedulers to share the same thread index because they can't be running at the same time.
+	// So each task scheduler needs to keep its own thread counter value
+	if (!m_isActive)
+	{
+		gThreadCounter.mCounter = m_savedThreadCounter;  // restore saved thread counter
+		m_isActive = true;
+	}
+}
+
+void btITaskScheduler::deactivate()
+{
+	if (m_isActive)
+	{
+		m_savedThreadCounter = gThreadCounter.mCounter;  // save thread counter
+		m_isActive = false;
+	}
+}
+
+void btPushThreadsAreRunning()
+{
+	gThreadsRunningCounterMutex.lock();
+	gThreadsRunningCounter++;
+	gThreadsRunningCounterMutex.unlock();
+}
+
+void btPopThreadsAreRunning()
+{
+	gThreadsRunningCounterMutex.lock();
+	gThreadsRunningCounter--;
+	gThreadsRunningCounterMutex.unlock();
+}
+
+bool btThreadsAreRunning()
+{
+	return gThreadsRunningCounter != 0;
+}
+
+void btSetTaskScheduler(btITaskScheduler* ts)
+{
+	int threadId = btGetCurrentThreadIndex();  // make sure we call this on main thread at least once before any workers run
+	if (threadId != 0)
+	{
+		btAssert(!"btSetTaskScheduler must be called from the main thread!");
+		return;
+	}
+	if (gBtTaskScheduler)
+	{
+		// deactivate old task scheduler
+		gBtTaskScheduler->deactivate();
+	}
+	gBtTaskScheduler = ts;
+	if (ts)
+	{
+		// activate new task scheduler
+		ts->activate();
+	}
+}
+
+btITaskScheduler* btGetTaskScheduler()
+{
+	return gBtTaskScheduler;
+}
+
+void btParallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody& body)
+{
+#if BT_THREADSAFE
+
+#if BT_DETECT_BAD_THREAD_INDEX
+	if (!btThreadsAreRunning())
+	{
+		// clear out thread ids
+		for (int i = 0; i < BT_MAX_THREAD_COUNT; ++i)
+		{
+			gDebugThreadIds[i] = kInvalidThreadId;
+		}
+	}
+#endif  // #if BT_DETECT_BAD_THREAD_INDEX
+
+	btAssert(gBtTaskScheduler != NULL);  // call btSetTaskScheduler() with a valid task scheduler first!
+	gBtTaskScheduler->parallelFor(iBegin, iEnd, grainSize, body);
+
+#else  // #if BT_THREADSAFE
+
+	// non-parallel version of btParallelFor
+	btAssert(!"called btParallelFor in non-threadsafe build. enable BT_THREADSAFE");
+	body.forLoop(iBegin, iEnd);
+
+#endif  // #if BT_THREADSAFE
+}
+
+btScalar btParallelSum(int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body)
+{
+#if BT_THREADSAFE
+
+#if BT_DETECT_BAD_THREAD_INDEX
+	if (!btThreadsAreRunning())
+	{
+		// clear out thread ids
+		for (int i = 0; i < BT_MAX_THREAD_COUNT; ++i)
+		{
+			gDebugThreadIds[i] = kInvalidThreadId;
+		}
+	}
+#endif  // #if BT_DETECT_BAD_THREAD_INDEX
+
+	btAssert(gBtTaskScheduler != NULL);  // call btSetTaskScheduler() with a valid task scheduler first!
+	return gBtTaskScheduler->parallelSum(iBegin, iEnd, grainSize, body);
+
+#else  // #if BT_THREADSAFE
+
+	// non-parallel version of btParallelSum
+	btAssert(!"called btParallelFor in non-threadsafe build. enable BT_THREADSAFE");
+	return body.sumLoop(iBegin, iEnd);
+
+#endif  //#else // #if BT_THREADSAFE
+}
+
+///
+/// btTaskSchedulerSequential -- non-threaded implementation of task scheduler
+///                              (really just useful for testing performance of single threaded vs multi)
+///
+class btTaskSchedulerSequential : public btITaskScheduler
+{
+public:
+	btTaskSchedulerSequential() : btITaskScheduler("Sequential") {}
+	virtual int getMaxNumThreads() const BT_OVERRIDE { return 1; }
+	virtual int getNumThreads() const BT_OVERRIDE { return 1; }
+	virtual void setNumThreads(int numThreads) BT_OVERRIDE {}
+	virtual void parallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody& body) BT_OVERRIDE
+	{
+		BT_PROFILE("parallelFor_sequential");
+		body.forLoop(iBegin, iEnd);
+	}
+	virtual btScalar parallelSum(int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body) BT_OVERRIDE
+	{
+		BT_PROFILE("parallelSum_sequential");
+		return body.sumLoop(iBegin, iEnd);
+	}
+};
+
+#if BT_USE_OPENMP && BT_THREADSAFE
+///
+/// btTaskSchedulerOpenMP -- wrapper around OpenMP task scheduler
+///
+class btTaskSchedulerOpenMP : public btITaskScheduler
+{
+	int m_numThreads;
+
+public:
+	btTaskSchedulerOpenMP() : btITaskScheduler("OpenMP")
+	{
+		m_numThreads = 0;
+	}
+	virtual int getMaxNumThreads() const BT_OVERRIDE
+	{
+		return omp_get_max_threads();
+	}
+	virtual int getNumThreads() const BT_OVERRIDE
+	{
+		return m_numThreads;
+	}
+	virtual void setNumThreads(int numThreads) BT_OVERRIDE
+	{
+		// With OpenMP, because it is a standard with various implementations, we can't
+		// know for sure if every implementation has the same behavior of destroying all
+		// previous threads when resizing the threadpool
+		m_numThreads = (std::max)(1, (std::min)(int(BT_MAX_THREAD_COUNT), numThreads));
+		omp_set_num_threads(1);  // hopefully, all previous threads get destroyed here
+		omp_set_num_threads(m_numThreads);
+		m_savedThreadCounter = 0;
+		if (m_isActive)
+		{
+			btResetThreadIndexCounter();
+		}
+	}
+	virtual void parallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody& body) BT_OVERRIDE
+	{
+		BT_PROFILE("parallelFor_OpenMP");
+		btPushThreadsAreRunning();
+#pragma omp parallel for schedule(static, 1)
+		for (int i = iBegin; i < iEnd; i += grainSize)
+		{
+			BT_PROFILE("OpenMP_forJob");
+			body.forLoop(i, (std::min)(i + grainSize, iEnd));
+		}
+		btPopThreadsAreRunning();
+	}
+	virtual btScalar parallelSum(int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body) BT_OVERRIDE
+	{
+		BT_PROFILE("parallelFor_OpenMP");
+		btPushThreadsAreRunning();
+		btScalar sum = btScalar(0);
+#pragma omp parallel for schedule(static, 1) reduction(+ \
+													   : sum)
+		for (int i = iBegin; i < iEnd; i += grainSize)
+		{
+			BT_PROFILE("OpenMP_sumJob");
+			sum += body.sumLoop(i, (std::min)(i + grainSize, iEnd));
+		}
+		btPopThreadsAreRunning();
+		return sum;
+	}
+};
+#endif  // #if BT_USE_OPENMP && BT_THREADSAFE
+
+#if BT_USE_TBB && BT_THREADSAFE
+///
+/// btTaskSchedulerTBB -- wrapper around Intel Threaded Building Blocks task scheduler
+///
+class btTaskSchedulerTBB : public btITaskScheduler
+{
+	int m_numThreads;
+	tbb::task_scheduler_init* m_tbbSchedulerInit;
+
+public:
+	btTaskSchedulerTBB() : btITaskScheduler("IntelTBB")
+	{
+		m_numThreads = 0;
+		m_tbbSchedulerInit = NULL;
+	}
+	~btTaskSchedulerTBB()
+	{
+		if (m_tbbSchedulerInit)
+		{
+			delete m_tbbSchedulerInit;
+			m_tbbSchedulerInit = NULL;
+		}
+	}
+
+	virtual int getMaxNumThreads() const BT_OVERRIDE
+	{
+		return tbb::task_scheduler_init::default_num_threads();
+	}
+	virtual int getNumThreads() const BT_OVERRIDE
+	{
+		return m_numThreads;
+	}
+	virtual void setNumThreads(int numThreads) BT_OVERRIDE
+	{
+		m_numThreads = (std::max)(1, (std::min)(int(BT_MAX_THREAD_COUNT), numThreads));
+		if (m_tbbSchedulerInit)
+		{
+			// destroys all previous threads
+			delete m_tbbSchedulerInit;
+			m_tbbSchedulerInit = NULL;
+		}
+		m_tbbSchedulerInit = new tbb::task_scheduler_init(m_numThreads);
+		m_savedThreadCounter = 0;
+		if (m_isActive)
+		{
+			btResetThreadIndexCounter();
+		}
+	}
+	struct ForBodyAdapter
+	{
+		const btIParallelForBody* mBody;
+
+		ForBodyAdapter(const btIParallelForBody* body) : mBody(body) {}
+		void operator()(const tbb::blocked_range<int>& range) const
+		{
+			BT_PROFILE("TBB_forJob");
+			mBody->forLoop(range.begin(), range.end());
+		}
+	};
+	virtual void parallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody& body) BT_OVERRIDE
+	{
+		BT_PROFILE("parallelFor_TBB");
+		ForBodyAdapter tbbBody(&body);
+		btPushThreadsAreRunning();
+		tbb::parallel_for(tbb::blocked_range<int>(iBegin, iEnd, grainSize),
+						  tbbBody,
+						  tbb::simple_partitioner());
+		btPopThreadsAreRunning();
+	}
+	struct SumBodyAdapter
+	{
+		const btIParallelSumBody* mBody;
+		btScalar mSum;
+
+		SumBodyAdapter(const btIParallelSumBody* body) : mBody(body), mSum(btScalar(0)) {}
+		SumBodyAdapter(const SumBodyAdapter& src, tbb::split) : mBody(src.mBody), mSum(btScalar(0)) {}
+		void join(const SumBodyAdapter& src) { mSum += src.mSum; }
+		void operator()(const tbb::blocked_range<int>& range)
+		{
+			BT_PROFILE("TBB_sumJob");
+			mSum += mBody->sumLoop(range.begin(), range.end());
+		}
+	};
+	virtual btScalar parallelSum(int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body) BT_OVERRIDE
+	{
+		BT_PROFILE("parallelSum_TBB");
+		SumBodyAdapter tbbBody(&body);
+		btPushThreadsAreRunning();
+		tbb::parallel_deterministic_reduce(tbb::blocked_range<int>(iBegin, iEnd, grainSize), tbbBody);
+		btPopThreadsAreRunning();
+		return tbbBody.mSum;
+	}
+};
+#endif  // #if BT_USE_TBB && BT_THREADSAFE
+
+#if BT_USE_PPL && BT_THREADSAFE
+///
+/// btTaskSchedulerPPL -- wrapper around Microsoft Parallel Patterns Lib task scheduler
+///
+class btTaskSchedulerPPL : public btITaskScheduler
+{
+	int m_numThreads;
+	concurrency::combinable<btScalar> m_sum;  // for parallelSum
+public:
+	btTaskSchedulerPPL() : btITaskScheduler("PPL")
+	{
+		m_numThreads = 0;
+	}
+	virtual int getMaxNumThreads() const BT_OVERRIDE
+	{
+		return concurrency::GetProcessorCount();
+	}
+	virtual int getNumThreads() const BT_OVERRIDE
+	{
+		return m_numThreads;
+	}
+	virtual void setNumThreads(int numThreads) BT_OVERRIDE
+	{
+		// capping the thread count for PPL due to a thread-index issue
+		const int maxThreadCount = (std::min)(int(BT_MAX_THREAD_COUNT), 31);
+		m_numThreads = (std::max)(1, (std::min)(maxThreadCount, numThreads));
+		using namespace concurrency;
+		if (CurrentScheduler::Id() != -1)
+		{
+			CurrentScheduler::Detach();
+		}
+		SchedulerPolicy policy;
+		{
+			// PPL seems to destroy threads when threadpool is shrunk, but keeps reusing old threads
+			// force it to destroy old threads
+			policy.SetConcurrencyLimits(1, 1);
+			CurrentScheduler::Create(policy);
+			CurrentScheduler::Detach();
+		}
+		policy.SetConcurrencyLimits(m_numThreads, m_numThreads);
+		CurrentScheduler::Create(policy);
+		m_savedThreadCounter = 0;
+		if (m_isActive)
+		{
+			btResetThreadIndexCounter();
+		}
+	}
+	struct ForBodyAdapter
+	{
+		const btIParallelForBody* mBody;
+		int mGrainSize;
+		int mIndexEnd;
+
+		ForBodyAdapter(const btIParallelForBody* body, int grainSize, int end) : mBody(body), mGrainSize(grainSize), mIndexEnd(end) {}
+		void operator()(int i) const
+		{
+			BT_PROFILE("PPL_forJob");
+			mBody->forLoop(i, (std::min)(i + mGrainSize, mIndexEnd));
+		}
+	};
+	virtual void parallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody& body) BT_OVERRIDE
+	{
+		BT_PROFILE("parallelFor_PPL");
+		// PPL dispatch
+		ForBodyAdapter pplBody(&body, grainSize, iEnd);
+		btPushThreadsAreRunning();
+		// note: MSVC 2010 doesn't support partitioner args, so avoid them
+		concurrency::parallel_for(iBegin,
+								  iEnd,
+								  grainSize,
+								  pplBody);
+		btPopThreadsAreRunning();
+	}
+	struct SumBodyAdapter
+	{
+		const btIParallelSumBody* mBody;
+		concurrency::combinable<btScalar>* mSum;
+		int mGrainSize;
+		int mIndexEnd;
+
+		SumBodyAdapter(const btIParallelSumBody* body, concurrency::combinable<btScalar>* sum, int grainSize, int end) : mBody(body), mSum(sum), mGrainSize(grainSize), mIndexEnd(end) {}
+		void operator()(int i) const
+		{
+			BT_PROFILE("PPL_sumJob");
+			mSum->local() += mBody->sumLoop(i, (std::min)(i + mGrainSize, mIndexEnd));
+		}
+	};
+	static btScalar sumFunc(btScalar a, btScalar b) { return a + b; }
+	virtual btScalar parallelSum(int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body) BT_OVERRIDE
+	{
+		BT_PROFILE("parallelSum_PPL");
+		m_sum.clear();
+		SumBodyAdapter pplBody(&body, &m_sum, grainSize, iEnd);
+		btPushThreadsAreRunning();
+		// note: MSVC 2010 doesn't support partitioner args, so avoid them
+		concurrency::parallel_for(iBegin,
+								  iEnd,
+								  grainSize,
+								  pplBody);
+		btPopThreadsAreRunning();
+		return m_sum.combine(sumFunc);
+	}
+};
+#endif  // #if BT_USE_PPL && BT_THREADSAFE
+
+// create a non-threaded task scheduler (always available)
+btITaskScheduler* btGetSequentialTaskScheduler()
+{
+	static btTaskSchedulerSequential sTaskScheduler;
+	return &sTaskScheduler;
+}
+
+// create an OpenMP task scheduler (if available, otherwise returns null)
+btITaskScheduler* btGetOpenMPTaskScheduler()
+{
+#if BT_USE_OPENMP && BT_THREADSAFE
+	static btTaskSchedulerOpenMP sTaskScheduler;
+	return &sTaskScheduler;
+#else
+	return NULL;
+#endif
+}
+
+// create an Intel TBB task scheduler (if available, otherwise returns null)
+btITaskScheduler* btGetTBBTaskScheduler()
+{
+#if BT_USE_TBB && BT_THREADSAFE
+	static btTaskSchedulerTBB sTaskScheduler;
+	return &sTaskScheduler;
+#else
+	return NULL;
+#endif
+}
+
+// create a PPL task scheduler (if available, otherwise returns null)
+btITaskScheduler* btGetPPLTaskScheduler()
+{
+#if BT_USE_PPL && BT_THREADSAFE
+	static btTaskSchedulerPPL sTaskScheduler;
+	return &sTaskScheduler;
+#else
+	return NULL;
+#endif
+}
diff --git a/extern/bullet2/src/LinearMath/btThreads.h b/extern/bullet2/src/LinearMath/btThreads.h
new file mode 100644
index 00000000000..b2227e17247
--- /dev/null
+++ b/extern/bullet2/src/LinearMath/btThreads.h
@@ -0,0 +1,177 @@
+/*
+Copyright (c) 2003-2014 Erwin Coumans  http://bullet.googlecode.com
+
+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_THREADS_H
+#define BT_THREADS_H
+
+#include "btScalar.h"  // has definitions like SIMD_FORCE_INLINE
+
+#if defined(_MSC_VER) && _MSC_VER >= 1600
+// give us a compile error if any signatures of overriden methods is changed
+#define BT_OVERRIDE override
+#endif
+
+#ifndef BT_OVERRIDE
+#define BT_OVERRIDE
+#endif
+
+// Don't set this to larger than 64, without modifying btThreadSupportPosix
+// and btThreadSupportWin32. They use UINT64 bit-masks.
+const unsigned int BT_MAX_THREAD_COUNT = 64;  // only if BT_THREADSAFE is 1
+
+// for internal use only
+bool btIsMainThread();
+bool btThreadsAreRunning();
+unsigned int btGetCurrentThreadIndex();
+void btResetThreadIndexCounter();  // notify that all worker threads have been destroyed
+
+///
+/// btSpinMutex -- lightweight spin-mutex implemented with atomic ops, never puts
+///               a thread to sleep because it is designed to be used with a task scheduler
+///               which has one thread per core and the threads don't sleep until they
+///               run out of tasks. Not good for general purpose use.
+///
+class btSpinMutex
+{
+	int mLock;
+
+public:
+	btSpinMutex()
+	{
+		mLock = 0;
+	}
+	void lock();
+	void unlock();
+	bool tryLock();
+};
+
+//
+// NOTE: btMutex* is for internal Bullet use only
+//
+// If BT_THREADSAFE is undefined or 0, should optimize away to nothing.
+// This is good because for the single-threaded build of Bullet, any calls
+// to these functions will be optimized out.
+//
+// However, for users of the multi-threaded build of Bullet this is kind
+// of bad because if you call any of these functions from external code
+// (where BT_THREADSAFE is undefined) you will get unexpected race conditions.
+//
+SIMD_FORCE_INLINE void btMutexLock(btSpinMutex* mutex)
+{
+#if BT_THREADSAFE
+	mutex->lock();
+#else
+	(void)mutex;
+#endif  // #if BT_THREADSAFE
+}
+
+SIMD_FORCE_INLINE void btMutexUnlock(btSpinMutex* mutex)
+{
+#if BT_THREADSAFE
+	mutex->unlock();
+#else
+	(void)mutex;
+#endif  // #if BT_THREADSAFE
+}
+
+SIMD_FORCE_INLINE bool btMutexTryLock(btSpinMutex* mutex)
+{
+#if BT_THREADSAFE
+	return mutex->tryLock();
+#else
+	(void)mutex;
+	return true;
+#endif  // #if BT_THREADSAFE
+}
+
+//
+// btIParallelForBody -- subclass this to express work that can be done in parallel
+//
+class btIParallelForBody
+{
+public:
+	virtual ~btIParallelForBody() {}
+	virtual void forLoop(int iBegin, int iEnd) const = 0;
+};
+
+//
+// btIParallelSumBody -- subclass this to express work that can be done in parallel
+//                       and produces a sum over all loop elements
+//
+class btIParallelSumBody
+{
+public:
+	virtual ~btIParallelSumBody() {}
+	virtual btScalar sumLoop(int iBegin, int iEnd) const = 0;
+};
+
+//
+// btITaskScheduler -- subclass this to implement a task scheduler that can dispatch work to
+//                     worker threads
+//
+class btITaskScheduler
+{
+public:
+	btITaskScheduler(const char* name);
+	virtual ~btITaskScheduler() {}
+	const char* getName() const { return m_name; }
+
+	virtual int getMaxNumThreads() const = 0;
+	virtual int getNumThreads() const = 0;
+	virtual void setNumThreads(int numThreads) = 0;
+	virtual void parallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody& body) = 0;
+	virtual btScalar parallelSum(int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body) = 0;
+	virtual void sleepWorkerThreadsHint() {}  // hint the task scheduler that we may not be using these threads for a little while
+
+	// internal use only
+	virtual void activate();
+	virtual void deactivate();
+
+protected:
+	const char* m_name;
+	unsigned int m_savedThreadCounter;
+	bool m_isActive;
+};
+
+// set the task scheduler to use for all calls to btParallelFor()
+// NOTE: you must set this prior to using any of the multi-threaded "Mt" classes
+void btSetTaskScheduler(btITaskScheduler* ts);
+
+// get the current task scheduler
+btITaskScheduler* btGetTaskScheduler();
+
+// get non-threaded task scheduler (always available)
+btITaskScheduler* btGetSequentialTaskScheduler();
+
+// create a default task scheduler (Win32 or pthreads based)
+btITaskScheduler* btCreateDefaultTaskScheduler();
+
+// get OpenMP task scheduler (if available, otherwise returns null)
+btITaskScheduler* btGetOpenMPTaskScheduler();
+
+// get Intel TBB task scheduler (if available, otherwise returns null)
+btITaskScheduler* btGetTBBTaskScheduler();
+
+// get PPL task scheduler (if available, otherwise returns null)
+btITaskScheduler* btGetPPLTaskScheduler();
+
+// btParallelFor -- call this to dispatch work like a for-loop
+//                 (iterations may be done out of order, so no dependencies are allowed)
+void btParallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody& body);
+
+// btParallelSum -- call this to dispatch work like a for-loop, returns the sum of all iterations
+//                 (iterations may be done out of order, so no dependencies are allowed)
+btScalar btParallelSum(int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body);
+
+#endif
diff --git a/extern/bullet2/src/LinearMath/btTransform.h b/extern/bullet2/src/LinearMath/btTransform.h
index d4f939a5d99..6f2f99818cf 100644
--- a/extern/bullet2/src/LinearMath/btTransform.h
+++ b/extern/bullet2/src/LinearMath/btTransform.h
@@ -12,12 +12,9 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef BT_TRANSFORM_H
 #define BT_TRANSFORM_H
 
-
 #include "btMatrix3x3.h"
 
 #ifdef BT_USE_DOUBLE_PRECISION
@@ -26,46 +23,45 @@ subject to the following restrictions:
 #define btTransformData btTransformFloatData
 #endif
 
-
-
-
 /**@brief The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear.
  *It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes. */
-ATTRIBUTE_ALIGNED16(class) btTransform {
-	
-  ///Storage for the rotation
+ATTRIBUTE_ALIGNED16(class)
+btTransform
+{
+	///Storage for the rotation
 	btMatrix3x3 m_basis;
-  ///Storage for the translation
-	btVector3   m_origin;
+	///Storage for the translation
+	btVector3 m_origin;
 
 public:
-	
-  /**@brief No initialization constructor */
+	/**@brief No initialization constructor */
 	btTransform() {}
-  /**@brief Constructor from btQuaternion (optional btVector3 )
+	/**@brief Constructor from btQuaternion (optional btVector3 )
    * @param q Rotation from quaternion 
    * @param c Translation from Vector (default 0,0,0) */
-	explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q, 
-		const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) 
+	explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q,
+										   const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
 		: m_basis(q),
-		m_origin(c)
-	{}
+		  m_origin(c)
+	{
+	}
 
-  /**@brief Constructor from btMatrix3x3 (optional btVector3)
+	/**@brief Constructor from btMatrix3x3 (optional btVector3)
    * @param b Rotation from Matrix 
    * @param c Translation from Vector default (0,0,0)*/
-	explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b, 
-		const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
+	explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b,
+										   const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
 		: m_basis(b),
-		m_origin(c)
-	{}
-  /**@brief Copy constructor */
-	SIMD_FORCE_INLINE btTransform (const btTransform& other)
+		  m_origin(c)
+	{
+	}
+	/**@brief Copy constructor */
+	SIMD_FORCE_INLINE btTransform(const btTransform& other)
 		: m_basis(other.m_basis),
-		m_origin(other.m_origin)
+		  m_origin(other.m_origin)
 	{
 	}
-  /**@brief Assignment Operator */
+	/**@brief Assignment Operator */
 	SIMD_FORCE_INLINE btTransform& operator=(const btTransform& other)
 	{
 		m_basis = other.m_basis;
@@ -73,70 +69,70 @@ public:
 		return *this;
 	}
 
-
-  /**@brief Set the current transform as the value of the product of two transforms
+	/**@brief Set the current transform as the value of the product of two transforms
    * @param t1 Transform 1
    * @param t2 Transform 2
    * This = Transform1 * Transform2 */
-		SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2) {
-			m_basis = t1.m_basis * t2.m_basis;
-			m_origin = t1(t2.m_origin);
-		}
+	SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2)
+	{
+		m_basis = t1.m_basis * t2.m_basis;
+		m_origin = t1(t2.m_origin);
+	}
 
-/*		void multInverseLeft(const btTransform& t1, const btTransform& t2) {
+	/*		void multInverseLeft(const btTransform& t1, const btTransform& t2) {
 			btVector3 v = t2.m_origin - t1.m_origin;
 			m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis);
 			m_origin = v * t1.m_basis;
 		}
 		*/
 
-/**@brief Return the transform of the vector */
+	/**@brief Return the transform of the vector */
 	SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const
 	{
-        return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin;
+		return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin;
 	}
 
-  /**@brief Return the transform of the vector */
+	/**@brief Return the transform of the vector */
 	SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const
 	{
 		return (*this)(x);
 	}
 
-  /**@brief Return the transform of the btQuaternion */
+	/**@brief Return the transform of the btQuaternion */
 	SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q) const
 	{
 		return getRotation() * q;
 	}
 
-  /**@brief Return the basis matrix for the rotation */
-	SIMD_FORCE_INLINE btMatrix3x3&       getBasis()          { return m_basis; }
-  /**@brief Return the basis matrix for the rotation */
-	SIMD_FORCE_INLINE const btMatrix3x3& getBasis()    const { return m_basis; }
+	/**@brief Return the basis matrix for the rotation */
+	SIMD_FORCE_INLINE btMatrix3x3& getBasis() { return m_basis; }
+	/**@brief Return the basis matrix for the rotation */
+	SIMD_FORCE_INLINE const btMatrix3x3& getBasis() const { return m_basis; }
 
-  /**@brief Return the origin vector translation */
-	SIMD_FORCE_INLINE btVector3&         getOrigin()         { return m_origin; }
-  /**@brief Return the origin vector translation */
-	SIMD_FORCE_INLINE const btVector3&   getOrigin()   const { return m_origin; }
+	/**@brief Return the origin vector translation */
+	SIMD_FORCE_INLINE btVector3& getOrigin() { return m_origin; }
+	/**@brief Return the origin vector translation */
+	SIMD_FORCE_INLINE const btVector3& getOrigin() const { return m_origin; }
 
-  /**@brief Return a quaternion representing the rotation */
-	btQuaternion getRotation() const { 
+	/**@brief Return a quaternion representing the rotation */
+	btQuaternion getRotation() const
+	{
 		btQuaternion q;
 		m_basis.getRotation(q);
 		return q;
 	}
-	
-	
-  /**@brief Set from an array 
+
+	/**@brief Set from an array 
    * @param m A pointer to a 16 element array (12 rotation(row major padded on the right by 1), and 3 translation */
-	void setFromOpenGLMatrix(const btScalar *m)
+	void setFromOpenGLMatrix(const btScalar* m)
 	{
 		m_basis.setFromOpenGLSubMatrix(m);
-		m_origin.setValue(m[12],m[13],m[14]);
+		m_origin.setValue(m[12], m[13], m[14]);
 	}
 
-  /**@brief Fill an array representation
+	/**@brief Fill an array representation
    * @param m A pointer to a 16 element array (12 rotation(row major padded on the right by 1), and 3 translation */
-	void getOpenGLMatrix(btScalar *m) const 
+	void getOpenGLMatrix(btScalar * m) const
 	{
 		m_basis.getOpenGLSubMatrix(m);
 		m[12] = m_origin.x();
@@ -145,80 +141,76 @@ public:
 		m[15] = btScalar(1.0);
 	}
 
-  /**@brief Set the translational element
+	/**@brief Set the translational element
    * @param origin The vector to set the translation to */
-	SIMD_FORCE_INLINE void setOrigin(const btVector3& origin) 
-	{ 
+	SIMD_FORCE_INLINE void setOrigin(const btVector3& origin)
+	{
 		m_origin = origin;
 	}
 
 	SIMD_FORCE_INLINE btVector3 invXform(const btVector3& inVec) const;
 
-
-  /**@brief Set the rotational element by btMatrix3x3 */
+	/**@brief Set the rotational element by btMatrix3x3 */
 	SIMD_FORCE_INLINE void setBasis(const btMatrix3x3& basis)
-	{ 
+	{
 		m_basis = basis;
 	}
 
-  /**@brief Set the rotational element by btQuaternion */
+	/**@brief Set the rotational element by btQuaternion */
 	SIMD_FORCE_INLINE void setRotation(const btQuaternion& q)
 	{
 		m_basis.setRotation(q);
 	}
 
-
-  /**@brief Set this transformation to the identity */
+	/**@brief Set this transformation to the identity */
 	void setIdentity()
 	{
 		m_basis.setIdentity();
 		m_origin.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	}
 
-  /**@brief Multiply this Transform by another(this = this * another) 
+	/**@brief Multiply this Transform by another(this = this * another) 
    * @param t The other transform */
-	btTransform& operator*=(const btTransform& t) 
+	btTransform& operator*=(const btTransform& t)
 	{
 		m_origin += m_basis * t.m_origin;
 		m_basis *= t.m_basis;
 		return *this;
 	}
 
-  /**@brief Return the inverse of this transform */
+	/**@brief Return the inverse of this transform */
 	btTransform inverse() const
-	{ 
+	{
 		btMatrix3x3 inv = m_basis.transpose();
 		return btTransform(inv, inv * -m_origin);
 	}
 
-  /**@brief Return the inverse of this transform times the other transform
+	/**@brief Return the inverse of this transform times the other transform
    * @param t The other transform 
    * return this.inverse() * the other */
-	btTransform inverseTimes(const btTransform& t) const;  
+	btTransform inverseTimes(const btTransform& t) const;
 
-  /**@brief Return the product of this transform and the other */
+	/**@brief Return the product of this transform and the other */
 	btTransform operator*(const btTransform& t) const;
 
-  /**@brief Return an identity transform */
-	static const btTransform&	getIdentity()
+	/**@brief Return an identity transform */
+	static const btTransform& getIdentity()
 	{
 		static const btTransform identityTransform(btMatrix3x3::getIdentity());
 		return identityTransform;
 	}
 
-	void	serialize(struct	btTransformData& dataOut) const;
-
-	void	serializeFloat(struct	btTransformFloatData& dataOut) const;
+	void serialize(struct btTransformData & dataOut) const;
 
-	void	deSerialize(const struct	btTransformData& dataIn);
+	void serializeFloat(struct btTransformFloatData & dataOut) const;
 
-	void	deSerializeDouble(const struct	btTransformDoubleData& dataIn);
+	void deSerialize(const struct btTransformData& dataIn);
 
-	void	deSerializeFloat(const struct	btTransformFloatData& dataIn);
+	void deSerializeDouble(const struct btTransformDoubleData& dataIn);
 
+	void deSerializeFloat(const struct btTransformFloatData& dataIn);
 };
 
-
 SIMD_FORCE_INLINE btVector3
 btTransform::invXform(const btVector3& inVec) const
 {
@@ -226,80 +218,69 @@ btTransform::invXform(const btVector3& inVec) const
 	return (m_basis.transpose() * v);
 }
 
-SIMD_FORCE_INLINE btTransform 
-btTransform::inverseTimes(const btTransform& t) const  
+SIMD_FORCE_INLINE btTransform
+btTransform::inverseTimes(const btTransform& t) const
 {
 	btVector3 v = t.getOrigin() - m_origin;
-		return btTransform(m_basis.transposeTimes(t.m_basis),
-			v * m_basis);
+	return btTransform(m_basis.transposeTimes(t.m_basis),
+					   v * m_basis);
 }
 
-SIMD_FORCE_INLINE btTransform 
-btTransform::operator*(const btTransform& t) const
+SIMD_FORCE_INLINE btTransform
+	btTransform::operator*(const btTransform& t) const
 {
-	return btTransform(m_basis * t.m_basis, 
-		(*this)(t.m_origin));
+	return btTransform(m_basis * t.m_basis,
+					   (*this)(t.m_origin));
 }
 
 /**@brief Test if two transforms have all elements equal */
 SIMD_FORCE_INLINE bool operator==(const btTransform& t1, const btTransform& t2)
 {
-   return ( t1.getBasis()  == t2.getBasis() &&
-            t1.getOrigin() == t2.getOrigin() );
+	return (t1.getBasis() == t2.getBasis() &&
+			t1.getOrigin() == t2.getOrigin());
 }
 
-
 ///for serialization
-struct	btTransformFloatData
+struct btTransformFloatData
 {
-	btMatrix3x3FloatData	m_basis;
-	btVector3FloatData	m_origin;
+	btMatrix3x3FloatData m_basis;
+	btVector3FloatData m_origin;
 };
 
-struct	btTransformDoubleData
+struct btTransformDoubleData
 {
-	btMatrix3x3DoubleData	m_basis;
-	btVector3DoubleData	m_origin;
+	btMatrix3x3DoubleData m_basis;
+	btVector3DoubleData m_origin;
 };
 
-
-
-SIMD_FORCE_INLINE	void	btTransform::serialize(btTransformData& dataOut) const
+SIMD_FORCE_INLINE void btTransform::serialize(btTransformData& dataOut) const
 {
 	m_basis.serialize(dataOut.m_basis);
 	m_origin.serialize(dataOut.m_origin);
 }
 
-SIMD_FORCE_INLINE	void	btTransform::serializeFloat(btTransformFloatData& dataOut) const
+SIMD_FORCE_INLINE void btTransform::serializeFloat(btTransformFloatData& dataOut) const
 {
 	m_basis.serializeFloat(dataOut.m_basis);
 	m_origin.serializeFloat(dataOut.m_origin);
 }
 
-
-SIMD_FORCE_INLINE	void	btTransform::deSerialize(const btTransformData& dataIn)
+SIMD_FORCE_INLINE void btTransform::deSerialize(const btTransformData& dataIn)
 {
 	m_basis.deSerialize(dataIn.m_basis);
 	m_origin.deSerialize(dataIn.m_origin);
 }
 
-SIMD_FORCE_INLINE	void	btTransform::deSerializeFloat(const btTransformFloatData& dataIn)
+SIMD_FORCE_INLINE void btTransform::deSerializeFloat(const btTransformFloatData& dataIn)
 {
 	m_basis.deSerializeFloat(dataIn.m_basis);
 	m_origin.deSerializeFloat(dataIn.m_origin);
 }
 
-SIMD_FORCE_INLINE	void	btTransform::deSerializeDouble(const btTransformDoubleData& dataIn)
+SIMD_FORCE_INLINE void btTransform::deSerializeDouble(const btTransformDoubleData& dataIn)
 {
 	m_basis.deSerializeDouble(dataIn.m_basis);
 	m_origin.deSerializeDouble(dataIn.m_origin);
 }
 
-
-#endif //BT_TRANSFORM_H
-
-
-
-
-
-
+#endif  //BT_TRANSFORM_H
diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet2/src/LinearMath/btTransformUtil.h
index 2303c274275..b874dd6807d 100644
--- a/extern/bullet2/src/LinearMath/btTransformUtil.h
+++ b/extern/bullet2/src/LinearMath/btTransformUtil.h
@@ -12,204 +12,202 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_TRANSFORM_UTIL_H
 #define BT_TRANSFORM_UTIL_H
 
 #include "btTransform.h"
-#define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI
-
-
+#define ANGULAR_MOTION_THRESHOLD btScalar(0.5) * SIMD_HALF_PI
 
-
-SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
+SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents, const btVector3& supportDir)
 {
 	return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(),
-      supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(),
-      supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); 
+					 supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(),
+					 supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z());
 }
 
-
-
-
-
-
 /// Utils related to temporal transforms
 class btTransformUtil
 {
-
 public:
-
-	static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform)
+	static void integrateTransform(const btTransform& curTrans, const btVector3& linvel, const btVector3& angvel, btScalar timeStep, btTransform& predictedTransform)
 	{
 		predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
-//	#define QUATERNION_DERIVATIVE
-	#ifdef QUATERNION_DERIVATIVE
+		//	#define QUATERNION_DERIVATIVE
+#ifdef QUATERNION_DERIVATIVE
 		btQuaternion predictedOrn = curTrans.getRotation();
 		predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
-		predictedOrn.normalize();
-	#else
+		predictedOrn.safeNormalize();
+#else
 		//Exponential map
 		//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
 
 		btVector3 axis;
-		btScalar	fAngle = angvel.length(); 
+		btScalar fAngle2 = angvel.length2();
+		btScalar fAngle = 0;
+		if (fAngle2 > SIMD_EPSILON)
+		{
+			fAngle = btSqrt(fAngle2);
+		}
+
 		//limit the angular motion
-		if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
+		if (fAngle * timeStep > ANGULAR_MOTION_THRESHOLD)
 		{
 			fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
 		}
 
-		if ( fAngle < btScalar(0.001) )
+		if (fAngle < btScalar(0.001))
 		{
 			// use Taylor's expansions of sync function
-			axis   = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle );
+			axis = angvel * (btScalar(0.5) * timeStep - (timeStep * timeStep * timeStep) * (btScalar(0.020833333333)) * fAngle * fAngle);
 		}
 		else
 		{
 			// sync(fAngle) = sin(c*fAngle)/t
-			axis   = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle );
+			axis = angvel * (btSin(btScalar(0.5) * fAngle * timeStep) / fAngle);
 		}
-		btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) ));
+		btQuaternion dorn(axis.x(), axis.y(), axis.z(), btCos(fAngle * timeStep * btScalar(0.5)));
 		btQuaternion orn0 = curTrans.getRotation();
 
 		btQuaternion predictedOrn = dorn * orn0;
-		predictedOrn.normalize();
-	#endif
-		predictedTransform.setRotation(predictedOrn);
+		predictedOrn.safeNormalize();
+#endif
+		if (predictedOrn.length2() > SIMD_EPSILON)
+		{
+			predictedTransform.setRotation(predictedOrn);
+		}
+		else
+		{
+			predictedTransform.setBasis(curTrans.getBasis());
+		}
 	}
 
-	static void	calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
+	static void calculateVelocityQuaternion(const btVector3& pos0, const btVector3& pos1, const btQuaternion& orn0, const btQuaternion& orn1, btScalar timeStep, btVector3& linVel, btVector3& angVel)
 	{
 		linVel = (pos1 - pos0) / timeStep;
 		btVector3 axis;
-		btScalar  angle;
+		btScalar angle;
 		if (orn0 != orn1)
 		{
-			calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle);
+			calculateDiffAxisAngleQuaternion(orn0, orn1, axis, angle);
 			angVel = axis * angle / timeStep;
-		} else
+		}
+		else
 		{
-			angVel.setValue(0,0,0);
+			angVel.setValue(0, 0, 0);
 		}
 	}
 
-	static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle)
+	static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0, const btQuaternion& orn1a, btVector3& axis, btScalar& angle)
 	{
 		btQuaternion orn1 = orn0.nearest(orn1a);
 		btQuaternion dorn = orn1 * orn0.inverse();
 		angle = dorn.getAngle();
-		axis = btVector3(dorn.x(),dorn.y(),dorn.z());
+		axis = btVector3(dorn.x(), dorn.y(), dorn.z());
 		axis[3] = btScalar(0.);
 		//check for axis length
 		btScalar len = axis.length2();
-		if (len < SIMD_EPSILON*SIMD_EPSILON)
-			axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
+		if (len < SIMD_EPSILON * SIMD_EPSILON)
+			axis = btVector3(btScalar(1.), btScalar(0.), btScalar(0.));
 		else
 			axis /= btSqrt(len);
 	}
 
-	static void	calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
+	static void calculateVelocity(const btTransform& transform0, const btTransform& transform1, btScalar timeStep, btVector3& linVel, btVector3& angVel)
 	{
 		linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
 		btVector3 axis;
-		btScalar  angle;
-		calculateDiffAxisAngle(transform0,transform1,axis,angle);
+		btScalar angle;
+		calculateDiffAxisAngle(transform0, transform1, axis, angle);
 		angVel = axis * angle / timeStep;
 	}
 
-	static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle)
+	static void calculateDiffAxisAngle(const btTransform& transform0, const btTransform& transform1, btVector3& axis, btScalar& angle)
 	{
 		btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
 		btQuaternion dorn;
 		dmat.getRotation(dorn);
 
-		///floating point inaccuracy can lead to w component > 1..., which breaks 
+		///floating point inaccuracy can lead to w component > 1..., which breaks
 		dorn.normalize();
-		
+
 		angle = dorn.getAngle();
-		axis = btVector3(dorn.x(),dorn.y(),dorn.z());
+		axis = btVector3(dorn.x(), dorn.y(), dorn.z());
 		axis[3] = btScalar(0.);
 		//check for axis length
 		btScalar len = axis.length2();
-		if (len < SIMD_EPSILON*SIMD_EPSILON)
-			axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
+		if (len < SIMD_EPSILON * SIMD_EPSILON)
+			axis = btVector3(btScalar(1.), btScalar(0.), btScalar(0.));
 		else
 			axis /= btSqrt(len);
 	}
-
 };
 
-
-///The btConvexSeparatingDistanceUtil can help speed up convex collision detection 
+///The btConvexSeparatingDistanceUtil can help speed up convex collision detection
 ///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
-class	btConvexSeparatingDistanceUtil
+class btConvexSeparatingDistanceUtil
 {
-	btQuaternion	m_ornA;
-	btQuaternion	m_ornB;
-	btVector3	m_posA;
-	btVector3	m_posB;
-	
-	btVector3	m_separatingNormal;
+	btQuaternion m_ornA;
+	btQuaternion m_ornB;
+	btVector3 m_posA;
+	btVector3 m_posB;
 
-	btScalar	m_boundingRadiusA;
-	btScalar	m_boundingRadiusB;
-	btScalar	m_separatingDistance;
+	btVector3 m_separatingNormal;
 
-public:
+	btScalar m_boundingRadiusA;
+	btScalar m_boundingRadiusB;
+	btScalar m_separatingDistance;
 
-	btConvexSeparatingDistanceUtil(btScalar	boundingRadiusA,btScalar	boundingRadiusB)
-		:m_boundingRadiusA(boundingRadiusA),
-		m_boundingRadiusB(boundingRadiusB),
-		m_separatingDistance(0.f)
+public:
+	btConvexSeparatingDistanceUtil(btScalar boundingRadiusA, btScalar boundingRadiusB)
+		: m_boundingRadiusA(boundingRadiusA),
+		  m_boundingRadiusB(boundingRadiusB),
+		  m_separatingDistance(0.f)
 	{
 	}
 
-	btScalar	getConservativeSeparatingDistance()
+	btScalar getConservativeSeparatingDistance()
 	{
 		return m_separatingDistance;
 	}
 
-	void	updateSeparatingDistance(const btTransform& transA,const btTransform& transB)
+	void updateSeparatingDistance(const btTransform& transA, const btTransform& transB)
 	{
 		const btVector3& toPosA = transA.getOrigin();
 		const btVector3& toPosB = transB.getOrigin();
 		btQuaternion toOrnA = transA.getRotation();
 		btQuaternion toOrnB = transB.getRotation();
 
-		if (m_separatingDistance>0.f)
+		if (m_separatingDistance > 0.f)
 		{
-			
-
-			btVector3 linVelA,angVelA,linVelB,angVelB;
-			btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA);
-			btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB);
+			btVector3 linVelA, angVelA, linVelB, angVelB;
+			btTransformUtil::calculateVelocityQuaternion(m_posA, toPosA, m_ornA, toOrnA, btScalar(1.), linVelA, angVelA);
+			btTransformUtil::calculateVelocityQuaternion(m_posB, toPosB, m_ornB, toOrnB, btScalar(1.), linVelB, angVelB);
 			btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
-			btVector3 relLinVel = (linVelB-linVelA);
+			btVector3 relLinVel = (linVelB - linVelA);
 			btScalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
-			if (relLinVelocLength<0.f)
+			if (relLinVelocLength < 0.f)
 			{
 				relLinVelocLength = 0.f;
 			}
-	
-			btScalar	projectedMotion = maxAngularProjectedVelocity +relLinVelocLength;
+
+			btScalar projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
 			m_separatingDistance -= projectedMotion;
 		}
-	
+
 		m_posA = toPosA;
 		m_posB = toPosB;
 		m_ornA = toOrnA;
 		m_ornB = toOrnB;
 	}
 
-	void	initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB)
+	void initSeparatingDistance(const btVector3& separatingVector, btScalar separatingDistance, const btTransform& transA, const btTransform& transB)
 	{
 		m_separatingDistance = separatingDistance;
 
-		if (m_separatingDistance>0.f)
+		if (m_separatingDistance > 0.f)
 		{
 			m_separatingNormal = separatingVector;
-			
+
 			const btVector3& toPosA = transA.getOrigin();
 			const btVector3& toPosB = transB.getOrigin();
 			btQuaternion toOrnA = transA.getRotation();
@@ -220,9 +218,6 @@ public:
 			m_ornB = toOrnB;
 		}
 	}
-
 };
 
-
-#endif //BT_TRANSFORM_UTIL_H
-
+#endif  //BT_TRANSFORM_UTIL_H
diff --git a/extern/bullet2/src/LinearMath/btVector3.cpp b/extern/bullet2/src/LinearMath/btVector3.cpp
index dbcf2b6ab57..13111157af4 100644
--- a/extern/bullet2/src/LinearMath/btVector3.cpp
+++ b/extern/bullet2/src/LinearMath/btVector3.cpp
@@ -15,282 +15,285 @@
  This source version has been altered.
  */
 
-//#if defined (_WIN32) || defined (__i386__)
-//#define BT_USE_SSE_IN_API
-//#endif
-
+#if defined(_WIN32) || defined(__i386__)
+#define BT_USE_SSE_IN_API
+#endif
 
 #include "btVector3.h"
 
-
-
 #if defined BT_USE_SIMD_VECTOR3
 
 #if DEBUG
-#include <string.h>//for memset
+#include <string.h>  //for memset
 #endif
 
-
 #ifdef __APPLE__
 #include <stdint.h>
-typedef  float float4 __attribute__ ((vector_size(16)));
+typedef float float4 __attribute__((vector_size(16)));
 #else
 #define float4 __m128
 #endif
 //typedef  uint32_t uint4 __attribute__ ((vector_size(16)));
 
-
 #if defined BT_USE_SSE || defined _WIN32
 
-#define LOG2_ARRAY_SIZE     6
-#define STACK_ARRAY_COUNT   (1UL << LOG2_ARRAY_SIZE)
+#define LOG2_ARRAY_SIZE 6
+#define STACK_ARRAY_COUNT (1UL << LOG2_ARRAY_SIZE)
 
 #include <emmintrin.h>
 
-long _maxdot_large( const float *vv, const float *vec, unsigned long count, float *dotResult );
-long _maxdot_large( const float *vv, const float *vec, unsigned long count, float *dotResult )
+long _maxdot_large(const float *vv, const float *vec, unsigned long count, float *dotResult);
+long _maxdot_large(const float *vv, const float *vec, unsigned long count, float *dotResult)
 {
-    const float4 *vertices = (const float4*) vv;
-    static const unsigned char indexTable[16] = {(unsigned char)-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 };
-    float4 dotMax = btAssign128( -BT_INFINITY,  -BT_INFINITY,  -BT_INFINITY,  -BT_INFINITY );
-    float4 vvec = _mm_loadu_ps( vec );
-    float4 vHi = btCastiTo128f(_mm_shuffle_epi32( btCastfTo128i( vvec), 0xaa ));          /// zzzz
-    float4 vLo = _mm_movelh_ps( vvec, vvec );                               /// xyxy
-    
-    long maxIndex = -1L;
-    
-    size_t segment = 0;
-    float4 stack_array[ STACK_ARRAY_COUNT ];
-    
+	const float4 *vertices = (const float4 *)vv;
+	static const unsigned char indexTable[16] = {(unsigned char)-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0};
+	float4 dotMax = btAssign128(-BT_INFINITY, -BT_INFINITY, -BT_INFINITY, -BT_INFINITY);
+	float4 vvec = _mm_loadu_ps(vec);
+	float4 vHi = btCastiTo128f(_mm_shuffle_epi32(btCastfTo128i(vvec), 0xaa));  /// zzzz
+	float4 vLo = _mm_movelh_ps(vvec, vvec);                                    /// xyxy
+
+	long maxIndex = -1L;
+
+	size_t segment = 0;
+	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;
-    float4 max;
-    // Faster loop without cleanup code for full tiles
-    for ( segment = 0; segment + STACK_ARRAY_COUNT*4 <= count; segment += STACK_ARRAY_COUNT*4 ) 
-    {
-        max = dotMax;
-        
-        for( index = 0; index < STACK_ARRAY_COUNT; index+= 4 )   
-        { // do four dot products at a time. Carefully avoid touching the w element.
-            float4 v0 = vertices[0];
-            float4 v1 = vertices[1];
-            float4 v2 = vertices[2];
-            float4 v3 = vertices[3];            vertices += 4;
-            
-            float4 lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            float4 hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            float4 lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            float4 hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index] = x;
-            max = _mm_max_ps( x, max );         // control the order here so that max is never NaN even if x is nan
-            
-            v0 = vertices[0];
-            v1 = vertices[1];
-            v2 = vertices[2];
-            v3 = vertices[3];            vertices += 4;
-            
-            lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index+1] = x;
-            max = _mm_max_ps( x, max );         // control the order here so that max is never NaN even if x is nan
-            
-            v0 = vertices[0];
-            v1 = vertices[1];
-            v2 = vertices[2];
-            v3 = vertices[3];            vertices += 4;
-            
-            lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index+2] = x;
-            max = _mm_max_ps( x, max );         // control the order here so that max is never NaN even if x is nan
-            
-            v0 = vertices[0];
-            v1 = vertices[1];
-            v2 = vertices[2];
-            v3 = vertices[3];            vertices += 4;
-            
-            lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index+3] = x;
-            max = _mm_max_ps( x, max );         // control the order here so that max is never NaN even if x is nan
-            
-            // It is too costly to keep the index of the max here. We will look for it again later.  We save a lot of work this way.
-        }
-        
-        // If we found a new max
-        if( 0xf != _mm_movemask_ps( (float4) _mm_cmpeq_ps(max, dotMax)))
-        { 
-            // copy the new max across all lanes of our max accumulator
-            max = _mm_max_ps(max, (float4) _mm_shuffle_ps( max, max, 0x4e));
-            max = _mm_max_ps(max, (float4) _mm_shuffle_ps( max, max, 0xb1));
-            
-            dotMax = max;
-            
-            // find first occurrence of that max  
-            size_t test;
-            for( index = 0; 0 == (test=_mm_movemask_ps( _mm_cmpeq_ps( stack_array[index], max))); index++ )   // local_count must be a multiple of 4
-            {}
-            // record where it is.
-            maxIndex = 4*index + segment + indexTable[test];
-        }
-    }
-    
-    // account for work we've already done
-    count -= segment;
-    
-    // Deal with the last < STACK_ARRAY_COUNT vectors
-    max = dotMax;
-    index = 0;
-    
-    
-    if( btUnlikely( count > 16) )
-    {
-        for( ; index + 4 <= count / 4; index+=4 )   
-        { // do four dot products at a time. Carefully avoid touching the w element.
-            float4 v0 = vertices[0];
-            float4 v1 = vertices[1];
-            float4 v2 = vertices[2];
-            float4 v3 = vertices[3];            vertices += 4;
-            
-            float4 lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            float4 hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            float4 lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            float4 hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index] = x;
-            max = _mm_max_ps( x, max );         // control the order here so that max is never NaN even if x is nan
-            
-            v0 = vertices[0];
-            v1 = vertices[1];
-            v2 = vertices[2];
-            v3 = vertices[3];            vertices += 4;
-            
-            lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index+1] = x;
-            max = _mm_max_ps( x, max );         // control the order here so that max is never NaN even if x is nan
-            
-            v0 = vertices[0];
-            v1 = vertices[1];
-            v2 = vertices[2];
-            v3 = vertices[3];            vertices += 4;
-            
-            lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index+2] = x;
-            max = _mm_max_ps( x, max );         // control the order here so that max is never NaN even if x is nan
-            
-            v0 = vertices[0];
-            v1 = vertices[1];
-            v2 = vertices[2];
-            v3 = vertices[3];            vertices += 4;
-            
-            lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index+3] = x;
-            max = _mm_max_ps( x, max );         // control the order here so that max is never NaN even if x is nan
-            
-            // It is too costly to keep the index of the max here. We will look for it again later.  We save a lot of work this way.
-        }
-    }
-    
-    size_t localCount = (count & -4L) - 4*index;
-    if( localCount )
-    {
+
+	size_t index;
+	float4 max;
+	// Faster loop without cleanup code for full tiles
+	for (segment = 0; segment + STACK_ARRAY_COUNT * 4 <= count; segment += STACK_ARRAY_COUNT * 4)
+	{
+		max = dotMax;
+
+		for (index = 0; index < STACK_ARRAY_COUNT; index += 4)
+		{  // do four dot products at a time. Carefully avoid touching the w element.
+			float4 v0 = vertices[0];
+			float4 v1 = vertices[1];
+			float4 v2 = vertices[2];
+			float4 v3 = vertices[3];
+			vertices += 4;
+
+			float4 lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			float4 hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			float4 lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			float4 hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 1] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 2] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 3] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			// It is too costly to keep the index of the max here. We will look for it again later.  We save a lot of work this way.
+		}
+
+		// If we found a new max
+		if (0xf != _mm_movemask_ps((float4)_mm_cmpeq_ps(max, dotMax)))
+		{
+			// copy the new max across all lanes of our max accumulator
+			max = _mm_max_ps(max, (float4)_mm_shuffle_ps(max, max, 0x4e));
+			max = _mm_max_ps(max, (float4)_mm_shuffle_ps(max, max, 0xb1));
+
+			dotMax = max;
+
+			// find first occurrence of that max
+			size_t test;
+			for (index = 0; 0 == (test = _mm_movemask_ps(_mm_cmpeq_ps(stack_array[index], max))); index++)  // local_count must be a multiple of 4
+			{
+			}
+			// record where it is.
+			maxIndex = 4 * index + segment + indexTable[test];
+		}
+	}
+
+	// account for work we've already done
+	count -= segment;
+
+	// Deal with the last < STACK_ARRAY_COUNT vectors
+	max = dotMax;
+	index = 0;
+
+	if (btUnlikely(count > 16))
+	{
+		for (; index + 4 <= count / 4; index += 4)
+		{  // do four dot products at a time. Carefully avoid touching the w element.
+			float4 v0 = vertices[0];
+			float4 v1 = vertices[1];
+			float4 v2 = vertices[2];
+			float4 v3 = vertices[3];
+			vertices += 4;
+
+			float4 lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			float4 hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			float4 lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			float4 hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 1] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 2] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 3] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			// It is too costly to keep the index of the max here. We will look for it again later.  We save a lot of work this way.
+		}
+	}
+
+	size_t localCount = (count & -4L) - 4 * index;
+	if (localCount)
+	{
 #ifdef __APPLE__
-        float4 t0, t1, t2, t3, t4;
-        float4 * sap = &stack_array[index + localCount / 4];
-          vertices += localCount;      // counter the offset
-         size_t byteIndex = -(localCount) * sizeof(float);
-        //AT&T Code style assembly
-        asm volatile
-        (   ".align 4                                                                   \n\
+		float4 t0, t1, t2, t3, t4;
+		float4 *sap = &stack_array[index + localCount / 4];
+		vertices += localCount;  // counter the offset
+		size_t byteIndex = -(localCount) * sizeof(float);
+		//AT&T Code style assembly
+		asm volatile(
+			".align 4                                                                   \n\
              0: movaps  %[max], %[t2]                            // move max out of the way to avoid propagating NaNs in max \n\
           movaps  (%[vertices], %[byteIndex], 4),    %[t0]    // vertices[0]      \n\
           movaps  16(%[vertices], %[byteIndex], 4),  %[t1]    // vertices[1]      \n\
@@ -316,368 +319,374 @@ long _maxdot_large( const float *vv, const float *vec, unsigned long count, floa
          add     $16, %[byteIndex]                           // advance loop counter\n\
          jnz     0b                                          \n\
      "
-         : [max] "+x" (max), [t0] "=&x" (t0), [t1] "=&x" (t1), [t2] "=&x" (t2), [t3] "=&x" (t3), [t4] "=&x" (t4), [byteIndex] "+r" (byteIndex)
-         : [vLo] "x" (vLo), [vHi] "x" (vHi), [vertices] "r" (vertices), [sap] "r" (sap)
-         : "memory", "cc"
-         );
-        index += localCount/4;
+			: [max] "+x"(max), [t0] "=&x"(t0), [t1] "=&x"(t1), [t2] "=&x"(t2), [t3] "=&x"(t3), [t4] "=&x"(t4), [byteIndex] "+r"(byteIndex)
+			: [vLo] "x"(vLo), [vHi] "x"(vHi), [vertices] "r"(vertices), [sap] "r"(sap)
+			: "memory", "cc");
+		index += localCount / 4;
 #else
-        {
-            for( unsigned int i=0; i<localCount/4; i++,index++)   
-            { // do four dot products at a time. Carefully avoid touching the w element.
-                float4 v0 = vertices[0];
-                float4 v1 = vertices[1];
-                float4 v2 = vertices[2];
-                float4 v3 = vertices[3];            
-                vertices += 4;
-                
-                float4 lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-                float4 hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-                float4 lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-                float4 hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-                
-                lo0 = lo0*vLo;
-                lo1 = lo1*vLo;
-                float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
-                float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
-                float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-                z = z*vHi;
-                x = x+y;
-                x = x+z;
-                stack_array[index] = x;
-                max = _mm_max_ps( x, max );         // control the order here so that max is never NaN even if x is nan
-            }
-        }
-#endif //__APPLE__
-    }
-
-    // process the last few points
-    if( count & 3 )
-    {
-        float4 v0, v1, v2, x, y, z;
-        switch( count & 3 )
-        {
-            case 3:
-            {
-                v0 = vertices[0];
-                v1 = vertices[1];
-                v2 = vertices[2];
-                
-                // Calculate 3 dot products, transpose, duplicate v2
-                float4 lo0 = _mm_movelh_ps( v0, v1);        // xyxy.lo
-                float4 hi0 = _mm_movehl_ps( v1, v0);        // z?z?.lo
-                lo0 = lo0*vLo;
-                z = _mm_shuffle_ps(hi0, v2,  0xa8 );           // z0z1z2z2
-                z = z*vHi;
-                float4 lo1 = _mm_movelh_ps(v2, v2);          // xyxy
-                lo1 = lo1*vLo;
-                x = _mm_shuffle_ps(lo0, lo1, 0x88);
-                y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            }
-                break;
-            case 2:
-            {
-                v0 = vertices[0];
-                v1 = vertices[1];
-                float4 xy = _mm_movelh_ps(v0, v1);
-                z = _mm_movehl_ps(v1, v0);
-                xy = xy*vLo;
-                z = _mm_shuffle_ps( z, z,  0xa8);
-                x = _mm_shuffle_ps( xy, xy, 0xa8);
-                y = _mm_shuffle_ps( xy, xy, 0xfd);
-                z = z*vHi;
-            }
-                break;
-            case 1:
-            {
-                float4 xy = vertices[0];
-                z =  _mm_shuffle_ps( xy, xy, 0xaa);
-                xy = xy*vLo;
-                z = z*vHi;
-                x = _mm_shuffle_ps(xy, xy, 0);
-                y = _mm_shuffle_ps(xy, xy, 0x55);
-            }
-                break;
-        }
-        x = x+y;
-        x = x+z;
-        stack_array[index] = x;
-        max = _mm_max_ps( x, max );         // control the order here so that max is never NaN even if x is nan
-        index++;
-    }
-    
-    // if we found a new max. 
-    if( 0 == segment || 0xf != _mm_movemask_ps( (float4) _mm_cmpeq_ps(max, dotMax)))
-    { // we found a new max. Search for it
-      // find max across the max vector, place in all elements of max -- big latency hit here
-        max = _mm_max_ps(max, (float4) _mm_shuffle_ps( max, max, 0x4e));
-        max = _mm_max_ps(max, (float4) _mm_shuffle_ps( max, max, 0xb1));
-        
-        // It is slightly faster to do this part in scalar code when count < 8. However, the common case for
-        // this where it actually makes a difference is handled in the early out at the top of the function, 
-        // so it is less than a 1% difference here. I opted for improved code size, fewer branches and reduced 
-        // complexity, and removed it.
-        
-        dotMax = max;
-        
-        // scan for the first occurence of max in the array  
-        size_t test;
-        for( index = 0; 0 == (test=_mm_movemask_ps( _mm_cmpeq_ps( stack_array[index], max))); index++ )   // local_count must be a multiple of 4
-        {}
-        maxIndex = 4*index + segment + indexTable[test];
-    }
-    
-    _mm_store_ss( dotResult, dotMax);
-    return maxIndex;
+		{
+			for (unsigned int i = 0; i < localCount / 4; i++, index++)
+			{  // do four dot products at a time. Carefully avoid touching the w element.
+				float4 v0 = vertices[0];
+				float4 v1 = vertices[1];
+				float4 v2 = vertices[2];
+				float4 v3 = vertices[3];
+				vertices += 4;
+
+				float4 lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+				float4 hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+				float4 lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+				float4 hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+				lo0 = lo0 * vLo;
+				lo1 = lo1 * vLo;
+				float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
+				float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
+				float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+				z = z * vHi;
+				x = x + y;
+				x = x + z;
+				stack_array[index] = x;
+				max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+			}
+		}
+#endif  //__APPLE__
+	}
+
+	// process the last few points
+	if (count & 3)
+	{
+		float4 v0, v1, v2, x, y, z;
+		switch (count & 3)
+		{
+			case 3:
+			{
+				v0 = vertices[0];
+				v1 = vertices[1];
+				v2 = vertices[2];
+
+				// Calculate 3 dot products, transpose, duplicate v2
+				float4 lo0 = _mm_movelh_ps(v0, v1);  // xyxy.lo
+				float4 hi0 = _mm_movehl_ps(v1, v0);  // z?z?.lo
+				lo0 = lo0 * vLo;
+				z = _mm_shuffle_ps(hi0, v2, 0xa8);  // z0z1z2z2
+				z = z * vHi;
+				float4 lo1 = _mm_movelh_ps(v2, v2);  // xyxy
+				lo1 = lo1 * vLo;
+				x = _mm_shuffle_ps(lo0, lo1, 0x88);
+				y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			}
+			break;
+			case 2:
+			{
+				v0 = vertices[0];
+				v1 = vertices[1];
+				float4 xy = _mm_movelh_ps(v0, v1);
+				z = _mm_movehl_ps(v1, v0);
+				xy = xy * vLo;
+				z = _mm_shuffle_ps(z, z, 0xa8);
+				x = _mm_shuffle_ps(xy, xy, 0xa8);
+				y = _mm_shuffle_ps(xy, xy, 0xfd);
+				z = z * vHi;
+			}
+			break;
+			case 1:
+			{
+				float4 xy = vertices[0];
+				z = _mm_shuffle_ps(xy, xy, 0xaa);
+				xy = xy * vLo;
+				z = z * vHi;
+				x = _mm_shuffle_ps(xy, xy, 0);
+				y = _mm_shuffle_ps(xy, xy, 0x55);
+			}
+			break;
+		}
+		x = x + y;
+		x = x + z;
+		stack_array[index] = x;
+		max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+		index++;
+	}
+
+	// if we found a new max.
+	if (0 == segment || 0xf != _mm_movemask_ps((float4)_mm_cmpeq_ps(max, dotMax)))
+	{  // we found a new max. Search for it
+		// find max across the max vector, place in all elements of max -- big latency hit here
+		max = _mm_max_ps(max, (float4)_mm_shuffle_ps(max, max, 0x4e));
+		max = _mm_max_ps(max, (float4)_mm_shuffle_ps(max, max, 0xb1));
+
+		// It is slightly faster to do this part in scalar code when count < 8. However, the common case for
+		// this where it actually makes a difference is handled in the early out at the top of the function,
+		// so it is less than a 1% difference here. I opted for improved code size, fewer branches and reduced
+		// complexity, and removed it.
+
+		dotMax = max;
+
+		// scan for the first occurence of max in the array
+		size_t test;
+		for (index = 0; 0 == (test = _mm_movemask_ps(_mm_cmpeq_ps(stack_array[index], max))); index++)  // local_count must be a multiple of 4
+		{
+		}
+		maxIndex = 4 * index + segment + indexTable[test];
+	}
+
+	_mm_store_ss(dotResult, dotMax);
+	return maxIndex;
 }
 
-long _mindot_large( const float *vv, const float *vec, unsigned long count, float *dotResult );
+long _mindot_large(const float *vv, const float *vec, unsigned long count, float *dotResult);
 
-long _mindot_large( const float *vv, const float *vec, unsigned long count, float *dotResult )
+long _mindot_large(const float *vv, const float *vec, unsigned long count, float *dotResult)
 {
-    const float4 *vertices = (const float4*) vv;
-    static const unsigned char indexTable[16] = {(unsigned char)-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 };
-    float4 dotmin = btAssign128( BT_INFINITY,  BT_INFINITY,  BT_INFINITY,  BT_INFINITY );
-    float4 vvec = _mm_loadu_ps( vec );
-    float4 vHi = btCastiTo128f(_mm_shuffle_epi32( btCastfTo128i( vvec), 0xaa ));          /// zzzz
-    float4 vLo = _mm_movelh_ps( vvec, vvec );                               /// xyxy
-    
-    long minIndex = -1L;
-
-    size_t segment = 0;
-    float4 stack_array[ STACK_ARRAY_COUNT ];
-    
+	const float4 *vertices = (const float4 *)vv;
+	static const unsigned char indexTable[16] = {(unsigned char)-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0};
+	float4 dotmin = btAssign128(BT_INFINITY, BT_INFINITY, BT_INFINITY, BT_INFINITY);
+	float4 vvec = _mm_loadu_ps(vec);
+	float4 vHi = btCastiTo128f(_mm_shuffle_epi32(btCastfTo128i(vvec), 0xaa));  /// zzzz
+	float4 vLo = _mm_movelh_ps(vvec, vvec);                                    /// xyxy
+
+	long minIndex = -1L;
+
+	size_t segment = 0;
+	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;
-    float4 min;
-    // Faster loop without cleanup code for full tiles
-    for ( segment = 0; segment + STACK_ARRAY_COUNT*4 <= count; segment += STACK_ARRAY_COUNT*4 ) 
-    {
-        min = dotmin;
-        
-        for( index = 0; index < STACK_ARRAY_COUNT; index+= 4 )   
-        { // do four dot products at a time. Carefully avoid touching the w element.
-            float4 v0 = vertices[0];
-            float4 v1 = vertices[1];
-            float4 v2 = vertices[2];
-            float4 v3 = vertices[3];            vertices += 4;
-            
-            float4 lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            float4 hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            float4 lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            float4 hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index] = x;
-            min = _mm_min_ps( x, min );         // control the order here so that min is never NaN even if x is nan
-            
-            v0 = vertices[0];
-            v1 = vertices[1];
-            v2 = vertices[2];
-            v3 = vertices[3];            vertices += 4;
-            
-            lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index+1] = x;
-            min = _mm_min_ps( x, min );         // control the order here so that min is never NaN even if x is nan
-            
-            v0 = vertices[0];
-            v1 = vertices[1];
-            v2 = vertices[2];
-            v3 = vertices[3];            vertices += 4;
-            
-            lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index+2] = x;
-            min = _mm_min_ps( x, min );         // control the order here so that min is never NaN even if x is nan
-            
-            v0 = vertices[0];
-            v1 = vertices[1];
-            v2 = vertices[2];
-            v3 = vertices[3];            vertices += 4;
-            
-            lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index+3] = x;
-            min = _mm_min_ps( x, min );         // control the order here so that min is never NaN even if x is nan
-            
-            // It is too costly to keep the index of the min here. We will look for it again later.  We save a lot of work this way.
-        }
-        
-        // If we found a new min
-        if( 0xf != _mm_movemask_ps( (float4) _mm_cmpeq_ps(min, dotmin)))
-        { 
-            // copy the new min across all lanes of our min accumulator
-            min = _mm_min_ps(min, (float4) _mm_shuffle_ps( min, min, 0x4e));
-            min = _mm_min_ps(min, (float4) _mm_shuffle_ps( min, min, 0xb1));
-            
-            dotmin = min;
-            
-            // find first occurrence of that min  
-            size_t test;
-            for( index = 0; 0 == (test=_mm_movemask_ps( _mm_cmpeq_ps( stack_array[index], min))); index++ )   // local_count must be a multiple of 4
-            {}
-            // record where it is.
-            minIndex = 4*index + segment + indexTable[test];
-        }
-    }
-    
-    // account for work we've already done
-    count -= segment;
-    
-    // Deal with the last < STACK_ARRAY_COUNT vectors
-    min = dotmin;
-    index = 0;
-    
-    
-    if(btUnlikely( count > 16) )
-    {
-        for( ; index + 4 <= count / 4; index+=4 )   
-        { // do four dot products at a time. Carefully avoid touching the w element.
-            float4 v0 = vertices[0];
-            float4 v1 = vertices[1];
-            float4 v2 = vertices[2];
-            float4 v3 = vertices[3];            vertices += 4;
-            
-            float4 lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            float4 hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            float4 lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            float4 hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index] = x;
-            min = _mm_min_ps( x, min );         // control the order here so that min is never NaN even if x is nan
-            
-            v0 = vertices[0];
-            v1 = vertices[1];
-            v2 = vertices[2];
-            v3 = vertices[3];            vertices += 4;
-            
-            lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index+1] = x;
-            min = _mm_min_ps( x, min );         // control the order here so that min is never NaN even if x is nan
-            
-            v0 = vertices[0];
-            v1 = vertices[1];
-            v2 = vertices[2];
-            v3 = vertices[3];            vertices += 4;
-            
-            lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index+2] = x;
-            min = _mm_min_ps( x, min );         // control the order here so that min is never NaN even if x is nan
-            
-            v0 = vertices[0];
-            v1 = vertices[1];
-            v2 = vertices[2];
-            v3 = vertices[3];            vertices += 4;
-            
-            lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-            hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-            lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-            hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-            
-            lo0 = lo0*vLo;
-            lo1 = lo1*vLo;
-            z = _mm_shuffle_ps(hi0, hi1, 0x88);
-            x = _mm_shuffle_ps(lo0, lo1, 0x88);
-            y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            z = z*vHi;
-            x = x+y;
-            x = x+z;
-            stack_array[index+3] = x;
-            min = _mm_min_ps( x, min );         // control the order here so that min is never NaN even if x is nan
-            
-            // It is too costly to keep the index of the min here. We will look for it again later.  We save a lot of work this way.
-        }
-    }
-    
-    size_t localCount = (count & -4L) - 4*index;
-    if( localCount )
-    {
-        
-        
+
+	size_t index;
+	float4 min;
+	// Faster loop without cleanup code for full tiles
+	for (segment = 0; segment + STACK_ARRAY_COUNT * 4 <= count; segment += STACK_ARRAY_COUNT * 4)
+	{
+		min = dotmin;
+
+		for (index = 0; index < STACK_ARRAY_COUNT; index += 4)
+		{  // do four dot products at a time. Carefully avoid touching the w element.
+			float4 v0 = vertices[0];
+			float4 v1 = vertices[1];
+			float4 v2 = vertices[2];
+			float4 v3 = vertices[3];
+			vertices += 4;
+
+			float4 lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			float4 hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			float4 lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			float4 hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 1] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 2] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 3] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			// It is too costly to keep the index of the min here. We will look for it again later.  We save a lot of work this way.
+		}
+
+		// If we found a new min
+		if (0xf != _mm_movemask_ps((float4)_mm_cmpeq_ps(min, dotmin)))
+		{
+			// copy the new min across all lanes of our min accumulator
+			min = _mm_min_ps(min, (float4)_mm_shuffle_ps(min, min, 0x4e));
+			min = _mm_min_ps(min, (float4)_mm_shuffle_ps(min, min, 0xb1));
+
+			dotmin = min;
+
+			// find first occurrence of that min
+			size_t test;
+			for (index = 0; 0 == (test = _mm_movemask_ps(_mm_cmpeq_ps(stack_array[index], min))); index++)  // local_count must be a multiple of 4
+			{
+			}
+			// record where it is.
+			minIndex = 4 * index + segment + indexTable[test];
+		}
+	}
+
+	// account for work we've already done
+	count -= segment;
+
+	// Deal with the last < STACK_ARRAY_COUNT vectors
+	min = dotmin;
+	index = 0;
+
+	if (btUnlikely(count > 16))
+	{
+		for (; index + 4 <= count / 4; index += 4)
+		{  // do four dot products at a time. Carefully avoid touching the w element.
+			float4 v0 = vertices[0];
+			float4 v1 = vertices[1];
+			float4 v2 = vertices[2];
+			float4 v3 = vertices[3];
+			vertices += 4;
+
+			float4 lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			float4 hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			float4 lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			float4 hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 1] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 2] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 3] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			// It is too costly to keep the index of the min here. We will look for it again later.  We save a lot of work this way.
+		}
+	}
+
+	size_t localCount = (count & -4L) - 4 * index;
+	if (localCount)
+	{
 #ifdef __APPLE__
-        vertices += localCount;      // counter the offset
-        float4 t0, t1, t2, t3, t4;
-        size_t byteIndex = -(localCount) * sizeof(float);
-        float4 * sap = &stack_array[index + localCount / 4];
-        
-        asm volatile
-        (   ".align 4                                                                   \n\
+		vertices += localCount;  // counter the offset
+		float4 t0, t1, t2, t3, t4;
+		size_t byteIndex = -(localCount) * sizeof(float);
+		float4 *sap = &stack_array[index + localCount / 4];
+
+		asm volatile(
+			".align 4                                                                   \n\
              0: movaps  %[min], %[t2]                            // move min out of the way to avoid propagating NaNs in min \n\
              movaps  (%[vertices], %[byteIndex], 4),    %[t0]    // vertices[0]      \n\
              movaps  16(%[vertices], %[byteIndex], 4),  %[t1]    // vertices[1]      \n\
@@ -703,968 +712,953 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa
              add     $16, %[byteIndex]                           // advance loop counter\n\
              jnz     0b                                          \n\
              "
-         : [min] "+x" (min), [t0] "=&x" (t0), [t1] "=&x" (t1), [t2] "=&x" (t2), [t3] "=&x" (t3), [t4] "=&x" (t4), [byteIndex] "+r" (byteIndex)
-         : [vLo] "x" (vLo), [vHi] "x" (vHi), [vertices] "r" (vertices), [sap] "r" (sap)
-         : "memory", "cc"
-         );
-        index += localCount/4;
+			: [min] "+x"(min), [t0] "=&x"(t0), [t1] "=&x"(t1), [t2] "=&x"(t2), [t3] "=&x"(t3), [t4] "=&x"(t4), [byteIndex] "+r"(byteIndex)
+			: [vLo] "x"(vLo), [vHi] "x"(vHi), [vertices] "r"(vertices), [sap] "r"(sap)
+			: "memory", "cc");
+		index += localCount / 4;
 #else
-        {
-            for( unsigned int i=0; i<localCount/4; i++,index++)   
-            { // do four dot products at a time. Carefully avoid touching the w element.
-                float4 v0 = vertices[0];
-                float4 v1 = vertices[1];
-                float4 v2 = vertices[2];
-                float4 v3 = vertices[3];            
-                vertices += 4;
-                
-                float4 lo0 = _mm_movelh_ps( v0, v1);    // x0y0x1y1
-                float4 hi0 = _mm_movehl_ps( v1, v0);    // z0?0z1?1
-                float4 lo1 = _mm_movelh_ps( v2, v3);    // x2y2x3y3
-                float4 hi1 = _mm_movehl_ps( v3, v2);    // z2?2z3?3
-                
-                lo0 = lo0*vLo;
-                lo1 = lo1*vLo;
-                float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
-                float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
-                float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-                z = z*vHi;
-                x = x+y;
-                x = x+z;
-                stack_array[index] = x;
-                min = _mm_min_ps( x, min );         // control the order here so that max is never NaN even if x is nan
-            }
-        }
+		{
+			for (unsigned int i = 0; i < localCount / 4; i++, index++)
+			{  // do four dot products at a time. Carefully avoid touching the w element.
+				float4 v0 = vertices[0];
+				float4 v1 = vertices[1];
+				float4 v2 = vertices[2];
+				float4 v3 = vertices[3];
+				vertices += 4;
+
+				float4 lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+				float4 hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+				float4 lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+				float4 hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+				lo0 = lo0 * vLo;
+				lo1 = lo1 * vLo;
+				float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
+				float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
+				float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+				z = z * vHi;
+				x = x + y;
+				x = x + z;
+				stack_array[index] = x;
+				min = _mm_min_ps(x, min);  // control the order here so that max is never NaN even if x is nan
+			}
+		}
 
 #endif
-    }
-    
-    // process the last few points
-    if( count & 3 )
-    {
-        float4 v0, v1, v2, x, y, z;
-        switch( count & 3 )
-        {
-            case 3:
-            {
-                v0 = vertices[0];
-                v1 = vertices[1];
-                v2 = vertices[2];
-                
-                // Calculate 3 dot products, transpose, duplicate v2
-                float4 lo0 = _mm_movelh_ps( v0, v1);        // xyxy.lo
-                float4 hi0 = _mm_movehl_ps( v1, v0);        // z?z?.lo
-                lo0 = lo0*vLo;
-                z = _mm_shuffle_ps(hi0, v2,  0xa8 );           // z0z1z2z2
-                z = z*vHi;
-                float4 lo1 = _mm_movelh_ps(v2, v2);          // xyxy
-                lo1 = lo1*vLo;
-                x = _mm_shuffle_ps(lo0, lo1, 0x88);
-                y = _mm_shuffle_ps(lo0, lo1, 0xdd);
-            }
-                break;
-            case 2:
-            {
-                v0 = vertices[0];
-                v1 = vertices[1];
-                float4 xy = _mm_movelh_ps(v0, v1);
-                z = _mm_movehl_ps(v1, v0);
-                xy = xy*vLo;
-                z = _mm_shuffle_ps( z, z,  0xa8);
-                x = _mm_shuffle_ps( xy, xy, 0xa8);
-                y = _mm_shuffle_ps( xy, xy, 0xfd);
-                z = z*vHi;
-            }
-                break;
-            case 1:
-            {
-                float4 xy = vertices[0];
-                z =  _mm_shuffle_ps( xy, xy, 0xaa);
-                xy = xy*vLo;
-                z = z*vHi;
-                x = _mm_shuffle_ps(xy, xy, 0);
-                y = _mm_shuffle_ps(xy, xy, 0x55);
-            }
-                break;
-        }
-        x = x+y;
-        x = x+z;
-        stack_array[index] = x;
-        min = _mm_min_ps( x, min );         // control the order here so that min is never NaN even if x is nan
-        index++;
-    }
-    
-    // if we found a new min. 
-    if( 0 == segment || 0xf != _mm_movemask_ps( (float4) _mm_cmpeq_ps(min, dotmin)))
-    { // we found a new min. Search for it
-      // find min across the min vector, place in all elements of min -- big latency hit here
-        min = _mm_min_ps(min, (float4) _mm_shuffle_ps( min, min, 0x4e));
-        min = _mm_min_ps(min, (float4) _mm_shuffle_ps( min, min, 0xb1));
-        
-        // It is slightly faster to do this part in scalar code when count < 8. However, the common case for
-        // this where it actually makes a difference is handled in the early out at the top of the function, 
-        // so it is less than a 1% difference here. I opted for improved code size, fewer branches and reduced 
-        // complexity, and removed it.
-        
-        dotmin = min;
-        
-        // scan for the first occurence of min in the array  
-        size_t test;
-        for( index = 0; 0 == (test=_mm_movemask_ps( _mm_cmpeq_ps( stack_array[index], min))); index++ )   // local_count must be a multiple of 4
-        {}
-        minIndex = 4*index + segment + indexTable[test];
-    }
-    
-    _mm_store_ss( dotResult, dotmin);
-    return minIndex;
-}
+	}
+
+	// process the last few points
+	if (count & 3)
+	{
+		float4 v0, v1, v2, x, y, z;
+		switch (count & 3)
+		{
+			case 3:
+			{
+				v0 = vertices[0];
+				v1 = vertices[1];
+				v2 = vertices[2];
+
+				// Calculate 3 dot products, transpose, duplicate v2
+				float4 lo0 = _mm_movelh_ps(v0, v1);  // xyxy.lo
+				float4 hi0 = _mm_movehl_ps(v1, v0);  // z?z?.lo
+				lo0 = lo0 * vLo;
+				z = _mm_shuffle_ps(hi0, v2, 0xa8);  // z0z1z2z2
+				z = z * vHi;
+				float4 lo1 = _mm_movelh_ps(v2, v2);  // xyxy
+				lo1 = lo1 * vLo;
+				x = _mm_shuffle_ps(lo0, lo1, 0x88);
+				y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			}
+			break;
+			case 2:
+			{
+				v0 = vertices[0];
+				v1 = vertices[1];
+				float4 xy = _mm_movelh_ps(v0, v1);
+				z = _mm_movehl_ps(v1, v0);
+				xy = xy * vLo;
+				z = _mm_shuffle_ps(z, z, 0xa8);
+				x = _mm_shuffle_ps(xy, xy, 0xa8);
+				y = _mm_shuffle_ps(xy, xy, 0xfd);
+				z = z * vHi;
+			}
+			break;
+			case 1:
+			{
+				float4 xy = vertices[0];
+				z = _mm_shuffle_ps(xy, xy, 0xaa);
+				xy = xy * vLo;
+				z = z * vHi;
+				x = _mm_shuffle_ps(xy, xy, 0);
+				y = _mm_shuffle_ps(xy, xy, 0x55);
+			}
+			break;
+		}
+		x = x + y;
+		x = x + z;
+		stack_array[index] = x;
+		min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+		index++;
+	}
+
+	// if we found a new min.
+	if (0 == segment || 0xf != _mm_movemask_ps((float4)_mm_cmpeq_ps(min, dotmin)))
+	{  // we found a new min. Search for it
+		// find min across the min vector, place in all elements of min -- big latency hit here
+		min = _mm_min_ps(min, (float4)_mm_shuffle_ps(min, min, 0x4e));
+		min = _mm_min_ps(min, (float4)_mm_shuffle_ps(min, min, 0xb1));
+
+		// It is slightly faster to do this part in scalar code when count < 8. However, the common case for
+		// this where it actually makes a difference is handled in the early out at the top of the function,
+		// so it is less than a 1% difference here. I opted for improved code size, fewer branches and reduced
+		// complexity, and removed it.
 
+		dotmin = min;
+
+		// scan for the first occurence of min in the array
+		size_t test;
+		for (index = 0; 0 == (test = _mm_movemask_ps(_mm_cmpeq_ps(stack_array[index], min))); index++)  // local_count must be a multiple of 4
+		{
+		}
+		minIndex = 4 * index + segment + indexTable[test];
+	}
+
+	_mm_store_ss(dotResult, dotmin);
+	return minIndex;
+}
 
 #elif defined BT_USE_NEON
 
-#define ARM_NEON_GCC_COMPATIBILITY  1
+#define ARM_NEON_GCC_COMPATIBILITY 1
 #include <arm_neon.h>
 #include <sys/types.h>
-#include <sys/sysctl.h> //for sysctlbyname
-
-static long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult );
-static long _maxdot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult );
-static long _maxdot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult );
-static long _mindot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult );
-static long _mindot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult );
-static long _mindot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult );
+#include <sys/sysctl.h>  //for sysctlbyname
 
-long (*_maxdot_large)( const float *vv, const float *vec, unsigned long count, float *dotResult ) = _maxdot_large_sel;
-long (*_mindot_large)( const float *vv, const float *vec, unsigned long count, float *dotResult ) = _mindot_large_sel;
+static long _maxdot_large_v0(const float *vv, const float *vec, unsigned long count, float *dotResult);
+static long _maxdot_large_v1(const float *vv, const float *vec, unsigned long count, float *dotResult);
+static long _maxdot_large_sel(const float *vv, const float *vec, unsigned long count, float *dotResult);
+static long _mindot_large_v0(const float *vv, const float *vec, unsigned long count, float *dotResult);
+static long _mindot_large_v1(const float *vv, const float *vec, unsigned long count, float *dotResult);
+static long _mindot_large_sel(const float *vv, const float *vec, unsigned long count, float *dotResult);
 
+long (*_maxdot_large)(const float *vv, const float *vec, unsigned long count, float *dotResult) = _maxdot_large_sel;
+long (*_mindot_large)(const float *vv, const float *vec, unsigned long count, float *dotResult) = _mindot_large_sel;
 
-static inline uint32_t btGetCpuCapabilities( void )
+static inline uint32_t btGetCpuCapabilities(void)
 {
-    static uint32_t capabilities = 0;
-    static bool testedCapabilities = false;
+	static uint32_t capabilities = 0;
+	static bool testedCapabilities = false;
 
-    if( 0 == testedCapabilities)
-    {
-        uint32_t hasFeature = 0;
-        size_t featureSize = sizeof( hasFeature );
-        int err = sysctlbyname( "hw.optional.neon_hpfp", &hasFeature, &featureSize, NULL, 0 );
+	if (0 == testedCapabilities)
+	{
+		uint32_t hasFeature = 0;
+		size_t featureSize = sizeof(hasFeature);
+		int err = sysctlbyname("hw.optional.neon_hpfp", &hasFeature, &featureSize, NULL, 0);
 
-        if( 0 == err && hasFeature)
-            capabilities |= 0x2000;
+		if (0 == err && hasFeature)
+			capabilities |= 0x2000;
 
 		testedCapabilities = true;
-    }
-    
-    return capabilities;
-}
-
-
+	}
 
+	return capabilities;
+}
 
-static long _maxdot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult )
+static long _maxdot_large_sel(const float *vv, const float *vec, unsigned long count, float *dotResult)
 {
+	if (btGetCpuCapabilities() & 0x2000)
+		_maxdot_large = _maxdot_large_v1;
+	else
+		_maxdot_large = _maxdot_large_v0;
 
-    if( btGetCpuCapabilities() & 0x2000 )
-        _maxdot_large = _maxdot_large_v1;
-    else
-        _maxdot_large = _maxdot_large_v0;
-    
-    return _maxdot_large(vv, vec, count, dotResult);
+	return _maxdot_large(vv, vec, count, dotResult);
 }
 
-static long _mindot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult )
+static long _mindot_large_sel(const float *vv, const float *vec, unsigned long count, float *dotResult)
 {
+	if (btGetCpuCapabilities() & 0x2000)
+		_mindot_large = _mindot_large_v1;
+	else
+		_mindot_large = _mindot_large_v0;
 
-    if( btGetCpuCapabilities() & 0x2000 )
-        _mindot_large = _mindot_large_v1;
-    else
-        _mindot_large = _mindot_large_v0;
-    
-    return _mindot_large(vv, vec, count, dotResult);
+	return _mindot_large(vv, vec, count, dotResult);
 }
 
-
-
 #if defined __arm__
-# define vld1q_f32_aligned_postincrement( _ptr ) ({ float32x4_t _r; asm( "vld1.f32 {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; })
+#define vld1q_f32_aligned_postincrement(_ptr) ({ float32x4_t _r; asm( "vld1.f32 {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; })
 #else
 //support 64bit arm
-# define vld1q_f32_aligned_postincrement( _ptr) ({ float32x4_t _r = ((float32x4_t*)(_ptr))[0]; (_ptr) = (const float*) ((const char*)(_ptr) + 16L); /*return*/ _r; })
+#define vld1q_f32_aligned_postincrement(_ptr) ({ float32x4_t _r = ((float32x4_t*)(_ptr))[0]; (_ptr) = (const float*) ((const char*)(_ptr) + 16L); /*return*/ _r; })
 #endif
 
-
-long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult )
+long _maxdot_large_v0(const float *vv, const float *vec, unsigned long count, float *dotResult)
 {
-    unsigned long i = 0;
-    float32x4_t vvec = vld1q_f32_aligned_postincrement( vec );
-    float32x2_t vLo = vget_low_f32(vvec);
-    float32x2_t vHi = vdup_lane_f32(vget_high_f32(vvec), 0);
-    float32x2_t dotMaxLo = (float32x2_t) { -BT_INFINITY, -BT_INFINITY };
-    float32x2_t dotMaxHi = (float32x2_t) { -BT_INFINITY, -BT_INFINITY };
-    uint32x2_t indexLo = (uint32x2_t) {0, 1};
-    uint32x2_t indexHi = (uint32x2_t) {2, 3};
-    uint32x2_t iLo = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
-    uint32x2_t iHi = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
-    const uint32x2_t four = (uint32x2_t) {4,4};
-
-    for( ; i+8 <= count; i+= 8 )
-    {
-        float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
-        
-        float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
-        float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
-        float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo);
-        float32x2_t xy3 = vmul_f32( vget_low_f32(v3), vLo);
-        
-        float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
-        float32x2x2_t z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3));
-        float32x2_t zLo = vmul_f32( z0.val[0], vHi);
-        float32x2_t zHi = vmul_f32( z1.val[0], vHi);
-        
-        float32x2_t rLo = vpadd_f32( xy0, xy1);
-        float32x2_t rHi = vpadd_f32( xy2, xy3);
-        rLo = vadd_f32(rLo, zLo);
-        rHi = vadd_f32(rHi, zHi);
-        
-        uint32x2_t maskLo = vcgt_f32( rLo, dotMaxLo );
-        uint32x2_t maskHi = vcgt_f32( rHi, dotMaxHi );
-        dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo);
-        dotMaxHi = vbsl_f32( maskHi, rHi, dotMaxHi);
-        iLo = vbsl_u32(maskLo, indexLo, iLo);
-        iHi = vbsl_u32(maskHi, indexHi, iHi);
-        indexLo = vadd_u32(indexLo, four); 
-        indexHi = vadd_u32(indexHi, four);
-
-        v0 = vld1q_f32_aligned_postincrement( vv );
-        v1 = vld1q_f32_aligned_postincrement( vv );
-        v2 = vld1q_f32_aligned_postincrement( vv );
-        v3 = vld1q_f32_aligned_postincrement( vv );
-        
-        xy0 = vmul_f32( vget_low_f32(v0), vLo);
-        xy1 = vmul_f32( vget_low_f32(v1), vLo);
-        xy2 = vmul_f32( vget_low_f32(v2), vLo);
-        xy3 = vmul_f32( vget_low_f32(v3), vLo);
-        
-        z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
-        z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3));
-        zLo = vmul_f32( z0.val[0], vHi);
-        zHi = vmul_f32( z1.val[0], vHi);
-        
-        rLo = vpadd_f32( xy0, xy1);
-        rHi = vpadd_f32( xy2, xy3);
-        rLo = vadd_f32(rLo, zLo);
-        rHi = vadd_f32(rHi, zHi);
-        
-        maskLo = vcgt_f32( rLo, dotMaxLo );
-        maskHi = vcgt_f32( rHi, dotMaxHi );
-        dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo);
-        dotMaxHi = vbsl_f32( maskHi, rHi, dotMaxHi);
-        iLo = vbsl_u32(maskLo, indexLo, iLo);
-        iHi = vbsl_u32(maskHi, indexHi, iHi);
-        indexLo = vadd_u32(indexLo, four);
-        indexHi = vadd_u32(indexHi, four);
-    }
-
-    for( ; i+4 <= count; i+= 4 )
-    {
-        float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
-        
-        float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
-        float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
-        float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo);
-        float32x2_t xy3 = vmul_f32( vget_low_f32(v3), vLo);
-        
-        float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
-        float32x2x2_t z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3));
-        float32x2_t zLo = vmul_f32( z0.val[0], vHi);
-        float32x2_t zHi = vmul_f32( z1.val[0], vHi);
-        
-        float32x2_t rLo = vpadd_f32( xy0, xy1);
-        float32x2_t rHi = vpadd_f32( xy2, xy3);
-        rLo = vadd_f32(rLo, zLo);
-        rHi = vadd_f32(rHi, zHi);
-        
-        uint32x2_t maskLo = vcgt_f32( rLo, dotMaxLo );
-        uint32x2_t maskHi = vcgt_f32( rHi, dotMaxHi );
-        dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo);
-        dotMaxHi = vbsl_f32( maskHi, rHi, dotMaxHi);
-        iLo = vbsl_u32(maskLo, indexLo, iLo);
-        iHi = vbsl_u32(maskHi, indexHi, iHi);
-        indexLo = vadd_u32(indexLo, four);
-        indexHi = vadd_u32(indexHi, four);
-    }
-    
-    switch( count & 3 )
-    {
-        case 3:
-        {
-            float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-            float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-            float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
-            
-            float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
-            float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
-            float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo);
-            
-            float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
-            float32x2_t zLo = vmul_f32( z0.val[0], vHi);
-            float32x2_t zHi = vmul_f32( vdup_lane_f32(vget_high_f32(v2), 0), vHi);
-            
-            float32x2_t rLo = vpadd_f32( xy0, xy1);
-            float32x2_t rHi = vpadd_f32( xy2, xy2);
-            rLo = vadd_f32(rLo, zLo);
-            rHi = vadd_f32(rHi, zHi);
-            
-            uint32x2_t maskLo = vcgt_f32( rLo, dotMaxLo );
-            uint32x2_t maskHi = vcgt_f32( rHi, dotMaxHi );
-            dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo);
-            dotMaxHi = vbsl_f32( maskHi, rHi, dotMaxHi);
-            iLo = vbsl_u32(maskLo, indexLo, iLo);
-            iHi = vbsl_u32(maskHi, indexHi, iHi);
-        }
-            break;
-        case 2:
-        {
-            float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-            float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-            
-            float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
-            float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
-            
-            float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
-            float32x2_t zLo = vmul_f32( z0.val[0], vHi);
-            
-            float32x2_t rLo = vpadd_f32( xy0, xy1);
-            rLo = vadd_f32(rLo, zLo);
-            
-            uint32x2_t maskLo = vcgt_f32( rLo, dotMaxLo );
-            dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo);
-            iLo = vbsl_u32(maskLo, indexLo, iLo);
-        }
-            break;
-        case 1:
-        {
-            float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-            float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
-            float32x2_t z0 = vdup_lane_f32(vget_high_f32(v0), 0);
-            float32x2_t zLo = vmul_f32( z0, vHi);
-            float32x2_t rLo = vpadd_f32( xy0, xy0);
-            rLo = vadd_f32(rLo, zLo);
-            uint32x2_t maskLo = vcgt_f32( rLo, dotMaxLo );
-            dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo);
-            iLo = vbsl_u32(maskLo, indexLo, iLo);
-        }
-            break;
-        
-        default:
-            break;
-    }
-    
-    // select best answer between hi and lo results
-    uint32x2_t mask = vcgt_f32( dotMaxHi, dotMaxLo );
-    dotMaxLo = vbsl_f32(mask, dotMaxHi, dotMaxLo);
-    iLo = vbsl_u32(mask, iHi, iLo);
-    
-    // select best answer between even and odd results
-    dotMaxHi = vdup_lane_f32(dotMaxLo, 1);
-    iHi = vdup_lane_u32(iLo, 1);
-    mask = vcgt_f32( dotMaxHi, dotMaxLo );
-    dotMaxLo = vbsl_f32(mask, dotMaxHi, dotMaxLo);
-    iLo = vbsl_u32(mask, iHi, iLo);
-    
-    *dotResult = vget_lane_f32( dotMaxLo, 0);
-    return vget_lane_u32(iLo, 0);
-}
+	unsigned long i = 0;
+	float32x4_t vvec = vld1q_f32_aligned_postincrement(vec);
+	float32x2_t vLo = vget_low_f32(vvec);
+	float32x2_t vHi = vdup_lane_f32(vget_high_f32(vvec), 0);
+	float32x2_t dotMaxLo = (float32x2_t){-BT_INFINITY, -BT_INFINITY};
+	float32x2_t dotMaxHi = (float32x2_t){-BT_INFINITY, -BT_INFINITY};
+	uint32x2_t indexLo = (uint32x2_t){0, 1};
+	uint32x2_t indexHi = (uint32x2_t){2, 3};
+	uint32x2_t iLo = (uint32x2_t){static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
+	uint32x2_t iHi = (uint32x2_t){static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
+	const uint32x2_t four = (uint32x2_t){4, 4};
+
+	for (; i + 8 <= count; i += 8)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+		float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+		float32x2_t xy2 = vmul_f32(vget_low_f32(v2), vLo);
+		float32x2_t xy3 = vmul_f32(vget_low_f32(v3), vLo);
+
+		float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x2x2_t z1 = vtrn_f32(vget_high_f32(v2), vget_high_f32(v3));
+		float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+		float32x2_t zHi = vmul_f32(z1.val[0], vHi);
+
+		float32x2_t rLo = vpadd_f32(xy0, xy1);
+		float32x2_t rHi = vpadd_f32(xy2, xy3);
+		rLo = vadd_f32(rLo, zLo);
+		rHi = vadd_f32(rHi, zHi);
+
+		uint32x2_t maskLo = vcgt_f32(rLo, dotMaxLo);
+		uint32x2_t maskHi = vcgt_f32(rHi, dotMaxHi);
+		dotMaxLo = vbsl_f32(maskLo, rLo, dotMaxLo);
+		dotMaxHi = vbsl_f32(maskHi, rHi, dotMaxHi);
+		iLo = vbsl_u32(maskLo, indexLo, iLo);
+		iHi = vbsl_u32(maskHi, indexHi, iHi);
+		indexLo = vadd_u32(indexLo, four);
+		indexHi = vadd_u32(indexHi, four);
+
+		v0 = vld1q_f32_aligned_postincrement(vv);
+		v1 = vld1q_f32_aligned_postincrement(vv);
+		v2 = vld1q_f32_aligned_postincrement(vv);
+		v3 = vld1q_f32_aligned_postincrement(vv);
+
+		xy0 = vmul_f32(vget_low_f32(v0), vLo);
+		xy1 = vmul_f32(vget_low_f32(v1), vLo);
+		xy2 = vmul_f32(vget_low_f32(v2), vLo);
+		xy3 = vmul_f32(vget_low_f32(v3), vLo);
+
+		z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+		z1 = vtrn_f32(vget_high_f32(v2), vget_high_f32(v3));
+		zLo = vmul_f32(z0.val[0], vHi);
+		zHi = vmul_f32(z1.val[0], vHi);
+
+		rLo = vpadd_f32(xy0, xy1);
+		rHi = vpadd_f32(xy2, xy3);
+		rLo = vadd_f32(rLo, zLo);
+		rHi = vadd_f32(rHi, zHi);
 
+		maskLo = vcgt_f32(rLo, dotMaxLo);
+		maskHi = vcgt_f32(rHi, dotMaxHi);
+		dotMaxLo = vbsl_f32(maskLo, rLo, dotMaxLo);
+		dotMaxHi = vbsl_f32(maskHi, rHi, dotMaxHi);
+		iLo = vbsl_u32(maskLo, indexLo, iLo);
+		iHi = vbsl_u32(maskHi, indexHi, iHi);
+		indexLo = vadd_u32(indexLo, four);
+		indexHi = vadd_u32(indexHi, four);
+	}
 
-long _maxdot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult )
+	for (; i + 4 <= count; i += 4)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+		float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+		float32x2_t xy2 = vmul_f32(vget_low_f32(v2), vLo);
+		float32x2_t xy3 = vmul_f32(vget_low_f32(v3), vLo);
+
+		float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x2x2_t z1 = vtrn_f32(vget_high_f32(v2), vget_high_f32(v3));
+		float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+		float32x2_t zHi = vmul_f32(z1.val[0], vHi);
+
+		float32x2_t rLo = vpadd_f32(xy0, xy1);
+		float32x2_t rHi = vpadd_f32(xy2, xy3);
+		rLo = vadd_f32(rLo, zLo);
+		rHi = vadd_f32(rHi, zHi);
+
+		uint32x2_t maskLo = vcgt_f32(rLo, dotMaxLo);
+		uint32x2_t maskHi = vcgt_f32(rHi, dotMaxHi);
+		dotMaxLo = vbsl_f32(maskLo, rLo, dotMaxLo);
+		dotMaxHi = vbsl_f32(maskHi, rHi, dotMaxHi);
+		iLo = vbsl_u32(maskLo, indexLo, iLo);
+		iHi = vbsl_u32(maskHi, indexHi, iHi);
+		indexLo = vadd_u32(indexLo, four);
+		indexHi = vadd_u32(indexHi, four);
+	}
+
+	switch (count & 3)
+	{
+		case 3:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+
+			float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+			float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+			float32x2_t xy2 = vmul_f32(vget_low_f32(v2), vLo);
+
+			float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+			float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+			float32x2_t zHi = vmul_f32(vdup_lane_f32(vget_high_f32(v2), 0), vHi);
+
+			float32x2_t rLo = vpadd_f32(xy0, xy1);
+			float32x2_t rHi = vpadd_f32(xy2, xy2);
+			rLo = vadd_f32(rLo, zLo);
+			rHi = vadd_f32(rHi, zHi);
+
+			uint32x2_t maskLo = vcgt_f32(rLo, dotMaxLo);
+			uint32x2_t maskHi = vcgt_f32(rHi, dotMaxHi);
+			dotMaxLo = vbsl_f32(maskLo, rLo, dotMaxLo);
+			dotMaxHi = vbsl_f32(maskHi, rHi, dotMaxHi);
+			iLo = vbsl_u32(maskLo, indexLo, iLo);
+			iHi = vbsl_u32(maskHi, indexHi, iHi);
+		}
+		break;
+		case 2:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+
+			float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+			float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+
+			float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+			float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+
+			float32x2_t rLo = vpadd_f32(xy0, xy1);
+			rLo = vadd_f32(rLo, zLo);
+
+			uint32x2_t maskLo = vcgt_f32(rLo, dotMaxLo);
+			dotMaxLo = vbsl_f32(maskLo, rLo, dotMaxLo);
+			iLo = vbsl_u32(maskLo, indexLo, iLo);
+		}
+		break;
+		case 1:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+			float32x2_t z0 = vdup_lane_f32(vget_high_f32(v0), 0);
+			float32x2_t zLo = vmul_f32(z0, vHi);
+			float32x2_t rLo = vpadd_f32(xy0, xy0);
+			rLo = vadd_f32(rLo, zLo);
+			uint32x2_t maskLo = vcgt_f32(rLo, dotMaxLo);
+			dotMaxLo = vbsl_f32(maskLo, rLo, dotMaxLo);
+			iLo = vbsl_u32(maskLo, indexLo, iLo);
+		}
+		break;
+
+		default:
+			break;
+	}
+
+	// select best answer between hi and lo results
+	uint32x2_t mask = vcgt_f32(dotMaxHi, dotMaxLo);
+	dotMaxLo = vbsl_f32(mask, dotMaxHi, dotMaxLo);
+	iLo = vbsl_u32(mask, iHi, iLo);
+
+	// select best answer between even and odd results
+	dotMaxHi = vdup_lane_f32(dotMaxLo, 1);
+	iHi = vdup_lane_u32(iLo, 1);
+	mask = vcgt_f32(dotMaxHi, dotMaxLo);
+	dotMaxLo = vbsl_f32(mask, dotMaxHi, dotMaxLo);
+	iLo = vbsl_u32(mask, iHi, iLo);
+
+	*dotResult = vget_lane_f32(dotMaxLo, 0);
+	return vget_lane_u32(iLo, 0);
+}
+
+long _maxdot_large_v1(const float *vv, const float *vec, unsigned long count, float *dotResult)
 {
-    float32x4_t vvec = vld1q_f32_aligned_postincrement( vec );
-    float32x4_t vLo = vcombine_f32(vget_low_f32(vvec), vget_low_f32(vvec));
-    float32x4_t vHi = vdupq_lane_f32(vget_high_f32(vvec), 0);
-    const uint32x4_t four = (uint32x4_t){ 4, 4, 4, 4 };
-    uint32x4_t local_index = (uint32x4_t) {0, 1, 2, 3};
-    uint32x4_t index = (uint32x4_t) { static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1) };
-    float32x4_t maxDot = (float32x4_t) { -BT_INFINITY, -BT_INFINITY, -BT_INFINITY, -BT_INFINITY };
-    
-    unsigned long i = 0;
-    for( ; i + 8 <= count; i += 8 )
-    {
-        float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
-        
-        // the next two lines should resolve to a single vswp d, d
-        float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
-        float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3));
-        // the next two lines should resolve to a single vswp d, d
-        float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
-        float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3));
-        
-        xy0 = vmulq_f32(xy0, vLo);
-        xy1 = vmulq_f32(xy1, vLo);
-        
-        float32x4x2_t zb = vuzpq_f32( z0, z1);
-        float32x4_t z = vmulq_f32( zb.val[0], vHi);
-        float32x4x2_t xy = vuzpq_f32( xy0, xy1);
-        float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
-        x = vaddq_f32(x, z);
-        
-        uint32x4_t mask = vcgtq_f32(x, maxDot);
-        maxDot = vbslq_f32( mask, x, maxDot);
-        index = vbslq_u32(mask, local_index, index);
-        local_index = vaddq_u32(local_index, four);
-
-        v0 = vld1q_f32_aligned_postincrement( vv );
-        v1 = vld1q_f32_aligned_postincrement( vv );
-        v2 = vld1q_f32_aligned_postincrement( vv );
-        v3 = vld1q_f32_aligned_postincrement( vv );
-        
-        // the next two lines should resolve to a single vswp d, d
-        xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
-        xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3));
-        // the next two lines should resolve to a single vswp d, d
-        z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
-        z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3));
-        
-        xy0 = vmulq_f32(xy0, vLo);
-        xy1 = vmulq_f32(xy1, vLo);
-        
-        zb = vuzpq_f32( z0, z1);
-        z = vmulq_f32( zb.val[0], vHi);
-        xy = vuzpq_f32( xy0, xy1);
-        x = vaddq_f32(xy.val[0], xy.val[1]);
-        x = vaddq_f32(x, z);
-        
-        mask = vcgtq_f32(x, maxDot);
-        maxDot = vbslq_f32( mask, x, maxDot);
-        index = vbslq_u32(mask, local_index, index);
-        local_index = vaddq_u32(local_index, four);
-    }
-
-    for( ; i + 4 <= count; i += 4 )
-    {
-        float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
-
-        // the next two lines should resolve to a single vswp d, d
-        float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
-        float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3));
-        // the next two lines should resolve to a single vswp d, d
-        float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
-        float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3));
-        
-        xy0 = vmulq_f32(xy0, vLo);
-        xy1 = vmulq_f32(xy1, vLo);
-        
-        float32x4x2_t zb = vuzpq_f32( z0, z1);
-        float32x4_t z = vmulq_f32( zb.val[0], vHi);
-        float32x4x2_t xy = vuzpq_f32( xy0, xy1);
-        float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
-        x = vaddq_f32(x, z);
-        
-        uint32x4_t mask = vcgtq_f32(x, maxDot);
-        maxDot = vbslq_f32( mask, x, maxDot);
-        index = vbslq_u32(mask, local_index, index);
-        local_index = vaddq_u32(local_index, four);
-    }
-    
-    switch (count & 3) {
-        case 3:
-        {
-            float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-            float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-            float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
-            
-            // the next two lines should resolve to a single vswp d, d
-            float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
-            float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v2));
-            // the next two lines should resolve to a single vswp d, d
-            float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
-            float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v2));
-            
-            xy0 = vmulq_f32(xy0, vLo);
-            xy1 = vmulq_f32(xy1, vLo);
-            
-            float32x4x2_t zb = vuzpq_f32( z0, z1);
-            float32x4_t z = vmulq_f32( zb.val[0], vHi);
-            float32x4x2_t xy = vuzpq_f32( xy0, xy1);
-            float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
-            x = vaddq_f32(x, z);
-            
-            uint32x4_t mask = vcgtq_f32(x, maxDot);
-            maxDot = vbslq_f32( mask, x, maxDot);
-            index = vbslq_u32(mask, local_index, index);
-            local_index = vaddq_u32(local_index, four);
-        }
-            break;
-
-        case 2:
-        {
-            float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-            float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-            
-            // the next two lines should resolve to a single vswp d, d
-            float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
-            // the next two lines should resolve to a single vswp d, d
-            float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
-            
-            xy0 = vmulq_f32(xy0, vLo);
-            
-            float32x4x2_t zb = vuzpq_f32( z0, z0);
-            float32x4_t z = vmulq_f32( zb.val[0], vHi);
-            float32x4x2_t xy = vuzpq_f32( xy0, xy0);
-            float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
-            x = vaddq_f32(x, z);
-            
-            uint32x4_t mask = vcgtq_f32(x, maxDot);
-            maxDot = vbslq_f32( mask, x, maxDot);
-            index = vbslq_u32(mask, local_index, index);
-            local_index = vaddq_u32(local_index, four);
-        }
-            break;
-
-        case 1:
-        {
-            float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-            
-            // the next two lines should resolve to a single vswp d, d
-            float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v0));
-            // the next two lines should resolve to a single vswp d, d
-            float32x4_t z = vdupq_lane_f32(vget_high_f32(v0), 0); 
-            
-            xy0 = vmulq_f32(xy0, vLo);
-            
-            z = vmulq_f32( z, vHi);
-            float32x4x2_t xy = vuzpq_f32( xy0, xy0);
-            float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
-            x = vaddq_f32(x, z);
-            
-            uint32x4_t mask = vcgtq_f32(x, maxDot);
-            maxDot = vbslq_f32( mask, x, maxDot);
-            index = vbslq_u32(mask, local_index, index);
-            local_index = vaddq_u32(local_index, four);
-        }
-            break;
-
-        default:
-            break;
-    }
-    
-    
-    // select best answer between hi and lo results
-    uint32x2_t mask = vcgt_f32( vget_high_f32(maxDot), vget_low_f32(maxDot));
-    float32x2_t maxDot2 = vbsl_f32(mask, vget_high_f32(maxDot), vget_low_f32(maxDot));
-    uint32x2_t index2 = vbsl_u32(mask, vget_high_u32(index), vget_low_u32(index));
-    
-    // select best answer between even and odd results
-    float32x2_t maxDotO = vdup_lane_f32(maxDot2, 1);
-    uint32x2_t indexHi = vdup_lane_u32(index2, 1);
-    mask = vcgt_f32( maxDotO, maxDot2 );
-    maxDot2 = vbsl_f32(mask, maxDotO, maxDot2);
-    index2 = vbsl_u32(mask, indexHi, index2);
-    
-    *dotResult = vget_lane_f32( maxDot2, 0);
-    return vget_lane_u32(index2, 0);
-    
+	float32x4_t vvec = vld1q_f32_aligned_postincrement(vec);
+	float32x4_t vLo = vcombine_f32(vget_low_f32(vvec), vget_low_f32(vvec));
+	float32x4_t vHi = vdupq_lane_f32(vget_high_f32(vvec), 0);
+	const uint32x4_t four = (uint32x4_t){4, 4, 4, 4};
+	uint32x4_t local_index = (uint32x4_t){0, 1, 2, 3};
+	uint32x4_t index = (uint32x4_t){static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
+	float32x4_t maxDot = (float32x4_t){-BT_INFINITY, -BT_INFINITY, -BT_INFINITY, -BT_INFINITY};
+
+	unsigned long i = 0;
+	for (; i + 8 <= count; i += 8)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+		float32x4_t xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v3));
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x4_t z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v3));
+
+		xy0 = vmulq_f32(xy0, vLo);
+		xy1 = vmulq_f32(xy1, vLo);
+
+		float32x4x2_t zb = vuzpq_f32(z0, z1);
+		float32x4_t z = vmulq_f32(zb.val[0], vHi);
+		float32x4x2_t xy = vuzpq_f32(xy0, xy1);
+		float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+		x = vaddq_f32(x, z);
+
+		uint32x4_t mask = vcgtq_f32(x, maxDot);
+		maxDot = vbslq_f32(mask, x, maxDot);
+		index = vbslq_u32(mask, local_index, index);
+		local_index = vaddq_u32(local_index, four);
+
+		v0 = vld1q_f32_aligned_postincrement(vv);
+		v1 = vld1q_f32_aligned_postincrement(vv);
+		v2 = vld1q_f32_aligned_postincrement(vv);
+		v3 = vld1q_f32_aligned_postincrement(vv);
+
+		// the next two lines should resolve to a single vswp d, d
+		xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+		xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v3));
+		// the next two lines should resolve to a single vswp d, d
+		z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+		z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v3));
+
+		xy0 = vmulq_f32(xy0, vLo);
+		xy1 = vmulq_f32(xy1, vLo);
+
+		zb = vuzpq_f32(z0, z1);
+		z = vmulq_f32(zb.val[0], vHi);
+		xy = vuzpq_f32(xy0, xy1);
+		x = vaddq_f32(xy.val[0], xy.val[1]);
+		x = vaddq_f32(x, z);
+
+		mask = vcgtq_f32(x, maxDot);
+		maxDot = vbslq_f32(mask, x, maxDot);
+		index = vbslq_u32(mask, local_index, index);
+		local_index = vaddq_u32(local_index, four);
+	}
+
+	for (; i + 4 <= count; i += 4)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+		float32x4_t xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v3));
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x4_t z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v3));
+
+		xy0 = vmulq_f32(xy0, vLo);
+		xy1 = vmulq_f32(xy1, vLo);
+
+		float32x4x2_t zb = vuzpq_f32(z0, z1);
+		float32x4_t z = vmulq_f32(zb.val[0], vHi);
+		float32x4x2_t xy = vuzpq_f32(xy0, xy1);
+		float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+		x = vaddq_f32(x, z);
+
+		uint32x4_t mask = vcgtq_f32(x, maxDot);
+		maxDot = vbslq_f32(mask, x, maxDot);
+		index = vbslq_u32(mask, local_index, index);
+		local_index = vaddq_u32(local_index, four);
+	}
+
+	switch (count & 3)
+	{
+		case 3:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+			float32x4_t xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v2));
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+			float32x4_t z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v2));
+
+			xy0 = vmulq_f32(xy0, vLo);
+			xy1 = vmulq_f32(xy1, vLo);
+
+			float32x4x2_t zb = vuzpq_f32(z0, z1);
+			float32x4_t z = vmulq_f32(zb.val[0], vHi);
+			float32x4x2_t xy = vuzpq_f32(xy0, xy1);
+			float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+			x = vaddq_f32(x, z);
+
+			uint32x4_t mask = vcgtq_f32(x, maxDot);
+			maxDot = vbslq_f32(mask, x, maxDot);
+			index = vbslq_u32(mask, local_index, index);
+			local_index = vaddq_u32(local_index, four);
+		}
+		break;
+
+		case 2:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+
+			xy0 = vmulq_f32(xy0, vLo);
+
+			float32x4x2_t zb = vuzpq_f32(z0, z0);
+			float32x4_t z = vmulq_f32(zb.val[0], vHi);
+			float32x4x2_t xy = vuzpq_f32(xy0, xy0);
+			float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+			x = vaddq_f32(x, z);
+
+			uint32x4_t mask = vcgtq_f32(x, maxDot);
+			maxDot = vbslq_f32(mask, x, maxDot);
+			index = vbslq_u32(mask, local_index, index);
+			local_index = vaddq_u32(local_index, four);
+		}
+		break;
+
+		case 1:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v0));
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t z = vdupq_lane_f32(vget_high_f32(v0), 0);
+
+			xy0 = vmulq_f32(xy0, vLo);
+
+			z = vmulq_f32(z, vHi);
+			float32x4x2_t xy = vuzpq_f32(xy0, xy0);
+			float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+			x = vaddq_f32(x, z);
+
+			uint32x4_t mask = vcgtq_f32(x, maxDot);
+			maxDot = vbslq_f32(mask, x, maxDot);
+			index = vbslq_u32(mask, local_index, index);
+			local_index = vaddq_u32(local_index, four);
+		}
+		break;
+
+		default:
+			break;
+	}
+
+	// select best answer between hi and lo results
+	uint32x2_t mask = vcgt_f32(vget_high_f32(maxDot), vget_low_f32(maxDot));
+	float32x2_t maxDot2 = vbsl_f32(mask, vget_high_f32(maxDot), vget_low_f32(maxDot));
+	uint32x2_t index2 = vbsl_u32(mask, vget_high_u32(index), vget_low_u32(index));
+
+	// select best answer between even and odd results
+	float32x2_t maxDotO = vdup_lane_f32(maxDot2, 1);
+	uint32x2_t indexHi = vdup_lane_u32(index2, 1);
+	mask = vcgt_f32(maxDotO, maxDot2);
+	maxDot2 = vbsl_f32(mask, maxDotO, maxDot2);
+	index2 = vbsl_u32(mask, indexHi, index2);
+
+	*dotResult = vget_lane_f32(maxDot2, 0);
+	return vget_lane_u32(index2, 0);
 }
 
-long _mindot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult )
+long _mindot_large_v0(const float *vv, const float *vec, unsigned long count, float *dotResult)
 {
-    unsigned long i = 0;
-    float32x4_t vvec = vld1q_f32_aligned_postincrement( vec );
-    float32x2_t vLo = vget_low_f32(vvec);
-    float32x2_t vHi = vdup_lane_f32(vget_high_f32(vvec), 0);
-    float32x2_t dotMinLo = (float32x2_t) { BT_INFINITY, BT_INFINITY };
-    float32x2_t dotMinHi = (float32x2_t) { BT_INFINITY, BT_INFINITY };
-    uint32x2_t indexLo = (uint32x2_t) {0, 1};
-    uint32x2_t indexHi = (uint32x2_t) {2, 3};
-    uint32x2_t iLo = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
-    uint32x2_t iHi = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
-    const uint32x2_t four = (uint32x2_t) {4,4};
-    
-    for( ; i+8 <= count; i+= 8 )
-    {
-        float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
-        
-        float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
-        float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
-        float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo);
-        float32x2_t xy3 = vmul_f32( vget_low_f32(v3), vLo);
-        
-        float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
-        float32x2x2_t z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3));
-        float32x2_t zLo = vmul_f32( z0.val[0], vHi);
-        float32x2_t zHi = vmul_f32( z1.val[0], vHi);
-        
-        float32x2_t rLo = vpadd_f32( xy0, xy1);
-        float32x2_t rHi = vpadd_f32( xy2, xy3);
-        rLo = vadd_f32(rLo, zLo);
-        rHi = vadd_f32(rHi, zHi);
-        
-        uint32x2_t maskLo = vclt_f32( rLo, dotMinLo );
-        uint32x2_t maskHi = vclt_f32( rHi, dotMinHi );
-        dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo);
-        dotMinHi = vbsl_f32( maskHi, rHi, dotMinHi);
-        iLo = vbsl_u32(maskLo, indexLo, iLo);
-        iHi = vbsl_u32(maskHi, indexHi, iHi);
-        indexLo = vadd_u32(indexLo, four);
-        indexHi = vadd_u32(indexHi, four);
-        
-        v0 = vld1q_f32_aligned_postincrement( vv );
-        v1 = vld1q_f32_aligned_postincrement( vv );
-        v2 = vld1q_f32_aligned_postincrement( vv );
-        v3 = vld1q_f32_aligned_postincrement( vv );
-        
-        xy0 = vmul_f32( vget_low_f32(v0), vLo);
-        xy1 = vmul_f32( vget_low_f32(v1), vLo);
-        xy2 = vmul_f32( vget_low_f32(v2), vLo);
-        xy3 = vmul_f32( vget_low_f32(v3), vLo);
-        
-        z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
-        z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3));
-        zLo = vmul_f32( z0.val[0], vHi);
-        zHi = vmul_f32( z1.val[0], vHi);
-        
-        rLo = vpadd_f32( xy0, xy1);
-        rHi = vpadd_f32( xy2, xy3);
-        rLo = vadd_f32(rLo, zLo);
-        rHi = vadd_f32(rHi, zHi);
-        
-        maskLo = vclt_f32( rLo, dotMinLo );
-        maskHi = vclt_f32( rHi, dotMinHi );
-        dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo);
-        dotMinHi = vbsl_f32( maskHi, rHi, dotMinHi);
-        iLo = vbsl_u32(maskLo, indexLo, iLo);
-        iHi = vbsl_u32(maskHi, indexHi, iHi);
-        indexLo = vadd_u32(indexLo, four);
-        indexHi = vadd_u32(indexHi, four);
-    }
-
-    for( ; i+4 <= count; i+= 4 )
-    {
-        float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
-        
-        float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
-        float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
-        float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo);
-        float32x2_t xy3 = vmul_f32( vget_low_f32(v3), vLo);
-        
-        float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
-        float32x2x2_t z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3));
-        float32x2_t zLo = vmul_f32( z0.val[0], vHi);
-        float32x2_t zHi = vmul_f32( z1.val[0], vHi);
-        
-        float32x2_t rLo = vpadd_f32( xy0, xy1);
-        float32x2_t rHi = vpadd_f32( xy2, xy3);
-        rLo = vadd_f32(rLo, zLo);
-        rHi = vadd_f32(rHi, zHi);
-        
-        uint32x2_t maskLo = vclt_f32( rLo, dotMinLo );
-        uint32x2_t maskHi = vclt_f32( rHi, dotMinHi );
-        dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo);
-        dotMinHi = vbsl_f32( maskHi, rHi, dotMinHi);
-        iLo = vbsl_u32(maskLo, indexLo, iLo);
-        iHi = vbsl_u32(maskHi, indexHi, iHi);
-        indexLo = vadd_u32(indexLo, four);
-        indexHi = vadd_u32(indexHi, four);
-    }
-    switch( count & 3 )
-    {
-        case 3:
-        {
-            float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-            float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-            float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
-            
-            float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
-            float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
-            float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo);
-            
-            float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
-            float32x2_t zLo = vmul_f32( z0.val[0], vHi);
-            float32x2_t zHi = vmul_f32( vdup_lane_f32(vget_high_f32(v2), 0), vHi);
-            
-            float32x2_t rLo = vpadd_f32( xy0, xy1);
-            float32x2_t rHi = vpadd_f32( xy2, xy2);
-            rLo = vadd_f32(rLo, zLo);
-            rHi = vadd_f32(rHi, zHi);
-            
-            uint32x2_t maskLo = vclt_f32( rLo, dotMinLo );
-            uint32x2_t maskHi = vclt_f32( rHi, dotMinHi );
-            dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo);
-            dotMinHi = vbsl_f32( maskHi, rHi, dotMinHi);
-            iLo = vbsl_u32(maskLo, indexLo, iLo);
-            iHi = vbsl_u32(maskHi, indexHi, iHi);
-        }
-            break;
-        case 2:
-        {
-            float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-            float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-            
-            float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
-            float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
-            
-            float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
-            float32x2_t zLo = vmul_f32( z0.val[0], vHi);
-            
-            float32x2_t rLo = vpadd_f32( xy0, xy1);
-            rLo = vadd_f32(rLo, zLo);
-            
-            uint32x2_t maskLo = vclt_f32( rLo, dotMinLo );
-            dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo);
-            iLo = vbsl_u32(maskLo, indexLo, iLo);
-        }
-            break;
-        case 1:
-        {
-            float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-            float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
-            float32x2_t z0 = vdup_lane_f32(vget_high_f32(v0), 0);
-            float32x2_t zLo = vmul_f32( z0, vHi);
-            float32x2_t rLo = vpadd_f32( xy0, xy0);
-            rLo = vadd_f32(rLo, zLo);
-            uint32x2_t maskLo = vclt_f32( rLo, dotMinLo );
-            dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo);
-            iLo = vbsl_u32(maskLo, indexLo, iLo);
-        }
-            break;
-            
-        default:
-            break;
-    }
-    
-    // select best answer between hi and lo results
-    uint32x2_t mask = vclt_f32( dotMinHi, dotMinLo );
-    dotMinLo = vbsl_f32(mask, dotMinHi, dotMinLo);
-    iLo = vbsl_u32(mask, iHi, iLo);
-    
-    // select best answer between even and odd results
-    dotMinHi = vdup_lane_f32(dotMinLo, 1);
-    iHi = vdup_lane_u32(iLo, 1);
-    mask = vclt_f32( dotMinHi, dotMinLo );
-    dotMinLo = vbsl_f32(mask, dotMinHi, dotMinLo);
-    iLo = vbsl_u32(mask, iHi, iLo);
-    
-    *dotResult = vget_lane_f32( dotMinLo, 0);
-    return vget_lane_u32(iLo, 0);
+	unsigned long i = 0;
+	float32x4_t vvec = vld1q_f32_aligned_postincrement(vec);
+	float32x2_t vLo = vget_low_f32(vvec);
+	float32x2_t vHi = vdup_lane_f32(vget_high_f32(vvec), 0);
+	float32x2_t dotMinLo = (float32x2_t){BT_INFINITY, BT_INFINITY};
+	float32x2_t dotMinHi = (float32x2_t){BT_INFINITY, BT_INFINITY};
+	uint32x2_t indexLo = (uint32x2_t){0, 1};
+	uint32x2_t indexHi = (uint32x2_t){2, 3};
+	uint32x2_t iLo = (uint32x2_t){static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
+	uint32x2_t iHi = (uint32x2_t){static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
+	const uint32x2_t four = (uint32x2_t){4, 4};
+
+	for (; i + 8 <= count; i += 8)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+		float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+		float32x2_t xy2 = vmul_f32(vget_low_f32(v2), vLo);
+		float32x2_t xy3 = vmul_f32(vget_low_f32(v3), vLo);
+
+		float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x2x2_t z1 = vtrn_f32(vget_high_f32(v2), vget_high_f32(v3));
+		float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+		float32x2_t zHi = vmul_f32(z1.val[0], vHi);
+
+		float32x2_t rLo = vpadd_f32(xy0, xy1);
+		float32x2_t rHi = vpadd_f32(xy2, xy3);
+		rLo = vadd_f32(rLo, zLo);
+		rHi = vadd_f32(rHi, zHi);
+
+		uint32x2_t maskLo = vclt_f32(rLo, dotMinLo);
+		uint32x2_t maskHi = vclt_f32(rHi, dotMinHi);
+		dotMinLo = vbsl_f32(maskLo, rLo, dotMinLo);
+		dotMinHi = vbsl_f32(maskHi, rHi, dotMinHi);
+		iLo = vbsl_u32(maskLo, indexLo, iLo);
+		iHi = vbsl_u32(maskHi, indexHi, iHi);
+		indexLo = vadd_u32(indexLo, four);
+		indexHi = vadd_u32(indexHi, four);
+
+		v0 = vld1q_f32_aligned_postincrement(vv);
+		v1 = vld1q_f32_aligned_postincrement(vv);
+		v2 = vld1q_f32_aligned_postincrement(vv);
+		v3 = vld1q_f32_aligned_postincrement(vv);
+
+		xy0 = vmul_f32(vget_low_f32(v0), vLo);
+		xy1 = vmul_f32(vget_low_f32(v1), vLo);
+		xy2 = vmul_f32(vget_low_f32(v2), vLo);
+		xy3 = vmul_f32(vget_low_f32(v3), vLo);
+
+		z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+		z1 = vtrn_f32(vget_high_f32(v2), vget_high_f32(v3));
+		zLo = vmul_f32(z0.val[0], vHi);
+		zHi = vmul_f32(z1.val[0], vHi);
+
+		rLo = vpadd_f32(xy0, xy1);
+		rHi = vpadd_f32(xy2, xy3);
+		rLo = vadd_f32(rLo, zLo);
+		rHi = vadd_f32(rHi, zHi);
+
+		maskLo = vclt_f32(rLo, dotMinLo);
+		maskHi = vclt_f32(rHi, dotMinHi);
+		dotMinLo = vbsl_f32(maskLo, rLo, dotMinLo);
+		dotMinHi = vbsl_f32(maskHi, rHi, dotMinHi);
+		iLo = vbsl_u32(maskLo, indexLo, iLo);
+		iHi = vbsl_u32(maskHi, indexHi, iHi);
+		indexLo = vadd_u32(indexLo, four);
+		indexHi = vadd_u32(indexHi, four);
+	}
+
+	for (; i + 4 <= count; i += 4)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+		float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+		float32x2_t xy2 = vmul_f32(vget_low_f32(v2), vLo);
+		float32x2_t xy3 = vmul_f32(vget_low_f32(v3), vLo);
+
+		float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x2x2_t z1 = vtrn_f32(vget_high_f32(v2), vget_high_f32(v3));
+		float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+		float32x2_t zHi = vmul_f32(z1.val[0], vHi);
+
+		float32x2_t rLo = vpadd_f32(xy0, xy1);
+		float32x2_t rHi = vpadd_f32(xy2, xy3);
+		rLo = vadd_f32(rLo, zLo);
+		rHi = vadd_f32(rHi, zHi);
+
+		uint32x2_t maskLo = vclt_f32(rLo, dotMinLo);
+		uint32x2_t maskHi = vclt_f32(rHi, dotMinHi);
+		dotMinLo = vbsl_f32(maskLo, rLo, dotMinLo);
+		dotMinHi = vbsl_f32(maskHi, rHi, dotMinHi);
+		iLo = vbsl_u32(maskLo, indexLo, iLo);
+		iHi = vbsl_u32(maskHi, indexHi, iHi);
+		indexLo = vadd_u32(indexLo, four);
+		indexHi = vadd_u32(indexHi, four);
+	}
+	switch (count & 3)
+	{
+		case 3:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+
+			float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+			float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+			float32x2_t xy2 = vmul_f32(vget_low_f32(v2), vLo);
+
+			float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+			float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+			float32x2_t zHi = vmul_f32(vdup_lane_f32(vget_high_f32(v2), 0), vHi);
+
+			float32x2_t rLo = vpadd_f32(xy0, xy1);
+			float32x2_t rHi = vpadd_f32(xy2, xy2);
+			rLo = vadd_f32(rLo, zLo);
+			rHi = vadd_f32(rHi, zHi);
+
+			uint32x2_t maskLo = vclt_f32(rLo, dotMinLo);
+			uint32x2_t maskHi = vclt_f32(rHi, dotMinHi);
+			dotMinLo = vbsl_f32(maskLo, rLo, dotMinLo);
+			dotMinHi = vbsl_f32(maskHi, rHi, dotMinHi);
+			iLo = vbsl_u32(maskLo, indexLo, iLo);
+			iHi = vbsl_u32(maskHi, indexHi, iHi);
+		}
+		break;
+		case 2:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+
+			float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+			float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+
+			float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+			float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+
+			float32x2_t rLo = vpadd_f32(xy0, xy1);
+			rLo = vadd_f32(rLo, zLo);
+
+			uint32x2_t maskLo = vclt_f32(rLo, dotMinLo);
+			dotMinLo = vbsl_f32(maskLo, rLo, dotMinLo);
+			iLo = vbsl_u32(maskLo, indexLo, iLo);
+		}
+		break;
+		case 1:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+			float32x2_t z0 = vdup_lane_f32(vget_high_f32(v0), 0);
+			float32x2_t zLo = vmul_f32(z0, vHi);
+			float32x2_t rLo = vpadd_f32(xy0, xy0);
+			rLo = vadd_f32(rLo, zLo);
+			uint32x2_t maskLo = vclt_f32(rLo, dotMinLo);
+			dotMinLo = vbsl_f32(maskLo, rLo, dotMinLo);
+			iLo = vbsl_u32(maskLo, indexLo, iLo);
+		}
+		break;
+
+		default:
+			break;
+	}
+
+	// select best answer between hi and lo results
+	uint32x2_t mask = vclt_f32(dotMinHi, dotMinLo);
+	dotMinLo = vbsl_f32(mask, dotMinHi, dotMinLo);
+	iLo = vbsl_u32(mask, iHi, iLo);
+
+	// select best answer between even and odd results
+	dotMinHi = vdup_lane_f32(dotMinLo, 1);
+	iHi = vdup_lane_u32(iLo, 1);
+	mask = vclt_f32(dotMinHi, dotMinLo);
+	dotMinLo = vbsl_f32(mask, dotMinHi, dotMinLo);
+	iLo = vbsl_u32(mask, iHi, iLo);
+
+	*dotResult = vget_lane_f32(dotMinLo, 0);
+	return vget_lane_u32(iLo, 0);
 }
 
-long _mindot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult )
+long _mindot_large_v1(const float *vv, const float *vec, unsigned long count, float *dotResult)
 {
-    float32x4_t vvec = vld1q_f32_aligned_postincrement( vec );
-    float32x4_t vLo = vcombine_f32(vget_low_f32(vvec), vget_low_f32(vvec));
-    float32x4_t vHi = vdupq_lane_f32(vget_high_f32(vvec), 0);
-    const uint32x4_t four = (uint32x4_t){ 4, 4, 4, 4 };
-    uint32x4_t local_index = (uint32x4_t) {0, 1, 2, 3};
-    uint32x4_t index = (uint32x4_t) { static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1) };
-    float32x4_t minDot = (float32x4_t) { BT_INFINITY, BT_INFINITY, BT_INFINITY, BT_INFINITY };
-    
-    unsigned long i = 0;
-    for( ; i + 8 <= count; i += 8 )
-    {
-        float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
-        
-        // the next two lines should resolve to a single vswp d, d
-        float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
-        float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3));
-        // the next two lines should resolve to a single vswp d, d
-        float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
-        float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3));
-        
-        xy0 = vmulq_f32(xy0, vLo);
-        xy1 = vmulq_f32(xy1, vLo);
-        
-        float32x4x2_t zb = vuzpq_f32( z0, z1);
-        float32x4_t z = vmulq_f32( zb.val[0], vHi);
-        float32x4x2_t xy = vuzpq_f32( xy0, xy1);
-        float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
-        x = vaddq_f32(x, z);
-        
-        uint32x4_t mask = vcltq_f32(x, minDot);
-        minDot = vbslq_f32( mask, x, minDot);
-        index = vbslq_u32(mask, local_index, index);
-        local_index = vaddq_u32(local_index, four);
-        
-        v0 = vld1q_f32_aligned_postincrement( vv );
-        v1 = vld1q_f32_aligned_postincrement( vv );
-        v2 = vld1q_f32_aligned_postincrement( vv );
-        v3 = vld1q_f32_aligned_postincrement( vv );
-        
-        // the next two lines should resolve to a single vswp d, d
-        xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
-        xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3));
-        // the next two lines should resolve to a single vswp d, d
-        z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
-        z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3));
-        
-        xy0 = vmulq_f32(xy0, vLo);
-        xy1 = vmulq_f32(xy1, vLo);
-        
-        zb = vuzpq_f32( z0, z1);
-        z = vmulq_f32( zb.val[0], vHi);
-        xy = vuzpq_f32( xy0, xy1);
-        x = vaddq_f32(xy.val[0], xy.val[1]);
-        x = vaddq_f32(x, z);
-        
-        mask = vcltq_f32(x, minDot);
-        minDot = vbslq_f32( mask, x, minDot);
-        index = vbslq_u32(mask, local_index, index);
-        local_index = vaddq_u32(local_index, four);
-    }
-    
-    for( ; i + 4 <= count; i += 4 )
-    {
-        float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
-        float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
-        
-        // the next two lines should resolve to a single vswp d, d
-        float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
-        float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3));
-        // the next two lines should resolve to a single vswp d, d
-        float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
-        float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3));
-        
-        xy0 = vmulq_f32(xy0, vLo);
-        xy1 = vmulq_f32(xy1, vLo);
-        
-        float32x4x2_t zb = vuzpq_f32( z0, z1);
-        float32x4_t z = vmulq_f32( zb.val[0], vHi);
-        float32x4x2_t xy = vuzpq_f32( xy0, xy1);
-        float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
-        x = vaddq_f32(x, z);
-        
-        uint32x4_t mask = vcltq_f32(x, minDot);
-        minDot = vbslq_f32( mask, x, minDot);
-        index = vbslq_u32(mask, local_index, index);
-        local_index = vaddq_u32(local_index, four);
-    }
-    
-    switch (count & 3) {
-        case 3:
-        {
-            float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-            float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-            float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
-            
-            // the next two lines should resolve to a single vswp d, d
-            float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
-            float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v2));
-            // the next two lines should resolve to a single vswp d, d
-            float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
-            float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v2));
-            
-            xy0 = vmulq_f32(xy0, vLo);
-            xy1 = vmulq_f32(xy1, vLo);
-            
-            float32x4x2_t zb = vuzpq_f32( z0, z1);
-            float32x4_t z = vmulq_f32( zb.val[0], vHi);
-            float32x4x2_t xy = vuzpq_f32( xy0, xy1);
-            float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
-            x = vaddq_f32(x, z);
-            
-            uint32x4_t mask = vcltq_f32(x, minDot);
-            minDot = vbslq_f32( mask, x, minDot);
-            index = vbslq_u32(mask, local_index, index);
-            local_index = vaddq_u32(local_index, four);
-        }
-            break;
-            
-        case 2:
-        {
-            float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-            float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
-            
-            // the next two lines should resolve to a single vswp d, d
-            float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
-            // the next two lines should resolve to a single vswp d, d
-            float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
-            
-            xy0 = vmulq_f32(xy0, vLo);
-            
-            float32x4x2_t zb = vuzpq_f32( z0, z0);
-            float32x4_t z = vmulq_f32( zb.val[0], vHi);
-            float32x4x2_t xy = vuzpq_f32( xy0, xy0);
-            float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
-            x = vaddq_f32(x, z);
-            
-            uint32x4_t mask = vcltq_f32(x, minDot);
-            minDot = vbslq_f32( mask, x, minDot);
-            index = vbslq_u32(mask, local_index, index);
-            local_index = vaddq_u32(local_index, four);
-        }
-            break;
-            
-        case 1:
-        {
-            float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
-            
-            // the next two lines should resolve to a single vswp d, d
-            float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v0));
-            // the next two lines should resolve to a single vswp d, d
-            float32x4_t z = vdupq_lane_f32(vget_high_f32(v0), 0); 
-            
-            xy0 = vmulq_f32(xy0, vLo);
-            
-            z = vmulq_f32( z, vHi);
-            float32x4x2_t xy = vuzpq_f32( xy0, xy0);
-            float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
-            x = vaddq_f32(x, z);
-            
-            uint32x4_t mask = vcltq_f32(x, minDot);
-            minDot = vbslq_f32( mask, x, minDot);
-            index = vbslq_u32(mask, local_index, index);
-            local_index = vaddq_u32(local_index, four);
-        }
-            break;
-            
-        default:
-            break;
-    }
-    
-    
-    // select best answer between hi and lo results
-    uint32x2_t mask = vclt_f32( vget_high_f32(minDot), vget_low_f32(minDot));
-    float32x2_t minDot2 = vbsl_f32(mask, vget_high_f32(minDot), vget_low_f32(minDot));
-    uint32x2_t index2 = vbsl_u32(mask, vget_high_u32(index), vget_low_u32(index));
-    
-    // select best answer between even and odd results
-    float32x2_t minDotO = vdup_lane_f32(minDot2, 1);
-    uint32x2_t indexHi = vdup_lane_u32(index2, 1);
-    mask = vclt_f32( minDotO, minDot2 );
-    minDot2 = vbsl_f32(mask, minDotO, minDot2);
-    index2 = vbsl_u32(mask, indexHi, index2);
-    
-    *dotResult = vget_lane_f32( minDot2, 0);
-    return vget_lane_u32(index2, 0);
-    
+	float32x4_t vvec = vld1q_f32_aligned_postincrement(vec);
+	float32x4_t vLo = vcombine_f32(vget_low_f32(vvec), vget_low_f32(vvec));
+	float32x4_t vHi = vdupq_lane_f32(vget_high_f32(vvec), 0);
+	const uint32x4_t four = (uint32x4_t){4, 4, 4, 4};
+	uint32x4_t local_index = (uint32x4_t){0, 1, 2, 3};
+	uint32x4_t index = (uint32x4_t){static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
+	float32x4_t minDot = (float32x4_t){BT_INFINITY, BT_INFINITY, BT_INFINITY, BT_INFINITY};
+
+	unsigned long i = 0;
+	for (; i + 8 <= count; i += 8)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+		float32x4_t xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v3));
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x4_t z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v3));
+
+		xy0 = vmulq_f32(xy0, vLo);
+		xy1 = vmulq_f32(xy1, vLo);
+
+		float32x4x2_t zb = vuzpq_f32(z0, z1);
+		float32x4_t z = vmulq_f32(zb.val[0], vHi);
+		float32x4x2_t xy = vuzpq_f32(xy0, xy1);
+		float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+		x = vaddq_f32(x, z);
+
+		uint32x4_t mask = vcltq_f32(x, minDot);
+		minDot = vbslq_f32(mask, x, minDot);
+		index = vbslq_u32(mask, local_index, index);
+		local_index = vaddq_u32(local_index, four);
+
+		v0 = vld1q_f32_aligned_postincrement(vv);
+		v1 = vld1q_f32_aligned_postincrement(vv);
+		v2 = vld1q_f32_aligned_postincrement(vv);
+		v3 = vld1q_f32_aligned_postincrement(vv);
+
+		// the next two lines should resolve to a single vswp d, d
+		xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+		xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v3));
+		// the next two lines should resolve to a single vswp d, d
+		z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+		z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v3));
+
+		xy0 = vmulq_f32(xy0, vLo);
+		xy1 = vmulq_f32(xy1, vLo);
+
+		zb = vuzpq_f32(z0, z1);
+		z = vmulq_f32(zb.val[0], vHi);
+		xy = vuzpq_f32(xy0, xy1);
+		x = vaddq_f32(xy.val[0], xy.val[1]);
+		x = vaddq_f32(x, z);
+
+		mask = vcltq_f32(x, minDot);
+		minDot = vbslq_f32(mask, x, minDot);
+		index = vbslq_u32(mask, local_index, index);
+		local_index = vaddq_u32(local_index, four);
+	}
+
+	for (; i + 4 <= count; i += 4)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+		float32x4_t xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v3));
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x4_t z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v3));
+
+		xy0 = vmulq_f32(xy0, vLo);
+		xy1 = vmulq_f32(xy1, vLo);
+
+		float32x4x2_t zb = vuzpq_f32(z0, z1);
+		float32x4_t z = vmulq_f32(zb.val[0], vHi);
+		float32x4x2_t xy = vuzpq_f32(xy0, xy1);
+		float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+		x = vaddq_f32(x, z);
+
+		uint32x4_t mask = vcltq_f32(x, minDot);
+		minDot = vbslq_f32(mask, x, minDot);
+		index = vbslq_u32(mask, local_index, index);
+		local_index = vaddq_u32(local_index, four);
+	}
+
+	switch (count & 3)
+	{
+		case 3:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+			float32x4_t xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v2));
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+			float32x4_t z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v2));
+
+			xy0 = vmulq_f32(xy0, vLo);
+			xy1 = vmulq_f32(xy1, vLo);
+
+			float32x4x2_t zb = vuzpq_f32(z0, z1);
+			float32x4_t z = vmulq_f32(zb.val[0], vHi);
+			float32x4x2_t xy = vuzpq_f32(xy0, xy1);
+			float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+			x = vaddq_f32(x, z);
+
+			uint32x4_t mask = vcltq_f32(x, minDot);
+			minDot = vbslq_f32(mask, x, minDot);
+			index = vbslq_u32(mask, local_index, index);
+			local_index = vaddq_u32(local_index, four);
+		}
+		break;
+
+		case 2:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+
+			xy0 = vmulq_f32(xy0, vLo);
+
+			float32x4x2_t zb = vuzpq_f32(z0, z0);
+			float32x4_t z = vmulq_f32(zb.val[0], vHi);
+			float32x4x2_t xy = vuzpq_f32(xy0, xy0);
+			float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+			x = vaddq_f32(x, z);
+
+			uint32x4_t mask = vcltq_f32(x, minDot);
+			minDot = vbslq_f32(mask, x, minDot);
+			index = vbslq_u32(mask, local_index, index);
+			local_index = vaddq_u32(local_index, four);
+		}
+		break;
+
+		case 1:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v0));
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t z = vdupq_lane_f32(vget_high_f32(v0), 0);
+
+			xy0 = vmulq_f32(xy0, vLo);
+
+			z = vmulq_f32(z, vHi);
+			float32x4x2_t xy = vuzpq_f32(xy0, xy0);
+			float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+			x = vaddq_f32(x, z);
+
+			uint32x4_t mask = vcltq_f32(x, minDot);
+			minDot = vbslq_f32(mask, x, minDot);
+			index = vbslq_u32(mask, local_index, index);
+			local_index = vaddq_u32(local_index, four);
+		}
+		break;
+
+		default:
+			break;
+	}
+
+	// select best answer between hi and lo results
+	uint32x2_t mask = vclt_f32(vget_high_f32(minDot), vget_low_f32(minDot));
+	float32x2_t minDot2 = vbsl_f32(mask, vget_high_f32(minDot), vget_low_f32(minDot));
+	uint32x2_t index2 = vbsl_u32(mask, vget_high_u32(index), vget_low_u32(index));
+
+	// select best answer between even and odd results
+	float32x2_t minDotO = vdup_lane_f32(minDot2, 1);
+	uint32x2_t indexHi = vdup_lane_u32(index2, 1);
+	mask = vclt_f32(minDotO, minDot2);
+	minDot2 = vbsl_f32(mask, minDotO, minDot2);
+	index2 = vbsl_u32(mask, indexHi, index2);
+
+	*dotResult = vget_lane_f32(minDot2, 0);
+	return vget_lane_u32(index2, 0);
 }
 
 #else
-    #error Unhandled __APPLE__ arch
+#error Unhandled __APPLE__ arch
 #endif
 
-#endif  /* __APPLE__ */
-
-
+#endif /* __APPLE__ */
diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h
index 30581953d2e..d65ed9808d1 100644
--- a/extern/bullet2/src/LinearMath/btVector3.h
+++ b/extern/bullet2/src/LinearMath/btVector3.h
@@ -12,8 +12,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef BT_VECTOR3_H
 #define BT_VECTOR3_H
 
@@ -28,25 +26,24 @@ subject to the following restrictions:
 #else
 #define btVector3Data btVector3FloatData
 #define btVector3DataName "btVector3FloatData"
-#endif //BT_USE_DOUBLE_PRECISION
+#endif  //BT_USE_DOUBLE_PRECISION
 
 #if defined BT_USE_SSE
 
 //typedef  uint32_t __m128i __attribute__ ((vector_size(16)));
 
 #ifdef _MSC_VER
-#pragma warning(disable: 4556) // value of intrinsic immediate argument '4294967239' is out of range '0 - 255'
+#pragma warning(disable : 4556)  // value of intrinsic immediate argument '4294967239' is out of range '0 - 255'
 #endif
 
-
-#define BT_SHUFFLE(x,y,z,w) (((w) << 6 | (z) << 4 | (y) << 2 | (x)) & 0xff)
+#define BT_SHUFFLE(x, y, z, w) (((w) << 6 | (z) << 4 | (y) << 2 | (x)) & 0xff)
 //#define bt_pshufd_ps( _a, _mask ) (__m128) _mm_shuffle_epi32((__m128i)(_a), (_mask) )
-#define bt_pshufd_ps( _a, _mask ) _mm_shuffle_ps((_a), (_a), (_mask) )
-#define bt_splat3_ps( _a, _i ) bt_pshufd_ps((_a), BT_SHUFFLE(_i,_i,_i, 3) )
-#define bt_splat_ps( _a, _i )  bt_pshufd_ps((_a), BT_SHUFFLE(_i,_i,_i,_i) )
+#define bt_pshufd_ps(_a, _mask) _mm_shuffle_ps((_a), (_a), (_mask))
+#define bt_splat3_ps(_a, _i) bt_pshufd_ps((_a), BT_SHUFFLE(_i, _i, _i, 3))
+#define bt_splat_ps(_a, _i) bt_pshufd_ps((_a), BT_SHUFFLE(_i, _i, _i, _i))
 
 #define btv3AbsiMask (_mm_set_epi32(0x00000000, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF))
-#define btvAbsMask (_mm_set_epi32( 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF))
+#define btvAbsMask (_mm_set_epi32(0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF))
 #define btvFFF0Mask (_mm_set_epi32(0x00000000, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF))
 #define btv3AbsfMask btCastiTo128f(btv3AbsiMask)
 #define btvFFF0fMask btCastiTo128f(btvFFF0Mask)
@@ -55,9 +52,9 @@ subject to the following restrictions:
 
 //there is an issue with XCode 3.2 (LCx errors)
 #define btvMzeroMask (_mm_set_ps(-0.0f, -0.0f, -0.0f, -0.0f))
-#define v1110		 (_mm_set_ps(0.0f, 1.0f, 1.0f, 1.0f))
-#define vHalf		 (_mm_set_ps(0.5f, 0.5f, 0.5f, 0.5f))
-#define v1_5		 (_mm_set_ps(1.5f, 1.5f, 1.5f, 1.5f))
+#define v1110 (_mm_set_ps(0.0f, 1.0f, 1.0f, 1.0f))
+#define vHalf (_mm_set_ps(0.5f, 0.5f, 0.5f, 0.5f))
+#define v1_5 (_mm_set_ps(1.5f, 1.5f, 1.5f, 1.5f))
 
 //const __m128 ATTRIBUTE_ALIGNED16(btvMzeroMask) = {-0.0f, -0.0f, -0.0f, -0.0f};
 //const __m128 ATTRIBUTE_ALIGNED16(v1110) = {1.0f, 1.0f, 1.0f, 0.0f};
@@ -70,7 +67,7 @@ subject to the following restrictions:
 
 const float32x4_t ATTRIBUTE_ALIGNED16(btvMzeroMask) = (float32x4_t){-0.0f, -0.0f, -0.0f, -0.0f};
 const int32x4_t ATTRIBUTE_ALIGNED16(btvFFF0Mask) = (int32x4_t){static_cast<int32_t>(0xFFFFFFFF),
-	static_cast<int32_t>(0xFFFFFFFF), static_cast<int32_t>(0xFFFFFFFF), 0x0};
+															   static_cast<int32_t>(0xFFFFFFFF), static_cast<int32_t>(0xFFFFFFFF), 0x0};
 const int32x4_t ATTRIBUTE_ALIGNED16(btvAbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF};
 const int32x4_t ATTRIBUTE_ALIGNED16(btv3AbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x0};
 
@@ -80,50 +77,48 @@ const int32x4_t ATTRIBUTE_ALIGNED16(btv3AbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FF
  * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
  * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
  */
-ATTRIBUTE_ALIGNED16(class) btVector3
+ATTRIBUTE_ALIGNED16(class)
+btVector3
 {
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-#if defined (__SPU__) && defined (__CELLOS_LV2__)
-		btScalar	m_floats[4];
+#if defined(__SPU__) && defined(__CELLOS_LV2__)
+	btScalar m_floats[4];
+
 public:
-	SIMD_FORCE_INLINE const vec_float4&	get128() const
+	SIMD_FORCE_INLINE const vec_float4& get128() const
 	{
 		return *((const vec_float4*)&m_floats[0]);
 	}
+
 public:
-#else //__CELLOS_LV2__ __SPU__
-    #if defined (BT_USE_SSE) || defined(BT_USE_NEON) // _WIN32 || ARM
-        union {
-            btSimdFloat4      mVec128;
-            btScalar	m_floats[4];
-        };
-        SIMD_FORCE_INLINE	btSimdFloat4	get128() const
-        {
-            return mVec128;
-        }
-        SIMD_FORCE_INLINE	void	set128(btSimdFloat4 v128)
-        {
-            mVec128 = v128;
-        }
-    #else
-        btScalar	m_floats[4];
-    #endif
-#endif //__CELLOS_LV2__ __SPU__
-
-	public:
-
-  /**@brief No initialization constructor */
-	SIMD_FORCE_INLINE btVector3() 
+#else                                            //__CELLOS_LV2__ __SPU__
+#if defined(BT_USE_SSE) || defined(BT_USE_NEON)  // _WIN32 || ARM
+	union {
+		btSimdFloat4 mVec128;
+		btScalar m_floats[4];
+	};
+	SIMD_FORCE_INLINE btSimdFloat4 get128() const
 	{
+		return mVec128;
+	}
+	SIMD_FORCE_INLINE void set128(btSimdFloat4 v128)
+	{
+		mVec128 = v128;
+	}
+#else
+	btScalar m_floats[4];
+#endif
+#endif  //__CELLOS_LV2__ __SPU__
 
+public:
+	/**@brief No initialization constructor */
+	SIMD_FORCE_INLINE btVector3()
+	{
 	}
 
- 
-	
-  /**@brief Constructor from scalars 
+	/**@brief Constructor from scalars 
    * @param x X value
    * @param y Y value 
    * @param z Z value 
@@ -136,9 +131,9 @@ public:
 		m_floats[3] = btScalar(0.f);
 	}
 
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) )|| defined (BT_USE_NEON)
-	// Set Vector 
-	SIMD_FORCE_INLINE btVector3( btSimdFloat4 v)
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
+	// Set Vector
+	SIMD_FORCE_INLINE btVector3(btSimdFloat4 v)
 	{
 		mVec128 = v;
 	}
@@ -150,73 +145,72 @@ public:
 	}
 
 	// Assignment Operator
-	SIMD_FORCE_INLINE btVector3& 
-	operator=(const btVector3& v) 
+	SIMD_FORCE_INLINE btVector3&
+	operator=(const btVector3& v)
 	{
 		mVec128 = v.mVec128;
-		
+
 		return *this;
 	}
-#endif // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON) 
-    
-/**@brief Add a vector to this one 
+#endif  // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
+
+	/**@brief Add a vector to this one 
  * @param The vector to add to this one */
 	SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
 	{
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		mVec128 = _mm_add_ps(mVec128, v.mVec128);
 #elif defined(BT_USE_NEON)
 		mVec128 = vaddq_f32(mVec128, v.mVec128);
 #else
-		m_floats[0] += v.m_floats[0]; 
+		m_floats[0] += v.m_floats[0];
 		m_floats[1] += v.m_floats[1];
 		m_floats[2] += v.m_floats[2];
 #endif
 		return *this;
 	}
 
-
-  /**@brief Subtract a vector from this one
+	/**@brief Subtract a vector from this one
    * @param The vector to subtract */
-	SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) 
+	SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v)
 	{
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		mVec128 = _mm_sub_ps(mVec128, v.mVec128);
 #elif defined(BT_USE_NEON)
 		mVec128 = vsubq_f32(mVec128, v.mVec128);
 #else
-		m_floats[0] -= v.m_floats[0]; 
+		m_floats[0] -= v.m_floats[0];
 		m_floats[1] -= v.m_floats[1];
 		m_floats[2] -= v.m_floats[2];
 #endif
 		return *this;
 	}
-	
-  /**@brief Scale the vector
+
+	/**@brief Scale the vector
    * @param s Scale factor */
 	SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
 	{
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
-		__m128	vs = _mm_load_ss(&s);	//	(S 0 0 0)
-		vs = bt_pshufd_ps(vs, 0x80);	//	(S S S 0.0)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+		__m128 vs = _mm_load_ss(&s);  //	(S 0 0 0)
+		vs = bt_pshufd_ps(vs, 0x80);  //	(S S S 0.0)
 		mVec128 = _mm_mul_ps(mVec128, vs);
 #elif defined(BT_USE_NEON)
 		mVec128 = vmulq_n_f32(mVec128, s);
 #else
-		m_floats[0] *= s; 
+		m_floats[0] *= s;
 		m_floats[1] *= s;
 		m_floats[2] *= s;
 #endif
 		return *this;
 	}
 
-  /**@brief Inversely scale the vector 
+	/**@brief Inversely scale the vector 
    * @param s Scale factor to divide by */
-	SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) 
+	SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s)
 	{
 		btFullAssert(s != btScalar(0.0));
 
-#if 0 //defined(BT_USE_SSE_IN_API)
+#if 0  //defined(BT_USE_SSE_IN_API)
 // this code is not faster !
 		__m128 vs = _mm_load_ss(&s);
 		vs = _mm_div_ss(v1110, vs);
@@ -230,11 +224,11 @@ public:
 #endif
 	}
 
-  /**@brief Return the dot product
+	/**@brief Return the dot product
    * @param v The other vector in the dot product */
 	SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
 	{
-#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		__m128 vd = _mm_mul_ps(mVec128, v.mVec128);
 		__m128 z = _mm_movehl_ps(vd, vd);
 		__m128 y = _mm_shuffle_ps(vd, vd, 0x55);
@@ -243,23 +237,23 @@ public:
 		return _mm_cvtss_f32(vd);
 #elif defined(BT_USE_NEON)
 		float32x4_t vd = vmulq_f32(mVec128, v.mVec128);
-		float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_low_f32(vd));  
+		float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_low_f32(vd));
 		x = vadd_f32(x, vget_high_f32(vd));
 		return vget_lane_f32(x, 0);
-#else	
-		return	m_floats[0] * v.m_floats[0] + 
-				m_floats[1] * v.m_floats[1] + 
-				m_floats[2] * v.m_floats[2];
+#else
+		return m_floats[0] * v.m_floats[0] +
+			   m_floats[1] * v.m_floats[1] +
+			   m_floats[2] * v.m_floats[2];
 #endif
 	}
 
-  /**@brief Return the length of the vector squared */
+	/**@brief Return the length of the vector squared */
 	SIMD_FORCE_INLINE btScalar length2() const
 	{
 		return dot(*this);
 	}
 
-  /**@brief Return the length of the vector */
+	/**@brief Return the length of the vector */
 	SIMD_FORCE_INLINE btScalar length() const
 	{
 		return btSqrt(length2());
@@ -271,121 +265,130 @@ public:
 		return length();
 	}
 
-  /**@brief Return the distance squared between the ends of this and another vector
+	/**@brief Return the norm (length) of the vector */
+	SIMD_FORCE_INLINE btScalar safeNorm() const
+	{
+		btScalar d = length2();
+		//workaround for some clang/gcc issue of sqrtf(tiny number) = -INF
+		if (d > SIMD_EPSILON)
+			return btSqrt(d);
+		return btScalar(0);
+	}
+
+	/**@brief Return the distance squared between the ends of this and another vector
    * This is symantically treating the vector like a point */
 	SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
 
-  /**@brief Return the distance between the ends of this and another vector
+	/**@brief Return the distance between the ends of this and another vector
    * This is symantically treating the vector like a point */
 	SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
 
-	SIMD_FORCE_INLINE btVector3& safeNormalize() 
+	SIMD_FORCE_INLINE btVector3& safeNormalize()
 	{
-		btVector3 absVec = this->absolute();
-		int maxIndex = absVec.maxAxis();
-		if (absVec[maxIndex]>0)
+		btScalar l2 = length2();
+		//triNormal.normalize();
+		if (l2 >= SIMD_EPSILON * SIMD_EPSILON)
+		{
+			(*this) /= btSqrt(l2);
+		}
+		else
 		{
-			*this /= absVec[maxIndex];
-			return *this /= length();
+			setValue(1, 0, 0);
 		}
-		setValue(1,0,0);
 		return *this;
 	}
 
-  /**@brief Normalize this vector 
+	/**@brief Normalize this vector 
    * x^2 + y^2 + z^2 = 1 */
-	SIMD_FORCE_INLINE btVector3& normalize() 
+	SIMD_FORCE_INLINE btVector3& normalize()
 	{
-		
 		btAssert(!fuzzyZero());
 
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)		
-        // dot product first
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+		// dot product first
 		__m128 vd = _mm_mul_ps(mVec128, mVec128);
 		__m128 z = _mm_movehl_ps(vd, vd);
 		__m128 y = _mm_shuffle_ps(vd, vd, 0x55);
 		vd = _mm_add_ss(vd, y);
 		vd = _mm_add_ss(vd, z);
-		
-        #if 0
+
+#if 0
         vd = _mm_sqrt_ss(vd);
 		vd = _mm_div_ss(v1110, vd);
 		vd = bt_splat_ps(vd, 0x80);
 		mVec128 = _mm_mul_ps(mVec128, vd);
-        #else
-        
-        // NR step 1/sqrt(x) - vd is x, y is output 
-        y = _mm_rsqrt_ss(vd); // estimate 
-        
-        //  one step NR 
-        z = v1_5;
-        vd = _mm_mul_ss(vd, vHalf); // vd * 0.5	
-        //x2 = vd;
-        vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0
-        vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0 * y0
-        z = _mm_sub_ss(z, vd);  // 1.5 - vd * 0.5 * y0 * y0 
-
-        y = _mm_mul_ss(y, z);   // y0 * (1.5 - vd * 0.5 * y0 * y0)
+#else
+
+		// NR step 1/sqrt(x) - vd is x, y is output
+		y = _mm_rsqrt_ss(vd);  // estimate
+
+		//  one step NR
+		z = v1_5;
+		vd = _mm_mul_ss(vd, vHalf);  // vd * 0.5
+		//x2 = vd;
+		vd = _mm_mul_ss(vd, y);  // vd * 0.5 * y0
+		vd = _mm_mul_ss(vd, y);  // vd * 0.5 * y0 * y0
+		z = _mm_sub_ss(z, vd);   // 1.5 - vd * 0.5 * y0 * y0
+
+		y = _mm_mul_ss(y, z);  // y0 * (1.5 - vd * 0.5 * y0 * y0)
 
 		y = bt_splat_ps(y, 0x80);
 		mVec128 = _mm_mul_ps(mVec128, y);
 
-        #endif
+#endif
 
-		
 		return *this;
-#else	
+#else
 		return *this /= length();
 #endif
 	}
 
-  /**@brief Return a normalized version of this vector */
+	/**@brief Return a normalized version of this vector */
 	SIMD_FORCE_INLINE btVector3 normalized() const;
 
-  /**@brief Return a rotated version of this vector
+	/**@brief Return a rotated version of this vector
    * @param wAxis The axis to rotate about 
    * @param angle The angle to rotate by */
-	SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ) const;
+	SIMD_FORCE_INLINE btVector3 rotate(const btVector3& wAxis, const btScalar angle) const;
 
-  /**@brief Return the angle between this and another vector
+	/**@brief Return the angle between this and another vector
    * @param v The other vector */
-	SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const 
+	SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const
 	{
 		btScalar s = btSqrt(length2() * v.length2());
 		btFullAssert(s != btScalar(0.0));
 		return btAcos(dot(v) / s);
 	}
-	
-  /**@brief Return a vector will the absolute values of each element */
-	SIMD_FORCE_INLINE btVector3 absolute() const 
-	{
 
-#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) 
+	/**@brief Return a vector with the absolute values of each element */
+	SIMD_FORCE_INLINE btVector3 absolute() const
+	{
+#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		return btVector3(_mm_and_ps(mVec128, btv3AbsfMask));
 #elif defined(BT_USE_NEON)
 		return btVector3(vabsq_f32(mVec128));
-#else	
+#else
 		return btVector3(
-			btFabs(m_floats[0]), 
-			btFabs(m_floats[1]), 
+			btFabs(m_floats[0]),
+			btFabs(m_floats[1]),
 			btFabs(m_floats[2]));
 #endif
 	}
-	
-  /**@brief Return the cross product between this and another vector 
+
+	/**@brief Return the cross product between this and another vector 
    * @param v The other vector */
 	SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
 	{
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
-		__m128	T, V;
-		
-		T = bt_pshufd_ps(mVec128, BT_SHUFFLE(1, 2, 0, 3));	//	(Y Z X 0)
-		V = bt_pshufd_ps(v.mVec128, BT_SHUFFLE(1, 2, 0, 3));	//	(Y Z X 0)
-		
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+		__m128 T, V;
+
+		T = bt_pshufd_ps(mVec128, BT_SHUFFLE(1, 2, 0, 3));    //	(Y Z X 0)
+		V = bt_pshufd_ps(v.mVec128, BT_SHUFFLE(1, 2, 0, 3));  //	(Y Z X 0)
+
 		V = _mm_mul_ps(V, mVec128);
 		T = _mm_mul_ps(T, v.mVec128);
 		V = _mm_sub_ps(V, T);
-		
+
 		V = bt_pshufd_ps(V, BT_SHUFFLE(1, 2, 0, 3));
 		return btVector3(V);
 #elif defined(BT_USE_NEON)
@@ -395,7 +398,7 @@ public:
 		float32x2_t Vlow = vget_low_f32(v.mVec128);
 		T = vcombine_f32(vext_f32(Tlow, vget_high_f32(mVec128), 1), Tlow);
 		V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v.mVec128), 1), Vlow);
-		
+
 		V = vmulq_f32(V, mVec128);
 		T = vmulq_f32(T, v.mVec128);
 		V = vsubq_f32(V, T);
@@ -403,7 +406,7 @@ public:
 		// form (Y, Z, X, _);
 		V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
 		V = (float32x4_t)vandq_s32((int32x4_t)V, btvFFF0Mask);
-		
+
 		return btVector3(V);
 #else
 		return btVector3(
@@ -415,18 +418,18 @@ public:
 
 	SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
 	{
-#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		// cross:
-		__m128 T = _mm_shuffle_ps(v1.mVec128, v1.mVec128, BT_SHUFFLE(1, 2, 0, 3));	//	(Y Z X 0)
-		__m128 V = _mm_shuffle_ps(v2.mVec128, v2.mVec128, BT_SHUFFLE(1, 2, 0, 3));	//	(Y Z X 0)
-		
+		__m128 T = _mm_shuffle_ps(v1.mVec128, v1.mVec128, BT_SHUFFLE(1, 2, 0, 3));  //	(Y Z X 0)
+		__m128 V = _mm_shuffle_ps(v2.mVec128, v2.mVec128, BT_SHUFFLE(1, 2, 0, 3));  //	(Y Z X 0)
+
 		V = _mm_mul_ps(V, v1.mVec128);
 		T = _mm_mul_ps(T, v2.mVec128);
 		V = _mm_sub_ps(V, T);
-		
+
 		V = _mm_shuffle_ps(V, V, BT_SHUFFLE(1, 2, 0, 3));
 
-		// dot: 
+		// dot:
 		V = _mm_mul_ps(V, mVec128);
 		__m128 z = _mm_movehl_ps(V, V);
 		__m128 y = _mm_shuffle_ps(V, V, 0x55);
@@ -442,7 +445,7 @@ public:
 		float32x2_t Vlow = vget_low_f32(v2.mVec128);
 		T = vcombine_f32(vext_f32(Tlow, vget_high_f32(v1.mVec128), 1), Tlow);
 		V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v2.mVec128), 1), Vlow);
-		
+
 		V = vmulq_f32(V, v1.mVec128);
 		T = vmulq_f32(T, v2.mVec128);
 		V = vsubq_f32(V, T);
@@ -450,31 +453,30 @@ public:
 		// form (Y, Z, X, _);
 		V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
 
-		// dot: 
+		// dot:
 		V = vmulq_f32(mVec128, V);
-		float32x2_t x = vpadd_f32(vget_low_f32(V), vget_low_f32(V));  
+		float32x2_t x = vpadd_f32(vget_low_f32(V), vget_low_f32(V));
 		x = vadd_f32(x, vget_high_f32(V));
 		return vget_lane_f32(x, 0);
 #else
-		return 
-			m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + 
-			m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + 
-			m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
+		return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
+			   m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
+			   m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
 #endif
 	}
 
-  /**@brief Return the axis with the smallest value 
+	/**@brief Return the axis with the smallest value 
    * Note return values are 0,1,2 for x, y, or z */
 	SIMD_FORCE_INLINE int minAxis() const
 	{
-		return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
+		return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2);
 	}
 
-  /**@brief Return the axis with the largest value 
+	/**@brief Return the axis with the largest value 
    * Note return values are 0,1,2 for x, y, or z */
-	SIMD_FORCE_INLINE int maxAxis() const 
+	SIMD_FORCE_INLINE int maxAxis() const
 	{
-		return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
+		return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0);
 	}
 
 	SIMD_FORCE_INLINE int furthestAxis() const
@@ -482,23 +484,22 @@ public:
 		return absolute().minAxis();
 	}
 
-	SIMD_FORCE_INLINE int closestAxis() const 
+	SIMD_FORCE_INLINE int closestAxis() const
 	{
 		return absolute().maxAxis();
 	}
 
-	
 	SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
 	{
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
-		__m128	vrt = _mm_load_ss(&rt);	//	(rt 0 0 0)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+		__m128 vrt = _mm_load_ss(&rt);  //	(rt 0 0 0)
 		btScalar s = btScalar(1.0) - rt;
-		__m128	vs = _mm_load_ss(&s);	//	(S 0 0 0)
-		vs = bt_pshufd_ps(vs, 0x80);	//	(S S S 0.0)
+		__m128 vs = _mm_load_ss(&s);  //	(S 0 0 0)
+		vs = bt_pshufd_ps(vs, 0x80);  //	(S S S 0.0)
 		__m128 r0 = _mm_mul_ps(v0.mVec128, vs);
-		vrt = bt_pshufd_ps(vrt, 0x80);	//	(rt rt rt 0.0)
+		vrt = bt_pshufd_ps(vrt, 0x80);  //	(rt rt rt 0.0)
 		__m128 r1 = _mm_mul_ps(v1.mVec128, vrt);
-		__m128 tmp3 = _mm_add_ps(r0,r1);
+		__m128 tmp3 = _mm_add_ps(r0, r1);
 		mVec128 = tmp3;
 #elif defined(BT_USE_NEON)
 		float32x4_t vl = vsubq_f32(v1.mVec128, v0.mVec128);
@@ -514,101 +515,100 @@ public:
 #endif
 	}
 
-  /**@brief Return the linear interpolation between this and another vector 
+	/**@brief Return the linear interpolation between this and another vector 
    * @param v The other vector 
    * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
-	SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const 
+	SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
 	{
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
-		__m128	vt = _mm_load_ss(&t);	//	(t 0 0 0)
-		vt = bt_pshufd_ps(vt, 0x80);	//	(rt rt rt 0.0)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+		__m128 vt = _mm_load_ss(&t);  //	(t 0 0 0)
+		vt = bt_pshufd_ps(vt, 0x80);  //	(rt rt rt 0.0)
 		__m128 vl = _mm_sub_ps(v.mVec128, mVec128);
 		vl = _mm_mul_ps(vl, vt);
 		vl = _mm_add_ps(vl, mVec128);
-		
+
 		return btVector3(vl);
 #elif defined(BT_USE_NEON)
 		float32x4_t vl = vsubq_f32(v.mVec128, mVec128);
 		vl = vmulq_n_f32(vl, t);
 		vl = vaddq_f32(vl, mVec128);
-		
+
 		return btVector3(vl);
-#else	
-		return 
-			btVector3(	m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
-						m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
-						m_floats[2] + (v.m_floats[2] - m_floats[2]) * t);
+#else
+		return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
+						 m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
+						 m_floats[2] + (v.m_floats[2] - m_floats[2]) * t);
 #endif
 	}
 
-  /**@brief Elementwise multiply this vector by the other 
+	/**@brief Elementwise multiply this vector by the other 
    * @param v The other vector */
 	SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
 	{
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		mVec128 = _mm_mul_ps(mVec128, v.mVec128);
 #elif defined(BT_USE_NEON)
 		mVec128 = vmulq_f32(mVec128, v.mVec128);
-#else	
-		m_floats[0] *= v.m_floats[0]; 
+#else
+		m_floats[0] *= v.m_floats[0];
 		m_floats[1] *= v.m_floats[1];
 		m_floats[2] *= v.m_floats[2];
 #endif
 		return *this;
 	}
 
-	 /**@brief Return the x value */
-		SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
-  /**@brief Return the y value */
-		SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
-  /**@brief Return the z value */
-		SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
-  /**@brief Set the x value */
-		SIMD_FORCE_INLINE void	setX(btScalar _x) { m_floats[0] = _x;};
-  /**@brief Set the y value */
-		SIMD_FORCE_INLINE void	setY(btScalar _y) { m_floats[1] = _y;};
-  /**@brief Set the z value */
-		SIMD_FORCE_INLINE void	setZ(btScalar _z) { m_floats[2] = _z;};
-  /**@brief Set the w value */
-		SIMD_FORCE_INLINE void	setW(btScalar _w) { m_floats[3] = _w;};
-  /**@brief Return the x value */
-		SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
-  /**@brief Return the y value */
-		SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
-  /**@brief Return the z value */
-		SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
-  /**@brief Return the w value */
-		SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
-
-	//SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_floats[0])[i];	}      
+	/**@brief Return the x value */
+	SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
+	/**@brief Return the y value */
+	SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
+	/**@brief Return the z value */
+	SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
+	/**@brief Set the x value */
+	SIMD_FORCE_INLINE void setX(btScalar _x) { m_floats[0] = _x; };
+	/**@brief Set the y value */
+	SIMD_FORCE_INLINE void setY(btScalar _y) { m_floats[1] = _y; };
+	/**@brief Set the z value */
+	SIMD_FORCE_INLINE void setZ(btScalar _z) { m_floats[2] = _z; };
+	/**@brief Set the w value */
+	SIMD_FORCE_INLINE void setW(btScalar _w) { m_floats[3] = _w; };
+	/**@brief Return the x value */
+	SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
+	/**@brief Return the y value */
+	SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
+	/**@brief Return the z value */
+	SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
+	/**@brief Return the w value */
+	SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
+
+	//SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_floats[0])[i];	}
 	//SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
 	///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
-	SIMD_FORCE_INLINE	operator       btScalar *()       { return &m_floats[0]; }
-	SIMD_FORCE_INLINE	operator const btScalar *() const { return &m_floats[0]; }
+	SIMD_FORCE_INLINE operator btScalar*() { return &m_floats[0]; }
+	SIMD_FORCE_INLINE operator const btScalar*() const { return &m_floats[0]; }
 
-	SIMD_FORCE_INLINE	bool	operator==(const btVector3& other) const
+	SIMD_FORCE_INLINE bool operator==(const btVector3& other) const
 	{
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
-        return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
-#else 
-		return ((m_floats[3]==other.m_floats[3]) && 
-                (m_floats[2]==other.m_floats[2]) && 
-                (m_floats[1]==other.m_floats[1]) && 
-                (m_floats[0]==other.m_floats[0]));
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+		return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
+#else
+		return ((m_floats[3] == other.m_floats[3]) &&
+				(m_floats[2] == other.m_floats[2]) &&
+				(m_floats[1] == other.m_floats[1]) &&
+				(m_floats[0] == other.m_floats[0]));
 #endif
 	}
 
-	SIMD_FORCE_INLINE	bool	operator!=(const btVector3& other) const
+	SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const
 	{
 		return !(*this == other);
 	}
 
-  /**@brief Set each element to the max of the current values and the values of another btVector3
+	/**@brief Set each element to the max of the current values and the values of another btVector3
    * @param other The other btVector3 to compare with 
    */
-	SIMD_FORCE_INLINE void	setMax(const btVector3& other)
+	SIMD_FORCE_INLINE void setMax(const btVector3& other)
 	{
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		mVec128 = _mm_max_ps(mVec128, other.mVec128);
 #elif defined(BT_USE_NEON)
 		mVec128 = vmaxq_f32(mVec128, other.mVec128);
@@ -620,12 +620,12 @@ public:
 #endif
 	}
 
-  /**@brief Set each element to the min of the current values and the values of another btVector3
+	/**@brief Set each element to the min of the current values and the values of another btVector3
    * @param other The other btVector3 to compare with 
    */
-	SIMD_FORCE_INLINE void	setMin(const btVector3& other)
+	SIMD_FORCE_INLINE void setMin(const btVector3& other)
 	{
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		mVec128 = _mm_min_ps(mVec128, other.mVec128);
 #elif defined(BT_USE_NEON)
 		mVec128 = vminq_f32(mVec128, other.mVec128);
@@ -637,154 +637,155 @@ public:
 #endif
 	}
 
-	SIMD_FORCE_INLINE void 	setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z)
+	SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z)
 	{
-		m_floats[0]=_x;
-		m_floats[1]=_y;
-		m_floats[2]=_z;
+		m_floats[0] = _x;
+		m_floats[1] = _y;
+		m_floats[2] = _z;
 		m_floats[3] = btScalar(0.f);
 	}
 
-	void	getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const
+	void getSkewSymmetricMatrix(btVector3 * v0, btVector3 * v1, btVector3 * v2) const
 	{
-#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
- 
-		__m128 V  = _mm_and_ps(mVec128, btvFFF0fMask);
+#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+
+		__m128 V = _mm_and_ps(mVec128, btvFFF0fMask);
 		__m128 V0 = _mm_xor_ps(btvMzeroMask, V);
 		__m128 V2 = _mm_movelh_ps(V0, V);
-		
+
 		__m128 V1 = _mm_shuffle_ps(V, V0, 0xCE);
-		
-        V0 = _mm_shuffle_ps(V0, V, 0xDB);
+
+		V0 = _mm_shuffle_ps(V0, V, 0xDB);
 		V2 = _mm_shuffle_ps(V2, V, 0xF9);
-		
+
 		v0->mVec128 = V0;
 		v1->mVec128 = V1;
 		v2->mVec128 = V2;
 #else
-		v0->setValue(0.		,-z()		,y());
-		v1->setValue(z()	,0.			,-x());
-		v2->setValue(-y()	,x()	,0.);
+		v0->setValue(0., -z(), y());
+		v1->setValue(z(), 0., -x());
+		v2->setValue(-y(), x(), 0.);
 #endif
 	}
 
 	void setZero()
 	{
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		mVec128 = (__m128)_mm_xor_ps(mVec128, mVec128);
 #elif defined(BT_USE_NEON)
-		int32x4_t vi = vdupq_n_s32(0); 
+		int32x4_t vi = vdupq_n_s32(0);
 		mVec128 = vreinterpretq_f32_s32(vi);
-#else	
-		setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+#else
+		setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 #endif
 	}
 
-	SIMD_FORCE_INLINE bool isZero() const 
+	SIMD_FORCE_INLINE bool isZero() const
 	{
 		return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
 	}
 
-
-	SIMD_FORCE_INLINE bool fuzzyZero() const 
+	SIMD_FORCE_INLINE bool fuzzyZero() const
 	{
-		return length2() < SIMD_EPSILON*SIMD_EPSILON;
+		return length2() < SIMD_EPSILON * SIMD_EPSILON;
 	}
 
-	SIMD_FORCE_INLINE	void	serialize(struct	btVector3Data& dataOut) const;
+	SIMD_FORCE_INLINE void serialize(struct btVector3Data & dataOut) const;
+
+	SIMD_FORCE_INLINE void deSerialize(const struct btVector3DoubleData& dataIn);
+
+	SIMD_FORCE_INLINE void deSerialize(const struct btVector3FloatData& dataIn);
 
-	SIMD_FORCE_INLINE	void	deSerialize(const struct	btVector3Data& dataIn);
+	SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData & dataOut) const;
 
-	SIMD_FORCE_INLINE	void	serializeFloat(struct	btVector3FloatData& dataOut) const;
+	SIMD_FORCE_INLINE void deSerializeFloat(const struct btVector3FloatData& dataIn);
 
-	SIMD_FORCE_INLINE	void	deSerializeFloat(const struct	btVector3FloatData& dataIn);
+	SIMD_FORCE_INLINE void serializeDouble(struct btVector3DoubleData & dataOut) const;
 
-	SIMD_FORCE_INLINE	void	serializeDouble(struct	btVector3DoubleData& dataOut) const;
+	SIMD_FORCE_INLINE void deSerializeDouble(const struct btVector3DoubleData& dataIn);
 
-	SIMD_FORCE_INLINE	void	deSerializeDouble(const struct	btVector3DoubleData& dataIn);
-    
-        /**@brief returns index of maximum dot product between this and vectors in array[]
+	/**@brief returns index of maximum dot product between this and vectors in array[]
          * @param array The other vectors 
          * @param array_count The number of other vectors 
          * @param dotOut The maximum dot product */
-        SIMD_FORCE_INLINE   long    maxDot( const btVector3 *array, long array_count, btScalar &dotOut ) const; 
+	SIMD_FORCE_INLINE long maxDot(const btVector3* array, long array_count, btScalar& dotOut) const;
 
-        /**@brief returns index of minimum dot product between this and vectors in array[]
+	/**@brief returns index of minimum dot product between this and vectors in array[]
          * @param array The other vectors 
          * @param array_count The number of other vectors 
-         * @param dotOut The minimum dot product */    
-        SIMD_FORCE_INLINE   long    minDot( const btVector3 *array, long array_count, btScalar &dotOut ) const; 
-
-    /* create a vector as  btVector3( this->dot( btVector3 v0 ), this->dot( btVector3 v1), this->dot( btVector3 v2 ))  */
-    SIMD_FORCE_INLINE btVector3  dot3( const btVector3 &v0, const btVector3 &v1, const btVector3 &v2 ) const
-    {
-#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
-
-        __m128 a0 = _mm_mul_ps( v0.mVec128, this->mVec128 );
-        __m128 a1 = _mm_mul_ps( v1.mVec128, this->mVec128 );
-        __m128 a2 = _mm_mul_ps( v2.mVec128, this->mVec128 );
-        __m128 b0 = _mm_unpacklo_ps( a0, a1 );
-        __m128 b1 = _mm_unpackhi_ps( a0, a1 );
-        __m128 b2 = _mm_unpacklo_ps( a2, _mm_setzero_ps() );
-        __m128 r = _mm_movelh_ps( b0, b2 );
-        r = _mm_add_ps( r, _mm_movehl_ps( b2, b0 ));
-        a2 = _mm_and_ps( a2, btvxyzMaskf);
-        r = _mm_add_ps( r, btCastdTo128f (_mm_move_sd( btCastfTo128d(a2), btCastfTo128d(b1) )));
-        return btVector3(r);
-        
+         * @param dotOut The minimum dot product */
+	SIMD_FORCE_INLINE long minDot(const btVector3* array, long array_count, btScalar& dotOut) const;
+
+	/* create a vector as  btVector3( this->dot( btVector3 v0 ), this->dot( btVector3 v1), this->dot( btVector3 v2 ))  */
+	SIMD_FORCE_INLINE btVector3 dot3(const btVector3& v0, const btVector3& v1, const btVector3& v2) const
+	{
+#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+
+		__m128 a0 = _mm_mul_ps(v0.mVec128, this->mVec128);
+		__m128 a1 = _mm_mul_ps(v1.mVec128, this->mVec128);
+		__m128 a2 = _mm_mul_ps(v2.mVec128, this->mVec128);
+		__m128 b0 = _mm_unpacklo_ps(a0, a1);
+		__m128 b1 = _mm_unpackhi_ps(a0, a1);
+		__m128 b2 = _mm_unpacklo_ps(a2, _mm_setzero_ps());
+		__m128 r = _mm_movelh_ps(b0, b2);
+		r = _mm_add_ps(r, _mm_movehl_ps(b2, b0));
+		a2 = _mm_and_ps(a2, btvxyzMaskf);
+		r = _mm_add_ps(r, btCastdTo128f(_mm_move_sd(btCastfTo128d(a2), btCastfTo128d(b1))));
+		return btVector3(r);
+
 #elif defined(BT_USE_NEON)
-        static const uint32x4_t xyzMask = (const uint32x4_t){ static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), 0 };
-        float32x4_t a0 = vmulq_f32( v0.mVec128, this->mVec128);
-        float32x4_t a1 = vmulq_f32( v1.mVec128, this->mVec128);
-        float32x4_t a2 = vmulq_f32( v2.mVec128, this->mVec128);
-        float32x2x2_t zLo = vtrn_f32( vget_high_f32(a0), vget_high_f32(a1));
-        a2 = (float32x4_t) vandq_u32((uint32x4_t) a2, xyzMask );
-        float32x2_t b0 = vadd_f32( vpadd_f32( vget_low_f32(a0), vget_low_f32(a1)), zLo.val[0] );
-        float32x2_t b1 = vpadd_f32( vpadd_f32( vget_low_f32(a2), vget_high_f32(a2)), vdup_n_f32(0.0f));
-        return btVector3( vcombine_f32(b0, b1) );
-#else	
-		return btVector3( dot(v0), dot(v1), dot(v2));
+		static const uint32x4_t xyzMask = (const uint32x4_t){static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), 0};
+		float32x4_t a0 = vmulq_f32(v0.mVec128, this->mVec128);
+		float32x4_t a1 = vmulq_f32(v1.mVec128, this->mVec128);
+		float32x4_t a2 = vmulq_f32(v2.mVec128, this->mVec128);
+		float32x2x2_t zLo = vtrn_f32(vget_high_f32(a0), vget_high_f32(a1));
+		a2 = (float32x4_t)vandq_u32((uint32x4_t)a2, xyzMask);
+		float32x2_t b0 = vadd_f32(vpadd_f32(vget_low_f32(a0), vget_low_f32(a1)), zLo.val[0]);
+		float32x2_t b1 = vpadd_f32(vpadd_f32(vget_low_f32(a2), vget_high_f32(a2)), vdup_n_f32(0.0f));
+		return btVector3(vcombine_f32(b0, b1));
+#else
+		return btVector3(dot(v0), dot(v1), dot(v2));
 #endif
-    }
+	}
 };
 
 /**@brief Return the sum of two vectors (Point symantics)*/
-SIMD_FORCE_INLINE btVector3 
-operator+(const btVector3& v1, const btVector3& v2) 
+SIMD_FORCE_INLINE btVector3
+operator+(const btVector3& v1, const btVector3& v2)
 {
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 	return btVector3(_mm_add_ps(v1.mVec128, v2.mVec128));
 #elif defined(BT_USE_NEON)
 	return btVector3(vaddq_f32(v1.mVec128, v2.mVec128));
 #else
 	return btVector3(
-			v1.m_floats[0] + v2.m_floats[0], 
-			v1.m_floats[1] + v2.m_floats[1], 
-			v1.m_floats[2] + v2.m_floats[2]);
+		v1.m_floats[0] + v2.m_floats[0],
+		v1.m_floats[1] + v2.m_floats[1],
+		v1.m_floats[2] + v2.m_floats[2]);
 #endif
 }
 
 /**@brief Return the elementwise product of two vectors */
-SIMD_FORCE_INLINE btVector3 
-operator*(const btVector3& v1, const btVector3& v2) 
+SIMD_FORCE_INLINE btVector3
+operator*(const btVector3& v1, const btVector3& v2)
 {
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 	return btVector3(_mm_mul_ps(v1.mVec128, v2.mVec128));
 #elif defined(BT_USE_NEON)
 	return btVector3(vmulq_f32(v1.mVec128, v2.mVec128));
 #else
 	return btVector3(
-			v1.m_floats[0] * v2.m_floats[0], 
-			v1.m_floats[1] * v2.m_floats[1], 
-			v1.m_floats[2] * v2.m_floats[2]);
+		v1.m_floats[0] * v2.m_floats[0],
+		v1.m_floats[1] * v2.m_floats[1],
+		v1.m_floats[2] * v2.m_floats[2]);
 #endif
 }
 
 /**@brief Return the difference between two vectors */
-SIMD_FORCE_INLINE btVector3 
+SIMD_FORCE_INLINE btVector3
 operator-(const btVector3& v1, const btVector3& v2)
 {
-#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API)  && defined(BT_USE_SSE))
+#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
 
 	//	without _mm_and_ps this code causes slowdown in Concave moving
 	__m128 r = _mm_sub_ps(v1.mVec128, v2.mVec128);
@@ -794,33 +795,33 @@ operator-(const btVector3& v1, const btVector3& v2)
 	return btVector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask));
 #else
 	return btVector3(
-			v1.m_floats[0] - v2.m_floats[0], 
-			v1.m_floats[1] - v2.m_floats[1], 
-			v1.m_floats[2] - v2.m_floats[2]);
+		v1.m_floats[0] - v2.m_floats[0],
+		v1.m_floats[1] - v2.m_floats[1],
+		v1.m_floats[2] - v2.m_floats[2]);
 #endif
 }
 
 /**@brief Return the negative of the vector */
-SIMD_FORCE_INLINE btVector3 
+SIMD_FORCE_INLINE btVector3
 operator-(const btVector3& v)
 {
-#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
+#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
 	__m128 r = _mm_xor_ps(v.mVec128, btvMzeroMask);
-	return btVector3(_mm_and_ps(r, btvFFF0fMask)); 
+	return btVector3(_mm_and_ps(r, btvFFF0fMask));
 #elif defined(BT_USE_NEON)
 	return btVector3((btSimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)btvMzeroMask));
-#else	
+#else
 	return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
 #endif
 }
 
 /**@brief Return the vector scaled by s */
-SIMD_FORCE_INLINE btVector3 
+SIMD_FORCE_INLINE btVector3
 operator*(const btVector3& v, const btScalar& s)
 {
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
-	__m128	vs = _mm_load_ss(&s);	//	(S 0 0 0)
-	vs = bt_pshufd_ps(vs, 0x80);	//	(S S S 0.0)
+#if defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
+	__m128 vs = _mm_load_ss(&s);  //	(S 0 0 0)
+	vs = bt_pshufd_ps(vs, 0x80);  //	(S S S 0.0)
 	return btVector3(_mm_mul_ps(v.mVec128, vs));
 #elif defined(BT_USE_NEON)
 	float32x4_t r = vmulq_n_f32(v.mVec128, s);
@@ -831,10 +832,10 @@ operator*(const btVector3& v, const btScalar& s)
 }
 
 /**@brief Return the vector scaled by s */
-SIMD_FORCE_INLINE btVector3 
+SIMD_FORCE_INLINE btVector3
 operator*(const btScalar& s, const btVector3& v)
-{ 
-	return v * s; 
+{
+	return v * s;
 }
 
 /**@brief Return the vector inversely scaled by s */
@@ -842,7 +843,7 @@ SIMD_FORCE_INLINE btVector3
 operator/(const btVector3& v, const btScalar& s)
 {
 	btFullAssert(s != btScalar(0.0));
-#if 0 //defined(BT_USE_SSE_IN_API)
+#if 0  //defined(BT_USE_SSE_IN_API)
 // this code is not faster !
 	__m128 vs = _mm_load_ss(&s);
     vs = _mm_div_ss(v1110, vs);
@@ -858,67 +859,65 @@ operator/(const btVector3& v, const btScalar& s)
 SIMD_FORCE_INLINE btVector3
 operator/(const btVector3& v1, const btVector3& v2)
 {
-#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE))
+#if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
 	__m128 vec = _mm_div_ps(v1.mVec128, v2.mVec128);
 	vec = _mm_and_ps(vec, btvFFF0fMask);
-	return btVector3(vec); 
+	return btVector3(vec);
 #elif defined(BT_USE_NEON)
 	float32x4_t x, y, v, m;
 
 	x = v1.mVec128;
 	y = v2.mVec128;
-	
-	v = vrecpeq_f32(y);			// v ~ 1/y
-	m = vrecpsq_f32(y, v);		// m = (2-v*y)
-	v = vmulq_f32(v, m);		// vv = v*m ~~ 1/y
-	m = vrecpsq_f32(y, v);		// mm = (2-vv*y)
-	v = vmulq_f32(v, x);		// x*vv
-	v = vmulq_f32(v, m);		// (x*vv)*(2-vv*y) = x*(vv(2-vv*y)) ~~~ x/y
+
+	v = vrecpeq_f32(y);     // v ~ 1/y
+	m = vrecpsq_f32(y, v);  // m = (2-v*y)
+	v = vmulq_f32(v, m);    // vv = v*m ~~ 1/y
+	m = vrecpsq_f32(y, v);  // mm = (2-vv*y)
+	v = vmulq_f32(v, x);    // x*vv
+	v = vmulq_f32(v, m);    // (x*vv)*(2-vv*y) = x*(vv(2-vv*y)) ~~~ x/y
 
 	return btVector3(v);
 #else
 	return btVector3(
-			v1.m_floats[0] / v2.m_floats[0], 
-			v1.m_floats[1] / v2.m_floats[1],
-			v1.m_floats[2] / v2.m_floats[2]);
+		v1.m_floats[0] / v2.m_floats[0],
+		v1.m_floats[1] / v2.m_floats[1],
+		v1.m_floats[2] / v2.m_floats[2]);
 #endif
 }
 
 /**@brief Return the dot product between two vectors */
-SIMD_FORCE_INLINE btScalar 
-btDot(const btVector3& v1, const btVector3& v2) 
-{ 
-	return v1.dot(v2); 
+SIMD_FORCE_INLINE btScalar
+btDot(const btVector3& v1, const btVector3& v2)
+{
+	return v1.dot(v2);
 }
 
-
 /**@brief Return the distance squared between two vectors */
 SIMD_FORCE_INLINE btScalar
-btDistance2(const btVector3& v1, const btVector3& v2) 
-{ 
-	return v1.distance2(v2); 
+btDistance2(const btVector3& v1, const btVector3& v2)
+{
+	return v1.distance2(v2);
 }
 
-
 /**@brief Return the distance between two vectors */
 SIMD_FORCE_INLINE btScalar
-btDistance(const btVector3& v1, const btVector3& v2) 
-{ 
-	return v1.distance(v2); 
+btDistance(const btVector3& v1, const btVector3& v2)
+{
+	return v1.distance(v2);
 }
 
 /**@brief Return the angle between two vectors */
 SIMD_FORCE_INLINE btScalar
-btAngle(const btVector3& v1, const btVector3& v2) 
-{ 
-	return v1.angle(v2); 
+btAngle(const btVector3& v1, const btVector3& v2)
+{
+	return v1.angle(v2);
 }
 
 /**@brief Return the cross product of two vectors */
-SIMD_FORCE_INLINE btVector3 
-btCross(const btVector3& v1, const btVector3& v2) 
-{ 
-	return v1.cross(v2); 
+SIMD_FORCE_INLINE btVector3
+btCross(const btVector3& v1, const btVector3& v2)
+{
+	return v1.cross(v2);
 }
 
 SIMD_FORCE_INLINE btScalar
@@ -931,14 +930,12 @@ btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
  * @param v1 One vector 
  * @param v2 The other vector 
  * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
-SIMD_FORCE_INLINE btVector3 
+SIMD_FORCE_INLINE btVector3
 lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
 {
 	return v1.lerp(v2, t);
 }
 
-
-
 SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
 {
 	return (v - *this).length2();
@@ -954,140 +951,137 @@ SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
 	btVector3 nrm = *this;
 
 	return nrm.normalize();
-} 
+}
 
-SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const
+SIMD_FORCE_INLINE btVector3 btVector3::rotate(const btVector3& wAxis, const btScalar _angle) const
 {
 	// wAxis must be a unit lenght vector
 
-#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 
-    __m128 O = _mm_mul_ps(wAxis.mVec128, mVec128);
-	btScalar ssin = btSin( _angle );
-    __m128 C = wAxis.cross( mVec128 ).mVec128;
+	__m128 O = _mm_mul_ps(wAxis.mVec128, mVec128);
+	btScalar ssin = btSin(_angle);
+	__m128 C = wAxis.cross(mVec128).mVec128;
 	O = _mm_and_ps(O, btvFFF0fMask);
-    btScalar scos = btCos( _angle );
-	
-	__m128 vsin = _mm_load_ss(&ssin);	//	(S 0 0 0)
-    __m128 vcos = _mm_load_ss(&scos);	//	(S 0 0 0)
-	
-	__m128 Y = bt_pshufd_ps(O, 0xC9);	//	(Y Z X 0)
-	__m128 Z = bt_pshufd_ps(O, 0xD2);	//	(Z X Y 0)
+	btScalar scos = btCos(_angle);
+
+	__m128 vsin = _mm_load_ss(&ssin);  //	(S 0 0 0)
+	__m128 vcos = _mm_load_ss(&scos);  //	(S 0 0 0)
+
+	__m128 Y = bt_pshufd_ps(O, 0xC9);  //	(Y Z X 0)
+	__m128 Z = bt_pshufd_ps(O, 0xD2);  //	(Z X Y 0)
 	O = _mm_add_ps(O, Y);
-	vsin = bt_pshufd_ps(vsin, 0x80);	//	(S S S 0)
+	vsin = bt_pshufd_ps(vsin, 0x80);  //	(S S S 0)
 	O = _mm_add_ps(O, Z);
-    vcos = bt_pshufd_ps(vcos, 0x80);	//	(S S S 0)
-	
-    vsin = vsin * C; 
-	O = O * wAxis.mVec128; 
-	__m128 X = mVec128 - O; 
-	
-    O = O + vsin;
+	vcos = bt_pshufd_ps(vcos, 0x80);  //	(S S S 0)
+
+	vsin = vsin * C;
+	O = O * wAxis.mVec128;
+	__m128 X = mVec128 - O;
+
+	O = O + vsin;
 	vcos = vcos * X;
-	O = O + vcos;	
-	
+	O = O + vcos;
+
 	return btVector3(O);
 #else
-	btVector3 o = wAxis * wAxis.dot( *this );
+	btVector3 o = wAxis * wAxis.dot(*this);
 	btVector3 _x = *this - o;
 	btVector3 _y;
 
-	_y = wAxis.cross( *this );
+	_y = wAxis.cross(*this);
 
-	return ( o + _x * btCos( _angle ) + _y * btSin( _angle ) );
+	return (o + _x * btCos(_angle) + _y * btSin(_angle));
 #endif
 }
 
-SIMD_FORCE_INLINE   long    btVector3::maxDot( const btVector3 *array, long array_count, btScalar &dotOut ) const
+SIMD_FORCE_INLINE long btVector3::maxDot(const btVector3* array, long array_count, btScalar& dotOut) const
 {
-#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
-    #if defined _WIN32 || defined (BT_USE_SSE)
-        const long scalar_cutoff = 10;
-        long _maxdot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
-    #elif defined BT_USE_NEON
-        const long scalar_cutoff = 4;
-        extern long (*_maxdot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
-    #endif
-    if( array_count < scalar_cutoff )	
+#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined(BT_USE_NEON)
+#if defined _WIN32 || defined(BT_USE_SSE)
+	const long scalar_cutoff = 10;
+	long _maxdot_large(const float* array, const float* vec, unsigned long array_count, float* dotOut);
+#elif defined BT_USE_NEON
+	const long scalar_cutoff = 4;
+	extern long (*_maxdot_large)(const float* array, const float* vec, unsigned long array_count, float* dotOut);
 #endif
-    {
-        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 > maxDot1 )
-            {
-                maxDot1 = dot;
-                ptIndex = i;
-            }
-        }
-        
-        dotOut = maxDot1;
-        return ptIndex;
-    }
-#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
-    return _maxdot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
+	if (array_count < scalar_cutoff)
+#endif
+	{
+		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 > maxDot1)
+			{
+				maxDot1 = dot;
+				ptIndex = i;
+			}
+		}
+
+		dotOut = maxDot1;
+		return ptIndex;
+	}
+#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined(BT_USE_NEON)
+	return _maxdot_large((float*)array, (float*)&m_floats[0], array_count, &dotOut);
 #endif
 }
 
-SIMD_FORCE_INLINE   long    btVector3::minDot( const btVector3 *array, long array_count, btScalar &dotOut ) const
+SIMD_FORCE_INLINE long btVector3::minDot(const btVector3* array, long array_count, btScalar& dotOut) const
 {
-#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
-    #if defined BT_USE_SSE
-        const long scalar_cutoff = 10;
-        long _mindot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
-    #elif defined BT_USE_NEON
-        const long scalar_cutoff = 4;
-        extern long (*_mindot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
-    #else
-        #error unhandled arch!
-    #endif
-    
-    if( array_count < scalar_cutoff )
+#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined(BT_USE_NEON)
+#if defined BT_USE_SSE
+	const long scalar_cutoff = 10;
+	long _mindot_large(const float* array, const float* vec, unsigned long array_count, float* dotOut);
+#elif defined BT_USE_NEON
+	const long scalar_cutoff = 4;
+	extern long (*_mindot_large)(const float* array, const float* vec, unsigned long array_count, float* dotOut);
+#else
+#error unhandled arch!
 #endif
-    {
-        btScalar  minDot = SIMD_INFINITY;
-        int i = 0;
-        int ptIndex = -1;
-        
-        for( i = 0; i < array_count; i++ )
-        {
-            btScalar dot = array[i].dot(*this);
-            
-            if( dot < minDot )
-            {
-                minDot = dot;
-                ptIndex = i;
-            }
-        }
-        
-        dotOut = minDot;
-        
-        return ptIndex;
-    }
-#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
-    return _mindot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
-#endif//BT_USE_SIMD_VECTOR3
-}
 
+	if (array_count < scalar_cutoff)
+#endif
+	{
+		btScalar minDot = SIMD_INFINITY;
+		int i = 0;
+		int ptIndex = -1;
+
+		for (i = 0; i < array_count; i++)
+		{
+			btScalar dot = array[i].dot(*this);
+
+			if (dot < minDot)
+			{
+				minDot = dot;
+				ptIndex = i;
+			}
+		}
+
+		dotOut = minDot;
+
+		return ptIndex;
+	}
+#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined(BT_USE_NEON)
+	return _mindot_large((float*)array, (float*)&m_floats[0], array_count, &dotOut);
+#endif  //BT_USE_SIMD_VECTOR3
+}
 
 class btVector4 : public btVector3
 {
 public:
-
 	SIMD_FORCE_INLINE btVector4() {}
 
-
-	SIMD_FORCE_INLINE btVector4(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w) 
-		: btVector3(_x,_y,_z)
+	SIMD_FORCE_INLINE btVector4(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w)
+		: btVector3(_x, _y, _z)
 	{
 		m_floats[3] = _w;
 	}
 
-#if (defined (BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined (BT_USE_NEON) 
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
 	SIMD_FORCE_INLINE btVector4(const btSimdFloat4 vec)
 	{
 		mVec128 = vec;
@@ -1098,34 +1092,32 @@ public:
 		mVec128 = rhs.mVec128;
 	}
 
-	SIMD_FORCE_INLINE btVector4& 
-	operator=(const btVector4& v) 
+	SIMD_FORCE_INLINE btVector4&
+	operator=(const btVector4& v)
 	{
 		mVec128 = v.mVec128;
 		return *this;
 	}
-#endif // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON) 
+#endif  // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
 
-	SIMD_FORCE_INLINE btVector4 absolute4() const 
+	SIMD_FORCE_INLINE btVector4 absolute4() const
 	{
-#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) 
+#if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
 		return btVector4(_mm_and_ps(mVec128, btvAbsfMask));
 #elif defined(BT_USE_NEON)
 		return btVector4(vabsq_f32(mVec128));
-#else	
+#else
 		return btVector4(
-			btFabs(m_floats[0]), 
-			btFabs(m_floats[1]), 
+			btFabs(m_floats[0]),
+			btFabs(m_floats[1]),
 			btFabs(m_floats[2]),
 			btFabs(m_floats[3]));
 #endif
 	}
 
+	btScalar getW() const { return m_floats[3]; }
 
-	btScalar	getW() const { return m_floats[3];}
-
-
-		SIMD_FORCE_INLINE int maxAxis4() const
+	SIMD_FORCE_INLINE int maxAxis4() const
 	{
 		int maxIndex = -1;
 		btScalar maxVal = btScalar(-BT_LARGE_FLOAT);
@@ -1142,18 +1134,16 @@ public:
 		if (m_floats[2] > maxVal)
 		{
 			maxIndex = 2;
-			maxVal =m_floats[2];
+			maxVal = m_floats[2];
 		}
 		if (m_floats[3] > maxVal)
 		{
 			maxIndex = 3;
-			maxVal = m_floats[3];
 		}
 
 		return maxIndex;
 	}
 
-
 	SIMD_FORCE_INLINE int minAxis4() const
 	{
 		int minIndex = -1;
@@ -1171,183 +1161,176 @@ public:
 		if (m_floats[2] < minVal)
 		{
 			minIndex = 2;
-			minVal =m_floats[2];
+			minVal = m_floats[2];
 		}
 		if (m_floats[3] < minVal)
 		{
 			minIndex = 3;
-			minVal = m_floats[3];
 		}
-		
+
 		return minIndex;
 	}
 
-
-	SIMD_FORCE_INLINE int closestAxis4() const 
+	SIMD_FORCE_INLINE int closestAxis4() const
 	{
 		return absolute4().maxAxis4();
 	}
 
-	
- 
-
-  /**@brief Set x,y,z and zero w 
+	/**@brief Set x,y,z and zero w 
    * @param x Value of x
    * @param y Value of y
    * @param z Value of z
    */
-		
 
-/*		void getValue(btScalar *m) const 
+	/*		void getValue(btScalar *m) const 
 		{
 			m[0] = m_floats[0];
 			m[1] = m_floats[1];
 			m[2] =m_floats[2];
 		}
 */
-/**@brief Set the values 
+	/**@brief Set the values 
    * @param x Value of x
    * @param y Value of y
    * @param z Value of z
    * @param w Value of w
    */
-		SIMD_FORCE_INLINE void	setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
-		{
-			m_floats[0]=_x;
-			m_floats[1]=_y;
-			m_floats[2]=_z;
-			m_floats[3]=_w;
-		}
-
-
+	SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w)
+	{
+		m_floats[0] = _x;
+		m_floats[1] = _y;
+		m_floats[2] = _z;
+		m_floats[3] = _w;
+	}
 };
 
-
 ///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
-SIMD_FORCE_INLINE void	btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
+SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
 {
-	#ifdef BT_USE_DOUBLE_PRECISION
-	unsigned char* dest = (unsigned char*) &destVal;
-	unsigned char* src  = (unsigned char*) &sourceVal;
+#ifdef BT_USE_DOUBLE_PRECISION
+	unsigned char* dest = (unsigned char*)&destVal;
+	const unsigned char* src = (const unsigned char*)&sourceVal;
 	dest[0] = src[7];
-    dest[1] = src[6];
-    dest[2] = src[5];
-    dest[3] = src[4];
-    dest[4] = src[3];
-    dest[5] = src[2];
-    dest[6] = src[1];
-    dest[7] = src[0];
+	dest[1] = src[6];
+	dest[2] = src[5];
+	dest[3] = src[4];
+	dest[4] = src[3];
+	dest[5] = src[2];
+	dest[6] = src[1];
+	dest[7] = src[0];
 #else
-	unsigned char* dest = (unsigned char*) &destVal;
-	unsigned char* src  = (unsigned char*) &sourceVal;
+	unsigned char* dest = (unsigned char*)&destVal;
+	const unsigned char* src = (const unsigned char*)&sourceVal;
 	dest[0] = src[3];
-    dest[1] = src[2];
-    dest[2] = src[1];
-    dest[3] = src[0];
-#endif //BT_USE_DOUBLE_PRECISION
+	dest[1] = src[2];
+	dest[2] = src[1];
+	dest[3] = src[0];
+#endif  //BT_USE_DOUBLE_PRECISION
 }
 ///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
-SIMD_FORCE_INLINE void	btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
+SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
 {
-	for (int i=0;i<4;i++)
+	for (int i = 0; i < 4; i++)
 	{
-		btSwapScalarEndian(sourceVec[i],destVec[i]);
+		btSwapScalarEndian(sourceVec[i], destVec[i]);
 	}
-
 }
 
 ///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
-SIMD_FORCE_INLINE void	btUnSwapVector3Endian(btVector3& vector)
+SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector)
 {
-
-	btVector3	swappedVec;
-	for (int i=0;i<4;i++)
+	btVector3 swappedVec;
+	for (int i = 0; i < 4; i++)
 	{
-		btSwapScalarEndian(vector[i],swappedVec[i]);
+		btSwapScalarEndian(vector[i], swappedVec[i]);
 	}
 	vector = swappedVec;
 }
 
 template <class T>
-SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q)
+SIMD_FORCE_INLINE void btPlaneSpace1(const T& n, T& p, T& q)
 {
-  if (btFabs(n[2]) > SIMDSQRT12) {
-    // choose p in y-z plane
-    btScalar a = n[1]*n[1] + n[2]*n[2];
-    btScalar k = btRecipSqrt (a);
-    p[0] = 0;
-	p[1] = -n[2]*k;
-	p[2] = n[1]*k;
-    // set q = n x p
-    q[0] = a*k;
-	q[1] = -n[0]*p[2];
-	q[2] = n[0]*p[1];
-  }
-  else {
-    // choose p in x-y plane
-    btScalar a = n[0]*n[0] + n[1]*n[1];
-    btScalar k = btRecipSqrt (a);
-    p[0] = -n[1]*k;
-	p[1] = n[0]*k;
-	p[2] = 0;
-    // set q = n x p
-    q[0] = -n[2]*p[1];
-	q[1] = n[2]*p[0];
-	q[2] = a*k;
-  }
+	if (btFabs(n[2]) > SIMDSQRT12)
+	{
+		// choose p in y-z plane
+		btScalar a = n[1] * n[1] + n[2] * n[2];
+		btScalar k = btRecipSqrt(a);
+		p[0] = 0;
+		p[1] = -n[2] * k;
+		p[2] = n[1] * k;
+		// set q = n x p
+		q[0] = a * k;
+		q[1] = -n[0] * p[2];
+		q[2] = n[0] * p[1];
+	}
+	else
+	{
+		// choose p in x-y plane
+		btScalar a = n[0] * n[0] + n[1] * n[1];
+		btScalar k = btRecipSqrt(a);
+		p[0] = -n[1] * k;
+		p[1] = n[0] * k;
+		p[2] = 0;
+		// set q = n x p
+		q[0] = -n[2] * p[1];
+		q[1] = n[2] * p[0];
+		q[2] = a * k;
+	}
 }
 
-
-struct	btVector3FloatData
+struct btVector3FloatData
 {
-	float	m_floats[4];
+	float m_floats[4];
 };
 
-struct	btVector3DoubleData
+struct btVector3DoubleData
 {
-	double	m_floats[4];
-
+	double m_floats[4];
 };
 
-SIMD_FORCE_INLINE	void	btVector3::serializeFloat(struct	btVector3FloatData& dataOut) const
+SIMD_FORCE_INLINE void btVector3::serializeFloat(struct btVector3FloatData& dataOut) const
 {
 	///could also do a memcpy, check if it is worth it
-	for (int i=0;i<4;i++)
+	for (int i = 0; i < 4; i++)
 		dataOut.m_floats[i] = float(m_floats[i]);
 }
 
-SIMD_FORCE_INLINE void	btVector3::deSerializeFloat(const struct	btVector3FloatData& dataIn)
+SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData& dataIn)
 {
-	for (int i=0;i<4;i++)
+	for (int i = 0; i < 4; i++)
 		m_floats[i] = btScalar(dataIn.m_floats[i]);
 }
 
-
-SIMD_FORCE_INLINE	void	btVector3::serializeDouble(struct	btVector3DoubleData& dataOut) const
+SIMD_FORCE_INLINE void btVector3::serializeDouble(struct btVector3DoubleData& dataOut) const
 {
 	///could also do a memcpy, check if it is worth it
-	for (int i=0;i<4;i++)
+	for (int i = 0; i < 4; i++)
 		dataOut.m_floats[i] = double(m_floats[i]);
 }
 
-SIMD_FORCE_INLINE void	btVector3::deSerializeDouble(const struct	btVector3DoubleData& dataIn)
+SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData& dataIn)
 {
-	for (int i=0;i<4;i++)
+	for (int i = 0; i < 4; i++)
 		m_floats[i] = btScalar(dataIn.m_floats[i]);
 }
 
-
-SIMD_FORCE_INLINE	void	btVector3::serialize(struct	btVector3Data& dataOut) const
+SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& dataOut) const
 {
 	///could also do a memcpy, check if it is worth it
-	for (int i=0;i<4;i++)
+	for (int i = 0; i < 4; i++)
 		dataOut.m_floats[i] = m_floats[i];
 }
 
-SIMD_FORCE_INLINE void	btVector3::deSerialize(const struct	btVector3Data& dataIn)
+SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3FloatData& dataIn)
+{
+	for (int i = 0; i < 4; i++)
+		m_floats[i] = (btScalar)dataIn.m_floats[i];
+}
+
+SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3DoubleData& dataIn)
 {
-	for (int i=0;i<4;i++)
-		m_floats[i] = dataIn.m_floats[i];
+	for (int i = 0; i < 4; i++)
+		m_floats[i] = (btScalar)dataIn.m_floats[i];
 }
 
-#endif //BT_VECTOR3_H
+#endif  //BT_VECTOR3_H
diff --git a/extern/bullet2/src/btBulletCollisionCommon.h b/extern/bullet2/src/btBulletCollisionCommon.h
index af981b5d36d..4f523756a74 100644
--- a/extern/bullet2/src/btBulletCollisionCommon.h
+++ b/extern/bullet2/src/btBulletCollisionCommon.h
@@ -52,7 +52,6 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
 #include "BulletCollision/BroadphaseCollision/btAxisSweep3.h"
-#include "BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h"
 #include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h"
 
 ///Math library & Utils
@@ -63,6 +62,4 @@ subject to the following restrictions:
 #include "LinearMath/btIDebugDraw.h"
 #include "LinearMath/btSerializer.h"
 
-
-#endif //BULLET_COLLISION_COMMON_H
-
+#endif  //BULLET_COLLISION_COMMON_H
diff --git a/extern/bullet2/src/btBulletDynamicsCommon.h b/extern/bullet2/src/btBulletDynamicsCommon.h
index 50282bf2105..a421fa4461f 100644
--- a/extern/bullet2/src/btBulletDynamicsCommon.h
+++ b/extern/bullet2/src/btBulletDynamicsCommon.h
@@ -35,17 +35,9 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btGearConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btFixedConstraint.h"
 
-
 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
 
-
 ///Vehicle simulation, with wheel contact simulated by raycasts
 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"
 
-
-
-
-
-
-#endif //BULLET_DYNAMICS_COMMON_H
-
+#endif  //BULLET_DYNAMICS_COMMON_H