Bullet: added per-contact point user-cache, + callback. This allows Collision Detection to be used more generically, and still have persistent info stored, like contact constraints.

Bullet: added support for restitution

17 files changed, 323 insertions, 95 deletions

diff --git a/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h b/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h
index 579731569f8..7f8351bcd93 100644
--- a/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h
+++ b/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h
@@ -76,6 +76,8 @@ public:
 
 	virtual void ReleaseManifold(PersistentManifold* manifold)=0;
 
+	virtual void ClearManifold(PersistentManifold* manifold)=0;
+
 	virtual bool	NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) = 0;
 
 	virtual	ManifoldResult* GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold) =0;
diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp
index 5fe5157b420..0c746781758 100644
--- a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp
+++ b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp
@@ -25,6 +25,8 @@ subject to the following restrictions:
 #include "CollisionDispatch/CollisionObject.h"
 #include <algorithm>
 
+int gNumManifold = 0;
+
 void CollisionDispatcher::FindUnions()
 {
 	if (m_useIslands)
@@ -76,6 +78,8 @@ CollisionDispatcher::CollisionDispatcher ():
 	
 PersistentManifold*	CollisionDispatcher::GetNewManifold(void* b0,void* b1) 
 { 
+	gNumManifold++;
+	//printf("GetNewManifoldResult: gNumManifold %d\n",gNumManifold);
 
 	CollisionObject* body0 = (CollisionObject*)b0;
 	CollisionObject* body1 = (CollisionObject*)b1;
@@ -86,10 +90,20 @@ PersistentManifold*	CollisionDispatcher::GetNewManifold(void* b0,void* b1)
 	return manifold;
 }
 
+void CollisionDispatcher::ClearManifold(PersistentManifold* manifold)
+{
+	manifold->ClearManifold();
+}
+
 	
 void CollisionDispatcher::ReleaseManifold(PersistentManifold* manifold)
 {
-	manifold->ClearManifold();
+	
+	gNumManifold--;
+
+	//printf("ReleaseManifold: gNumManifold %d\n",gNumManifold);
+
+	ClearManifold(manifold);
 
 	std::vector<PersistentManifold*>::iterator i =
 		std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold);
@@ -272,6 +286,8 @@ bool	CollisionDispatcher::NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy
 ///allows the user to get contact point callbacks 
 ManifoldResult*	CollisionDispatcher::GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold)
 {
+
+
 	//in-place, this prevents parallel dispatching, but just adding a list would fix that.
 	ManifoldResult* manifoldResult = new (&m_defaultManifoldResult)	ManifoldResult(obj0,obj1,manifold);
 	return manifoldResult;
diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h
index e3f3c09e76d..3896c19e161 100644
--- a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h
+++ b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h
@@ -113,6 +113,9 @@ public:
 
 	///allows the user to get contact point callbacks 
 	virtual	void	ReleaseManifoldResult(ManifoldResult*);
+
+	virtual void ClearManifold(PersistentManifold* manifold);
+
 			
 	CollisionAlgorithm* FindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
 	{
diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp
index 4775db1b1c1..1c057412962 100644
--- a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp
+++ b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp
@@ -27,6 +27,31 @@ subject to the following restrictions:
 
 #include <algorithm>
 
+CollisionWorld::~CollisionWorld()
+{
+	//clean up remaining objects
+	std::vector<CollisionObject*>::iterator i;
+
+	int index = 0;
+	for (i=m_collisionObjects.begin();
+	!(i==m_collisionObjects.end()); i++)
+
+	{
+		CollisionObject* collisionObject= (*i);
+		
+		BroadphaseProxy* bp = collisionObject->m_broadphaseHandle;
+		if (bp)
+		{
+			//
+			// only clear the cached algorithms
+			//
+			GetBroadphase()->CleanProxyFromPairs(bp);
+			GetBroadphase()->DestroyProxy(bp);
+		}
+	}
+
+}
+
 void	CollisionWorld::UpdateActivationState()
 {
 	m_dispatcher->InitUnionFind(m_collisionObjects.size());
diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h
index 1d1249ec88e..611ee9616fc 100644
--- a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h
+++ b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h
@@ -46,7 +46,7 @@ public:
 	{
 
 	}
-	virtual ~CollisionWorld() {}
+	virtual ~CollisionWorld();
 
 	virtual	void	UpdateActivationState();
 
diff --git a/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp b/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp
index 7032a863a54..a72432ef952 100644
--- a/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp
+++ b/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp
@@ -62,8 +62,7 @@ BoxTriangleCallback::~BoxTriangleCallback()
 
 void	BoxTriangleCallback::ClearCache()
 {
-
-	m_manifoldPtr->ClearManifold();
+	m_dispatcher->ClearManifold(m_manifoldPtr);
 };
 
 
@@ -161,7 +160,7 @@ void ConvexConcaveCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,Broadp
 					
 			m_boxTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle,dispatchInfo);
 #ifdef USE_BOX_TRIANGLE
-			m_boxTriangleCallback.m_manifoldPtr->ClearManifold();
+			m_dispatcher->ClearManifold(m_boxTriangleCallback.m_manifoldPtr);
 #endif
 			m_boxTriangleCallback.m_manifoldPtr->SetBodies(m_convex.m_clientObject,m_concave.m_clientObject);
 
diff --git a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp
index 6963cf23548..c3ca2ef6be9 100644
--- a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp
+++ b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp
@@ -282,7 +282,7 @@ void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy
 				Transform trB = GetTransformFromSimdTransform(col1->m_worldTransform);
 
 				//either use persistent manifold or clear it every time
-				m_manifoldPtr->ClearManifold();
+				m_dispatcher->ClearManifold(m_manifoldPtr);
 				ManifoldResult* resultOut = m_dispatcher->GetNewManifoldResult(col0,col1,m_manifoldPtr);
 
 				ManifoldResultCollector hullContactCollector(*resultOut);
@@ -339,6 +339,7 @@ bool disableCcd = false;
 float	ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo)
 {
 
+
 	CheckPenetrationDepthSolver();
 
 	//An adhoc way of testing the Continuous Collision Detection algorithms
diff --git a/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.cpp b/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.cpp
index ccd9a4b2cbc..15b6512a538 100644
--- a/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.cpp
+++ b/extern/bullet/Bullet/CollisionDispatch/ManifoldResult.cpp
@@ -25,6 +25,7 @@ ManifoldResult::ManifoldResult(CollisionObject* body0,CollisionObject* body1,Per
 	{
 	}
 
+
 void ManifoldResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
 {
 	if (depth > m_manifoldPtr->GetContactBreakingTreshold())
@@ -42,9 +43,13 @@ void ManifoldResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const S
 	int insertIndex = m_manifoldPtr->GetCacheEntry(newPt);
 	if (insertIndex >= 0)
 	{
-		const ManifoldPoint& oldPoint = m_manifoldPtr->GetContactPoint(insertIndex);
-		newPt.CopyPersistentInformation(oldPoint);
-		m_manifoldPtr->ReplaceContactPoint(newPt,insertIndex);
+
+// This is not needed, just use the old info!
+//		const ManifoldPoint& oldPoint = m_manifoldPtr->GetContactPoint(insertIndex);
+//		newPt.CopyPersistentInformation(oldPoint);
+//		m_manifoldPtr->ReplaceContactPoint(newPt,insertIndex);
+
+
 	} else
 	{
 		m_manifoldPtr->AddManifoldPoint(newPt);
diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp
index 9ed5f240b9f..a34add4c04f 100644
--- a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp
+++ b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp
@@ -41,11 +41,6 @@ ConvexTriangleCallback::~ConvexTriangleCallback()
 }
   
 
-void	ConvexTriangleCallback::ClearCache()
-{
-	m_manifoldPtr->ClearManifold();
-};
-
 
 
 void ConvexTriangleCallback::ProcessTriangle(SimdVector3* triangle)
diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h
index 17be96fc27e..9160d1f3226 100644
--- a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h
+++ b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h
@@ -47,7 +47,6 @@ int	m_triangleCount;
 
 	virtual void ProcessTriangle(SimdVector3* triangle);
 	
-	void ClearCache();
 
 	inline const SimdVector3& GetAabbMin() const
 	{
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h b/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h
index 000dc4c1057..35ebc4a40c6 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h
@@ -20,12 +20,16 @@ subject to the following restrictions:
 #include "SimdTransformUtil.h"
 
 
+
+
+
 /// ManifoldContactPoint collects and maintains persistent contactpoints.
 /// used to improve stability and performance of rigidbody dynamics response.
 class ManifoldPoint
 	{
 		public:
 			ManifoldPoint()
+				:m_userPersistentData(0)
 			{
 			}
 
@@ -36,51 +40,26 @@ class ManifoldPoint
 					m_localPointB( pointB ), 
 					m_normalWorldOnB( normal ), 
 					m_distance1( distance ),
-					m_appliedImpulse(0.f),
-					m_prevAppliedImpulse(0.f),
-					m_accumulatedTangentImpulse0(0.f),
-					m_accumulatedTangentImpulse1(0.f),
-					m_jacDiagABInv(0.f),
+					m_userPersistentData(0),					
 					m_lifeTime(0)
 			{
-				SimdPlaneSpace1(m_normalWorldOnB,m_frictionWorldTangential0,m_frictionWorldTangential1);
+				
 					
 			}
 
+			
+
 			SimdVector3 m_localPointA;			
 			SimdVector3 m_localPointB;			
 			SimdVector3	m_positionWorldOnB;
 			///m_positionWorldOnA is redundant information, see GetPositionWorldOnA(), but for clarity
 			SimdVector3	m_positionWorldOnA;
 			SimdVector3 m_normalWorldOnB;
-			
-			SimdVector3	m_frictionWorldTangential0;
-			SimdVector3	m_frictionWorldTangential1;
-			
-			
-
-
+		
 			float	m_distance1;
-			/// total applied impulse during most recent frame
-			float	m_appliedImpulse;
-			float	m_prevAppliedImpulse;
-			float	m_accumulatedTangentImpulse0;
-			float	m_accumulatedTangentImpulse1;
-			
-			float	m_jacDiagABInv;
-			float	m_jacDiagABInvTangent0;
-			float	m_jacDiagABInvTangent1;
-
 
-			void	CopyPersistentInformation(const ManifoldPoint& otherPoint)
-			{
-				m_appliedImpulse = otherPoint.m_appliedImpulse;
-				m_accumulatedTangentImpulse0 = otherPoint.m_accumulatedTangentImpulse0;
-				m_accumulatedTangentImpulse1 = otherPoint.m_accumulatedTangentImpulse1;
-				m_prevAppliedImpulse = otherPoint.m_prevAppliedImpulse;
-				m_lifeTime = otherPoint.m_lifeTime;
-
-			}
+				
+			void*	m_userPersistentData;
 
 			int		m_lifeTime;//lifetime of the contactpoint in frames
 			
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp
index 7aa225cdce2..df72b9a0417 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp
@@ -18,7 +18,9 @@ subject to the following restrictions:
 #include "SimdTransform.h"
 #include <assert.h>
 
-float gContactBreakingTreshold = 0.02f;
+float						gContactBreakingTreshold = 0.02f;
+ContactDestroyedCallback	gContactCallback = 0;
+
 
 PersistentManifold::PersistentManifold()
 :m_body0(0),
@@ -31,9 +33,61 @@ m_index1(0)
 
 void	PersistentManifold::ClearManifold()
 {
+	int i;
+	for (i=0;i<m_cachedPoints;i++)
+	{
+		ClearUserCache(m_pointCache[i]);
+	}
 	m_cachedPoints = 0;
 }
 
+#ifdef DEBUG_PERSISTENCY
+#include <stdio.h>
+void	PersistentManifold::DebugPersistency()
+{
+	int i;
+	printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
+	for (i=0;i<m_cachedPoints;i++)
+	{
+		printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
+	}
+}
+#endif //DEBUG_PERSISTENCY
+
+void PersistentManifold::ClearUserCache(ManifoldPoint& pt)
+{
+
+	void* oldPtr = pt.m_userPersistentData;
+	if (oldPtr)
+	{
+#ifdef DEBUG_PERSISTENCY
+		int i;
+		int occurance = 0;
+		for (i=0;i<m_cachedPoints;i++)
+		{
+			if (m_pointCache[i].m_userPersistentData == oldPtr)
+			{
+				occurance++;
+				if (occurance>1)
+					printf("error in ClearUserCache\n");
+			}
+		}
+		assert(occurance<=0);
+#endif //DEBUG_PERSISTENCY
+
+		if (pt.m_userPersistentData && gContactCallback)
+		{
+			(*gContactCallback)(pt.m_userPersistentData);
+			pt.m_userPersistentData = 0;
+		}
+		
+#ifdef DEBUG_PERSISTENCY
+		DebugPersistency();
+#endif
+	}
+
+	
+}
 
 
 
@@ -114,9 +168,12 @@ void PersistentManifold::AddManifoldPoint(const ManifoldPoint& newPoint)
 		insertIndex = 0;
 #endif
 
+		
 	} else
 	{
 		m_cachedPoints++;
+
+		
 	}
 	ReplaceContactPoint(newPoint,insertIndex);
 }
@@ -163,6 +220,9 @@ void PersistentManifold::RefreshContactPoints(const SimdTransform& trA,const Sim
 			}
 		}
 	}
+#ifdef DEBUG_PERSISTENCY
+	DebugPersistency();
+#endif //
 }
 
 
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h
index 235f604a2f8..98bef621f60 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h
@@ -26,6 +26,12 @@ struct CollisionResult;
 ///contact breaking and merging treshold
 extern float gContactBreakingTreshold;
 
+typedef bool (*ContactDestroyedCallback)(void* userPersistentData);
+
+extern ContactDestroyedCallback	gContactCallback;
+
+
+
 #define MANIFOLD_CACHE_SIZE 4
 
 ///PersistentManifold maintains contact points, and reduces them to 4
@@ -69,8 +75,12 @@ public:
 		m_body1 = body1;
 	}
 
-	
+	void ClearUserCache(ManifoldPoint& pt);
 
+#ifdef DEBUG_PERSISTENCY
+	void	DebugPersistency();
+#endif //
+	
 	inline int	GetNumContacts() const { return m_cachedPoints;}
 
 	inline const ManifoldPoint& GetContactPoint(int index) const
@@ -94,12 +104,20 @@ public:
 
 	void RemoveContactPoint (int index)
 	{
-		m_pointCache[index] = m_pointCache[GetNumContacts() - 1];
+		ClearUserCache(m_pointCache[index]);
+
+		int lastUsedIndex = GetNumContacts() - 1;
+		m_pointCache[index] = m_pointCache[lastUsedIndex];
+		//get rid of duplicated userPersistentData pointer
+		m_pointCache[lastUsedIndex].m_userPersistentData = 0;
 		m_cachedPoints--;
 	}
 	void ReplaceContactPoint(const ManifoldPoint& newPoint,int insertIndex)
 	{
 		assert(ValidContactDistance(newPoint));
+
+		ClearUserCache(m_pointCache[insertIndex]);
+		
 		m_pointCache[insertIndex] = newPoint;
 	}
 
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp
index f2b0a905491..e55c18e4716 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp
@@ -25,17 +25,12 @@ subject to the following restrictions:
 #define ASSERT2 assert
 
 //some values to find stable tresholds
-static SimdScalar ContactThreshold = -10.0f;  
+
 float useGlobalSettingContacts = false;//true;
 SimdScalar contactDamping = 0.2f;
 SimdScalar contactTau = .02f;//0.02f;//*0.02f;
 
 
-SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution)
-{
-	return 0.f;
-	//return restitution * GEN_min(1.0f, rel_vel / ContactThreshold);
-}
 
 
 SimdScalar	calculateCombinedFriction(RigidBody& body0,RigidBody& body1)
@@ -118,8 +113,7 @@ float resolveSingleCollision(
 
 	const SimdVector3& pos1 = contactPoint.GetPositionWorldOnA();
 	const SimdVector3& pos2 = contactPoint.GetPositionWorldOnB();
-    SimdScalar distance = contactPoint.GetDistance();
-
+    
 	
 //	printf("distance=%f\n",distance);
 
@@ -136,7 +130,7 @@ float resolveSingleCollision(
 	
 	float combinedRestitution = body1.getRestitution() * body2.getRestitution();
 
-	SimdScalar restitution = restitutionCurve(rel_vel, combinedRestitution);
+	
 
 	SimdScalar Kfps = 1.f / solverInfo.m_timeStep ;
 
@@ -153,24 +147,30 @@ float resolveSingleCollision(
 
 	//printf("dist=%f\n",distance);
 
+		ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+
+	SimdScalar distance = cpd->m_penetration;//contactPoint.GetDistance();
+	
 
 	//distance = 0.f;
 	SimdScalar positionalError = Kcor *-distance;
 	//jacDiagABInv;
-	SimdScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping;
+	SimdScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
 
-	SimdScalar penetrationImpulse = positionalError * contactPoint.m_jacDiagABInv;
+	
+	SimdScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
 
-	SimdScalar	velocityImpulse = velocityError * contactPoint.m_jacDiagABInv;
+	SimdScalar	velocityImpulse = velocityError * cpd->m_jacDiagABInv;
 
 	SimdScalar normalImpulse = penetrationImpulse+velocityImpulse;
 	
 	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-	float oldNormalImpulse = contactPoint.m_appliedImpulse;
+	float oldNormalImpulse = cpd->m_appliedImpulse;
 	float sum = oldNormalImpulse + normalImpulse;
-	contactPoint.m_appliedImpulse = 0.f > sum ? 0.f: sum;
+	cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum;
 
-	normalImpulse = contactPoint.m_appliedImpulse - oldNormalImpulse;
+	normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
 
 	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
 	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
@@ -194,8 +194,11 @@ float resolveSingleFriction(
 	SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
 	SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
 	float combinedFriction = calculateCombinedFriction(body1,body2);
-	
-	SimdScalar limit = contactPoint.m_appliedImpulse * combinedFriction;
+
+	ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+
+	SimdScalar limit = cpd->m_appliedImpulse * combinedFriction;
 	//if (contactPoint.m_appliedImpulse>0.f)
 	//friction
 	{
@@ -208,17 +211,17 @@ float resolveSingleFriction(
 			SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
 			SimdVector3 vel = vel1 - vel2;
 			
-			SimdScalar vrel = contactPoint.m_frictionWorldTangential0.dot(vel);
+			SimdScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
 
 			// calculate j that moves us to zero relative velocity
-			SimdScalar j = -vrel * contactPoint.m_jacDiagABInvTangent0;
-			float total = contactPoint.m_accumulatedTangentImpulse0 + j;
+			SimdScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
+			float total = cpd->m_accumulatedTangentImpulse0 + j;
 			GEN_set_min(total, limit);
 			GEN_set_max(total, -limit);
-			j = total - contactPoint.m_accumulatedTangentImpulse0;
-			contactPoint.m_accumulatedTangentImpulse0 = total;
-			body1.applyImpulse(j * contactPoint.m_frictionWorldTangential0, rel_pos1);
-			body2.applyImpulse(j * -contactPoint.m_frictionWorldTangential0, rel_pos2);
+			j = total - cpd->m_accumulatedTangentImpulse0;
+			cpd->m_accumulatedTangentImpulse0 = total;
+			body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
+			body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
 		}
 
 				
@@ -228,18 +231,18 @@ float resolveSingleFriction(
 			SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
 			SimdVector3 vel = vel1 - vel2;
 
-			SimdScalar vrel = contactPoint.m_frictionWorldTangential1.dot(vel);
+			SimdScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
 			
 			// calculate j that moves us to zero relative velocity
-			SimdScalar j = -vrel * contactPoint.m_jacDiagABInvTangent1;
-			float total = contactPoint.m_accumulatedTangentImpulse1 + j;
+			SimdScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
+			float total = cpd->m_accumulatedTangentImpulse1 + j;
 			GEN_set_min(total, limit);
 			GEN_set_max(total, -limit);
-			j = total - contactPoint.m_accumulatedTangentImpulse1;
-			contactPoint.m_accumulatedTangentImpulse1 = total;
-			body1.applyImpulse(j * contactPoint.m_frictionWorldTangential1, rel_pos1);
-			body2.applyImpulse(j * -contactPoint.m_frictionWorldTangential1, rel_pos2);
+			j = total - cpd->m_accumulatedTangentImpulse1;
+			cpd->m_accumulatedTangentImpulse1 = total;
+			body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
+			body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
 		}
 	} 
-	return contactPoint.m_appliedImpulse;
+	return cpd->m_appliedImpulse;
 }
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h
index 8de9f8e30c4..847f2881f41 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h
@@ -24,6 +24,39 @@ class RigidBody;
 struct ContactSolverInfo;
 class ManifoldPoint;
 
+struct ConstraintPersistentData
+{
+	inline ConstraintPersistentData()
+	:m_appliedImpulse(0.f),
+	m_prevAppliedImpulse(0.f),
+	m_accumulatedTangentImpulse0(0.f),
+	m_accumulatedTangentImpulse1(0.f),
+	m_jacDiagABInv(0.f),
+	m_persistentLifeTime(0),
+	m_restitution(0.f),
+	m_penetration(0.f)
+	{
+	}
+	
+					
+				/// total applied impulse during most recent frame
+			float	m_appliedImpulse;
+			float	m_prevAppliedImpulse;
+			float	m_accumulatedTangentImpulse0;
+			float	m_accumulatedTangentImpulse1;
+			
+			float	m_jacDiagABInv;
+			float	m_jacDiagABInvTangent0;
+			float	m_jacDiagABInvTangent1;
+			int		m_persistentLifeTime;
+			float	m_restitution;
+			float	m_penetration;
+			SimdVector3	m_frictionWorldTangential0;
+			SimdVector3	m_frictionWorldTangential1;
+
+
+};
+
 ///bilateral constraint between two dynamic objects
 ///positive distance = separation, negative distance = penetration
 void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp
index be1025bb994..9f7d360f7eb 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp
@@ -30,9 +30,31 @@ subject to the following restrictions:
 #include "quickprof.h"
 #endif //USE_PROFILE
 
+int totalCpd = 0;
+
+
+
+bool  MyContactDestroyedCallback(void* userPersistentData)
+{
+	assert (userPersistentData);
+	ConstraintPersistentData* cpd = (ConstraintPersistentData*)userPersistentData;
+	delete cpd;
+	totalCpd--;
+	//printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
+	return true;
+}
+
+
+SimpleConstraintSolver::SimpleConstraintSolver()
+{
+	gContactCallback = &MyContactDestroyedCallback;
+}
+
+
 /// SimpleConstraintSolver Sequentially applies impulses
 float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
 {
+	
 	ContactSolverInfo info = infoGlobal;
 
 	int numiter = infoGlobal.m_numIterations;
@@ -82,6 +104,12 @@ float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int n
 
 
 float penetrationResolveFactor = 0.9f;
+SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution)
+{
+	SimdScalar rest = restitution * -rel_vel;
+	return rest;
+}
+
 
 
 float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
@@ -111,48 +139,108 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta
 				SimdVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); 
 				SimdVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
 				
+
 				//this jacobian entry is re-used for all iterations
 				JacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
 					body1->getCenterOfMassTransform().getBasis().transpose(),
 					rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
 					body1->getInvInertiaDiagLocal(),body1->getInvMass());
 
+				
 				SimdScalar jacDiagAB = jac.getDiagonal();
+
+				ConstraintPersistentData* cpd = (ConstraintPersistentData*) 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) ConstraintPersistentData;
+						cpd->m_persistentLifeTime = cp.GetLifeTime();
+
+					} else
+					{
+						//printf("Persistent: cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.GetLifeTime());
+						
+					}
+				} else
+				{
+						
+					cpd = new ConstraintPersistentData();
+					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 = 1.f / jacDiagAB;
+
+
+				SimdVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
+				SimdVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
+				SimdVector3 vel = vel1 - vel2;
+				SimdScalar rel_vel;
+				rel_vel = cp.m_normalWorldOnB.dot(vel);
 				
-				cp.m_jacDiagABInv = 1.f / jacDiagAB;
+				float combinedRestitution = body0->getRestitution() * body1->getRestitution();
+
+				cpd->m_penetration = cp.GetDistance();
 
+				cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
+				if (cpd->m_restitution <= 0.) //0.f)
+				{
+					cpd->m_restitution = 0.0f;
+
+				};
+				
+				//restitution and penetration work in same direction so
+				//rel_vel 
+				
+				SimdScalar penVel = -cpd->m_penetration/info.m_timeStep;
+
+				if (cpd->m_restitution >= penVel)
+				{
+					cpd->m_penetration = 0.f;
+				} 				
 				
 
 				float relaxation = info.m_damping;
-				cp.m_appliedImpulse *= relaxation;
+				cpd->m_appliedImpulse *= relaxation;
 				//for friction
-				cp.m_prevAppliedImpulse = cp.m_appliedImpulse;
+				cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
 				
 				//re-calculate friction direction every frame, todo: check if this is really needed
-				SimdPlaneSpace1(cp.m_normalWorldOnB,cp.m_frictionWorldTangential0,cp.m_frictionWorldTangential1);
+				SimdPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
+
+
 #define NO_FRICTION_WARMSTART 1
 
 	#ifdef NO_FRICTION_WARMSTART
-				cp.m_accumulatedTangentImpulse0 = 0.f;
-				cp.m_accumulatedTangentImpulse1 = 0.f;
+				cpd->m_accumulatedTangentImpulse0 = 0.f;
+				cpd->m_accumulatedTangentImpulse1 = 0.f;
 	#endif //NO_FRICTION_WARMSTART
-				float denom0 = body0->ComputeImpulseDenominator(pos1,cp.m_frictionWorldTangential0);
-				float denom1 = body1->ComputeImpulseDenominator(pos2,cp.m_frictionWorldTangential0);
+				float denom0 = body0->ComputeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
+				float denom1 = body1->ComputeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
 				float denom = relaxation/(denom0+denom1);
-				cp.m_jacDiagABInvTangent0 = denom;
+				cpd->m_jacDiagABInvTangent0 = denom;
 
 
-				denom0 = body0->ComputeImpulseDenominator(pos1,cp.m_frictionWorldTangential1);
-				denom1 = body1->ComputeImpulseDenominator(pos2,cp.m_frictionWorldTangential1);
+				denom0 = body0->ComputeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
+				denom1 = body1->ComputeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
 				denom = relaxation/(denom0+denom1);
-				cp.m_jacDiagABInvTangent1 = denom;
+				cpd->m_jacDiagABInvTangent1 = denom;
 
 				SimdVector3 totalImpulse = 
 	#ifndef NO_FRICTION_WARMSTART
 					cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+
 					cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+
 	#endif //NO_FRICTION_WARMSTART
-					cp.m_normalWorldOnB*cp.m_appliedImpulse;
+					cp.m_normalWorldOnB*cpd->m_appliedImpulse;
 
 				//apply previous frames impulse on both bodies
 				body0->applyImpulse(totalImpulse, rel_pos1);
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h
index e58f8020a7a..99b77623b4b 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h
@@ -31,6 +31,8 @@ class SimpleConstraintSolver : public ConstraintSolver
 
 public:
 
+	SimpleConstraintSolver();
+
 	virtual ~SimpleConstraintSolver() {}
 	
 	virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info, IDebugDraw* debugDrawer=0);