Refactoring:

Moved optional code to Extras: AlgebraicCCD,EPA,quickstep
Moved SimpleBroadphase data to OverlappingPairCache, and derive both SimpleBroadphase and AxisSweep3 from OverlappingPairCache.
Added ParallelPhysicsEnvironment (prepair more parallel mainloop)
Upgraded hardcoded limit from 1024/8192 to 32766/65535 (max objects / max overlapping pairs)

Extras/EPA/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 );
+}

Extras/EPA/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
+

Extras/EPA/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
+

Extras/EPA/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
+//}
+

Extras/EPA/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
+

Extras/EPA/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
+

Extras/EPA/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 );
+}
+

Extras/EPA/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
+

Extras/EPA/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
+

Extras/EPA/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
+