Index: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp	(revision 2882)
@@ -20,5 +20,4 @@
 #include "btAxisSweep3.h"
 
-#include <assert.h>
 
 btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
Index: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h	(revision 2882)
@@ -43,4 +43,5 @@
 public:
 	
+ BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	class Edge
@@ -139,5 +140,5 @@
 	SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
 
-	void resetPool();
+	virtual void resetPool(btDispatcher* dispatcher);
 
 	void	processAllOverlappingPairs(btOverlapCallback* callback);
@@ -221,5 +222,5 @@
 
 	if (checkCardinality)
-		assert(numEdges == m_numHandles*2+1);
+		btAssert(numEdges == m_numHandles*2+1);
 }
 #endif //DEBUG_BROADPHASE
@@ -347,5 +348,5 @@
 	}
 
-	//assert(bounds.HasVolume());
+	//btAssert(bounds.HasVolume());
 
 	// init bounds
@@ -453,5 +454,5 @@
 BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle()
 {
-	assert(m_firstFreeHandle);
+	btAssert(m_firstFreeHandle);
 
 	BP_FP_INT_TYPE handle = m_firstFreeHandle;
@@ -465,5 +466,5 @@
 void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
 {
-	assert(handle > 0 && handle < m_maxHandles);
+	btAssert(handle > 0 && handle < m_maxHandles);
 
 	getHandle(handle)->SetNextFree(m_firstFreeHandle);
@@ -588,5 +589,5 @@
 
 template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool()
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool(btDispatcher* dispatcher)
 {
 	if (m_numHandles == 0)
@@ -642,4 +643,5 @@
 			if (!isDuplicate)
 			{
+				///important to use an AABB test that is consistent with the broadphase
 				bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
 
@@ -687,8 +689,4 @@
 	}
 
-
-
-	
-
 }
 
@@ -731,6 +729,6 @@
 void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
 {
-//	assert(bounds.IsFinite());
-	//assert(bounds.HasVolume());
+//	btAssert(bounds.IsFinite());
+	//btAssert(bounds.HasVolume());
 
 	Handle* pHandle = getHandle(handle);
Index: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h	(revision 2882)
@@ -65,4 +65,7 @@
 	virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const =0;
 
+	///reset broadphase internal structures, to ensure determinism/reproducability
+	virtual void resetPool(btDispatcher* dispatcher) {};
+
 	virtual void	printStats() = 0;
 
Index: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h	(revision 2882)
@@ -70,5 +70,6 @@
 
 	SOFTBODY_SHAPE_PROXYTYPE,
-
+	HFFLUID_SHAPE_PROXYTYPE,
+	HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE,
 	INVALID_SHAPE_PROXYTYPE,
 
@@ -188,5 +189,5 @@
 
 		//keep them sorted, so the std::set operations work
-		if (&proxy0 < &proxy1)
+		if (proxy0.m_uniqueId < proxy1.m_uniqueId)
         { 
             m_pProxy0 = &proxy0; 
@@ -229,6 +230,11 @@
 		bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b )
 		{
-			 return a.m_pProxy0 > b.m_pProxy0 || 
-				(a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 > b.m_pProxy1) ||
+			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); 
 		}
Index: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp	(revision 2882)
@@ -394,5 +394,5 @@
 }
 
-//
+#if 0
 static DBVT_INLINE btDbvtNode*	walkup(btDbvtNode* n,int count)
 {
@@ -400,4 +400,5 @@
 	return(n);
 }
+#endif
 
 //
@@ -424,7 +425,12 @@
 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_stkStack.clear();
+	m_opath		=	0;
+	
 }
 
Index: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h	(revision 2882)
@@ -58,5 +58,5 @@
 
 //SSE gives errors on a MSVC 7.1
-#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
+#ifdef BT_USE_SSE
 #define DBVT_SELECT_IMPL		DBVT_IMPL_SSE
 #define DBVT_MERGE_IMPL			DBVT_IMPL_SSE
@@ -83,5 +83,5 @@
 #define DBVT_PREFIX					template <typename T>
 #define DBVT_IPOLICY				T& policy
-#define DBVT_CHECKTYPE				static const ICollide&	typechecker=*(T*)0;
+#define DBVT_CHECKTYPE				static const ICollide&	typechecker=*(T*)1;(void)typechecker;
 #else
 #define	DBVT_VIRTUAL_DTOR(a)		virtual ~a() {}
@@ -147,7 +147,5 @@
 	DBVT_INLINE friend bool			Intersect(	const btDbvtAabbMm& a,
 		const btDbvtAabbMm& b);
-	DBVT_INLINE friend bool			Intersect(	const btDbvtAabbMm& a,
-		const btDbvtAabbMm& b,
-		const btTransform& xform);
+	
 	DBVT_INLINE friend bool			Intersect(	const btDbvtAabbMm& a,
 		const btVector3& b);
@@ -305,5 +303,5 @@
 		  const btDbvtNode* root1,
 		  DBVT_IPOLICY);
-
+#if 0
 	DBVT_PREFIX
 		void		collideTT(	const btDbvtNode* root0,
@@ -317,4 +315,6 @@
 		const btTransform& xform1,
 		DBVT_IPOLICY);
+#endif
+
 	DBVT_PREFIX
 		void		collideTV(	const btDbvtNode* root,
@@ -531,19 +531,5 @@
 }
 
-//
-DBVT_INLINE bool		Intersect(	const btDbvtAabbMm& a,
-								  const btDbvtAabbMm& b,
-								  const btTransform& xform)
-{
-	const btVector3		d0=xform*b.Center()-a.Center();
-	const btVector3		d1=d0*xform.getBasis();
-	btScalar			s0[2]={0,0};
-	btScalar			s1[2]={dot(xform.getOrigin(),d0),s1[0]};
-	a.AddSpan(d0,s0[0],s0[1]);
-	b.AddSpan(d1,s1[0],s1[1]);
-	if(s0[0]>(s1[1])) return(false);
-	if(s0[1]<(s1[0])) return(false);
-	return(true);
-}
+
 
 //
@@ -849,5 +835,5 @@
 }
 
-
+#if 0
 //
 DBVT_PREFIX
@@ -905,5 +891,4 @@
 		}
 }
-
 //
 DBVT_PREFIX
@@ -917,4 +902,5 @@
 	collideTT(root0,root1,xform,policy);
 }
+#endif 
 
 //
Index: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp	(revision 2882)
@@ -101,5 +101,6 @@
 			btDbvtProxy*	pb=(btDbvtProxy*)nb->data;
 #if DBVT_BP_SORTPAIRS
-			if(pa>pb) btSwap(pa,pb);
+			if(pa->m_uniqueId>pb->m_uniqueId) 
+				btSwap(pa,pb);
 #endif
 			pbp->m_paircache->addOverlappingPair(pa,pb);
@@ -133,7 +134,5 @@
 	m_updates_done		=	0;
 	m_updates_ratio		=	0;
-	m_paircache			=	paircache?
-paircache	:
-	new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
+	m_paircache			=	paircache? paircache	: new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
 	m_gid				=	0;
 	m_pid				=	0;
@@ -211,21 +210,20 @@
 }
 
+struct	BroadphaseRayTester : btDbvt::ICollide
+{
+	btBroadphaseRayCallback& m_rayCallback;
+	BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
+		:m_rayCallback(orgCallback)
+	{
+	}
+	void					Process(const btDbvtNode* leaf)
+	{
+		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)
 {
-
-	struct	BroadphaseRayTester : btDbvt::ICollide
-	{
-		btBroadphaseRayCallback& m_rayCallback;
-		BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
-			:m_rayCallback(orgCallback)
-		{
-		}
-		void					Process(const btDbvtNode* leaf)
-		{
-			btDbvtProxy*	proxy=(btDbvtProxy*)leaf->data;
-			m_rayCallback.process(proxy);
-		}
-	};	
-
 	BroadphaseRayTester callback(rayCallback);
 
@@ -342,4 +340,79 @@
 	}
 #endif
+
+	performDeferredRemoval(dispatcher);
+
+}
+
+void btDbvtBroadphase::performDeferredRemoval(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());
+
+		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++)
+		{
+		
+			btBroadphasePair& pair = overlappingPairArray[i];
+
+			bool isDuplicate = (pair == previousPair);
+
+			previousPair = pair;
+
+			bool needsRemoval = false;
+
+			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);
+
+				if (hasOverlap)
+				{
+					needsRemoval = false;
+				} else
+				{
+					needsRemoval = true;
+				}
+			} else
+			{
+				//remove duplicate
+				needsRemoval = true;
+				//should have no algorithm
+				btAssert(!pair.m_algorithm);
+			}
+			
+			if (needsRemoval)
+			{
+				m_paircache->cleanOverlappingPair(pair,dispatcher);
+
+				pair.m_pProxy0 = 0;
+				pair.m_pProxy1 = 0;
+				invalidPair++;
+			} 
+			
+		}
+
+		//perform a sort, to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+		overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
+	}
 }
 
@@ -347,4 +420,21 @@
 void							btDbvtBroadphase::collide(btDispatcher* dispatcher)
 {
+	/*printf("---------------------------------------------------------\n");
+	printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
+	printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves);
+	printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs());
+	{
+		int i;
+		for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
+		{
+			printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
+				getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
+		}
+		printf("\n");
+	}
+*/
+
+
+
 	SPC(m_profiling.m_total);
 	/* optimize				*/ 
@@ -402,9 +492,9 @@
 		if(pairs.size()>0)
 		{
-			const int	ci=pairs.size();
-			int			ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
+
+			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)%ci];
+				btBroadphasePair&	p=pairs[(m_cid+i)%pairs.size()];
 				btDbvtProxy*		pa=(btDbvtProxy*)p.m_pProxy0;
 				btDbvtProxy*		pb=(btDbvtProxy*)p.m_pProxy1;
@@ -412,5 +502,6 @@
 				{
 #if DBVT_BP_SORTPAIRS
-					if(pa>pb) btSwap(pa,pb);
+					if(pa->m_uniqueId>pb->m_uniqueId) 
+						btSwap(pa,pb);
 #endif
 					m_paircache->removeOverlappingPair(pa,pb,dispatcher);
@@ -467,4 +558,37 @@
 	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_prediction		=	1/(btScalar)2;
+		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;
+		}
+	}
 }
 
@@ -601,2 +725,3 @@
 #undef	SPC
 #endif
+
Index: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h	(revision 2882)
@@ -25,5 +25,5 @@
 
 #define	DBVT_BP_PROFILE					0
-#define DBVT_BP_SORTPAIRS				1
+//#define DBVT_BP_SORTPAIRS				1
 #define DBVT_BP_PREVENTFALSEUPDATE		0
 #define DBVT_BP_ACCURATESLEEPING		0
@@ -115,4 +115,11 @@
 	void							printStats();
 	static void						benchmark(btBroadphaseInterface*);
+
+
+	void	performDeferredRemoval(btDispatcher* dispatcher);
+
+	///reset broadphase internal structures, to ensure determinism/reproducability
+	virtual void resetPool(btDispatcher* dispatcher);
+
 };
 
Index: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp	(revision 2882)
@@ -483,2 +483,7 @@
 
 }
+
+void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher)
+{
+	// not yet
+}
Index: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h	(revision 2882)
@@ -144,4 +144,7 @@
 	void quicksort (btBroadphasePairArray& a, int lo, int hi);
 
+	///reset broadphase internal structures, to ensure determinism/reproducability
+	virtual void resetPool(btDispatcher* dispatcher);
+
 };
 
Index: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp	(revision 2882)
@@ -20,4 +20,5 @@
 #include "btDispatcher.h"
 #include "btCollisionAlgorithm.h"
+#include "LinearMath/btAabbUtil2.h"
 
 #include <stdio.h>
@@ -136,5 +137,6 @@
 {
 	gFindPairs++;
-	if(proxy0>proxy1) btSwap(proxy0,proxy1);
+	if(proxy0->m_uniqueId>proxy1->m_uniqueId) 
+		btSwap(proxy0,proxy1);
 	int proxyId1 = proxy0->getUid();
 	int proxyId2 = proxy1->getUid();
@@ -212,5 +214,6 @@
 btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
 {
-	if(proxy0>proxy1) btSwap(proxy0,proxy1);
+	if(proxy0->m_uniqueId>proxy1->m_uniqueId) 
+		btSwap(proxy0,proxy1);
 	int proxyId1 = proxy0->getUid();
 	int proxyId2 = proxy1->getUid();
@@ -271,5 +274,6 @@
 {
 	gRemovePairs++;
-	if(proxy0>proxy1) btSwap(proxy0,proxy1);
+	if(proxy0->m_uniqueId>proxy1->m_uniqueId) 
+		btSwap(proxy0,proxy1);
 	int proxyId1 = proxy0->getUid();
 	int proxyId2 = proxy1->getUid();
@@ -393,4 +397,33 @@
 }
 
+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++)
+	{
+		tmpPairs.push_back(m_overlappingPairArray[i]);
+	}
+
+	for (i=0;i<tmpPairs.size();i++)
+	{
+		removeOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1,dispatcher);
+	}
+	
+	for (i = 0; i < m_next.size(); i++)
+	{
+		m_next[i] = BT_NULL_PAIR;
+	}
+
+	tmpPairs.quickSort(btBroadphasePairSortPredicate());
+
+	for (i=0;i<tmpPairs.size();i++)
+	{
+		addOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1);
+	}
+
+	
+}
 
 
@@ -430,5 +463,5 @@
 {
 	//don't add overlap with own
-	assert(proxy0 != proxy1);
+	btAssert(proxy0 != proxy1);
 
 	if (!needsBroadphaseCollision(proxy0,proxy1))
@@ -461,5 +494,5 @@
 	if (findIndex < m_overlappingPairArray.size())
 	{
-		//assert(it != m_overlappingPairSet.end());
+		//btAssert(it != m_overlappingPairSet.end());
 		 btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
 		return pair;
@@ -491,6 +524,7 @@
 		{
 			cleanOverlappingPair(*pair,dispatcher);
-
-			m_overlappingPairArray.swap(i,m_overlappingPairArray.capacity()-1);
+			pair->m_pProxy0 = 0;
+			pair->m_pProxy1 = 0;
+			m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
 			m_overlappingPairArray.pop_back();
 			gOverlappingPairs--;
@@ -592,2 +626,8 @@
 	processAllOverlappingPairs(&removeCallback,dispatcher);
 }
+
+void	btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
+{
+	//should already be sorted
+}
+
Index: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h	(revision 2882)
@@ -84,4 +84,7 @@
 
 	virtual	void	setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0;
+
+	virtual void	sortOverlappingPairs(btDispatcher* dispatcher) = 0;
+
 
 };
@@ -260,5 +263,8 @@
 	}
 
-public:
+	virtual void	sortOverlappingPairs(btDispatcher* dispatcher);
+	
+
+protected:
 	
 	btAlignedObjectArray<int>	m_hashTable;
@@ -370,4 +376,6 @@
 		}
 
+		virtual void	sortOverlappingPairs(btDispatcher* dispatcher);
+		
 
 };
@@ -448,4 +456,7 @@
 	}
 	
+	virtual void	sortOverlappingPairs(btDispatcher* dispatcher)
+	{
+	}
 
 
Index: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp	(revision 2882)
@@ -470,5 +470,4 @@
 
 #ifdef RAYAABB2
-	btVector3 rayFrom = raySource;
 	btVector3 rayDir = (rayTarget-raySource);
 	rayDir.normalize ();
@@ -559,5 +558,4 @@
 
 #ifdef RAYAABB2
-	btVector3 rayFrom = raySource;
 	btVector3 rayDirection = (rayTarget-raySource);
 	rayDirection.normalize ();
@@ -818,4 +816,5 @@
 #include <new>
 
+#if 0
 //PCK: consts
 static const unsigned BVH_ALIGNMENT = 16;
@@ -823,5 +822,5 @@
 
 static const unsigned BVH_ALIGNMENT_BLOCKS = 2;
-
+#endif
 
 
@@ -1147,2 +1146,3 @@
 
 
+
Index: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp	(revision 2882)
@@ -325,4 +325,6 @@
 }
 
-
-
+void	btSimpleBroadphase::resetPool(btDispatcher* dispatcher)
+{
+	//not yet
+}
Index: code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h	(revision 2882)
@@ -109,4 +109,7 @@
 	}
 
+	///reset broadphase internal structures, to ensure determinism/reproducability
+	virtual void resetPool(btDispatcher* dispatcher);
+
 
 	void	validate();
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp	(revision 2882)
@@ -53,5 +53,5 @@
 		{
 			m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
-			assert(m_doubleDispatch[i][j]);
+			btAssert(m_doubleDispatch[i][j]);
 		}
 	}
@@ -80,6 +80,10 @@
 	btCollisionObject* body1 = (btCollisionObject*)b1;
 
-	btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()));
-	
+	//test for Bullet 2.74: use a relative contact breaking threshold without clamping against 'gContactBreakingThreshold'
+	//btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()));
+	btScalar contactBreakingThreshold = btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold());
+
+	btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold());
+		
 	void* mem = 0;
 	
@@ -92,5 +96,5 @@
 
 	}
-	btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold);
+	btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold);
 	manifold->m_index1a = m_manifoldsPtr.size();
 	m_manifoldsPtr.push_back(manifold);
@@ -143,5 +147,4 @@
 	return algo;
 }
-
 
 
@@ -161,6 +164,6 @@
 bool	btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionObject* body1)
 {
-	assert(body0);
-	assert(body1);
+	btAssert(body0);
+	btAssert(body1);
 
 	bool needsCollision = true;
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp	(revision 2882)
@@ -18,5 +18,8 @@
 
 btCollisionObject::btCollisionObject()
-	:	m_broadphaseHandle(0),
+	:	m_anisotropicFriction(1.f,1.f,1.f),
+	m_hasAnisotropicFriction(false),
+	m_contactProcessingThreshold(1e30f),
+		m_broadphaseHandle(0),
 		m_collisionShape(0),
 		m_rootCollisionShape(0),
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h	(revision 2882)
@@ -53,4 +53,9 @@
 	btVector3	m_interpolationLinearVelocity;
 	btVector3	m_interpolationAngularVelocity;
+	
+	btVector3		m_anisotropicFriction;
+	bool				m_hasAnisotropicFriction;
+	btScalar		m_contactProcessingThreshold;	
+
 	btBroadphaseProxy*		m_broadphaseHandle;
 	btCollisionShape*		m_collisionShape;
@@ -115,8 +120,9 @@
 		CO_COLLISION_OBJECT =1,
 		CO_RIGID_BODY,
-		CO_SOFT_BODY,
 		///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
+		CO_GHOST_OBJECT,
+		CO_SOFT_BODY,
+		CO_HF_FLUID
 	};
 
@@ -127,4 +133,28 @@
 	}
 
+	const btVector3& getAnisotropicFriction() const
+	{
+		return m_anisotropicFriction;
+	}
+	void	setAnisotropicFriction(const btVector3& anisotropicFriction)
+	{
+		m_anisotropicFriction = anisotropicFriction;
+		m_hasAnisotropicFriction = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f);
+	}
+	bool	hasAnisotropicFriction() const
+	{
+		return m_hasAnisotropicFriction;
+	}
+
+	///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)
+	{
+		m_contactProcessingThreshold = contactProcessingThreshold;
+	}
+	btScalar	getContactProcessingThreshold() const
+	{
+		return m_contactProcessingThreshold;
+	}
 
 	SIMD_FORCE_INLINE bool		isStaticObject() const {
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp	(revision 2882)
@@ -70,4 +70,5 @@
 			getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
 			getBroadphase()->destroyProxy(bp,m_dispatcher1);
+			collisionObject->setBroadphaseHandle(0);
 		}
 	}
@@ -119,4 +120,37 @@
 
 
+void	btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
+{
+	btVector3 minAabb,maxAabb;
+	colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+	//need to increase the aabb for contact thresholds
+	btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
+	minAabb -= contactThreshold;
+	maxAabb += contactThreshold;
+
+	btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
+
+	//moving objects should be moderately sized, probably something wrong if not
+	if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
+	{
+		bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
+	} else
+	{
+		//something went wrong, investigate
+		//this assert is unwanted in 3D modelers (danger of loosing work)
+		colObj->setActivationState(DISABLE_SIMULATION);
+
+		static bool reportMe = true;
+		if (reportMe && m_debugDrawer)
+		{
+			reportMe = false;
+			m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
+			m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
+			m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
+			m_debugDrawer->reportErrorWarning("Thanks.\n");
+		}
+	}
+}
+
 void	btCollisionWorld::updateAabbs()
 {
@@ -131,36 +165,7 @@
 		if (colObj->isActive())
 		{
-			btVector3 minAabb,maxAabb;
-			colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
-			//need to increase the aabb for contact thresholds
-			btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
-			minAabb -= contactThreshold;
-			maxAabb += contactThreshold;
-
-			btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
-
-			//moving objects should be moderately sized, probably something wrong if not
-			if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
-			{
-				bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
-			} else
-			{
-				//something went wrong, investigate
-				//this assert is unwanted in 3D modelers (danger of loosing work)
-				colObj->setActivationState(DISABLE_SIMULATION);
-
-				static bool reportMe = true;
-				if (reportMe && m_debugDrawer)
-				{
-					reportMe = false;
-					m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
-					m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
-					m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
-					m_debugDrawer->reportErrorWarning("Thanks.\n");
-				}
-			}
-		}
-	}
-
+			updateSingleAabb(colObj);
+		}
+	}
 }
 
@@ -294,5 +299,6 @@
 					BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
 						btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*	triangleMesh):
-						btTriangleRaycastCallback(from,to),
+                  //@BP Mod
+						btTriangleRaycastCallback(from,to, resultCallback->m_flags),
 							m_resultCallback(resultCallback),
 							m_collisionObject(collisionObject),
@@ -343,5 +349,6 @@
 					BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
 						btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape*	triangleMesh):
-						btTriangleRaycastCallback(from,to),
+                  //@BP Mod
+                  btTriangleRaycastCallback(from,to, resultCallback->m_flags),
 							m_resultCallback(resultCallback),
 							m_collisionObject(collisionObject),
@@ -664,5 +671,5 @@
 			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
 			//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
-			
+#if 0
 #ifdef RECALCULATE_AABB
 			btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
@@ -672,4 +679,5 @@
 			const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
 			const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
+#endif
 #endif
 			//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h	(revision 2882)
@@ -139,6 +139,7 @@
 	}
 
+	void	updateSingleAabb(btCollisionObject* colObj);
+
 	virtual void	updateAabbs();
-
 
 	
@@ -193,4 +194,6 @@
 		short int	m_collisionFilterGroup;
 		short int	m_collisionFilterMask;
+      //@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback
+      unsigned int m_flags;
 
 		virtual ~RayResultCallback()
@@ -206,5 +209,7 @@
 			m_collisionObject(0),
 			m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
-			m_collisionFilterMask(btBroadphaseProxy::AllFilter)
+			m_collisionFilterMask(btBroadphaseProxy::AllFilter),
+         //@BP Mod
+         m_flags(0)
 		{
 		}
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp	(revision 2882)
@@ -20,4 +20,5 @@
 #include "LinearMath/btIDebugDraw.h"
 #include "LinearMath/btAabbUtil2.h"
+#include "btManifoldResult.h"
 
 btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
@@ -29,8 +30,20 @@
 
 	btCollisionObject* colObj = m_isSwapped? body1 : body0;
+	btAssert (colObj->getCollisionShape()->isCompound());
+	
+	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+	m_compoundShapeRevision = compoundShape->getUpdateRevision();
+	
+	preallocateChildAlgorithms(body0,body1);
+}
+
+void	btCompoundCollisionAlgorithm::preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1)
+{
+	btCollisionObject* colObj = m_isSwapped? body1 : body0;
 	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
-	assert (colObj->getCollisionShape()->isCompound());
+	btAssert (colObj->getCollisionShape()->isCompound());
 	
 	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+
 	int numChildren = compoundShape->getNumChildShapes();
 	int i;
@@ -47,5 +60,5 @@
 			btCollisionShape* childShape = compoundShape->getChildShape(i);
 			colObj->internalSetTemporaryCollisionShape( childShape );
-			m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj,m_sharedManifold);
+			m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(colObj,otherObj,m_sharedManifold);
 			colObj->internalSetTemporaryCollisionShape( tmpShape );
 		}
@@ -53,6 +66,5 @@
 }
 
-
-btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
+void	btCompoundCollisionAlgorithm::removeChildAlgorithms()
 {
 	int numChildren = m_childCollisionAlgorithms.size();
@@ -66,4 +78,9 @@
 		}
 	}
+}
+
+btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
+{
+	removeChildAlgorithms();
 }
 
@@ -168,6 +185,19 @@
 	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
 
-	assert (colObj->getCollisionShape()->isCompound());
+	
+
+	btAssert (colObj->getCollisionShape()->isCompound());
 	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+
+	///btCompoundShape might have changed:
+	////make sure the internal child collision algorithm caches are still valid
+	if (compoundShape->getUpdateRevision() != m_compoundShapeRevision)
+	{
+		///clear and update all
+		removeChildAlgorithms();
+		
+		preallocateChildAlgorithms(body0,body1);
+	}
+
 
 	btDbvt* tree = compoundShape->getDynamicAabbTree();
@@ -175,4 +205,28 @@
 	btCompoundLeafCallback  callback(colObj,otherObj,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++)
+		{
+			if (m_childCollisionAlgorithms[i])
+			{
+				m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
+				for (int m=0;m<manifoldArray.size();m++)
+				{
+					if (manifoldArray[m]->getNumContacts())
+					{
+						resultOut->setPersistentManifold(manifoldArray[m]);
+						resultOut->refreshContactPoints();
+						resultOut->setPersistentManifold(0);//??necessary?
+					}
+				}
+				manifoldArray.clear();
+			}
+		}
+	}
 
 	if (tree)
@@ -243,5 +297,5 @@
 	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
 
-	assert (colObj->getCollisionShape()->isCompound());
+	btAssert (colObj->getCollisionShape()->isCompound());
 	
 	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
@@ -286,2 +340,3 @@
 
 
+
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h	(revision 2882)
@@ -27,4 +27,5 @@
 #include "LinearMath/btAlignedObjectArray.h"
 class btDispatcher;
+class btCollisionObject;
 
 /// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
@@ -36,5 +37,11 @@
 	class btPersistentManifold*	m_sharedManifold;
 	bool					m_ownsManifold;
+
+	int	m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated
 	
+	void	removeChildAlgorithms();
+	
+	void	preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1);
+
 public:
 
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp	(revision 2882)
@@ -52,4 +52,6 @@
 btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
 {
+	m_numPerturbationIterations = 0;
+	m_minimumPointsPerturbationThreshold = 3;
 	m_simplexSolver = simplexSolver;
 	m_pdSolver = pdSolver;
@@ -60,5 +62,5 @@
 }
 
-btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
 : btActivatingCollisionAlgorithm(ci,body0,body1),
 m_simplexSolver(simplexSolver),
@@ -66,9 +68,11 @@
 m_ownManifold (false),
 m_manifoldPtr(mf),
-m_lowLevelOfDetail(false)
+m_lowLevelOfDetail(false),
 #ifdef USE_SEPDISTANCE_UTIL2
 ,m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
-			  (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc())
+			  (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
 #endif
+m_numPerturbationIterations(numPerturbationIterations),
+m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
 {
 	(void)body0;
@@ -94,6 +98,61 @@
 
 
-
-
+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_perturbA(perturbA),
+		m_unPerturbedTransform(unPerturbedTransform),
+		m_debugDrawer(debugDrawer)
+	{
+	}
+	virtual ~ btPerturbedContactResult()
+	{
+	}
+
+	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
+	{
+		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
+		{
+			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_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
+	}
+
+};
+
+extern btScalar gContactBreakingThreshold;
 
 //
@@ -111,4 +170,6 @@
 	resultOut->setPersistentManifold(m_manifoldPtr);
 
+	//comment-out next line to test multi-contact generation
+	//resultOut->getPersistentManifold()->clearManifold();
 	
 
@@ -147,6 +208,75 @@
 
 	gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
-
 	btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
+
+	//now perturbe directions to get multiple contact points
+	btVector3 v0,v1;
+	btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
+	btPlaneSpace1(sepNormalWorldSpace,v0,v1);
+	//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
+	
+	//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
+	if (resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
+	{
+		
+		int i;
+
+		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
+		{
+			perturbeAngle = gContactBreakingThreshold / radiusB;
+			perturbeA = false;
+		}
+		if ( perturbeAngle > angleLimit ) 
+				perturbeAngle = angleLimit;
+
+		btTransform unPerturbedTransform;
+		if (perturbeA)
+		{
+			unPerturbedTransform = input.m_transformA;
+		} else
+		{
+			unPerturbedTransform = input.m_transformB;
+		}
+		
+		for ( i=0;i<m_numPerturbationIterations;i++)
+		{
+			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)*body0->getWorldTransform().getBasis());
+				input.m_transformB = body1->getWorldTransform();
+#ifdef DEBUG_CONTACTS
+				dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
+#endif //DEBUG_CONTACTS
+			} else
+			{
+				input.m_transformA = body0->getWorldTransform();
+				input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->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
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h	(revision 2882)
@@ -34,5 +34,7 @@
 ///#define USE_SEPDISTANCE_UTIL2 1
 
-///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations.
+///The convexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations between two convex objects.
+///Multiple contact points are calculated by perturbing the orientation of the smallest object orthogonal to the separating normal.
+///This idea was described by Gino van den Bergen in this forum topic http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=4&t=288&p=888#p888
 class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
 {
@@ -48,4 +50,8 @@
 	bool			m_lowLevelOfDetail;
 	
+	int m_numPerturbationIterations;
+	int m_minimumPointsPerturbationThreshold;
+
+
 	///cache separating vector to speedup collision detection
 	
@@ -53,5 +59,6 @@
 public:
 
-	btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+	btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
+
 
 	virtual ~btConvexConvexAlgorithm();
@@ -79,7 +86,10 @@
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
+
 		btConvexPenetrationDepthSolver*		m_pdSolver;
 		btSimplexSolverInterface*			m_simplexSolver;
-		
+		int m_numPerturbationIterations;
+		int m_minimumPointsPerturbationThreshold;
+
 		CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
 		
@@ -89,5 +99,5 @@
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm));
-			return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver);
+			return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
 		}
 	};
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp	(revision 2882)
@@ -5,6 +5,6 @@
 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:
 
@@ -23,13 +23,15 @@
 //#include <stdio.h>
 
-btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
+btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold)
 : btCollisionAlgorithm(ci),
 m_ownManifold(false),
 m_manifoldPtr(mf),
-m_isSwapped(isSwapped)
+m_isSwapped(isSwapped),
+m_numPerturbationIterations(numPerturbationIterations),
+m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
 {
 	btCollisionObject* convexObj = m_isSwapped? col1 : col0;
 	btCollisionObject* planeObj = m_isSwapped? col0 : col1;
-	
+
 	if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj))
 	{
@@ -49,14 +51,7 @@
 }
 
-
-
-void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
-	(void)dispatchInfo;
-	(void)resultOut;
-	if (!m_manifoldPtr)
-		return;
-
-	btCollisionObject* convexObj = m_isSwapped? body1 : body0;
+    btCollisionObject* convexObj = m_isSwapped? body1 : body0;
 	btCollisionObject* planeObj = m_isSwapped? body0: body1;
 
@@ -64,13 +59,18 @@
 	btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();
 
-	bool hasCollision = false;
+    bool hasCollision = false;
 	const btVector3& planeNormal = planeShape->getPlaneNormal();
 	const btScalar& planeConstant = planeShape->getPlaneConstant();
+	
+	btTransform convexWorldTransform = convexObj->getWorldTransform();
+	btTransform convexInPlaneTrans;
+	convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexWorldTransform;
+	//now perturbe the convex-world transform
+	convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot);
 	btTransform planeInConvex;
-	planeInConvex= convexObj->getWorldTransform().inverse() * planeObj->getWorldTransform();
-	btTransform convexInPlaneTrans;
-	convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexObj->getWorldTransform();
+	planeInConvex= convexWorldTransform.inverse() * planeObj->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);
@@ -88,4 +88,51 @@
 		resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
 	}
+}
+
+
+void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)dispatchInfo;
+	if (!m_manifoldPtr)
+		return;
+
+    btCollisionObject* convexObj = m_isSwapped? body1 : body0;
+	btCollisionObject* planeObj = m_isSwapped? body0: body1;
+
+	btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape();
+	btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();
+
+    bool hasCollision = false;
+	const btVector3& planeNormal = planeShape->getPlaneNormal();
+	const btScalar& planeConstant = planeShape->getPlaneConstant();
+
+	//first perform a collision query with the non-perturbated collision objects
+	{
+		btQuaternion rotq(0,0,0,1);
+		collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut);
+	}
+
+	if (resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold)
+	{
+		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;
+
+		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,body0,body1,dispatchInfo,resultOut);
+		}
+	}
+
 	if (m_ownManifold)
 	{
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h	(revision 2882)
@@ -5,6 +5,6 @@
 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:
 
@@ -29,15 +29,19 @@
 class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm
 {
-	bool	m_ownManifold;
+	bool		m_ownManifold;
 	btPersistentManifold*	m_manifoldPtr;
-	bool	m_isSwapped;
-	
+	bool		m_isSwapped;
+	int			m_numPerturbationIterations;
+	int			m_minimumPointsPerturbationThreshold;
+
 public:
 
-	btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
+	btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold);
 
 	virtual ~btConvexPlaneCollisionAlgorithm();
 
 	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	void collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
 	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
@@ -53,4 +57,13 @@
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
+		int	m_numPerturbationIterations;
+		int m_minimumPointsPerturbationThreshold;
+			
+		CreateFunc() 
+			: m_numPerturbationIterations(3),
+			m_minimumPointsPerturbationThreshold(3)
+		{
+		}
+		
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
@@ -58,8 +71,8 @@
 			if (!m_swapped)
 			{
-				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false);
+				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
 			} else
 			{
-				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true);
+				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
 			}
 		}
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp	(revision 2882)
@@ -289,2 +289,9 @@
 	return m_emptyCreateFunc;
 }
+
+void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+{
+	btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*) m_convexConvexCreateFunc;
+	convexConvex->m_numPerturbationIterations = numPerturbationIterations;
+	convexConvex->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
+}
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h	(revision 2882)
@@ -112,4 +112,12 @@
 	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(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.
+	///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature.
+	///@param minimumPointsPerturbationThreshold is the minimum number of points in the contact cache, above which the feature is disabled
+	///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);
 
 };
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp	(revision 2882)
@@ -64,4 +64,5 @@
 btPairCachingGhostObject::~btPairCachingGhostObject()
 {
+	m_hashPairCache->~btHashedOverlappingPairCache();
 	btAlignedFree( m_hashPairCache );
 }
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp	(revision 2882)
@@ -56,5 +56,5 @@
 void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
 {
-	assert(m_manifoldPtr);
+	btAssert(m_manifoldPtr);
 	//order in manifold needs to match
 	
@@ -93,5 +93,5 @@
    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)
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h	(revision 2882)
@@ -46,4 +46,6 @@
 	int m_index0;
 	int m_index1;
+	
+
 public:
 
@@ -78,4 +80,5 @@
 	}
 
+
 	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth);
 
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp	(revision 2882)
@@ -25,5 +25,6 @@
 #include "LinearMath/btQuickprof.h"
 
-btSimulationIslandManager::btSimulationIslandManager()
+btSimulationIslandManager::btSimulationIslandManager():
+m_splitIslands(true)
 {
 }
@@ -44,8 +45,8 @@
 	
 	{
-		btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr();
-
+		
 		for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++)
 		{
+			btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr();
 			const btBroadphasePair& collisionPair = pairPtr[i];
 			btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
@@ -186,5 +187,5 @@
 			}
 
-			assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+			btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 			if (colObj0->getIslandTag() == islandId)
 			{
@@ -213,5 +214,5 @@
 				}
 
-				assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 
 				if (colObj0->getIslandTag() == islandId)
@@ -234,5 +235,5 @@
 				}
 
-				assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 
 				if (colObj0->getIslandTag() == islandId)
@@ -252,9 +253,9 @@
 	int maxNumManifolds = dispatcher->getNumManifolds();
 
-#define SPLIT_ISLANDS 1
-#ifdef SPLIT_ISLANDS
-
-	
-#endif //SPLIT_ISLANDS
+//#define SPLIT_ISLANDS 1
+//#ifdef SPLIT_ISLANDS
+
+	
+//#endif //SPLIT_ISLANDS
 
 	
@@ -280,9 +281,10 @@
 				colObj0->activate();
 			}
-#ifdef SPLIT_ISLANDS
-	//		//filtering for response
-			if (dispatcher->needsResponse(colObj0,colObj1))
-				m_islandmanifold.push_back(manifold);
-#endif //SPLIT_ISLANDS
+			if(m_splitIslands)
+			{ 
+				//filtering for response
+				if (dispatcher->needsResponse(colObj0,colObj1))
+					m_islandmanifold.push_back(manifold);
+			}
 		}
 	}
@@ -304,83 +306,85 @@
 	BT_PROFILE("processIslands");
 
-#ifndef SPLIT_ISLANDS
-	btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
-	
-	callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
-#else
-	// Sort manifolds, based on islands
-	// Sort the vector using predicate and std::sort
-	//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
-
-	int numManifolds = int (m_islandmanifold.size());
-
-	//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
-	m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
-
-	//now process all active islands (sets of manifolds for now)
-
-	int startManifoldIndex = 0;
-	int endManifoldIndex = 1;
-
-	//int islandId;
-
-	
-
-//	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)
-	{
-		int islandId = getUnionFind().getElement(startIslandIndex).m_id;
-
-
-	       bool 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 = true;
-                }
-                
-
-		//find the accompanying contact manifold for this islandId
-		int numIslandManifolds = 0;
-		btPersistentManifold** startManifold = 0;
-
-		if (startManifoldIndex<numManifolds)
-		{
-			int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
-			if (curIslandId == islandId)
-			{
-				startManifold = &m_islandmanifold[startManifoldIndex];
+	if(!m_splitIslands)
+	{
+		btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
+		int maxNumManifolds = dispatcher->getNumManifolds();
+		callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
+	}
+	else
+	{
+		// Sort manifolds, based on islands
+		// Sort the vector using predicate and std::sort
+		//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
+
+		int numManifolds = int (m_islandmanifold.size());
+
+		//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
+		m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
+
+		//now process all active islands (sets of manifolds for now)
+
+		int startManifoldIndex = 0;
+		int endManifoldIndex = 1;
+
+		//int islandId;
+
+		
+
+	//	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)
+		{
+			int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+
+
+			   bool 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 = true;
+					}
+	                
+
+			//find the accompanying contact manifold for this islandId
+			int numIslandManifolds = 0;
+			btPersistentManifold** startManifold = 0;
+
+			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++)
+					{
+
+					}
+					/// Process the actual simulation, only if not sleeping/deactivated
+					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);
+			}
 			
-				for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
-				{
-
-				}
-				/// Process the actual simulation, only if not sleeping/deactivated
-				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);
-		}
-		
-		if (numIslandManifolds)
-		{
-			startManifoldIndex = endManifoldIndex;
-		}
-
-		m_islandBodies.resize(0);
-	}
-#endif //SPLIT_ISLANDS
-
-
-}
+			if (numIslandManifolds)
+			{
+				startManifoldIndex = endManifoldIndex;
+			}
+
+			m_islandBodies.resize(0);
+		}
+	} // else if(!splitIslands) 
+
+}
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h	(revision 2882)
@@ -36,4 +36,5 @@
 	btAlignedObjectArray<btCollisionObject* >  m_islandBodies;
 	
+	bool m_splitIslands;
 	
 public:
@@ -66,4 +67,13 @@
 	void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
 
+	bool getSplitIslands()
+	{
+		return m_splitIslands;
+	}
+	void setSplitIslands(bool doSplitIslands)
+	{
+		m_splitIslands = doSplitIslands;
+	}
+
 };
 
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp	(revision 2882)
@@ -79,5 +79,5 @@
 
 	///point on A (worldspace)
-	btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
+	///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
 	///point on B (worldspace)
 	btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB;
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp	(revision 2882)
@@ -15,6 +15,4 @@
 
 #include "btUnionFind.h"
-#include <assert.h>
-
 
 
Index: code/trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.h	(revision 2882)
@@ -99,6 +99,6 @@
 		int find(int x)
 		{ 
-			//assert(x < m_N);
-			//assert(x >= 0);
+			//btAssert(x < m_N);
+			//btAssert(x >= 0);
 
 			while (x != m_elements[x].m_id) 
@@ -111,6 +111,6 @@
 		#endif //
 				x = m_elements[x].m_id;
-				//assert(x < m_N);
-				//assert(x >= 0);
+				//btAssert(x < m_N);
+				//btAssert(x >= 0);
 
 			}
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btBoxShape.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btBoxShape.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btBoxShape.h	(revision 2882)
@@ -162,29 +162,23 @@
 		{
 		case 0:
-			plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.));
-			plane[3] = -halfExtents.x();
+			plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x());
 			break;
 		case 1:
-			plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.));
-			plane[3] = -halfExtents.x();
+			plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x());
 			break;
 		case 2:
-			plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.));
-			plane[3] = -halfExtents.y();
+			plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y());
 			break;
 		case 3:
-			plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.));
-			plane[3] = -halfExtents.y();
+			plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y());
 			break;
 		case 4:
-			plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.));
-			plane[3] = -halfExtents.z();
+			plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z());
 			break;
 		case 5:
-			plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.));
-			plane[3] = -halfExtents.z();
+			plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z());
 			break;
 		default:
-			assert(0);
+			btAssert(0);
 		}
 	}
@@ -313,5 +307,5 @@
 			break;
 		default:
-			assert(0);
+			btAssert(0);
 		}
 	}
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btCollisionShape.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btCollisionShape.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btCollisionShape.cpp	(revision 2882)
@@ -15,4 +15,7 @@
 
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
+
+
+btScalar gContactThresholdFactor=btScalar(0.02);
 
 
@@ -45,6 +48,5 @@
 btScalar	btCollisionShape::getContactBreakingThreshold() const
 {
-	///@todo make this 0.1 configurable
-	return getAngularMotionDisc() * btScalar(0.1);
+	return getAngularMotionDisc() * gContactThresholdFactor;
 }
 btScalar	btCollisionShape::getAngularMotionDisc() const
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp	(revision 2882)
@@ -23,5 +23,6 @@
 m_collisionMargin(btScalar(0.)),
 m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
-m_dynamicAabbTree(0)
+m_dynamicAabbTree(0),
+m_updateRevision(1)
 {
 	m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
@@ -47,4 +48,5 @@
 void	btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape)
 {
+	m_updateRevision++;
 	//m_childTransforms.push_back(localTransform);
 	//m_childShapes.push_back(shape);
@@ -55,6 +57,5 @@
 	child.m_childMargin = shape->getMargin();
 
-	m_children.push_back(child);
-
+	
 	//extend the local aabbMin/aabbMax
 	btVector3 localAabbMin,localAabbMax;
@@ -75,7 +76,9 @@
 	{
 		const btDbvtVolume	bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
-		int index = m_children.size()-1;
+		int index = m_children.size();
 		child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
 	}
+
+	m_children.push_back(child);
 
 }
@@ -100,4 +103,5 @@
 void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
 {
+	m_updateRevision++;
 	btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size());
 	if (m_dynamicAabbTree)
@@ -114,4 +118,5 @@
 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!
@@ -140,4 +145,5 @@
 	// Recalculate the local aabb
 	// Brute force, it iterates over all the shapes left.
+
 	m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
 	m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
@@ -162,6 +168,14 @@
 {
 	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 += btVector3(getMargin(),getMargin(),getMargin());
-	btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
+		
 
 	btMatrix3x3 abs_b = trans.getBasis().absolute();  
@@ -174,5 +188,5 @@
 	aabbMin = center-extent;
 	aabbMax = center+extent;
-
+	
 }
 
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.h	(revision 2882)
@@ -59,4 +59,7 @@
 
 	btDbvt*							m_dynamicAabbTree;
+
+	///increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be updated
+	int								m_updateRevision;
 
 public:
@@ -153,4 +156,8 @@
 	void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const;
 
+	int	getUpdateRevision() const
+	{
+		return m_updateRevision;
+	}
 
 private:
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btConeShape.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btConeShape.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btConeShape.cpp	(revision 2882)
@@ -61,5 +61,5 @@
 		break;
 	default:
-		assert(0);
+		btAssert(0);
 	};
 }
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btConvexHullShape.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btConvexHullShape.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btConvexHullShape.cpp	(revision 2882)
@@ -182,5 +182,5 @@
 bool btConvexHullShape::isInside(const btVector3& ,btScalar ) const
 {
-	assert(0);
+	btAssert(0);
 	return false;
 }
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp	(revision 2882)
@@ -151,5 +151,5 @@
 bool btConvexPointCloudShape::isInside(const btVector3& ,btScalar ) const
 {
-	assert(0);
+	btAssert(0);
 	return false;
 }
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp	(revision 2882)
@@ -155,5 +155,4 @@
 
 		btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
-		btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions();
 		btScalar halfHeight = capsuleShape->getHalfHeight();
 		int capsuleUpAxis = capsuleShape->getUpAxis();
@@ -302,6 +301,6 @@
 	{
 		btSphereShape* sphereShape = (btSphereShape*)this;
-		float radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
-		float margin = radius + sphereShape->getMarginNonVirtual();
+		btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
+		btScalar margin = radius + sphereShape->getMarginNonVirtual();
 		const btVector3& center = t.getOrigin();
 		btVector3 extent(margin,margin,margin);
@@ -315,5 +314,5 @@
 	{
 		btBoxShape* convexShape = (btBoxShape*)this;
-		float margin=convexShape->getMarginNonVirtual();
+		btScalar margin=convexShape->getMarginNonVirtual();
 		btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
 		halfExtents += btVector3(margin,margin,margin);
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp	(revision 2882)
@@ -145,5 +145,10 @@
 }
 
-btScalar	btHeightfieldTerrainShape::getHeightFieldValue(int x,int y) const
+
+/// 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
 {
 	btScalar val = 0.f;
@@ -182,8 +187,7 @@
 
 
-
+/// this returns the vertex in bullet-local coordinates
 void	btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const
 {
-
 	btAssert(x>=0);
 	btAssert(y>=0);
@@ -191,6 +195,5 @@
 	btAssert(y<m_heightStickLength);
 
-
-	btScalar	height = getHeightFieldValue(x,y);
+	btScalar	height = getRawHeightFieldValue(x,y);
 
 	switch (m_upAxis)
@@ -199,5 +202,5 @@
 		{
 		vertex.setValue(
-			height,
+			height - m_localOrigin.getX(),
 			(-m_width/btScalar(2.0)) + x,
 			(-m_length/btScalar(2.0) ) + y
@@ -209,5 +212,5 @@
 			vertex.setValue(
 			(-m_width/btScalar(2.0)) + x,
-			height,
+			height - m_localOrigin.getY(),
 			(-m_length/btScalar(2.0)) + y
 			);
@@ -219,5 +222,5 @@
 			(-m_width/btScalar(2.0)) + x,
 			(-m_length/btScalar(2.0)) + y,
-			height
+			height - m_localOrigin.getZ()
 			);
 			break;
@@ -231,5 +234,4 @@
 
 	vertex*=m_localScaling;
-	
 }
 
@@ -239,5 +241,5 @@
 getQuantized
 (
-float x
+btScalar x
 )
 {
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h	(revision 2882)
@@ -30,4 +30,15 @@
   center (as determined by width and length and height, with each
   axis multiplied by the localScaling).
+
+  \b NOTE: be careful with coordinates.  If you have a heightfield with a local
+  min height of -100m, and a max height of +500m, you may be tempted to place it
+  at the origin (0,0) and expect the heights in world coordinates to be
+  -100 to +500 meters.
+  Actually, the heights will be -300 to +300m, because bullet will re-center
+  the heightfield based on its AABB (which is determined by the min/max
+  heights).  So keep in mind that once you create a btHeightfieldTerrainShape
+  object, the heights will be adjusted relative to the center of the AABB.  This
+  is different to the behavior of many rendering engines, but is useful for
+  physics engines.
 
   Most (but not all) rendering and heightfield libraries assume upAxis = 1
@@ -89,5 +100,5 @@
 	btVector3	m_localScaling;
 
-	virtual btScalar	getHeightFieldValue(int x,int y) const;
+	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;
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp	(revision 2882)
@@ -29,6 +29,6 @@
 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()));
+	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;
 }
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h	(revision 2882)
@@ -32,8 +32,10 @@
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-    btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {}
+    btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;}
     btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true):
         btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh)
         {
+            m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
+
             btVector3 m_triangle[3];
             const unsigned char *vertexbase;
@@ -68,4 +70,6 @@
         btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh)
         {
+            m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
+
             btVector3 m_triangle[3];
             const unsigned char *vertexbase;
@@ -108,9 +112,4 @@
 */
     }
-	virtual int	getShapeType() const
-	{
-		return MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
-	}
-	
 	//debugging
 	virtual const char*	getName()const {return "MULTIMATERIALTRIANGLEMESH";}
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btSphereShape.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btSphereShape.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btSphereShape.h	(revision 2882)
@@ -48,4 +48,10 @@
 	btScalar	getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX();}
 
+	void	setUnscaledRadius(btScalar	radius)
+	{
+		m_implicitShapeDimensions.setX(radius);
+		btConvexInternalShape::setMargin(radius);
+	}
+
 	//debugging
 	virtual const char*	getName()const {return "SPHERE";}
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp	(revision 2882)
@@ -83,5 +83,6 @@
 }
 
-void	btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax )
+
+void	btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const
 {
 	m_aabbMin = aabbMin;
@@ -96,2 +97,3 @@
 }
 
+
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h	(revision 2882)
@@ -53,7 +53,7 @@
 	IndexedMeshArray	m_indexedMeshes;
 	int m_pad[2];
-	int m_hasAabb; // using int instead of bool to maintain alignment
-	btVector3 m_aabbMin;
-	btVector3 m_aabbMax;
+	mutable int m_hasAabb; // using int instead of bool to maintain alignment
+	mutable btVector3 m_aabbMin;
+	mutable btVector3 m_aabbMax;
 
 public:
@@ -107,5 +107,5 @@
 
 	virtual bool	hasPremadeAabb() const;
-	virtual void	setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax );
+	virtual void	setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const;
 	virtual void	getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const;
 
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp	(revision 2882)
@@ -1,2 +1,3 @@
+
 /*
 Bullet Continuous Collision Detection and Physics Library
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMesh.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMesh.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMesh.cpp	(revision 2882)
@@ -36,5 +36,5 @@
 	{
 		m_indexedMeshes[0].m_numTriangles = m_32bitIndices.size()/3;
-		m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_32bitIndices[0];
+		m_indexedMeshes[0].m_triangleIndexBase = 0;
 		m_indexedMeshes[0].m_indexType = PHY_INTEGER;
 		m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(int);
@@ -42,5 +42,5 @@
 	{
 		m_indexedMeshes[0].m_numTriangles = m_16bitIndices.size()/3;
-		m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_16bitIndices[0];
+		m_indexedMeshes[0].m_triangleIndexBase = 0;
 		m_indexedMeshes[0].m_indexType = PHY_SHORT;
 		m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(short int);
@@ -50,10 +50,10 @@
 	{
 		m_indexedMeshes[0].m_numVertices = m_4componentVertices.size();
-		m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_4componentVertices[0];
+		m_indexedMeshes[0].m_vertexBase = 0;
 		m_indexedMeshes[0].m_vertexStride = sizeof(btVector3);
 	} else
 	{
 		m_indexedMeshes[0].m_numVertices = m_3componentVertices.size()/3;
-		m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0];
+		m_indexedMeshes[0].m_vertexBase = 0;
 		m_indexedMeshes[0].m_vertexStride = 3*sizeof(btScalar);
 	}
@@ -112,7 +112,7 @@
 			}
 	}
-		m_3componentVertices.push_back(vertex.getX());
-		m_3componentVertices.push_back(vertex.getY());
-		m_3componentVertices.push_back(vertex.getZ());
+		m_3componentVertices.push_back((float)vertex.getX());
+		m_3componentVertices.push_back((float)vertex.getY());
+		m_3componentVertices.push_back((float)vertex.getZ());
 		m_indexedMeshes[0].m_numVertices++;
 		m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0];
Index: code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.h	(revision 2882)
@@ -41,5 +41,5 @@
 	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const
 	{
-		assert(0);
+		btAssert(0);
 		return localGetSupportingVertex(vec);
 	}
Index: code/trunk/src/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp	(revision 2882)
@@ -151,6 +151,5 @@
 	}
 
-	bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
-	btAssert(!unbal);
+	btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
 
 	return splitIndex;
Index: code/trunk/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp	(revision 2882)
@@ -172,6 +172,5 @@
 	}
 
-	bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
-	btAssert(!unbal);
+	btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
 
 	return splitIndex;
Index: code/trunk/src/bullet/BulletCollision/Gimpact/btGImpactShape.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/Gimpact/btGImpactShape.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/Gimpact/btGImpactShape.h	(revision 2882)
@@ -882,4 +882,6 @@
 class btGImpactMeshShape : public btGImpactShapeInterface
 {
+	btStridingMeshInterface* m_meshInterface;
+
 protected:
 	btAlignedObjectArray<btGImpactMeshShapePart*> m_mesh_parts;
@@ -908,4 +910,5 @@
 	btGImpactMeshShape(btStridingMeshInterface * meshInterface)
 	{
+		m_meshInterface = meshInterface;
 		buildMeshParts(meshInterface);
 	}
@@ -923,4 +926,13 @@
 
 
+	btStridingMeshInterface* getMeshInterface()
+	{
+		return m_meshInterface;
+	}
+
+	const btStridingMeshInterface* getMeshInterface() const
+	{
+		return m_meshInterface;
+	}
 
 	int getMeshPartCount() const
@@ -1034,10 +1046,10 @@
 
 	//! call when reading child shapes
-	virtual void lockChildShapes()
-	{
-		btAssert(0);
-	}
-
-	virtual void unlockChildShapes()
+	virtual void lockChildShapes() const
+	{
+		btAssert(0);
+	}
+
+	virtual void unlockChildShapes() const
 	{
 		btAssert(0);
Index: code/trunk/src/bullet/BulletCollision/Gimpact/gim_box_set.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/Gimpact/gim_box_set.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/Gimpact/gim_box_set.cpp	(revision 2882)
@@ -111,6 +111,5 @@
 	}
 
-	bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
-	btAssert(!unbal);
+	btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
 
 	return splitIndex;
@@ -181,2 +180,3 @@
 }
 
+
Index: code/trunk/src/bullet/BulletCollision/Gimpact/gim_math.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/Gimpact/gim_math.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/Gimpact/gim_math.h	(revision 2882)
@@ -108,5 +108,5 @@
 #define GIM_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
 
-#define GIM_GREATER(x, y)	fabsf(x) > (y)
+#define GIM_GREATER(x, y)	btFabs(x) > (y)
 
 ///Swap numbers
Index: code/trunk/src/bullet/BulletCollision/Gimpact/gim_tri_collision.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/Gimpact/gim_tri_collision.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/Gimpact/gim_tri_collision.h	(revision 2882)
@@ -276,5 +276,5 @@
 		else
 		{
-			float sumuv;
+			btScalar sumuv;
 			sumuv = u+v;
 			if(sumuv<-G_EPSILON)
Index: code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp	(revision 2882)
@@ -136,4 +136,7 @@
 			//btScalar clippedDist  = dist;
 			
+			//don't report time of impact for motion away from the contact normal (or causes minor penetration)
+			if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
+				return false;
 			
 			dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
@@ -197,9 +200,8 @@
 
 		}
-
-		//don't report time of impact for motion away from the contact normal (or causes minor penetration)
+	
 		if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=result.m_allowedPenetration)//SIMD_EPSILON)
 			return false;
-
+			
 		result.m_fraction = lambda;
 		result.m_normal = n;
Index: code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp	(revision 2882)
@@ -151,5 +151,6 @@
 			if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) 
 			{
-				checkPenetration = false;
+				checkSimplex=true;
+				//checkPenetration = false;
 				break;
 			}
Index: code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h	(revision 2882)
@@ -113,4 +113,10 @@
 			}
 			
+			///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver
+			btScalar	getAppliedImpulse() const
+			{
+				return m_appliedImpulse;
+			}
+
 			
 
Index: code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp	(revision 2882)
@@ -184,4 +184,7 @@
 		
 	}
+	if (insertIndex<0)
+		insertIndex=0;
+
 	btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
 	m_pointCache[insertIndex] = newPoint;
Index: code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h	(revision 2882)
@@ -56,4 +56,5 @@
 
 	btScalar	m_contactBreakingThreshold;
+	btScalar	m_contactProcessingThreshold;
 
 	
@@ -71,7 +72,8 @@
 	btPersistentManifold();
 
-	btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold)
+	btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold)
 		: m_body0(body0),m_body1(body1),m_cachedPoints(0),
-		m_contactBreakingThreshold(contactBreakingThreshold)
+		m_contactBreakingThreshold(contactBreakingThreshold),
+		m_contactProcessingThreshold(contactProcessingThreshold)
 	{
 		
@@ -112,4 +114,9 @@
 	///@todo: get this margin from the current physics / collision environment
 	btScalar	getContactBreakingThreshold() const;
+
+	btScalar	getContactProcessingThreshold() const
+	{
+		return m_contactProcessingThreshold;
+	}
 	
 	int getCacheEntry(const btManifoldPoint& newPoint) const;
Index: code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp	(revision 2882)
@@ -24,8 +24,10 @@
 #include "btRaycastCallback.h"
 
-btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to)
+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.))
 {
@@ -56,4 +58,10 @@
 		return ; // same sign
 	}
+   //@BP Mod - Backface filtering
+   if (((m_flags & kF_FilterBackfaces) != 0) && (dist_a > btScalar(0.0)))
+   {
+      // Backface, skip check
+      return;
+   }
 	
 	const btScalar proj_length=dist_a-dist_b;
@@ -90,12 +98,16 @@
 					if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance) 
 					{
+                  //@BP Mod
+                  // Triangle normal isn't normalized
+				      triangleNormal.normalize();
 
-						if ( dist_a > 0 )
+                  //@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);
 						}
 					}
Index: code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
===================================================================
--- code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h	(revision 2882)
@@ -30,7 +30,18 @@
 	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
+
+      kF_Terminator        = 0xFFFFFFFF
+   };
+   unsigned int m_flags;
+
 	btScalar	m_hitFraction;
 
-	btTriangleRaycastCallback(const btVector3& from,const btVector3& to);
+	btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags=0);
 	
 	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
Index: code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
===================================================================
--- code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp	(revision 2882)
@@ -26,6 +26,4 @@
 
 #include "btVoronoiSimplexSolver.h"
-#include <assert.h>
-//#include <stdio.h>
 
 #define VERTA  0
@@ -38,5 +36,5 @@
 {
 	
-	assert(m_numVertices>0);
+	btAssert(m_numVertices>0);
 	m_numVertices--;
 	m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
Index: code/trunk/src/bullet/BulletDynamics/CMakeLists.txt
===================================================================
--- code/trunk/src/bullet/BulletDynamics/CMakeLists.txt	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/CMakeLists.txt	(revision 2882)
@@ -36,4 +36,5 @@
 	ConstraintSolver/btTypedConstraint.h
 
+	Dynamics/btActionInterface.h
 	Dynamics/btContinuousDynamicsWorld.h
 	Dynamics/btDiscreteDynamicsWorld.h
Index: code/trunk/src/bullet/BulletDynamics/Character/btCharacterControllerInterface.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/Character/btCharacterControllerInterface.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/Character/btCharacterControllerInterface.h	(revision 2882)
@@ -18,4 +18,5 @@
 
 #include "LinearMath/btVector3.h"
+#include "BulletDynamics/Dynamics/btActionInterface.h"
 
 class btCollisionShape;
@@ -23,5 +24,5 @@
 class btCollisionWorld;
 
-class btCharacterControllerInterface
+class btCharacterControllerInterface : public btActionInterface
 {
 public:
Index: code/trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp
===================================================================
--- code/trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp	(revision 2882)
@@ -23,4 +23,6 @@
 #include "btKinematicCharacterController.h"
 
+static btVector3 upAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
+
 ///@todo Interact with dynamic objects,
 ///Ride kinematicly animated platforms properly
@@ -94,6 +96,7 @@
 }
 
-btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight)
-{
+btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
+{
+	m_upAxis = upAxis;
 	m_addedMargin = 0.02f;
 	m_walkDirection.setValue(0,0,0);
@@ -102,6 +105,5 @@
 	m_stepHeight = stepHeight;
 	m_turnAngle = btScalar(0.0);
-	m_convexShape=convexShape;
-	
+	m_convexShape=convexShape;	
 }
 
@@ -110,5 +112,4 @@
 }
 
-
 btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
 {
@@ -116,5 +117,5 @@
 }
 
-bool btKinematicCharacterController::recoverFromPenetration (btCollisionWorld* collisionWorld)
+bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
 {
 
@@ -173,5 +174,5 @@
 	// phase 1: up
 	btTransform start, end;
-	m_targetPosition = m_currentPosition + btVector3 (btScalar(0.0), m_stepHeight, btScalar(0.0));
+	m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight;
 
 	start.setIdentity ();
@@ -179,5 +180,5 @@
 
 	/* FIXME: Handle penetration properly */
-	start.setOrigin (m_currentPosition + btVector3(btScalar(0.0), btScalar(0.1), btScalar(0.0)));
+	start.setOrigin (m_currentPosition + upAxisDirection[m_upAxis] * btScalar(0.1f));
 	end.setOrigin (m_targetPosition);
 
@@ -344,6 +345,6 @@
 
 	// phase 3: down
-	btVector3 step_drop = btVector3(btScalar(0.0), m_currentStepOffset, btScalar(0.0));
-	btVector3 gravity_drop = btVector3(btScalar(0.0), m_stepHeight, btScalar(0.0));
+	btVector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset;
+	btVector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight; 
 	m_targetPosition -= (step_drop + gravity_drop);
 
@@ -390,5 +391,5 @@
 
 
-void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
+void btKinematicCharacterController::preStep (  btCollisionWorld* collisionWorld)
 {
 	
@@ -413,5 +414,5 @@
 }
 
-void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
+void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
 {
 	btTransform xform;
@@ -469,2 +470,7 @@
 	return true;
 }
+
+
+void	btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
+{
+}
Index: code/trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.h	(revision 2882)
@@ -63,4 +63,6 @@
 
 	bool	m_useGhostObjectSweepTest;
+
+	int m_upAxis;
 	
 	btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
@@ -68,5 +70,5 @@
 	btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal);
 
-	bool recoverFromPenetration (btCollisionWorld* collisionWorld);
+	bool recoverFromPenetration ( btCollisionWorld* collisionWorld);
 	void stepUp (btCollisionWorld* collisionWorld);
 	void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
@@ -74,7 +76,27 @@
 	void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
 public:
-	btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight);
+	btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
 	~btKinematicCharacterController ();
 	
+
+	///btActionInterface interface
+	virtual void updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime)
+	{
+		preStep ( collisionWorld);
+		playerStep (collisionWorld, deltaTime);
+	}
+	
+	///btActionInterface interface
+	void	debugDraw(btIDebugDraw* debugDrawer);
+
+	void setUpAxis (int axis)
+	{
+		if (axis < 0)
+			axis = 0;
+		if (axis > 2)
+			axis = 2;
+		m_upAxis = axis;
+	}
+
 	virtual void	setWalkDirection(const btVector3& walkDirection)
 	{
@@ -85,6 +107,6 @@
 	void warp (const btVector3& origin);
 
-	void preStep ( btCollisionWorld* collisionWorld);
-	void playerStep (btCollisionWorld* collisionWorld, btScalar dt);
+	void preStep (  btCollisionWorld* collisionWorld);
+	void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
 
 	void setFallSpeed (btScalar fallSpeed);
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp	(revision 2882)
@@ -23,6 +23,14 @@
 #include <new>
 
+//-----------------------------------------------------------------------------
+
+#define CONETWIST_USE_OBSOLETE_SOLVER false
+#define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
+
+//-----------------------------------------------------------------------------
+
 btConeTwistConstraint::btConeTwistConstraint()
-:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE)
+:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
+m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
 {
 }
@@ -32,67 +40,226 @@
 											 const btTransform& rbAFrame,const btTransform& rbBFrame)
 											 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
-											 m_angularOnly(false)
-{
-	m_swingSpan1 = btScalar(1e30);
-	m_swingSpan2 = btScalar(1e30);
-	m_twistSpan  = btScalar(1e30);
-	m_biasFactor = 0.3f;
-	m_relaxationFactor = 1.0f;
-
+											 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)
+{
+	m_rbBFrame = m_rbAFrame;
+	init();	
+}
+
+
+void btConeTwistConstraint::init()
+{
+	m_angularOnly = false;
 	m_solveTwistLimit = false;
 	m_solveSwingLimit = false;
-
-}
-
-btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
-											:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
-											 m_angularOnly(false)
-{
-	m_rbBFrame = m_rbAFrame;
+	m_bMotorEnabled = false;
+	m_maxMotorImpulse = btScalar(-1);
+
+	setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30));
+	m_damping = btScalar(0.01);
+	m_fixThresh = CONETWIST_DEF_FIX_THRESH;
+}
+
+
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
+	} 
+	else
+	{
+		info->m_numConstraintRows = 3;
+		info->nub = 3;
+		calcAngleInfo2();
+		if(m_solveSwingLimit)
+		{
+			info->m_numConstraintRows++;
+			info->nub--;
+			if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+			{
+				info->m_numConstraintRows++;
+				info->nub--;
+			}
+		}
+		if(m_solveTwistLimit)
+		{
+			info->m_numConstraintRows++;
+			info->nub--;
+		}
+	}
+} // btConeTwistConstraint::getInfo1()
 	
-	m_swingSpan1 = btScalar(1e30);
-	m_swingSpan2 = btScalar(1e30);
-	m_twistSpan  = btScalar(1e30);
-	m_biasFactor = 0.3f;
-	m_relaxationFactor = 1.0f;
-
-	m_solveTwistLimit = false;
-	m_solveSwingLimit = false;
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	btAssert(!m_useSolveConstraintObsolete);
+	//retrieve matrices
+	btTransform body0_trans;
+	body0_trans = m_rbA.getCenterOfMassTransform();
+    btTransform body1_trans;
+	body1_trans = m_rbB.getCenterOfMassTransform();
+    // 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() * 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 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+	btVector3 a2 = body1_trans.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);
+	}
+    // set right hand side
+    btScalar k = info->fps * info->erp;
+    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]);
+		info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
+		info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
+    }
+	int row = 3;
+    int srow = row * info->rowskip;
+	btVector3 ax1;
+	// angular limits
+	if(m_solveSwingLimit)
+	{
+		btScalar *J1 = info->m_J1angularAxis;
+		btScalar *J2 = info->m_J2angularAxis;
+		if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+		{
+			btTransform trA = m_rbA.getCenterOfMassTransform()*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];
+			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_lowerLimit[srow] = -SIMD_INFINITY;
+			info->m_upperLimit[srow] = SIMD_INFINITY;
+			info->m_lowerLimit[srow1] = -SIMD_INFINITY;
+			info->m_upperLimit[srow1] = SIMD_INFINITY;
+			srow = srow1 + info->rowskip;
+		}
+		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];
+			btScalar k = info->fps * m_biasFactor;
+
+			info->m_constraintError[srow] = k * m_swingCorrection;
+			info->cfm[srow] = 0.0f;
+			// m_swingCorrection is always positive or 0
+			info->m_lowerLimit[srow] = 0;
+			info->m_upperLimit[srow] = SIMD_INFINITY;
+			srow += info->rowskip;
+		}
+	}
+	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 k = info->fps * m_biasFactor;
+		info->m_constraintError[srow] = k * m_twistCorrection;
+		info->cfm[srow] = 0.0f;
+		if(m_twistSpan > 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
+		{
+			info->m_lowerLimit[srow] = -SIMD_INFINITY;
+			info->m_upperLimit[srow] = SIMD_INFINITY;
+		}
+		srow += info->rowskip;
+	}
+}
 	
-}			
+//-----------------------------------------------------------------------------
 
 void	btConeTwistConstraint::buildJacobian()
 {
-	m_appliedImpulse = btScalar(0.);
-
-	//set bias, sign, clear accumulator
-	m_swingCorrection = btScalar(0.);
-	m_twistLimitSign = btScalar(0.);
-	m_solveTwistLimit = false;
-	m_solveSwingLimit = false;
-	m_accTwistLimitImpulse = btScalar(0.);
-	m_accSwingLimitImpulse = btScalar(0.);
-
-	if (!m_angularOnly)
-	{
-		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
-		btVector3 relPos = pivotBInW - pivotAInW;
-
-		btVector3 normal[3];
-		if (relPos.length2() > SIMD_EPSILON)
-		{
-			normal[0] = relPos.normalized();
-		}
-		else
-		{
-			normal[0].setValue(btScalar(1.0),0,0);
-		}
-
-		btPlaneSpace1(normal[0], normal[1], normal[2]);
-
-		for (int i=0;i<3;i++)
-		{
-			new (&m_jac[i]) btJacobianEntry(
+	if (m_useSolveConstraintObsolete)
+	{
+		m_appliedImpulse = btScalar(0.);
+		m_accTwistLimitImpulse = btScalar(0.);
+		m_accSwingLimitImpulse = btScalar(0.);
+
+		if (!m_angularOnly)
+		{
+			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+			btVector3 relPos = pivotBInW - pivotAInW;
+
+			btVector3 normal[3];
+			if (relPos.length2() > SIMD_EPSILON)
+			{
+				normal[0] = relPos.normalized();
+			}
+			else
+			{
+				normal[0].setValue(btScalar(1.0),0,0);
+			}
+
+			btPlaneSpace1(normal[0], normal[1], normal[2]);
+
+			for (int i=0;i<3;i++)
+			{
+				new (&m_jac[i]) btJacobianEntry(
 				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -104,6 +271,240 @@
 				m_rbB.getInvInertiaDiagLocal(),
 				m_rbB.getInvMass());
-		}
-	}
+			}
+		}
+
+		calcAngleInfo2();
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		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_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+			btVector3 vel1;
+			bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+			btVector3 vel2;
+			bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+			btVector3 vel = vel1 - vel2;
+
+			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;
+				m_appliedImpulse += impulse;
+				
+				btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
+				btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
+				bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
+				bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
+		
+			}
+		}
+
+		// apply motor
+		if (m_bMotorEnabled)
+		{
+			// compute current and predicted transforms
+			btTransform trACur = m_rbA.getCenterOfMassTransform();
+			btTransform trBCur = m_rbB.getCenterOfMassTransform();
+			btVector3 omegaA; bodyA.getAngularVelocity(omegaA);
+			btVector3 omegaB; bodyB.getAngularVelocity(omegaB);
+			btTransform trAPred; trAPred.setIdentity(); 
+			btVector3 zerovec(0,0,0);
+			btTransformUtil::integrateTransform(
+				trACur, zerovec, omegaA, timeStep, trAPred);
+			btTransform trBPred; trBPred.setIdentity(); 
+			btTransformUtil::integrateTransform(
+				trBCur, zerovec, omegaB, timeStep, trBPred);
+
+			// compute desired transforms in world
+			btTransform trPose(m_qTarget);
+			btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
+			btTransform trADes = trBPred * trABDes;
+			btTransform trBDes = trAPred * trABDes.inverse();
+
+			// compute desired omegas in world
+			btVector3 omegaADes, omegaBDes;
+			
+			btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
+			btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
+
+			// compute delta omegas
+			btVector3 dOmegaA = omegaADes - omegaA;
+			btVector3 dOmegaB = omegaBDes - omegaB;
+
+			// compute weighted avg axis of dOmega (weighting based on inertias)
+			btVector3 axisA, axisB;
+			btScalar kAxisAInv = 0, kAxisBInv = 0;
+
+			if (dOmegaA.length2() > SIMD_EPSILON)
+			{
+				axisA = dOmegaA.normalized();
+				kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
+			}
+
+			if (dOmegaB.length2() > SIMD_EPSILON)
+			{
+				axisB = dOmegaB.normalized();
+				kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
+			}
+
+			btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
+
+			static bool bDoTorque = true;
+			if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
+			{
+				avgAxis.normalize();
+				kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
+				kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
+				btScalar kInvCombined = kAxisAInv + kAxisBInv;
+
+				btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
+									(kInvCombined * kInvCombined);
+
+				if (m_maxMotorImpulse >= 0)
+				{
+					btScalar fMaxImpulse = m_maxMotorImpulse;
+					if (m_bNormalizedMotorStrength)
+						fMaxImpulse = fMaxImpulse/kAxisAInv;
+
+					btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
+					btScalar  newUnclampedMag = newUnclampedAccImpulse.length();
+					if (newUnclampedMag > fMaxImpulse)
+					{
+						newUnclampedAccImpulse.normalize();
+						newUnclampedAccImpulse *= fMaxImpulse;
+						impulse = newUnclampedAccImpulse - m_accMotorImpulse;
+					}
+					m_accMotorImpulse += impulse;
+				}
+
+				btScalar  impulseMag  = impulse.length();
+				btVector3 impulseAxis =  impulse / impulseMag;
+
+				bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
+				bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+
+			}
+		}
+		else // no motor: do a little damping
+		{
+			const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
+			const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
+			btVector3 relVel = angVelB - angVelA;
+			if (relVel.length2() > SIMD_EPSILON)
+			{
+				btVector3 relVelAxis = relVel.normalized();
+				btScalar m_kDamping =  btScalar(1.) /
+					(getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
+					 getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
+				btVector3 impulse = m_damping * m_kDamping * relVel;
+
+				btScalar  impulseMag  = impulse.length();
+				btVector3 impulseAxis = impulse / impulseMag;
+				bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
+				bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+			}
+		}
+
+		// joint limits
+		{
+			///solve angular part
+			btVector3 angVelA;
+			bodyA.getAngularVelocity(angVelA);
+			btVector3 angVelB;
+			bodyB.getAngularVelocity(angVelB);
+
+			// solve swing limit
+			if (m_solveSwingLimit)
+			{
+				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;
+				btScalar impulseMag = amplitude * m_kSwing;
+
+				// Clamp the accumulated impulse
+				btScalar temp = m_accSwingLimitImpulse;
+				m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
+				impulseMag = m_accSwingLimitImpulse - temp;
+
+				btVector3 impulse = m_swingAxis * impulseMag;
+
+				// don't let cone response affect twist
+				// (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
+				{
+					btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
+					btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
+					impulse = impulseNoTwistCouple;
+				}
+
+				impulseMag = impulse.length();
+				btVector3 noTwistSwingAxis = impulse / impulseMag;
+
+				bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
+				bodyB.applyImpulse(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)
+					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) );
+				impulseMag = m_accTwistLimitImpulse - temp;
+
+				btVector3 impulse = m_twistAxis * impulseMag;
+
+				bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
+				bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
+			}		
+		}
+	}
+
+}
+
+//-----------------------------------------------------------------------------
+
+void	btConeTwistConstraint::updateRHS(btScalar	timeStep)
+{
+	(void)timeStep;
+
+}
+
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::calcAngleInfo()
+{
+	m_swingCorrection = btScalar(0.);
+	m_twistLimitSign = btScalar(0.);
+	m_solveTwistLimit = false;
+	m_solveSwingLimit = false;
 
 	btVector3 b1Axis1,b1Axis2,b1Axis3;
@@ -123,5 +524,4 @@
 	{
 		b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
-//		swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
 		swx = b2Axis1.dot(b1Axis1);
 		swy = b2Axis1.dot(b1Axis2);
@@ -130,5 +530,4 @@
 		fact = fact / (fact + btScalar(1.0));
 		swing1 *= fact; 
-
 	}
 
@@ -136,5 +535,4 @@
 	{
 		b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);			
-//		swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
 		swx = b2Axis1.dot(b1Axis1);
 		swy = b2Axis1.dot(b1Axis3);
@@ -153,15 +551,9 @@
 		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.normalize();
-
 		btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
 		m_swingAxis *= swingAxisSign;
-
-		m_kSwing =  btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
-			getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
-
 	}
 
@@ -173,117 +565,425 @@
 		btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
 		btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
-
-		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
+		m_twistAngle = twist;
+
+//		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)
 		{
 			m_twistCorrection = -(twist + m_twistSpan);
 			m_solveTwistLimit = true;
-
 			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
 			m_twistAxis.normalize();
 			m_twistAxis *= -1.0f;
-
-			m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
-				getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
-
-		}	else
-			if (twist >  m_twistSpan*lockedFreeFactor)
-			{
-				m_twistCorrection = (twist - m_twistSpan);
+		}
+		else if (twist >  m_twistSpan*lockedFreeFactor)
+		{
+			m_twistCorrection = (twist - m_twistSpan);
+			m_solveTwistLimit = true;
+			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+			m_twistAxis.normalize();
+		}
+	}
+} // btConeTwistConstraint::calcAngleInfo()
+
+
+static btVector3 vTwist(1,0,0); // twist axis in constraint's space
+
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::calcAngleInfo2()
+{
+	m_swingCorrection = btScalar(0.);
+	m_twistLimitSign = btScalar(0.);
+	m_solveTwistLimit = false;
+	m_solveSwingLimit = false;
+
+	{
+		// compute rotation of A wrt B (in constraint space)
+		btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation();
+		btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation();
+		btQuaternion qAB = qB.inverse() * qA;
+
+		// split rotation into cone and twist
+		// (all this is done from B's perspective. Maybe I should be averaging axes...)
+		btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
+		btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
+		btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
+
+		if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
+		{
+			btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
+			computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
+
+			if (swingAngle > swingLimit * m_limitSoftness)
+			{
+				m_solveSwingLimit = true;
+
+				// compute limit ratio: 0->1, where
+				// 0 == beginning of soft limit
+				// 1 == hard/real limit
+				m_swingLimitRatio = 1.f;
+				if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
+				{
+					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);
+
+				// adjustment of swing axis (based on ellipse normal)
+				adjustSwingAxisToUseEllipseNormal(swingAxis);
+
+				// Calculate necessary axis & factors		
+				m_swingAxis = quatRotate(qB, -swingAxis);
+
+				m_twistAxisA.setValue(0,0,0);
+
+				m_kSwing =  btScalar(1.) /
+					(getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
+					 getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
+			}
+		}
+		else
+		{
+			// you haven't set any limits;
+			// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
+			// anyway, we have either hinge or fixed joint
+			btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
+			btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
+			btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+			btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0);
+			btVector3 target;
+			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))))
+				{
+					m_solveSwingLimit = true;
+					m_swingAxis = -ivB.cross(ivA);
+				}
+			}
+			else
+			{
+				if(m_swingSpan1 < m_fixThresh)
+				{ // hinge around Y axis
+					if(!(btFuzzyZero(y)))
+					{
+						m_solveSwingLimit = true;
+						if(m_swingSpan2 >= m_fixThresh)
+						{
+							y = btScalar(0.f);
+							btScalar span2 = btAtan2(z, x);
+							if(span2 > m_swingSpan2)
+							{
+								x = btCos(m_swingSpan2);
+								z = btSin(m_swingSpan2);
+							}
+							else if(span2 < -m_swingSpan2)
+							{
+								x =  btCos(m_swingSpan2);
+								z = -btSin(m_swingSpan2);
+							}
+						}
+					}
+				}
+				else
+				{ // hinge around Z axis
+					if(!btFuzzyZero(z))
+					{
+						m_solveSwingLimit = true;
+						if(m_swingSpan1 >= m_fixThresh)
+						{
+							z = btScalar(0.f);
+							btScalar span1 = btAtan2(y, x);
+							if(span1 > m_swingSpan1)
+							{
+								x = btCos(m_swingSpan1);
+								y = btSin(m_swingSpan1);
+							}
+							else if(span1 < -m_swingSpan1)
+							{
+								x =  btCos(m_swingSpan1);
+								y = -btSin(m_swingSpan1);
+							}
+						}
+					}
+				}
+				target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
+				target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
+				target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
+				target.normalize();
+				m_swingAxis = -ivB.cross(target);
+				m_swingCorrection = m_swingAxis.length();
+				m_swingAxis.normalize();
+			}
+		}
+
+		if (m_twistSpan >= btScalar(0.f))
+		{
+			btVector3 twistAxis;
+			computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
+
+			if (m_twistAngle > m_twistSpan*m_limitSoftness)
+			{
 				m_solveTwistLimit = true;
 
-				m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
-				m_twistAxis.normalize();
-
-				m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
-					getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
-
-			}
-	}
-}
-
-void	btConeTwistConstraint::solveConstraint(btScalar	timeStep)
-{
-
-	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_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-
-		btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-		btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-		btVector3 vel = vel1 - vel2;
-
-		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;
-			m_appliedImpulse += impulse;
-			btVector3 impulse_vector = normal * impulse;
-			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
-			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
-		}
-	}
-	
-	{
-		///solve angular part
-		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
-		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
-
-		// solve swing limit
-		if (m_solveSwingLimit)
-		{
-			btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
-			btScalar impulseMag = amplitude * m_kSwing;
-
-			// Clamp the accumulated impulse
-			btScalar temp = m_accSwingLimitImpulse;
-			m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
-			impulseMag = m_accSwingLimitImpulse - temp;
-
-			btVector3 impulse = m_swingAxis * impulseMag;
-
-			m_rbA.applyTorqueImpulse(impulse);
-			m_rbB.applyTorqueImpulse(-impulse);
-
-		}
-
-		// solve twist limit
-		if (m_solveTwistLimit)
-		{
-			btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
-			btScalar impulseMag = amplitude * m_kTwist;
-
-			// Clamp the accumulated impulse
-			btScalar temp = m_accTwistLimitImpulse;
-			m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
-			impulseMag = m_accTwistLimitImpulse - temp;
-
-			btVector3 impulse = m_twistAxis * impulseMag;
-
-			m_rbA.applyTorqueImpulse(impulse);
-			m_rbB.applyTorqueImpulse(-impulse);
-
-		}
-	
-	}
-
-}
-
-void	btConeTwistConstraint::updateRHS(btScalar	timeStep)
-{
-	(void)timeStep;
-
-}
+				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);
+				}
+
+				// twist correction tries to get back to soft limit
+				m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
+
+				m_twistAxis = quatRotate(qB, -twistAxis);
+
+				m_kTwist = btScalar(1.) /
+					(getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
+					 getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+			}
+
+			if (m_solveSwingLimit)
+				m_twistAxisA = quatRotate(qA, -twistAxis);
+		}
+		else
+		{
+			m_twistAngle = btScalar(0.f);
+		}
+	}
+} // btConeTwistConstraint::calcAngleInfo2()
+
+
+
+// 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
+{
+	swingAngle = qCone.getAngle();
+	if (swingAngle > SIMD_EPSILON)
+	{
+		vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
+		vSwingAxis.normalize();
+		if (fabs(vSwingAxis.x()) > SIMD_EPSILON)
+		{
+			// non-zero twist?! this should never happen.
+			int wtf = 0; wtf = wtf;
+		}
+
+		// 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.)
+
+		// 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 yEllipse = -vSwingAxis.z();
+
+		// Now, we use the slope of the vector (using x/yEllipse) and find the length
+		// of the line that intersects the ellipse:
+		//  x^2   y^2
+		//  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
+		//  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
+		if (fabs(xEllipse) > SIMD_EPSILON)
+		{
+			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);
+		}
+
+		// test!
+		/*swingLimit = m_swingSpan2;
+		if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
+		{
+		btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
+		btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
+		btScalar phi = asin(sinphi);
+		btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
+		btScalar alpha = 3.14159f - theta - phi;
+		btScalar sinalpha = sin(alpha);
+		swingLimit = m_swingSpan1 * sinphi/sinalpha;
+		}*/
+	}
+	else if (swingAngle < 0)
+	{
+		// this should never happen!
+		int wtf = 0; wtf = wtf;
+	}
+}
+
+btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
+{
+	// compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
+	btScalar xEllipse = btCos(fAngleInRadians);
+	btScalar yEllipse = btSin(fAngleInRadians);
+
+	// Use the slope of the vector (using x/yEllipse) and find the length
+	// of the line that intersects the ellipse:
+	//  x^2   y^2
+	//  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
+	//  a^2   b^2
+	// Do the math and it should be clear.
+
+	float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
+	if (fabs(xEllipse) > SIMD_EPSILON)
+	{
+		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);
+	}
+
+	// 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);
+	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
+{
+	btQuaternion qMinTwist = qTwist;
+	twistAngle = qTwist.getAngle();
+
+	if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
+	{
+		qMinTwist = operator-(qTwist);
+		twistAngle = qMinTwist.getAngle();
+	}
+	if (twistAngle < 0)
+	{
+		// this should never happen
+		int wtf = 0; wtf = wtf;			
+	}
+
+	vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
+	if (twistAngle > SIMD_EPSILON)
+		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 
+	// 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();
+
+	// do the math...
+	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;
+		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);
+		else
+			y = -fabs(grad * z);
+
+		// convert ellipse direction back to swing axis
+		vSwingAxis.setZ(-y);
+		vSwingAxis.setY( z);
+		vSwingAxis.normalize();
+	}
+}
+
+
+
+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);
+	btQuaternion qConstraintCur = trConstraintCur.getRotation();
+
+	btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
+	setMotorTargetInConstraintSpace(qConstraint);
+}
+
+
+void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
+{
+	m_qTarget = q;
+
+	// clamp motor target to within limits
+	{
+		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();
+
+		// clamp cone
+		if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
+		{
+			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;
+				qTargetCone = btQuaternion(swingAxis, swingAngle);
+			}
+		}
+
+		// clamp twist
+		if (m_twistSpan >= btScalar(0.05f))
+		{
+			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;
+				qTargetTwist = btQuaternion(twistAxis, twistAngle);
+			}
+		}
+
+		m_qTarget = qTargetCone * qTargetTwist;
+	}
+}
+
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+
+
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h	(revision 2882)
@@ -43,8 +43,12 @@
 	btScalar	m_relaxationFactor;
 
+	btScalar	m_damping;
+
 	btScalar	m_swingSpan1;
 	btScalar	m_swingSpan2;
 	btScalar	m_twistSpan;
 
+	btScalar	m_fixThresh;
+
 	btVector3   m_swingAxis;
 	btVector3	m_twistAxis;
@@ -57,4 +61,6 @@
 	btScalar	m_twistCorrection;
 
+	btScalar	m_twistAngle;
+
 	btScalar	m_accSwingLimitImpulse;
 	btScalar	m_accTwistLimitImpulse;
@@ -64,4 +70,17 @@
 	bool		m_solveSwingLimit;
 
+	bool	m_useSolveConstraintObsolete;
+
+	// not yet used...
+	btScalar	m_swingLimitRatio;
+	btScalar	m_twistLimitRatio;
+	btVector3   m_twistAxisA;
+
+	// motor
+	bool		 m_bMotorEnabled;
+	bool		 m_bNormalizedMotorStrength;
+	btQuaternion m_qTarget;
+	btScalar	 m_maxMotorImpulse;
+	btVector3	 m_accMotorImpulse;
 	
 public:
@@ -75,5 +94,10 @@
 	virtual void	buildJacobian();
 
-	virtual	void	solveConstraint(btScalar	timeStep);
+	virtual void getInfo1 (btConstraintInfo1* info);
+	
+	virtual void getInfo2 (btConstraintInfo2* info);
+	
+
+	virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
 
 	void	updateRHS(btScalar	timeStep);
@@ -93,5 +117,30 @@
 	}
 
-	void	setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+	void	setLimit(int limitIndex,btScalar limitValue)
+	{
+		switch (limitIndex)
+		{
+		case 3:
+			{
+				m_twistSpan = limitValue;
+				break;
+			}
+		case 4:
+			{
+				m_swingSpan2 = limitValue;
+				break;
+			}
+		case 5:
+			{
+				m_swingSpan1 = limitValue;
+				break;
+			}
+		default:
+			{
+			}
+		};
+	}
+
+	void	setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
 	{
 		m_swingSpan1 = _swingSpan1;
@@ -122,4 +171,58 @@
 	}
 
+	void calcAngleInfo();
+	void calcAngleInfo2();
+
+	inline btScalar getSwingSpan1()
+	{
+		return m_swingSpan1;
+	}
+	inline btScalar getSwingSpan2()
+	{
+		return m_swingSpan2;
+	}
+	inline btScalar getTwistSpan()
+	{
+		return m_twistSpan;
+	}
+	inline btScalar getTwistAngle()
+	{
+		return m_twistAngle;
+	}
+	bool isPastSwingLimit() { return m_solveSwingLimit; }
+
+
+	void setDamping(btScalar damping) { m_damping = damping; }
+
+	void enableMotor(bool b) { m_bMotorEnabled = b; }
+	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; }
+
+	// setMotorTarget:
+	// 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);
+
+	// same as above, but q is the desired rotation of frameA wrt frameB in constraint space
+	void setMotorTargetInConstraintSpace(const btQuaternion &q);
+
+	btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
+
+
+
+protected:
+	void init();
+
+	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;
 };
 
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp	(revision 2882)
@@ -23,5 +23,5 @@
 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
 
-#define ASSERT2 assert
+#define ASSERT2 btAssert
 
 #define USE_INTERNAL_APPLY_IMPULSE 1
@@ -53,5 +53,5 @@
 	
 
-	  btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
+	   btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
 		body2.getCenterOfMassTransform().getBasis().transpose(),
 		rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
@@ -115,5 +115,5 @@
 
 	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	assert(cpd);
+	btAssert(cpd);
 	btScalar distance = cpd->m_penetration;
 	btScalar positionalError = Kcor *-distance;
@@ -167,5 +167,5 @@
 	
 	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	assert(cpd);
+	btAssert(cpd);
 
 	btScalar combinedFriction = cpd->m_friction;
@@ -256,5 +256,5 @@
 	
 	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	assert(cpd);
+	btAssert(cpd);
 
 	btScalar combinedFriction = cpd->m_friction;
@@ -338,5 +338,5 @@
 
 	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	assert(cpd);
+	btAssert(cpd);
 	btScalar distance = cpd->m_penetration;
 	btScalar positionalError = Kcor *-distance;
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h	(revision 2882)
@@ -23,5 +23,10 @@
 	SOLVER_USE_WARMSTARTING = 4,
 	SOLVER_USE_FRICTION_WARMSTARTING = 8,
-	SOLVER_CACHE_FRIENDLY = 16
+	SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
+	SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
+	SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
+	SOLVER_CACHE_FRIENDLY = 128,
+	SOLVER_SIMD = 256,	//enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version
+	SOLVER_CUDA = 512	//will be open sourced during Game Developers Conference 2009. Much faster.
 };
 
@@ -40,4 +45,5 @@
 	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;
@@ -66,10 +72,11 @@
 		m_erp = btScalar(0.2);
 		m_erp2 = btScalar(0.1);
-		m_sor = btScalar(1.3);
+		m_globalCfm = btScalar(0.);
+		m_sor = btScalar(1.);
 		m_splitImpulse = false;
 		m_splitImpulsePenetrationThreshold = -0.02f;
 		m_linearSlop = btScalar(0.0);
 		m_warmstartingFactor=btScalar(0.85);
-		m_solverMode = SOLVER_CACHE_FRIENDLY |  SOLVER_RANDMIZE_ORDER |  SOLVER_USE_WARMSTARTING;
+		m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD ;//SOLVER_RANDMIZE_ORDER
 		m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
 	}
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp	(revision 2882)
@@ -20,5 +20,4 @@
 */
 
-
 #include "btGeneric6DofConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
@@ -27,5 +26,31 @@
 
 
+#define D6_USE_OBSOLETE_METHOD false
+//-----------------------------------------------------------------------------
+
+btGeneric6DofConstraint::btGeneric6DofConstraint()
+:btTypedConstraint(D6_CONSTRAINT_TYPE),
+m_useLinearReferenceFrameA(true),
+m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
+{
+}
+
+//-----------------------------------------------------------------------------
+
+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_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
+{
+
+}
+//-----------------------------------------------------------------------------
+
+
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
+
+//-----------------------------------------------------------------------------
 
 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
@@ -37,48 +62,45 @@
 }
 
+//-----------------------------------------------------------------------------
+
 ///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)
 {
-//	// 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
-//
-
-		if (btGetMatrixElem(mat,2) < btScalar(1.0))
-		{
-			if (btGetMatrixElem(mat,2) > btScalar(-1.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[1] = -SIMD_HALF_PI;
-				xyz[2] = btScalar(0.0);
-				return false;
-			}
+	//	// rot =  cy*cz          -cy*sz           sy
+	//	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+	//	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+	//
+
+	btScalar fi = btGetMatrixElem(mat,2);
+	if (fi < btScalar(1.0f))
+	{
+		if (fi > btScalar(-1.0f))
+		{
+			xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
+			xyz[1] = btAsin(btGetMatrixElem(mat,2));
+			xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+			return true;
 		}
 		else
 		{
-			// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
-			xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
-			xyz[1] = SIMD_HALF_PI;
-			xyz[2] = 0.0;
-
-		}
-
-
+			// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
+			xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+			xyz[1] = -SIMD_HALF_PI;
+			xyz[2] = btScalar(0.0);
+			return false;
+		}
+	}
+	else
+	{
+		// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
+		xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+		xyz[1] = SIMD_HALF_PI;
+		xyz[2] = 0.0;
+	}
 	return false;
 }
 
-
-
 //////////////////////////// btRotationalLimitMotor ////////////////////////////////////
-
 
 int btRotationalLimitMotor::testLimitValue(btScalar test_value)
@@ -105,83 +127,88 @@
 	m_currentLimit = 0;//Free from violation
 	return 0;
-	
-}
-
+
+}
+
+//-----------------------------------------------------------------------------
 
 btScalar btRotationalLimitMotor::solveAngularLimits(
-		btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
-	 	btRigidBody * body0, btRigidBody * body1)
-{
-    if (needApplyTorques()==false) return 0.0f;
-
-    btScalar target_velocity = m_targetVelocity;
-    btScalar maxMotorForce = m_maxMotorForce;
+	btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
+	btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
+{
+	if (needApplyTorques()==false) return 0.0f;
+
+	btScalar target_velocity = m_targetVelocity;
+	btScalar maxMotorForce = m_maxMotorForce;
 
 	//current error correction
-    if (m_currentLimit!=0)
-    {
-        target_velocity = -m_ERP*m_currentLimitError/(timeStep);
-        maxMotorForce = m_maxLimitForce;
-    }
-
-    maxMotorForce *= timeStep;
-
-    // current velocity difference
-    btVector3 vel_diff = body0->getAngularVelocity();
-    if (body1)
-    {
-        vel_diff -= body1->getAngularVelocity();
-    }
-
-
-
-    btScalar rel_vel = axis.dot(vel_diff);
+	if (m_currentLimit!=0)
+	{
+		target_velocity = -m_ERP*m_currentLimitError/(timeStep);
+		maxMotorForce = m_maxLimitForce;
+	}
+
+	maxMotorForce *= timeStep;
+
+	// current velocity difference
+
+	btVector3 angVelA;
+	bodyA.getAngularVelocity(angVelA);
+	btVector3 angVelB;
+	bodyB.getAngularVelocity(angVelB);
+
+	btVector3 vel_diff;
+	vel_diff = angVelA-angVelB;
+
+
+
+	btScalar rel_vel = axis.dot(vel_diff);
 
 	// correction velocity
-    btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
-
-
-    if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
-    {
-        return 0.0f;//no need for applying force
-    }
+	btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
+
+
+	if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
+	{
+		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)
-    {
-        clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
-    }
-    else
-    {
-        clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
-    }
+	btScalar clippedMotorImpulse;
+
+	///@todo: should clip against accumulated impulse
+	if (unclippedMotorImpulse>0.0f)
+	{
+		clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
+	}
+	else
+	{
+		clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
+	}
 
 
 	// sort with accumulated impulses
-    btScalar	lo = btScalar(-1e30);
-    btScalar	hi = btScalar(1e30);
-
-    btScalar oldaccumImpulse = m_accumulatedImpulse;
-    btScalar sum = oldaccumImpulse + clippedMotorImpulse;
-    m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
-
-    clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
-
-
-
-    btVector3 motorImp = clippedMotorImpulse * axis;
-
-
-    body0->applyTorqueImpulse(motorImp);
-    if (body1) body1->applyTorqueImpulse(-motorImp);
-
-    return clippedMotorImpulse;
+	btScalar	lo = btScalar(-1e30);
+	btScalar	hi = btScalar(1e30);
+
+	btScalar oldaccumImpulse = m_accumulatedImpulse;
+	btScalar sum = oldaccumImpulse + clippedMotorImpulse;
+	m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+
+	clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
+
+	btVector3 motorImp = clippedMotorImpulse * axis;
+
+	//body0->applyTorqueImpulse(motorImp);
+	//body1->applyTorqueImpulse(-motorImp);
+
+	bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
+	bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
+
+
+	return clippedMotorImpulse;
 
 
@@ -190,125 +217,147 @@
 //////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
 
+
+
+
 //////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
+
+
+int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
+{
+	btScalar loLimit = m_lowerLimit[limitIndex];
+	btScalar hiLimit = m_upperLimit[limitIndex];
+	if(loLimit > hiLimit)
+	{
+		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;
+		return 2;
+	}
+	else if (test_value> hiLimit)
+	{
+		m_currentLimit[limitIndex] = 1;//High limit violation
+		m_currentLimitError[limitIndex] = test_value - hiLimit;
+		return 1;
+	};
+
+	m_currentLimit[limitIndex] = 0;//Free from violation
+	m_currentLimitError[limitIndex] = btScalar(0.f);
+	return 0;
+} // btTranslationalLimitMotor::testLimitValue()
+
+//-----------------------------------------------------------------------------
+
 btScalar btTranslationalLimitMotor::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)
-{
-
-///find relative velocity
-//    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
-//    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
-    btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
-    btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
-
-    btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-    btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-    btVector3 vel = vel1 - vel2;
-
-    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(-1e30);
-    btScalar	hi = btScalar(1e30);
-
-    btScalar minLimit = m_lowerLimit[limit_index];
-    btScalar maxLimit = m_upperLimit[limit_index];
-
-    //handle the limits
-    if (minLimit < maxLimit)
-    {
-        {
-            if (depth > maxLimit)
-            {
-                depth -= maxLimit;
-                lo = btScalar(0.);
-
-            }
-            else
-            {
-                if (depth < minLimit)
-                {
-                    depth -= minLimit;
-                    hi = btScalar(0.);
-                }
-                else
-                {
-                    return 0.0f;
-                }
-            }
-        }
-    }
-
-    btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
-
-
-
-
-    btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
-    btScalar sum = oldNormalImpulse + normalImpulse;
-    m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
-    normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
-
-    btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
-    body1.applyImpulse( impulse_vector, rel_pos1);
-    body2.applyImpulse(-impulse_vector, rel_pos2);
-    return normalImpulse;
+	btScalar timeStep,
+	btScalar jacDiagABInv,
+	btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
+	btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
+	int limit_index,
+	const btVector3 & axis_normal_on_a,
+	const btVector3 & anchorPos)
+{
+
+	///find relative velocity
+	//    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
+	//    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
+	btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
+	btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
+
+	btVector3 vel1;
+	bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+	btVector3 vel2;
+	bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+	btVector3 vel = vel1 - vel2;
+
+	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(-1e30);
+	btScalar	hi = btScalar(1e30);
+
+	btScalar minLimit = m_lowerLimit[limit_index];
+	btScalar maxLimit = m_upperLimit[limit_index];
+
+	//handle the limits
+	if (minLimit < maxLimit)
+	{
+		{
+			if (depth > maxLimit)
+			{
+				depth -= maxLimit;
+				lo = btScalar(0.);
+
+			}
+			else
+			{
+				if (depth < minLimit)
+				{
+					depth -= minLimit;
+					hi = btScalar(0.);
+				}
+				else
+				{
+					return 0.0f;
+				}
+			}
+		}
+	}
+
+	btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
+
+
+
+
+	btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
+	btScalar sum = oldNormalImpulse + normalImpulse;
+	m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+	normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
+
+	btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
+	//body1.applyImpulse( impulse_vector, rel_pos1);
+	//body2.applyImpulse(-impulse_vector, rel_pos2);
+
+	btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
+	btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
+	bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+	bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+
+
+
+
+	return normalImpulse;
 }
 
 //////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
 
-
-btGeneric6DofConstraint::btGeneric6DofConstraint()
-        :btTypedConstraint(D6_CONSTRAINT_TYPE),
-		m_useLinearReferenceFrameA(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)
-{
-
-}
-
-
-
-
-
 void btGeneric6DofConstraint::calculateAngleInfo()
 {
 	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]) :
-  //
-  //    to get			constrain w2-w1 along		...not
-  //    ------			---------------------		------
-  //    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
-  //    d(angle[1])/dt = 0	ax[1]
-  //    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
-  //
-  // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
-  // to prove the result for angle[0], write the expression for angle[0] from
-  // GetInfo1 then take the derivative. to prove this for angle[2] it is
-  // easier to take the euler rate expression for d(angle[2])/dt with respect
-  // to the components of w and set that to 0.
-
+	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
+	//
+	//    to get			constrain w2-w1 along		...not
+	//    ------			---------------------		------
+	//    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
+	//    d(angle[1])/dt = 0	ax[1]
+	//    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
+	//
+	// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+	// to prove the result for angle[0], write the expression for angle[0] from
+	// GetInfo1 then take the derivative. to prove this for angle[2] it is
+	// easier to take the euler rate expression for d(angle[2])/dt with respect
+	// to the components of w and set that to 0.
 	btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
 	btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
@@ -318,32 +367,27 @@
 	m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
 
-
-//    if(m_debugDrawer)
-//    {
-//
-//    	char buff[300];
-//		sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
-//		m_calculatedAxisAngleDiff[0],
-//		m_calculatedAxisAngleDiff[1],
-//		m_calculatedAxisAngleDiff[2]);
-//    	m_debugDrawer->reportErrorWarning(buff);
-//    }
-
-}
+	m_calculatedAxis[0].normalize();
+	m_calculatedAxis[1].normalize();
+	m_calculatedAxis[2].normalize();
+
+}
+
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::calculateTransforms()
 {
-    m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
-    m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
-
-    calculateAngleInfo();
-}
-
+	m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
+	m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
+	calculateLinearInfo();
+	calculateAngleInfo();
+}
+
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::buildLinearJacobian(
-    btJacobianEntry & jacLinear,const btVector3 & normalWorld,
-    const btVector3 & pivotAInW,const btVector3 & pivotBInW)
-{
-    new (&jacLinear) btJacobianEntry(
+	btJacobianEntry & jacLinear,const btVector3 & normalWorld,
+	const btVector3 & pivotAInW,const btVector3 & pivotBInW)
+{
+	new (&jacLinear) btJacobianEntry(
         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -355,11 +399,12 @@
         m_rbB.getInvInertiaDiagLocal(),
         m_rbB.getInvMass());
-
-}
+}
+
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::buildAngularJacobian(
-    btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
-{
-    new (&jacAngular)	btJacobianEntry(jointAxisW,
+	btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
+{
+	 new (&jacAngular)	btJacobianEntry(jointAxisW,
                                       m_rbA.getCenterOfMassTransform().getBasis().transpose(),
                                       m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -369,139 +414,257 @@
 }
 
+//-----------------------------------------------------------------------------
+
 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 {
-    btScalar angle = m_calculatedAxisAngleDiff[axis_index];
-
-    //test limits
-    m_angularLimits[axis_index].testLimitValue(angle);
-    return m_angularLimits[axis_index].needApplyTorques();
-}
+	btScalar angle = m_calculatedAxisAngleDiff[axis_index];
+	//test limits
+	m_angularLimits[axis_index].testLimitValue(angle);
+	return m_angularLimits[axis_index].needApplyTorques();
+}
+
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::buildJacobian()
 {
-
-	// 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++)
-    {
-        m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
-    }
-    //calculates transform
-    calculateTransforms();
-
-//  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
-//  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
-	calcAnchorPos();
-	btVector3 pivotAInW = m_AnchorPos;
-	btVector3 pivotBInW = m_AnchorPos;
-
-// not used here
-//    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
-//    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-
-    btVector3 normalWorld;
-    //linear part
-    for (i=0;i<3;i++)
-    {
-        if (m_linearLimits.isLimited(i))
-        {
-			if (m_useLinearReferenceFrameA)
-	            normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
-			else
-	            normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
-
-            buildLinearJacobian(
-                m_jacLinear[i],normalWorld ,
-                pivotAInW,pivotBInW);
-
-        }
-    }
-
-    // angular part
-    for (i=0;i<3;i++)
-    {
-        //calculates error angle
-        if (testAngularLimitMotor(i))
-        {
-            normalWorld = this->getAxis(i);
-            // Create angular atom
-            buildAngularJacobian(m_jacAng[i],normalWorld);
-        }
-    }
-
-
-}
-
-
-void btGeneric6DofConstraint::solveConstraint(btScalar	timeStep)
-{
-    m_timeStep = timeStep;
-
-    //calculateTransforms();
-
-    int i;
-
-    // linear
-
-    btVector3 pointInA = m_calculatedTransformA.getOrigin();
-	btVector3 pointInB = m_calculatedTransformB.getOrigin();
-
-	btScalar jacDiagABInv;
-	btVector3 linear_axis;
-    for (i=0;i<3;i++)
-    {
-        if (m_linearLimits.isLimited(i))
-        {
-            jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
-
-			if (m_useLinearReferenceFrameA)
-	            linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
-			else
-	            linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
-
-            m_linearLimits.solveLinearAxis(
-            	m_timeStep,
-            	jacDiagABInv,
-            	m_rbA,pointInA,
-                m_rbB,pointInB,
-                i,linear_axis, m_AnchorPos);
-
-        }
-    }
-
-    // angular
-    btVector3 angular_axis;
-    btScalar angularJacDiagABInv;
-    for (i=0;i<3;i++)
-    {
-        if (m_angularLimits[i].needApplyTorques())
-        {
-
-			// get axis
-			angular_axis = getAxis(i);
-
-			angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
-
-			m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB);
-        }
-    }
-}
+	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++)
+		{
+			m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
+		}
+		//calculates transform
+		calculateTransforms();
+
+		//  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
+		//  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
+		calcAnchorPos();
+		btVector3 pivotAInW = m_AnchorPos;
+		btVector3 pivotBInW = m_AnchorPos;
+
+		// not used here
+		//    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
+		//    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+		btVector3 normalWorld;
+		//linear part
+		for (i=0;i<3;i++)
+		{
+			if (m_linearLimits.isLimited(i))
+			{
+				if (m_useLinearReferenceFrameA)
+					normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+				else
+					normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
+
+				buildLinearJacobian(
+					m_jacLinear[i],normalWorld ,
+					pivotAInW,pivotBInW);
+
+			}
+		}
+
+		// angular part
+		for (i=0;i<3;i++)
+		{
+			//calculates error angle
+			if (testAngularLimitMotor(i))
+			{
+				normalWorld = this->getAxis(i);
+				// Create angular atom
+				buildAngularJacobian(m_jacAng[i],normalWorld);
+			}
+		}
+
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
+	} else
+	{
+		//prepare constraint
+		calculateTransforms();
+		info->m_numConstraintRows = 0;
+		info->nub = 6;
+		int i;
+		//test linear limits
+		for(i = 0; i < 3; i++)
+		{
+			if(m_linearLimits.needApplyForce(i))
+			{
+				info->m_numConstraintRows++;
+				info->nub--;
+			}
+		}
+		//test angular limits
+		for (i=0;i<3 ;i++ )
+		{
+			if(testAngularLimitMotor(i))
+			{
+				info->m_numConstraintRows++;
+				info->nub--;
+			}
+		}
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	btAssert(!m_useSolveConstraintObsolete);
+	int row = setLinearLimits(info);
+	setAngularLimits(info, row);
+}
+
+//-----------------------------------------------------------------------------
+
+int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
+{
+	btGeneric6DofConstraint * d6constraint = this;
+	int row = 0;
+	//solve linear limits
+	btRotationalLimitMotor limot;
+	for (int i=0;i<3 ;i++ )
+	{
+		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_currentLimitError  = m_linearLimits.m_currentLimitError[i];
+			limot.m_damping  = m_linearLimits.m_damping;
+			limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
+			limot.m_ERP  = m_linearLimits.m_restitution;
+			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);
+			row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0);
+		}
+	}
+	return row;
+}
+
+//-----------------------------------------------------------------------------
+
+int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
+{
+	btGeneric6DofConstraint * d6constraint = this;
+	int row = row_offset;
+	//solve angular limits
+	for (int i=0;i<3 ;i++ )
+	{
+		if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
+		{
+			btVector3 axis = d6constraint->getAxis(i);
+			row += get_limit_motor_info2(
+				d6constraint->getRotationalLimitMotor(i),
+				&m_rbA,
+				&m_rbB,
+				info,row,axis,1);
+		}
+	}
+
+	return row;
+}
+
+//-----------------------------------------------------------------------------
+
+void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+
+
+		m_timeStep = timeStep;
+
+		//calculateTransforms();
+
+		int i;
+
+		// linear
+
+		btVector3 pointInA = m_calculatedTransformA.getOrigin();
+		btVector3 pointInB = m_calculatedTransformB.getOrigin();
+
+		btScalar jacDiagABInv;
+		btVector3 linear_axis;
+		for (i=0;i<3;i++)
+		{
+			if (m_linearLimits.isLimited(i))
+			{
+				jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
+
+				if (m_useLinearReferenceFrameA)
+					linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
+				else
+					linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
+
+				m_linearLimits.solveLinearAxis(
+					m_timeStep,
+					jacDiagABInv,
+					m_rbA,bodyA,pointInA,
+					m_rbB,bodyB,pointInB,
+					i,linear_axis, m_AnchorPos);
+
+			}
+		}
+
+		// angular
+		btVector3 angular_axis;
+		btScalar angularJacDiagABInv;
+		for (i=0;i<3;i++)
+		{
+			if (m_angularLimits[i].needApplyTorques())
+			{
+
+				// get axis
+				angular_axis = getAxis(i);
+
+				angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
+
+				m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB);
+			}
+		}
+	}
+}
+
+//-----------------------------------------------------------------------------
 
 void	btGeneric6DofConstraint::updateRHS(btScalar	timeStep)
 {
-    (void)timeStep;
-
-}
+	(void)timeStep;
+
+}
+
+//-----------------------------------------------------------------------------
 
 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
 {
-    return m_calculatedAxis[axis_index];
-}
+	return m_calculatedAxis[axis_index];
+}
+
+//-----------------------------------------------------------------------------
 
 btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
 {
-    return m_calculatedAxisAngleDiff[axis_index];
-}
+	return m_calculatedAxisAngleDiff[axis_index];
+}
+
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::calcAnchorPos(void)
@@ -524,2 +687,143 @@
 } // btGeneric6DofConstraint::calcAnchorPos()
 
+//-----------------------------------------------------------------------------
+
+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++)
+	{
+		m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
+	}
+} // btGeneric6DofConstraint::calculateLinearInfo()
+
+//-----------------------------------------------------------------------------
+
+int btGeneric6DofConstraint::get_limit_motor_info2(
+	btRotationalLimitMotor * limot,
+	btRigidBody * body0, btRigidBody * body1,
+	btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
+{
+    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 : 0;
+        J1[srow+0] = ax1[0];
+        J1[srow+1] = ax1[1];
+        J1[srow+2] = ax1[2];
+        if(rotational)
+        {
+            J2[srow+0] = -ax1[0];
+            J2[srow+1] = -ax1[1];
+            J2[srow+2] = -ax1[2];
+        }
+        if((!rotational) && limit)
+        {
+			btVector3 ltd;	// Linear Torque Decoupling vector
+			btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition();
+			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];
+
+			c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition();
+			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];
+        }
+        // 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)
+        {
+            info->cfm[srow] = 0.0f;
+            if(!limit)
+            {
+                info->m_constraintError[srow] += 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_ERP;
+			if(!rotational)
+			{
+				info->m_constraintError[srow] += k * limot->m_currentLimitError;
+			}
+			else
+			{
+				info->m_constraintError[srow] += -k * limot->m_currentLimitError;
+			}
+            info->cfm[srow] = 0.0f;
+            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 = body0->getAngularVelocity().dot(ax1);
+                        if (body1)
+                            vel -= body1->getAngularVelocity().dot(ax1);
+                    }
+                    else
+                    {
+                        vel = body0->getLinearVelocity().dot(ax1);
+                        if (body1)
+                            vel -= body1->getLinearVelocity().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]) 
+								info->m_constraintError[srow] = newc;
+                        }
+                    }
+                }
+            }
+        }
+        return 1;
+    }
+    else return 0;
+}
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h	(revision 2882)
@@ -29,4 +29,6 @@
 
 class btRigidBody;
+
+
 
 
@@ -93,5 +95,5 @@
     bool isLimited()
     {
-    	if(m_loLimit>=m_hiLimit) return false;
+    	if(m_loLimit > m_hiLimit) return false;
     	return true;
     }
@@ -111,6 +113,5 @@
 
 	//! 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, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
 
 };
@@ -130,4 +131,9 @@
     btScalar	m_restitution;//! Bounce parameter for linear 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
+    int			m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
 
     btTranslationalLimitMotor()
@@ -140,4 +146,10 @@
     	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);
+		}
     }
 
@@ -151,4 +163,10 @@
     	m_damping = other.m_damping;
     	m_restitution = other.m_restitution;
+		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];
+		}
     }
 
@@ -164,4 +182,10 @@
        return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
     }
+    inline bool needApplyForce(int limitIndex)
+    {
+    	if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
+    	return true;
+    }
+	int testLimitValue(int limitIndex, btScalar test_value);
 
 
@@ -169,6 +193,6 @@
     	btScalar timeStep,
         btScalar jacDiagABInv,
-        btRigidBody& body1,const btVector3 &pointInA,
-        btRigidBody& body2,const btVector3 &pointInB,
+        btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
+        btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
         int limit_index,
         const btVector3 & axis_normal_on_a,
@@ -248,4 +272,5 @@
     btVector3 m_calculatedAxisAngleDiff;
     btVector3 m_calculatedAxis[3];
+    btVector3 m_calculatedLinearDiff;
     
 	btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
@@ -263,4 +288,7 @@
 
 
+	int setAngularLimits(btConstraintInfo2 *info, int row_offset);
+
+	int setLinearLimits(btConstraintInfo2 *info);
 
     void buildLinearJacobian(
@@ -270,4 +298,6 @@
     void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
 
+	// tests linear limits
+	void calculateLinearInfo();
 
 	//! calcs the euler angles between the two bodies.
@@ -277,4 +307,8 @@
 
 public:
+
+	///for backwards compatibility during the transition to 'getInfo/getInfo2'
+	bool		m_useSolveConstraintObsolete;
+
     btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
 
@@ -331,5 +365,9 @@
     virtual void	buildJacobian();
 
-    virtual	void	solveConstraint(btScalar	timeStep);
+	virtual void getInfo1 (btConstraintInfo1* info);
+
+	virtual void getInfo2 (btConstraintInfo2* info);
+
+    virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
 
     void	updateRHS(btScalar	timeStep);
@@ -433,4 +471,9 @@
 	virtual void calcAnchorPos(void); // overridable
 
+	int get_limit_motor_info2(	btRotationalLimitMotor * limot,
+								btRigidBody * body0, btRigidBody * body1,
+								btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
+
+
 };
 
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp	(revision 2882)
@@ -20,17 +20,31 @@
 #include "LinearMath/btMinMax.h"
 #include <new>
+#include "btSolverBody.h"
+
+//-----------------------------------------------------------------------------
+
+#define HINGE_USE_OBSOLETE_SOLVER false
+
+//-----------------------------------------------------------------------------
 
 
 btHingeConstraint::btHingeConstraint()
 : btTypedConstraint (HINGE_CONSTRAINT_TYPE),
-m_enableAngularMotor(false)
-{
-}
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(false)
+{
+	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
-									 btVector3& axisInA,btVector3& axisInB)
+									 btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
 									 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
 									 m_angularOnly(false),
-									 m_enableAngularMotor(false)
+									 m_enableAngularMotor(false),
+									 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+									 m_useReferenceFrameA(useReferenceFrameA)
 {
 	m_rbAFrame.getOrigin() = pivotInA;
@@ -61,7 +75,7 @@
 	
 	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() );
+	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
+									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
+									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
 	
 	//start with free
@@ -72,11 +86,13 @@
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
-
-}
-
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
-:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
+	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), 
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
 {
 
@@ -91,5 +107,5 @@
 									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
 
-	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
+	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
 
 	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
@@ -110,17 +126,17 @@
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
-}
+	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
-								     const btTransform& rbAFrame, const btTransform& rbBFrame)
+								     const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
 m_angularOnly(false),
-m_enableAngularMotor(false)
-{
-	// flip axis
-	m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
-
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
+{
 	//start with free
 	m_lowerLimit = btScalar(1e30);
@@ -130,19 +146,17 @@
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
+	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }			
 
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
+//-----------------------------------------------------------------------------
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
 m_angularOnly(false),
-m_enableAngularMotor(false)
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
 {
 	///not providing rigidbody B means implicitly using worldspace for body B
-
-	// flip axis
-	m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
 
 	m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
@@ -155,31 +169,36 @@
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
-}
+	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
 
 void	btHingeConstraint::buildJacobian()
 {
-	m_appliedImpulse = btScalar(0.);
-
-	if (!m_angularOnly)
-	{
-		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
-		btVector3 relPos = pivotBInW - pivotAInW;
-
-		btVector3 normal[3];
-		if (relPos.length2() > SIMD_EPSILON)
-		{
-			normal[0] = relPos.normalized();
-		}
-		else
-		{
-			normal[0].setValue(btScalar(1.0),0,0);
-		}
-
-		btPlaneSpace1(normal[0], normal[1], normal[2]);
-
-		for (int i=0;i<3;i++)
-		{
-			new (&m_jac[i]) btJacobianEntry(
+	if (m_useSolveConstraintObsolete)
+	{
+		m_appliedImpulse = btScalar(0.);
+
+		if (!m_angularOnly)
+		{
+			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+			btVector3 relPos = pivotBInW - pivotAInW;
+
+			btVector3 normal[3];
+			if (relPos.length2() > SIMD_EPSILON)
+			{
+				normal[0] = relPos.normalized();
+			}
+			else
+			{
+				normal[0].setValue(btScalar(1.0),0,0);
+			}
+
+			btPlaneSpace1(normal[0], normal[1], normal[2]);
+
+			for (int i=0;i<3;i++)
+			{
+				new (&m_jac[i]) btJacobianEntry(
 				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -191,212 +210,457 @@
 				m_rbB.getInvInertiaDiagLocal(),
 				m_rbB.getInvMass());
+			}
 		}
-	}
-
-	//calculate two perpendicular jointAxis, orthogonal to hingeAxis
-	//these two jointAxis require equal angular velocities for both bodies
-
-	//this is unused for now, it's a todo
-	btVector3 jointAxis0local;
-	btVector3 jointAxis1local;
-	
-	btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
-
-	getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
-	btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
-	btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
-	btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+
+		//calculate two perpendicular jointAxis, orthogonal to hingeAxis
+		//these two jointAxis require equal angular velocities for both bodies
+
+		//this is unused for now, it's a todo
+		btVector3 jointAxis0local;
+		btVector3 jointAxis1local;
 		
-	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());
-
-
+		btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
+
+		getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+		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();
+
+		//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));
+
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+void btHingeConstraint::getInfo1(btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
+	}
+	else
+	{
+		info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
+		info->nub = 1; 
+		//prepare constraint
+		testLimit();
+		if(getSolveLimit() || getEnableAngularMotor())
+		{
+			info->m_numConstraintRows++; // limit 3rd anguar as well
+			info->nub--; 
+		}
+	}
+} // btHingeConstraint::getInfo1 ()
+
+//-----------------------------------------------------------------------------
+
+void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	btAssert(!m_useSolveConstraintObsolete);
+	int i, s = info->rowskip;
+	// transforms in world space
+	btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
+	btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
+	// pivot point
+	btVector3 pivotAInW = trA.getOrigin();
+	btVector3 pivotBInW = trB.getOrigin();
+	// linear (all fixed)
+    info->m_J1linearAxis[0] = 1;
+    info->m_J1linearAxis[s + 1] = 1;
+    info->m_J1linearAxis[2 * s + 2] = 1;
+	btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
+		btVector3 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+	btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
+		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+	// linear RHS
+    btScalar k = info->fps * info->erp;
+	for(i = 0; i < 3; i++)
+    {
+        info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]);
+    }
+	// make rotations around X and Y equal
+	// the hinge axis should be the only unconstrained
+	// rotational axis, the angular velocity of the two bodies perpendicular to
+	// the hinge axis should be equal. thus the constraint equations are
+	//    p*w1 - p*w2 = 0
+	//    q*w1 - q*w2 = 0
+	// where p and q are unit vectors normal to the hinge axis, and w1 and w2
+	// are the angular velocity vectors of the two bodies.
+	// get hinge axis (Z)
+	btVector3 ax1 = trA.getBasis().getColumn(2);
+	// 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;
+
+	info->m_J1angularAxis[s3 + 0] = p[0];
+	info->m_J1angularAxis[s3 + 1] = p[1];
+	info->m_J1angularAxis[s3 + 2] = p[2];
+	info->m_J1angularAxis[s4 + 0] = q[0];
+	info->m_J1angularAxis[s4 + 1] = q[1];
+	info->m_J1angularAxis[s4 + 2] = q[2];
+
+	info->m_J2angularAxis[s3 + 0] = -p[0];
+	info->m_J2angularAxis[s3 + 1] = -p[1];
+	info->m_J2angularAxis[s3 + 2] = -p[2];
+	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);
+	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 srow;
+	btScalar limit_err = btScalar(0.0);
+	int limit = 0;
+	if(getSolveLimit())
+	{
+		limit_err = m_correction * m_referenceSign;
+		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) 
+	{
+		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_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))
+		{  // the joint motor is ineffective
+			powered = 0;
+		}
+		info->m_constraintError[srow] = btScalar(0.0f);
+		if(powered)
+		{
+            info->cfm[srow] = btScalar(0.0); 
+			btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
+			info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
+			info->m_lowerLimit[srow] = - m_maxMotorImpulse;
+			info->m_upperLimit[srow] =   m_maxMotorImpulse;
+		}
+		if(limit)
+		{
+			k = info->fps * info->erp;
+			info->m_constraintError[srow] += k * limit_err;
+			info->cfm[srow] = btScalar(0.0);
+			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
+				info->m_lowerLimit[srow] = 0;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			}
+			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 = m_relaxationFactor;
+			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)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc > info->m_constraintError[srow])
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+				else
+				{	// high limit - all those computations are reversed
+					if(vel > 0)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc < info->m_constraintError[srow])
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+			}
+			info->m_constraintError[srow] *= m_biasFactor;
+		} // if(limit)
+	} // if angular limit or powered
+}
+
+//-----------------------------------------------------------------------------
+
+void	btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
+{
+
+	///for backwards compatibility during the transition to 'getInfo/getInfo2'
+	if (m_useSolveConstraintObsolete)
+	{
+
+		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_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+			btVector3 vel1,vel2;
+			bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+			bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+			btVector3 vel = vel1 - vel2;
+
+			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;
+				m_appliedImpulse += impulse;
+				btVector3 impulse_vector = normal * impulse;
+				btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
+				btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
+				bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
+				bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
+			}
+		}
+
+		
+		{
+			///solve angular part
+
+			// get axes in world space
+			btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
+			btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
+
+			btVector3 angVelA;
+			bodyA.getAngularVelocity(angVelA);
+			btVector3 angVelB;
+			bodyB.getAngularVelocity(angVelB);
+
+			btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
+			btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
+
+			btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
+			btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
+			btVector3 velrelOrthog = angAorthog-angBorthog;
+			{
+				
+
+				//solve orthogonal angular velocity correction
+				btScalar relaxation = btScalar(1.);
+				btScalar len = velrelOrthog.length();
+				if (len > btScalar(0.00001))
+				{
+					btVector3 normal = velrelOrthog.normalized();
+					btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
+						getRigidBodyB().computeAngularImpulseDenominator(normal);
+					// scale for mass and relaxation
+					//velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
+
+					bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
+					bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));
+
+				}
+
+				//solve angular positional correction
+				btVector3 angularError =  axisA.cross(axisB) *(btScalar(1.)/timeStep);
+				btScalar len2 = angularError.length();
+				if (len2>btScalar(0.00001))
+				{
+					btVector3 normal2 = angularError.normalized();
+					btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
+							getRigidBodyB().computeAngularImpulseDenominator(normal2);
+					//angularError *= (btScalar(1.)/denom2) * relaxation;
+					
+					bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
+					bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));
+
+				}
+				
+				
+
+
+
+				// solve limit
+				if (m_solveLimit)
+				{
+					btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
+
+					btScalar impulseMag = amplitude * m_kHinge;
+
+					// Clamp the accumulated impulse
+					btScalar temp = m_accLimitImpulse;
+					m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
+					impulseMag = m_accLimitImpulse - temp;
+
+
+					
+					bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign);
+					bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign));
+
+				}
+			}
+
+			//apply motor
+			if (m_enableAngularMotor) 
+			{
+				//todo: add limits too
+				btVector3 angularLimit(0,0,0);
+
+				btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
+				btScalar projRelVel = velrel.dot(axisA);
+
+				btScalar desiredMotorVel = m_motorTargetVelocity;
+				btScalar motor_relvel = desiredMotorVel - projRelVel;
+
+				btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
+				//todo: should clip against accumulated impulse
+				btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
+				clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
+				btVector3 motorImp = clippedMotorImpulse * axisA;
+			
+				bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
+				bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
+				
+			}
+		}
+	}
+
+}
+
+//-----------------------------------------------------------------------------
+
+void	btHingeConstraint::updateRHS(btScalar	timeStep)
+{
+	(void)timeStep;
+
+}
+
+//-----------------------------------------------------------------------------
+
+btScalar btHingeConstraint::getHingeAngle()
+{
+	const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
+	const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
+	const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
+	btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+	return m_referenceSign * angle;
+}
+
+//-----------------------------------------------------------------------------
+
+void btHingeConstraint::testLimit()
+{
 	// Compute limit information
-	btScalar hingeAngle = getHingeAngle();  
-
-	//set bias, sign, clear accumulator
+	m_hingeAngle = getHingeAngle();  
 	m_correction = btScalar(0.);
 	m_limitSign = btScalar(0.);
 	m_solveLimit = false;
-	m_accLimitImpulse = btScalar(0.);
-
-//	if (m_lowerLimit < m_upperLimit)
 	if (m_lowerLimit <= m_upperLimit)
 	{
-//		if (hingeAngle <= m_lowerLimit*m_limitSoftness)
-		if (hingeAngle <= m_lowerLimit)
-		{
-			m_correction = (m_lowerLimit - hingeAngle);
+		if (m_hingeAngle <= m_lowerLimit)
+		{
+			m_correction = (m_lowerLimit - m_hingeAngle);
 			m_limitSign = 1.0f;
 			m_solveLimit = true;
 		} 
-//		else if (hingeAngle >= m_upperLimit*m_limitSoftness)
-		else if (hingeAngle >= m_upperLimit)
-		{
-			m_correction = m_upperLimit - hingeAngle;
+		else if (m_hingeAngle >= m_upperLimit)
+		{
+			m_correction = m_upperLimit - m_hingeAngle;
 			m_limitSign = -1.0f;
 			m_solveLimit = true;
 		}
 	}
-
-	//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));
-
-}
-
-void	btHingeConstraint::solveConstraint(btScalar	timeStep)
-{
-
-	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_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-
-		btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-		btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-		btVector3 vel = vel1 - vel2;
-
-		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;
-			m_appliedImpulse += impulse;
-			btVector3 impulse_vector = normal * impulse;
-			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
-			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
-		}
-	}
-
-	
-	{
-		///solve angular part
-
-		// get axes in world space
-		btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
-		btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
-
-		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
-		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
-
-		btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
-		btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
-
-		btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
-		btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
-		btVector3 velrelOrthog = angAorthog-angBorthog;
-		{
-			//solve orthogonal angular velocity correction
-			btScalar relaxation = btScalar(1.);
-			btScalar len = velrelOrthog.length();
-			if (len > btScalar(0.00001))
-			{
-				btVector3 normal = velrelOrthog.normalized();
-				btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
-					getRigidBodyB().computeAngularImpulseDenominator(normal);
-				// scale for mass and relaxation
-				velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
-			}
-
-			//solve angular positional correction
-			btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
-			btScalar len2 = angularError.length();
-			if (len2>btScalar(0.00001))
-			{
-				btVector3 normal2 = angularError.normalized();
-				btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
-						getRigidBodyB().computeAngularImpulseDenominator(normal2);
-				angularError *= (btScalar(1.)/denom2) * relaxation;
-			}
-
-			m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
-			m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
-
-			// solve limit
-			if (m_solveLimit)
-			{
-				btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
-
-				btScalar impulseMag = amplitude * m_kHinge;
-
-				// Clamp the accumulated impulse
-				btScalar temp = m_accLimitImpulse;
-				m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
-				impulseMag = m_accLimitImpulse - temp;
-
-
-				btVector3 impulse = axisA * impulseMag * m_limitSign;
-				m_rbA.applyTorqueImpulse(impulse);
-				m_rbB.applyTorqueImpulse(-impulse);
-			}
-		}
-
-		//apply motor
-		if (m_enableAngularMotor) 
-		{
-			//todo: add limits too
-			btVector3 angularLimit(0,0,0);
-
-			btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
-			btScalar projRelVel = velrel.dot(axisA);
-
-			btScalar desiredMotorVel = m_motorTargetVelocity;
-			btScalar motor_relvel = desiredMotorVel - projRelVel;
-
-			btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
-			//todo: should clip against accumulated impulse
-			btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
-			clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
-			btVector3 motorImp = clippedMotorImpulse * axisA;
-
-			m_rbA.applyTorqueImpulse(motorImp+angularLimit);
-			m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
-			
-		}
-	}
-
-}
-
-void	btHingeConstraint::updateRHS(btScalar	timeStep)
-{
-	(void)timeStep;
-
-}
-
-btScalar btHingeConstraint::getHingeAngle()
-{
-	const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
-	const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
-	const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
-
-	return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
-}
-
+	return;
+} // btHingeConstraint::testLimit()
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h	(revision 2882)
@@ -54,19 +54,23 @@
 
 	btScalar	m_accLimitImpulse;
+	btScalar	m_hingeAngle;
+	btScalar    m_referenceSign;
 
 	bool		m_angularOnly;
 	bool		m_enableAngularMotor;
 	bool		m_solveLimit;
+	bool		m_useSolveConstraintObsolete;
+	bool		m_useReferenceFrameA;
 
 	
 public:
 
-	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB);
+	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false);
 
-	btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA);
+	btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false);
 	
-	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
+	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
 
-	btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
+	btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
 
 	btHingeConstraint();
@@ -74,5 +78,9 @@
 	virtual void	buildJacobian();
 
-	virtual	void	solveConstraint(btScalar	timeStep);
+	virtual void getInfo1 (btConstraintInfo1* info);
+
+	virtual void getInfo2 (btConstraintInfo2* info);
+	
+	virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
 
 	void	updateRHS(btScalar	timeStep);
@@ -87,4 +95,14 @@
 	}
 
+	btRigidBody& getRigidBodyA()	
+	{		
+		return m_rbA;	
+	}	
+
+	btRigidBody& getRigidBodyB()	
+	{		
+		return m_rbB;	
+	}	
+	
 	void	setAngularOnly(bool angularOnly)
 	{
@@ -123,4 +141,6 @@
 	btScalar getHingeAngle();
 
+	void testLimit();
+
 
 	const btTransform& getAFrame() { return m_rbAFrame; };	
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp	(revision 2882)
@@ -22,10 +22,12 @@
 
 btPoint2PointConstraint::btPoint2PointConstraint()
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE)
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE),
+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)
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
+m_useSolveConstraintObsolete(false)
 {
 
@@ -34,5 +36,6 @@
 
 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+m_useSolveConstraintObsolete(false)
 {
 	
@@ -41,12 +44,14 @@
 void	btPoint2PointConstraint::buildJacobian()
 {
-	m_appliedImpulse = btScalar(0.);
-
-	btVector3	normal(0,0,0);
-
-	for (int i=0;i<3;i++)
-	{
-		normal[i] = 1;
-		new (&m_jac[i]) btJacobianEntry(
+	///we need it for both methods
+	{
+		m_appliedImpulse = btScalar(0.);
+
+		btVector3	normal(0,0,0);
+
+		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(),
@@ -59,62 +64,160 @@
 			m_rbB.getInvMass());
 		normal[i] = 0;
-	}
-
-}
-
-void	btPoint2PointConstraint::solveConstraint(btScalar	timeStep)
-{
-	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
-	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
-
-
-	btVector3 normal(0,0,0);
-	
-
-//	btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-//	btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-
-	for (int i=0;i<3;i++)
-	{		
-		normal[i] = 1;
-		btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
-
-		btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-		btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-		//this jacobian entry could be re-used for all iterations
-		
-		btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-		btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-		btVector3 vel = vel1 - vel2;
-		
-		btScalar rel_vel;
-		rel_vel = normal.dot(vel);
-
-	/*
-		//velocity error (first order error)
-		btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
-														m_rbB.getLinearVelocity(),angvelB);
+		}
+	}
+
+}
+
+
+void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
+	} else
+	{
+		info->m_numConstraintRows = 3;
+		info->nub = 3;
+	}
+}
+
+void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	btAssert(!m_useSolveConstraintObsolete);
+
+	 //retrieve matrices
+	btTransform body0_trans;
+	body0_trans = m_rbA.getCenterOfMassTransform();
+    btTransform body1_trans;
+	body1_trans = m_rbB.getCenterOfMassTransform();
+
+	// 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();
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+		btVector3 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+    
+	/*info->m_J2linearAxis[0] = -1;
+    info->m_J2linearAxis[s+1] = -1;
+    info->m_J2linearAxis[2*s+2] = -1;
 	*/
 	
-		//positional error (zeroth order error)
-		btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+	btVector3 a2 = body1_trans.getBasis()*getPivotInB();
+   
+	{
+		btVector3 a2n = -a2;
+		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+    
+
+
+    // set right hand side
+    btScalar k = info->fps * info->erp;
+    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]);
+    }
+
+	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;
+		}
+	}
+	
+}
+
+
+void	btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
+		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
+
+
+		btVector3 normal(0,0,0);
 		
-		btScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
-
-		btScalar impulseClamp = m_setting.m_impulseClamp;
-		if (impulseClamp > 0)
-		{
-			if (impulse < -impulseClamp) 
-				impulse = -impulseClamp;
-			if (impulse > impulseClamp) 
-				impulse = impulseClamp;
+
+	//	btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+	//	btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+
+		for (int i=0;i<3;i++)
+		{		
+			normal[i] = 1;
+			btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+
+			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+			btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+			//this jacobian entry could be re-used for all iterations
+			
+			btVector3 vel1,vel2;
+			bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+			bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+			btVector3 vel = vel1 - vel2;
+			
+			btScalar rel_vel;
+			rel_vel = normal.dot(vel);
+
+		/*
+			//velocity error (first order error)
+			btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
+															m_rbB.getLinearVelocity(),angvelB);
+		*/
+		
+			//positional error (zeroth order error)
+			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+			
+			btScalar deltaImpulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
+
+			btScalar impulseClamp = m_setting.m_impulseClamp;
+			
+			const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse;
+			if (sum < -impulseClamp)
+			{
+				deltaImpulse = -impulseClamp-m_appliedImpulse;
+				m_appliedImpulse = -impulseClamp;
+			}
+			else if (sum > impulseClamp) 
+			{
+				deltaImpulse = impulseClamp-m_appliedImpulse;
+				m_appliedImpulse = impulseClamp;
+			}
+			else
+			{
+				m_appliedImpulse = sum;
+			}
+
+			
+			btVector3 impulse_vector = normal * deltaImpulse;
+			
+			btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
+			btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
+			bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse);
+			bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse);
+
+
+			normal[i] = 0;
 		}
-
-		m_appliedImpulse+=impulse;
-		btVector3 impulse_vector = normal * impulse;
-		m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
-		m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
-		
-		normal[i] = 0;
 	}
 }
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h	(revision 2882)
@@ -51,4 +51,7 @@
 public:
 
+	///for backwards compatibility during the transition to 'getInfo/getInfo2'
+	bool		m_useSolveConstraintObsolete;
+
 	btConstraintSetting	m_setting;
 
@@ -61,6 +64,10 @@
 	virtual void	buildJacobian();
 
+	virtual void getInfo1 (btConstraintInfo1* info);
 
-	virtual	void	solveConstraint(btScalar	timeStep);
+	virtual void getInfo2 (btConstraintInfo2* info);
+
+
+	virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
 
 	void	updateRHS(btScalar	timeStep);
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp	(revision 2882)
@@ -16,5 +16,4 @@
 //#define COMPUTE_IMPULSE_DENOM 1
 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
-//#define FORCE_REFESH_CONTACT_MANIFOLDS 1
 
 #include "btSequentialImpulseConstraintSolver.h"
@@ -33,29 +32,154 @@
 #include "btSolverBody.h"
 #include "btSolverConstraint.h"
-
-
 #include "LinearMath/btAlignedObjectArray.h"
-
-
-int totalCpd = 0;
-
-int	gTotalContactPoints = 0;
-
-struct	btOrderIndex
-{
-	int	m_manifoldIndex;
-	int	m_pointIndex;
-};
-
-
-
-#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
-static btOrderIndex	gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
+#include <string.h> //for memset
+
+btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
+:m_btSeed2(0)
+{
+
+}
+
+btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
+{
+}
+
+#ifdef USE_SIMD
+#include <emmintrin.h>
+#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
+static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
+{
+	__m128 result = _mm_mul_ps( vec0, vec1);
+	return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
+}
+#endif//USE_SIMD
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
+	__m128	lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
+	__m128 deltaVel1Dotn	=	_mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
+	__m128 deltaVel2Dotn	=	_mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
+	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+	btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
+	btSimdScalar resultLowerLess,resultUpperLess;
+	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
+	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
+	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
+	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
+	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
+	__m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
+	deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
+	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
+	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
+	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+	__m128 impulseMagnitude = deltaImpulse;
+	body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+	body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+	body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+	body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+#else
+	resolveSingleConstraintRowGeneric(body1,body2,c);
+#endif
+}
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
+	const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.m_deltaLinearVelocity) 	+ c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
+	const btScalar deltaVel2Dotn	=	-c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
+
+	const btScalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
+	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
+	deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;
+
+	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
+	if (sum < c.m_lowerLimit)
+	{
+		deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+		c.m_appliedImpulse = c.m_lowerLimit;
+	}
+	else if (sum > c.m_upperLimit) 
+	{
+		deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
+		c.m_appliedImpulse = c.m_upperLimit;
+	}
+	else
+	{
+		c.m_appliedImpulse = sum;
+	}
+	if (body1.m_invMass)
+		body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
+	if (body2.m_invMass)
+		body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
+}
+
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
+	__m128	lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
+	__m128 deltaVel1Dotn	=	_mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
+	__m128 deltaVel2Dotn	=	_mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
+	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+	btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
+	btSimdScalar resultLowerLess,resultUpperLess;
+	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
+	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
+	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
+	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
+	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
+	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
+	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+	__m128 impulseMagnitude = deltaImpulse;
+	body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+	body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+	body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+	body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+#else
+	resolveSingleConstraintRowLowerLimit(body1,body2,c);
+#endif
+}
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
+	const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.m_deltaLinearVelocity) 	+ c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
+	const btScalar deltaVel2Dotn	=	-c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
+
+	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
+	deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;
+	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
+	if (sum < c.m_lowerLimit)
+	{
+		deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+		c.m_appliedImpulse = c.m_lowerLimit;
+	}
+	else
+	{
+		c.m_appliedImpulse = sum;
+	}
+	if (body1.m_invMass)
+		body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
+	if (body2.m_invMass)
+		body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
+}
+
 
 
 unsigned long btSequentialImpulseConstraintSolver::btRand2()
 {
-  m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
-  return m_btSeed2;
+	m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
+	return m_btSeed2;
 }
 
@@ -65,91 +189,49 @@
 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
 {
-  // seems good; xor-fold and modulus
-  const unsigned long un = static_cast<unsigned long>(n);
-  unsigned long r = btRand2();
-
-  // 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) {
-    r ^= (r >> 16);
-    if (un <= 0x00000100UL) {
-      r ^= (r >> 8);
-      if (un <= 0x00000010UL) {
-        r ^= (r >> 4);
-        if (un <= 0x00000004UL) {
-          r ^= (r >> 2);
-          if (un <= 0x00000002UL) {
-            r ^= (r >> 1);
-          }
-        }
-     }
-    }
-   }
-
-  return (int) (r % un);
-}
-
-
-
-
-bool  MyContactDestroyedCallback(void* userPersistentData);
-bool  MyContactDestroyedCallback(void* userPersistentData)
-{
-	assert (userPersistentData);
-	btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
-	btAlignedFree(cpd);
-	totalCpd--;
-	//printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
-	return true;
-}
-
-
-
-btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
-:m_btSeed2(0)
-{
-	gContactDestroyedCallback = &MyContactDestroyedCallback;
-
-	//initialize default friction/contact funcs
-	int i,j;
-	for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
-		for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
-		{
-
-			m_contactDispatch[i][j] = resolveSingleCollision;
-			m_frictionDispatch[i][j] = resolveSingleFriction;
-		}
-}
-
-btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
-{
-
-}
-
-void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
-void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
-{
-	btRigidBody* rb = btRigidBody::upcast(collisionObject);
+	// seems good; xor-fold and modulus
+	const unsigned long un = static_cast<unsigned long>(n);
+	unsigned long r = btRand2();
+
+	// 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) {
+		r ^= (r >> 16);
+		if (un <= 0x00000100UL) {
+			r ^= (r >> 8);
+			if (un <= 0x00000010UL) {
+				r ^= (r >> 4);
+				if (un <= 0x00000004UL) {
+					r ^= (r >> 2);
+					if (un <= 0x00000002UL) {
+						r ^= (r >> 1);
+					}
+				}
+			}
+		}
+	}
+
+	return (int) (r % un);
+}
+
+
+
+void	btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
+{
+	btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
+
+	solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
+	solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+
 	if (rb)
 	{
-		solverBody->m_angularVelocity = rb->getAngularVelocity() ;
-		solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
-		solverBody->m_friction = collisionObject->getFriction();
 		solverBody->m_invMass = rb->getInvMass();
-		solverBody->m_linearVelocity = rb->getLinearVelocity();
 		solverBody->m_originalBody = rb;
 		solverBody->m_angularFactor = rb->getAngularFactor();
 	} else
 	{
-		solverBody->m_angularVelocity.setValue(0,0,0);
-		solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
-		solverBody->m_friction = collisionObject->getFriction();
 		solverBody->m_invMass = 0.f;
-		solverBody->m_linearVelocity.setValue(0,0,0);
 		solverBody->m_originalBody = 0;
 		solverBody->m_angularFactor = 1.f;
 	}
-	solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
-	solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
 }
 
@@ -157,6 +239,5 @@
 int		gNumSplitImpulseRecoveries = 0;
 
-btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
-btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
+btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
 {
 	btScalar rest = restitution * -rel_vel;
@@ -165,274 +246,19 @@
 
 
-void	resolveSplitPenetrationImpulseCacheFriendly(
-        btSolverBody& body1,
-        btSolverBody& body2,
-        const btSolverConstraint& contactConstraint,
-        const btContactSolverInfo& solverInfo);
-
-//SIMD_FORCE_INLINE
-void	resolveSplitPenetrationImpulseCacheFriendly(
-        btSolverBody& body1,
-        btSolverBody& body2,
-        const btSolverConstraint& contactConstraint,
-        const btContactSolverInfo& solverInfo)
-{
-        (void)solverInfo;
-
-		if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold)
-        {
-
-				gNumSplitImpulseRecoveries++;
-                btScalar normalImpulse;
-
-                //  Optimized version of projected relative velocity, use precomputed cross products with normal
-                //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
-                //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
-                //      btVector3 vel = vel1 - vel2;
-                //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
-
-                btScalar rel_vel;
-                btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
-                + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
-                btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
-                + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
-
-                rel_vel = vel1Dotn-vel2Dotn;
-
-
-				btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep;
-                //      btScalar positionalError = contactConstraint.m_penetration;
-
-                btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
-
-                btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
-                btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
-                normalImpulse = penetrationImpulse+velocityImpulse;
-
-                // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-                btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
-                btScalar sum = oldNormalImpulse + normalImpulse;
-                contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-
-                normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
-
-				body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
-               
-				body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
-               
-        }
-
-}
-
-
-//velocity + friction
-//response  between two dynamic objects with friction
-
-btScalar resolveSingleCollisionCombinedCacheFriendly(
-	btSolverBody& body1,
-	btSolverBody& body2,
-	const btSolverConstraint& contactConstraint,
-	const btContactSolverInfo& solverInfo);
-
-//SIMD_FORCE_INLINE 
-btScalar resolveSingleCollisionCombinedCacheFriendly(
-	btSolverBody& body1,
-	btSolverBody& body2,
-	const btSolverConstraint& contactConstraint,
-	const btContactSolverInfo& solverInfo)
-{
-	(void)solverInfo;
-
-	btScalar normalImpulse;
-
-	{
-
-		
-		//  Optimized version of projected relative velocity, use precomputed cross products with normal
-		//	body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
-		//	body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
-		//	btVector3 vel = vel1 - vel2;
-		//	btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
-
-		btScalar rel_vel;
-		btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
-					+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
-		btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
-					+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
-
-		rel_vel = vel1Dotn-vel2Dotn;
-
-		btScalar positionalError = 0.f;
-		if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold))
-		{
- 			positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep;
-		}
-
-		btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
-
-		btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
-		btScalar	velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
-		normalImpulse = penetrationImpulse+velocityImpulse;
-		
-		
-		// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-		btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
-		btScalar sum = oldNormalImpulse + normalImpulse;
-		contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-
-		normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
-
-		body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
-				contactConstraint.m_angularComponentA,normalImpulse);
-		
-		body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
-				contactConstraint.m_angularComponentB,-normalImpulse);
-	}
-
-	return normalImpulse;
-}
-
-
-#ifndef NO_FRICTION_TANGENTIALS
-
-btScalar resolveSingleFrictionCacheFriendly(
-	btSolverBody& body1,
-	btSolverBody& body2,
-	const btSolverConstraint& contactConstraint,
-	const btContactSolverInfo& solverInfo,
-	btScalar appliedNormalImpulse);
-
-//SIMD_FORCE_INLINE 
-btScalar resolveSingleFrictionCacheFriendly(
-	btSolverBody& body1,
-	btSolverBody& body2,
-	const btSolverConstraint& contactConstraint,
-	const btContactSolverInfo& solverInfo,
-	btScalar appliedNormalImpulse)
-{
-	(void)solverInfo;
-
-	
-	const btScalar combinedFriction = contactConstraint.m_friction;
-	
-	const btScalar limit = appliedNormalImpulse * combinedFriction;
-	
-	if (appliedNormalImpulse>btScalar(0.))
-	//friction
-	{
-		
-		btScalar j1;
-		{
-
-			btScalar rel_vel;
-			const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
-						+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
-			const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
-				+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
-			rel_vel = vel1Dotn-vel2Dotn;
-
-			// calculate j that moves us to zero relative velocity
-			j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
-#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
-#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
-			btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
-			contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
-			
-			if (limit < contactConstraint.m_appliedImpulse)
-			{
-				contactConstraint.m_appliedImpulse = limit;
-			} else
-			{
-				if (contactConstraint.m_appliedImpulse < -limit)
-					contactConstraint.m_appliedImpulse = -limit;
-			}
-			j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
-#else
-			if (limit < j1)
-			{
-				j1 = limit;
-			} else
-			{
-				if (j1 < -limit)
-					j1 = -limit;
-			}
-
-#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
-
-			//GEN_set_min(contactConstraint.m_appliedImpulse, limit);
-			//GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
-
-			
-
-		}
-	
-		body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
-		
-		body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
-
-	} 
-	return 0.f;
-}
-
-
-#else
-
-//velocity + friction
-//response  between two dynamic objects with friction
-btScalar resolveSingleFrictionCacheFriendly(
-	btSolverBody& body1,
-	btSolverBody& body2,
-	btSolverConstraint& contactConstraint,
-	const btContactSolverInfo& solverInfo)
-{
-
-	btVector3 vel1;
-	btVector3 vel2;
-	btScalar normalImpulse(0.f);
-
-	{
-		const btVector3& normal = contactConstraint.m_contactNormal;
-		if (contactConstraint.m_penetration < 0.f)
-			return 0.f;
-
-
-		body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
-		body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
-		btVector3 vel = vel1 - vel2;
-		btScalar rel_vel;
-		rel_vel = normal.dot(vel);
-
-		btVector3 lat_vel = vel - normal * rel_vel;
-		btScalar lat_rel_vel = lat_vel.length2();
-
-		btScalar combinedFriction = contactConstraint.m_friction;
-		const btVector3& rel_pos1 = contactConstraint.m_rel_posA;
-		const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
-
-
-		if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
-		{
-			lat_rel_vel = btSqrt(lat_rel_vel);
-
-			lat_vel /= lat_rel_vel;
-			btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel);
-			btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
-			btScalar friction_impulse = lat_rel_vel /
-				(body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
-			btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
-
-			GEN_set_min(friction_impulse, normal_impulse);
-			GEN_set_max(friction_impulse, -normal_impulse);
-			body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
-			body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
-		}
-	}
-
-	return normalImpulse;
-}
-
-#endif //NO_FRICTION_TANGENTIALS
-
-
+
+void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
+void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
+{
+	if (colObj && colObj->hasAnisotropicFriction())
+	{
+		// transform to local coordinates
+		btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
+		const btVector3& friction_scaling = colObj->getAnisotropicFriction();
+		//apply anisotropic friction
+		loc_lateral *= friction_scaling;
+		// ... and transform it back to global coordinates
+		frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
+	}
+}
 
 
@@ -440,14 +266,15 @@
 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)
 {
+
 
 	btRigidBody* body0=btRigidBody::upcast(colObj0);
 	btRigidBody* body1=btRigidBody::upcast(colObj1);
 
-	btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
+	btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
+	memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
 	solverConstraint.m_contactNormal = normalAxis;
 
 	solverConstraint.m_solverBodyIdA = solverBodyIdA;
 	solverConstraint.m_solverBodyIdB = solverBodyIdB;
-	solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
 	solverConstraint.m_frictionIndex = frictionIndex;
 
@@ -455,16 +282,16 @@
 	solverConstraint.m_originalContactPoint = 0;
 
-	solverConstraint.m_appliedImpulse = btScalar(0.);
-	solverConstraint.m_appliedPushImpulse = 0.f;
-	solverConstraint.m_penetration = 0.f;
+	solverConstraint.m_appliedImpulse = 0.f;
+	//	solverConstraint.m_appliedPushImpulse = 0.f;
+
 	{
 		btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
 		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
-	}
-	{
-		btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
+		solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
+	}
+	{
+		btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
 		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
+		solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
 	}
 
@@ -483,5 +310,5 @@
 	if (body1)
 	{
-		vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
+		vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
 		denom1 = body1->getInvMass() + normalAxis.dot(vec);
 	}
@@ -492,7 +319,314 @@
 	solverConstraint.m_jacDiagABInv = denom;
 
+#ifdef _USE_JACOBIAN
+	solverConstraint.m_jac =  btJacobianEntry (
+		rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
+		body0->getInvInertiaDiagLocal(),
+		body0->getInvMass(),
+		body1->getInvInertiaDiagLocal(),
+		body1->getInvMass());
+#endif //_USE_JACOBIAN
+
+
+	{
+		btScalar rel_vel;
+		btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) 
+			+ solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
+		btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) 
+			+ solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
+
+		rel_vel = vel1Dotn+vel2Dotn;
+
+		btScalar positionalError = 0.f;
+
+		btSimdScalar velocityError =  - rel_vel;
+		btSimdScalar	velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
+		solverConstraint.m_rhs = velocityImpulse;
+		solverConstraint.m_cfm = 0.f;
+		solverConstraint.m_lowerLimit = 0;
+		solverConstraint.m_upperLimit = 1e10f;
+	}
+
 	return solverConstraint;
 }
 
+int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
+{
+	int solverBodyIdA = -1;
+
+	if (body.getCompanionId() >= 0)
+	{
+		//body has already been converted
+		solverBodyIdA = body.getCompanionId();
+	} else
+	{
+		btRigidBody* rb = btRigidBody::upcast(&body);
+		if (rb && rb->getInvMass())
+		{
+			solverBodyIdA = m_tmpSolverBodyPool.size();
+			btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(&solverBody,&body);
+			body.setCompanionId(solverBodyIdA);
+		} else
+		{
+			return 0;//assume first one is a fixed solver body
+		}
+	}
+	return solverBodyIdA;
+}
+#include <stdio.h>
+
+
+
+void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
+{
+	btCollisionObject* colObj0=0,*colObj1=0;
+
+	colObj0 = (btCollisionObject*)manifold->getBody0();
+	colObj1 = (btCollisionObject*)manifold->getBody1();
+
+	int solverBodyIdA=-1;
+	int solverBodyIdB=-1;
+
+	if (manifold->getNumContacts())
+	{
+		solverBodyIdA = getOrInitSolverBody(*colObj0);
+		solverBodyIdB = getOrInitSolverBody(*colObj1);
+	}
+
+	///avoid collision response between two static objects
+	if (!solverBodyIdA && !solverBodyIdB)
+		return;
+
+	btVector3 rel_pos1;
+	btVector3 rel_pos2;
+	btScalar relaxation;
+
+	for (int j=0;j<manifold->getNumContacts();j++)
+	{
+
+		btManifoldPoint& cp = manifold->getContactPoint(j);
+
+		if (cp.getDistance() <= manifold->getContactProcessingThreshold())
+		{
+
+			const btVector3& pos1 = cp.getPositionWorldOnA();
+			const btVector3& pos2 = cp.getPositionWorldOnB();
+
+			rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
+			rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
+
+
+			relaxation = 1.f;
+			btScalar rel_vel;
+			btVector3 vel;
+
+			int frictionIndex = m_tmpSolverContactConstraintPool.size();
+
+			{
+				btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
+				btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+				btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+
+				solverConstraint.m_solverBodyIdA = solverBodyIdA;
+				solverConstraint.m_solverBodyIdB = solverBodyIdB;
+
+				solverConstraint.m_originalContactPoint = &cp;
+
+				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;
+				}
+
+				solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
+				solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
+				solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
+
+
+				btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
+				btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+
+				vel  = vel1 - vel2;
+
+				rel_vel = cp.m_normalWorldOnB.dot(vel);
+
+				btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
+
+
+				solverConstraint.m_friction = cp.m_combinedFriction;
+
+				btScalar restitution = 0.f;
+				
+				if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
+				{
+					restitution = 0.f;
+				} else
+				{
+					restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
+					if (restitution <= btScalar(0.))
+					{
+						restitution = 0.f;
+					};
+				}
+
+
+				///warm starting (or zero if disabled)
+				if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+				{
+					solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+					if (rb0)
+						m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+					if (rb1)
+						m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
+				} else
+				{
+					solverConstraint.m_appliedImpulse = 0.f;
+				}
+
+				//							solverConstraint.m_appliedPushImpulse = 0.f;
+
+				{
+					btScalar rel_vel;
+					btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) 
+						+ solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
+					btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) 
+						+ solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
+
+					rel_vel = vel1Dotn+vel2Dotn;
+
+					btScalar positionalError = 0.f;
+					positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
+					btScalar	velocityError = restitution - rel_vel;// * damping;
+					btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+					btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+					solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+					solverConstraint.m_cfm = 0.f;
+					solverConstraint.m_lowerLimit = 0;
+					solverConstraint.m_upperLimit = 1e10f;
+				}
+
+
+				/////setup the friction constraints
+
+
+
+				if (1)
+				{
+					solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
+					if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+					{
+						cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+						btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+						if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
+						{
+							cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
+							applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+							applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+							addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+							if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+							{
+								cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+								cp.m_lateralFrictionDir2.normalize();//??
+								applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+								applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+								addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+							}
+							cp.m_lateralFrictionInitialized = true;
+						} else
+						{
+							//re-calculate friction direction every frame, todo: check if this is really needed
+							btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+							applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+							applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+
+							addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+							if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+							{
+								applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+								applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+								addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+							}
+							cp.m_lateralFrictionInitialized = true;
+						}
+
+					} else
+					{
+						addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+						if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+							addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					}
+
+					if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
+					{
+						{
+							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)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
+								if (rb1)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
+							} else
+							{
+								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)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
+								if (rb1)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
+							} else
+							{
+								frictionConstraint2.m_appliedImpulse = 0.f;
+							}
+						}
+					} else
+					{
+						btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
+						frictionConstraint1.m_appliedImpulse = 0.f;
+						if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+						{
+							btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+							frictionConstraint2.m_appliedImpulse = 0.f;
+						}
+					}
+				}
+			}
+
+
+		}
+	}
+}
 
 
@@ -506,335 +640,9 @@
 	if (!(numConstraints + numManifolds))
 	{
-//		printf("empty\n");
+		//		printf("empty\n");
 		return 0.f;
 	}
-	btPersistentManifold* manifold = 0;
-	btCollisionObject* colObj0=0,*colObj1=0;
-
-	//btRigidBody* rb0=0,*rb1=0;
-
-
-#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
-
-	BEGIN_PROFILE("refreshManifolds");
-
-	int i;
-	
-	
-
-	for (i=0;i<numManifolds;i++)
-	{
-		manifold = manifoldPtr[i];
-		rb1 = (btRigidBody*)manifold->getBody1();
-		rb0 = (btRigidBody*)manifold->getBody0();
-		
-		manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
-
-	}
-
-	END_PROFILE("refreshManifolds");
-#endif //FORCE_REFESH_CONTACT_MANIFOLDS
-
-	
-
-
-
-	//int sizeofSB = sizeof(btSolverBody);
-	//int sizeofSC = sizeof(btSolverConstraint);
-
-
-	//if (1)
-	{
-		//if m_stackAlloc, try to pack bodies/constraints to speed up solving
-//		btBlock*					sablock;
-//		sablock = stackAlloc->beginBlock();
-
-	//	int memsize = 16;
-//		unsigned char* stackMemory = stackAlloc->allocate(memsize);
-
-		
-		//todo: use stack allocator for this temp memory
-//		int minReservation = numManifolds*2;
-
-		//m_tmpSolverBodyPool.reserve(minReservation);
-
-		//don't convert all bodies, only the one we need so solver the constraints
-/*
-		{
-			for (int i=0;i<numBodies;i++)
-			{
-				btRigidBody* rb = btRigidBody::upcast(bodies[i]);
-				if (rb && 	(rb->getIslandTag() >= 0))
-				{
-					btAssert(rb->getCompanionId() < 0);
-					int solverBodyId = m_tmpSolverBodyPool.size();
-					btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-					initSolverBody(&solverBody,rb);
-					rb->setCompanionId(solverBodyId);
-				} 
-			}
-		}
-*/
-		
-		//m_tmpSolverConstraintPool.reserve(minReservation);
-		//m_tmpSolverFrictionConstraintPool.reserve(minReservation);
-
-		{
-			int i;
-
-			for (i=0;i<numManifolds;i++)
-			{
-				manifold = manifoldPtr[i];
-				colObj0 = (btCollisionObject*)manifold->getBody0();
-				colObj1 = (btCollisionObject*)manifold->getBody1();
-			
-				int solverBodyIdA=-1;
-				int solverBodyIdB=-1;
-
-				if (manifold->getNumContacts())
-				{
-
-					
-
-					if (colObj0->getIslandTag() >= 0)
-					{
-						if (colObj0->getCompanionId() >= 0)
-						{
-							//body has already been converted
-							solverBodyIdA = colObj0->getCompanionId();
-						} else
-						{
-							solverBodyIdA = m_tmpSolverBodyPool.size();
-							btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-							initSolverBody(&solverBody,colObj0);
-							colObj0->setCompanionId(solverBodyIdA);
-						}
-					} else
-					{
-						//create a static body
-						solverBodyIdA = m_tmpSolverBodyPool.size();
-						btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-						initSolverBody(&solverBody,colObj0);
-					}
-
-					if (colObj1->getIslandTag() >= 0)
-					{
-						if (colObj1->getCompanionId() >= 0)
-						{
-							solverBodyIdB = colObj1->getCompanionId();
-						} else
-						{
-							solverBodyIdB = m_tmpSolverBodyPool.size();
-							btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-							initSolverBody(&solverBody,colObj1);
-							colObj1->setCompanionId(solverBodyIdB);
-						}
-					} else
-					{
-						//create a static body
-						solverBodyIdB = m_tmpSolverBodyPool.size();
-						btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-						initSolverBody(&solverBody,colObj1);
-					}
-				}
-
-				btVector3 rel_pos1;
-				btVector3 rel_pos2;
-				btScalar relaxation;
-
-				for (int j=0;j<manifold->getNumContacts();j++)
-				{
-					
-					btManifoldPoint& cp = manifold->getContactPoint(j);
-					
-					if (cp.getDistance() <= btScalar(0.))
-					{
-						
-						const btVector3& pos1 = cp.getPositionWorldOnA();
-						const btVector3& pos2 = cp.getPositionWorldOnB();
-
-						 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
-						 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
-
-						
-						relaxation = 1.f;
-						btScalar rel_vel;
-						btVector3 vel;
-
-						int frictionIndex = m_tmpSolverConstraintPool.size();
-
-						{
-							btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
-							btRigidBody* rb0 = btRigidBody::upcast(colObj0);
-							btRigidBody* rb1 = btRigidBody::upcast(colObj1);
-
-							solverConstraint.m_solverBodyIdA = solverBodyIdA;
-							solverConstraint.m_solverBodyIdB = solverBodyIdB;
-							solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
-
-							solverConstraint.m_originalContactPoint = &cp;
-
-							btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
-							solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
-							btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
-							solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : 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;
-							}
-
-							solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
-							solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
-							solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
-
-
-							btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
-							btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
-				
-							vel  = vel1 - vel2;
-							
-							rel_vel = cp.m_normalWorldOnB.dot(vel);
-							
-							solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.));
-							//solverConstraint.m_penetration = cp.getDistance();
-
-							solverConstraint.m_friction = cp.m_combinedFriction;
-
-							
-							if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
-							{
-								solverConstraint.m_restitution = 0.f;
-							} else
-							{
-								solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
-								if (solverConstraint.m_restitution <= btScalar(0.))
-								{
-									solverConstraint.m_restitution = 0.f;
-								};
-							}
-
-							
-							btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
-
-							
-
-							if (solverConstraint.m_restitution > penVel)
-							{
-								solverConstraint.m_penetration = btScalar(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)
-									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-								if (rb1)
-									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
-							} else
-							{
-								solverConstraint.m_appliedImpulse = 0.f;
-							}
-
-							solverConstraint.m_appliedPushImpulse = 0.f;
-							
-							solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
-							if (!cp.m_lateralFrictionInitialized)
-							{
-								cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
-								btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
-								if (lat_rel_vel > SIMD_EPSILON)//0.0f)
-								{
-									cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
-									addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-									if(infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
-									{
-										cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
-										cp.m_lateralFrictionDir2.normalize();//??
-										addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-										cp.m_lateralFrictionInitialized = true;
-									}
-								} else
-								{
-									//re-calculate friction direction every frame, todo: check if this is really needed
-									btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
-									addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-									addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-									if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
-									{
-										cp.m_lateralFrictionInitialized = true;
-									}
-								}
-								
-							} else
-							{
-								addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-								if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
-									addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-							}
-
-							if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
-							{
-								{
-									btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
-									if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-									{
-										frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
-										if (rb0)
-											m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
-										if (rb1)
-											m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
-									} else
-									{
-										frictionConstraint1.m_appliedImpulse = 0.f;
-									}
-								}
-								{
-									btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
-									if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-									{
-										frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
-										if (rb0)
-											m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
-										if (rb1)
-											m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
-									} else
-									{
-										frictionConstraint2.m_appliedImpulse = 0.f;
-									}
-								}
-							}
-						}
-
-
-					}
-				}
-			}
-		}
-	}
-	
-	btContactSolverInfo info = infoGlobal;
-
+
+	if (1)
 	{
 		int j;
@@ -845,9 +653,161 @@
 		}
 	}
-	
-	
-
-	int numConstraintPool = m_tmpSolverConstraintPool.size();
-	int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
+
+	btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+	initSolverBody(&fixedBody,0);
+
+	//btRigidBody* rb0=0,*rb1=0;
+
+	//if (1)
+	{
+		{
+
+			int totalNumRows = 0;
+			int i;
+			//calculate the total number of contraint rows
+			for (i=0;i<numConstraints;i++)
+			{
+
+				btTypedConstraint::btConstraintInfo1 info1;
+				constraints[i]->getInfo1(&info1);
+				totalNumRows += info1.m_numConstraintRows;
+			}
+			m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
+
+			btTypedConstraint::btConstraintInfo1 info1;
+			info1.m_numConstraintRows = 0;
+
+
+			///setup the btSolverConstraints
+			int currentRow = 0;
+
+			for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
+			{
+				constraints[i]->getInfo1(&info1);
+				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);
+					int solverBodyIdB = getOrInitSolverBody(rbB);
+
+					btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
+					btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
+
+					int j;
+					for ( j=0;j<info1.m_numConstraintRows;j++)
+					{
+						memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
+						currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
+						currentConstraintRow[j].m_upperLimit = FLT_MAX;
+						currentConstraintRow[j].m_appliedImpulse = 0.f;
+						currentConstraintRow[j].m_appliedPushImpulse = 0.f;
+						currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
+						currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
+					}
+
+					bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
+					bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+					bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
+					bodyBPtr->m_deltaAngularVelocity.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_contactNormal;
+					info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
+					info2.m_J2linearAxis = 0;
+					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;
+					info2.cfm = &currentConstraintRow->m_cfm;
+					info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
+					info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
+					constraints[i]->getInfo2(&info2);
+
+					///finalize the constraint setup
+					for ( j=0;j<info1.m_numConstraintRows;j++)
+					{
+						btSolverConstraint& solverConstraint = currentConstraintRow[j];
+
+						{
+							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_contactNormal*rbA.getInvMass();
+							btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
+							btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
+							btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
+
+							btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
+							sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
+							sum += iMJlB.dot(solverConstraint.m_contactNormal);
+							sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
+
+							solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
+						}
+
+
+						///fix rhs
+						///todo: add force/torque accelerators
+						{
+							btScalar rel_vel;
+							btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
+							btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
+
+							rel_vel = vel1Dotn+vel2Dotn;
+
+							btScalar restitution = 0.f;
+							btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
+							btScalar	velocityError = restitution - rel_vel;// * damping;
+							btScalar	penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+							btScalar	velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+							solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+							solverConstraint.m_appliedImpulse = 0.f;
+
+						}
+					}
+				}
+			}
+		}
+
+		{
+			int i;
+			btPersistentManifold* manifold = 0;
+			btCollisionObject* colObj0=0,*colObj1=0;
+
+
+			for (i=0;i<numManifolds;i++)
+			{
+				manifold = manifoldPtr[i];
+				convertContact(manifold,infoGlobal);
+			}
+		}
+	}
+
+	btContactSolverInfo info = infoGlobal;
+
+
+
+	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
 
 	///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
@@ -866,6 +826,4 @@
 	}
 
-
-
 	return 0.f;
 
@@ -875,6 +833,7 @@
 {
 	BT_PROFILE("solveGroupCacheFriendlyIterations");
-	int numConstraintPool = m_tmpSolverConstraintPool.size();
-	int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
+
+	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
 
 	//should traverse the contacts random order...
@@ -904,89 +863,110 @@
 			}
 
-			for (j=0;j<numConstraints;j++)
+			if (infoGlobal.m_solverMode & SOLVER_SIMD)
 			{
-				btTypedConstraint* constraint = constraints[j];
-				///todo: use solver bodies, so we don't need to copy from/to btRigidBody
-
-				if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
-				{
-					m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
-				}
-				if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
-				{
-					m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
-				}
-
-				constraint->solveConstraint(infoGlobal.m_timeStep);
-
-				if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
-				{
-					m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
-				}
-				if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
-				{
-					m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
-				}
-
+				///solve all joint constraints, using SIMD, if available
+				for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+					resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				}
+
+				for (j=0;j<numConstraints;j++)
+				{
+					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
+					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
+					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
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				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);
+
+				}
+				///solve all friction constraints, using SIMD, if available
+				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+				for (j=0;j<numFrictionPoolConstraints;j++)
+				{
+					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+					if (totalImpulse>btScalar(0))
+					{
+						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],	m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					}
+				}
+			} else
+			{
+
+				///solve all joint constraints
+				for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				}
+
+				for (j=0;j<numConstraints;j++)
+				{
+					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
+					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
+					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 (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);
+				}
+				///solve all friction constraints
+				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+				for (j=0;j<numFrictionPoolConstraints;j++)
+				{
+					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+					if (totalImpulse>btScalar(0))
+					{
+						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+						resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],							m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					}
+				}
 			}
 
-			{
-				int numPoolConstraints = m_tmpSolverConstraintPool.size();
-				for (j=0;j<numPoolConstraints;j++)
-				{
-					
-					const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
-					resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
-						m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
-				}
-			}
-
-			{
-				 int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
-				
-				 for (j=0;j<numFrictionPoolConstraints;j++)
-				{
-					const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-					btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
-								m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;			
-
-						resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
-							m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
-							totalImpulse);
-				}
-			}
-			
-
-
-		}
+
+
+		}
+	}
+	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,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
+{
+
 	
-		if (infoGlobal.m_splitImpulse)
-		{
-			
-			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
-			{
-				{
-					int numPoolConstraints = m_tmpSolverConstraintPool.size();
-					int j;
-					for (j=0;j<numPoolConstraints;j++)
-					{
-						const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
-
-						resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
-							m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
-					}
-				}
-			}
-
-		}
-
-	}
-
-	return 0.f;
-}
-
-
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
-{
+
+	BT_PROFILE("solveGroup");
+	//we only implement SOLVER_CACHE_FRIENDLY now
+	//you need to provide at least some bodies
+	btAssert(bodies);
+	btAssert(numBodies);
+
 	int i;
 
@@ -994,10 +974,11 @@
 	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
 
-	int numPoolConstraints = m_tmpSolverConstraintPool.size();
+	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 	int j;
+
 	for (j=0;j<numPoolConstraints;j++)
 	{
-		
-		const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
+
+		const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
 		btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
 		btAssert(pt);
@@ -1005,10 +986,9 @@
 		if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
 		{
-			pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-			pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
+			pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+			pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
 		}
 
 		//do a callback here?
-
 	}
 
@@ -1022,421 +1002,24 @@
 	{
 		for ( i=0;i<m_tmpSolverBodyPool.size();i++)
-        {
-                m_tmpSolverBodyPool[i].writebackVelocity();
-        }
-	}
-
-//	printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
-
-/*
-	printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
-	printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
-	printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
-
-	
-	printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
-	printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
-	printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
-*/
+		{
+			m_tmpSolverBodyPool[i].writebackVelocity();
+		}
+	}
+
 
 	m_tmpSolverBodyPool.resize(0);
-	m_tmpSolverConstraintPool.resize(0);
-	m_tmpSolverFrictionConstraintPool.resize(0);
-
+	m_tmpSolverContactConstraintPool.resize(0);
+	m_tmpSolverNonContactConstraintPool.resize(0);
+	m_tmpSolverContactFrictionConstraintPool.resize(0);
 
 	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,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
-{
-	BT_PROFILE("solveGroup");
-	if (infoGlobal.m_solverMode & SOLVER_CACHE_FRIENDLY)
-	{
-		//you need to provide at least some bodies
-		//btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
-		btAssert(bodies);
-		btAssert(numBodies);
-		return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
-	}
-
-	
-
-	btContactSolverInfo info = infoGlobal;
-
-	int numiter = infoGlobal.m_numIterations;
-
-	int totalPoints = 0;
-
-
-	{
-		short j;
-		for (j=0;j<numManifolds;j++)
-		{
-			btPersistentManifold* manifold = manifoldPtr[j];
-			prepareConstraints(manifold,info,debugDrawer);
-
-			for (short p=0;p<manifoldPtr[j]->getNumContacts();p++)
-			{
-				gOrder[totalPoints].m_manifoldIndex = j;
-				gOrder[totalPoints].m_pointIndex = p;
-				totalPoints++;
-			}
-		}
-	}
-
-	{
-		int j;
-		for (j=0;j<numConstraints;j++)
-		{
-			btTypedConstraint* constraint = constraints[j];
-			constraint->buildJacobian();
-		}
-	}
-	
-
-	//should traverse the contacts random order...
-	int iteration;
-
-	{
-		for ( iteration = 0;iteration<numiter;iteration++)
-		{
-			int j;
-			if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
-			{
-				if ((iteration & 7) == 0) {
-					for (j=0; j<totalPoints; ++j) {
-						btOrderIndex tmp = gOrder[j];
-						int swapi = btRandInt2(j+1);
-						gOrder[j] = gOrder[swapi];
-						gOrder[swapi] = tmp;
-					}
-				}
-			}
-
-			for (j=0;j<numConstraints;j++)
-			{
-				btTypedConstraint* constraint = constraints[j];
-				constraint->solveConstraint(info.m_timeStep);
-			}
-
-			for (j=0;j<totalPoints;j++)
-			{
-				btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
-				solve( (btRigidBody*)manifold->getBody0(),
-									(btRigidBody*)manifold->getBody1()
-				,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
-			}
-		
-			for (j=0;j<totalPoints;j++)
-			{
-				btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
-				solveFriction((btRigidBody*)manifold->getBody0(),
-					(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
-			}
-			
-		}
-	}
-		
-
-
-
-	return btScalar(0.);
-}
-
-
-
-
-
-
-
-void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
-{
-
-	(void)debugDrawer;
-
-	btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
-	btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
-
-
-	//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
-	{
-#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
-		manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
-#endif //FORCE_REFESH_CONTACT_MANIFOLDS		
-		int numpoints = manifoldPtr->getNumContacts();
-
-		gTotalContactPoints += numpoints;
-
-		
-		for (int i=0;i<numpoints ;i++)
-		{
-			btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
-			if (cp.getDistance() <= btScalar(0.))
-			{
-				const btVector3& pos1 = cp.getPositionWorldOnA();
-				const btVector3& pos2 = cp.getPositionWorldOnB();
-
-				btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); 
-				btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
-				
-
-				//this jacobian entry is re-used for all iterations
-				btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
-					body1->getCenterOfMassTransform().getBasis().transpose(),
-					rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
-					body1->getInvInertiaDiagLocal(),body1->getInvMass());
-
-				
-				btScalar jacDiagAB = jac.getDiagonal();
-
-				btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
-				if (cpd)
-				{
-					//might be invalid
-					cpd->m_persistentLifeTime++;
-					if (cpd->m_persistentLifeTime != cp.getLifeTime())
-					{
-						//printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
-						new (cpd) btConstraintPersistentData;
-						cpd->m_persistentLifeTime = cp.getLifeTime();
-
-					} else
-					{
-						//printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
-						
-					}
-				} else
-				{
-						
-					//todo: should this be in a pool?
-					void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
-					cpd = new (mem)btConstraintPersistentData;
-					assert(cpd);
-
-					totalCpd ++;
-					//printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
-					cp.m_userPersistentData = cpd;
-					cpd->m_persistentLifeTime = cp.getLifeTime();
-					//printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
-					
-				}
-				assert(cpd);
-
-				cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB;
-
-				//Dependent on Rigidbody A and B types, fetch the contact/friction response func
-				//perhaps do a similar thing for friction/restutution combiner funcs...
-				
-				cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
-				cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
-				
-				btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
-				btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
-				btVector3 vel = vel1 - vel2;
-				btScalar rel_vel;
-				rel_vel = cp.m_normalWorldOnB.dot(vel);
-				
-				btScalar combinedRestitution = cp.m_combinedRestitution;
-				
-				cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
-				cpd->m_friction = cp.m_combinedFriction;
-				if (cp.m_lifeTime>info.m_restingContactRestitutionThreshold)
-				{
-					cpd->m_restitution = 0.f;
-				} else
-				{
-					cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
-					if (cpd->m_restitution <= btScalar(0.))
-					{
-						cpd->m_restitution = btScalar(0.0);
-					};
-				}
-				
-				//restitution and penetration work in same direction so
-				//rel_vel 
-				
-				btScalar penVel = -cpd->m_penetration/info.m_timeStep;
-
-				if (cpd->m_restitution > penVel)
-				{
-					cpd->m_penetration = btScalar(0.);
-				} 				
-				
-
-				btScalar relaxation = info.m_damping;
-				if (info.m_solverMode & SOLVER_USE_WARMSTARTING)
-				{
-					cpd->m_appliedImpulse *= relaxation;
-				} else
-				{
-					cpd->m_appliedImpulse =btScalar(0.);
-				}
-	
-				//for friction
-				cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
-				
-				//re-calculate friction direction every frame, todo: check if this is really needed
-				btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
-
-
-#define NO_FRICTION_WARMSTART 1
-
-	#ifdef NO_FRICTION_WARMSTART
-				cpd->m_accumulatedTangentImpulse0 = btScalar(0.);
-				cpd->m_accumulatedTangentImpulse1 = btScalar(0.);
-	#endif //NO_FRICTION_WARMSTART
-				btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
-				btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
-				btScalar denom = relaxation/(denom0+denom1);
-				cpd->m_jacDiagABInvTangent0 = denom;
-
-
-				denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
-				denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
-				denom = relaxation/(denom0+denom1);
-				cpd->m_jacDiagABInvTangent1 = denom;
-
-				btVector3 totalImpulse = 
-	#ifndef NO_FRICTION_WARMSTART
-					cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
-					cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
-	#endif //NO_FRICTION_WARMSTART
-					cp.m_normalWorldOnB*cpd->m_appliedImpulse;
-
-
-
-				///
-				{
-				btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
-				cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
-				btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
-				cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
-				}
-				{
-					btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
-					cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
-				}
-				{
-					btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
-					cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
-				}
-				{
-					btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
-					cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
-				}
-				{
-					btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
-					cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
-				}
-				
-				///
-
-
-
-				//apply previous frames impulse on both bodies
-				body0->applyImpulse(totalImpulse, rel_pos1);
-				body1->applyImpulse(-totalImpulse, rel_pos2);
-			}
-			
-		}
-	}
-}
-
-
-btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
-{
-	btScalar maxImpulse = btScalar(0.);
-
-	{
-
-		
-		{
-			if (cp.getDistance() <= btScalar(0.))
-			{
-
-				
-
-				{
-
-					//btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
-					btScalar impulse = resolveSingleCollisionCombined(
-						*body0,*body1,
-						cp,
-						info);
-
-					if (maxImpulse < impulse)
-						maxImpulse  = impulse;
-
-				}
-			}
-		}
-	}
-	return maxImpulse;
-}
-
-
-
-btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
-{
-
-	btScalar maxImpulse = btScalar(0.);
-
-	{
-
-		
-		{
-			if (cp.getDistance() <= btScalar(0.))
-			{
-
-				
-
-				{
-
-					btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
-					btScalar impulse = cpd->m_contactSolverFunc(
-						*body0,*body1,
-						cp,
-						info);
-
-					if (maxImpulse < impulse)
-						maxImpulse  = impulse;
-
-				}
-			}
-		}
-	}
-	return maxImpulse;
-}
-
-btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
-{
-
-	(void)debugDrawer;
-	(void)iter;
-
-
-	{
-
-		
-		{
-			
-			if (cp.getDistance() <= btScalar(0.))
-			{
-
-				btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
-				cpd->m_frictionSolverFunc(
-					*body0,*body1,
-					cp,
-					info);
-
-				
-			}
-		}
-
-	
-	}
-	return btScalar(0.);
-}
+
+
+
+
+
+
 
 
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h	(revision 2882)
@@ -24,65 +24,57 @@
 
 
-///The btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
-///The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
-///Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
-///Applies impulses for combined restitution and penetration recovery and to simulate friction
+
+///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
 class btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
+protected:
 
 	btAlignedObjectArray<btSolverBody>	m_tmpSolverBodyPool;
-	btAlignedObjectArray<btSolverConstraint>	m_tmpSolverConstraintPool;
-	btAlignedObjectArray<btSolverConstraint>	m_tmpSolverFrictionConstraintPool;
+	btConstraintArray			m_tmpSolverContactConstraintPool;
+	btConstraintArray			m_tmpSolverNonContactConstraintPool;
+	btConstraintArray			m_tmpSolverContactFrictionConstraintPool;
 	btAlignedObjectArray<int>	m_orderTmpConstraintPool;
 	btAlignedObjectArray<int>	m_orderFrictionConstraintPool;
 
-
-protected:
-	btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
-	btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
-	void  prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
 	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);
-
-	ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
-	ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
-
 	
 	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
 	unsigned long	m_btSeed2;
 
+	void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
+	btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
+
+	void	convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
+
+	void	resolveSplitPenetrationImpulseCacheFriendly(
+        btSolverBody& body1,
+        btSolverBody& body2,
+        const btSolverConstraint& contactConstraint,
+        const btContactSolverInfo& solverInfo);
+
+	//internal method
+	int	getOrInitSolverBody(btCollisionObject& body);
+
+	void	resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+
+	void	resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+	
+	void	resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+	
+	void	resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+		
 public:
 
 	
 	btSequentialImpulseConstraintSolver();
+	virtual ~btSequentialImpulseConstraintSolver();
 
-	///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
-	///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
-	void	setContactSolverFunc(ContactSolverFunc func,int type0,int type1)
-	{
-		m_contactDispatch[type0][type1] = func;
-	}
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
 	
-	///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
-	///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
-	void	SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1)
-	{
-		m_frictionDispatch[type0][type1] = func;
-	}
-
-	virtual ~btSequentialImpulseConstraintSolver();
-	
-	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
-
-	virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+	btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
 	btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
-	btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
-
 
 	///clear internal cached data and reset random seed
 	virtual	void	reset();
-
-	btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
-
-
 	
 	unsigned long btRand2();
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp	(revision 2882)
@@ -69,5 +69,7 @@
 btSliderConstraint::btSliderConstraint()
         :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
-		m_useLinearReferenceFrameA(true)
+		m_useLinearReferenceFrameA(true),
+		m_useSolveConstraintObsolete(false)
+//		m_useSolveConstraintObsolete(true)
 {
 	initParams();
@@ -80,5 +82,7 @@
         , m_frameInA(frameInA)
         , m_frameInB(frameInB),
-		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+		m_useLinearReferenceFrameA(useLinearReferenceFrameA),
+		m_useSolveConstraintObsolete(false)
+//		m_useSolveConstraintObsolete(true)
 {
 	initParams();
@@ -89,4 +93,8 @@
 void btSliderConstraint::buildJacobian()
 {
+	if (!m_useSolveConstraintObsolete) 
+	{
+		return;
+	}
 	if(m_useLinearReferenceFrameA)
 	{
@@ -156,14 +164,357 @@
 //-----------------------------------------------------------------------------
 
-void btSliderConstraint::solveConstraint(btScalar timeStep)
-{
-    m_timeStep = timeStep;
-	if(m_useLinearReferenceFrameA)
-	{
-		solveConstraintInt(m_rbA, m_rbB);
+void btSliderConstraint::getInfo1(btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
 	}
 	else
 	{
-		solveConstraintInt(m_rbB, m_rbA);
+		info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
+		info->nub = 2; 
+		//prepare constraint
+		calculateTransforms();
+		testLinLimits();
+		if(getSolveLinLimit() || getPoweredLinMotor())
+		{
+			info->m_numConstraintRows++; // limit 3rd linear as well
+			info->nub--; 
+		}
+		testAngLimits();
+		if(getSolveAngLimit() || getPoweredAngMotor())
+		{
+			info->m_numConstraintRows++; // limit 3rd angular as well
+			info->nub--; 
+		}
+	}
+} // btSliderConstraint::getInfo1()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::getInfo2(btConstraintInfo2* info)
+{
+	btAssert(!m_useSolveConstraintObsolete);
+	int i, s = info->rowskip;
+	const btTransform& trA = getCalculatedTransformA();
+	const btTransform& trB = getCalculatedTransformB();
+	btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
+	// make rotations around Y and Z equal
+	// the slider axis should be the only unconstrained
+	// rotational axis, the angular velocity of the two bodies perpendicular to
+	// the slider axis should be equal. thus the constraint equations are
+	//    p*w1 - p*w2 = 0
+	//    q*w1 - q*w2 = 0
+	// where p and q are unit vectors normal to the slider axis, and w1 and w2
+	// are the angular velocity vectors of the two bodies.
+	// get slider axis (X)
+	btVector3 ax1 = trA.getBasis().getColumn(0);
+	// get 2 orthos to slider axis (Y, Z)
+	btVector3 p = trA.getBasis().getColumn(1);
+	btVector3 q = trA.getBasis().getColumn(2);
+	// set the two slider rows 
+	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_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];
+	// 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 ax1,ax2 are the unit length slider 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.
+	btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
+    btVector3 ax2 = trB.getBasis().getColumn(0);
+	btVector3 u = ax1.cross(ax2);
+	info->m_constraintError[0] = k * u.dot(p);
+	info->m_constraintError[s] = k * u.dot(q);
+	// pull out pos and R for both bodies. also get the connection
+	// vector c = pos2-pos1.
+	// next two rows. we want: vel2 = vel1 + w1 x c ... but this would
+	// result in three equations, so we project along the planespace vectors
+	// so that sliding along the slider axis is disregarded. for symmetry we
+	// also consider rotation around center of mass of two bodies (factA and factB).
+	btTransform bodyA_trans = m_rbA.getCenterOfMassTransform();
+	btTransform bodyB_trans = m_rbB.getCenterOfMassTransform();
+	int s2 = 2 * s, s3 = 3 * s;
+	btVector3 c;
+	btScalar miA = m_rbA.getInvMass();
+	btScalar miB = m_rbB.getInvMass();
+	btScalar miS = miA + miB;
+	btScalar factA, factB;
+	if(miS > btScalar(0.f))
+	{
+		factA = miB / miS;
+	}
+	else 
+	{
+		factA = btScalar(0.5f);
+	}
+	if(factA > 0.99f) factA = 0.99f;
+	if(factA < 0.01f) factA = 0.01f;
+	factB = btScalar(1.0f) - factA;
+	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];
+	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_J1linearAxis[s2+i] = p[i];
+	for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
+	// compute two elements of right hand side. we want to align the offset
+	// point (in body 2's frame) with the center of body 1.
+	btVector3 ofs; // offset point in global coordinates
+	ofs = trB.getOrigin() - trA.getOrigin();
+	k = info->fps * info->erp * getSoftnessOrthoLin();
+	info->m_constraintError[s2] = k * p.dot(ofs);
+	info->m_constraintError[s3] = k * q.dot(ofs);
+	int nrow = 3; // last filled row
+	int srow;
+	// check linear limits linear
+	btScalar limit_err = btScalar(0.0);
+	int limit = 0;
+	if(getSolveLinLimit())
+	{
+		limit_err = getLinDepth() *  signFact;
+		limit = (limit_err > btScalar(0.0)) ? 2 : 1;
+	}
+	int powered = 0;
+	if(getPoweredLinMotor())
+	{
+		powered = 1;
+	}
+	// if the slider has joint limits or motor, add in the extra row
+	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];
+		// linear torque decoupling step:
+		//
+		// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
+		// do not create a torque couple. in other words, the points that the
+		// 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.
+		// the solution used here is to apply the constraint forces at the center of mass of the two bodies
+		btVector3 ltd;	// Linear Torque Decoupling vector (a torque)
+//		c = btScalar(0.5) * c;
+		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];
+		// right-hand part
+		btScalar lostop = getLowerLinLimit();
+		btScalar histop = getUpperLinLimit();
+		if(limit && (lostop == histop))
+		{  // the joint motor is ineffective
+			powered = 0;
+		}
+		info->m_constraintError[srow] = 0.;
+		info->m_lowerLimit[srow] = 0.;
+		info->m_upperLimit[srow] = 0.;
+		if(powered)
+		{
+            info->cfm[nrow] = btScalar(0.0); 
+			btScalar tag_vel = getTargetLinMotorVelocity();
+			btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
+//			info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity();
+			info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
+			info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
+			info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
+		}
+		if(limit)
+		{
+			k = info->fps * info->erp;
+			info->m_constraintError[srow] += k * limit_err;
+			info->cfm[srow] = btScalar(0.0); // stop_cfm;
+			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
+				info->m_lowerLimit[srow] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = 0;
+			}
+			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))
+			{
+				btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
+				vel -= m_rbB.getLinearVelocity().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)
+					{
+						btScalar newc = -bounce * vel;
+						if (newc > info->m_constraintError[srow])
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+				else
+				{ // high limit - all those computations are reversed
+					if(vel > 0)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc < info->m_constraintError[srow]) 
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+			}
+			info->m_constraintError[srow] *= getSoftnessLimLin();
+		} // if(limit)
+	} // if linear limit
+	// check angular limits
+	limit_err = btScalar(0.0);
+	limit = 0;
+	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) 
+	{
+		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_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))
+		{  // the joint motor is ineffective
+			powered = 0;
+		}
+		if(powered)
+		{
+            info->cfm[srow] = btScalar(0.0); 
+			btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
+			info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
+			info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
+			info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
+		}
+		if(limit)
+		{
+			k = info->fps * info->erp;
+			info->m_constraintError[srow] += k * limit_err;
+			info->cfm[srow] = btScalar(0.0); // stop_cfm;
+			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
+				info->m_lowerLimit[srow] = 0;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			}
+			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))
+			{
+				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)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc > info->m_constraintError[srow])
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+				else
+				{	// high limit - all those computations are reversed
+					if(vel > 0)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc < info->m_constraintError[srow])
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+			}
+			info->m_constraintError[srow] *= getSoftnessLimAng();
+		} // if(limit)
+	} // if angular limit or powered
+} // btSliderConstraint::getInfo2()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		m_timeStep = timeStep;
+		if(m_useLinearReferenceFrameA)
+		{
+			solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB);
+		}
+		else
+		{
+			solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
+		}
 	}
 } // btSliderConstraint::solveConstraint()
@@ -171,10 +522,12 @@
 //-----------------------------------------------------------------------------
 
-void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
+void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
 {
     int i;
     // linear
-    btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA);
-    btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB);
+    btVector3 velA;
+	bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
+    btVector3 velB;
+	bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
     btVector3 vel = velA - velB;
 	for(i = 0; i < 3; i++)
@@ -191,6 +544,16 @@
 		btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
 		btVector3 impulse_vector = normal * normalImpulse;
-		rbA.applyImpulse( impulse_vector, m_relPosA);
-		rbB.applyImpulse(-impulse_vector, m_relPosB);
+		
+		//rbA.applyImpulse( impulse_vector, m_relPosA);
+		//rbB.applyImpulse(-impulse_vector, m_relPosB);
+		{
+			btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
+			btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
+			bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+			bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+		}
+
+
+
 		if(m_poweredLinMotor && (!i))
 		{ // apply linear motor
@@ -218,6 +581,16 @@
 				// apply clamped impulse
 				impulse_vector = normal * normalImpulse;
-				rbA.applyImpulse( impulse_vector, m_relPosA);
-				rbB.applyImpulse(-impulse_vector, m_relPosB);
+				//rbA.applyImpulse( impulse_vector, m_relPosA);
+				//rbB.applyImpulse(-impulse_vector, m_relPosB);
+
+				{
+					btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
+					btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
+					bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+					bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+				}
+
+
+
 			}
 		}
@@ -228,6 +601,8 @@
 	btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
 
-	const btVector3& angVelA = rbA.getAngularVelocity();
-	const btVector3& angVelB = rbB.getAngularVelocity();
+	btVector3 angVelA;
+	bodyA.getAngularVelocity(angVelA);
+	btVector3 angVelB;
+	bodyB.getAngularVelocity(angVelB);
 
 	btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
@@ -239,12 +614,18 @@
 	//solve orthogonal angular velocity correction
 	btScalar len = velrelOrthog.length();
+	btScalar orthorImpulseMag = 0.f;
+
 	if (len > btScalar(0.00001))
 	{
 		btVector3 normal = velrelOrthog.normalized();
 		btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
-		velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+		//velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+		orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
 	}
 	//solve angular positional correction
 	btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
+	btVector3 angularAxis = angularError;
+	btScalar angularImpulseMag = 0;
+
 	btScalar len2 = angularError.length();
 	if (len2>btScalar(0.00001))
@@ -252,9 +633,17 @@
 		btVector3 normal2 = angularError.normalized();
 		btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
-		angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+		angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+		angularError *= angularImpulseMag;
 	}
 	// apply impulse
-	rbA.applyTorqueImpulse(-velrelOrthog+angularError);
-	rbB.applyTorqueImpulse(velrelOrthog-angularError);
+	//rbA.applyTorqueImpulse(-velrelOrthog+angularError);
+	//rbB.applyTorqueImpulse(velrelOrthog-angularError);
+
+	bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag);
+	bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag);
+	bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag);
+	bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag);
+
+
 	btScalar impulseMag;
 	//solve angular limits
@@ -270,6 +659,12 @@
 	}
 	btVector3 impulse = axisA * impulseMag;
-	rbA.applyTorqueImpulse(impulse);
-	rbB.applyTorqueImpulse(-impulse);
+	//rbA.applyTorqueImpulse(impulse);
+	//rbB.applyTorqueImpulse(-impulse);
+
+	bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag);
+	bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag);
+
+
+
 	//apply angular motor
 	if(m_poweredAngMotor) 
@@ -302,6 +697,9 @@
 			// apply clamped impulse
 			btVector3 motorImp = angImpulse * axisA;
-			m_rbA.applyTorqueImpulse(motorImp);
-			m_rbB.applyTorqueImpulse(-motorImp);
+			//rbA.applyTorqueImpulse(motorImp);
+			//rbB.applyTorqueImpulse(-motorImp);
+
+			bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse);
+			bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse);
 		}
 	}
@@ -313,5 +711,5 @@
 
 void btSliderConstraint::calculateTransforms(void){
-	if(m_useLinearReferenceFrameA)
+	if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
 	{
 		m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
@@ -326,5 +724,12 @@
 	m_realPivotBInW = m_calculatedTransformB.getOrigin();
 	m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
-	m_delta = m_realPivotBInW - m_realPivotAInW;
+	if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
+	{
+		m_delta = m_realPivotBInW - m_realPivotAInW;
+	}
+	else
+	{
+		m_delta = m_realPivotAInW - m_realPivotBInW;
+	}
 	m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
     btVector3 normalWorld;
@@ -368,5 +773,4 @@
 
 //-----------------------------------------------------------------------------
- 
 
 void btSliderConstraint::testAngLimits(void)
@@ -380,4 +784,5 @@
 		const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
 		btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));  
+		m_angPos = rot;
 		if(rot < m_lowerAngLimit)
 		{
@@ -392,9 +797,6 @@
 	}
 } // btSliderConstraint::testAngLimits()
-
 	
 //-----------------------------------------------------------------------------
-
-
 
 btVector3 btSliderConstraint::getAncorInA(void)
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h	(revision 2882)
@@ -47,4 +47,6 @@
 {
 protected:
+	///for backwards compatibility during the transition to 'getInfo/getInfo2'
+	bool		m_useSolveConstraintObsolete;
 	btTransform	m_frameInA;
     btTransform	m_frameInB;
@@ -105,4 +107,5 @@
 
 	btScalar m_linPos;
+	btScalar m_angPos;
 
 	btScalar m_angDepth;
@@ -127,5 +130,11 @@
 	// overrides
     virtual void	buildJacobian();
-    virtual	void	solveConstraint(btScalar	timeStep);
+    virtual void getInfo1 (btConstraintInfo1* info);
+	
+	virtual void getInfo2 (btConstraintInfo2* info);
+
+    virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
+	
+
 	// access
     const btRigidBody& getRigidBodyA() const { return m_rbA; }
@@ -195,4 +204,5 @@
 	btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
 	btScalar getLinearPos() { return m_linPos; }
+	
 
 	// access for ODE solver
@@ -203,8 +213,9 @@
 	// internal
     void	buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
-    void	solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB);
+    void	solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
 	// shared code used by ODE solver
 	void	calculateTransforms(void);
 	void	testLinLimits(void);
+	void	testLinLimits2(btConstraintInfo2* info);
 	void	testAngLimits(void);
 	// access for PE Solver
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h	(revision 2882)
@@ -24,4 +24,84 @@
 #include "LinearMath/btTransformUtil.h"
 
+///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 //
+
+
+#ifdef USE_SIMD
+
+struct	btSimdScalar
+{
+	SIMD_FORCE_INLINE	btSimdScalar()
+	{
+
+	}
+
+	SIMD_FORCE_INLINE	btSimdScalar(float	fl)
+	:m_vec128 (_mm_set1_ps(fl))
+	{
+	}
+
+	SIMD_FORCE_INLINE	btSimdScalar(__m128 v128)
+		:m_vec128(v128)
+	{
+	}
+	union
+	{
+		__m128		m_vec128;
+		float		m_floats[4];
+		int			m_ints[4];
+		btScalar	m_unusedPadding;
+	};
+	SIMD_FORCE_INLINE	__m128	get128()
+	{
+		return m_vec128;
+	}
+
+	SIMD_FORCE_INLINE	const __m128	get128() const
+	{
+		return m_vec128;
+	}
+
+	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 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) 
+{
+	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) 
+{
+	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.
@@ -29,43 +109,43 @@
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
+	btVector3		m_deltaLinearVelocity;
+	btVector3		m_deltaAngularVelocity;
+	btScalar		m_angularFactor;
+	btScalar		m_invMass;
+	btScalar		m_friction;
+	btRigidBody*	m_originalBody;
+	btVector3		m_pushVelocity;
+	//btVector3		m_turnVelocity;	
+
 	
-	btMatrix3x3		m_worldInvInertiaTensor;
-
-	btVector3		m_angularVelocity;
-	btVector3		m_linearVelocity;
-	btVector3		m_centerOfMassPosition;
-	btVector3		m_pushVelocity;
-	btVector3		m_turnVelocity;
-
-	float			m_angularFactor;
-	float			m_invMass;
-	float			m_friction;
-	btRigidBody*	m_originalBody;
-	
-	
-	SIMD_FORCE_INLINE void	getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
+	SIMD_FORCE_INLINE void	getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
 	{
-		velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
+		if (m_originalBody)
+			velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
+		else
+			velocity.setValue(0,0,0);
 	}
 
+	SIMD_FORCE_INLINE void	getAngularVelocity(btVector3& angVel) const
+	{
+		if (m_originalBody)
+			angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
+		else
+			angVel.setValue(0,0,0);
+	}
+
+
 	//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,btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
 	{
-		if (m_invMass)
+		//if (m_invMass)
 		{
-			m_linearVelocity += linearComponent*impulseMagnitude;
-			m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
-		}
-	}
-	
-	SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
-	{
-		if (m_invMass)
-		{
-			m_pushVelocity += linearComponent*impulseMagnitude;
-			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+			m_deltaLinearVelocity += linearComponent*impulseMagnitude;
+			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
 		}
 	}
 
+	
+/*
 	
 	void	writebackVelocity()
@@ -73,37 +153,21 @@
 		if (m_invMass)
 		{
-			m_originalBody->setLinearVelocity(m_linearVelocity);
-			m_originalBody->setAngularVelocity(m_angularVelocity);
+			m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
+			m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
 			
 			//m_originalBody->setCompanionId(-1);
 		}
 	}
-	
+	*/
 
-	void	writebackVelocity(btScalar timeStep)
+	void	writebackVelocity(btScalar timeStep=0)
 	{
 		if (m_invMass)
 		{
-			m_originalBody->setLinearVelocity(m_linearVelocity);
-			m_originalBody->setAngularVelocity(m_angularVelocity);
-			
-			//correct the position/orientation based on push/turn recovery
-			btTransform newTransform;
-			btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
-			m_originalBody->setWorldTransform(newTransform);
-			
+			m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
+			m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
 			//m_originalBody->setCompanionId(-1);
 		}
 	}
-
-	void	readVelocity()
-	{
-		if (m_invMass)
-		{
-			m_linearVelocity = m_originalBody->getLinearVelocity();
-			m_angularVelocity = m_originalBody->getAngularVelocity();
-		}
-	}
-
 	
 
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h	(revision 2882)
@@ -20,6 +20,9 @@
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btMatrix3x3.h"
+#include "btJacobianEntry.h"
 
 //#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.
@@ -28,30 +31,51 @@
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btVector3	m_relpos1CrossNormal;
-	btVector3	m_contactNormal;
+	btVector3		m_relpos1CrossNormal;
+	btVector3		m_contactNormal;
 
-	btVector3	m_relpos2CrossNormal;
-	btVector3	m_angularComponentA;
+	btVector3		m_relpos2CrossNormal;
+	//btVector3		m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
 
-	btVector3	m_angularComponentB;
-
-	mutable btScalar	m_appliedPushImpulse;
+	btVector3		m_angularComponentA;
+	btVector3		m_angularComponentB;
 	
-	mutable btScalar	m_appliedImpulse;
-	int			m_solverBodyIdA;
-	int			m_solverBodyIdB;
+	mutable btSimdScalar	m_appliedPushImpulse;
+	mutable btSimdScalar	m_appliedImpulse;
+	
 	
 	btScalar	m_friction;
-	btScalar	m_restitution;
 	btScalar	m_jacDiagABInv;
-	btScalar	m_penetration;
+	union
+	{
+		int	m_numConsecutiveRowsPerKernel;
+		btScalar	m_unusedPadding0;
+	};
+
+	union
+	{
+		int			m_frictionIndex;
+		btScalar	m_unusedPadding1;
+	};
+	union
+	{
+		int			m_solverBodyIdA;
+		btScalar	m_unusedPadding2;
+	};
+	union
+	{
+		int			m_solverBodyIdB;
+		btScalar	m_unusedPadding3;
+	};
 	
+	union
+	{
+		void*		m_originalContactPoint;
+		btScalar	m_unusedPadding4;
+	};
 
-	
-	int			m_constraintType;
-	int			m_frictionIndex;
-	void*		m_originalContactPoint;
-	int			m_unusedPadding[1];
-
+	btScalar		m_rhs;
+	btScalar		m_cfm;
+	btScalar		m_lowerLimit;
+	btScalar		m_upperLimit;
 
 	enum		btSolverConstraintType
@@ -62,7 +86,5 @@
 };
 
-
-
-
+typedef btAlignedObjectArray<btSolverConstraint>	btConstraintArray;
 
 
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp	(revision 2882)
@@ -20,4 +20,6 @@
 static btRigidBody s_fixed(0, 0,0);
 
+#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
+
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
 :m_userConstraintType(-1),
@@ -26,5 +28,6 @@
 m_rbA(s_fixed),
 m_rbB(s_fixed),
-m_appliedImpulse(btScalar(0.))
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 {
 	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
@@ -36,5 +39,6 @@
 m_rbA(rbA),
 m_rbB(s_fixed),
-m_appliedImpulse(btScalar(0.))
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 {
 		s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
@@ -49,5 +53,6 @@
 m_rbA(rbA),
 m_rbB(rbB),
-m_appliedImpulse(btScalar(0.))
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 {
 		s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
@@ -55,2 +60,55 @@
 }
 
+
+//-----------------------------------------------------------------------------
+
+btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
+{
+	if(lowLim > uppLim)
+	{
+		return btScalar(1.0f);
+	}
+	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((pos >= lowLim) && (pos < (lowLim - delta_max)))
+		{
+			lim_fact = (lowLim - pos) / delta_max;
+		}
+		else if(pos  < lowLim)
+		{
+			lim_fact = btScalar(0.0f);
+		}
+		else
+		{
+			lim_fact = btScalar(1.0f);
+		}
+	}
+	else if(delta_max > btScalar(0.0f))
+	{
+		if((pos <= uppLim) && (pos > (uppLim - delta_max)))
+		{
+			lim_fact = (uppLim - pos) / delta_max;
+		}
+		else if(pos  > uppLim)
+		{
+			lim_fact = btScalar(0.0f);
+		}
+		else
+		{
+			lim_fact = btScalar(1.0f);
+		}
+	}
+	else
+	{
+			lim_fact = btScalar(0.0f);
+	}
+	return lim_fact;
+} // btTypedConstraint::getMotorFactor()
+
+
Index: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h	(revision 2882)
@@ -19,4 +19,9 @@
 class btRigidBody;
 #include "LinearMath/btScalar.h"
+#include "btSolverConstraint.h"
+struct  btSolverBody;
+
+
+
 
 enum btTypedConstraintType
@@ -26,5 +31,4 @@
 	CONETWIST_CONSTRAINT_TYPE,
 	D6_CONSTRAINT_TYPE,
-	VEHICLE_CONSTRAINT_TYPE,
 	SLIDER_CONSTRAINT_TYPE
 };
@@ -49,4 +53,5 @@
 	btRigidBody&	m_rbB;
 	btScalar	m_appliedImpulse;
+	btScalar	m_dbgDrawSize;
 
 
@@ -56,11 +61,53 @@
 	virtual ~btTypedConstraint() {};
 	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
+	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
 
-	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
+	struct btConstraintInfo1 {
+		int m_numConstraintRows,nub;
+	};
+
+	struct btConstraintInfo2 {
+		// integrator parameters: frames per second (1/stepsize), default error
+		// reduction parameter (0..1).
+		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;
+
+		// elements to jump from one row to the next in J's
+		int rowskip;
+
+		// 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;
+
+		// lo and hi limits for variables (set to -/+ infinity on entry).
+		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;
+	};
+
 
 	virtual void	buildJacobian() = 0;
 
-	virtual	void	solveConstraint(btScalar	timeStep) = 0;
+	virtual	void	setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
+	{
+	}
+	virtual void getInfo1 (btConstraintInfo1* info)=0;
 
+	virtual void getInfo2 (btConstraintInfo2* info)=0;
+
+	virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep) = 0;
+
+	btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
+	
 	const btRigidBody& getRigidBodyA() const
 	{
@@ -95,5 +142,5 @@
 		m_userConstraintId = uid;
 	}
-	
+
 	int getUserConstraintId() const
 	{
@@ -115,5 +162,14 @@
 		return m_constraintType;
 	}
-
+	
+	void setDbgDrawSize(btScalar dbgDrawSize)
+	{
+		m_dbgDrawSize = dbgDrawSize;
+	}
+	btScalar getDbgDrawSize()
+	{
+		return m_dbgDrawSize;
+	}
+	
 };
 
Index: code/trunk/src/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp
===================================================================
--- code/trunk/src/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp	(revision 2882)
@@ -111,5 +111,5 @@
 {
 	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
-	assert(dynamicsWorld);
+	btAssert(dynamicsWorld);
 	dynamicsWorld->stepSimulation(timeStep);
 }
@@ -118,7 +118,7 @@
 {
 	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
-	assert(dynamicsWorld);
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	assert(body);
+	btAssert(dynamicsWorld);
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
 
 	dynamicsWorld->addRigidBody(body);
@@ -128,7 +128,7 @@
 {
 	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
-	assert(dynamicsWorld);
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	assert(body);
+	btAssert(dynamicsWorld);
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
 
 	dynamicsWorld->removeRigidBody(body);
@@ -143,5 +143,5 @@
 	btVector3 localInertia(0,0,0);
 	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
-	assert(shape);
+	btAssert(shape);
 	if (mass)
 	{
@@ -159,5 +159,5 @@
 {
 	btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody);
-	assert(body);
+	btAssert(body);
 	btAlignedFree( body);
 }
@@ -263,5 +263,5 @@
 {
 	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
-	assert(shape);
+	btAssert(shape);
 	btAlignedFree(shape);
 }
@@ -269,5 +269,5 @@
 {
 	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
-	assert(shape);
+	btAssert(shape);
 	btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]);
 	shape->setLocalScaling(scaling);	
Index: code/trunk/src/bullet/BulletDynamics/Dynamics/btActionInterface.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/Dynamics/btActionInterface.h	(revision 2882)
+++ code/trunk/src/bullet/BulletDynamics/Dynamics/btActionInterface.h	(revision 2882)
@@ -0,0 +1,39 @@
+/*
+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_ACTION_INTERFACE_H
+#define _BT_ACTION_INTERFACE_H
+
+class btIDebugDraw;
+class btCollisionWorld;
+
+#include "LinearMath/btScalar.h"
+
+///Basic interface to allow actions such as vehicles and characters to be updated inside a btDynamicsWorld
+class btActionInterface
+{
+	public:
+
+	virtual ~btActionInterface()
+	{
+	}
+
+	virtual void updateAction( btCollisionWorld* collisionWorld, btScalar deltaTimeStep)=0;
+
+	virtual void debugDraw(btIDebugDraw* debugDrawer) = 0;
+
+};
+
+#endif //_BT_ACTION_INTERFACE_H
Index: code/trunk/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp
===================================================================
--- code/trunk/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp	(revision 2882)
@@ -91,6 +91,5 @@
 
 	///update vehicle simulation
-	updateVehicles(timeStep);
-
+	updateActions(timeStep);
 
 	updateActivationState( timeStep );
Index: code/trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
===================================================================
--- code/trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp	(revision 2882)
@@ -30,4 +30,9 @@
 #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/btSliderConstraint.h"
 
 //for debug rendering
@@ -47,13 +52,5 @@
 
 
-
-//vehicle
-#include "BulletDynamics/Vehicle/btRaycastVehicle.h"
-#include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
-#include "BulletDynamics/Vehicle/btWheelInfo.h"
-//character
-#include "BulletDynamics/Character/btCharacterControllerInterface.h"
-
-#include "LinearMath/btIDebugDraw.h"
+#include "BulletDynamics/Dynamics/btActionInterface.h"
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btMotionState.h"
@@ -114,5 +111,4 @@
 		if (body)
 		{
-				btTransform predictedTrans;
 				if (body->getActivationState() != ISLAND_SLEEPING)
 				{
@@ -149,4 +145,22 @@
 		}
 	}
+	bool drawConstraints = false;
+	if (getDebugDrawer())
+	{
+		int mode = getDebugDrawer()->getDebugMode();
+		if(mode  & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
+		{
+			drawConstraints = true;
+		}
+	}
+	if(drawConstraints)
+	{
+		for(int i = getNumConstraints()-1; i>=0 ;i--)
+		{
+			btTypedConstraint* constraint = getConstraint(i);
+			debugDrawConstraint(constraint);
+		}
+	}
+
 
 
@@ -191,30 +205,9 @@
 		}
 	
-		for (  i=0;i<this->m_vehicles.size();i++)
-		{
-			for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
-			{
-				btVector3 wheelColor(0,255,255);
-				if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
-				{
-					wheelColor.setValue(0,0,255);
-				} else
-				{
-					wheelColor.setValue(255,0,255);
-				}
-		
-				btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
-
-				btVector3 axle = btVector3(	
-					m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
-					m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
-					m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
-
-
-				//m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
-				//debug wheels (cylinders)
-				m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
-				m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
-
+		if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
+		{
+			for (i=0;i<m_actions.size();i++)
+			{
+				m_actions[i]->debugDraw(m_debugDrawer);
 			}
 		}
@@ -288,5 +281,5 @@
 		}
 	}
-
+/*
 	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
 	{
@@ -300,4 +293,6 @@
 		}
 	}
+	*/
+
 
 }
@@ -340,5 +335,6 @@
 	if (getDebugDrawer())
 	{
-		gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
+		btIDebugDraw* debugDrawer = getDebugDrawer ();
+		gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
 	}
 	if (numSimulationSubSteps)
@@ -404,8 +400,6 @@
 
 	///update vehicle simulation
-	updateVehicles(timeStep);
-	
-	updateCharacters(timeStep);
-
+	updateActions(timeStep);
+	
 	updateActivationState( timeStep );
 
@@ -471,27 +465,13 @@
 
 
-void	btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
-{
-	BT_PROFILE("updateVehicles");
-	
-	for ( int i=0;i<m_vehicles.size();i++)
-	{
-		btRaycastVehicle* vehicle = m_vehicles[i];
-		vehicle->updateVehicle( timeStep);
-	}
-}
-
-void	btDiscreteDynamicsWorld::updateCharacters(btScalar timeStep)
-{
-	BT_PROFILE("updateCharacters");
-	
-	for ( int i=0;i<m_characters.size();i++)
-	{
-		btCharacterControllerInterface* character = m_characters[i];
-		character->preStep (this);
-		character->playerStep (this,timeStep);
-	}
-}
-
+void	btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
+{
+	BT_PROFILE("updateActions");
+	
+	for ( int i=0;i<m_actions.size();i++)
+	{
+		m_actions[i]->updateAction( this, timeStep);
+	}
+}
 	
 	
@@ -550,22 +530,33 @@
 }
 
-void	btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
-{
-	m_vehicles.push_back(vehicle);
-}
-
-void	btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
-{
-	m_vehicles.remove(vehicle);
-}
-
-void	btDiscreteDynamicsWorld::addCharacter(btCharacterControllerInterface* character)
-{
-	m_characters.push_back(character);
-}
-
-void	btDiscreteDynamicsWorld::removeCharacter(btCharacterControllerInterface* character)
-{
-	m_characters.remove(character);
+void	btDiscreteDynamicsWorld::addAction(btActionInterface* action)
+{
+	m_actions.push_back(action);
+}
+
+void	btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
+{
+	m_actions.remove(action);
+}
+
+
+void	btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
+{
+	addAction(vehicle);
+}
+
+void	btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
+{
+	removeAction(vehicle);
+}
+
+void	btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
+{
+	addAction(character);
+}
+
+void	btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
+{
+	removeAction(character);
 }
 
@@ -643,6 +634,9 @@
 			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_stackAlloc,m_dispatcher);
+				if (numManifolds + m_numConstraints)
+				{
+					///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_stackAlloc,m_dispatcher);
+				}
 			} else
 			{
@@ -690,5 +684,5 @@
 	}
 
-//	assert(0);
+//	btAssert(0);
 		
 	
@@ -754,12 +748,14 @@
 	btScalar m_allowedPenetration;
 	btOverlappingPairCache* m_pairCache;
+	btDispatcher* m_dispatcher;
 
 
 public:
-	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) : 
+	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
 	  btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
 		m_allowedPenetration(0.0f),
 		m_me(me),
-		m_pairCache(pairCache)
+		m_pairCache(pairCache),
+		m_dispatcher(dispatcher)
 	{
 	}
@@ -796,18 +792,24 @@
 			return false;
 
-		///don't do CCD when there are already contact points (touching contact/penetration)
-		btAlignedObjectArray<btPersistentManifold*> manifoldArray;
-		btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
-		if (collisionPair)
-		{
-			if (collisionPair->m_algorithm)
-			{
-				manifoldArray.resize(0);
-				collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
-				for (int j=0;j<manifoldArray.size();j++)
-				{
-					btPersistentManifold* manifold = manifoldArray[j];
-					if (manifold->getNumContacts()>0)
-						return false;
+		btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
+
+		//call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
+		if (m_dispatcher->needsResponse(m_me,otherObj))
+		{
+			///don't do CCD when there are already contact points (touching contact/penetration)
+			btAlignedObjectArray<btPersistentManifold*> manifoldArray;
+			btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
+			if (collisionPair)
+			{
+				if (collisionPair->m_algorithm)
+				{
+					manifoldArray.resize(0);
+					collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
+					for (int j=0;j<manifoldArray.size();j++)
+					{
+						btPersistentManifold* manifold = manifoldArray[j];
+						if (manifold->getNumContacts()>0)
+							return false;
+					}
 				}
 			}
@@ -847,5 +849,5 @@
 						gNumClampedCcdMotions++;
 						
-						btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache());
+						btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
 						btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
 						btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
@@ -1188,4 +1190,187 @@
 
 
+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))
+	{
+		return;
+	}
+
+	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)
+				{
+					break;
+				}
+				bool drawSect = true;
+				if(minAng > maxAng)
+				{
+					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);
+				}
+			}
+			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)
+				{
+					//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));
+
+						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;
+		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) 
+				{
+					tr = p6DOF->getCalculatedTransformA();
+					const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
+					btVector3 up = tr.getBasis().getColumn(2);
+					btVector3 axis = tr.getBasis().getColumn(0);
+					btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
+					btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
+					btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
+					btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
+					getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
+					axis = tr.getBasis().getColumn(1);
+					btScalar ay = p6DOF->getAngle(1);
+					btScalar az = p6DOF->getAngle(2);
+					btScalar cy = btCos(ay);
+					btScalar sy = btSin(ay);
+					btScalar cz = btCos(az);
+					btScalar sz = btSin(az);
+					btVector3 ref;
+					ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
+					ref[1] = -sz*axis[0] + cz*axis[1];
+					ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
+					tr = p6DOF->getCalculatedTransformB();
+					btVector3 normal = -tr.getBasis().getColumn(0);
+					btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
+					btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
+					if(minFi > maxFi)
+					{
+						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);
+					}
+					else if(minFi < maxFi)
+					{
+						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);
+					}
+					tr = p6DOF->getCalculatedTransformA();
+					btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
+					btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
+					getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));
+				}
+			}
+			break;
+		case SLIDER_CONSTRAINT_TYPE:
+			{
+				btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
+				btTransform tr = pSlider->getCalculatedTransformA();
+				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				tr = pSlider->getCalculatedTransformB();
+				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				if(drawLimits)
+				{
+					btTransform tr = pSlider->getCalculatedTransformA();
+					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;
+	}
+	return;
+} // btDiscreteDynamicsWorld::debugDrawConstraint()
+
+
+
+
+
 void	btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 {
@@ -1216,2 +1401,4 @@
 	return m_constraints[index];
 }
+
+
Index: code/trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h	(revision 2882)
@@ -24,8 +24,6 @@
 class btSimulationIslandManager;
 class btTypedConstraint;
+class btActionInterface;
 
-
-class btRaycastVehicle;
-class btCharacterControllerInterface;
 class btIDebugDraw;
 #include "LinearMath/btAlignedObjectArray.h"
@@ -53,10 +51,6 @@
 	bool	m_ownsConstraintSolver;
 
+	btAlignedObjectArray<btActionInterface*>	m_actions;
 	
-	btAlignedObjectArray<btRaycastVehicle*>	m_vehicles;
-	
-	btAlignedObjectArray<btCharacterControllerInterface*> m_characters;
-	
-
 	int	m_profileTimings;
 
@@ -71,7 +65,5 @@
 	void	updateActivationState(btScalar timeStep);
 
-	void	updateVehicles(btScalar timeStep);
-
-	void	updateCharacters(btScalar timeStep);
+	void	updateActions(btScalar timeStep);
 
 	void	startProfiling(btScalar timeStep);
@@ -106,13 +98,8 @@
 	virtual void	removeConstraint(btTypedConstraint* constraint);
 
-	virtual void	addVehicle(btRaycastVehicle* vehicle);
+	virtual void	addAction(btActionInterface*);
 
-	virtual void	removeVehicle(btRaycastVehicle* vehicle);
+	virtual void	removeAction(btActionInterface*);
 	
-	virtual void	addCharacter(btCharacterControllerInterface* character);
-
-	virtual void	removeCharacter(btCharacterControllerInterface* character);
-		
-
 	btSimulationIslandManager*	getSimulationIslandManager()
 	{
@@ -131,4 +118,5 @@
 
 	virtual void	setGravity(const btVector3& gravity);
+
 	virtual btVector3 getGravity () const;
 
@@ -140,4 +128,6 @@
 
 	void	debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
+
+	void	debugDrawConstraint(btTypedConstraint* constraint);
 
 	virtual void	debugDrawWorld();
@@ -170,4 +160,19 @@
 	}
 
+	///obsolete, use updateActions instead
+	virtual void updateVehicles(btScalar timeStep)
+	{
+		updateActions(timeStep);
+	}
+
+	///obsolete, use addAction instead
+	virtual void	addVehicle(btActionInterface* vehicle);
+	///obsolete, use removeAction instead
+	virtual void	removeVehicle(btActionInterface* vehicle);
+	///obsolete, use addAction instead
+	virtual void	addCharacter(btActionInterface* character);
+	///obsolete, use removeAction instead
+	virtual void	removeCharacter(btActionInterface* character);
+
 };
 
Index: code/trunk/src/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h	(revision 2882)
@@ -21,8 +21,8 @@
 
 class btTypedConstraint;
-class btRaycastVehicle;
+class btActionInterface;
 class btConstraintSolver;
 class btDynamicsWorld;
-class btCharacterControllerInterface;
+
 
 /// Type for the callback for each tick
@@ -73,12 +73,7 @@
 		virtual void	removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
 
-		virtual void	addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
+		virtual void	addAction(btActionInterface* action) = 0;
 
-		virtual void	removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
-
-		virtual void	addCharacter(btCharacterControllerInterface* character) {(void)character;}
-
-		virtual void	removeCharacter(btCharacterControllerInterface* character) {(void)character;}
-
+		virtual void	removeAction(btActionInterface* action) = 0;
 
 		//once a rigidbody is added to the dynamics world, it will get this gravity assigned
@@ -130,4 +125,14 @@
 
 
+		///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;}
+
+
 };
 
Index: code/trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
===================================================================
--- code/trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.cpp	(revision 2882)
@@ -47,4 +47,5 @@
 	m_angularFactor = btScalar(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)),
@@ -125,4 +126,5 @@
 		m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
 	}
+	m_gravity_acceleration = acceleration;
 }
 
Index: code/trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.h	(revision 2882)
@@ -49,4 +49,5 @@
 
 	btVector3		m_gravity;	
+	btVector3		m_gravity_acceleration;
 	btVector3		m_invInertiaLocal;
 	btVector3		m_totalForce;
@@ -182,5 +183,5 @@
 	const btVector3&	getGravity() const
 	{
-		return m_gravity;
+		return m_gravity_acceleration;
 	}
 
@@ -232,6 +233,16 @@
 		m_totalForce += force;
 	}
+
+	const btVector3& getTotalForce()
+	{
+		return m_totalForce;
+	};
+
+	const btVector3& getTotalTorque()
+	{
+		return m_totalTorque;
+	};
     
-	const btVector3& getInvInertiaDiagLocal()
+	const btVector3& getInvInertiaDiagLocal() const
 	{
 		return m_invInertiaLocal;
Index: code/trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp
===================================================================
--- code/trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp	(revision 2882)
@@ -20,6 +20,5 @@
 #include "btWheelInfo.h"
 #include "LinearMath/btMinMax.h"
-
-
+#include "LinearMath/btIDebugDraw.h"
 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
 
@@ -27,6 +26,5 @@
 
 btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,	btVehicleRaycaster* raycaster )
-: btTypedConstraint(VEHICLE_CONSTRAINT_TYPE),
-m_vehicleRaycaster(raycaster),
+:m_vehicleRaycaster(raycaster),
 m_pitchControl(btScalar(0.))
 {
@@ -88,5 +86,5 @@
 const btTransform&	btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
 {
-	assert(wheelIndex < getNumWheels());
+	btAssert(wheelIndex < getNumWheels());
 	const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
 	return wheel.m_worldTransform;
@@ -176,5 +174,5 @@
 	btVehicleRaycaster::btVehicleRaycasterResult	rayResults;
 
-	assert(m_vehicleRaycaster);
+	btAssert(m_vehicleRaycaster);
 
 	void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
@@ -360,5 +358,5 @@
 void	btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
 {
-	assert(wheel>=0 && wheel < getNumWheels());
+	btAssert(wheel>=0 && wheel < getNumWheels());
 
 	btWheelInfo& wheelInfo = getWheelInfo(wheel);
@@ -376,5 +374,5 @@
 void	btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
 {
-	assert(wheel>=0 && wheel < getNumWheels());
+	btAssert(wheel>=0 && wheel < getNumWheels());
 	btWheelInfo& wheelInfo = getWheelInfo(wheel);
 	wheelInfo.m_engineForce = force;
@@ -705,4 +703,34 @@
 
 
+
+void	btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
+{
+
+	for (int v=0;v<this->getNumWheels();v++)
+	{
+		btVector3 wheelColor(0,255,255);
+		if (getWheelInfo(v).m_raycastInfo.m_isInContact)
+		{
+			wheelColor.setValue(0,0,255);
+		} else
+		{
+			wheelColor.setValue(255,0,255);
+		}
+
+		btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin();
+
+		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);
+
+	}
+}
+
+
 void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
 {
@@ -728,2 +756,3 @@
 	return 0;
 }
+
Index: code/trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h
===================================================================
--- code/trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h	(revision 2881)
+++ code/trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h	(revision 2882)
@@ -18,9 +18,10 @@
 #include "LinearMath/btAlignedObjectArray.h"
 #include "btWheelInfo.h"
+#include "BulletDynamics/Dynamics/btActionInterface.h"
 
 class btVehicleTuning;
 
 ///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
-class btRaycastVehicle : public btTypedConstraint
+class btRaycastVehicle : public btActionInterface
 {
 
@@ -74,5 +75,15 @@
 	virtual ~btRaycastVehicle() ;
 
-		
+
+	///btActionInterface interface
+	virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step)
+	{
+		updateVehicle(step);
+	}
+	
+
+	///btActionInterface interface
+	void	debugDraw(btIDebugDraw* debugDrawer);
+			
 	const btTransform& getChassisWorldTransform() const;
 	
@@ -80,5 +91,6 @@
 
 	virtual void updateVehicle(btScalar step);
-
+	
+	
 	void resetSuspension();
 
@@ -176,14 +188,4 @@
 	}
 
-	virtual void	buildJacobian()
-	{
-		//not yet
-	}
-
-	virtual	void	solveConstraint(btScalar	timeStep)
-	{
-		(void)timeStep;
-		//not yet
-	}
 
 
Index: code/trunk/src/bullet/LinearMath/btAlignedAllocator.cpp
===================================================================
--- code/trunk/src/bullet/LinearMath/btAlignedAllocator.cpp	(revision 2881)
+++ code/trunk/src/bullet/LinearMath/btAlignedAllocator.cpp	(revision 2882)
@@ -20,4 +20,19 @@
 int gTotalBytesAlignedAllocs = 0;//detect memory leaks
 
+static void *btAllocDefault(size_t size)
+{
+	return malloc(size);
+}
+
+static void btFreeDefault(void *ptr)
+{
+	free(ptr);
+}
+
+static btAllocFunc *sAllocFunc = btAllocDefault;
+static btFreeFunc *sFreeFunc = btFreeDefault;
+
+
+
 #if defined (BT_HAS_ALIGNED_ALLOCATOR)
 #include <malloc.h>
@@ -50,5 +65,5 @@
   unsigned long offset;
 
-  real = (char *)malloc(size + sizeof(void *) + (alignment-1));
+  real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1));
   if (real) {
     offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1);
@@ -67,23 +82,12 @@
   if (ptr) {
     real = *((void **)(ptr)-1);
-    free(real);
+    sFreeFunc(real);
   }
 }
 #endif
 
-static void *btAllocDefault(size_t size)
-{
-	return malloc(size);
-}
-
-static void btFreeDefault(void *ptr)
-{
-	free(ptr);
-}
 
 static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault;
 static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault;
-static btAllocFunc *sAllocFunc = btAllocDefault;
-static btFreeFunc *sFreeFunc = btFreeDefault;
 
 void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc)
Index: code/trunk/src/bullet/LinearMath/btAlignedAllocator.h
===================================================================
--- code/trunk/src/bullet/LinearMath/btAlignedAllocator.h	(revision 2881)
+++ code/trunk/src/bullet/LinearMath/btAlignedAllocator.h	(revision 2882)
@@ -39,5 +39,5 @@
 	void	btAlignedFreeInternal	(void* ptr);
 
-	#define btAlignedAlloc(a,b) btAlignedAllocInternal(a,b)
+	#define btAlignedAlloc(size,alignment) btAlignedAllocInternal(size,alignment)
 	#define btAlignedFree(ptr) btAlignedFreeInternal(ptr)
 
@@ -50,6 +50,9 @@
 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);
+///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 btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc);
+
 
 ///The btAlignedAllocator is a portable class for aligned memory allocations.
Index: code/trunk/src/bullet/LinearMath/btAlignedObjectArray.h
===================================================================
--- code/trunk/src/bullet/LinearMath/btAlignedObjectArray.h	(revision 2881)
+++ code/trunk/src/bullet/LinearMath/btAlignedObjectArray.h	(revision 2882)
@@ -59,5 +59,5 @@
 			return (size ? size*2 : 1);
 		}
-		SIMD_FORCE_INLINE	void	copy(int start,int end, T* dest)
+		SIMD_FORCE_INLINE	void	copy(int start,int end, T* dest) const
 		{
 			int i;
@@ -121,11 +121,19 @@
 		}
 
-		SIMD_FORCE_INLINE	int capacity() const
-		{	// return current length of allocated storage
-			return m_capacity;
-		}
-		
+		///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);
+		}
+
+		
+		
+		/// return the number of elements in the array
 		SIMD_FORCE_INLINE	int size() const
-		{	// return length of sequence
+		{	
 			return m_size;
 		}
@@ -142,4 +150,5 @@
 		
 
+		///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()
 		{
@@ -157,4 +166,6 @@
 		}
 
+		///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	resize(int newsize, const T& fillData=T())
 		{
@@ -220,4 +231,9 @@
 
 	
+		/// 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)
Index: code/trunk/src/bullet/LinearMath/btConvexHull.cpp
===================================================================
--- code/trunk/src/bullet/LinearMath/btConvexHull.cpp	(revision 2881)
+++ code/trunk/src/bullet/LinearMath/btConvexHull.cpp	(revision 2882)
@@ -263,6 +263,6 @@
 		for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
 		{
-			btScalar s = sinf(SIMD_RADS_PER_DEG*(x));
-			btScalar c = cosf(SIMD_RADS_PER_DEG*(x));
+			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)
@@ -276,6 +276,6 @@
 				for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0))
 				{
-					btScalar s = sinf(SIMD_RADS_PER_DEG*(xx));
-					btScalar c = cosf(SIMD_RADS_PER_DEG*(xx));
+					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)
Index: code/trunk/src/bullet/LinearMath/btIDebugDraw.h
===================================================================
--- code/trunk/src/bullet/LinearMath/btIDebugDraw.h	(revision 2881)
+++ code/trunk/src/bullet/LinearMath/btIDebugDraw.h	(revision 2882)
@@ -30,4 +30,5 @@
 
 #include "btVector3.h"
+#include "btTransform.h"
 
 
@@ -53,8 +54,23 @@
 		DBG_DisableBulletLCP = 512,
 		DBG_EnableCCD = 1024,
+		DBG_DrawConstraints = (1 << 11),
+		DBG_DrawConstraintLimits = (1 << 12),
 		DBG_MAX_DEBUG_DRAW_MODE
 	};
 
 	virtual ~btIDebugDraw() {};
+
+	virtual void    drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor)
+	{
+		drawLine (from, to, fromColor);
+	}
+
+	virtual void	drawBox (const btVector3& boxMin, const btVector3& boxMax, const btVector3& color, btScalar alpha)
+	{
+	}
+
+	virtual void	drawSphere (const btVector3& p, btScalar radius, const btVector3& color)
+	{
+	}
 
 	virtual void	drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
@@ -110,4 +126,169 @@
 		}
 	}
+	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));
+	}
+
+	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)((maxAngle - minAngle) / step);
+		if(!nSteps) nSteps = 1;
+		btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle);
+		if(drawSect)
+		{
+			drawLine(center, prev, color);
+		}
+		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)
+		{
+			drawLine(center, prev, color);
+		}
+	}
+	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))
+	{
+		btVector3 vA[74];
+		btVector3 vB[74];
+		btVector3 *pvA = vA, *pvB = vB, *pT;
+		btVector3 npole = center + up * radius;
+		btVector3 spole = center - up * radius;
+		btVector3 arcStart;
+		btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
+		const btVector3& kv = up;
+		const btVector3& iv = axis;
+		btVector3 jv = kv.cross(iv);
+		bool drawN = false;
+		bool drawS = false;
+		if(minTh <= -SIMD_HALF_PI)
+		{
+			minTh = -SIMD_HALF_PI + step;
+			drawN = true;
+		}
+		if(maxTh >= SIMD_HALF_PI)
+		{
+			maxTh = SIMD_HALF_PI - step;
+			drawS = true;
+		}
+		if(minTh > maxTh)
+		{
+			minTh = -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;
+		btScalar step_h = (maxTh - minTh) / btScalar(n_hor - 1);
+		bool isClosed = false;
+		if(minPs > maxPs)
+		{
+			minPs = -SIMD_PI + step;
+			maxPs =  SIMD_PI;
+			isClosed = true;
+		}
+		else if((maxPs - minPs) >= SIMD_PI * btScalar(2.f))
+		{
+			isClosed = true;
+		}
+		else
+		{
+			isClosed = false;
+		}
+		int n_vert = (int)((maxPs - minPs) / step) + 1;
+		if(n_vert < 2) n_vert = 2;
+		btScalar step_v = (maxPs - minPs) / btScalar(n_vert - 1);
+		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++)
+			{
+				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)
+				{
+					drawLine(pvA[j], pvB[j], color);
+				}
+				else if(drawS)
+				{
+					drawLine(spole, pvB[j], color);
+				}
+				if(j)
+				{
+					drawLine(pvB[j-1], pvB[j], color);
+				}
+				else
+				{
+					arcStart = pvB[j];
+				}
+				if((i == (n_hor - 1)) && drawN)
+				{
+					drawLine(npole, pvB[j], color);
+				}
+				if(isClosed)
+				{
+					if(j == (n_vert-1))
+					{
+						drawLine(arcStart, pvB[j], color);
+					}
+				}
+				else
+				{
+					if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1))))
+					{
+						drawLine(center, pvB[j], color);
+					}
+				}
+			}
+			pT = pvA; pvA = pvB; pvB = pT;
+		}
+	}
+	
+	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);
+		drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
+		drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
+		drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
+		drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+		drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+		drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+		drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+		drawLine(btVector3(bbMin[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+		drawLine(btVector3(bbMax[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+		drawLine(btVector3(bbMax[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+		drawLine(btVector3(bbMin[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+	}
+	void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color)
+	{
+		drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
+		drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
+		drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
+		drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
+		drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+		drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+		drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+		drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+		drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+		drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+		drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+		drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+	}
 };
 
Index: code/trunk/src/bullet/LinearMath/btMatrix3x3.h
===================================================================
--- code/trunk/src/bullet/LinearMath/btMatrix3x3.h	(revision 2881)
+++ code/trunk/src/bullet/LinearMath/btMatrix3x3.h	(revision 2882)
@@ -193,4 +193,13 @@
 					 btScalar(0.0), btScalar(0.0), btScalar(1.0)); 
 		}
+
+		static const btMatrix3x3&	getIdentity()
+		{
+			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));
+			return identityMatrix;
+		}
+
   /**@brief Fill the values of the matrix into a 9 element array 
    * @param m The array to be filled */
Index: code/trunk/src/bullet/LinearMath/btQuadWord.h
===================================================================
--- code/trunk/src/bullet/LinearMath/btQuadWord.h	(revision 2881)
+++ code/trunk/src/bullet/LinearMath/btQuadWord.h	(revision 2882)
@@ -25,11 +25,11 @@
 #endif
 
-/**@brief The btQuadWordStorage class is base class for btVector3 and btQuaternion. 
+/**@brief The btQuadWord class is base class for btVector3 and btQuaternion. 
  * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
  */
 #ifndef USE_LIBSPE2
-ATTRIBUTE_ALIGNED16(class) btQuadWordStorage
+ATTRIBUTE_ALIGNED16(class) btQuadWord
 #else
-class btQuadWordStorage
+class btQuadWord
 #endif
 {
@@ -46,13 +46,9 @@
 		return mVec128;
 	}
+protected:
 #else //__CELLOS_LV2__ __SPU__
 	btScalar	m_floats[4];
 #endif //__CELLOS_LV2__ __SPU__
 
-};
-
-/** @brief The btQuadWord is base-class for vectors, points */
-class	btQuadWord : public btQuadWordStorage
-{
 	public:
   
@@ -135,9 +131,5 @@
 		{
 		}
-  /**@brief Copy constructor */
-		SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q)
-		{
-			*((btQuadWordStorage*)this) = q;
-		}
+ 
   /**@brief Three argument constructor (zeros w)
    * @param x Value of x
Index: code/trunk/src/bullet/LinearMath/btQuaternion.h
===================================================================
--- code/trunk/src/bullet/LinearMath/btQuaternion.h	(revision 2881)
+++ code/trunk/src/bullet/LinearMath/btQuaternion.h	(revision 2882)
@@ -20,4 +20,5 @@
 
 #include "btVector3.h"
+#include "btQuadWord.h"
 
 /**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
@@ -58,5 +59,5 @@
 	{
 		btScalar d = axis.length();
-		assert(d != btScalar(0.0));
+		btAssert(d != btScalar(0.0));
 		btScalar s = btSin(angle * btScalar(0.5)) / d;
 		setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
@@ -177,5 +178,5 @@
 	btQuaternion operator/(const btScalar& s) const
 	{
-		assert(s != btScalar(0.0));
+		btAssert(s != btScalar(0.0));
 		return *this * (btScalar(1.0) / s);
 	}
@@ -185,5 +186,5 @@
 	btQuaternion& operator/=(const btScalar& s) 
 	{
-		assert(s != btScalar(0.0));
+		btAssert(s != btScalar(0.0));
 		return *this *= btScalar(1.0) / s;
 	}
@@ -199,5 +200,5 @@
 	{
 		btScalar s = btSqrt(length2() * q.length2());
-		assert(s != btScalar(0.0));
+		btAssert(s != btScalar(0.0));
 		return btAcos(dot(q) / s);
 	}
@@ -275,4 +276,10 @@
 	}
 
+	static const btQuaternion&	getIdentity()
+	{
+		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]; }
 
Index: code/trunk/src/bullet/LinearMath/btScalar.h
===================================================================
--- code/trunk/src/bullet/LinearMath/btScalar.h	(revision 2881)
+++ code/trunk/src/bullet/LinearMath/btScalar.h	(revision 2882)
@@ -26,5 +26,5 @@
 #include <float.h>
 
-#define BT_BULLET_VERSION 273
+#define BT_BULLET_VERSION 274
 
 inline int	btGetVersion()
@@ -46,5 +46,5 @@
 			#define ATTRIBUTE_ALIGNED128(a) a
 		#else
-			#define BT_HAS_ALIGNED_ALLOCATOR
+			//#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.
@@ -62,6 +62,12 @@
  			#define btFsel(a,b,c) __fsel((a),(b),(c))
 		#else
+
+#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
 			#define BT_USE_SSE
-		#endif
+			#include <emmintrin.h>
+#endif
+
+		#endif//_XBOX
+
 		#endif //__MINGW32__
 
@@ -125,4 +131,7 @@
 
 		#define SIMD_FORCE_INLINE inline
+		///@todo: check out alignment methods for other platforms/compilers
+		///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+		///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
 		#define ATTRIBUTE_ALIGNED16(a) a
 		#define ATTRIBUTE_ALIGNED128(a) a
@@ -228,10 +237,6 @@
 SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
 SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
-  #if defined( __MINGW32__ )
-  SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
-  #else
-  SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
-  #endif
-
+SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
+	
 #endif
 
Index: code/trunk/src/bullet/LinearMath/btTransform.h
===================================================================
--- code/trunk/src/bullet/LinearMath/btTransform.h	(revision 2881)
+++ code/trunk/src/bullet/LinearMath/btTransform.h	(revision 2882)
@@ -191,9 +191,8 @@
 
   /**@brief Return an identity transform */
-	static btTransform	getIdentity()
-	{
-		btTransform tr;
-		tr.setIdentity();
-		return tr;
+	static const btTransform&	getIdentity()
+	{
+		static const btTransform identityTransform(btMatrix3x3::getIdentity());
+		return identityTransform;
 	}
 	
Index: code/trunk/src/bullet/LinearMath/btVector3.h
===================================================================
--- code/trunk/src/bullet/LinearMath/btVector3.h	(revision 2881)
+++ code/trunk/src/bullet/LinearMath/btVector3.h	(revision 2882)
@@ -18,22 +18,53 @@
 #define SIMD__VECTOR3_H
 
-#include "btQuadWord.h"
-
+
+#include "btScalar.h"
+#include "btScalar.h"
+#include "btMinMax.h"
 /**@brief btVector3 can be used to represent 3D points and vectors.
  * 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
  */
-class	btVector3 : public btQuadWord {
-
+
+ATTRIBUTE_ALIGNED16(class) btVector3
+{
 public:
+
+#if defined (__SPU__) && defined (__CELLOS_LV2__)
+	union {
+		vec_float4 mVec128;
+		btScalar	m_floats[4];
+	};
+public:
+	vec_float4	get128() const
+	{
+		return mVec128;
+	}
+public:
+#else //__CELLOS_LV2__ __SPU__
+#ifdef BT_USE_SSE // WIN32
+	union {
+		__m128 mVec128;
+		btScalar	m_floats[4];
+	};
+	SIMD_FORCE_INLINE	__m128	get128() const
+	{
+		return mVec128;
+	}
+	SIMD_FORCE_INLINE	void	set128(__m128 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 btQuadWordStorage (btVector3 inherits from this so is also valid)
-   * Note: Vector3 derives from btQuadWordStorage*/
-	SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q) 
-		: btQuadWord(q)
-	{
-	}
+ 
 	
   /**@brief Constructor from scalars 
@@ -42,13 +73,11 @@
    * @param z Z value 
    */
-	SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) 
-		:btQuadWord(x,y,z,btScalar(0.))
-	{
-	}
-
-//	SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 
-//		: btQuadWord(x,y,z,w)
-//	{
-//	}
+	SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
+	{
+		m_floats[0] = x;
+		m_floats[1] = y;
+		m_floats[2] = z;
+		m_floats[3] = btScalar(0.);
+	}
 
 	
@@ -58,5 +87,5 @@
 	{
 
-		m_floats[0] += v.x(); m_floats[1] += v.y(); m_floats[2] += v.z();
+		m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2];
 		return *this;
 	}
@@ -67,5 +96,5 @@
 	SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) 
 	{
-		m_floats[0] -= v.x(); m_floats[1] -= v.y(); m_floats[2] -= v.z();
+		m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2];
 		return *this;
 	}
@@ -74,5 +103,5 @@
 	SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
 	{
-		m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s;
+		m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s;
 		return *this;
 	}
@@ -90,5 +119,5 @@
 	SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
 	{
-		return m_floats[0] * v.x() + m_floats[1] * v.y() + m_floats[2] * v.z();
+		return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2];
 	}
 
@@ -149,14 +178,14 @@
 	{
 		return btVector3(
-			m_floats[1] * v.z() - m_floats[2] * v.y(),
-			m_floats[2] * v.x() - m_floats[0] * v.z(),
-			m_floats[0] * v.y() - m_floats[1] * v.x());
+			m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1],
+			m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
+			m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
 	}
 
 	SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
 	{
-		return m_floats[0] * (v1.y() * v2.z() - v1.z() * v2.y()) + 
-			m_floats[1] * (v1.z() * v2.x() - v1.x() * v2.z()) + 
-			m_floats[2] * (v1.x() * v2.y() - v1.y() * v2.x());
+		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]);
 	}
 
@@ -165,5 +194,5 @@
 	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);
 	}
 
@@ -172,5 +201,5 @@
 	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);
 	}
 
@@ -188,7 +217,7 @@
 	{
 		btScalar s = btScalar(1.0) - rt;
-		m_floats[0] = s * v0.x() + rt * v1.x();
-		m_floats[1] = s * v0.y() + rt * v1.y();
-		m_floats[2] = s * v0.z() + rt * v1.z();
+		m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
+		m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
+		m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
 		//don't do the unused w component
 		//		m_co[3] = s * v0[3] + rt * v1[3];
@@ -200,7 +229,7 @@
 	SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const 
 	{
-		return btVector3(m_floats[0] + (v.x() - m_floats[0]) * t,
-			m_floats[1] + (v.y() - m_floats[1]) * t,
-			m_floats[2] + (v.z() - m_floats[2]) * t);
+		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);
 	}
 
@@ -209,9 +238,82 @@
 	SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
 	{
-		m_floats[0] *= v.x(); m_floats[1] *= v.y(); m_floats[2] *= v.z();
+		m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2];
 		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];	}      
+	//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	bool	operator==(const btVector3& other) const
+	{
+		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]));
+	}
+
+	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
+   * @param other The other btVector3 to compare with 
+   */
+		SIMD_FORCE_INLINE void	setMax(const btVector3& other)
+		{
+			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.w());
+		}
+  /**@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)
+		{
+			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.w());
+		}
+
+		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	getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const
+		{
+			v0->setValue(0.		,-z()		,y());
+			v1->setValue(z()	,0.			,-x());
+			v2->setValue(-y()	,x()	,0.);
+		}
 
 };
@@ -221,5 +323,5 @@
 operator+(const btVector3& v1, const btVector3& v2) 
 {
-	return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z());
+	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]);
 }
 
@@ -228,5 +330,5 @@
 operator*(const btVector3& v1, const btVector3& v2) 
 {
-	return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z());
+	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]);
 }
 
@@ -235,5 +337,5 @@
 operator-(const btVector3& v1, const btVector3& v2)
 {
-	return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
+	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]);
 }
 /**@brief Return the negative of the vector */
@@ -241,5 +343,5 @@
 operator-(const btVector3& v)
 {
-	return btVector3(-v.x(), -v.y(), -v.z());
+	return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
 }
 
@@ -248,5 +350,5 @@
 operator*(const btVector3& v, const btScalar& s)
 {
-	return btVector3(v.x() * s, v.y() * s, v.z() * s);
+	return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
 }
 
@@ -270,5 +372,5 @@
 operator/(const btVector3& v1, const btVector3& v2)
 {
-	return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z());
+	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]);
 }
 
@@ -326,9 +428,5 @@
 }
 
-/**@brief Test if each element of the vector is equivalent */
-SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) 
-{
-	return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z();
-}
+
 
 SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
@@ -405,5 +503,5 @@
 		{
 			maxIndex = 2;
-			maxVal = m_floats[2];
+			maxVal =m_floats[2];
 		}
 		if (m_floats[3] > maxVal)
@@ -438,5 +536,5 @@
 		{
 			minIndex = 2;
-			minVal = m_floats[2];
+			minVal =m_floats[2];
 		}
 		if (m_floats[3] < minVal)
@@ -455,4 +553,38 @@
 		return absolute4().maxAxis4();
 	}
+
+	
+ 
+
+  /**@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 
+		{
+			m[0] = m_floats[0];
+			m[1] = m_floats[1];
+			m[2] =m_floats[2];
+		}
+*/
+/**@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;
+		}
+
+
+ 
 
 };
