Nasty merge from trunk's r18216.

23 files changed, 409 insertions, 205 deletions

diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
index 2c565734e97..6551cfb92fe 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
@@ -16,7 +16,6 @@ subject to the following restrictions:
 
 #include "btContinuousConvexCollision.h"
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
-#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
 #include "LinearMath/btTransformUtil.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
@@ -26,7 +25,7 @@ subject to the following restrictions:
 
 
 
-btContinuousConvexCollision::btContinuousConvexCollision ( btConvexShape*	convexA,btConvexShape*	convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
+btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape*	convexA,const btConvexShape*	convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
 :m_simplexSolver(simplexSolver),
 m_penetrationDepthSolver(penetrationDepthSolver),
 m_convexA(convexA),m_convexB(convexB)
@@ -35,7 +34,7 @@ m_convexA(convexA),m_convexB(convexB)
 
 /// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
 /// You don't want your game ever to lock-up.
-#define MAX_ITERATIONS 1000
+#define MAX_ITERATIONS 64
 
 bool	btContinuousConvexCollision::calcTimeOfImpact(
 				const btTransform& fromA,
@@ -52,10 +51,18 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 	btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA);
 	btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB);
 
+
 	btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
 	btScalar boundingRadiusB = m_convexB->getAngularMotionDisc();
 
 	btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
+	btVector3 relLinVel = (linVelB-linVelA);
+
+	btScalar relLinVelocLength = (linVelB-linVelA).length();
+	
+	if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f)
+		return false;
+
 
 	btScalar radius = btScalar(0.001);
 
@@ -93,7 +100,7 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 		btGjkPairDetector::ClosestPointInput input;
 	
 		//we don't use margins during CCD
-		gjk.setIgnoreMargin(true);
+	//	gjk.setIgnoreMargin(true);
 
 		input.m_transformA = fromA;
 		input.m_transformB = fromB;
@@ -108,25 +115,31 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 		btScalar dist;
 		dist = pointCollector1.m_distance;
 		n = pointCollector1.m_normalOnBInWorld;
+
+		btScalar projectedLinearVelocity = relLinVel.dot(n);
 		
 		//not close enough
 		while (dist > radius)
 		{
 			numIter++;
 			if (numIter > maxIter)
+			{
 				return false; //todo: report a failure
-
+			}
 			btScalar dLambda = btScalar(0.);
 
+			projectedLinearVelocity = relLinVel.dot(n);
+
 			//calculate safe moving fraction from distance / (linear+rotational velocity)
 			
 			//btScalar clippedDist  = GEN_min(angularConservativeRadius,dist);
 			//btScalar clippedDist  = dist;
 			
-			btScalar projectedLinearVelocity = (linVelB-linVelA).dot(n);
 			
 			dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
 
+			
+			
 			lambda = lambda + dLambda;
 
 			if (lambda > btScalar(1.))
@@ -135,9 +148,14 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 			if (lambda < btScalar(0.))
 				return false;
 
+
 			//todo: next check with relative epsilon
 			if (lambda <= lastLambda)
+			{
+				return false;
+				//n.setValue(0,0,0);
 				break;
+			}
 			lastLambda = lambda;
 
 			
@@ -163,11 +181,13 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 				{
 					//degenerate ?!
 					result.m_fraction = lastLambda;
-					result.m_normal = n;
+					n = pointCollector.m_normalOnBInWorld;
+					result.m_normal=n;//.setValue(1,1,1);// = n;
+					result.m_hitPoint = pointCollector.m_pointInWorld;
 					return true;
 				}
 				c = pointCollector.m_pointInWorld;		
-				
+				n = pointCollector.m_normalOnBInWorld;
 				dist = pointCollector.m_distance;
 			} else
 			{
@@ -177,8 +197,13 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 
 		}
 
+		//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;
+		result.m_hitPoint = c;
 		return true;
 	}
 
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
index 9901bab4b45..28c2b4d6156 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
@@ -30,13 +30,13 @@ class btContinuousConvexCollision : public btConvexCast
 {
 	btSimplexSolverInterface* m_simplexSolver;
 	btConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
-	btConvexShape*	m_convexA;
-	btConvexShape*	m_convexB;
+	const btConvexShape*	m_convexA;
+	const btConvexShape*	m_convexB;
 
 
 public:
 
-	btContinuousConvexCollision (btConvexShape*	shapeA,btConvexShape*	shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
+	btContinuousConvexCollision (const btConvexShape*	shapeA,const btConvexShape*	shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
 
 	virtual bool	calcTimeOfImpact(
 				const btTransform& fromA,
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
index 3101b59993d..0edf4dcd496 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
@@ -17,11 +17,11 @@ subject to the following restrictions:
 #ifndef CONVEX_CAST_H
 #define CONVEX_CAST_H
 
-#include "../../LinearMath/btTransform.h"
-#include "../../LinearMath/btVector3.h"
-#include "../../LinearMath/btScalar.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btScalar.h"
 class btMinkowskiSumShape;
-#include "../../LinearMath/btIDebugDraw.h"
+#include "LinearMath/btIDebugDraw.h"
 
 /// btConvexCast is an interface for Casting
 class btConvexCast
@@ -42,19 +42,21 @@ public:
 
 		CastResult()
 			:m_fraction(btScalar(1e30)),
-			m_debugDrawer(0)
+			m_debugDrawer(0),
+			m_allowedPenetration(btScalar(0))
 		{
 		}
 
 
 		virtual ~CastResult() {};
 
-		btVector3	m_normal;
-		btScalar	m_fraction;
 		btTransform	m_hitTransformA;
 		btTransform	m_hitTransformB;
-
+		btVector3	m_normal;
+		btVector3   m_hitPoint;
+		btScalar	m_fraction; //input and output
 		btIDebugDraw* m_debugDrawer;
+		btScalar	m_allowedPenetration;
 
 	};
 
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
index 7caeba4be45..99690921317 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
@@ -21,7 +21,7 @@ class btStackAlloc;
 class btVector3;
 #include "btSimplexSolverInterface.h"
 class btConvexShape;
-#include "../../LinearMath/btPoint3.h"
+#include "LinearMath/btPoint3.h"
 class btTransform;
 
 ///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
@@ -31,7 +31,7 @@ public:
 	
 	virtual ~btConvexPenetrationDepthSolver() {};
 	virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
-		btConvexShape* convexA,btConvexShape* convexB,
+		const btConvexShape* convexA,const btConvexShape* convexB,
 					const btTransform& transA,const btTransform& transB,
 				btVector3& v, btPoint3& pa, btPoint3& pb,
 				class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
index 15000c1ab61..db797d5141f 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
@@ -16,8 +16,8 @@ subject to the following restrictions:
 
 #ifndef DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
 #define DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
-#include "../../LinearMath/btTransform.h"
-#include "../../LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
 class btStackAlloc;
 
 /// This interface is made to be used by an iterative approach to do TimeOfImpact calculations
@@ -58,7 +58,7 @@ struct btDiscreteCollisionDetectorInterface
 	// give either closest points (distance > 0) or penetration (distance)
 	// the normal always points from B towards A
 	//
-	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) = 0;
+	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false) = 0;
 
 };
 
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp
index 93edffeafd6..bef697a0a11 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp
@@ -17,12 +17,17 @@ subject to the following restrictions:
 
 #include "btGjkConvexCast.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
-#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
 #include "btGjkPairDetector.h"
 #include "btPointCollector.h"
+#include "LinearMath/btTransformUtil.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define MAX_ITERATIONS 64
+#else
+#define MAX_ITERATIONS 32
+#endif
 
-btGjkConvexCast::btGjkConvexCast(btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
+btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
 :m_simplexSolver(simplexSolver),
 m_convexA(convexA),
 m_convexB(convexB)
@@ -38,120 +43,113 @@ bool	btGjkConvexCast::calcTimeOfImpact(
 {
 
 
-	btMinkowskiSumShape combi(m_convexA,m_convexB);
-	btMinkowskiSumShape* convex = &combi;
-
-	btTransform	rayFromLocalA;
-	btTransform	rayToLocalA;
-
-	rayFromLocalA = fromA.inverse()* fromB;
-	rayToLocalA = toA.inverse()* toB;
-
-
-	btTransform trA,trB;
-	trA = btTransform(fromA);
-	trB = btTransform(fromB);
-	trA.setOrigin(btPoint3(0,0,0));
-	trB.setOrigin(btPoint3(0,0,0));
-
-	convex->setTransformA(trA);
-	convex->setTransformB(trB);
-
+	m_simplexSolver->reset();
 
+	/// compute linear velocity for this interval, to interpolate
+	//assume no rotation/angular velocity, assert here?
+	btVector3 linVelA,linVelB;
+	linVelA = toA.getOrigin()-fromA.getOrigin();
+	linVelB = toB.getOrigin()-fromB.getOrigin();
 
+	btScalar radius = btScalar(0.001);
+	btScalar lambda = btScalar(0.);
+	btVector3 v(1,0,0);
 
-	btScalar radius = btScalar(0.01);
+	int maxIter = MAX_ITERATIONS;
 
-	btScalar lambda = btScalar(0.);
-	btVector3 s = rayFromLocalA.getOrigin();
-	btVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin();
-	btVector3 x = s;
 	btVector3 n;
-	n.setValue(0,0,0);
+	n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
 	bool hasResult = false;
 	btVector3 c;
+	btVector3 r = (linVelA-linVelB);
 
 	btScalar lastLambda = lambda;
+	//btScalar epsilon = btScalar(0.001);
 
+	int numIter = 0;
 	//first solution, using GJK
 
-	//no penetration support for now, perhaps pass a pointer when we really want it
-	btConvexPenetrationDepthSolver* penSolverPtr = 0;
 
 	btTransform identityTrans;
 	identityTrans.setIdentity();
 
-	btSphereShape	raySphere(btScalar(0.0));
-	raySphere.setMargin(btScalar(0.));
 
-	btTransform sphereTr;
-	sphereTr.setIdentity();
-	sphereTr.setOrigin( rayFromLocalA.getOrigin());
+//	result.drawCoordSystem(sphereTr);
 
-	result.drawCoordSystem(sphereTr);
-	{
-		btPointCollector	pointCollector1;
-		btGjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr);		
+	btPointCollector	pointCollector;
 
-		btGjkPairDetector::ClosestPointInput input;
-		input.m_transformA = sphereTr;
-		input.m_transformB = identityTrans;
-		gjk.getClosestPoints(input,pointCollector1,0);
+		
+	btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver);		
+	btGjkPairDetector::ClosestPointInput input;
 
-		hasResult = pointCollector1.m_hasResult;
-		c = pointCollector1.m_pointInWorld;
-		n = pointCollector1.m_normalOnBInWorld;
-	}
+	//we don't use margins during CCD
+	//	gjk.setIgnoreMargin(true);
 
-	
+	input.m_transformA = fromA;
+	input.m_transformB = fromB;
+	gjk.getClosestPoints(input,pointCollector,0);
+
+	hasResult = pointCollector.m_hasResult;
+	c = pointCollector.m_pointInWorld;
 
 	if (hasResult)
 	{
 		btScalar dist;
-		dist = (c-x).length();
-		if (dist < radius)
-		{
-			//penetration
-			lastLambda = btScalar(1.);
-		}
+		dist = pointCollector.m_distance;
+		n = pointCollector.m_normalOnBInWorld;
+
+	
 
 		//not close enough
 		while (dist > radius)
 		{
+			numIter++;
+			if (numIter > maxIter)
+			{
+				return false; //todo: report a failure
+			}
+			btScalar dLambda = btScalar(0.);
+
+			btScalar projectedLinearVelocity = r.dot(n);
 			
-			n = x - c;
-			btScalar nDotr = n.dot(r);
+			dLambda = dist / (projectedLinearVelocity);
+
+			lambda = lambda - dLambda;
 
-			if (nDotr >= -(SIMD_EPSILON*SIMD_EPSILON))
+			if (lambda > btScalar(1.))
 				return false;
-			
-			lambda = lambda - n.dot(n) / nDotr;
+
+			if (lambda < btScalar(0.))
+				return false;
+
+			//todo: next check with relative epsilon
 			if (lambda <= lastLambda)
+			{
+				return false;
+				//n.setValue(0,0,0);
 				break;
-
+			}
 			lastLambda = lambda;
 
-			x = s + lambda * r;
-
-			sphereTr.setOrigin( x );
-			result.drawCoordSystem(sphereTr);
-			btPointCollector	pointCollector;
-			btGjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr);
-			btGjkPairDetector::ClosestPointInput input;
-			input.m_transformA = sphereTr;
-			input.m_transformB = identityTrans;
+			//interpolate to next lambda
+			result.DebugDraw( lambda );
+			input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
+			input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
+			
 			gjk.getClosestPoints(input,pointCollector,0);
 			if (pointCollector.m_hasResult)
 			{
 				if (pointCollector.m_distance < btScalar(0.))
 				{
-					//degeneracy, report a hit
 					result.m_fraction = lastLambda;
-					result.m_normal = n;
+					n = pointCollector.m_normalOnBInWorld;
+					result.m_normal=n;
+					result.m_hitPoint = pointCollector.m_pointInWorld;
 					return true;
 				}
-				c = pointCollector.m_pointInWorld;			
-				dist = (c-x).length();
+				c = pointCollector.m_pointInWorld;		
+				n = pointCollector.m_normalOnBInWorld;
+				dist = pointCollector.m_distance;
 			} else
 			{
 				//??
@@ -160,15 +158,19 @@ bool	btGjkConvexCast::calcTimeOfImpact(
 
 		}
 
-		if (lastLambda < btScalar(1.))
-		{
-		
-			result.m_fraction = lastLambda;
-			result.m_normal = n;
-			return true;
-		}
+		//is n normalized?
+		//don't report time of impact for motion away from the contact normal (or causes minor penetration)
+		if (n.dot(r)>=-result.m_allowedPenetration)
+			return false;
+
+		result.m_fraction = lambda;
+		result.m_normal = n;
+		result.m_hitPoint = c;
+		return true;
 	}
 
 	return false;
+
+
 }
 
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
index 3905c45e6d6..a977c9e83f7 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
@@ -18,9 +18,9 @@ subject to the following restrictions:
 #ifndef GJK_CONVEX_CAST_H
 #define GJK_CONVEX_CAST_H
 
-#include "../CollisionShapes/btCollisionMargin.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
 
-#include "../../LinearMath/btVector3.h"
+#include "LinearMath/btVector3.h"
 #include "btConvexCast.h"
 class btConvexShape;
 class btMinkowskiSumShape;
@@ -30,12 +30,12 @@ class btMinkowskiSumShape;
 class btGjkConvexCast : public btConvexCast
 {
 	btSimplexSolverInterface*	m_simplexSolver;
-	btConvexShape*	m_convexA;
-	btConvexShape*	m_convexB;
+	const btConvexShape*	m_convexA;
+	const btConvexShape*	m_convexB;
 
 public:
 
-	btGjkConvexCast(btConvexShape*	convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver);
+	btGjkConvexCast(const btConvexShape*	convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver);
 
 	/// cast a convex against another convex object
 	virtual bool	calcTimeOfImpact(
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp
index 8abdfdbb7e5..36cdeeaefdb 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp
@@ -26,7 +26,7 @@ Nov.2006
 
 #include "btGjkEpa.h"
 #include <string.h> //for memset
-#include <LinearMath/btStackAlloc.h>
+#include "LinearMath/btStackAlloc.h"
 
 #if defined(DEBUG) || defined (_DEBUG)
 #include <stdio.h> //for debug printf
@@ -580,8 +580,8 @@ using namespace	gjkepa_impl;
 
 
 //
-bool	btGjkEpaSolver::Collide(btConvexShape *shape0,const btTransform &wtrs0,
-								btConvexShape *shape1,const btTransform &wtrs1,
+bool	btGjkEpaSolver::Collide(const btConvexShape *shape0,const btTransform &wtrs0,
+								const btConvexShape *shape1,const btTransform &wtrs1,
 								btScalar	radialmargin,
 								btStackAlloc* stackAlloc,
 								sResults&	results)
@@ -602,13 +602,13 @@ GJK			gjk(stackAlloc,
 				wtrs1.getBasis(),wtrs1.getOrigin(),shape1,
 				radialmargin+EPA_accuracy);
 const Z		collide(gjk.SearchOrigin());
-results.gjk_iterations	=	gjk.iterations+1;
+results.gjk_iterations	=	static_cast<int>(gjk.iterations+1);
 if(collide)
 	{
 	/* Then EPA for penetration depth	*/
 	EPA			epa(&gjk);
 	const F		pd(epa.EvaluatePD());
-	results.epa_iterations	=	epa.iterations+1;
+	results.epa_iterations	=	static_cast<int>(epa.iterations+1);
 	if(pd>0)
 		{
 		results.status			=	sResults::Penetrating;
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h
index 759b30bb17f..1c256f41939 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h
@@ -21,7 +21,7 @@ Nov.2006
 
 #ifndef _05E48D53_04E0_49ad_BB0A_D74FE62E7366_
 #define _05E48D53_04E0_49ad_BB0A_D74FE62E7366_
-#include "../CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
 
 class btStackAlloc;
 
@@ -35,7 +35,7 @@ struct	sResults
 		Separated,		/* Shapes doesnt penetrate												*/ 
 		Penetrating,	/* Shapes are penetrating												*/ 
 		GJK_Failed,		/* GJK phase fail, no big issue, shapes are probably just 'touching'	*/ 
-		EPA_Failed,		/* EPA phase fail, bigger problem, need to save parameters, and debug	*/ 
+		EPA_Failed		/* EPA phase fail, bigger problem, need to save parameters, and debug	*/ 
 		}		status;
 	btVector3	witnesses[2];
 	btVector3	normal;
@@ -43,8 +43,8 @@ struct	sResults
 	int	epa_iterations;
 	int	gjk_iterations;
 	};
-static bool	Collide(btConvexShape* shape0,const btTransform& wtrs0,
-					btConvexShape* shape1,const btTransform& wtrs1,
+static bool	Collide(const btConvexShape* shape0,const btTransform& wtrs0,
+					const btConvexShape* shape1,const btTransform& wtrs1,
 					btScalar	radialmargin,
 					btStackAlloc* stackAlloc,
 					sResults&	results);
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
index 87330493b60..c4f84ed4d75 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
@@ -18,9 +18,10 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 #include "btGjkEpaPenetrationDepthSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
 
 bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
-											  btConvexShape* pConvexA, btConvexShape* pConvexB,
+											  const btConvexShape* pConvexA, const btConvexShape* pConvexB,
 											  const btTransform& transformA, const btTransform& transformB,
 											  btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
 											  class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc )
@@ -32,10 +33,20 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim
 
 	const btScalar				radialmargin(btScalar(0.));
 	
+//#define USE_ORIGINAL_GJK 1
+#ifdef USE_ORIGINAL_GJK
 	btGjkEpaSolver::sResults	results;
 	if(btGjkEpaSolver::Collide(	pConvexA,transformA,
 								pConvexB,transformB,
 								radialmargin,stackAlloc,results))
+#else
+	btVector3	guessVector(transformA.getOrigin()-transformB.getOrigin());
+	btGjkEpaSolver2::sResults	results;
+	if(btGjkEpaSolver2::Penetration(pConvexA,transformA,
+								pConvexB,transformB,
+								guessVector,results))
+	
+#endif
 		{
 	//	debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
 		//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
index 3916ba0776c..2dc069ce5cf 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
@@ -26,7 +26,7 @@ class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
 	public :
 
 		bool			calcPenDepth( btSimplexSolverInterface& simplexSolver,
-									  btConvexShape* pConvexA, btConvexShape* pConvexB,
+									  const btConvexShape* pConvexA, const btConvexShape* pConvexB,
 									  const btTransform& transformA, const btTransform& transformB,
 									  btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
 									  class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc );
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
index f1f3f7f7f6c..01fb1a4b068 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
@@ -35,7 +35,7 @@ int gNumGjkChecks = 0;
 
 
 
-btGjkPairDetector::btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver)
+btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver)
 :m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)),
 m_penetrationDepthSolver(penetrationDepthSolver),
 m_simplexSolver(simplexSolver),
@@ -47,7 +47,7 @@ m_catchDegeneracies(1)
 {
 }
 
-void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
 {
 	btScalar distance=btScalar(0.);
 	btVector3	normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
@@ -144,6 +144,13 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 				break;
 			}
 
+			if(m_cachedSeparatingAxis.length2()<REL_ERROR2)
+            {
+                m_degenerateSimplex = 6;
+                checkSimplex = true;
+                break;
+            }
+
 			btScalar previousSquaredDistance = squaredDistance;
 			squaredDistance = m_cachedSeparatingAxis.length2();
 			
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
index af0fe32f6c7..550fc4677e0 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
@@ -20,8 +20,8 @@ subject to the following restrictions:
 #define GJK_PAIR_DETECTOR_H
 
 #include "btDiscreteCollisionDetectorInterface.h"
-#include "../../LinearMath/btPoint3.h"
-#include "../CollisionShapes/btCollisionMargin.h"
+#include "LinearMath/btPoint3.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
 
 class btConvexShape;
 #include "btSimplexSolverInterface.h"
@@ -35,8 +35,8 @@ class btGjkPairDetector : public btDiscreteCollisionDetectorInterface
 	btVector3	m_cachedSeparatingAxis;
 	btConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
 	btSimplexSolverInterface* m_simplexSolver;
-	btConvexShape* m_minkowskiA;
-	btConvexShape* m_minkowskiB;
+	const btConvexShape* m_minkowskiA;
+	const btConvexShape* m_minkowskiB;
 	bool		m_ignoreMargin;
 	
 
@@ -49,10 +49,10 @@ public:
 	int			m_catchDegeneracies;
 
 
-	btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver);
+	btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver);
 	virtual ~btGjkPairDetector() {};
 
-	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw);
+	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
 
 	void setMinkowskiA(btConvexShape* minkA)
 	{
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
index f6a893151da..e75fc1bee96 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
@@ -16,8 +16,8 @@ subject to the following restrictions:
 #ifndef MANIFOLD_CONTACT_POINT_H
 #define MANIFOLD_CONTACT_POINT_H
 
-#include "../../LinearMath/btVector3.h"
-#include "../../LinearMath/btTransformUtil.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransformUtil.h"
 
 
 
@@ -30,6 +30,8 @@ class btManifoldPoint
 		public:
 			btManifoldPoint()
 				:m_userPersistentData(0),
+				m_appliedImpulse(0.f),
+				m_lateralFrictionInitialized(false),
 				m_lifeTime(0)
 			{
 			}
@@ -43,7 +45,11 @@ class btManifoldPoint
 					m_distance1( distance ),
 					m_combinedFriction(btScalar(0.)),
 					m_combinedRestitution(btScalar(0.)),
-					m_userPersistentData(0),					
+					m_userPersistentData(0),
+					m_appliedImpulse(0.f),
+					m_lateralFrictionInitialized(false),
+					m_appliedImpulseLateral1(0.f),
+					m_appliedImpulseLateral2(0.f),
 					m_lifeTime(0)
 			{
 				
@@ -63,11 +69,23 @@ class btManifoldPoint
 			btScalar	m_combinedFriction;
 			btScalar	m_combinedRestitution;
 
+         //BP mod, store contact triangles.
+         int	   m_partId0;
+         int      m_partId1;
+         int      m_index0;
+         int      m_index1;
 				
 			mutable void*	m_userPersistentData;
+			btScalar		m_appliedImpulse;
 
-			int		m_lifeTime;//lifetime of the contactpoint in frames
+			bool			m_lateralFrictionInitialized;
+			btScalar		m_appliedImpulseLateral1;
+			btScalar		m_appliedImpulseLateral2;
+			int				m_lifeTime;//lifetime of the contactpoint in frames
 			
+			btVector3		m_lateralFrictionDir1;
+			btVector3		m_lateralFrictionDir2;
+
 			btScalar getDistance() const
 			{
 				return m_distance1;
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
index c4bab3a134a..0e6fa2e6dfe 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
@@ -14,10 +14,10 @@ subject to the following restrictions:
 */
 
 #include "btMinkowskiPenetrationDepthSolver.h"
-#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
 
 
 
@@ -71,7 +71,7 @@ btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
 
 
 bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
-												   btConvexShape* convexA,btConvexShape* convexB,
+												   const btConvexShape* convexA,const btConvexShape* convexB,
 												   const btTransform& transA,const btTransform& transB,
 												   btVector3& v, btPoint3& pa, btPoint3& pb,
 												   class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
@@ -112,8 +112,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 
 	//just take fixed number of orientation, and sample the penetration depth in that direction
 	btScalar minProj = btScalar(1e30);
-	btVector3 minNorm;
-	btVector3 minVertex;
+	btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
 	btVector3 minA,minB;
 	btVector3 seperatingAxisInA,seperatingAxisInB;
 	btVector3 pInA,qInB,pWorld,qWorld,w;
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
index b348b21b52a..27b42c2b47e 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
@@ -25,7 +25,7 @@ class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver
 public:
 
 	virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
-	btConvexShape* convexA,btConvexShape* convexB,
+	const btConvexShape* convexA,const btConvexShape* convexB,
 				const btTransform& transA,const btTransform& transB,
 			btVector3& v, btPoint3& pa, btPoint3& pb,
 			class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
index 08cb3ed334d..386885d2ac8 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
@@ -20,6 +20,7 @@ subject to the following restrictions:
 
 btScalar					gContactBreakingThreshold = btScalar(0.02);
 ContactDestroyedCallback	gContactDestroyedCallback = 0;
+ContactProcessedCallback	gContactProcessedCallback = 0;
 
 
 
@@ -27,20 +28,12 @@ btPersistentManifold::btPersistentManifold()
 :m_body0(0),
 m_body1(0),
 m_cachedPoints (0),
-m_index1(0)
+m_index1a(0)
 {
 }
 
 
-void	btPersistentManifold::clearManifold()
-{
-	int i;
-	for (i=0;i<m_cachedPoints;i++)
-	{
-		clearUserCache(m_pointCache[i]);
-	}
-	m_cachedPoints = 0;
-}
+
 
 #ifdef DEBUG_PERSISTENCY
 #include <stdio.h>
@@ -169,7 +162,7 @@ int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
 	return nearestPoint;
 }
 
-void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint)
+int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint)
 {
 	assert(validContactDistance(newPoint));
 
@@ -182,7 +175,7 @@ void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint)
 #else
 		insertIndex = 0;
 #endif
-
+		clearUserCache(m_pointCache[insertIndex]);
 		
 	} else
 	{
@@ -190,7 +183,9 @@ void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint)
 
 		
 	}
-	replaceContactPoint(newPoint,insertIndex);
+	btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
+	m_pointCache[insertIndex] = newPoint;
+	return insertIndex;
 }
 
 btScalar	btPersistentManifold::getContactBreakingThreshold() const
@@ -198,10 +193,20 @@ btScalar	btPersistentManifold::getContactBreakingThreshold() const
 	return gContactBreakingThreshold;
 }
 
+
+
 void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
 {
 	int i;
-
+#ifdef DEBUG_PERSISTENCY
+	printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
+		trA.getOrigin().getX(),
+		trA.getOrigin().getY(),
+		trA.getOrigin().getZ(),
+		trB.getOrigin().getX(),
+		trB.getOrigin().getY(),
+		trB.getOrigin().getZ());
+#endif //DEBUG_PERSISTENCY
 	/// first refresh worldspace positions and distance
 	for (i=getNumContacts()-1;i>=0;i--)
 	{
@@ -232,6 +237,11 @@ void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btT
 			if (distance2d  > getContactBreakingThreshold()*getContactBreakingThreshold() )
 			{
 				removeContactPoint(i);
+			} else
+			{
+				//contact point processed callback
+				if (gContactProcessedCallback)
+					(*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1);
 			}
 		}
 	}
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
index a5918b84db3..c122eb865e8 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
@@ -17,9 +17,10 @@ subject to the following restrictions:
 #define PERSISTENT_MANIFOLD_H
 
 
-#include "../../LinearMath/btVector3.h"
-#include "../../LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
 #include "btManifoldPoint.h"
+#include "LinearMath/btAlignedAllocator.h"
 
 struct btCollisionResult;
 
@@ -27,6 +28,7 @@ struct btCollisionResult;
 extern btScalar gContactBreakingThreshold;
 
 typedef bool (*ContactDestroyedCallback)(void* userPersistentData);
+typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1);
 extern ContactDestroyedCallback	gContactDestroyedCallback;
 
 
@@ -34,8 +36,13 @@ extern ContactDestroyedCallback	gContactDestroyedCallback;
 
 #define MANIFOLD_CACHE_SIZE 4
 
-///btPersistentManifold maintains contact points, and reduces them to 4.
-///It does contact filtering/contact reduction.
+///btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping in the broadphase.
+///Those contact points are created by the collision narrow phase.
+///The cache can be empty, or hold 1,2,3 or 4 points. Some collision algorithms (GJK) might only add one point at a time.
+///updates/refreshes old contact points, and throw them away if necessary (distance becomes too large)
+///reduces the cache to 4 points, when more then 4 points are added, using following rules:
+///the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points
+///note that some pairs of objects might have more then one contact manifold.
 ATTRIBUTE_ALIGNED16( class) btPersistentManifold 
 {
 
@@ -55,20 +62,23 @@ ATTRIBUTE_ALIGNED16( class) btPersistentManifold
 
 public:
 
-	int m_index1;
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	int m_index1a;
 
 	btPersistentManifold();
 
-	btPersistentManifold(void* body0,void* body1)
+	btPersistentManifold(void* body0,void* body1,int bla)
 		: m_body0(body0),m_body1(body1),m_cachedPoints(0)
 	{
+		(void)bla;
 	}
 
-	inline void* getBody0() { return m_body0;}
-	inline void* getBody1() { return m_body1;}
+	SIMD_FORCE_INLINE void* getBody0() { return m_body0;}
+	SIMD_FORCE_INLINE void* getBody1() { return m_body1;}
 
-	inline const void* getBody0() const { return m_body0;}
-	inline const void* getBody1() const { return m_body1;}
+	SIMD_FORCE_INLINE const void* getBody0() const { return m_body0;}
+	SIMD_FORCE_INLINE const void* getBody1() const { return m_body1;}
 
 	void	setBodies(void* body0,void* body1)
 	{
@@ -82,15 +92,15 @@ public:
 	void	DebugPersistency();
 #endif //
 	
-	inline int	getNumContacts() const { return m_cachedPoints;}
+	SIMD_FORCE_INLINE int	getNumContacts() const { return m_cachedPoints;}
 
-	inline const btManifoldPoint& getContactPoint(int index) const
+	SIMD_FORCE_INLINE const btManifoldPoint& getContactPoint(int index) const
 	{
 		btAssert(index < m_cachedPoints);
 		return m_pointCache[index];
 	}
 
-	inline btManifoldPoint& getContactPoint(int index)
+	SIMD_FORCE_INLINE btManifoldPoint& getContactPoint(int index)
 	{
 		btAssert(index < m_cachedPoints);
 		return m_pointCache[index];
@@ -101,7 +111,7 @@ public:
 	
 	int getCacheEntry(const btManifoldPoint& newPoint) const;
 
-	void AddManifoldPoint( const btManifoldPoint& newPoint);
+	int addManifoldPoint( const btManifoldPoint& newPoint);
 
 	void removeContactPoint (int index)
 	{
@@ -114,6 +124,11 @@ public:
 			m_pointCache[index] = m_pointCache[lastUsedIndex]; 
 			//get rid of duplicated userPersistentData pointer
 			m_pointCache[lastUsedIndex].m_userPersistentData = 0;
+			m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
+			m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false;
+			m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
+			m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
+			m_pointCache[lastUsedIndex].m_lifeTime = 0;
 		}
 
 		btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0);
@@ -126,12 +141,20 @@ public:
 #define MAINTAIN_PERSISTENCY 1
 #ifdef MAINTAIN_PERSISTENCY
 		int	lifeTime = m_pointCache[insertIndex].getLifeTime();
+		btScalar	appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
+		btScalar	appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
+		btScalar	appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
+				
 		btAssert(lifeTime>=0);
 		void* cache = m_pointCache[insertIndex].m_userPersistentData;
 		
 		m_pointCache[insertIndex] = newPoint;
 
 		m_pointCache[insertIndex].m_userPersistentData = cache;
+		m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
+		m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
+		m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
+		
 		m_pointCache[insertIndex].m_lifeTime = lifeTime;
 #else
 		clearUserCache(m_pointCache[insertIndex]);
@@ -147,7 +170,16 @@ public:
 	/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
 	void	refreshContactPoints(  const btTransform& trA,const btTransform& trB);
 
-	void	clearManifold();
+	
+	SIMD_FORCE_INLINE	void	clearManifold()
+	{
+		int i;
+		for (i=0;i<m_cachedPoints;i++)
+		{
+			clearUserCache(m_pointCache[i]);
+		}
+		m_cachedPoints = 0;
+	}
 
 
 
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
index 31b91467777..a70a3ae56c4 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
@@ -13,7 +13,14 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
+//#include <stdio.h>
+
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
 #include "btRaycastCallback.h"
 
 btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to)
@@ -29,8 +36,6 @@ btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const
 
 void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
 {
-	
-
 	const btVector3 &vert0=triangle[0];
 	const btVector3 &vert1=triangle[1];
 	const btVector3 &vert2=triangle[2];
@@ -99,3 +104,60 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId,
 		}
 	}
 }
+
+
+btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin)
+{
+	m_convexShape = convexShape;
+	m_convexShapeFrom = convexShapeFrom;
+	m_convexShapeTo = convexShapeTo;
+	m_triangleToWorld = triangleToWorld;
+	m_hitFraction = 1.0;
+    m_triangleCollisionMargin = triangleCollisionMargin;
+}
+
+void
+btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex)
+{
+	btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]);
+    triangleShape.setMargin(m_triangleCollisionMargin);
+
+	btVoronoiSimplexSolver	simplexSolver;
+	btGjkEpaPenetrationDepthSolver	gjkEpaPenetrationSolver;
+
+//#define  USE_SUBSIMPLEX_CONVEX_CAST 1
+//if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented out code below
+#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+	btSubsimplexConvexCast convexCaster(m_convexShape, &triangleShape, &simplexSolver);
+#else
+	//btGjkConvexCast	convexCaster(m_convexShape,&triangleShape,&simplexSolver);
+	btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,&gjkEpaPenetrationSolver);
+#endif //#USE_SUBSIMPLEX_CONVEX_CAST
+	
+	btConvexCast::CastResult castResult;
+	castResult.m_fraction = btScalar(1.);
+	if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult))
+	{
+		//add hit
+		if (castResult.m_normal.length2() > btScalar(0.0001))
+		{					
+			if (castResult.m_fraction < m_hitFraction)
+			{
+/* btContinuousConvexCast's normal is already in world space */
+/*
+#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+				//rotate normal into worldspace
+				castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal;
+#endif //USE_SUBSIMPLEX_CONVEX_CAST
+*/
+				castResult.m_normal.normalize();
+
+				reportHit (castResult.m_normal,
+							castResult.m_hitPoint,
+							castResult.m_fraction,
+							partId,
+							triangleIndex);
+			}
+		}
+	}
+}
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
index a0bbc9f8fe9..d2b4b80f8ba 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
@@ -16,15 +16,16 @@ subject to the following restrictions:
 #ifndef RAYCAST_TRI_CALLBACK_H
 #define RAYCAST_TRI_CALLBACK_H
 
-#include "../CollisionShapes/btTriangleCallback.h"
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "LinearMath/btTransform.h"
 struct btBroadphaseProxy;
-
+class btConvexShape;
 
 class  btTriangleRaycastCallback: public btTriangleCallback
 {
 public:
 
-		//input
+	//input
 	btVector3 m_from;
 	btVector3 m_to;
 
@@ -38,5 +39,22 @@ public:
 	
 };
 
+class btTriangleConvexcastCallback : public btTriangleCallback
+{
+public:
+	const btConvexShape* m_convexShape;
+	btTransform m_convexShapeFrom;
+	btTransform m_convexShapeTo;
+	btTransform m_triangleToWorld;
+	btScalar m_hitFraction;
+    btScalar m_triangleCollisionMargin;
+
+	btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin);
+
+	virtual void processTriangle (btVector3* triangle, int partId, int triangleIndex);
+
+	virtual btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0;
+};
+
 #endif //RAYCAST_TRI_CALLBACK_H
 
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
index 58393b2eab9..cf65f46505b 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
@@ -18,8 +18,8 @@ subject to the following restrictions:
 #ifndef SIMPLEX_SOLVER_INTERFACE_H
 #define SIMPLEX_SOLVER_INTERFACE_H
 
-#include "../../LinearMath/btVector3.h"
-#include "../../LinearMath/btPoint3.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btPoint3.h"
 
 #define NO_VIRTUAL_INTERFACE 1
 #ifdef NO_VIRTUAL_INTERFACE
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
index 687738b7fa9..4c709a8c3a9 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
@@ -16,9 +16,11 @@ subject to the following restrictions:
 
 #include "btSubSimplexConvexCast.h"
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
+
 #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
-
+#include "btPointCollector.h"
+#include "LinearMath/btTransformUtil.h"
 
 btSubsimplexConvexCast::btSubsimplexConvexCast (const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
 :m_simplexSolver(simplexSolver),
@@ -41,34 +43,24 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 		CastResult& result)
 {
 
-	btMinkowskiSumShape combi(m_convexA,m_convexB);
-	btMinkowskiSumShape* convex = &combi;
-
-	btTransform	rayFromLocalA;
-	btTransform	rayToLocalA;
-
-	rayFromLocalA = fromA.inverse()* fromB;
-	rayToLocalA = toA.inverse()* toB;
-
-
 	m_simplexSolver->reset();
 
-	convex->setTransformB(btTransform(rayFromLocalA.getBasis()));
-
-	//btScalar radius = btScalar(0.01);
+	btVector3 linVelA,linVelB;
+	linVelA = toA.getOrigin()-fromA.getOrigin();
+	linVelB = toB.getOrigin()-fromB.getOrigin();
 
 	btScalar lambda = btScalar(0.);
-	//todo: need to verify this:
-	//because of minkowski difference, we need the inverse direction
-	
-	btVector3 s = -rayFromLocalA.getOrigin();
-	btVector3 r = -(rayToLocalA.getOrigin()-rayFromLocalA.getOrigin());
-	btVector3 x = s;
+
+	btTransform interpolatedTransA = fromA;
+	btTransform interpolatedTransB = fromB;
+
+	///take relative motion
+	btVector3 r = (linVelA-linVelB);
 	btVector3 v;
-	btVector3 arbitraryPoint = convex->localGetSupportingVertex(r);
 	
-	v = x - arbitraryPoint;
-
+	btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis()));
+	btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis()));
+	v = supVertexA-supVertexB;
 	int maxIter = MAX_ITERATIONS;
 
 	btVector3 n;
@@ -90,11 +82,17 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 	
 	while ( (dist2 > epsilon) && maxIter--)
 	{
-		p = convex->localGetSupportingVertex( v);
-		 w = x - p;
+		supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis()));
+		supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis()));
+		w = supVertexA-supVertexB;
 
 		btScalar VdotW = v.dot(w);
 
+		if (lambda > btScalar(1.0))
+		{
+			return false;
+		}
+
 		if ( VdotW > btScalar(0.))
 		{
 			VdotR = v.dot(r);
@@ -104,20 +102,25 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 			else
 			{
 				lambda = lambda - VdotW / VdotR;
-				x = s + lambda * r;
-				m_simplexSolver->reset();
+				//interpolate to next lambda
+				//	x = s + lambda * r;
+				interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
+				interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
+				//m_simplexSolver->reset();
 				//check next line
-				w = x-p;
+				 w = supVertexA-supVertexB;
 				lastLambda = lambda;
 				n = v;
 				hasResult = true;
 			}
 		} 
-		m_simplexSolver->addVertex( w, x , p);
+		m_simplexSolver->addVertex( w, supVertexA , supVertexB);
 		if (m_simplexSolver->closest(v))
 		{
 			dist2 = v.length2();
 			hasResult = true;
+			//todo: check this normal for validity
+			//n=v;
 			//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
 			//printf("DIST2=%f\n",dist2);
 			//printf("numverts = %i\n",m_simplexSolver->numVertices());
@@ -129,11 +132,26 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 
 	//int numiter = MAX_ITERATIONS - maxIter;
 //	printf("number of iterations: %d", numiter);
-	result.m_fraction = lambda;
-	result.m_normal = n;
+	
+	//don't report a time of impact when moving 'away' from the hitnormal
+	
 
+	result.m_fraction = lambda;
+	if (n.length2() >= (SIMD_EPSILON*SIMD_EPSILON))
+		result.m_normal = n.normalized();
+	else
+		result.m_normal = btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+
+	//don't report time of impact for motion away from the contact normal (or causes minor penetration)
+	if (result.m_normal.dot(r)>=-result.m_allowedPenetration)
+		return false;
+
+	btVector3 hitA,hitB;
+	m_simplexSolver->compute_points(hitA,hitB);
+	result.m_hitPoint=hitB;
 	return true;
 }
 
 
 
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
index 105b7eccefa..cf8a3ab5eb1 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
@@ -26,7 +26,7 @@ subject to the following restrictions:
 
 #include "btVoronoiSimplexSolver.h"
 #include <assert.h>
-#include <stdio.h>
+//#include <stdio.h>
 
 #define VERTA  0
 #define VERTB  1