moved files around

Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.cpp

@@ -0,0 +1,360 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "BU_AlgebraicPolynomialSolver.h"
+#include <math.h>
+#include <SimdMinMax.h>
+
+int BU_AlgebraicPolynomialSolver::Solve2Quadratic(SimdScalar p, SimdScalar q) 
+{ 
+
+	SimdScalar basic_h_local;
+	SimdScalar basic_h_local_delta;
+	
+	basic_h_local       = p * 0.5f; 
+   basic_h_local_delta = basic_h_local * basic_h_local - q; 
+   if (basic_h_local_delta > 0.0f)  { 
+      basic_h_local_delta = SimdSqrt(basic_h_local_delta); 
+      m_roots[0]  = - basic_h_local + basic_h_local_delta; 
+      m_roots[1]  = - basic_h_local - basic_h_local_delta; 
+      return 2; 
+   } 
+   else if (SimdGreaterEqual(basic_h_local_delta, SIMD_EPSILON)) { 
+      m_roots[0]  = - basic_h_local; 
+      return 1; 
+   } 
+   else { 
+      return 0; 
+   } 
+ }
+
+
+int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c) 
+{ 
+    SimdScalar radical = b * b - 4.0f * a * c;
+    if(radical >= 0.f)
+    {
+        SimdScalar sqrtRadical = SimdSqrt(radical); 
+        SimdScalar idenom = 1.0f/(2.0f * a);
+        m_roots[0]=(-b + sqrtRadical) * idenom;
+        m_roots[1]=(-b - sqrtRadical) * idenom;
+		return 2;
+	}
+	return 0;
+}
+
+
+#define cubic_rt(x) \
+ ((x) > 0.0f ? SimdPow((SimdScalar)(x), 0.333333333333333333333333f) : \
+  ((x) < 0.0f ? -SimdPow((SimdScalar)-(x), 0.333333333333333333333333f) : 0.0f))
+
+
+
+/*                                                                           */
+/* this function solves the following cubic equation:                        */
+/*                                                                           */
+/*              3          2                                                */
+/*      lead * x   +  a * x   +  b * x  +  c  =  0.                          */
+/*                                                                           */
+/* it returns the number of different roots found, and stores the roots in   */
+/* roots[0,2]. it returns -1 for a degenerate equation 0 = 0.                */
+/*                                                                           */
+int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c)
+{ 
+   SimdScalar p, q, r;
+   SimdScalar delta, u, phi;
+   SimdScalar dummy;
+
+   if (lead != 1.0) {
+      /*                                                                     */
+      /* transform into normal form: x^3 + a x^2 + b x + c = 0               */
+      /*                                                                     */
+      if (SimdEqual(lead, SIMD_EPSILON)) {
+         /*                                                                  */
+         /* we have  a x^2 + b x + c = 0                                     */
+         /*                                                                  */
+         if (SimdEqual(a, SIMD_EPSILON)) {
+            /*                                                               */
+            /* we have  b x + c = 0                                          */
+            /*                                                               */
+            if (SimdEqual(b, SIMD_EPSILON)) {
+               if (SimdEqual(c, SIMD_EPSILON)) {
+                  return -1;
+               }
+               else {
+                  return 0;
+               }
+            }
+            else {
+               m_roots[0] = -c / b;
+               return 1;
+            }
+         }
+         else {
+            p = c / a;
+            q = b / a;
+            return Solve2QuadraticFull(a,b,c);
+         }
+      }
+      else {
+         a = a / lead;
+         b = b / lead;
+         c = c / lead;
+      }
+   }
+               
+   /*                                                                        */
+   /* we substitute  x = y - a / 3  in order to eliminate the quadric term.  */
+   /* we get   x^3 + p x + q = 0                                             */
+   /*                                                                        */
+   a /= 3.0f;
+   u  = a * a;
+   p  = b / 3.0f - u;
+   q  = a * (2.0f * u - b) + c;
+
+   /*                                                                        */
+   /* now use Cardano's formula                                              */
+   /*                                                                        */
+   if (SimdEqual(p, SIMD_EPSILON)) {
+      if (SimdEqual(q, SIMD_EPSILON)) {
+         /*                                                                  */
+         /* one triple root                                                  */
+         /*                                                                  */
+         m_roots[0] = -a;
+         return 1;
+      }
+      else {
+         /*                                                                  */
+         /* one real and two complex roots                                   */
+         /*                                                                  */
+         m_roots[0] = cubic_rt(-q) - a;
+         return 1;
+      }
+   }
+
+   q /= 2.0f;
+   delta = p * p * p + q * q;
+   if (delta > 0.0f) {
+      /*                                                                     */
+      /* one real and two complex roots. note that  v = -p / u.              */
+      /*                                                                     */
+      u = -q + SimdSqrt(delta);
+      u = cubic_rt(u);
+      m_roots[0] = u - p / u - a;
+      return 1;
+   }
+   else if (delta < 0.0) {
+      /*                                                                     */
+      /* Casus irreducibilis: we have three real roots                       */
+      /*                                                                     */
+      r        = SimdSqrt(-p);
+      p       *= -r;
+      r       *= 2.0;
+      phi      = SimdAcos(-q / p) / 3.0f;
+      dummy    = SIMD_2_PI / 3.0f; 
+      m_roots[0] = r * SimdCos(phi) - a;
+      m_roots[1] = r * SimdCos(phi + dummy) - a;
+      m_roots[2] = r * SimdCos(phi - dummy) - a;
+      return 3;
+   }
+   else {
+      /*                                                                     */
+      /* one single and one SimdScalar root                                      */
+      /*                                                                     */
+      r = cubic_rt(-q);
+      m_roots[0] = 2.0f * r - a;
+      m_roots[1] = -r - a;
+      return 2;
+   }
+}
+
+
+/*                                                                           */
+/* this function solves the following quartic equation:                      */
+/*                                                                           */
+/*             4           3          2                                      */
+/*     lead * x   +  a *  x   +  b * x   +  c * x  +  d = 0.                 */
+/*                                                                           */
+/* it returns the number of different roots found, and stores the roots in   */
+/* roots[0,3]. it returns -1 for a degenerate equation 0 = 0.                */
+/*                                                                           */
+int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d)
+{ 
+   SimdScalar p, q ,r;
+   SimdScalar u, v, w;
+   int i, num_roots, num_tmp;
+   //SimdScalar tmp[2];
+
+   if (lead != 1.0) {
+      /*                                                                     */
+      /* transform into normal form: x^4 + a x^3 + b x^2 + c x + d = 0       */
+      /*                                                                     */
+      if (SimdEqual(lead, SIMD_EPSILON)) {
+         /*                                                                  */
+         /* we have  a x^3 + b x^2 + c x + d = 0                             */
+         /*                                                                  */
+         if (SimdEqual(a, SIMD_EPSILON)) { 
+            /*                                                               */
+            /* we have  b x^2 + c x + d = 0                                  */
+            /*                                                               */
+            if (SimdEqual(b, SIMD_EPSILON)) {
+               /*                                                            */
+               /* we have  c x + d = 0                                       */
+               /*                                                            */
+               if (SimdEqual(c, SIMD_EPSILON)) {
+                  if (SimdEqual(d, SIMD_EPSILON)) {
+                     return -1;
+                  }
+                  else {
+                     return 0;
+                  }
+               }
+               else {
+                  m_roots[0] = -d / c;
+                  return 1;
+               }
+            }
+            else {
+               p = c / b;
+               q = d / b;
+               return Solve2QuadraticFull(b,c,d);
+               
+            }
+         }
+         else { 
+            return Solve3Cubic(1.0, b / a, c / a, d / a);
+         }
+      }
+      else {
+         a = a / lead;
+         b = b / lead;
+         c = c / lead;
+         d = d / lead;
+      }
+   }
+
+   /*                                                                        */
+   /* we substitute  x = y - a / 4  in order to eliminate the cubic term.    */
+   /* we get:  y^4 + p y^2 + q y + r = 0.                                    */
+   /*                                                                        */
+   a /= 4.0f;
+   p  = b - 6.0f * a * a;
+   q  = a * (8.0f * a * a - 2.0f * b) + c;
+   r  = a * (a * (b - 3.f * a * a) - c) + d;
+   if (SimdEqual(q, SIMD_EPSILON)) {
+      /*                                                                     */
+      /* biquadratic equation:  y^4 + p y^2 + r = 0.                         */
+      /*                                                                     */
+      num_roots = Solve2Quadratic(p, r);
+      if (num_roots > 0) {                 
+         if (m_roots[0] > 0.0f) {
+            if (num_roots > 1)  {
+               if ((m_roots[1] > 0.0f)  &&  (m_roots[1] != m_roots[0])) {
+                  u        = SimdSqrt(m_roots[1]);
+                  m_roots[2] =  u - a;
+                  m_roots[3] = -u - a;
+                  u        = SimdSqrt(m_roots[0]);
+                  m_roots[0] =  u - a;
+                  m_roots[1] = -u - a;
+                  return 4;
+               }
+               else {
+                  u        = SimdSqrt(m_roots[0]);
+                  m_roots[0] =  u - a;
+                  m_roots[1] = -u - a;
+                  return 2;
+               }
+            }
+            else {
+               u        = SimdSqrt(m_roots[0]);
+               m_roots[0] =  u - a;
+               m_roots[1] = -u - a;
+               return 2;
+            }
+         }
+      }
+      return 0;
+   }
+   else if (SimdEqual(r, SIMD_EPSILON)) {
+      /*                                                                     */
+      /* no absolute term:  y (y^3 + p y + q) = 0.                           */
+      /*                                                                     */
+      num_roots = Solve3Cubic(1.0, 0.0, p, q);
+      for (i = 0;  i < num_roots;  ++i)  m_roots[i] -= a;
+      if (num_roots != -1) {
+         m_roots[num_roots] = -a;
+         ++num_roots;
+      }
+      else {
+         m_roots[0]  = -a;
+         num_roots = 1;;
+      }
+      return num_roots;
+   }
+   else {
+      /*                                                                     */
+      /* we solve the resolvent cubic equation                               */
+      /*                                                                     */
+      num_roots = Solve3Cubic(1.0f, -0.5f * p, -r, 0.5f * r * p - 0.125f * q * q);
+      if (num_roots == -1) {
+         num_roots = 1;
+         m_roots[0]  = 0.0f;
+      }
+
+      /*                                                                     */
+      /* build two quadric equations                                         */
+      /*                                                                     */
+      w = m_roots[0];
+      u = w * w - r;
+      v = 2.0f * w - p;
+
+      if (SimdEqual(u, SIMD_EPSILON))
+         u = 0.0;
+      else if (u > 0.0f)
+         u = SimdSqrt(u);
+      else
+         return 0;
+      
+      if (SimdEqual(v, SIMD_EPSILON))
+         v = 0.0;
+      else if (v > 0.0f)
+         v = SimdSqrt(v);
+      else
+         return 0;
+
+      if (q < 0.0f)  v = -v;
+      w -= u;
+      num_roots=Solve2Quadratic(v, w);
+      for (i = 0;  i < num_roots;  ++i)  
+	  {
+		  m_roots[i] -= a;
+	  }
+      w += 2.0f *u;
+	  SimdScalar tmp[2];
+	  tmp[0] = m_roots[0];
+	  tmp[1] = m_roots[1];
+
+      num_tmp = Solve2Quadratic(-v, w);
+      for (i = 0;  i < num_tmp;  ++i)
+	  {
+		 m_roots[i + num_roots] = tmp[i] - a;
+		 m_roots[i]=tmp[i];
+	  }
+
+      return  (num_tmp + num_roots);
+   }
+}
+

Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.h

@@ -0,0 +1,45 @@
+/*
+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 BU_ALGEBRAIC_POLYNOMIAL_SOLVER_H
+#define BU_ALGEBRAIC_POLYNOMIAL_SOLVER_H
+
+#include "BU_PolynomialSolverInterface.h"
+
+/// BU_AlgebraicPolynomialSolver implements polynomial root finding by analytically solving algebraic equations.
+/// Polynomials up to 4rd degree are supported, Cardano's formula is used for 3rd degree
+class BU_AlgebraicPolynomialSolver : public BUM_PolynomialSolverInterface
+{
+public:
+	BU_AlgebraicPolynomialSolver() {};
+
+	int Solve2Quadratic(SimdScalar p, SimdScalar q);
+	int Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c);
+	int	Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c);
+	int Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d);
+	
+
+	SimdScalar GetRoot(int i) const 
+	{
+		return m_roots[i];
+	}
+
+private:
+	SimdScalar	m_roots[4];
+
+};
+
+#endif //BU_ALGEBRAIC_POLYNOMIAL_SOLVER_H

Bullet/NarrowPhaseCollision/BU_Collidable.cpp

@@ -0,0 +1,25 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "BU_Collidable.h"
+#include "CollisionShapes/CollisionShape.h"
+#include <SimdTransform.h>
+#include "BU_MotionStateInterface.h"
+
+BU_Collidable::BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape,void* userPointer )
+:m_motionState(motion),m_shape(shape),m_userPointer(userPointer)
+{
+}

Bullet/NarrowPhaseCollision/BU_Collidable.h

@@ -0,0 +1,57 @@
+/*
+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 BU_COLLIDABLE
+#define BU_COLLIDABLE
+
+
+class PolyhedralConvexShape;
+class BU_MotionStateInterface;
+#include <SimdPoint3.h>
+
+class BU_Collidable
+{
+public:
+	BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape, void* userPointer);
+
+	void*		GetUserPointer() const
+	{
+		return m_userPointer;
+	}
+
+	BU_MotionStateInterface&	GetMotionState()
+	{
+		return m_motionState;
+	}
+	inline const BU_MotionStateInterface&	GetMotionState() const
+	{
+		return m_motionState;
+	}
+	
+	inline const PolyhedralConvexShape&	GetShape() const
+	{
+		return m_shape;
+	};
+
+
+private:
+	BU_MotionStateInterface& m_motionState;
+	PolyhedralConvexShape&	m_shape;
+	void*		m_userPointer;
+
+};
+
+#endif //BU_COLLIDABLE

Bullet/NarrowPhaseCollision/BU_CollisionPair.cpp

@@ -0,0 +1,581 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "BU_CollisionPair.h"
+#include "NarrowPhaseCollision/BU_VertexPoly.h"
+#include "NarrowPhaseCollision/BU_EdgeEdge.h"
+#include "BU_Collidable.h"
+
+
+#include "BU_MotionStateInterface.h"
+#include "CollisionShapes/PolyhedralConvexShape.h"
+#include <SimdMinMax.h>
+#include "SimdTransformUtil.h"
+
+
+
+BU_CollisionPair::BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance)
+: m_convexA(convexA),m_convexB(convexB),m_screwing(SimdVector3(0,0,0),SimdVector3(0,0,0)),
+m_tolerance(tolerance)
+{
+
+}
+
+// if there exists a time-of-impact between any feature_pair (edgeA,edgeB),
+// (vertexA,faceB) or (vertexB,faceA) in [0..1], report true and smallest time
+
+
+/*
+bool BU_CollisionPair::GetTimeOfImpact(const SimdVector3& linearMotionA,const SimdQuaternion& angularMotionA,const SimdVector3& linearMotionB,const SimdQuaternion& angularMotionB, SimdScalar& toi,SimdTransform& impactTransA,SimdTransform& impactTransB)
+
+*/
+
+bool BU_CollisionPair::calcTimeOfImpact(
+					const SimdTransform& fromA,
+					const SimdTransform& toA,
+					const SimdTransform& fromB,
+					const SimdTransform& toB,
+					CastResult& result)
+{
+
+
+
+	
+	SimdVector3 linvelA,angvelA;
+	SimdVector3 linvelB,angvelB;
+
+	SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA);
+	SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB);
+
+
+	SimdVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin();
+	SimdQuaternion angularMotionA(0,0,0,1.f);
+	SimdVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin();
+	SimdQuaternion angularMotionB(0,0,0,1);
+	
+
+
+	result.m_fraction = 1.f;
+
+	SimdTransform impactTransA;
+	SimdTransform impactTransB;
+
+	int index=0;
+
+	SimdScalar toiUnscaled=result.m_fraction;
+	const SimdScalar toiUnscaledLimit = result.m_fraction;
+
+	SimdTransform a2w;
+	a2w = fromA;
+	SimdTransform b2w = fromB;
+
+/* debugging code
+	{
+		const int numvertsB = m_convexB->GetNumVertices();
+		for (int v=0;v<numvertsB;v++)
+		{
+			SimdPoint3 pt;
+			m_convexB->GetVertex(v,pt);
+			pt = b2w * pt;
+			char buf[1000];
+
+			if (pt.y() < 0.)
+			{
+				sprintf(buf,"PRE ERROR (%d) %.20E %.20E %.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
+				if (debugFile)
+					fwrite(buf,1,strlen(buf),debugFile);
+			} else
+			{
+				sprintf(buf,"PRE %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
+				if (debugFile)
+					fwrite(buf,1,strlen(buf),debugFile);
+
+			}
+		}
+	}
+*/
+
+
+	SimdTransform b2wp = b2w;
+	
+	b2wp.setOrigin(b2w.getOrigin() + linearMotionB);
+	b2wp.setRotation( b2w.getRotation() + angularMotionB);
+
+	impactTransB = b2wp;
+	
+	SimdTransform a2wp;
+	a2wp.setOrigin(a2w.getOrigin()+ linearMotionA);
+	a2wp.setRotation(a2w.getRotation()+angularMotionA);
+
+	impactTransA = a2wp;
+
+	SimdTransform a2winv;
+	a2winv = a2w.inverse();
+
+	SimdTransform b2wpinv;
+	b2wpinv = b2wp.inverse();
+
+	SimdTransform b2winv;
+	b2winv = b2w.inverse();
+
+	SimdTransform a2wpinv;
+	a2wpinv = a2wp.inverse();
+
+		//Redon's version with concatenated transforms
+
+	SimdTransform relative;
+
+	relative = b2w * b2wpinv * a2wp * a2winv;
+
+	//relative = a2winv * a2wp  * b2wpinv * b2w;
+
+	SimdQuaternion qrel;
+	relative.getBasis().getRotation(qrel);
+
+	SimdVector3 linvel = relative.getOrigin();
+
+	if (linvel.length() < SCREWEPSILON)
+	{
+		linvel.setValue(0.,0.,0.);
+	}
+	SimdVector3 angvel;
+	angvel[0] = 2.f * SimdAsin (qrel[0]);
+	angvel[1] = 2.f * SimdAsin (qrel[1]);
+	angvel[2] = 2.f * SimdAsin (qrel[2]);
+	
+	if (angvel.length() < SCREWEPSILON)
+	{
+		angvel.setValue(0.f,0.f,0.f);
+	}
+
+	//Redon's version with concatenated transforms
+	m_screwing = BU_Screwing(linvel,angvel);
+	
+	SimdTransform w2s;
+	m_screwing.LocalMatrix(w2s);
+
+	SimdTransform s2w;
+	s2w = w2s.inverse();
+
+	//impactTransA = a2w;
+	//impactTransB = b2w;
+
+	bool hit = false;
+	
+	if (SimdFuzzyZero(m_screwing.GetS()) && SimdFuzzyZero(m_screwing.GetW()))
+	{
+		//W = 0 , S = 0 , no collision
+		//toi = 0;
+	/*	
+		{
+			const int numvertsB = m_convexB->GetNumVertices();
+			for (int v=0;v<numvertsB;v++)
+			{
+				SimdPoint3 pt;
+				m_convexB->GetVertex(v,pt);
+				pt = impactTransB * pt;
+				char buf[1000];
+				
+				if (pt.y() < 0.)
+				{
+					sprintf(buf,"EARLY POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
+					if (debugFile)
+						fwrite(buf,1,strlen(buf),debugFile);
+				}
+				else
+				{
+					sprintf(buf,"EARLY POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
+					if (debugFile)
+						fwrite(buf,1,strlen(buf),debugFile);
+				}
+			}
+		}
+	*/	
+		
+		return false;//don't continue moving within epsilon
+	}
+
+#define EDGEEDGE
+#ifdef EDGEEDGE
+
+	BU_EdgeEdge edgeEdge;
+
+	//for all edged in A check agains all edges in B
+	for (int ea = 0;ea < m_convexA->GetNumEdges();ea++)
+	{
+		SimdPoint3 pA0,pA1;
+
+		m_convexA->GetEdge(ea,pA0,pA1);
+
+		pA0= a2w * pA0;//in world space
+		pA0 = w2s * pA0;//in screwing space
+
+		pA1= a2w * pA1;//in world space
+		pA1 = w2s * pA1;//in screwing space
+
+		int numedgesB = m_convexB->GetNumEdges();
+		for (int eb = 0; eb < numedgesB;eb++)
+		{
+			{
+				SimdPoint3 pB0,pB1;
+				m_convexB->GetEdge(eb,pB0,pB1);
+
+				pB0= b2w * pB0;//in world space
+				pB0 = w2s * pB0;//in screwing space
+
+				pB1= b2w * pB1;//in world space
+				pB1 = w2s * pB1;//in screwing space
+
+
+				SimdScalar lambda,mu;
+				
+				toiUnscaled = 1.;
+
+				SimdVector3 edgeDirA(pA1-pA0);
+				SimdVector3 edgeDirB(pB1-pB0);
+
+				if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu))
+				{
+					//printf("edgeedge potential hit\n");
+					if (toiUnscaled>=0)
+					{
+						if (toiUnscaled < toiUnscaledLimit)							
+						{
+		
+							//inside check is already done by checking the mu and gamma !
+
+							SimdPoint3 vtx  = pA0+lambda * (pA1-pA0);
+							SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
+							
+							SimdPoint3 hitptWorld =   s2w * hitpt;
+							{
+
+								if (toiUnscaled < result.m_fraction)
+									result.m_fraction = toiUnscaled;
+
+								hit = true;
+
+								SimdVector3 hitNormal = edgeDirB.cross(edgeDirA);
+								
+								hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled);
+							
+
+								hitNormal.normalize();
+								
+								//an approximated normal can be calculated by taking the cross product of both edges
+								//take care of the sign !
+								
+								SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
+						
+								SimdScalar dist = m_screwing.GetU().dot(hitNormalWorld);
+								if (dist > 0)
+									hitNormalWorld *= -1;
+								
+								//todo: this is the wrong point, because b2winv is still at begin of motion
+								// not at time-of-impact location!
+								//bhitpt = b2winv * hitptWorld;
+
+//								m_manifold.SetContactPoint(BUM_FeatureEdgeEdge,index,ea,eb,hitptWorld,hitNormalWorld);
+							}
+					
+						}
+					}
+				}
+			}
+
+			index++;
+		}
+	};
+#endif //EDGEEDGE
+
+#define VERTEXFACE
+#ifdef VERTEXFACE
+
+	// for all vertices in A, for each face in B,do vertex-face
+	{
+		const int numvertsA = m_convexA->GetNumVertices();
+		for (int v=0;v<numvertsA;v++)
+		//int v=3;
+
+		{
+			SimdPoint3 vtx;
+			m_convexA->GetVertex(v,vtx);
+
+			vtx = a2w * vtx;//in world space
+			vtx = w2s * vtx;//in screwing space
+
+			const int numplanesB = m_convexB->GetNumPlanes();
+
+			for (int p = 0 ; p < numplanesB; p++)
+			//int p=2;
+			{
+
+				{
+				
+					SimdVector3 planeNorm;
+					SimdPoint3 planeSupport;
+
+					m_convexB->GetPlane(planeNorm,planeSupport,p);
+
+
+					planeSupport = b2w * planeSupport;//transform to world space
+					SimdVector3 planeNormWorld =  b2w.getBasis() * planeNorm;
+				
+					planeSupport =  w2s * planeSupport  ; //transform to screwing space
+					planeNorm =  w2s.getBasis() * planeNormWorld;
+
+					planeNorm.normalize();
+
+					SimdScalar d = planeSupport.dot(planeNorm);
+					
+					SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
+				
+					BU_VertexPoly vtxApolyB;
+
+					toiUnscaled = 1.;
+
+					if ((p==2) && (v==6))
+					{
+//						printf("%f toiUnscaled\n",toiUnscaled);
+
+					}
+					if (vtxApolyB.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,false))
+					{
+					
+
+
+						
+						if (toiUnscaled >= 0. )
+						{
+							//not only collect the first point, get every contactpoint, later we have to check the
+							//manifold properly!
+
+							if (toiUnscaled <= toiUnscaledLimit)
+							{
+	//							printf("toiUnscaled %f\n",toiUnscaled );
+
+								SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
+								SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);
+
+								SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
+								SimdPoint3 hitptWorld = s2w * hitpt;
+
+
+								hitpt = b2winv * hitptWorld;
+								//vertex has to be 'within' the facet's boundary
+								if (m_convexB->IsInside(hitpt,m_tolerance))
+								{
+//									m_manifold.SetContactPoint(BUM_FeatureVertexFace, index,v,p,hitptWorld,hitNormalWorld);
+									
+									if (toiUnscaled < result.m_fraction)
+										result.m_fraction= toiUnscaled;
+									hit = true;
+
+								}
+							}
+						}
+					}
+					
+				}
+
+				index++;
+			}
+		}
+	}
+
+	//
+	// for all vertices in B, for each face in A,do vertex-face
+	//copy and pasted from all verts A -> all planes B so potential typos!
+	//todo: make this into one method with a kind of 'swapped' logic
+	//
+	{
+		const int numvertsB = m_convexB->GetNumVertices();
+		for (int v=0;v<numvertsB;v++)
+		//int v=0;
+
+		{
+			SimdPoint3 vtx;
+			m_convexB->GetVertex(v,vtx);
+
+			vtx = b2w * vtx;//in world space
+/*
+			
+			char buf[1000];
+
+			if (vtx.y() < 0.)
+			{
+				sprintf(buf,"ERROR !!!!!!!!!\n",v,vtx.x(),vtx.y(),vtx.z());
+				if (debugFile)
+					fwrite(buf,1,strlen(buf),debugFile);
+			}
+			sprintf(buf,"vertexWorld(%d) = (%.20E,%.20E,%.20E)\n",v,vtx.x(),vtx.y(),vtx.z());
+			if (debugFile)
+				fwrite(buf,1,strlen(buf),debugFile);
+
+*/			
+			vtx = w2s * vtx;//in screwing space
+
+			const int numplanesA = m_convexA->GetNumPlanes();
+
+			for (int p = 0 ; p < numplanesA; p++)
+			//int p=2;
+			{
+
+				{
+					SimdVector3 planeNorm;
+					SimdPoint3 planeSupport;
+
+					m_convexA->GetPlane(planeNorm,planeSupport,p);
+
+
+					planeSupport = a2w * planeSupport;//transform to world space
+					SimdVector3 planeNormWorld =  a2w.getBasis() * planeNorm;
+				
+					planeSupport =  w2s * planeSupport  ; //transform to screwing space
+					planeNorm =  w2s.getBasis() * planeNormWorld;
+
+					planeNorm.normalize();
+
+					SimdScalar d = planeSupport.dot(planeNorm);
+					
+					SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
+				
+					BU_VertexPoly vtxBpolyA;
+
+					toiUnscaled = 1.;
+
+					if (vtxBpolyA.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,true))
+					{
+						if (toiUnscaled>=0.)
+						{
+							if (toiUnscaled < toiUnscaledLimit)
+							{
+								SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
+								SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
+								//SimdScalar len =  hitNormal.length()-1;
+
+								//assert( SimdFuzzyZero(len) );
+
+								
+								SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
+								SimdPoint3 hitptWorld = s2w * hitpt;
+								hitpt = a2winv * hitptWorld;
+							
+							
+								//vertex has to be 'within' the facet's boundary
+								if (m_convexA->IsInside(hitpt,m_tolerance))
+								{
+									
+//									m_manifold.SetContactPoint(BUM_FeatureFaceVertex,index,p,v,hitptWorld,hitNormalWorld);
+									if (toiUnscaled <result.m_fraction)
+										result.m_fraction = toiUnscaled;
+									hit = true;
+								}
+							}
+						
+						}
+					
+					}
+					}
+
+			}
+		
+			index++;
+		}
+	}
+	
+
+#endif// VERTEXFACE
+
+	//the manifold now consists of all points/normals generated by feature-pairs that have a time-of-impact within this frame
+	//in addition there are contact points from previous frames
+	//we have to cleanup the manifold, using an additional epsilon/tolerance
+	//as long as the distance from the contactpoint (in worldspace) to both objects is within this epsilon we keep the point
+	//else throw it away
+	
+
+	if (hit)
+	{
+
+		//try to avoid numerical drift on close contact
+		
+		if (result.m_fraction < 0.00001)
+		{
+//			printf("toiUnscaledMin< 0.00001\n");
+			impactTransA = a2w;
+			impactTransB = b2w;
+
+		} else
+		{
+
+			//SimdScalar vel = linearMotionB.length();
+			
+			//todo: check this margin
+			result.m_fraction *= 0.99f;
+
+			//move B to new position
+			impactTransB.setOrigin(b2w.getOrigin()+ result.m_fraction*linearMotionB);
+			SimdQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction;
+			ornB.normalize();
+			impactTransB.setRotation(ornB);
+
+			//now transform A
+			SimdTransform a2s,a2b;
+			a2s.mult( w2s , a2w);
+			a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction);
+			a2s.multInverseLeft(w2s,a2s);
+			a2b.multInverseLeft(b2w, a2s);
+
+			//transform by motion B
+			impactTransA.mult(impactTransB, a2b);
+			//normalize rotation
+			SimdQuaternion orn;
+			impactTransA.getBasis().getRotation(orn);
+			orn.normalize();
+			impactTransA.setBasis(SimdMatrix3x3(orn));
+		}
+	}
+
+/*
+	{
+		const int numvertsB = m_convexB->GetNumVertices();
+		for (int v=0;v<numvertsB;v++)
+		{
+			SimdPoint3 pt;
+			m_convexB->GetVertex(v,pt);
+			pt = impactTransB * pt;
+			char buf[1000];
+
+			if (pt.y() < 0.)
+			{
+				sprintf(buf,"POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
+				if (debugFile)
+					fwrite(buf,1,strlen(buf),debugFile);
+			}
+			else
+			{
+				sprintf(buf,"POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
+				if (debugFile)
+					fwrite(buf,1,strlen(buf),debugFile);
+			}
+		}
+	}
+*/
+	return hit;
+}
+
+

Bullet/NarrowPhaseCollision/BU_CollisionPair.h

@@ -0,0 +1,54 @@
+/*
+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 BU_COLLISIONPAIR
+#define BU_COLLISIONPAIR
+
+#include <NarrowPhaseCollision/BU_Screwing.h>
+#include <NarrowPhaseCollision/ConvexCast.h>
+
+
+#include <SimdQuaternion.h>
+
+class PolyhedralConvexShape;
+
+
+///BU_CollisionPair implements collision algorithm for algebraic time of impact calculation of feature based shapes.
+class BU_CollisionPair : public ConvexCast
+{
+	
+public:
+	BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance=0.2f);
+	//toi
+
+	virtual bool	calcTimeOfImpact(
+					const SimdTransform& fromA,
+					const SimdTransform& toA,
+					const SimdTransform& fromB,
+					const SimdTransform& toB,
+					CastResult& result);
+
+	
+	
+
+private:
+	const PolyhedralConvexShape*	m_convexA;
+	const PolyhedralConvexShape*	m_convexB;
+	BU_Screwing	m_screwing;
+	SimdScalar	m_tolerance;
+	
+};
+#endif //BU_COLLISIONPAIR

Bullet/NarrowPhaseCollision/BU_EdgeEdge.cpp

@@ -0,0 +1,578 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "BU_EdgeEdge.h"
+#include "BU_Screwing.h"
+#include <SimdPoint3.h>
+#include <SimdPoint3.h>
+
+//#include "BU_IntervalArithmeticPolynomialSolver.h"
+#include "BU_AlgebraicPolynomialSolver.h"
+
+#define USE_ALGEBRAIC
+#ifdef USE_ALGEBRAIC	
+#define BU_Polynomial BU_AlgebraicPolynomialSolver
+#else	
+#define BU_Polynomial BU_IntervalArithmeticPolynomialSolver
+#endif
+
+BU_EdgeEdge::BU_EdgeEdge()
+{
+}
+
+
+bool BU_EdgeEdge::GetTimeOfImpact(
+								  const BU_Screwing& screwAB,
+								  const SimdPoint3& a,//edge in object A
+								  const SimdVector3& u,
+								  const SimdPoint3& c,//edge in object B
+								  const SimdVector3& v,
+								  SimdScalar &minTime,
+								  SimdScalar &lambda1,
+								  SimdScalar& mu1
+								  
+								  )
+{
+	bool hit=false;
+	
+	SimdScalar lambda;
+	SimdScalar mu;
+	
+	const SimdScalar w=screwAB.GetW();
+	const SimdScalar s=screwAB.GetS();
+	
+	if (SimdFuzzyZero(s) &&
+		SimdFuzzyZero(w))
+	{
+		//no motion, no collision
+		return false;
+	}
+	
+	if (SimdFuzzyZero(w) )
+	{
+		//pure translation W=0, S <> 0
+		//no trig, f(t)=t
+		SimdScalar det = u.y()*v.x()-u.x()*v.y();
+		if (!SimdFuzzyZero(det))
+		{		
+			lambda = (a.x()*v.y() - c.x() * v.y() - v.x() * a.y() + v.x() * c.y()) / det;
+			mu = (u.y() * a.x() - u.y() * c.x() - u.x() * a.y() + u.x() * c.y()) / det;
+
+			if (mu >=0 && mu <= 1 && lambda >= 0 && lambda <= 1)
+			{
+				// single potential collision is
+				SimdScalar t = (c.z()-a.z()+mu*v.z()-lambda*u.z())/s;
+				//if this is on the edge, and time t within [0..1] report hit
+				if (t>=0 && t <= minTime)
+				{
+					hit = true;
+					lambda1 = lambda;
+					mu1 = mu;
+					minTime=t;
+				}
+			}
+			
+		} else
+		{
+			//parallel case, not yet
+		}
+	} else
+	{
+		if (SimdFuzzyZero(s) )
+		{
+			if (SimdFuzzyZero(u.z()) )
+			{
+				if (SimdFuzzyZero(v.z()) )
+				{
+					//u.z()=0,v.z()=0
+					if (SimdFuzzyZero(a.z()-c.z()))
+					{
+						//printf("NOT YET planar problem, 4 vertex=edge cases\n");
+						
+					} else
+					{
+						//printf("parallel but distinct planes, no collision\n");
+						return false;
+					}
+					
+				} else
+				{
+					SimdScalar mu = (a.z() - c.z())/v.z();
+					if (0<=mu && mu <= 1)
+					{
+					//	printf("NOT YET//u.z()=0,v.z()<>0\n");
+					} else
+					{
+						return false;
+					}
+					
+				}
+			} else
+			{
+				//u.z()<>0
+				
+				if (SimdFuzzyZero(v.z()) )
+				{
+					//printf("u.z()<>0,v.z()=0\n");
+					lambda =  (c.z() - a.z())/u.z();
+					if (0<=lambda && lambda <= 1)
+					{
+						//printf("u.z()<>0,v.z()=0\n");
+						SimdPoint3 rotPt(a.x()+lambda * u.x(), a.y()+lambda * u.y(),0.f);
+						SimdScalar r2 = rotPt.length2();//px*px + py*py;
+						
+						//either y=a*x+b, or x = a*x+b...
+						//depends on whether value v.x() is zero or not
+						SimdScalar aa;
+						SimdScalar bb;
+						
+						if (SimdFuzzyZero(v.x()))
+						{
+							aa = v.x()/v.y();
+							bb= c.x()+  (-c.y() /v.y()) *v.x();
+						} else
+						{
+							//line is c+mu*v;
+							//x = c.x()+mu*v.x();
+							//mu = ((x-c.x())/v.x());
+							//y = c.y()+((x-c.x())/v.x())*v.y();
+							//y = c.y()+  (-c.x() /v.x()) *v.y() + (x /v.x())   *v.y();
+							//y = a*x+b,where a = v.y()/v.x(), b= c.y()+  (-c.x() /v.x()) *v.y();
+							aa = v.y()/v.x();
+							bb= c.y()+  (-c.x() /v.x()) *v.y();
+						}
+						
+						SimdScalar disc = aa*aa*r2 + r2 - bb*bb;
+						if (disc <0)
+						{
+							//edge doesn't intersect the circle (motion of the vertex)
+							return false;
+						}
+						SimdScalar rad = SimdSqrt(r2);
+						
+						if (SimdFuzzyZero(disc))
+						{
+							SimdPoint3 intersectPt;
+							
+							SimdScalar mu;
+							//intersectionPoint edge with circle;
+							if (SimdFuzzyZero(v.x()))
+							{
+								intersectPt.setY( (-2*aa*bb)/(2*(aa*aa+1)));
+								intersectPt.setX( aa*intersectPt.y()+bb );
+								mu = ((intersectPt.y()-c.y())/v.y());
+							} else
+							{
+								intersectPt.setX((-2*aa*bb)/(2*(aa*aa+1)));
+								intersectPt.setY(aa*intersectPt.x()+bb);
+								mu = ((intersectPt.getX()-c.getX())/v.getX());
+								
+							}
+							
+							if (0 <= mu && mu <= 1)
+							{
+								hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
+							}
+							//only one solution
+						} else
+						{
+							//two points...
+							//intersectionPoint edge with circle;
+							SimdPoint3 intersectPt;
+							//intersectionPoint edge with circle;
+							if (SimdFuzzyZero(v.x()))
+							{
+								SimdScalar mu;
+								
+								intersectPt.setY((-2.f*aa*bb+2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f)));
+								intersectPt.setX(aa*intersectPt.y()+bb);
+								mu = ((intersectPt.getY()-c.getY())/v.getY());
+								if (0.f <= mu && mu <= 1.f)
+								{
+									hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
+								}
+								intersectPt.setY((-2.f*aa*bb-2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f)));
+								intersectPt.setX(aa*intersectPt.y()+bb);
+								mu = ((intersectPt.getY()-c.getY())/v.getY());
+								if (0 <= mu && mu <= 1)
+								{
+									hit = hit || Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
+								}
+								
+							} else
+							{
+								SimdScalar mu;
+								
+								intersectPt.setX((-2.f*aa*bb+2.f*SimdSqrt(disc))/(2*(aa*aa+1.f)));
+								intersectPt.setY(aa*intersectPt.x()+bb);
+								mu = ((intersectPt.getX()-c.getX())/v.getX());
+								if (0 <= mu && mu <= 1)
+								{
+									hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
+								}
+								intersectPt.setX((-2.f*aa*bb-2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f)));
+								intersectPt.setY(aa*intersectPt.x()+bb);
+								mu = ((intersectPt.getX()-c.getX())/v.getX());
+								if (0.f <= mu && mu <= 1.f)
+								{
+									hit = hit || Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
+								}
+							}
+						}
+						
+						
+						
+						//int k=0;
+						
+					} else
+					{
+						return false;
+					}
+					
+					
+				} else
+				{
+					//u.z()<>0,v.z()<>0
+					//printf("general case with s=0\n");
+					hit = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu);
+					if (hit)
+					{
+						lambda1 = lambda;
+						mu1 = mu;
+						
+					}
+				}
+			}
+			
+		} else
+		{
+			//printf("general case, W<>0,S<>0\n");
+			hit = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu);
+			if (hit)
+			{
+				lambda1 = lambda;
+				mu1 = mu;
+			}
+			
+		}
+		
+		
+		//W <> 0,pure rotation
+	}
+	
+	return hit;
+}
+
+
+bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
+											 const BU_Screwing& screwAB,
+											 const SimdPoint3& a,//edge in object A
+											 const SimdVector3& u,
+											 const SimdPoint3& c,//edge in object B
+											 const SimdVector3& v,
+											 SimdScalar &minTime,
+											 SimdScalar &lambda,
+											 SimdScalar& mu
+											 
+											 )
+{
+	bool hit = false;
+	
+	SimdScalar coefs[4]={0.f,0.f,0.f,0.f};
+	BU_Polynomial polynomialSolver;
+	int numroots = 0;
+	
+	//SimdScalar eps=1e-15f;
+	//SimdScalar eps2=1e-20f;
+	SimdScalar s=screwAB.GetS();
+	SimdScalar w = screwAB.GetW();
+	
+	SimdScalar ax = a.x();
+	SimdScalar ay = a.y();
+	SimdScalar az = a.z();
+	SimdScalar cx = c.x();
+	SimdScalar cy = c.y();
+	SimdScalar cz = c.z();
+	SimdScalar vx = v.x();
+	SimdScalar vy = v.y();
+	SimdScalar vz = v.z();
+	SimdScalar ux = u.x();
+	SimdScalar uy = u.y();
+	SimdScalar uz = u.z();
+	
+	
+	if (!SimdFuzzyZero(v.z()))
+	{
+		
+		//Maple Autogenerated C code
+		SimdScalar t1,t2,t3,t4,t7,t8,t10;
+		SimdScalar t13,t14,t15,t16,t17,t18,t19,t20;
+		SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
+		SimdScalar t31,t32,t33,t34,t35,t36,t39,t40;
+		SimdScalar t41,t43,t48;
+		SimdScalar t63;
+		
+		SimdScalar aa,bb,cc,dd;//the coefficients
+		
+		t1 = v.y()*s;      t2 = t1*u.x();
+		t3 = v.x()*s;
+		t4 = t3*u.y();
+		t7 = SimdTan(w/2.0f);
+		t8 = 1.0f/t7;
+		t10 = 1.0f/v.z();
+		aa = (t2-t4)*t8*t10;
+		t13 = a.x()*t7;
+		t14 = u.z()*v.y();
+		t15 = t13*t14;
+		t16 = u.x()*v.z();
+		t17 = a.y()*t7;
+		t18 = t16*t17;
+		t19 = u.y()*v.z();
+		t20 = t13*t19;
+		t21 = v.y()*u.x();
+		t22 = c.z()*t7;
+		t23 = t21*t22;
+		t24 = v.x()*a.z();
+		t25 = t7*u.y();
+		t26 = t24*t25;
+		t27 = c.y()*t7;
+		t28 = t16*t27;
+		t29 = a.z()*t7;
+		t30 = t21*t29;
+		t31 = u.z()*v.x();
+		t32 = t31*t27;
+		t33 = t31*t17;
+		t34 = c.x()*t7;
+		t35 = t34*t19;
+		t36 = t34*t14;
+		t39 = v.x()*c.z();
+		t40 = t39*t25;
+		t41 = 2.0f*t1*u.y()-t15+t18-t20-t23-t26+t28+t30+t32+t33-t35-t36+2.0f*t3*u.x()+t40;
+		bb = t41*t8*t10;
+		t43 = t7*u.x();
+		t48 = u.y()*v.y();
+		cc = (-2.0f*t39*t43+2.0f*t24*t43+t4-2.0f*t48*t22+2.0f*t34*t16-2.0f*t31*t13-t2
+			-2.0f*t17*t14+2.0f*t19*t27+2.0f*t48*t29)*t8*t10;
+		t63 = -t36+t26+t32-t40+t23+t35-t20+t18-t28-t33+t15-t30;
+		dd = t63*t8*t10;
+		
+		coefs[0]=aa;
+		coefs[1]=bb;
+		coefs[2]=cc;
+		coefs[3]=dd;
+		
+	} else
+	{
+		
+		SimdScalar t1,t2,t3,t4,t7,t8,t10;
+		SimdScalar t13,t14,t15,t16,t17,t18,t19,t20;
+		SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
+		SimdScalar t31,t32,t33,t34,t35,t36,t37,t38,t57;
+		SimdScalar p1,p2,p3,p4;
+
+	  t1 = uy*s;
+      t2 = t1*vx;
+      t3 = ux*s;
+      t4 = t3*vy;
+      t7 = SimdTan(w/2.0f);
+      t8 = 1/t7;
+      t10 = 1/uz;
+      t13 = ux*az;
+      t14 = t7*vy;
+      t15 = t13*t14;
+      t16 = ax*t7;
+      t17 = uy*vz;
+      t18 = t16*t17;
+      t19 = cx*t7;
+      t20 = t19*t17;
+      t21 = vy*uz;
+      t22 = t19*t21;
+      t23 = ay*t7;
+      t24 = vx*uz;
+      t25 = t23*t24;
+      t26 = uy*cz;
+      t27 = t7*vx;
+      t28 = t26*t27;
+      t29 = t16*t21;
+      t30 = cy*t7;
+      t31 = ux*vz;
+      t32 = t30*t31;
+      t33 = ux*cz;
+      t34 = t33*t14;
+      t35 = t23*t31;
+      t36 = t30*t24;
+      t37 = uy*az;
+      t38 = t37*t27;
+
+	  p4 = (-t2+t4)*t8*t10;
+      p3 = 2.0f*t1*vy+t15-t18-t20-t22+t25+t28-t29+t32-t34+t35+t36-t38+2.0f*t3*vx;
+      p2 = -2.0f*t33*t27-2.0f*t26*t14-2.0f*t23*t21+2.0f*t37*t14+2.0f*t30*t17+2.0f*t13
+*t27+t2-t4+2.0f*t19*t31-2.0f*t16*t24;
+      t57 = -t22+t29+t36-t25-t32+t34+t35-t28-t15+t20-t18+t38;
+      p1 = t57*t8*t10;
+
+	coefs[0] = p4;
+	coefs[1] = p3;
+	coefs[2] = p2;
+	coefs[1] = p1;
+		
+	}
+	
+	numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
+	
+	for (int i=0;i<numroots;i++)
+	{
+		//SimdScalar tau = roots[i];//polynomialSolver.GetRoot(i);
+		SimdScalar tau = polynomialSolver.GetRoot(i);
+		
+		//check whether mu and lambda are in range [0..1]
+		
+		if (!SimdFuzzyZero(v.z()))
+		{
+			SimdScalar A1=(ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz);
+			SimdScalar B1=((1.f+tau*tau)*(cx*SimdTan(1.f/2.f*w)*vz+
+				vx*az*SimdTan(1.f/2.f*w)-vx*cz*SimdTan(1.f/2.f*w)+
+				vx*s*tau)/SimdTan(1.f/2.f*w)/vz)-(ax-ax*tau*tau-2.f*tau*ay);
+			lambda = B1/A1;
+			
+			mu = (a.z()-c.z()+lambda*u.z()+(s*tau)/(SimdTan(w/2.f)))/v.z();
+			
+			
+			//double check in original equation
+			
+			SimdScalar lhs = (a.x()+lambda*u.x())
+				*((1.f-tau*tau)/(1.f+tau*tau))-
+				(a.y()+lambda*u.y())*((2.f*tau)/(1.f+tau*tau));
+			
+			lhs = lambda*((ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz));
+			
+			SimdScalar rhs = c.x()+mu*v.x();
+			
+			rhs = ((1.f+tau*tau)*(cx*SimdTan(1.f/2.f*w)*vz+vx*az*SimdTan(1.f/2.f*w)-
+				vx*cz*SimdTan(1.f/2.f*w)+vx*s*tau)/(SimdTan(1.f/2.f*w)*vz))-
+				
+				(ax-ax*tau*tau-2.f*tau*ay);
+			
+			/*SimdScalar res = coefs[0]*tau*tau*tau+
+				coefs[1]*tau*tau+
+				coefs[2]*tau+
+				coefs[3];*/
+			
+			//lhs should be rhs !
+			
+			if (0.<= mu && mu <=1 && 0.<=lambda && lambda <= 1)
+			{
+				
+			} else
+			{
+				//skip this solution, not really touching
+				continue;				
+			}
+			
+		}
+		
+		SimdScalar t = 2.f*SimdAtan(tau)/screwAB.GetW();
+		//tau = tan (wt/2) so 2*atan (tau)/w
+		if (t>=0.f && t<minTime)
+		{
+#ifdef STATS_EDGE_EDGE
+			printf(" ax = %12.12f\n ay = %12.12f\n az = %12.12f\n",a.x(),a.y(),a.z());
+			printf(" ux = %12.12f\n uy = %12.12f\n uz = %12.12f\n",u.x(),u.y(),u.z());
+			printf(" cx = %12.12f\n cy = %12.12f\n cz = %12.12f\n",c.x(),c.y(),c.z());
+			printf(" vx = %12.12f\n vy = %12.12f\n vz = %12.12f\n",v.x(),v.y(),v.z());
+			printf(" s  = %12.12f\n w  = %12.12f\n",       s,     w);
+			
+			printf(" tau = %12.12f \n lambda = %12.12f \n mu = %f\n",tau,lambda,mu); 
+			printf(" ---------------------------------------------\n"); 
+			
+#endif
+			
+			//	v,u,a,c,s,w
+			
+			//	BU_IntervalArithmeticPolynomialSolver iaSolver;
+			//	int numroots2 = iaSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
+			
+			minTime = t;
+			hit = true;
+		}
+	}
+	
+	return hit;
+}
+
+
+//C -S
+//S C
+
+bool BU_EdgeEdge::Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar rotRadius, SimdScalar rotW,const SimdPoint3& intersectPt,SimdScalar& minTime)
+{
+	bool hit = false;
+	
+	// now calculate the planeEquation for the vertex motion,
+	// and check if the intersectionpoint is at the positive side
+	SimdPoint3 rotPt1(SimdCos(rotW)*rotPt.x()-SimdSin(rotW)*rotPt.y(),
+	SimdSin(rotW)*rotPt.x()+SimdCos(rotW)*rotPt.y(),
+	0.f);
+
+	SimdVector3 rotVec = rotPt1-rotPt;
+	
+	SimdVector3 planeNormal( -rotVec.y() , rotVec.x() ,0.f);
+	
+	//SimdPoint3 pt(a.x(),a.y());//for sake of readability,could write dot directly
+	SimdScalar planeD = planeNormal.dot(rotPt1);
+	
+	SimdScalar dist = (planeNormal.dot(intersectPt)-planeD);
+	hit = (dist >= -0.001);
+	
+	//if (hit)
+	{
+		//		minTime = 0;
+		//calculate the time of impact, using the fact of
+		//toi = alpha / screwAB.getW();
+		// cos (alpha) = adjacent/hypothenuse;
+		//adjacent = dotproduct(ipedge,point);
+		//hypothenuse = sqrt(r2);
+		SimdScalar adjacent = intersectPt.dot(rotPt)/rotRadius;
+		SimdScalar hypo = rotRadius;
+		SimdScalar alpha = SimdAcos(adjacent/hypo);
+		SimdScalar t = alpha / rotW;
+		if (t >= 0 && t < minTime)
+		{
+			hit = true;
+			minTime = t;
+		} else
+		{
+			hit = false;
+		}
+		
+	}
+	return hit;
+}
+
+bool BU_EdgeEdge::GetTimeOfImpactVertexEdge(
+											const BU_Screwing& screwAB,
+											const SimdPoint3& a,//edge in object A
+											const SimdVector3& u,
+											const SimdPoint3& c,//edge in object B
+											const SimdVector3& v,
+											SimdScalar &minTime,
+											SimdScalar &lamda,
+											SimdScalar& mu
+											
+											)
+{
+	return false;
+}

Bullet/NarrowPhaseCollision/BU_EdgeEdge.h

@@ -0,0 +1,76 @@
+/*
+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 BU_EDGEEDGE
+#define BU_EDGEEDGE
+
+class BU_Screwing;
+#include <SimdTransform.h>
+#include <SimdPoint3.h>
+#include <SimdVector3.h>
+
+//class BUM_Point2;
+
+#include <SimdScalar.h>
+
+///BU_EdgeEdge implements algebraic time of impact calculation between two (angular + linear) moving edges.
+class BU_EdgeEdge
+{
+public:
+	
+
+	BU_EdgeEdge();
+	bool GetTimeOfImpact(
+		const BU_Screwing& screwAB,
+		const SimdPoint3& a,//edge in object A
+		const SimdVector3& u,
+		const SimdPoint3& c,//edge in object B
+		const SimdVector3& v,
+		SimdScalar &minTime,
+		SimdScalar &lamda,
+		SimdScalar& mu
+		);
+private:
+
+	bool Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar rotRadius, SimdScalar rotW,const SimdPoint3& intersectPt,SimdScalar& minTime);
+	bool GetTimeOfImpactGeneralCase(
+		const BU_Screwing& screwAB,
+		const SimdPoint3& a,//edge in object A
+		const SimdVector3& u,
+		const SimdPoint3& c,//edge in object B
+		const SimdVector3& v,
+		SimdScalar &minTime,
+		SimdScalar &lamda,
+		SimdScalar& mu
+
+		);
+
+	
+	bool GetTimeOfImpactVertexEdge(
+		const BU_Screwing& screwAB,
+		const SimdPoint3& a,//edge in object A
+		const SimdVector3& u,
+		const SimdPoint3& c,//edge in object B
+		const SimdVector3& v,
+		SimdScalar &minTime,
+		SimdScalar &lamda,
+		SimdScalar& mu
+
+		);
+
+};
+
+#endif //BU_EDGEEDGE

Bullet/NarrowPhaseCollision/BU_MotionStateInterface.h

@@ -0,0 +1,50 @@
+/*
+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 BU_MOTIONSTATE
+#define BU_MOTIONSTATE
+
+
+#include <SimdTransform.h>
+#include <SimdPoint3.h>
+#include <SimdQuaternion.h>
+
+class BU_MotionStateInterface
+{
+public:
+	virtual ~BU_MotionStateInterface(){};
+
+	virtual void	SetTransform(const SimdTransform& trans) = 0;
+	virtual void	GetTransform(SimdTransform& trans) const = 0; 
+
+	virtual void	SetPosition(const SimdPoint3& position) = 0;
+	virtual void	GetPosition(SimdPoint3& position) const = 0; 
+
+	virtual void	SetOrientation(const SimdQuaternion& orientation) = 0;
+	virtual void	GetOrientation(SimdQuaternion& orientation) const = 0; 
+
+	virtual void	SetBasis(const SimdMatrix3x3& basis) = 0;
+	virtual void	GetBasis(SimdMatrix3x3& basis) const = 0; 
+
+	virtual void	SetLinearVelocity(const SimdVector3& linvel) = 0;
+	virtual void	GetLinearVelocity(SimdVector3& linvel) const = 0;
+	
+	virtual void	GetAngularVelocity(SimdVector3& angvel) const = 0;
+	virtual void	SetAngularVelocity(const SimdVector3& angvel) = 0;
+
+};
+
+#endif //BU_MOTIONSTATE

Bullet/NarrowPhaseCollision/BU_PolynomialSolverInterface.h

@@ -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 BUM_POLYNOMIAL_SOLVER_INTERFACE
+#define BUM_POLYNOMIAL_SOLVER_INTERFACE
+
+#include <SimdScalar.h>
+//
+//BUM_PolynomialSolverInterface is interface class for polynomial root finding.
+//The number of roots is returned as a result, query GetRoot to get the actual solution.
+//
+class BUM_PolynomialSolverInterface
+{
+public:
+	virtual ~BUM_PolynomialSolverInterface() {};
+
+	
+//	virtual int Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c) = 0;
+	
+	virtual int	Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c) = 0;
+	
+	virtual int Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d) = 0;
+
+	virtual SimdScalar GetRoot(int i) const = 0;
+
+};
+
+#endif //BUM_POLYNOMIAL_SOLVER_INTERFACE

Bullet/NarrowPhaseCollision/BU_Screwing.cpp

@@ -0,0 +1,200 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Stephane Redon / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "BU_Screwing.h"
+
+
+BU_Screwing::BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngVel) {
+
+
+	const SimdScalar dx=relLinVel[0];
+	const SimdScalar dy=relLinVel[1];
+	const SimdScalar dz=relLinVel[2];
+	const SimdScalar wx=relAngVel[0];
+	const SimdScalar wy=relAngVel[1];
+	const SimdScalar wz=relAngVel[2];
+
+	// Compute the screwing parameters :
+	// w : total amount of rotation
+	// s : total amount of translation
+	// u : vector along the screwing axis (||u||=1)
+	// o : point on the screwing axis
+	
+	m_w=SimdSqrt(wx*wx+wy*wy+wz*wz);
+	//if (!w) {
+	if (fabs(m_w)<SCREWEPSILON ) {
+
+		assert(m_w == 0.f);
+
+		m_w=0.;
+		m_s=SimdSqrt(dx*dx+dy*dy+dz*dz);
+		if (fabs(m_s)<SCREWEPSILON ) {
+			assert(m_s == 0.);
+
+			m_s=0.;
+			m_u=SimdPoint3(0.,0.,1.);
+			m_o=SimdPoint3(0.,0.,0.);
+		}
+		else {
+			float t=1.f/m_s;
+			m_u=SimdPoint3(dx*t,dy*t,dz*t);
+			m_o=SimdPoint3(0.f,0.f,0.f);
+		}
+	}
+	else { // there is some rotation
+		
+		// we compute u
+		
+		float v(1.f/m_w);
+		m_u=SimdPoint3(wx*v,wy*v,wz*v); // normalization
+		
+		// decomposition of the translation along u and one orthogonal vector
+		
+		SimdPoint3 t(dx,dy,dz);
+		m_s=t.dot(m_u); // component along u
+		if (fabs(m_s)<SCREWEPSILON)
+		{
+			//printf("m_s component along u < SCREWEPSILION\n");
+			m_s=0.f;
+		}
+		SimdPoint3 n1(t-(m_s*m_u)); // the remaining part (which is orthogonal to u)
+		
+		// now we have to compute o
+		
+		//SimdScalar len = n1.length2();
+		//(len >= BUM_EPSILON2) {
+		if (n1[0] || n1[1] || n1[2]) { // n1 is not the zero vector
+			n1.normalize();
+			SimdVector3 n1orth=m_u.cross(n1);
+
+			float n2x=SimdCos(0.5f*m_w);
+			float n2y=SimdSin(0.5f*m_w);
+			
+			m_o=0.5f*t.dot(n1)*(n1+n2x/n2y*n1orth);
+		}
+		else 
+		{
+			m_o=SimdPoint3(0.f,0.f,0.f);
+		}
+		
+	}
+	
+}
+
+//Then, I need to compute Pa, the matrix from the reference (global) frame to
+//the screwing frame :
+
+
+void BU_Screwing::LocalMatrix(SimdTransform &t) const {
+//So the whole computations do this : align the Oz axis along the
+//	screwing axis (thanks to u), and then find two others orthogonal axes to
+//	complete the basis.
+		
+		if ((m_u[0]>SCREWEPSILON)||(m_u[0]<-SCREWEPSILON)||(m_u[1]>SCREWEPSILON)||(m_u[1]<-SCREWEPSILON)) 
+		{ 
+			// to avoid numerical problems
+			float n=SimdSqrt(m_u[0]*m_u[0]+m_u[1]*m_u[1]);
+			float invn=1.0f/n;
+			SimdMatrix3x3 mat;
+
+	  		mat[0][0]=-m_u[1]*invn;
+			mat[0][1]=m_u[0]*invn;
+			mat[0][2]=0.f;
+	
+			mat[1][0]=-m_u[0]*invn*m_u[2];
+			mat[1][1]=-m_u[1]*invn*m_u[2];
+			mat[1][2]=n;
+
+			mat[2][0]=m_u[0];
+			mat[2][1]=m_u[1];
+			mat[2][2]=m_u[2];
+			
+			t.setOrigin(SimdPoint3(
+				  m_o[0]*m_u[1]*invn-m_o[1]*m_u[0]*invn,
+				-(m_o[0]*mat[1][0]+m_o[1]*mat[1][1]+m_o[2]*n),
+				-(m_o[0]*m_u[0]+m_o[1]*m_u[1]+m_o[2]*m_u[2])));
+
+			t.setBasis(mat);
+
+		}
+		else {
+
+			SimdMatrix3x3 m;
+
+			m[0][0]=1.;
+			m[1][0]=0.;
+			m[2][0]=0.;
+
+			m[0][1]=0.f;
+			m[1][1]=float(SimdSign(m_u[2]));
+			m[2][1]=0.f;
+
+			m[0][2]=0.f;
+			m[1][2]=0.f;
+			m[2][2]=float(SimdSign(m_u[2]));
+
+			t.setOrigin(SimdPoint3(
+					-m_o[0],
+					-SimdSign(m_u[2])*m_o[1],
+					-SimdSign(m_u[2])*m_o[2]
+				));
+			t.setBasis(m);
+
+		}
+}
+
+//gives interpolated transform for time in [0..1] in screwing frame
+SimdTransform	BU_Screwing::InBetweenTransform(const SimdTransform& tr,SimdScalar t) const
+{
+	SimdPoint3 org = tr.getOrigin();
+
+	SimdPoint3 neworg (
+	org.x()*SimdCos(m_w*t)-org.y()*SimdSin(m_w*t),
+	org.x()*SimdSin(m_w*t)+org.y()*SimdCos(m_w*t),
+	org.z()+m_s*CalculateF(t));
+		
+	SimdTransform newtr;
+	newtr.setOrigin(neworg);
+	SimdMatrix3x3 basis = tr.getBasis();
+	SimdMatrix3x3 basisorg = tr.getBasis();
+
+	SimdQuaternion rot(SimdVector3(0.,0.,1.),m_w*t);
+	SimdQuaternion tmpOrn;
+	tr.getBasis().getRotation(tmpOrn);
+	rot = rot *  tmpOrn;
+
+	//to avoid numerical drift, normalize quaternion
+	rot.normalize();
+	newtr.setBasis(SimdMatrix3x3(rot));
+	return newtr;
+
+}
+
+
+SimdScalar BU_Screwing::CalculateF(SimdScalar t) const
+{
+	SimdScalar result;
+	if (!m_w)
+	{
+		result = t;
+	} else
+	{
+		result = ( SimdTan((m_w*t)/2.f) / SimdTan(m_w/2.f));
+	}
+	return result;
+}
+	

Bullet/NarrowPhaseCollision/BU_Screwing.h

@@ -0,0 +1,77 @@
+/*
+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 B_SCREWING_H
+#define B_SCREWING_H
+
+
+#include <SimdVector3.h>
+#include <SimdPoint3.h>
+#include <SimdTransform.h>
+
+
+#define SCREWEPSILON 0.00001f
+
+///BU_Screwing implements screwing motion interpolation.
+class BU_Screwing
+{
+public:
+
+	
+	BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngVel);
+
+	~BU_Screwing() {
+	};
+	
+	SimdScalar CalculateF(SimdScalar t) const;
+	//gives interpolated position for time in [0..1] in screwing frame
+
+	inline SimdPoint3	InBetweenPosition(const SimdPoint3& pt,SimdScalar t) const
+	{
+		return SimdPoint3(
+		pt.x()*SimdCos(m_w*t)-pt.y()*SimdSin(m_w*t),
+		pt.x()*SimdSin(m_w*t)+pt.y()*SimdCos(m_w*t),
+		pt.z()+m_s*CalculateF(t));
+	}
+
+	inline SimdVector3	InBetweenVector(const SimdVector3& vec,SimdScalar t) const
+	{
+		return SimdVector3(
+		vec.x()*SimdCos(m_w*t)-vec.y()*SimdSin(m_w*t),
+		vec.x()*SimdSin(m_w*t)+vec.y()*SimdCos(m_w*t),
+		vec.z());
+	}
+
+	//gives interpolated transform for time in [0..1] in screwing frame
+	SimdTransform	InBetweenTransform(const SimdTransform& tr,SimdScalar t) const;
+
+	
+	//gives matrix from global frame into screwing frame
+	void	LocalMatrix(SimdTransform &t) const;
+
+	inline const SimdVector3& GetU() const {	return m_u;}
+	inline const SimdVector3& GetO() const {return m_o;}
+	inline const SimdScalar GetS() const{ return m_s;}
+	inline const SimdScalar GetW() const { return m_w;}
+	
+private:
+	float		m_w;
+	float		m_s;
+	SimdVector3 m_u;
+	SimdVector3	m_o;
+};
+
+#endif //B_SCREWING_H

Bullet/NarrowPhaseCollision/BU_StaticMotionState.h

@@ -0,0 +1,91 @@
+/*
+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 BU_STATIC_MOTIONSTATE
+#define BU_STATIC_MOTIONSTATE
+
+
+#include <CollisionShapes/BU_MotionStateInterface.h>
+
+class BU_StaticMotionState :public BU_MotionStateInterface
+{
+public:
+	virtual ~BU_StaticMotionState(){};
+
+	virtual void	SetTransform(const SimdTransform& trans)
+	{
+		m_trans = trans;
+	}
+	virtual void	GetTransform(SimdTransform& trans) const
+	{
+		trans = m_trans;
+	}
+	virtual void	SetPosition(const SimdPoint3& position)
+	{
+		m_trans.setOrigin( position );
+	}
+	virtual void	GetPosition(SimdPoint3& position) const
+	{
+		position = m_trans.getOrigin();
+	}
+
+	virtual void	SetOrientation(const SimdQuaternion& orientation)
+	{
+		m_trans.setRotation( orientation);
+	}
+	virtual void	GetOrientation(SimdQuaternion& orientation) const
+	{
+		orientation = m_trans.getRotation();
+	}
+
+	virtual void	SetBasis(const SimdMatrix3x3& basis)
+	{
+		m_trans.setBasis( basis);
+	}
+	virtual void	GetBasis(SimdMatrix3x3& basis) const
+	{ 
+		basis = m_trans.getBasis();
+	}
+
+	virtual void	SetLinearVelocity(const SimdVector3& linvel)
+	{
+		m_linearVelocity = linvel;
+	}
+	virtual void	GetLinearVelocity(SimdVector3& linvel) const
+	{
+		linvel = m_linearVelocity;
+	}
+	
+	virtual void	SetAngularVelocity(const SimdVector3& angvel)
+	{
+		m_angularVelocity = angvel;
+	}
+	virtual void	GetAngularVelocity(SimdVector3& angvel) const
+	{
+		angvel = m_angularVelocity;
+	}
+
+
+
+protected:
+
+	SimdTransform	m_trans;
+	SimdVector3		m_angularVelocity;
+	SimdVector3		m_linearVelocity;
+
+};
+
+#endif //BU_STATIC_MOTIONSTATE

Bullet/NarrowPhaseCollision/BU_VertexPoly.cpp

@@ -0,0 +1,159 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "BU_VertexPoly.h"
+#include "BU_Screwing.h"
+#include <SimdTransform.h>
+#include <SimdPoint3.h>
+#include <SimdVector3.h>
+
+#define USE_ALGEBRAIC
+#ifdef USE_ALGEBRAIC
+	#include "BU_AlgebraicPolynomialSolver.h"
+	#define BU_Polynomial BU_AlgebraicPolynomialSolver
+#else
+	#include "BU_IntervalArithmeticPolynomialSolver.h"
+	#define BU_Polynomial BU_IntervalArithmeticPolynomialSolver
+#endif
+
+inline bool      TestFuzzyZero(SimdScalar x) { return SimdFabs(x) < 0.0001f; }
+
+
+BU_VertexPoly::BU_VertexPoly()
+{
+
+}
+//return true if a collision will occur between [0..1]
+//false otherwise. If true, minTime contains the time of impact
+bool BU_VertexPoly::GetTimeOfImpact(
+		const BU_Screwing& screwAB,
+		const SimdPoint3& a,
+		const SimdVector4& planeEq,
+		SimdScalar &minTime,bool swapAB)
+{
+
+	bool hit = false;
+
+	// precondition: s=0 and w= 0 is catched by caller!
+	if (TestFuzzyZero(screwAB.GetS()) &&
+		TestFuzzyZero(screwAB.GetW()))
+	{
+		return false;
+	}
+
+
+	//case w<>0 and s<> 0
+	const SimdScalar w=screwAB.GetW();
+	const SimdScalar s=screwAB.GetS();
+
+	SimdScalar coefs[4];
+	const SimdScalar p=planeEq[0];
+	const SimdScalar q=planeEq[1];
+	const SimdScalar r=planeEq[2];
+	const SimdScalar d=planeEq[3];
+
+	const SimdVector3 norm(p,q,r);
+	BU_Polynomial polynomialSolver;
+	int numroots = 0;
+
+	//SimdScalar eps=1e-80f;
+	//SimdScalar eps2=1e-100f;
+	
+	if (TestFuzzyZero(screwAB.GetS()) )	
+	{
+		//S = 0 , W <> 0
+		
+		//ax^3+bx^2+cx+d=0
+		coefs[0]=0.;
+		coefs[1]=(-p*a.x()-q*a.y()+r*a.z()-d);
+		coefs[2]=-2*p*a.y()+2*q*a.x();
+		coefs[3]=p*a.x()+q*a.y()+r*a.z()-d;
+
+//		numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
+		numroots = polynomialSolver.Solve2QuadraticFull(coefs[1],coefs[2],coefs[3]);
+
+	} else
+	{
+		if (TestFuzzyZero(screwAB.GetW()))
+		{
+			// W = 0 , S <> 0
+			//pax+qay+r(az+st)=d
+			
+			SimdScalar dist = (d - a.dot(norm));
+
+			if (TestFuzzyZero(r))
+			{
+				if (TestFuzzyZero(dist))
+				{
+					// no hit
+				} else
+				{
+					// todo a a' might hit sides of polygon T
+					//printf("unhandled case, w=0,s<>0,r<>0, a a' might hit sides of polygon T \n");
+				}
+				
+			} else
+			{
+				SimdScalar etoi = (dist)/(r*screwAB.GetS());
+				if (swapAB)
+					etoi *= -1;
+
+				if (etoi >= 0. && etoi <= minTime)
+				{
+					minTime = etoi;
+					hit = true;
+				}
+			}
+
+		} else
+		{
+				//ax^3+bx^2+cx+d=0
+
+				//degenerate coefficients mess things up :(
+				SimdScalar ietsje = (r*s)/SimdTan(w/2.f);
+				if (ietsje*ietsje < 0.01f)
+					ietsje = 0.f;
+
+				coefs[0]=ietsje;//(r*s)/tan(w/2.);
+				coefs[1]=(-p*a.x()-q*a.y()+r*a.z()-d);
+				coefs[2]=-2.f*p*a.y()+2.f*q*a.x()+ietsje;//((r*s)/(tan(w/2.)));
+				coefs[3]=p*a.x()+q*a.y()+r*a.z()-d;
+
+				numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
+		}
+	}
+
+
+	for (int i=0;i<numroots;i++)
+	{
+		SimdScalar tau = polynomialSolver.GetRoot(i);
+
+		SimdScalar t = 2.f*SimdAtan(tau)/w;
+		//tau = tan (wt/2) so 2*atan (tau)/w
+		if (swapAB)
+		{
+			t *= -1.;
+		}
+		if (t>=0 && t<minTime)
+		{
+			minTime = t;
+			hit = true;
+		}
+	}
+	
+	return hit;
+}

Bullet/NarrowPhaseCollision/BU_VertexPoly.h

@@ -0,0 +1,43 @@
+/*
+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 VERTEX_POLY_H
+#define VERTEX_POLY_H
+
+
+class BU_Screwing;
+#include <SimdTransform.h>
+#include <SimdPoint3.h>
+#include <SimdScalar.h>
+
+///BU_VertexPoly implements algebraic time of impact calculation between vertex and a plane.
+class BU_VertexPoly
+{
+public:
+	BU_VertexPoly();
+	bool GetTimeOfImpact(
+		const BU_Screwing& screwAB,
+		const SimdPoint3& vtx,
+		const SimdVector4& planeEq,
+		SimdScalar &minTime,
+		bool swapAB);
+
+private:
+
+	//cached data (frame coherency etc.) here
+
+};
+#endif //VERTEX_POLY_H

Bullet/NarrowPhaseCollision/CollisionMargin.h

@@ -0,0 +1,26 @@
+/*
+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 COLLISION_MARGIN_H
+#define COLLISION_MARGIN_H
+
+//used by Gjk and some other algorithms
+
+#define CONVEX_DISTANCE_MARGIN 0.04f// 0.1f//;//0.01f
+
+
+
+#endif //COLLISION_MARGIN_H
+

Bullet/NarrowPhaseCollision/ContinuousConvexCollision.cpp

@@ -0,0 +1,200 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "ContinuousConvexCollision.h"
+#include "CollisionShapes/ConvexShape.h"
+#include "CollisionShapes/MinkowskiSumShape.h"
+#include "NarrowPhaseCollision/SimplexSolverInterface.h"
+#include "SimdTransformUtil.h"
+#include "CollisionShapes/SphereShape.h"
+
+#include "GjkPairDetector.h"
+#include "PointCollector.h"
+
+
+
+ContinuousConvexCollision::ContinuousConvexCollision ( ConvexShape*	convexA,ConvexShape*	convexB,SimplexSolverInterface* simplexSolver, ConvexPenetrationDepthSolver* penetrationDepthSolver)
+:m_simplexSolver(simplexSolver),
+m_penetrationDepthSolver(penetrationDepthSolver),
+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
+
+bool	ContinuousConvexCollision::calcTimeOfImpact(
+				const SimdTransform& fromA,
+				const SimdTransform& toA,
+				const SimdTransform& fromB,
+				const SimdTransform& toB,
+				CastResult& result)
+{
+
+	m_simplexSolver->reset();
+
+	/// compute linear and angular velocity for this interval, to interpolate
+	SimdVector3 linVelA,angVelA,linVelB,angVelB;
+	SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linVelA,angVelA);
+	SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linVelB,angVelB);
+
+	SimdScalar boundingRadiusA = m_convexA->GetAngularMotionDisc();
+	SimdScalar boundingRadiusB = m_convexB->GetAngularMotionDisc();
+
+	SimdScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
+
+	float radius = 0.001f;
+
+	SimdScalar lambda = 0.f;
+	SimdVector3 v(1,0,0);
+
+	int maxIter = MAX_ITERATIONS;
+
+	SimdVector3 n;
+	n.setValue(0.f,0.f,0.f);
+	bool hasResult = false;
+	SimdVector3 c;
+
+	float lastLambda = lambda;
+	//float epsilon = 0.001f;
+
+	int numIter = 0;
+	//first solution, using GJK
+
+
+	SimdTransform identityTrans;
+	identityTrans.setIdentity();
+
+	SphereShape	raySphere(0.0f);
+	raySphere.SetMargin(0.f);
+
+
+//	result.DrawCoordSystem(sphereTr);
+
+	PointCollector	pointCollector1;
+
+	{
+		
+		GjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);		
+		GjkPairDetector::ClosestPointInput input;
+	
+		//we don't use margins during CCD
+		gjk.SetIgnoreMargin(true);
+
+		input.m_transformA = fromA;
+		input.m_transformB = fromB;
+		gjk.GetClosestPoints(input,pointCollector1,0);
+
+		hasResult = pointCollector1.m_hasResult;
+		c = pointCollector1.m_pointInWorld;
+	}
+
+	if (hasResult)
+	{
+		SimdScalar dist;
+		dist = pointCollector1.m_distance;
+		n = pointCollector1.m_normalOnBInWorld;
+		
+		//not close enough
+		while (dist > radius)
+		{
+			numIter++;
+			if (numIter > maxIter)
+				return false; //todo: report a failure
+
+			float dLambda = 0.f;
+
+			//calculate safe moving fraction from distance / (linear+rotational velocity)
+			
+			//float clippedDist  = GEN_min(angularConservativeRadius,dist);
+			//float clippedDist  = dist;
+			
+			float projectedLinearVelocity = (linVelB-linVelA).dot(n);
+			
+			dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
+
+			lambda = lambda + dLambda;
+
+			if (lambda > 1.f)
+				return false;
+
+			if (lambda < 0.f)
+				return false;
+
+			//todo: next check with relative epsilon
+			if (lambda <= lastLambda)
+				break;
+			lastLambda = lambda;
+
+			
+
+			//interpolate to next lambda
+			SimdTransform interpolatedTransA,interpolatedTransB,relativeTrans;
+
+			SimdTransformUtil::IntegrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
+			SimdTransformUtil::IntegrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
+			relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
+
+			result.DebugDraw( lambda );
+
+			PointCollector	pointCollector;
+			GjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
+			GjkPairDetector::ClosestPointInput input;
+			input.m_transformA = interpolatedTransA;
+			input.m_transformB = interpolatedTransB;
+			gjk.GetClosestPoints(input,pointCollector,0);
+			if (pointCollector.m_hasResult)
+			{
+				if (pointCollector.m_distance < 0.f)
+				{
+					//degenerate ?!
+					result.m_fraction = lastLambda;
+					result.m_normal = n;
+					return true;
+				}
+				c = pointCollector.m_pointInWorld;		
+				
+				dist = pointCollector.m_distance;
+			} else
+			{
+				//??
+				return false;
+			}
+
+		}
+
+		result.m_fraction = lambda;
+		result.m_normal = n;
+		return true;
+	}
+
+	return false;
+
+/*
+//todo:
+	//if movement away from normal, discard result
+	SimdVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
+	if (result.m_fraction < 1.f)
+	{
+		if (move.dot(result.m_normal) <= 0.f)
+		{
+		}
+	}
+*/
+
+}
+

Bullet/NarrowPhaseCollision/ContinuousConvexCollision.h

@@ -0,0 +1,52 @@
+/*
+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 CONTINUOUS_COLLISION_CONVEX_CAST_H
+#define CONTINUOUS_COLLISION_CONVEX_CAST_H
+
+#include "ConvexCast.h"
+#include "SimplexSolverInterface.h"
+class ConvexPenetrationDepthSolver;
+class ConvexShape;
+
+/// ContinuousConvexCollision implements angular and linear time of impact for convex objects.
+/// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
+/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
+/// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
+class ContinuousConvexCollision : public ConvexCast
+{
+	SimplexSolverInterface* m_simplexSolver;
+	ConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
+	ConvexShape*	m_convexA;
+	ConvexShape*	m_convexB;
+
+
+public:
+
+	ContinuousConvexCollision (ConvexShape*	shapeA,ConvexShape*	shapeB ,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver* penetrationDepthSolver);
+
+	virtual bool	calcTimeOfImpact(
+				const SimdTransform& fromA,
+				const SimdTransform& toA,
+				const SimdTransform& fromB,
+				const SimdTransform& toB,
+				CastResult& result);
+
+
+};
+
+#endif //CONTINUOUS_COLLISION_CONVEX_CAST_H
+

Bullet/NarrowPhaseCollision/ConvexCast.cpp

@@ -0,0 +1,20 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "ConvexCast.h"
+
+ConvexCast::~ConvexCast()
+{
+}

Bullet/NarrowPhaseCollision/ConvexCast.h

@@ -0,0 +1,71 @@
+/*
+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 CONVEX_CAST_H
+#define CONVEX_CAST_H
+
+#include <SimdTransform.h>
+#include <SimdVector3.h>
+#include <SimdScalar.h>
+class MinkowskiSumShape;
+#include "IDebugDraw.h"
+
+/// ConvexCast is an interface for Casting
+class ConvexCast
+{
+public:
+
+
+	virtual ~ConvexCast();
+
+	///RayResult stores the closest result
+	/// alternatively, add a callback method to decide about closest/all results
+	struct	CastResult
+	{
+		//virtual bool	addRayResult(const SimdVector3& normal,SimdScalar	fraction) = 0;
+				
+		virtual void	DebugDraw(SimdScalar	fraction) {}
+		virtual void	DrawCoordSystem(const SimdTransform& trans) {}
+
+		CastResult()
+			:m_fraction(1e30f),
+			m_debugDrawer(0)
+		{
+		}
+
+
+		virtual ~CastResult() {};
+
+		SimdVector3	m_normal;
+		SimdScalar	m_fraction;
+		SimdTransform	m_hitTransformA;
+		SimdTransform	m_hitTransformB;
+
+		IDebugDraw* m_debugDrawer;
+
+	};
+
+
+	/// cast a convex against another convex object
+	virtual bool	calcTimeOfImpact(
+					const SimdTransform& fromA,
+					const SimdTransform& toA,
+					const SimdTransform& fromB,
+					const SimdTransform& toB,
+					CastResult& result) = 0;
+};
+
+#endif //CONVEX_CAST_H

Bullet/NarrowPhaseCollision/ConvexPenetrationDepthSolver.h

@@ -0,0 +1,42 @@
+/*
+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 CONVEX_PENETRATION_DEPTH_H
+#define CONVEX_PENETRATION_DEPTH_H
+
+class SimdVector3;
+#include "SimplexSolverInterface.h"
+class ConvexShape;
+#include "SimdPoint3.h"
+class SimdTransform;
+
+///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
+class ConvexPenetrationDepthSolver
+{
+public:	
+	
+	virtual ~ConvexPenetrationDepthSolver() {};
+	virtual bool CalcPenDepth( SimplexSolverInterface& simplexSolver,
+		ConvexShape* convexA,ConvexShape* convexB,
+					const SimdTransform& transA,const SimdTransform& transB,
+				SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb,
+				class IDebugDraw* debugDraw
+				) = 0;
+
+
+};
+#endif //CONVEX_PENETRATION_DEPTH_H
+

Bullet/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h

@@ -0,0 +1,86 @@
+/*
+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 DISCRETE_COLLISION_DETECTOR_INTERFACE_H
+#define DISCRETE_COLLISION_DETECTOR_INTERFACE_H
+#include "SimdTransform.h"
+#include "SimdVector3.h"
+
+
+/// This interface is made to be used by an iterative approach to do TimeOfImpact calculations
+/// This interface allows to query for closest points and penetration depth between two (convex) objects
+/// the closest point is on the second object (B), and the normal points from the surface on B towards A.
+/// distance is between closest points on B and closest point on A. So you can calculate closest point on A
+/// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB
+struct DiscreteCollisionDetectorInterface
+{
+	void operator delete(void* ptr) {};
+
+	struct Result
+	{
+		void operator delete(void* ptr) {};
+	
+		virtual ~Result(){}	
+		virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)=0;
+	};
+
+	struct ClosestPointInput
+	{
+		ClosestPointInput()
+			:m_maximumDistanceSquared(1e30f)
+		{
+		}
+
+		SimdTransform m_transformA;
+		SimdTransform m_transformB;
+		SimdScalar	m_maximumDistanceSquared;
+	};
+
+	virtual ~DiscreteCollisionDetectorInterface() {};
+
+	//
+	// 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 IDebugDraw* debugDraw) = 0;
+
+	SimdScalar	getCollisionMargin() { return 0.2f;}
+};
+
+struct StorageResult : public DiscreteCollisionDetectorInterface::Result
+{
+		SimdVector3	m_normalOnSurfaceB;
+		SimdVector3	m_closestPointInB;
+		SimdScalar	m_distance; //negative means penetration !
+
+		StorageResult() : m_distance(1e30f)
+		{
+
+		}
+		virtual ~StorageResult() {};
+
+		virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
+		{
+			if (depth < m_distance)
+			{
+				m_normalOnSurfaceB = normalOnBInWorld;
+				m_closestPointInB = pointInWorld;
+				m_distance = depth;
+			}
+		}
+};
+
+#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE_H

Bullet/NarrowPhaseCollision/Epa.cpp

@@ -0,0 +1,560 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "SimdScalar.h"
+#include "SimdVector3.h"
+#include "SimdPoint3.h"
+#include "SimdTransform.h"
+#include "SimdMinMax.h"
+
+#include "CollisionShapes/ConvexShape.h"
+
+#include <vector>
+#include <list>
+#include <algorithm>
+
+#include "NarrowPhaseCollision/SimplexSolverInterface.h"
+
+#include "NarrowPhaseCollision/EpaCommon.h"
+
+#include "NarrowPhaseCollision/EpaVertex.h"
+#include "NarrowPhaseCollision/EpaHalfEdge.h"
+#include "NarrowPhaseCollision/EpaFace.h"
+#include "NarrowPhaseCollision/EpaPolyhedron.h"
+#include "NarrowPhaseCollision/Epa.h"
+
+const SimdScalar EPA_MAX_RELATIVE_ERROR = 1e-2f;
+const SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD = EPA_MAX_RELATIVE_ERROR * EPA_MAX_RELATIVE_ERROR;
+
+Epa::Epa( ConvexShape* pConvexShapeA, ConvexShape* pConvexShapeB,
+		  const SimdTransform& transformA, const SimdTransform& transformB ) : m_pConvexShapeA( pConvexShapeA ),
+																			   m_pConvexShapeB( pConvexShapeB ),
+																			   m_transformA( transformA ),
+																			   m_transformB( transformB )
+{
+	m_faceEntries.reserve( EPA_MAX_FACE_ENTRIES );
+}
+
+Epa::~Epa()
+{
+}
+
+bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
+{
+	// Run GJK on the enlarged shapes to obtain a simplex of the enlarged CSO
+
+	SimdVector3 v( 1, 0, 0 );
+	SimdScalar squaredDistance = SIMD_INFINITY;
+
+	SimdScalar delta = 0.f;
+
+	simplexSolver.reset();
+
+	int nbIterations = 0;
+
+	while ( true )
+	{
+		assert( ( v.length2() > 0 ) && "Warning : v has zero magnitude!" );
+
+		SimdVector3 seperatingAxisInA = -v * m_transformA.getBasis();
+		SimdVector3 seperatingAxisInB =  v * m_transformB.getBasis();
+
+		SimdVector3 pInA = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
+		SimdVector3 qInB = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
+
+		SimdPoint3  pWorld = m_transformA( pInA );
+		SimdPoint3  qWorld = m_transformB( qInB );
+
+		SimdVector3 w = pWorld - qWorld;
+		delta = v.dot( w );
+
+		assert( ( delta <= 0 ) && "Shapes are disjoint, EPA should have never    been called!" );
+		assert( !simplexSolver.inSimplex( w ) && "Shapes are disjoint, EPA should have never been called!" );
+
+		// Add support point to simplex
+		simplexSolver.addVertex( w, pWorld, qWorld );
+
+		bool closestOk = simplexSolver.closest( v );
+		assert( closestOk && "Shapes are disjoint, EPA should have never been called!" );
+
+		SimdScalar prevVSqrd = squaredDistance;
+		squaredDistance = v.length2();
+
+		// Is v converging to v(A-B) ?
+		assert( ( ( prevVSqrd - squaredDistance ) > SIMD_EPSILON * prevVSqrd ) && 
+				"Shapes are disjoint, EPA should have never been called!" );
+
+		if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
+		{
+			break;
+		}
+
+		++nbIterations;
+	}
+
+	SimdPoint3 simplexPoints[ 5 ];
+	SimdPoint3 wSupportPointsOnA[ 5 ];
+	SimdPoint3 wSupportPointsOnB[ 5 ];
+
+	int nbSimplexPoints = simplexSolver.getSimplex( wSupportPointsOnA, wSupportPointsOnB, simplexPoints );
+
+	// nbSimplexPoints can't be one because cases where the origin is on the boundary are handled
+	// by hybrid penetration depth
+	assert( ( ( nbSimplexPoints > 1 ) && ( nbSimplexPoints <= 4 ) ) &&
+		    "Hybrid Penetration Depth algorithm failed!" );
+
+	int nbPolyhedronPoints = nbSimplexPoints;
+
+#ifndef EPA_POLYHEDRON_USE_PLANES
+	int initTetraIndices[ 4 ] = { 0, 1, 2, 3 };
+#endif
+
+	//	Prepare initial polyhedron to start EPA from
+	if ( nbSimplexPoints == 1 )
+	{
+		return false;
+	}
+	else if ( nbSimplexPoints == 2 )
+	{
+		// We have a line segment inside the CSO that contains the origin
+		// Create an hexahedron ( two tetrahedron glued together ) by adding 3 new points
+
+		SimdVector3 d = simplexPoints[ 0 ] - simplexPoints[ 1 ];
+		d.normalize();
+
+		SimdVector3 v1;
+		SimdVector3 v2;
+		SimdVector3 v3;
+
+		SimdVector3 e1;
+
+		SimdScalar absx = abs( d.getX() );
+		SimdScalar absy = abs( d.getY() );
+		SimdScalar absz = abs( d.getZ() );
+
+		if ( absx < absy )
+		{
+			if ( absx < absz )
+			{
+				e1.setX( 1 );
+			}
+			else
+			{
+				e1.setZ( 1 );
+			}
+		}
+		else
+		{
+			if ( absy < absz )
+			{
+				e1.setY( 1 );
+			}
+			else
+			{
+				e1.setZ( 1 );
+			}
+		}
+
+		v1 = d.cross( e1 );
+		v1.normalize();
+
+		v2 = v1.rotate( d, 120 * SIMD_RADS_PER_DEG );
+		v3 = v2.rotate( d, 120 * SIMD_RADS_PER_DEG );
+
+		nbPolyhedronPoints = 5;
+
+		SimdVector3 seperatingAxisInA =  v1 * m_transformA.getBasis();
+		SimdVector3 seperatingAxisInB = -v1 * m_transformB.getBasis();
+
+		SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
+		SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
+
+		SimdPoint3 pWorld = m_transformA( p );
+		SimdPoint3 qWorld = m_transformB( q );
+
+		wSupportPointsOnA[ 2 ] = pWorld;
+		wSupportPointsOnB[ 2 ] = qWorld;
+		simplexPoints[ 2 ]	   = wSupportPointsOnA[ 2 ] - wSupportPointsOnB[ 2 ];
+
+		seperatingAxisInA =  v2 * m_transformA.getBasis();
+		seperatingAxisInB = -v2 * m_transformB.getBasis();
+
+		p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
+		q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
+
+		pWorld = m_transformA( p );
+		qWorld = m_transformB( q );
+
+		wSupportPointsOnA[ 3 ] = pWorld;
+		wSupportPointsOnB[ 3 ] = qWorld;
+		simplexPoints[ 3 ]	   = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ];
+
+		seperatingAxisInA =  v3 * m_transformA.getBasis();
+		seperatingAxisInB = -v3 * m_transformB.getBasis();
+
+		p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
+		q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
+
+		pWorld = m_transformA( p );
+		qWorld = m_transformB( q );
+
+		wSupportPointsOnA[ 4 ] = pWorld;
+		wSupportPointsOnB[ 4 ] = qWorld;
+		simplexPoints[ 4 ]	   = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ];
+
+#ifndef EPA_POLYHEDRON_USE_PLANES
+		if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) )
+		{
+			initTetraIndices[ 1 ] = 2;
+			initTetraIndices[ 2 ] = 3;
+			initTetraIndices[ 3 ] = 4;
+		}
+		else
+		{
+			if ( TetrahedronContainsOrigin( simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) )
+			{
+				initTetraIndices[ 0 ] = 1;
+				initTetraIndices[ 1 ] = 2;
+				initTetraIndices[ 2 ] = 3;
+				initTetraIndices[ 3 ] = 4;
+			}
+			else
+			{
+				// No tetrahedron contains the origin
+				assert( false && "Unable to find an initial tetrahedron that contains the origin!" );
+				return false;
+			}
+		}
+#endif
+	}
+	else if ( nbSimplexPoints == 3 )
+	{
+		// We have a triangle inside the CSO that contains the origin
+		// Create an hexahedron ( two tetrahedron glued together ) by adding 2 new points
+
+		SimdVector3 v0 = simplexPoints[ 2 ] - simplexPoints[ 0 ];
+		SimdVector3 v1 = simplexPoints[ 1 ] - simplexPoints[ 0 ];
+		SimdVector3 triangleNormal = v0.cross( v1 );
+		triangleNormal.normalize();
+
+		nbPolyhedronPoints = 5;
+
+		SimdVector3 seperatingAxisInA =  triangleNormal * m_transformA.getBasis();
+		SimdVector3 seperatingAxisInB = -triangleNormal * m_transformB.getBasis();
+
+		SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
+		SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
+
+		SimdPoint3 pWorld = m_transformA( p );
+		SimdPoint3 qWorld = m_transformB( q );
+
+		wSupportPointsOnA[ 3 ] = pWorld;
+		wSupportPointsOnB[ 3 ] = qWorld;
+		simplexPoints[ 3 ]	   = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ];
+
+#ifndef EPA_POLYHEDRON_USE_PLANES
+		// We place this check here because if the tetrahedron contains the origin
+		// there is no need to sample another support point
+		if ( !TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ] ) )
+		{
+#endif
+			seperatingAxisInA = -triangleNormal * m_transformA.getBasis();
+			seperatingAxisInB =  triangleNormal * m_transformB.getBasis();
+
+			p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
+			q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
+
+			pWorld = m_transformA( p );
+			qWorld = m_transformB( q );
+
+			wSupportPointsOnA[ 4 ] = pWorld;
+			wSupportPointsOnB[ 4 ] = qWorld;
+			simplexPoints[ 4 ]	   = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ];
+
+#ifndef EPA_POLYHEDRON_USE_PLANES
+			if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 4 ] ) )
+			{
+				initTetraIndices[ 3 ] = 4;
+			}
+			else
+			{
+				// No tetrahedron contains the origin
+				assert( false && "Unable to find an initial tetrahedron that contains the origin!" );
+				return false;
+			}
+		}
+#endif
+	}
+#ifdef _DEBUG
+	else if ( nbSimplexPoints == 4 )
+	{
+		assert( TetrahedronContainsOrigin( simplexPoints ) && "Initial tetrahedron does not contain the origin!" );
+	}
+#endif
+
+#ifndef EPA_POLYHEDRON_USE_PLANES
+	SimdPoint3 wTetraPoints[ 4 ] = { simplexPoints[ initTetraIndices[ 0 ] ],
+									 simplexPoints[ initTetraIndices[ 1 ] ],
+									 simplexPoints[ initTetraIndices[ 2 ] ],
+									 simplexPoints[ initTetraIndices[ 3 ] ] };
+
+	SimdPoint3 wTetraSupportPointsOnA[ 4 ] = { wSupportPointsOnA[ initTetraIndices[ 0 ] ],
+											   wSupportPointsOnA[ initTetraIndices[ 1 ] ],
+											   wSupportPointsOnA[ initTetraIndices[ 2 ] ],
+											   wSupportPointsOnA[ initTetraIndices[ 3 ] ] };
+
+	SimdPoint3 wTetraSupportPointsOnB[ 4 ] = { wSupportPointsOnB[ initTetraIndices[ 0 ] ],
+											   wSupportPointsOnB[ initTetraIndices[ 1 ] ],
+											   wSupportPointsOnB[ initTetraIndices[ 2 ] ],
+											   wSupportPointsOnB[ initTetraIndices[ 3 ] ] };
+#endif
+
+#ifdef EPA_POLYHEDRON_USE_PLANES
+	if ( !m_polyhedron.Create( simplexPoints, wSupportPointsOnA, wSupportPointsOnB, nbPolyhedronPoints ) )
+#else
+	if ( !m_polyhedron.Create( wTetraPoints, wTetraSupportPointsOnA, wTetraSupportPointsOnB, 4 ) )
+#endif
+	{
+		// Failed to create initial polyhedron
+		assert( false && "Failed to create initial polyhedron!" );
+		return false;
+	}
+
+	// Add initial faces to priority queue
+
+#ifdef _DEBUG
+	//m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
+#endif
+
+	std::list< EpaFace* >& faces = m_polyhedron.GetFaces();
+
+	std::list< EpaFace* >::iterator facesItr( faces.begin() );
+
+	while ( facesItr != faces.end() )
+	{
+		EpaFace* pFace = *facesItr;
+
+		if ( !pFace->m_deleted )
+		{
+//#ifdef EPA_POLYHEDRON_USE_PLANES
+//			if ( pFace->m_planeDistance >= 0 )
+//			{
+//				m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
+//				assert( false && "Face's plane distance equal or greater than 0!" );
+//			}
+//#endif
+
+			if ( pFace->IsAffinelyDependent() )
+			{
+				assert( false && "One initial face is affinely dependent!" );
+				return false;
+			}
+
+			if ( pFace->m_vSqrd <= 0 )
+			{
+				assert( false && "Face containing the origin!" );
+				return false;
+			}
+
+			if ( pFace->IsClosestPointInternal() )
+			{
+				m_faceEntries.push_back( pFace );
+				std::push_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
+			}
+		}
+
+		++facesItr;
+	}
+
+#ifdef _DEBUG
+	//m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
+#endif
+
+	assert( !m_faceEntries.empty() && "No faces added to heap!" );
+
+	return true;
+}
+
+SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
+{
+	SimdVector3 v;
+
+	SimdScalar upperBoundSqrd = SIMD_INFINITY;
+	SimdScalar vSqrd = 0;
+#ifdef _DEBUG
+	SimdScalar prevVSqrd;
+#endif
+	SimdScalar delta;
+
+	bool isCloseEnough = false;
+
+	EpaFace* pEpaFace = NULL;
+
+	int nbIterations = 0;
+	//int nbMaxIterations = 1000;
+
+	do
+	{
+		pEpaFace = m_faceEntries.front();
+		std::pop_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
+		m_faceEntries.pop_back();
+
+		if ( !pEpaFace->m_deleted )
+		{
+#ifdef _DEBUG
+			prevVSqrd = vSqrd;
+#endif
+
+			vSqrd = pEpaFace->m_vSqrd;
+
+			if ( pEpaFace->m_planeDistance >= 0 )
+			{
+				v = pEpaFace->m_planeNormal;
+			}
+			else
+			{
+				v = pEpaFace->m_v;
+			}
+
+#ifdef _DEBUG
+			//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );
+			assert( ( vSqrd >= prevVSqrd ) && "vSqrd decreased!" );
+#endif //_DEBUG
+			assert( ( v.length2() > 0 ) && "Zero vector not allowed!" );
+
+			SimdVector3 seperatingAxisInA =  v * m_transformA.getBasis();
+			SimdVector3 seperatingAxisInB = -v * m_transformB.getBasis();
+
+			SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
+			SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
+
+			SimdPoint3 pWorld = m_transformA( p );
+			SimdPoint3 qWorld = m_transformB( q );
+
+			SimdPoint3 w = pWorld - qWorld;
+			delta = v.dot( w );
+
+			// Keep tighest upper bound
+			upperBoundSqrd = SimdMin( upperBoundSqrd, delta * delta / vSqrd );
+			//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );
+
+			isCloseEnough = ( upperBoundSqrd <= ( 1 + 1e-4f ) * vSqrd );
+
+			if ( !isCloseEnough )
+			{
+				std::list< EpaFace* > newFaces;
+				bool expandOk = m_polyhedron.Expand( w, pWorld, qWorld, pEpaFace, newFaces );
+
+				if ( expandOk )
+				{
+					assert( !newFaces.empty() && "EPA polyhedron not expanding ?" );
+
+					bool check    = true;
+					bool areEqual = false;
+
+					while ( !newFaces.empty() )
+					{
+						EpaFace* pNewFace = newFaces.front();
+						assert( !pNewFace->m_deleted && "New face is deleted!" );
+
+						if ( !pNewFace->m_deleted )
+						{
+							assert( ( pNewFace->m_vSqrd > 0 ) && "Face containing the origin!" );
+							assert( !pNewFace->IsAffinelyDependent() && "Face is affinely dependent!" );
+
+//#ifdef EPA_POLYHEDRON_USE_PLANES
+////							if ( pNewFace->m_planeDistance >= 0 )
+////							{
+//								// assert( false && "Face's plane distance greater than 0!" );
+//#ifdef _DEBUG
+////								m_polyhedron._dbgSaveToFile( "epa_beforeFix.dbg" );
+//#endif
+//								//pNewFace->FixOrder();
+//#ifdef _DEBUG
+//								//m_polyhedron._dbgSaveToFile( "epa_afterFix.dbg" );
+//#endif
+////							}
+//#endif
+//
+//#ifdef EPA_POLYHEDRON_USE_PLANES
+//							//assert( ( pNewFace->m_planeDistance < 0 ) && "Face's plane distance equal or greater than 0!" );
+//#endif
+
+							if ( pNewFace->IsClosestPointInternal() && ( vSqrd <= pNewFace->m_vSqrd ) && ( pNewFace->m_vSqrd <= upperBoundSqrd ) )
+							{
+								m_faceEntries.push_back( pNewFace );
+								std::push_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
+							}
+						}
+
+						newFaces.pop_front();
+					}
+				}
+				else
+				{
+					pEpaFace->CalcClosestPointOnA( wWitnessOnA );
+					pEpaFace->CalcClosestPointOnB( wWitnessOnB );
+
+#ifdef _DEBUG
+					//m_polyhedron._dbgSaveToFile( "epa_end.dbg" );
+#endif
+
+					return v.length();
+				}
+			}
+		}
+
+		++nbIterations;
+	}
+	while ( ( m_polyhedron.GetNbFaces() < EPA_MAX_FACE_ENTRIES ) &&/*( nbIterations < nbMaxIterations ) &&*/
+			!isCloseEnough && ( m_faceEntries.size() > 0 ) && ( m_faceEntries[ 0 ]->m_vSqrd <= upperBoundSqrd ) );
+
+#ifdef _DEBUG
+	//m_polyhedron._dbgSaveToFile( "epa_end.dbg" );
+#endif
+
+	assert( pEpaFace && "Invalid epa face!" );
+
+	pEpaFace->CalcClosestPointOnA( wWitnessOnA );
+	pEpaFace->CalcClosestPointOnB( wWitnessOnB );
+
+	return v.length();
+}
+
+bool Epa::TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3& point1,
+									 const SimdPoint3& point2, const SimdPoint3& point3 )
+{
+	SimdVector3 facesNormals[ 4 ] = { ( point1 - point0 ).cross( point2 - point0 ),
+									  ( point2 - point1 ).cross( point3 - point1 ),
+									  ( point3 - point2 ).cross( point0 - point2 ),
+									  ( point0 - point3 ).cross( point1 - point3 ) };
+
+	return ( ( facesNormals[ 0 ].dot( point0 ) > 0 ) != ( facesNormals[ 0 ].dot( point3 ) > 0 ) ) &&
+		   ( ( facesNormals[ 1 ].dot( point1 ) > 0 ) != ( facesNormals[ 1 ].dot( point0 ) > 0 ) ) &&
+		   ( ( facesNormals[ 2 ].dot( point2 ) > 0 ) != ( facesNormals[ 2 ].dot( point1 ) > 0 ) ) &&
+		   ( ( facesNormals[ 3 ].dot( point3 ) > 0 ) != ( facesNormals[ 3 ].dot( point2 ) > 0 ) );
+}
+
+bool Epa::TetrahedronContainsOrigin( SimdPoint3* pPoints )
+{
+	return TetrahedronContainsOrigin( pPoints[ 0 ], pPoints[ 1 ], pPoints[ 2 ], pPoints[ 3 ] );
+}
+
+bool CompareEpaFaceEntries( EpaFace* pFaceA, EpaFace* pFaceB )
+{
+	return ( pFaceA->m_vSqrd > pFaceB->m_vSqrd );
+}

Bullet/NarrowPhaseCollision/Epa.h

@@ -0,0 +1,66 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+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 EPA_H
+#define EPA_H
+
+#define EPA_MAX_FACE_ENTRIES 256
+
+extern const SimdScalar EPA_MAX_RELATIVE_ERROR;
+extern const SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD;
+
+class Epa
+{
+	private :
+
+		//! Prevents copying objects from this class
+		Epa( const Epa& epa );
+		const Epa&			operator = ( const Epa& epa );
+
+	public :
+
+		Epa( ConvexShape* pConvexShapeA, ConvexShape* pConvexShapeB,
+			 const SimdTransform& transformA, const SimdTransform& transformB );
+		~Epa();
+
+		bool				Initialize( SimplexSolverInterface& simplexSolver );
+
+		SimdScalar			CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB );
+
+	private :
+
+		bool				TetrahedronContainsOrigin( SimdPoint3* pPoints );
+		bool				TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3& point1,
+													   const SimdPoint3& point2, const SimdPoint3& point3 );
+
+	private :
+
+		//! Priority queue
+		std::vector< EpaFace* >	m_faceEntries;
+
+		ConvexShape*			m_pConvexShapeA;
+		ConvexShape*			m_pConvexShapeB;
+
+		SimdTransform			m_transformA;
+		SimdTransform			m_transformB;
+
+		EpaPolyhedron			m_polyhedron;
+};
+
+extern bool CompareEpaFaceEntries( EpaFace* pFaceA, EpaFace* pFaceB );
+
+#endif
+

Bullet/NarrowPhaseCollision/EpaCommon.h

@@ -0,0 +1,25 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+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 EPA_COMMON_H
+#define EPA_COMMON_H
+
+#define EPA_POLYHEDRON_USE_PLANES
+
+//#define EPA_USE_HYBRID
+
+#endif
+

Bullet/NarrowPhaseCollision/EpaFace.cpp

@@ -0,0 +1,254 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#include "SimdScalar.h"
+#include "SimdVector3.h"
+#include "SimdPoint3.h"
+
+#include "NarrowPhaseCollision/EpaCommon.h"
+
+#include "NarrowPhaseCollision/EpaVertex.h"
+#include "NarrowPhaseCollision/EpaHalfEdge.h"
+#include "NarrowPhaseCollision/EpaFace.h"
+
+#ifdef EPA_POLYHEDRON_USE_PLANES
+SimdScalar PLANE_THICKNESS = 1e-5f;
+#endif
+
+EpaFace::EpaFace() : m_pHalfEdge( 0 ), m_deleted( false )
+{
+	m_pVertices[ 0 ] = m_pVertices[ 1 ] = m_pVertices[ 2 ] = 0;
+}
+
+EpaFace::~EpaFace()
+{
+}
+
+bool EpaFace::Initialize()
+{
+	assert( m_pHalfEdge && "Must setup half-edge first!" );
+
+	CollectVertices( m_pVertices );
+	
+	const SimdVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
+	const SimdVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
+
+	const SimdScalar e0Sqrd = e0.length2();
+	const SimdScalar e1Sqrd = e1.length2();
+	const SimdScalar e0e1   = e0.dot( e1 );
+
+	m_determinant = e0Sqrd * e1Sqrd - e0e1 * e0e1;
+
+	const SimdScalar e0v0 = e0.dot( m_pVertices[ 0 ]->m_point );
+	const SimdScalar e1v0 = e1.dot( m_pVertices[ 0 ]->m_point );
+
+	m_lambdas[ 0 ] = e0e1 * e1v0 - e1Sqrd * e0v0;
+	m_lambdas[ 1 ] = e0e1 * e0v0 - e0Sqrd * e1v0;
+
+	if ( IsAffinelyDependent() )
+	{
+		return false;
+	}
+
+	CalcClosestPoint();
+
+#ifdef EPA_POLYHEDRON_USE_PLANES
+	if ( !CalculatePlane() )
+	{
+		return false;
+	}
+#endif
+
+	return true;
+}
+
+#ifdef EPA_POLYHEDRON_USE_PLANES
+bool EpaFace::CalculatePlane()
+{
+	assert( ( m_pVertices[ 0 ] && m_pVertices[ 1 ] && m_pVertices[ 2 ] )
+			&& "Must setup vertices pointers first!" );
+
+	// Traditional method
+
+	const SimdVector3 v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
+	const SimdVector3 v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
+
+	m_planeNormal = v2.cross( v1 );
+
+	if ( m_planeNormal.length2() == 0 )
+	{
+		return false;
+	}
+
+	m_planeNormal.normalize();
+
+	m_planeDistance = m_pVertices[ 0 ]->m_point.dot( -m_planeNormal );
+
+	// Robust method
+
+	//SimdVector3 _v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
+	//SimdVector3 _v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
+
+	//SimdVector3 n;
+
+	//n = _v2.cross( _v1 );
+
+	//_v1 = m_pVertices[ 0 ]->m_point - m_pVertices[ 1 ]->m_point;
+	//_v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 1 ]->m_point;
+
+	//n += ( _v1.cross( _v2 ) );
+
+	//_v1 = m_pVertices[ 0 ]->m_point - m_pVertices[ 2 ]->m_point;
+	//_v2 = m_pVertices[ 1 ]->m_point - m_pVertices[ 2 ]->m_point;
+
+	//n += ( _v2.cross( _v1 ) );
+
+	//n /= 3;	
+	//n.normalize();
+
+	//SimdVector3 c = ( m_pVertices[ 0 ]->m_point + m_pVertices[ 1 ]->m_point + m_pVertices[ 2 ]->m_point ) / 3;
+	//SimdScalar d  = c.dot( -n );
+
+	//m_robustPlaneNormal   = n;
+	//m_robustPlaneDistance = d;
+
+	// Compare results from both methods and check whether they disagree
+
+	//if ( d < 0 )
+	//{
+	//	assert( ( m_planeDistance < 0 ) && "He he! Busted!" );
+	//}
+	//else
+	//{
+	//	assert( ( m_planeDistance >= 0 ) && "He he! Busted!" );
+	//}
+
+	return true;
+}
+#endif
+
+void EpaFace::CalcClosestPoint()
+{
+	const SimdVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
+	const SimdVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
+
+	m_v =  m_pVertices[ 0 ]->m_point +
+		 ( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) / m_determinant;
+
+	m_vSqrd = m_v.length2();
+}
+
+void EpaFace::CalcClosestPointOnA( SimdVector3& closestPointOnA )
+{
+	const SimdVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA;
+	const SimdVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA;
+
+	closestPointOnA =  m_pVertices[ 0 ]->m_wSupportPointOnA +
+					 ( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) /
+					   m_determinant;
+}
+
+void EpaFace::CalcClosestPointOnB( SimdVector3& closestPointOnB )
+{
+	const SimdVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB;
+	const SimdVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB;
+
+	closestPointOnB =  m_pVertices[ 0 ]->m_wSupportPointOnB +
+					 ( e0 * m_lambdas[  0 ] + e1 * m_lambdas[ 1 ] ) /
+					   m_determinant;
+}
+
+bool EpaFace::IsAffinelyDependent() const
+{
+	return ( m_determinant <= SIMD_EPSILON );
+}
+
+bool EpaFace::IsClosestPointInternal() const
+{
+	return ( ( m_lambdas[ 0 ] >= 0 ) && ( m_lambdas[ 1 ] >= 0 ) && ( ( m_lambdas[ 0 ] + m_lambdas[ 1 ] <= m_determinant ) ) );
+}
+
+void EpaFace::CollectVertices( EpaVertex** ppVertices )
+{
+	assert( m_pHalfEdge && "Invalid half-edge pointer!" );
+
+	int vertexIndex = 0;
+
+	EpaHalfEdge* pCurrentHalfEdge = m_pHalfEdge;
+
+	do
+	{
+		assert( ( ( vertexIndex >= 0 ) && ( vertexIndex < 3 ) ) &&
+				"Face is not a triangle!" );
+
+		assert( pCurrentHalfEdge->m_pVertex && "Half-edge has an invalid vertex pointer!" );
+
+		ppVertices[ vertexIndex++ ] = pCurrentHalfEdge->m_pVertex;
+
+		pCurrentHalfEdge = pCurrentHalfEdge->m_pNextCCW;
+				
+	}
+	while( pCurrentHalfEdge != m_pHalfEdge );
+}
+
+//void EpaFace::FixOrder()
+//{
+//	EpaHalfEdge* pHalfEdges[ 3 ];
+//
+//	int halfEdgeIndex = 0;
+//
+//	EpaHalfEdge* pCurrentHalfEdge = m_pHalfEdge;
+//
+//	do
+//	{
+//		assert( ( ( halfEdgeIndex >= 0 ) && ( halfEdgeIndex < 3 ) ) &&
+//				"Face is not a triangle!" );
+//
+//		pHalfEdges[ halfEdgeIndex++ ] = pCurrentHalfEdge;
+//
+//		pCurrentHalfEdge = pCurrentHalfEdge->m_pNextCCW;
+//	}
+//	while( pCurrentHalfEdge != m_pHalfEdge );
+//
+//	EpaVertex* pVertices[ 3 ] = { pHalfEdges[ 0 ]->m_pVertex,
+//								  pHalfEdges[ 1 ]->m_pVertex,
+//								  pHalfEdges[ 2 ]->m_pVertex };
+//	
+//	// Make them run in the opposite direction
+//	pHalfEdges[ 0 ]->m_pNextCCW = pHalfEdges[ 2 ];
+//	pHalfEdges[ 1 ]->m_pNextCCW = pHalfEdges[ 0 ];
+//	pHalfEdges[ 2 ]->m_pNextCCW = pHalfEdges[ 1 ];
+//
+//	// Make half-edges point to their correct origin vertices
+//
+//	pHalfEdges[ 1 ]->m_pVertex = pVertices[ 2 ];
+//	pHalfEdges[ 2 ]->m_pVertex = pVertices[ 0 ];
+//	pHalfEdges[ 0 ]->m_pVertex = pVertices[ 1 ];
+//
+//	// Make vertices point to the correct half-edges
+//
+//	//pHalfEdges[ 0 ]->m_pVertex->m_pHalfEdge = pHalfEdges[ 0 ];
+//	//pHalfEdges[ 1 ]->m_pVertex->m_pHalfEdge = pHalfEdges[ 1 ];
+//	//pHalfEdges[ 2 ]->m_pVertex->m_pHalfEdge = pHalfEdges[ 2 ];
+//
+//	// Flip normal and change the sign of plane distance
+//
+//#ifdef EPA_POLYHEDRON_USE_PLANES
+//	m_planeNormal	= -m_planeNormal;
+//	m_planeDistance = -m_planeDistance;
+//#endif
+//}
+

Bullet/NarrowPhaseCollision/EpaFace.h

@@ -0,0 +1,83 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+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 EPA_FACE_H
+#define EPA_FACE_H
+
+class EpaVertex;
+class EpaHalfEdge;
+
+#ifdef EPA_POLYHEDRON_USE_PLANES
+extern SimdScalar PLANE_THICKNESS;
+#endif
+
+//! Note : This class is not supposed to be a base class
+class EpaFace
+{
+	private :
+
+		//! Prevents copying objects from this class
+		EpaFace( const EpaFace& epaFace );
+		const EpaFace&	operator = ( const EpaFace& epaFace );
+
+	public :
+
+		EpaFace();
+		~EpaFace();
+
+		bool			Initialize();
+
+#ifdef EPA_POLYHEDRON_USE_PLANES
+		bool			CalculatePlane();
+#endif
+		void			CalcClosestPoint();
+		void			CalcClosestPointOnA( SimdVector3& closestPointOnA );
+		void			CalcClosestPointOnB( SimdVector3& closestPointOnB );
+
+		bool			IsAffinelyDependent()							const;
+		bool			IsClosestPointInternal()						const;
+
+		void			CollectVertices( EpaVertex** ppVertices );
+
+		//void			FixOrder();
+
+	public :
+
+		EpaHalfEdge*	m_pHalfEdge;
+
+		// We keep vertices here so we don't need to call CollectVertices
+		// every time we need them
+		EpaVertex*		m_pVertices[ 3 ];
+
+#ifdef EPA_POLYHEDRON_USE_PLANES
+		SimdVector3		m_planeNormal;
+		SimdScalar		m_planeDistance;
+			
+		//SimdVector3		m_robustPlaneNormal;
+		//SimdScalar		m_robustPlaneDistance;
+#endif
+
+		SimdVector3		m_v;
+		SimdScalar		m_vSqrd;
+
+		SimdScalar		m_determinant;
+		SimdScalar		m_lambdas[ 2 ];
+
+		bool			m_deleted;
+};
+
+#endif
+

Bullet/NarrowPhaseCollision/EpaHalfEdge.h

@@ -0,0 +1,58 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+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 EPA_HALF_EDGE_H
+#define EPA_HALF_EDGE_H
+
+class EpaFace;
+class EpaVertex;
+
+//! Note : This class is not supposed to be a base class
+class EpaHalfEdge
+{
+	private :
+
+		//! Prevents copying objects from this class
+		EpaHalfEdge( const EpaHalfEdge& epaHalfEdge );
+		const EpaHalfEdge&		operator = ( const EpaHalfEdge& epaHalfEdge );
+
+	public :
+
+		EpaHalfEdge() : m_pTwin( 0 ), m_pNextCCW( 0 ), m_pFace( 0 ), m_pVertex( 0 )
+		{
+		}
+
+		~EpaHalfEdge()
+		{
+		}
+
+	public :
+
+		//! Twin half-edge link
+		EpaHalfEdge*			m_pTwin;
+
+		//! Next half-edge in counter clock-wise ( CCW ) order
+		EpaHalfEdge*			m_pNextCCW;
+
+		//! Parent face link
+		EpaFace*				m_pFace;
+
+		//! Origin vertex link
+		EpaVertex*				m_pVertex;
+};
+
+#endif
+

Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.cpp

@@ -0,0 +1,202 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#include "SimdScalar.h"
+#include "SimdVector3.h"
+#include "SimdPoint3.h"
+#include "SimdTransform.h"
+#include "SimdMinMax.h"
+
+#include <list>
+
+#include "CollisionShapes/ConvexShape.h"
+
+#include "NarrowPhaseCollision/SimplexSolverInterface.h"
+
+#include "NarrowPhaseCollision/EpaCommon.h"
+
+#include "NarrowPhaseCollision/EpaVertex.h"
+#include "NarrowPhaseCollision/EpaHalfEdge.h"
+#include "NarrowPhaseCollision/EpaFace.h"
+#include "NarrowPhaseCollision/EpaPolyhedron.h"
+#include "NarrowPhaseCollision/Epa.h"
+#include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h"
+#include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h"
+
+SimdScalar	g_GJKMaxRelError = 1e-3f;
+SimdScalar	g_GJKMaxRelErrorSqrd = g_GJKMaxRelError * g_GJKMaxRelError;
+
+bool EpaPenetrationDepthSolver::CalcPenDepth( SimplexSolverInterface& simplexSolver,
+											  ConvexShape* pConvexA, ConvexShape* pConvexB,
+											  const SimdTransform& transformA, const SimdTransform& transformB,
+											  SimdVector3& v, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
+											  class IDebugDraw* debugDraw )
+{
+	assert( pConvexA && "Convex shape A is invalid!" );
+	assert( pConvexB && "Convex shape B is invalid!" );
+
+	SimdScalar penDepth;
+
+#ifdef EPA_USE_HYBRID
+	bool needsEPA = !HybridPenDepth( simplexSolver, pConvexA, pConvexB, transformA, transformB,
+									 wWitnessOnA, wWitnessOnB, penDepth, v );
+
+	if ( needsEPA )
+	{
+#endif
+		penDepth = EpaPenDepth( simplexSolver, pConvexA, pConvexB,
+								transformA, transformB,
+								wWitnessOnA, wWitnessOnB );
+		assert( ( penDepth > 0 ) &&  "EPA or Hybrid Technique failed to calculate penetration depth!" );
+
+#ifdef EPA_USE_HYBRID
+	}
+#endif
+
+	return ( penDepth > 0 );
+}
+
+#ifdef EPA_USE_HYBRID
+bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexSolver,
+												ConvexShape* pConvexA, ConvexShape* pConvexB,
+												const SimdTransform& transformA, const SimdTransform& transformB,
+												SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
+											    SimdScalar& penDepth, SimdVector3& v )
+{
+	SimdScalar squaredDistance = SIMD_INFINITY;
+	SimdScalar delta = 0.f;
+
+	const SimdScalar margin     = pConvexA->GetMargin() + pConvexB->GetMargin();
+	const SimdScalar marginSqrd = margin * margin;
+
+	simplexSolver.reset();
+
+	int nbIterations = 0;
+
+	while ( true )
+	{
+		assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" );
+
+		SimdVector3 seperatingAxisInA = -v * transformA.getBasis();
+		SimdVector3 seperatingAxisInB =  v * transformB.getBasis();
+
+		SimdVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
+		SimdVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
+
+		SimdPoint3  pWorld = transformA( pInA );
+		SimdPoint3  qWorld = transformB( qInB );
+
+		SimdVector3 w = pWorld - qWorld;
+		delta = v.dot( w );
+
+		// potential exit, they don't overlap
+		if ( ( delta > 0 ) && ( ( delta * delta / squaredDistance ) > marginSqrd ) )
+		{
+			// Convex shapes do not overlap
+			// Returning true means that Hybrid's result is ok and there's no need to run EPA
+			penDepth = 0;
+			return true;
+		}
+
+		//exit 0: the new point is already in the simplex, or we didn't come any closer
+		if ( ( squaredDistance - delta <= squaredDistance * g_GJKMaxRelErrorSqrd ) || simplexSolver.inSimplex( w ) )
+		{
+			simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
+
+			assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
+			SimdScalar vLength = sqrt( squaredDistance );
+
+			wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
+			wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
+
+			penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
+
+			// Returning true means that Hybrid's result is ok and there's no need to run EPA
+			return true;
+		}
+
+		//add current vertex to simplex
+		simplexSolver.addVertex( w, pWorld, qWorld );
+
+		//calculate the closest point to the origin (update vector v)
+		if ( !simplexSolver.closest( v ) )
+		{
+			simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
+
+			assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
+			SimdScalar vLength = sqrt( squaredDistance );
+
+			wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
+			wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
+
+			penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
+
+			// Returning true means that Hybrid's result is ok and there's no need to run EPA
+			return true;
+		}
+
+		SimdScalar previousSquaredDistance = squaredDistance;
+		squaredDistance = v.length2();
+
+		//are we getting any closer ?
+		if ( previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance ) 
+		{ 
+			simplexSolver.backup_closest( v );
+			squaredDistance = v.length2();
+
+			simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
+
+			assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
+			SimdScalar vLength = sqrt( squaredDistance );
+
+			wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
+			wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
+
+			penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
+
+			// Returning true means that Hybrid's result is ok and there's no need to run EPA
+			return true;
+		}
+
+		if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
+		{
+			// Convex Shapes intersect - we need to run EPA
+			// Returning false means that Hybrid couldn't do anything for us
+			// and that we need to run EPA to calculate the pen depth
+			return false;
+		}
+
+		++nbIterations;
+	}
+}
+#endif
+
+SimdScalar EpaPenetrationDepthSolver::EpaPenDepth( SimplexSolverInterface& simplexSolver,
+												   ConvexShape* pConvexA, ConvexShape* pConvexB,
+												   const SimdTransform& transformA, const SimdTransform& transformB,
+												   SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
+{
+	Epa epa( pConvexA, pConvexB, transformA, transformB );
+
+	if ( !epa.Initialize( simplexSolver ) )
+	{
+		assert( false && "Epa failed to initialize!" );
+		return 0;
+	}
+
+	return epa.CalcPenDepth( wWitnessOnA, wWitnessOnB );
+}
+

Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.h

@@ -0,0 +1,56 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+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 EPA_PENETRATION_DEPTH_H
+#define EPA_PENETRATION_DEPTH_H
+
+/**
+*	EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to
+*	calculate the penetration depth between two convex shapes.
+*/
+
+extern SimdScalar	g_GJKMaxRelError;
+extern SimdScalar	g_GJKMaxRelErrorSqrd;
+
+//! Note : This class is not supposed to be a base class
+class EpaPenetrationDepthSolver : public ConvexPenetrationDepthSolver
+{
+	public :
+
+		bool			CalcPenDepth( SimplexSolverInterface& simplexSolver,
+									  ConvexShape* pConvexA, ConvexShape* pConvexB,
+									  const SimdTransform& transformA, const SimdTransform& transformB,
+									  SimdVector3& v, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
+									  class IDebugDraw* debugDraw );
+
+	private :
+
+#ifdef EPA_USE_HYBRID
+		bool			HybridPenDepth( SimplexSolverInterface& simplexSolver,
+										ConvexShape* pConvexA, ConvexShape* pConvexB,
+										const SimdTransform& transformA, const SimdTransform& transformB,
+										SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
+										SimdScalar& penDepth, SimdVector3& v );
+#endif
+
+		SimdScalar		EpaPenDepth( SimplexSolverInterface& simplexSolver,
+									 ConvexShape* pConvexA, ConvexShape* pConvexB,
+									 const SimdTransform& transformA, const SimdTransform& transformB,
+									 SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB );
+};
+
+#endif	// EPA_PENETRATION_DEPTH_H
+

Bullet/NarrowPhaseCollision/EpaPolyhedron.h

@@ -0,0 +1,89 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+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 EPA_POLYHEDRON_H
+#define EPA_POLYHEDRON_H
+
+class EpaFace;
+class EpaVertex;
+
+//! Note : This class is not supposed to be a base class
+class EpaPolyhedron
+{
+	private :
+
+		//! Prevents copying objects from this class
+		EpaPolyhedron( const EpaPolyhedron& epaPolyhedron );
+		const EpaPolyhedron& operator = ( const EpaPolyhedron& epaPolyhedron );
+
+	public :
+
+		EpaPolyhedron();
+		~EpaPolyhedron();
+
+		bool					Create( SimdPoint3* pInitialPoints,
+										SimdPoint3* pSupportPointsOnA, SimdPoint3* pSupportPointsOnB,
+										const int nbInitialPoints );
+		void					Destroy();
+
+		EpaFace*				CreateFace();
+		EpaHalfEdge*			CreateHalfEdge();
+		EpaVertex*				CreateVertex( const SimdPoint3& wSupportPoint,
+											  const SimdPoint3& wSupportPointOnA,
+											  const SimdPoint3& wSupportPointOnB );
+
+		void					DeleteFace( EpaFace* pFace );
+
+		void					DestroyAllFaces();
+		void					DestroyAllHalfEdges();
+		void					DestroyAllVertices();
+
+		bool					Expand( const SimdPoint3& wSupportPoint,
+										const SimdPoint3& wSupportPointOnA,
+										const SimdPoint3& wSupportPointOnB,
+										EpaFace* pFace, std::list< EpaFace* >& newFaces );
+
+		std::list< EpaFace* >&	GetFaces();
+		int						GetNbFaces()		const;
+
+	private :
+
+		void					DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace,
+													std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges );
+
+		void					CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge* >& baseTwinHalfEdges,
+											std::list< EpaFace* >& newFaces );
+		EpaFace*				CreateConeFace( EpaVertex* pAppexVertex, EpaHalfEdge* pBaseTwinHalfEdge,
+												std::list< EpaHalfEdge* >& halfEdgesToLink );
+
+#ifdef _DEBUG
+	public :
+		//! Please don't remove this method, it will help debugging if some problems arise in the future
+		bool					_dbgSaveToFile( const char* pFileName );
+#endif
+
+	private :
+
+		//! This is the number of valid faces, m_faces list also contain deleted faces
+		int							m_nbFaces;
+
+		std::list< EpaFace* >		m_faces;
+		std::list< EpaHalfEdge* >	m_halfEdges;
+		std::list< EpaVertex* >		m_vertices;
+};
+
+#endif
+

Bullet/NarrowPhaseCollision/EpaVertex.h

@@ -0,0 +1,61 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+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 EPA_VERTEX_H
+#define EPA_VERTEX_H
+
+class EpaHalfEdge;
+
+//! Note : This class is not supposed to be a base class
+class EpaVertex
+{
+	private :
+
+		//! Prevents copying objects from this class
+		EpaVertex( const EpaVertex& epaVertex );
+		const EpaVertex&	operator = ( const EpaVertex& epaVertex );
+
+	public :
+
+		EpaVertex( const SimdPoint3& point ) : /*m_pHalfEdge( 0 ),*/ m_point( point )
+		{
+		}
+
+		EpaVertex( const SimdPoint3& point,
+				   const SimdPoint3& wSupportPointOnA,
+				   const SimdPoint3& wSupportPointOnB ) : /*m_pHalfEdge( 0 ),*/ m_point( point ),
+														  m_wSupportPointOnA( wSupportPointOnA ),
+														  m_wSupportPointOnB( wSupportPointOnB )
+		{
+		}
+
+		~EpaVertex()
+		{
+		}
+
+	public :
+
+		//! This is not necessary
+		//EpaHalfEdge*		m_pHalfEdge;
+
+		SimdPoint3			m_point;
+
+		SimdPoint3			m_wSupportPointOnA;
+		SimdPoint3			m_wSupportPointOnB;
+};
+
+#endif
+

Bullet/NarrowPhaseCollision/GjkConvexCast.cpp

@@ -0,0 +1,174 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "GjkConvexCast.h"
+#include "CollisionShapes/SphereShape.h"
+#include "CollisionShapes/MinkowskiSumShape.h"
+#include "GjkPairDetector.h"
+#include "PointCollector.h"
+
+
+GjkConvexCast::GjkConvexCast(ConvexShape* convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver)
+:m_simplexSolver(simplexSolver),
+m_convexA(convexA),
+m_convexB(convexB)
+{
+}
+
+bool	GjkConvexCast::calcTimeOfImpact(
+					const SimdTransform& fromA,
+					const SimdTransform& toA,
+					const SimdTransform& fromB,
+					const SimdTransform& toB,
+					CastResult& result)
+{
+
+
+	MinkowskiSumShape combi(m_convexA,m_convexB);
+	MinkowskiSumShape* convex = &combi;
+
+	SimdTransform	rayFromLocalA;
+	SimdTransform	rayToLocalA;
+
+	rayFromLocalA = fromA.inverse()* fromB;
+	rayToLocalA = toA.inverse()* toB;
+
+
+	SimdTransform trA,trB;
+	trA = SimdTransform(fromA);
+	trB = SimdTransform(fromB);
+	trA.setOrigin(SimdPoint3(0,0,0));
+	trB.setOrigin(SimdPoint3(0,0,0));
+
+	convex->SetTransformA(trA);
+	convex->SetTransformB(trB);
+
+
+
+
+	float radius = 0.01f;
+
+	SimdScalar lambda = 0.f;
+	SimdVector3 s = rayFromLocalA.getOrigin();
+	SimdVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin();
+	SimdVector3 x = s;
+	SimdVector3 n;
+	n.setValue(0,0,0);
+	bool hasResult = false;
+	SimdVector3 c;
+
+	float lastLambda = lambda;
+
+	//first solution, using GJK
+
+	//no penetration support for now, perhaps pass a pointer when we really want it
+	ConvexPenetrationDepthSolver* penSolverPtr = 0;
+
+	SimdTransform identityTrans;
+	identityTrans.setIdentity();
+
+	SphereShape	raySphere(0.0f);
+	raySphere.SetMargin(0.f);
+
+	SimdTransform sphereTr;
+	sphereTr.setIdentity();
+	sphereTr.setOrigin( rayFromLocalA.getOrigin());
+
+	result.DrawCoordSystem(sphereTr);
+	{
+		PointCollector	pointCollector1;
+		GjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr);		
+
+		GjkPairDetector::ClosestPointInput input;
+		input.m_transformA = sphereTr;
+		input.m_transformB = identityTrans;
+		gjk.GetClosestPoints(input,pointCollector1,0);
+
+		hasResult = pointCollector1.m_hasResult;
+		c = pointCollector1.m_pointInWorld;
+		n = pointCollector1.m_normalOnBInWorld;
+	}
+
+	
+
+	if (hasResult)
+	{
+		SimdScalar dist;
+		dist = (c-x).length();
+		if (dist < radius)
+		{
+			//penetration
+			lastLambda = 1.f;
+		}
+
+		//not close enough
+		while (dist > radius)
+		{
+			
+			n = x - c;
+			SimdScalar nDotr = n.dot(r);
+
+			if (nDotr >= -(SIMD_EPSILON*SIMD_EPSILON))
+				return false;
+			
+			lambda = lambda - n.dot(n) / nDotr;
+			if (lambda <= lastLambda)
+				break;
+
+			lastLambda = lambda;
+
+			x = s + lambda * r;
+
+			sphereTr.setOrigin( x );
+			result.DrawCoordSystem(sphereTr);
+			PointCollector	pointCollector;
+			GjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr);
+			GjkPairDetector::ClosestPointInput input;
+			input.m_transformA = sphereTr;
+			input.m_transformB = identityTrans;
+			gjk.GetClosestPoints(input,pointCollector,0);
+			if (pointCollector.m_hasResult)
+			{
+				if (pointCollector.m_distance < 0.f)
+				{
+					//degeneracy, report a hit
+					result.m_fraction = lastLambda;
+					result.m_normal = n;
+					return true;
+				}
+				c = pointCollector.m_pointInWorld;			
+				dist = (c-x).length();
+			} else
+			{
+				//??
+				return false;
+			}
+
+		}
+
+		if (lastLambda < 1.f)
+		{
+		
+			result.m_fraction = lastLambda;
+			result.m_normal = n;
+			return true;
+		}
+	}
+
+	return false;
+}
+

Bullet/NarrowPhaseCollision/GjkConvexCast.h

@@ -0,0 +1,50 @@
+/*
+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 GJK_CONVEX_CAST_H
+#define GJK_CONVEX_CAST_H
+
+#include <CollisionShapes/CollisionMargin.h>
+
+#include "SimdVector3.h"
+#include "ConvexCast.h"
+class ConvexShape;
+class MinkowskiSumShape;
+#include "SimplexSolverInterface.h"
+
+///GjkConvexCast performs a raycast on a convex object using support mapping.
+class GjkConvexCast : public ConvexCast
+{
+	SimplexSolverInterface*	m_simplexSolver;
+	ConvexShape*	m_convexA;
+	ConvexShape*	m_convexB;
+
+public:
+
+	GjkConvexCast(ConvexShape*	convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver);
+
+	/// cast a convex against another convex object
+	virtual bool	calcTimeOfImpact(
+					const SimdTransform& fromA,
+					const SimdTransform& toA,
+					const SimdTransform& fromB,
+					const SimdTransform& toB,
+					CastResult& result);
+
+};
+
+#endif //GJK_CONVEX_CAST_H

Bullet/NarrowPhaseCollision/GjkPairDetector.cpp

@@ -0,0 +1,200 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "GjkPairDetector.h"
+#include "CollisionShapes/ConvexShape.h"
+#include "NarrowPhaseCollision/SimplexSolverInterface.h"
+#include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h"
+
+static const SimdScalar rel_error = SimdScalar(1.0e-5);
+SimdScalar rel_error2 = rel_error * rel_error;
+float maxdist2 = 1.e30f;
+
+
+
+GjkPairDetector::GjkPairDetector(ConvexShape* objectA,ConvexShape* objectB,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver*	penetrationDepthSolver)
+:m_cachedSeparatingAxis(0.f,0.f,1.f),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_simplexSolver(simplexSolver),
+m_minkowskiA(objectA),
+m_minkowskiB(objectB),
+m_ignoreMargin(false)
+{
+}
+
+void GjkPairDetector::GetClosestPoints(const ClosestPointInput& input,Result& output,class IDebugDraw* debugDraw)
+{
+	SimdScalar distance=0.f;
+	SimdVector3	normalInB(0.f,0.f,0.f);
+	SimdVector3 pointOnA,pointOnB;
+
+	float marginA = m_minkowskiA->GetMargin();
+	float marginB = m_minkowskiB->GetMargin();
+
+	//for CCD we don't use margins
+	if (m_ignoreMargin)
+	{
+		marginA = 0.f;
+		marginB = 0.f;
+	}
+
+	bool isValid = false;
+	bool checkSimplex = false;
+	bool checkPenetration = true;
+
+	{
+		SimdScalar squaredDistance = SIMD_INFINITY;
+		SimdScalar delta = 0.f;
+		
+		SimdScalar margin = marginA + marginB;
+		
+		
+
+		m_simplexSolver->reset();
+		
+		while (true)
+		{
+
+			SimdVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
+			SimdVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
+
+			SimdVector3 pInA = m_minkowskiA->LocalGetSupportingVertexWithoutMargin(seperatingAxisInA);
+			SimdVector3 qInB = m_minkowskiB->LocalGetSupportingVertexWithoutMargin(seperatingAxisInB);
+			SimdPoint3  pWorld = input.m_transformA(pInA);	
+			SimdPoint3  qWorld = input.m_transformB(qInB);
+			
+			SimdVector3 w	= pWorld - qWorld;
+			delta = m_cachedSeparatingAxis.dot(w);
+
+			// potential exit, they don't overlap
+			if ((delta > SimdScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) 
+			{
+				checkPenetration = false;
+				break;
+			}
+
+			//exit 0: the new point is already in the simplex, or we didn't come any closer
+			if (m_simplexSolver->inSimplex(w))
+			{
+				checkSimplex = true;
+				break;
+			}
+			// are we getting any closer ?
+			if (squaredDistance - delta <= squaredDistance * rel_error2)
+			{
+				checkSimplex = true;
+				break;
+			}
+			//add current vertex to simplex
+			m_simplexSolver->addVertex(w, pWorld, qWorld);
+
+			//calculate the closest point to the origin (update vector v)
+			if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
+			{
+				checkSimplex = true;
+				break;
+			}
+
+			SimdScalar previousSquaredDistance = squaredDistance;
+			squaredDistance = m_cachedSeparatingAxis.length2();
+			
+			//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
+
+			//are we getting any closer ?
+			if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) 
+			{ 
+				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+				checkSimplex = true;
+				break;
+			}
+			bool check = (!m_simplexSolver->fullSimplex());
+			//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
+
+			if (!check)
+			{
+				//do we need this backup_closest here ?
+				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+				break;
+			}
+		}
+
+		if (checkSimplex)
+		{
+			m_simplexSolver->compute_points(pointOnA, pointOnB);
+			normalInB = pointOnA-pointOnB;
+			float lenSqr = m_cachedSeparatingAxis.length2();
+			//valid normal
+			if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+			{
+				float rlen = 1.f / SimdSqrt(lenSqr );
+				normalInB *= rlen; //normalize
+				SimdScalar s = SimdSqrt(squaredDistance);
+				ASSERT(s > SimdScalar(0.0));
+				pointOnA -= m_cachedSeparatingAxis * (marginA / s);
+				pointOnB += m_cachedSeparatingAxis * (marginB / s);
+				distance = ((1.f/rlen) - margin);
+				isValid = true;
+			}
+		}
+
+		if (checkPenetration && !isValid)
+		{
+			//penetration case
+		
+			//if there is no way to handle penetrations, bail out
+			if (m_penetrationDepthSolver)
+			{
+				// Penetration depth case.
+				isValid = m_penetrationDepthSolver->CalcPenDepth( 
+					*m_simplexSolver, 
+					m_minkowskiA,m_minkowskiB,
+					input.m_transformA,input.m_transformB,
+					m_cachedSeparatingAxis, pointOnA, pointOnB,
+					debugDraw
+					);
+
+				if (isValid)
+				{
+					normalInB = pointOnB-pointOnA;
+					float lenSqr = normalInB.length2();
+					if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+					{
+						normalInB /= SimdSqrt(lenSqr);
+						distance = -(pointOnA-pointOnB).length();
+					} else
+					{
+						isValid = false;
+					}
+				}
+			}
+		}
+	}
+
+	if (isValid)
+	{
+		output.AddContactPoint(
+			normalInB,
+			pointOnB,
+			distance);
+		//printf("gjk add:%f",distance);
+	} 
+
+
+}
+
+
+
+
+

Bullet/NarrowPhaseCollision/GjkPairDetector.h

@@ -0,0 +1,76 @@
+/*
+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 GJK_PAIR_DETECTOR_H
+#define GJK_PAIR_DETECTOR_H
+
+#include "DiscreteCollisionDetectorInterface.h"
+#include "SimdPoint3.h"
+
+#include <CollisionShapes/CollisionMargin.h>
+
+class ConvexShape;
+#include "SimplexSolverInterface.h"
+class ConvexPenetrationDepthSolver;
+
+/// GjkPairDetector uses GJK to implement the DiscreteCollisionDetectorInterface
+class GjkPairDetector : public DiscreteCollisionDetectorInterface
+{
+	
+
+	SimdVector3	m_cachedSeparatingAxis;
+	ConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
+	SimplexSolverInterface* m_simplexSolver;
+	ConvexShape* m_minkowskiA;
+	ConvexShape* m_minkowskiB;
+	bool		m_ignoreMargin;
+
+public:
+
+	GjkPairDetector(ConvexShape* objectA,ConvexShape* objectB,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver*	penetrationDepthSolver);
+	virtual ~GjkPairDetector() {};
+
+	virtual void	GetClosestPoints(const ClosestPointInput& input,Result& output,class IDebugDraw* debugDraw);
+
+	void SetMinkowskiA(ConvexShape* minkA)
+	{
+		m_minkowskiA = minkA;
+	}
+
+	void SetMinkowskiB(ConvexShape* minkB)
+	{
+		m_minkowskiB = minkB;
+	}
+	void SetCachedSeperatingAxis(const SimdVector3& seperatingAxis)
+	{
+		m_cachedSeparatingAxis = seperatingAxis;
+	}
+
+	void	SetPenetrationDepthSolver(ConvexPenetrationDepthSolver*	penetrationDepthSolver)
+	{
+		m_penetrationDepthSolver = penetrationDepthSolver;
+	}
+
+	void	SetIgnoreMargin(bool ignoreMargin)
+	{
+		m_ignoreMargin = ignoreMargin;
+	}
+
+};
+
+#endif //GJK_PAIR_DETECTOR_H

Bullet/NarrowPhaseCollision/Hull.h

@@ -0,0 +1,174 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+//
+// Hull.h
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#ifndef SAT_HULL_H
+#define SAT_HULL_H
+
+#include "Maths.h"
+#include "Shape.h"
+
+
+class DynWorld;
+class HullContactCollector;
+
+/// Hull implements a convex collision detection algorithm based on Separating Axis Theorem (SAT). It is an alternative to GJK.
+/// It calculates the separating axis, and based on that it calculates the contact manifold (points) in one go.
+/// The separating axis calculation is approximated, not all edge-edge calculations are performed (performance reasons).
+/// Future idea is to combine this with GJK for polyhedra: GJK to calculate the separating axis, and Hull clipping code to calculate the full set of contacts.
+class Hull : public Shape
+{
+	friend class ShapeCollider;
+
+public:
+	struct Edge
+	{
+		short m_verts[2];
+		short m_faces[2];
+		short m_nextEdge[2];	// for each m_face
+	};
+
+	struct Face
+	{
+		short m_numEdges;
+		short m_firstEdge;
+	};
+
+private:
+	static const int kMaxVerts = 256;
+	static const int kMaxFaces = 256;
+	static const int kMaxEdges = 256;
+
+	short m_numVerts;
+	short m_numFaces;
+	short m_numEdges;
+
+	Point3* m_pVerts;
+	Face* m_pFaces;
+	Edge* m_pEdges;
+	Plane* m_pPlanes;
+
+	// hull construction stuff
+	static const int kTmpFaceMaxVerts = 64;
+	struct TmpFace
+	{
+		short m_index;
+		short m_next;
+		short m_numVerts;
+		short m_verts[kTmpFaceMaxVerts];
+		short m_edges[kTmpFaceMaxVerts];
+		Plane m_plane;
+	};
+
+	struct TmpEdge
+	{
+		short m_index;
+		short m_next;
+		short m_verts[2];
+		short m_faces[2];
+	};
+
+	static short s_firstFreeTmpFace;
+	static short s_firstUsedTmpFace;
+	static TmpFace* s_pTmpFaces;
+
+	static short s_firstFreeTmpEdge;
+	static short s_firstUsedTmpEdge;
+	static TmpEdge* s_pTmpEdges;
+
+	static const Point3* s_pPoints;
+
+	static short AllocTmpFace();
+	static void FreeTmpFace(short face);
+	static TmpFace* GetTmpFace(short index) {if (index < 0) return 0; return s_pTmpFaces + index;}
+
+	static short AllocTmpEdge();
+	static void FreeTmpEdge(short edge);
+	static TmpEdge* GetTmpEdge(short index) {if (index < 0) return 0; return s_pTmpEdges + index;}
+
+	static short MatchOrAddEdge(short vert0, short vert1, short face);
+	static void UnmatchOrRemoveEdge(short edge, short face);
+
+	static short AddTmpFace(short vert0, short vert1, short vert2);
+	static short AddTmpFace(short numVerts, short* pVerts);
+	static short AddTmpFace(short vert0, short numOtherVerts, short* pVerts);
+	static void RemoveTmpFace(short face);
+
+	static bool TmpFaceAddPoint(short point, short face);
+
+	static int RemoveVisibleFaces(const Point3& point);
+	static void FillHole(short newVertex);
+	static Hull* MakeHullFromTemp();
+
+public:
+	Hull();
+	~Hull();
+
+//	ObjectType GetObjectType() const {return kTypeHull;}
+
+	short GetNumVertices() const;
+	short GetNumFaces() const;
+	short GetNumEdges() const;
+
+	const Point3& GetVertex(short index) const;
+	const Face& GetFace(short index) const;
+	const Edge& GetEdge(short index) const;
+	const Plane& GetPlane(short index) const;
+
+	short GetFaceFirstEdge(short face) const;
+	short GetFaceNextEdge(short face, short prevEdge) const;
+
+	short GetEdgeVertex0(short face, short edge) const;
+	short GetEdgeVertex1(short face, short edge) const;
+
+	short GetEdgeOtherFace(short edge, short face) const;
+
+	Point3 GetFaceCentroid(short face) const;
+
+	//static void ProcessHullHull(Separation& sep);
+	static void ProcessHullHull(Separation& sep,const Hull& shapeA,const Hull& shapeB,const Transform& trA,const Transform& trB, HullContactCollector* collector);
+
+	virtual void ComputeInertia(const Transform& transform, Point3& centerOfMass, Matrix33& inertia, float totalMass) const;
+	virtual Bounds3 ComputeBounds(const Transform& transform) const;
+
+	static Hull* MakeHull(int numPoints, const Point3* pPoints);
+
+	//for contact generation
+
+
+	
+	/// Clips a face to the back of a plane
+	static int ClipFace(int numVerts, Point3** ppVtxIn, Point3** ppVtxOut, const Plane& plane);
+
+	static bool GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const Point3* pVertsB,
+										  const Transform& trA, const Transform& trB,
+										  const Hull& hullA,
+										  const Hull& hullB
+										  );
+
+	static int AddContactsHullHull(Separation& sep, const Point3* pVertsA, const Point3* pVertsB,
+									   const Transform& trA, const Transform& trB,const Hull& hullA,const Hull& hullB,
+									   HullContactCollector* hullContactCollector);
+
+
+
+};
+
+
+
+#include "hull.inl"
+
+#endif //SAT_HULL_H

Bullet/NarrowPhaseCollision/Hull.inl

@@ -0,0 +1,100 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+//
+// Hull.inl
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#pragma once
+
+#include <assert.h>
+
+inline short Hull::GetNumVertices() const
+{
+	return m_numVerts;
+}
+
+inline short Hull::GetNumFaces() const
+{
+	return m_numFaces;
+}
+
+inline short Hull::GetNumEdges() const
+{
+	return m_numEdges;
+}
+
+inline const Point3& Hull::GetVertex(short index) const
+{
+	return m_pVerts[index];
+}
+
+inline const Hull::Face& Hull::GetFace(short index) const
+{
+	return m_pFaces[index];
+}
+
+inline const Hull::Edge& Hull::GetEdge(short index) const
+{
+	return m_pEdges[index];
+}
+
+inline const Plane& Hull::GetPlane(short index) const
+{
+	return m_pPlanes[index];
+} 
+
+inline short Hull::GetFaceFirstEdge(short face) const
+{
+	assert(face >= 0 && face < m_numFaces);
+
+	return m_pFaces[face].m_firstEdge;
+}
+
+inline short Hull::GetFaceNextEdge(short face, short prevEdge) const
+{
+	assert(face >= 0 && face < m_numFaces);
+	assert(prevEdge >= 0 && prevEdge < m_numEdges);
+
+	const Edge& e = m_pEdges[prevEdge];
+	return e.m_nextEdge[face == e.m_faces[1]];
+}
+
+inline short Hull::GetEdgeVertex0(short face, short edge) const
+{
+	assert(face >= 0 && face < m_numFaces);
+	assert(edge >= 0 && edge < m_numEdges);
+
+	const Edge& e = m_pEdges[edge];
+	return e.m_verts[face == e.m_faces[0]];
+}
+
+inline short Hull::GetEdgeVertex1(short face, short edge) const
+{
+	assert(face >= 0 && face < m_numFaces);
+	assert(edge >= 0 && edge < m_numEdges);
+
+	const Edge& e = m_pEdges[edge];
+	return e.m_verts[face == e.m_faces[1]];
+}
+
+inline short Hull::GetEdgeOtherFace(short edge, short face) const
+{
+	assert(face >= 0 && face < m_numFaces);
+	assert(edge >= 0 && edge < m_numEdges);
+
+	const Edge& e = m_pEdges[edge];
+	assert(e.m_faces[0] == face || e.m_faces[1] == face);
+
+	return e.m_faces[face == e.m_faces[0]];
+}

Bullet/NarrowPhaseCollision/HullContactCollector.h

@@ -0,0 +1,38 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef HULL_CONTACT_COLLECTOR_H
+#define HULL_CONTACT_COLLECTOR_H
+
+class Vector3;
+class Point3;
+class Scalar;
+struct Separation;
+
+///HullContactCollector  collects the Hull computation to the contact point results
+class HullContactCollector
+{
+public:
+
+	virtual ~HullContactCollector() {};
+
+	virtual int	BatchAddContactGroup(const Separation& sep,int numContacts,const Vector3& normalWorld,const Vector3& tangent,const Point3* positionsWorld,const float* depths)=0;
+
+	virtual int		GetMaxNumContacts() const = 0;
+
+};
+
+#endif //HULL_CONTACT_COLLECTOR_H

Bullet/NarrowPhaseCollision/ManifoldContactAddResult.cpp

@@ -0,0 +1,48 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "ManifoldContactAddResult.h"
+#include "NarrowPhaseCollision/PersistentManifold.h"
+
+ManifoldContactAddResult::ManifoldContactAddResult(SimdTransform transA,SimdTransform transB,PersistentManifold* manifoldPtr)
+		:m_manifoldPtr(manifoldPtr)
+{
+	m_transAInv = transA.inverse();
+	m_transBInv = transB.inverse();
+
+}
+
+
+void ManifoldContactAddResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
+{
+	if (depth > m_manifoldPtr->GetContactBreakingTreshold())
+		return;
+
+
+	SimdVector3 pointA = pointInWorld + normalOnBInWorld * depth;
+	SimdVector3 localA = m_transAInv(pointA );
+	SimdVector3 localB = m_transBInv(pointInWorld);
+	ManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
+
+	int insertIndex = m_manifoldPtr->GetCacheEntry(newPt);
+	if (insertIndex >= 0)
+	{
+		m_manifoldPtr->ReplaceContactPoint(newPt,insertIndex);
+	} else
+	{
+		m_manifoldPtr->AddManifoldPoint(newPt);
+	}
+}
+

Bullet/NarrowPhaseCollision/ManifoldContactAddResult.h

@@ -0,0 +1,37 @@
+/*
+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 MANIFOLD_CONTACT_ADD_RESULT_H
+#define MANIFOLD_CONTACT_ADD_RESULT_H
+
+#include "NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h"
+class PersistentManifold;
+
+class ManifoldContactAddResult : public DiscreteCollisionDetectorInterface::Result
+{
+	PersistentManifold* m_manifoldPtr;
+	SimdTransform	m_transAInv;
+	SimdTransform	m_transBInv;
+
+public:
+
+	ManifoldContactAddResult(SimdTransform transA,SimdTransform transB,PersistentManifold* manifoldPtr);
+
+	virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth);
+
+};
+
+#endif //MANIFOLD_CONTACT_ADD_RESULT_H

Bullet/NarrowPhaseCollision/ManifoldPoint.h

@@ -0,0 +1,94 @@
+/*
+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 MANIFOLD_CONTACT_POINT_H
+#define MANIFOLD_CONTACT_POINT_H
+
+#include "SimdVector3.h"
+#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)
+			{
+			}
+
+			ManifoldPoint( const SimdVector3 &pointA, const SimdVector3 &pointB, 
+					const SimdVector3 &normal, 
+					SimdScalar distance ) :
+					m_localPointA( pointA ), 
+					m_localPointB( pointB ), 
+					m_normalWorldOnB( normal ), 
+					m_distance1( distance ),
+					m_userPersistentData(0),					
+					m_lifeTime(0)
+			{
+				
+					
+			}
+
+			
+
+			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;
+		
+			float	m_distance1;
+
+				
+			void*	m_userPersistentData;
+
+			int		m_lifeTime;//lifetime of the contactpoint in frames
+			
+			float GetDistance() const
+			{
+				return m_distance1;
+			}
+			int	GetLifeTime() const
+			{
+				return m_lifeTime;
+			}
+
+			SimdVector3 GetPositionWorldOnA() {
+				return m_positionWorldOnA;
+//				return m_positionWorldOnB + m_normalWorldOnB * m_distance1;
+			}
+
+			const SimdVector3& GetPositionWorldOnB()
+			{
+				return m_positionWorldOnB;
+			}
+
+			void	SetDistance(float dist)
+			{
+				m_distance1 = dist;
+			}
+			
+			
+
+	};
+
+#endif //MANIFOLD_CONTACT_POINT_H

Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp

@@ -0,0 +1,243 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "MinkowskiPenetrationDepthSolver.h"
+#include "CollisionShapes/MinkowskiSumShape.h"
+#include "NarrowPhaseCollision/SubSimplexConvexCast.h"
+#include "NarrowPhaseCollision/VoronoiSimplexSolver.h"
+#include "NarrowPhaseCollision/GjkPairDetector.h"
+
+
+struct MyResult : public DiscreteCollisionDetectorInterface::Result
+{
+
+	MyResult():m_hasResult(false)
+	{
+	}
+	
+	SimdVector3 m_normalOnBInWorld;
+	SimdVector3 m_pointInWorld;
+	float m_depth;
+	bool	m_hasResult;
+
+	void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
+	{
+		m_normalOnBInWorld = normalOnBInWorld;
+		m_pointInWorld = pointInWorld;
+		m_depth = depth;
+		m_hasResult = true;
+	}
+};
+
+#define NUM_UNITSPHERE_POINTS 42
+static SimdVector3	sPenetrationDirections[NUM_UNITSPHERE_POINTS] = 
+{
+SimdVector3(0.000000f , -0.000000f,-1.000000f),
+SimdVector3(0.723608f , -0.525725f,-0.447219f),
+SimdVector3(-0.276388f , -0.850649f,-0.447219f),
+SimdVector3(-0.894426f , -0.000000f,-0.447216f),
+SimdVector3(-0.276388f , 0.850649f,-0.447220f),
+SimdVector3(0.723608f , 0.525725f,-0.447219f),
+SimdVector3(0.276388f , -0.850649f,0.447220f),
+SimdVector3(-0.723608f , -0.525725f,0.447219f),
+SimdVector3(-0.723608f , 0.525725f,0.447219f),
+SimdVector3(0.276388f , 0.850649f,0.447219f),
+SimdVector3(0.894426f , 0.000000f,0.447216f),
+SimdVector3(-0.000000f , 0.000000f,1.000000f),
+SimdVector3(0.425323f , -0.309011f,-0.850654f),
+SimdVector3(-0.162456f , -0.499995f,-0.850654f),
+SimdVector3(0.262869f , -0.809012f,-0.525738f),
+SimdVector3(0.425323f , 0.309011f,-0.850654f),
+SimdVector3(0.850648f , -0.000000f,-0.525736f),
+SimdVector3(-0.525730f , -0.000000f,-0.850652f),
+SimdVector3(-0.688190f , -0.499997f,-0.525736f),
+SimdVector3(-0.162456f , 0.499995f,-0.850654f),
+SimdVector3(-0.688190f , 0.499997f,-0.525736f),
+SimdVector3(0.262869f , 0.809012f,-0.525738f),
+SimdVector3(0.951058f , 0.309013f,0.000000f),
+SimdVector3(0.951058f , -0.309013f,0.000000f),
+SimdVector3(0.587786f , -0.809017f,0.000000f),
+SimdVector3(0.000000f , -1.000000f,0.000000f),
+SimdVector3(-0.587786f , -0.809017f,0.000000f),
+SimdVector3(-0.951058f , -0.309013f,-0.000000f),
+SimdVector3(-0.951058f , 0.309013f,-0.000000f),
+SimdVector3(-0.587786f , 0.809017f,-0.000000f),
+SimdVector3(-0.000000f , 1.000000f,-0.000000f),
+SimdVector3(0.587786f , 0.809017f,-0.000000f),
+SimdVector3(0.688190f , -0.499997f,0.525736f),
+SimdVector3(-0.262869f , -0.809012f,0.525738f),
+SimdVector3(-0.850648f , 0.000000f,0.525736f),
+SimdVector3(-0.262869f , 0.809012f,0.525738f),
+SimdVector3(0.688190f , 0.499997f,0.525736f),
+SimdVector3(0.525730f , 0.000000f,0.850652f),
+SimdVector3(0.162456f , -0.499995f,0.850654f),
+SimdVector3(-0.425323f , -0.309011f,0.850654f),
+SimdVector3(-0.425323f , 0.309011f,0.850654f),
+SimdVector3(0.162456f , 0.499995f,0.850654f)
+};
+
+
+bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simplexSolver,
+												   ConvexShape* convexA,ConvexShape* convexB,
+												   const SimdTransform& transA,const SimdTransform& transB,
+												   SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb,
+												   class IDebugDraw* debugDraw
+												   )
+{
+
+	//just take fixed number of orientation, and sample the penetration depth in that direction
+	float minProj = 1e30f;
+	SimdVector3 minNorm;
+	SimdVector3 minVertex;
+	SimdVector3 minA,minB;
+	SimdVector3 seperatingAxisInA,seperatingAxisInB;
+	SimdVector3 pInA,qInB,pWorld,qWorld,w;
+
+#define USE_BATCHED_SUPPORT 1
+#ifdef USE_BATCHED_SUPPORT
+	SimdVector3	supportVerticesABatch[NUM_UNITSPHERE_POINTS];
+	SimdVector3	supportVerticesBBatch[NUM_UNITSPHERE_POINTS];
+	SimdVector3	seperatingAxisInABatch[NUM_UNITSPHERE_POINTS];
+	SimdVector3	seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS];
+	int i;
+
+	for (i=0;i<NUM_UNITSPHERE_POINTS;i++)
+	{
+		const SimdVector3& norm = sPenetrationDirections[i];
+		seperatingAxisInABatch[i] = (-norm)* transA.getBasis();
+		seperatingAxisInBBatch[i] = norm * transB.getBasis();
+	}
+
+	convexA->BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,NUM_UNITSPHERE_POINTS);
+	convexB->BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,NUM_UNITSPHERE_POINTS);
+	for (i=0;i<NUM_UNITSPHERE_POINTS;i++)
+	{
+		const SimdVector3& norm = sPenetrationDirections[i];
+		seperatingAxisInA = seperatingAxisInABatch[i];
+		seperatingAxisInB = seperatingAxisInBBatch[i];
+
+		pInA = supportVerticesABatch[i];
+		qInB = supportVerticesBBatch[i];
+
+		pWorld = transA(pInA);	
+		qWorld = transB(qInB);
+		w	= qWorld - pWorld;
+		float delta = norm.dot(w);
+		//find smallest delta
+		if (delta < minProj)
+		{
+			minProj = delta;
+			minNorm = norm;
+			minA = pWorld;
+			minB = qWorld;
+		}
+	}	
+#else
+	for (int i=0;i<NUM_UNITSPHERE_POINTS;i++)
+	{
+		const SimdVector3& norm = sPenetrationDirections[i];
+		seperatingAxisInA = (-norm)* transA.getBasis();
+		seperatingAxisInB = norm* transB.getBasis();
+		pInA = convexA->LocalGetSupportingVertexWithoutMargin(seperatingAxisInA);
+		qInB = convexB->LocalGetSupportingVertexWithoutMargin(seperatingAxisInB);
+		pWorld = transA(pInA);	
+		qWorld = transB(qInB);
+		w	= qWorld - pWorld;
+		float delta = norm.dot(w);
+		//find smallest delta
+		if (delta < minProj)
+		{
+			minProj = delta;
+			minNorm = norm;
+			minA = pWorld;
+			minB = qWorld;
+		}
+	}
+#endif //USE_BATCHED_SUPPORT
+
+	//add the margins
+
+	minA += minNorm*convexA->GetMargin();
+	minB -= minNorm*convexB->GetMargin();
+	minProj += (convexA->GetMargin() + convexB->GetMargin());
+
+
+	
+
+//#define DEBUG_DRAW 1
+#ifdef DEBUG_DRAW
+	if (debugDraw)
+	{
+		SimdVector3 color(0,1,0);
+		debugDraw->DrawLine(minA,minB,color);
+		color = SimdVector3 (1,1,1);
+		SimdVector3 vec = minB-minA;
+		float prj2 = minNorm.dot(vec);
+		debugDraw->DrawLine(minA,minA+(minNorm*minProj),color);
+
+	}
+#endif //DEBUG_DRAW
+
+	
+
+	GjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
+
+	SimdScalar offsetDist = minProj;
+	SimdVector3 offset = minNorm * offsetDist;
+	
+
+
+	GjkPairDetector::ClosestPointInput input;
+		
+	SimdVector3 newOrg = transA.getOrigin() + offset;
+
+	SimdTransform displacedTrans = transA;
+	displacedTrans.setOrigin(newOrg);
+
+	input.m_transformA = displacedTrans;
+	input.m_transformB = transB;
+	input.m_maximumDistanceSquared = 1e30f;//minProj;
+	
+	MyResult res;
+	gjkdet.GetClosestPoints(input,res,debugDraw);
+
+	float correctedMinNorm = minProj - res.m_depth;
+
+
+	//the penetration depth is over-estimated, relax it
+	float penetration_relaxation= 1.f;
+	minNorm*=penetration_relaxation;
+
+	if (res.m_hasResult)
+	{
+
+		pa = res.m_pointInWorld - minNorm * correctedMinNorm;
+		pb = res.m_pointInWorld;
+		
+#ifdef DEBUG_DRAW
+		if (debugDraw)
+		{
+			SimdVector3 color(1,0,0);
+			debugDraw->DrawLine(pa,pb,color);
+		}
+#endif//DEBUG_DRAW
+
+
+	}
+	return res.m_hasResult;
+}
+
+
+

Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.h

@@ -0,0 +1,37 @@
+/*
+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 MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+#define MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+
+#include "ConvexPenetrationDepthSolver.h"
+
+///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation.
+///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.
+class MinkowskiPenetrationDepthSolver : public ConvexPenetrationDepthSolver
+{
+public:
+
+	virtual bool CalcPenDepth( SimplexSolverInterface& simplexSolver,
+	ConvexShape* convexA,ConvexShape* convexB,
+				const SimdTransform& transA,const SimdTransform& transB,
+			SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb,
+			class IDebugDraw* debugDraw
+			);
+
+};
+
+#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+

Bullet/NarrowPhaseCollision/PersistentManifold.cpp

@@ -0,0 +1,231 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "PersistentManifold.h"
+#include "SimdTransform.h"
+#include <assert.h>
+
+float						gContactBreakingTreshold = 0.02f;
+ContactDestroyedCallback	gContactCallback = 0;
+
+
+PersistentManifold::PersistentManifold()
+:m_body0(0),
+m_body1(0),
+m_cachedPoints (0),
+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
+	}
+
+	
+}
+
+
+
+
+int PersistentManifold::SortCachedPoints(const ManifoldPoint& pt) 
+{
+
+		//calculate 4 possible cases areas, and take biggest area
+
+		SimdScalar res0,res1,res2,res3;
+
+		{
+			SimdVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
+			SimdVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
+			SimdVector3 cross = a0.cross(b0);
+			res0 = cross.length2();
+		}
+		{
+			SimdVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+			SimdVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
+			SimdVector3 cross = a1.cross(b1);
+			res1 = cross.length2();
+		}
+		{
+			SimdVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+			SimdVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
+			SimdVector3 cross = a2.cross(b2);
+			res2 = cross.length2();
+		}
+		{
+			SimdVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+			SimdVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
+			SimdVector3 cross = a3.cross(b3);
+			res3 = cross.length2();
+		}
+
+		SimdVector4 maxvec(res0,res1,res2,res3);
+		int biggestarea = maxvec.closestAxis4();
+
+		return biggestarea;
+
+
+}
+
+
+
+int PersistentManifold::GetCacheEntry(const ManifoldPoint& newPoint) const
+{
+	SimdScalar shortestDist =  GetContactBreakingTreshold() * GetContactBreakingTreshold();
+	int size = GetNumContacts();
+	int nearestPoint = -1;
+	for( int i = 0; i < size; i++ )
+	{
+		const ManifoldPoint &mp = m_pointCache[i];
+
+		SimdVector3 diffA =  mp.m_localPointA- newPoint.m_localPointA;
+		const SimdScalar distToManiPoint = diffA.dot(diffA);
+		if( distToManiPoint < shortestDist )
+		{
+			shortestDist = distToManiPoint;
+			nearestPoint = i;
+		}
+	}
+	return nearestPoint;
+}
+
+void PersistentManifold::AddManifoldPoint(const ManifoldPoint& newPoint)
+{
+	assert(ValidContactDistance(newPoint));
+
+	int insertIndex = GetNumContacts();
+	if (insertIndex == MANIFOLD_CACHE_SIZE)
+	{
+#if MANIFOLD_CACHE_SIZE >= 4
+		//sort cache so best points come first, based on area
+		insertIndex = SortCachedPoints(newPoint);
+#else
+		insertIndex = 0;
+#endif
+
+		
+	} else
+	{
+		m_cachedPoints++;
+
+		
+	}
+	ReplaceContactPoint(newPoint,insertIndex);
+}
+
+float	PersistentManifold::GetContactBreakingTreshold() const
+{
+	return gContactBreakingTreshold;
+}
+
+void PersistentManifold::RefreshContactPoints(const SimdTransform& trA,const SimdTransform& trB)
+{
+	int i;
+
+	/// first refresh worldspace positions and distance
+	for (i=GetNumContacts()-1;i>=0;i--)
+	{
+		ManifoldPoint &manifoldPoint = m_pointCache[i];
+		manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
+		manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
+		manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
+		manifoldPoint.m_lifeTime++;
+	}
+
+	/// then 
+	SimdScalar distance2d;
+	SimdVector3 projectedDifference,projectedPoint;
+	for (i=GetNumContacts()-1;i>=0;i--)
+	{
+		
+		ManifoldPoint &manifoldPoint = m_pointCache[i];
+		//contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
+		if (!ValidContactDistance(manifoldPoint))
+		{
+			RemoveContactPoint(i);
+		} else
+		{
+			//contact also becomes invalid when relative movement orthogonal to normal exceeds margin
+			projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
+			projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
+			distance2d = projectedDifference.dot(projectedDifference);
+			if (distance2d  > GetContactBreakingTreshold()*GetContactBreakingTreshold() )
+			{
+				RemoveContactPoint(i);
+			}
+		}
+	}
+#ifdef DEBUG_PERSISTENCY
+	DebugPersistency();
+#endif //
+}
+
+
+
+
+

Bullet/NarrowPhaseCollision/PersistentManifold.h

@@ -0,0 +1,140 @@
+/*
+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 PERSISTENT_MANIFOLD_H
+#define PERSISTENT_MANIFOLD_H
+
+
+#include "SimdVector3.h"
+#include "SimdTransform.h"
+#include "ManifoldPoint.h"
+
+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.
+///It does contact filtering/contact reduction.
+class PersistentManifold 
+{
+
+	ManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE];
+
+	/// this two body pointers can point to the physics rigidbody class.
+	/// void* will allow any rigidbody class
+	void* m_body0;
+	void* m_body1;
+	int	m_cachedPoints;
+
+	
+	/// sort cached points so most isolated points come first
+	int	SortCachedPoints(const ManifoldPoint& pt);
+
+	int		FindContactPoint(const ManifoldPoint* unUsed, int numUnused,const ManifoldPoint& pt);
+
+public:
+
+	int m_index1;
+
+	PersistentManifold();
+
+	PersistentManifold(void* body0,void* body1)
+		: m_body0(body0),m_body1(body1),m_cachedPoints(0)
+	{
+	}
+
+	inline void* GetBody0() { return m_body0;}
+	inline void* GetBody1() { return m_body1;}
+
+	inline const void* GetBody0() const { return m_body0;}
+	inline const void* GetBody1() const { return m_body1;}
+
+	void	SetBodies(void* body0,void* body1)
+	{
+		m_body0 = body0;
+		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
+	{
+		ASSERT(index < m_cachedPoints);
+		return m_pointCache[index];
+	}
+
+	inline ManifoldPoint& GetContactPoint(int index)
+	{
+		ASSERT(index < m_cachedPoints);
+		return m_pointCache[index];
+	}
+
+	/// todo: get this margin from the current physics / collision environment
+	float	GetContactBreakingTreshold() const;
+	
+	int GetCacheEntry(const ManifoldPoint& newPoint) const;
+
+	void AddManifoldPoint( const ManifoldPoint& newPoint);
+
+	void RemoveContactPoint (int index)
+	{
+		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;
+	}
+
+	bool ValidContactDistance(const ManifoldPoint& pt) const
+	{
+		return pt.m_distance1 <= GetContactBreakingTreshold();
+	}
+	/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
+	void	RefreshContactPoints(  const SimdTransform& trA,const SimdTransform& trB);
+
+	void	ClearManifold();
+
+
+
+};
+
+
+
+#endif //PERSISTENT_MANIFOLD_H

Bullet/NarrowPhaseCollision/PointCollector.h

@@ -0,0 +1,52 @@
+/*
+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 POINT_COLLECTOR_H
+#define POINT_COLLECTOR_H
+
+#include "DiscreteCollisionDetectorInterface.h"
+
+
+
+struct PointCollector : public DiscreteCollisionDetectorInterface::Result
+{
+	
+	
+	SimdVector3 m_normalOnBInWorld;
+	SimdVector3 m_pointInWorld;
+	SimdScalar	m_distance;//negative means penetration
+
+	bool	m_hasResult;
+
+	PointCollector () 
+		: m_distance(1e30f),m_hasResult(false)
+	{
+	}
+
+	virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
+	{
+		if (depth< m_distance)
+		{
+			m_hasResult = true;
+			m_normalOnBInWorld = normalOnBInWorld;
+			m_pointInWorld = pointInWorld;
+			//negative means penetration
+			m_distance = depth;
+		}
+	}
+};
+
+#endif //POINT_COLLECTOR_H
+

Bullet/NarrowPhaseCollision/RaycastCallback.cpp

@@ -0,0 +1,101 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "RaycastCallback.h"
+
+TriangleRaycastCallback::TriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to)
+	:
+	m_from(from),
+	m_to(to),
+	m_hitFraction(1.f)
+{
+
+}
+
+
+
+void TriangleRaycastCallback::ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex)
+{
+	
+
+	const SimdVector3 &vert0=triangle[0];
+	const SimdVector3 &vert1=triangle[1];
+	const SimdVector3 &vert2=triangle[2];
+
+	SimdVector3 v10; v10 = vert1 - vert0 ;
+	SimdVector3 v20; v20 = vert2 - vert0 ;
+
+	SimdVector3 triangleNormal; triangleNormal = v10.cross( v20 );
+	
+	const float dist = vert0.dot(triangleNormal);
+	float dist_a = triangleNormal.dot(m_from) ;
+	dist_a-= dist;
+	float dist_b = triangleNormal.dot(m_to);
+	dist_b -= dist;
+
+	if ( dist_a * dist_b >= 0.0f)
+	{
+		return ; // same sign
+	}
+	
+	const float proj_length=dist_a-dist_b;
+	const float distance = (dist_a)/(proj_length);
+	// Now we have the intersection point on the plane, we'll see if it's inside the triangle
+	// Add an epsilon as a tolerance for the raycast,
+	// in case the ray hits exacly on the edge of the triangle.
+	// It must be scaled for the triangle size.
+	
+	if(distance < m_hitFraction)
+	{
+		
+
+		float edge_tolerance =triangleNormal.length2();		
+		edge_tolerance *= -0.0001f;
+		SimdVector3 point; point.setInterpolate3( m_from, m_to, distance);
+		{
+			SimdVector3 v0p; v0p = vert0 - point;
+			SimdVector3 v1p; v1p = vert1 - point;
+			SimdVector3 cp0; cp0 = v0p.cross( v1p );
+
+			if ( (float)(cp0.dot(triangleNormal)) >=edge_tolerance) 
+			{
+						
+
+				SimdVector3 v2p; v2p = vert2 -  point;
+				SimdVector3 cp1;
+				cp1 = v1p.cross( v2p);
+				if ( (float)(cp1.dot(triangleNormal)) >=edge_tolerance) 
+				{
+					SimdVector3 cp2;
+					cp2 = v2p.cross(v0p);
+					
+					if ( (float)(cp2.dot(triangleNormal)) >=edge_tolerance) 
+					{
+
+						if ( dist_a > 0 )
+						{
+							m_hitFraction = ReportHit(triangleNormal,distance,partId,triangleIndex);
+						}
+						else
+						{
+							m_hitFraction = ReportHit(-triangleNormal,distance,partId,triangleIndex);
+						}
+					}
+				}
+			}
+		}
+	}
+}

Bullet/NarrowPhaseCollision/RaycastCallback.h

@@ -0,0 +1,42 @@
+/*
+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 RAYCAST_TRI_CALLBACK_H
+#define RAYCAST_TRI_CALLBACK_H
+
+#include "CollisionShapes/TriangleCallback.h"
+struct BroadphaseProxy;
+
+
+class  TriangleRaycastCallback: public TriangleCallback
+{
+public:
+
+		//input
+	SimdVector3 m_from;
+	SimdVector3 m_to;
+
+	float	m_hitFraction;
+
+	TriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to);
+	
+	virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex);
+
+	virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) = 0;
+	
+};
+
+#endif //RAYCAST_TRI_CALLBACK_H
+

Bullet/NarrowPhaseCollision/Shape.cpp

@@ -0,0 +1,34 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+//
+// Shape.cpp
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+
+
+#ifdef WIN32
+#if _MSC_VER >= 1310
+
+#include "shape.h"
+
+Shape::Shape()
+{
+}
+
+Shape::~Shape()
+{
+}
+
+#endif
+#endif //WIN32

Bullet/NarrowPhaseCollision/Shape.h

@@ -0,0 +1,67 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+//
+// Shape.h
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+//
+// Shape.h
+//
+#ifndef BULLET_SHAPE_H
+#define BULLET_SHAPE_H
+
+#include "Maths.h"
+
+
+
+
+struct Separation
+{
+
+	short m_featureA;
+	short m_featureB;
+	float m_dist;
+	Vector3 m_axis;		// in world space
+	
+	// separators
+	enum 
+	{
+		kFeatureNone,		// not separated
+		kFeatureA,
+		kFeatureB,
+		kFeatureBoth
+	};
+	short m_separator;
+
+	// contact between the 2 bodies (-1 if none)
+	short m_contact;
+};
+
+///Shape provides a interface for Hull class (convex hull calculation).
+class Shape
+{
+public:
+	Shape();
+	virtual ~Shape();
+
+	//virtual void ComputeInertia(Point3& centerOfMass, Matrix33& inertia, float totalMass) const = 0;
+	virtual void ComputeInertia(const Transform& transform, Point3& centerOfMass, Matrix33& inertia, float totalMass) const = 0;
+
+
+	virtual Bounds3 ComputeBounds(const Transform& transform) const = 0;
+};
+
+
+
+#endif //BULLET_SHAPE_H

Bullet/NarrowPhaseCollision/SimplexSolverInterface.h

@@ -0,0 +1,64 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef SIMPLEX_SOLVER_INTERFACE_H
+#define SIMPLEX_SOLVER_INTERFACE_H
+
+#include "SimdVector3.h"
+#include "SimdPoint3.h"
+
+#define NO_VIRTUAL_INTERFACE 1
+#ifdef NO_VIRTUAL_INTERFACE
+#include "VoronoiSimplexSolver.h"
+#define SimplexSolverInterface VoronoiSimplexSolver
+#else
+
+/// SimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices
+/// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on
+/// voronoi regions or barycentric coordinates
+class SimplexSolverInterface
+{
+	public:
+		virtual ~SimplexSolverInterface() {};
+
+	virtual void reset() = 0;
+
+	virtual void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q) = 0;
+	
+	virtual bool closest(SimdVector3& v) = 0;
+
+	virtual SimdScalar maxVertex() = 0;
+
+	virtual bool fullSimplex() const = 0;
+
+	virtual int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const = 0;
+
+	virtual bool inSimplex(const SimdVector3& w) = 0;
+	
+	virtual void backup_closest(SimdVector3& v) = 0;
+
+	virtual bool emptySimplex() const = 0;
+
+	virtual void compute_points(SimdPoint3& p1, SimdPoint3& p2) = 0;
+
+	virtual int numVertices() const =0;
+
+
+};
+#endif
+#endif //SIMPLEX_SOLVER_INTERFACE_H
+

Bullet/NarrowPhaseCollision/SubSimplexConvexCast.cpp

@@ -0,0 +1,132 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "SubSimplexConvexCast.h"
+#include "CollisionShapes/ConvexShape.h"
+#include "CollisionShapes/MinkowskiSumShape.h"
+#include "NarrowPhaseCollision/SimplexSolverInterface.h"
+
+
+SubsimplexConvexCast::SubsimplexConvexCast (ConvexShape* convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver)
+:m_simplexSolver(simplexSolver),
+m_convexA(convexA),m_convexB(convexB)
+{
+}
+
+
+#define MAX_ITERATIONS 1000
+
+bool	SubsimplexConvexCast::calcTimeOfImpact(
+		const SimdTransform& fromA,
+		const SimdTransform& toA,
+		const SimdTransform& fromB,
+		const SimdTransform& toB,
+		CastResult& result)
+{
+
+	MinkowskiSumShape combi(m_convexA,m_convexB);
+	MinkowskiSumShape* convex = &combi;
+
+	SimdTransform	rayFromLocalA;
+	SimdTransform	rayToLocalA;
+
+	rayFromLocalA = fromA.inverse()* fromB;
+	rayToLocalA = toA.inverse()* toB;
+
+
+	m_simplexSolver->reset();
+
+	convex->SetTransformB(SimdTransform(rayFromLocalA.getBasis()));
+
+	//float radius = 0.01f;
+
+	SimdScalar lambda = 0.f;
+	//todo: need to verify this:
+	//because of minkowski difference, we need the inverse direction
+	
+	SimdVector3 s = -rayFromLocalA.getOrigin();
+	SimdVector3 r = -(rayToLocalA.getOrigin()-rayFromLocalA.getOrigin());
+	SimdVector3 x = s;
+	SimdVector3 v;
+	SimdVector3 arbitraryPoint = convex->LocalGetSupportingVertex(r);
+	
+	v = x - arbitraryPoint;
+
+	int maxIter = MAX_ITERATIONS;
+
+	SimdVector3 n;
+	n.setValue(0.f,0.f,0.f);
+	bool hasResult = false;
+	SimdVector3 c;
+
+	float lastLambda = lambda;
+
+
+	float dist2 = v.length2();
+	float epsilon = 0.0001f;
+
+	SimdVector3	w,p;
+	float VdotR;
+	
+	while ( (dist2 > epsilon) && maxIter--)
+	{
+		p = convex->LocalGetSupportingVertex( v);
+		 w = x - p;
+
+		float VdotW = v.dot(w);
+
+		if ( VdotW > 0.f)
+		{
+			VdotR = v.dot(r);
+
+			if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
+				return false;
+			else
+			{
+				lambda = lambda - VdotW / VdotR;
+				x = s + lambda * r;
+				m_simplexSolver->reset();
+				//check next line
+				w = x-p;
+				lastLambda = lambda;
+				n = v;
+				hasResult = true;
+			}
+		} 
+		m_simplexSolver->addVertex( w, x , p);
+		if (m_simplexSolver->closest(v))
+		{
+			dist2 = v.length2();
+			hasResult = true;
+			//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
+			//printf("DIST2=%f\n",dist2);
+			//printf("numverts = %i\n",m_simplexSolver->numVertices());
+		} else
+		{
+			dist2 = 0.f;
+		} 
+	}
+
+	//int numiter = MAX_ITERATIONS - maxIter;
+//	printf("number of iterations: %d", numiter);
+	result.m_fraction = lambda;
+	result.m_normal = n;
+
+	return true;
+}
+
+
+

Bullet/NarrowPhaseCollision/SubSimplexConvexCast.h

@@ -0,0 +1,50 @@
+/*
+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 SUBSIMPLEX_CONVEX_CAST_H
+#define SUBSIMPLEX_CONVEX_CAST_H
+
+#include "ConvexCast.h"
+#include "SimplexSolverInterface.h"
+class ConvexShape;
+
+/// SubsimplexConvexCast implements Gino van den Bergens' paper
+///"Ray Casting against General Convex Objects with Application to Continuous Collision Detection"
+/// GJK based Ray Cast, optimized version
+/// Objects should not start in overlap, otherwise results are not defined.
+class SubsimplexConvexCast : public ConvexCast
+{
+	SimplexSolverInterface* m_simplexSolver;
+	ConvexShape*	m_convexA;
+	ConvexShape*	m_convexB;
+
+public:
+
+	SubsimplexConvexCast (ConvexShape*	shapeA,ConvexShape*	shapeB,SimplexSolverInterface* simplexSolver);
+
+	//virtual ~SubsimplexConvexCast();
+	///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
+	///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector.
+	virtual bool	calcTimeOfImpact(
+			const SimdTransform& fromA,
+			const SimdTransform& toA,
+			const SimdTransform& fromB,
+			const SimdTransform& toB,
+			CastResult& result);
+
+};
+
+#endif //SUBSIMPLEX_CONVEX_CAST_H

Bullet/NarrowPhaseCollision/VoronoiSimplexSolver.cpp

@@ -0,0 +1,598 @@
+
+/*
+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.
+	
+	Elsevier CDROM license agreements grants nonexclusive license to use the software
+	for any purpose, commercial or non-commercial as long as the following credit is included
+	identifying the original source of the software:
+
+	Parts of the source are "from the book Real-Time Collision Detection by
+	Christer Ericson, published by Morgan Kaufmann Publishers,
+	(c) 2005 Elsevier Inc."
+		
+*/
+
+
+#include "VoronoiSimplexSolver.h"
+#include <assert.h>
+#include <stdio.h>
+
+#define VERTA  0
+#define VERTB  1
+#define VERTC  2
+#define VERTD  3
+
+#define CATCH_DEGENERATE_TETRAHEDRON 1
+void	VoronoiSimplexSolver::removeVertex(int index)
+{
+	
+	assert(m_numVertices>0);
+	m_numVertices--;
+	m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
+	m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
+	m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
+}
+
+void	VoronoiSimplexSolver::ReduceVertices (const UsageBitfield& usedVerts)
+{
+	if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
+		removeVertex(3);
+
+	if ((numVertices() >= 3) && (!usedVerts.usedVertexC))
+		removeVertex(2);
+
+	if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
+		removeVertex(1);
+	
+	if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
+		removeVertex(0);
+
+}
+
+
+
+
+
+//clear the simplex, remove all the vertices
+void VoronoiSimplexSolver::reset()
+{
+	m_cachedValidClosest = false;
+	m_numVertices = 0;
+	m_needsUpdate = true;
+	m_lastW = SimdVector3(1e30f,1e30f,1e30f);
+	m_cachedBC.Reset();
+}
+
+
+
+	//add a vertex
+void VoronoiSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q)
+{
+	m_lastW = w;
+	m_needsUpdate = true;
+
+	m_simplexVectorW[m_numVertices] = w;
+	m_simplexPointsP[m_numVertices] = p;
+	m_simplexPointsQ[m_numVertices] = q;
+
+	m_numVertices++;
+}
+
+bool	VoronoiSimplexSolver::UpdateClosestVectorAndPoints()
+{
+	
+	if (m_needsUpdate)
+	{
+		m_cachedBC.Reset();
+
+		m_needsUpdate = false;
+
+		switch (numVertices())
+		{
+		case 0:
+				m_cachedValidClosest = false;
+				break;
+		case 1:
+			{
+				m_cachedP1 = m_simplexPointsP[0];
+				m_cachedP2 = m_simplexPointsQ[0];
+				m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
+				m_cachedBC.Reset();
+				m_cachedBC.SetBarycentricCoordinates(1.f,0.f,0.f,0.f);
+				m_cachedValidClosest = m_cachedBC.IsValid();
+				break;
+			};
+		case 2:
+			{
+			//closest point origin from line segment
+					const SimdVector3& from = m_simplexVectorW[0];
+					const SimdVector3& to = m_simplexVectorW[1];
+					SimdVector3 nearest;
+
+					SimdVector3 p (0.f,0.f,0.f);
+					SimdVector3 diff = p - from;
+					SimdVector3 v = to - from;
+					float t = v.dot(diff);
+					
+					if (t > 0) {
+						float dotVV = v.dot(v);
+						if (t < dotVV) {
+							t /= dotVV;
+							diff -= t*v;
+							m_cachedBC.m_usedVertices.usedVertexA = true;
+							m_cachedBC.m_usedVertices.usedVertexB = true;
+						} else {
+							t = 1;
+							diff -= v;
+							//reduce to 1 point
+							m_cachedBC.m_usedVertices.usedVertexB = true;
+						}
+					} else
+					{
+						t = 0;
+						//reduce to 1 point
+						m_cachedBC.m_usedVertices.usedVertexA = true;
+					}
+					m_cachedBC.SetBarycentricCoordinates(1-t,t);
+					nearest = from + t*v;
+
+					m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
+					m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
+					m_cachedV = m_cachedP1 - m_cachedP2;
+					
+					ReduceVertices(m_cachedBC.m_usedVertices);
+
+					m_cachedValidClosest = m_cachedBC.IsValid();
+					break;
+			}
+		case 3:
+			{
+				//closest point origin from triangle
+				SimdVector3 p (0.f,0.f,0.f);
+				
+				const SimdVector3& a = m_simplexVectorW[0];
+				const SimdVector3& b = m_simplexVectorW[1];
+				const SimdVector3& c = m_simplexVectorW[2];
+
+				ClosestPtPointTriangle(p,a,b,c,m_cachedBC);
+				m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
+								m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+								m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
+								m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
+
+				m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
+					m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+					m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
+					m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
+
+				m_cachedV = m_cachedP1-m_cachedP2;
+
+				ReduceVertices (m_cachedBC.m_usedVertices);
+				m_cachedValidClosest =  m_cachedBC.IsValid();
+
+				break;
+			}
+		case 4:
+			{
+
+				
+				SimdVector3 p (0.f,0.f,0.f);
+				
+				const SimdVector3& a = m_simplexVectorW[0];
+				const SimdVector3& b = m_simplexVectorW[1];
+				const SimdVector3& c = m_simplexVectorW[2];
+				const SimdVector3& d = m_simplexVectorW[3];
+
+				bool hasSeperation = ClosestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
+
+				if (hasSeperation)
+				{
+
+					m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
+						m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+						m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
+						m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
+
+					m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
+						m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+						m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
+						m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
+
+					m_cachedV = m_cachedP1-m_cachedP2;
+					ReduceVertices (m_cachedBC.m_usedVertices);
+				} else
+				{
+//					printf("sub distance got penetration\n");
+
+					if (m_cachedBC.m_degenerate)
+					{
+						m_cachedValidClosest = false;
+					} else
+					{
+						m_cachedValidClosest = true;
+						//degenerate case == false, penetration = true + zero
+						m_cachedV.setValue(0.f,0.f,0.f);
+					}
+					break;
+				}
+
+				m_cachedValidClosest = m_cachedBC.IsValid();
+
+				//closest point origin from tetrahedron
+				break;
+			}
+		default:
+			{
+				m_cachedValidClosest = false;
+			}
+		};
+	}
+
+	return m_cachedValidClosest;
+
+}
+
+//return/calculate the closest vertex
+bool VoronoiSimplexSolver::closest(SimdVector3& v)
+{
+	bool succes = UpdateClosestVectorAndPoints();
+	v = m_cachedV;
+	return succes;
+}
+
+
+
+SimdScalar VoronoiSimplexSolver::maxVertex()
+{
+	int i, numverts = numVertices();
+	SimdScalar maxV = 0.f;
+	for (i=0;i<numverts;i++)
+	{
+		SimdScalar curLen2 = m_simplexVectorW[i].length2();
+		if (maxV < curLen2)
+			maxV = curLen2;
+	}
+	return maxV;
+}
+
+
+
+	//return the current simplex
+int VoronoiSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const
+{
+	int i;
+	for (i=0;i<numVertices();i++)
+	{
+		yBuf[i] = m_simplexVectorW[i];
+		pBuf[i] = m_simplexPointsP[i];
+		qBuf[i] = m_simplexPointsQ[i];
+	}
+	return numVertices();
+}
+
+
+
+
+bool VoronoiSimplexSolver::inSimplex(const SimdVector3& w)
+{
+	bool found = false;
+	int i, numverts = numVertices();
+	//SimdScalar maxV = 0.f;
+	
+	//w is in the current (reduced) simplex
+	for (i=0;i<numverts;i++)
+	{
+		if (m_simplexVectorW[i] == w)
+			found = true;
+	}
+
+	//check in case lastW is already removed
+	if (w == m_lastW)
+		return true;
+    	
+	return found;
+}
+
+void VoronoiSimplexSolver::backup_closest(SimdVector3& v) 
+{
+	v = m_cachedV;
+}
+
+
+bool VoronoiSimplexSolver::emptySimplex() const 
+{
+	return (numVertices() == 0);
+
+}
+
+void VoronoiSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2) 
+{
+	UpdateClosestVectorAndPoints();
+	p1 = m_cachedP1;
+	p2 = m_cachedP2;
+
+}
+
+
+
+
+bool	VoronoiSimplexSolver::ClosestPtPointTriangle(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c,SubSimplexClosestResult& result)
+{
+	result.m_usedVertices.reset();
+
+    // Check if P in vertex region outside A
+    SimdVector3 ab = b - a;
+    SimdVector3 ac = c - a;
+    SimdVector3 ap = p - a;
+    float d1 = ab.dot(ap);
+    float d2 = ac.dot(ap);
+    if (d1 <= 0.0f && d2 <= 0.0f) 
+	{
+		result.m_closestPointOnSimplex = a;
+		result.m_usedVertices.usedVertexA = true;
+		result.SetBarycentricCoordinates(1,0,0);
+		return true;// a; // barycentric coordinates (1,0,0)
+	}
+
+    // Check if P in vertex region outside B
+    SimdVector3 bp = p - b;
+    float d3 = ab.dot(bp);
+    float d4 = ac.dot(bp);
+    if (d3 >= 0.0f && d4 <= d3) 
+	{
+		result.m_closestPointOnSimplex = b;
+		result.m_usedVertices.usedVertexB = true;
+		result.SetBarycentricCoordinates(0,1,0);
+
+		return true; // b; // barycentric coordinates (0,1,0)
+	}
+    // Check if P in edge region of AB, if so return projection of P onto AB
+    float vc = d1*d4 - d3*d2;
+    if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
+        float v = d1 / (d1 - d3);
+		result.m_closestPointOnSimplex = a + v * ab;
+		result.m_usedVertices.usedVertexA = true;
+		result.m_usedVertices.usedVertexB = true;
+		result.SetBarycentricCoordinates(1-v,v,0);
+		return true;
+        //return a + v * ab; // barycentric coordinates (1-v,v,0)
+    }
+
+    // Check if P in vertex region outside C
+    SimdVector3 cp = p - c;
+    float d5 = ab.dot(cp);
+    float d6 = ac.dot(cp);
+    if (d6 >= 0.0f && d5 <= d6) 
+	{
+		result.m_closestPointOnSimplex = c;
+		result.m_usedVertices.usedVertexC = true;
+		result.SetBarycentricCoordinates(0,0,1);
+		return true;//c; // barycentric coordinates (0,0,1)
+	}
+
+    // Check if P in edge region of AC, if so return projection of P onto AC
+    float vb = d5*d2 - d1*d6;
+    if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
+        float w = d2 / (d2 - d6);
+		result.m_closestPointOnSimplex = a + w * ac;
+		result.m_usedVertices.usedVertexA = true;
+		result.m_usedVertices.usedVertexC = true;
+		result.SetBarycentricCoordinates(1-w,0,w);
+		return true;
+        //return a + w * ac; // barycentric coordinates (1-w,0,w)
+    }
+
+    // Check if P in edge region of BC, if so return projection of P onto BC
+    float va = d3*d6 - d5*d4;
+    if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
+        float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+		
+		result.m_closestPointOnSimplex = b + w * (c - b);
+		result.m_usedVertices.usedVertexB = true;
+		result.m_usedVertices.usedVertexC = true;
+		result.SetBarycentricCoordinates(0,1-w,w);
+		return true;		
+       // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
+    }
+
+    // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
+    float denom = 1.0f / (va + vb + vc);
+    float v = vb * denom;
+    float w = vc * denom;
+    
+	result.m_closestPointOnSimplex = a + ab * v + ac * w;
+	result.m_usedVertices.usedVertexA = true;
+	result.m_usedVertices.usedVertexB = true;
+	result.m_usedVertices.usedVertexC = true;
+	result.SetBarycentricCoordinates(1-v-w,v,w);
+	
+	return true;
+//	return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0f - v - w
+
+}
+
+
+
+
+
+/// Test if point p and d lie on opposite sides of plane through abc
+int VoronoiSimplexSolver::PointOutsideOfPlane(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d)
+{
+	SimdVector3 normal = (b-a).cross(c-a);
+
+    float signp = (p - a).dot(normal); // [AP AB AC]
+    float signd = (d - a).dot( normal); // [AD AB AC]
+
+#ifdef CATCH_DEGENERATE_TETRAHEDRON
+	if (signd * signd < (1e-4f * 1e-4f))
+	{
+//		printf("affine dependent/degenerate\n");//
+		return -1;
+	}
+#endif
+	// Points on opposite sides if expression signs are opposite
+    return signp * signd < 0.f;
+}
+
+
+bool	VoronoiSimplexSolver::ClosestPtPointTetrahedron(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d, SubSimplexClosestResult& finalResult)
+{
+	SubSimplexClosestResult tempResult;
+
+    // Start out assuming point inside all halfspaces, so closest to itself
+	finalResult.m_closestPointOnSimplex = p;
+	finalResult.m_usedVertices.reset();
+    finalResult.m_usedVertices.usedVertexA = true;
+	finalResult.m_usedVertices.usedVertexB = true;
+	finalResult.m_usedVertices.usedVertexC = true;
+	finalResult.m_usedVertices.usedVertexD = true;
+
+    int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d);
+	int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b);
+  	int	pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c);
+	int	pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a);
+
+   if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
+   {
+	   finalResult.m_degenerate = true;
+	   return false;
+   }
+
+   if (!pointOutsideABC  && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
+	 {
+		 return false;
+	 }
+
+
+    float bestSqDist = FLT_MAX;
+    // If point outside face abc then compute closest point on abc
+	if (pointOutsideABC) 
+	{
+        ClosestPtPointTriangle(p, a, b, c,tempResult);
+		SimdPoint3 q = tempResult.m_closestPointOnSimplex;
+		
+        float sqDist = (q - p).dot( q - p);
+        // Update best closest point if (squared) distance is less than current best
+        if (sqDist < bestSqDist) {
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			//convert result bitmask!
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
+			finalResult.SetBarycentricCoordinates(
+					tempResult.m_barycentricCoords[VERTA],
+					tempResult.m_barycentricCoords[VERTB],
+					tempResult.m_barycentricCoords[VERTC],
+					0
+			);
+
+		}
+    }
+  
+
+	// Repeat test for face acd
+	if (pointOutsideACD) 
+	{
+        ClosestPtPointTriangle(p, a, c, d,tempResult);
+		SimdPoint3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+
+        float sqDist = (q - p).dot( q - p);
+        if (sqDist < bestSqDist) 
+		{
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
+			finalResult.SetBarycentricCoordinates(
+					tempResult.m_barycentricCoords[VERTA],
+					0,
+					tempResult.m_barycentricCoords[VERTB],
+					tempResult.m_barycentricCoords[VERTC]
+			);
+
+		}
+    }
+    // Repeat test for face adb
+
+	
+	if (pointOutsideADB)
+	{
+		ClosestPtPointTriangle(p, a, d, b,tempResult);
+		SimdPoint3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+
+        float sqDist = (q - p).dot( q - p);
+        if (sqDist < bestSqDist) 
+		{
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
+			finalResult.SetBarycentricCoordinates(
+					tempResult.m_barycentricCoords[VERTA],
+					tempResult.m_barycentricCoords[VERTC],
+					0,
+					tempResult.m_barycentricCoords[VERTB]
+			);
+
+		}
+    }
+    // Repeat test for face bdc
+    
+
+	if (pointOutsideBDC)
+	{
+        ClosestPtPointTriangle(p, b, d, c,tempResult);
+		SimdPoint3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+        float sqDist = (q - p).dot( q - p);
+        if (sqDist < bestSqDist) 
+		{
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
+
+			finalResult.SetBarycentricCoordinates(
+					0,
+					tempResult.m_barycentricCoords[VERTA],
+					tempResult.m_barycentricCoords[VERTC],
+					tempResult.m_barycentricCoords[VERTB]
+			);
+
+		}
+    }
+
+	//help! we ended up full !
+	
+	if (finalResult.m_usedVertices.usedVertexA &&
+		finalResult.m_usedVertices.usedVertexB &&
+		finalResult.m_usedVertices.usedVertexC &&
+		finalResult.m_usedVertices.usedVertexD) 
+	{
+		return true;
+	}
+
+    return true;
+}
+

Bullet/NarrowPhaseCollision/VoronoiSimplexSolver.h

@@ -0,0 +1,157 @@
+/*
+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 VoronoiSimplexSolver_H
+#define VoronoiSimplexSolver_H
+
+#include "SimplexSolverInterface.h"
+
+
+
+#define VORONOI_SIMPLEX_MAX_VERTS 5
+
+struct UsageBitfield{
+	UsageBitfield()
+	{
+		reset();
+	}
+
+	void reset()
+	{
+		usedVertexA = false;
+		usedVertexB = false;
+		usedVertexC = false;
+		usedVertexD = false;
+	}
+	unsigned short usedVertexA	: 1;
+	unsigned short usedVertexB	: 1;
+	unsigned short usedVertexC	: 1;
+	unsigned short usedVertexD	: 1;
+	unsigned short unused1		: 1;
+	unsigned short unused2		: 1;
+	unsigned short unused3		: 1;
+	unsigned short unused4		: 1;
+};
+
+
+struct	SubSimplexClosestResult
+{
+	SimdPoint3	m_closestPointOnSimplex;
+	//MASK for m_usedVertices
+	//stores the simplex vertex-usage, using the MASK, 
+	// if m_usedVertices & MASK then the related vertex is used
+	UsageBitfield	m_usedVertices;
+	float	m_barycentricCoords[4];
+	bool m_degenerate;
+
+	void	Reset()
+	{
+		m_degenerate = false;
+		SetBarycentricCoordinates();
+		m_usedVertices.reset();
+	}
+	bool	IsValid()
+	{
+		bool valid = (m_barycentricCoords[0] >= 0.f) &&
+			(m_barycentricCoords[1] >= 0.f) &&
+			(m_barycentricCoords[2] >= 0.f) &&
+			(m_barycentricCoords[3] >= 0.f);
+
+
+		return valid;
+	}
+	void	SetBarycentricCoordinates(float a=0.f,float b=0.f,float c=0.f,float d=0.f)
+	{
+		m_barycentricCoords[0] = a;
+		m_barycentricCoords[1] = b;
+		m_barycentricCoords[2] = c;
+		m_barycentricCoords[3] = d;
+	}
+
+};
+
+/// VoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin.
+/// Can be used with GJK, as an alternative to Johnson distance algorithm.
+#ifdef NO_VIRTUAL_INTERFACE
+class VoronoiSimplexSolver
+#else
+class VoronoiSimplexSolver : public SimplexSolverInterface
+#endif
+{
+public:
+
+	int	m_numVertices;
+
+	SimdVector3	m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
+	SimdPoint3	m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
+	SimdPoint3	m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
+
+	
+
+	SimdPoint3	m_cachedP1;
+	SimdPoint3	m_cachedP2;
+	SimdVector3	m_cachedV;
+	SimdVector3	m_lastW;
+	bool		m_cachedValidClosest;
+
+	SubSimplexClosestResult m_cachedBC;
+
+	bool	m_needsUpdate;
+	
+	void	removeVertex(int index);
+	void	ReduceVertices (const UsageBitfield& usedVerts);
+	bool	UpdateClosestVectorAndPoints();
+
+	bool	ClosestPtPointTetrahedron(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d, SubSimplexClosestResult& finalResult);
+	int		PointOutsideOfPlane(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d);
+	bool	ClosestPtPointTriangle(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c,SubSimplexClosestResult& result);
+
+public:
+
+	 void reset();
+
+	 void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q);
+
+
+	 bool closest(SimdVector3& v);
+
+	 SimdScalar maxVertex();
+
+	 bool fullSimplex() const
+	 {
+		 return (m_numVertices == 4);
+	 }
+
+	 int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const;
+
+	 bool inSimplex(const SimdVector3& w);
+	
+	 void backup_closest(SimdVector3& v) ;
+
+	 bool emptySimplex() const ;
+
+	 void compute_points(SimdPoint3& p1, SimdPoint3& p2) ;
+
+	 int numVertices() const 
+	 {
+		 return m_numVertices;
+	 }
+
+
+};
+
+#endif //VoronoiSimplexSolver