add bullet

libs/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp

@@ -0,0 +1,209 @@
+/*
+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 "LinearMath/btScalar.h"
+#include "SphereTriangleDetector.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+
+SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold)
+:m_sphere(sphere),
+m_triangle(triangle),
+m_contactBreakingThreshold(contactBreakingThreshold)
+{
+
+}
+
+void	SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
+{
+
+	(void)debugDraw;
+	const btTransform& transformA = input.m_transformA;
+	const btTransform& transformB = input.m_transformB;
+
+	btVector3 point,normal;
+	btScalar timeOfImpact = btScalar(1.);
+	btScalar depth = btScalar(0.);
+//	output.m_distance = btScalar(BT_LARGE_FLOAT);
+	//move sphere into triangle space
+	btTransform	sphereInTr = transformB.inverseTimes(transformA);
+
+	if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold))
+	{
+		if (swapResults)
+		{
+			btVector3 normalOnB = transformB.getBasis()*normal;
+			btVector3 normalOnA = -normalOnB;
+			btVector3 pointOnA = transformB*point+normalOnB*depth;
+			output.addContactPoint(normalOnA,pointOnA,depth);
+		} else
+		{
+			output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
+		}
+	}
+
+}
+
+#define MAX_OVERLAP btScalar(0.)
+
+
+
+// See also geometrictools.com
+// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
+btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest);
+
+btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
+	btVector3 diff = p - from;
+	btVector3 v = to - from;
+	btScalar t = v.dot(diff);
+	
+	if (t > 0) {
+		btScalar dotVV = v.dot(v);
+		if (t < dotVV) {
+			t /= dotVV;
+			diff -= t*v;
+		} else {
+			t = 1;
+			diff -= v;
+		}
+	} else
+		t = 0;
+
+	nearest = from + t*v;
+	return diff.dot(diff);	
+}
+
+bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal)  {
+	btVector3 lp(p);
+	btVector3 lnormal(normal);
+	
+	return pointInTriangle(vertices, lnormal, &lp);
+}
+
+///combined discrete/continuous sphere-triangle
+bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
+{
+
+	const btVector3* vertices = &m_triangle->getVertexPtr(0);
+	const btVector3& c = sphereCenter;
+	btScalar r = m_sphere->getRadius();
+
+	btVector3 delta (0,0,0);
+
+	btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
+	normal.normalize();
+	btVector3 p1ToCentre = c - vertices[0];
+	btScalar distanceFromPlane = p1ToCentre.dot(normal);
+
+	if (distanceFromPlane < btScalar(0.))
+	{
+		//triangle facing the other way
+	
+		distanceFromPlane *= btScalar(-1.);
+		normal *= btScalar(-1.);
+	}
+
+	btScalar contactMargin = contactBreakingThreshold;
+	bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
+	bool isInsideShellPlane = distanceFromPlane < r;
+	
+	btScalar deltaDotNormal = delta.dot(normal);
+	if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0))
+		return false;
+
+	// Check for contact / intersection
+	bool hasContact = false;
+	btVector3 contactPoint;
+	if (isInsideContactPlane) {
+		if (facecontains(c,vertices,normal)) {
+			// Inside the contact wedge - touches a point on the shell plane
+			hasContact = true;
+			contactPoint = c - normal*distanceFromPlane;
+		} else {
+			// Could be inside one of the contact capsules
+			btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
+			btVector3 nearestOnEdge;
+			for (int i = 0; i < m_triangle->getNumEdges(); i++) {
+				
+				btVector3 pa;
+				btVector3 pb;
+				
+				m_triangle->getEdge(i,pa,pb);
+
+				btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge);
+				if (distanceSqr < contactCapsuleRadiusSqr) {
+					// Yep, we're inside a capsule
+					hasContact = true;
+					contactPoint = nearestOnEdge;
+				}
+				
+			}
+		}
+	}
+
+	if (hasContact) {
+		btVector3 contactToCentre = c - contactPoint;
+		btScalar distanceSqr = contactToCentre.length2();
+		if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) {
+			btScalar distance = btSqrt(distanceSqr);
+			resultNormal = contactToCentre;
+			resultNormal.normalize();
+			point = contactPoint;
+			depth = -(r-distance);
+			return true;
+		}
+
+		if (delta.dot(contactToCentre) >= btScalar(0.0)) 
+			return false;
+		
+		// Moving towards the contact point -> collision
+		point = contactPoint;
+		timeOfImpact = btScalar(0.0);
+		return true;
+	}
+	
+	return false;
+}
+
+
+bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
+{
+	const btVector3* p1 = &vertices[0];
+	const btVector3* p2 = &vertices[1];
+	const btVector3* p3 = &vertices[2];
+
+	btVector3 edge1( *p2 - *p1 );
+	btVector3 edge2( *p3 - *p2 );
+	btVector3 edge3( *p1 - *p3 );
+
+	btVector3 p1_to_p( *p - *p1 );
+	btVector3 p2_to_p( *p - *p2 );
+	btVector3 p3_to_p( *p - *p3 );
+
+	btVector3 edge1_normal( edge1.cross(normal));
+	btVector3 edge2_normal( edge2.cross(normal));
+	btVector3 edge3_normal( edge3.cross(normal));
+	
+	btScalar r1, r2, r3;
+	r1 = edge1_normal.dot( p1_to_p );
+	r2 = edge2_normal.dot( p2_to_p );
+	r3 = edge3_normal.dot( p3_to_p );
+	if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
+	     ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
+		return true;
+	return false;
+
+}

libs/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.h

@@ -0,0 +1,51 @@
+/*
+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 SPHERE_TRIANGLE_DETECTOR_H
+#define SPHERE_TRIANGLE_DETECTOR_H
+
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+
+
+
+class btSphereShape;
+class btTriangleShape;
+
+
+
+/// sphere-triangle to match the btDiscreteCollisionDetectorInterface
+struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface
+{
+	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
+
+	SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle, btScalar contactBreakingThreshold);
+
+	virtual ~SphereTriangleDetector() {};
+
+	bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar	contactBreakingThreshold);
+
+private:
+
+	
+	bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p );
+	bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal);
+
+	btSphereShape* m_sphere;
+	btTriangleShape* m_triangle;
+	btScalar	m_contactBreakingThreshold;
+	
+};
+#endif //SPHERE_TRIANGLE_DETECTOR_H
+

libs/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp

@@ -0,0 +1,47 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans  http://bulletphysics.com
+
+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 "btActivatingCollisionAlgorithm.h"
+#include "btCollisionDispatcher.h"
+#include "btCollisionObject.h"
+
+btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci)
+:btCollisionAlgorithm(ci)
+//,
+//m_colObj0(0),
+//m_colObj1(0)
+{
+}
+btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1)
+:btCollisionAlgorithm(ci)
+//,
+//m_colObj0(0),
+//m_colObj1(0)
+{
+//	if (ci.m_dispatcher1->needsCollision(colObj0,colObj1))
+//	{
+//		m_colObj0 = colObj0;
+//		m_colObj1 = colObj1;
+//		
+//		m_colObj0->activate();
+//		m_colObj1->activate();
+//	}
+}
+
+btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm()
+{
+//		m_colObj0->activate();
+//		m_colObj1->activate();
+}

libs/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h

@@ -0,0 +1,36 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __BT_ACTIVATING_COLLISION_ALGORITHM_H
+#define __BT_ACTIVATING_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
+///This class is not enabled yet (work-in-progress) to more aggressively activate objects.
+class btActivatingCollisionAlgorithm : public btCollisionAlgorithm
+{
+//	btCollisionObject* m_colObj0;
+//	btCollisionObject* m_colObj1;
+
+public:
+
+	btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci);
+
+	btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1);
+
+	virtual ~btActivatingCollisionAlgorithm();
+
+};
+#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H

libs/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp

@@ -0,0 +1,435 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+* The b2CollidePolygons routines are Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+
+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.
+*/
+
+///btBox2dBox2dCollisionAlgorithm, with modified b2CollidePolygons routines from the Box2D library.
+///The modifications include: switching from b2Vec to btVector3, redefinition of b2Dot, b2Cross
+
+#include "btBox2dBox2dCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h"
+#include "BulletCollision/CollisionShapes/btBox2dShape.h"
+
+#define USE_PERSISTENT_CONTACTS 1
+
+btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1)
+: btActivatingCollisionAlgorithm(ci,obj0,obj1),
+m_ownManifold(false),
+m_manifoldPtr(mf)
+{
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1))
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1);
+		m_ownManifold = true;
+	}
+}
+
+btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm()
+{
+	
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+	
+}
+
+
+void b2CollidePolygons(btManifoldResult* manifold,  const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB);
+
+//#include <stdio.h>
+void btBox2dBox2dCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	if (!m_manifoldPtr)
+		return;
+
+	btCollisionObject*	col0 = body0;
+	btCollisionObject*	col1 = body1;
+	btBox2dShape* box0 = (btBox2dShape*)col0->getCollisionShape();
+	btBox2dShape* box1 = (btBox2dShape*)col1->getCollisionShape();
+
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
+	b2CollidePolygons(resultOut,box0,col0->getWorldTransform(),box1,col1->getWorldTransform());
+
+	//  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
+	if (m_ownManifold)
+	{
+		resultOut->refreshContactPoints();
+	}
+
+}
+
+btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
+{
+	//not yet
+	return 1.f;
+}
+
+
+struct ClipVertex
+{
+	btVector3 v;
+	int id;
+	//b2ContactID id;
+	//b2ContactID id;
+};
+
+#define b2Dot(a,b) (a).dot(b)
+#define b2Mul(a,b) (a)*(b)
+#define b2MulT(a,b) (a).transpose()*(b)
+#define b2Cross(a,b) (a).cross(b)
+#define btCrossS(a,s) btVector3(s * a.getY(), -s * a.getX(),0.f)
+
+int b2_maxManifoldPoints =2;
+
+static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
+					  const btVector3& normal, btScalar offset)
+{
+	// Start with no output points
+	int numOut = 0;
+
+	// Calculate the distance of end points to the line
+	btScalar distance0 = b2Dot(normal, vIn[0].v) - offset;
+	btScalar distance1 = b2Dot(normal, vIn[1].v) - offset;
+
+	// If the points are behind the plane
+	if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
+	if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
+
+	// If the points are on different sides of the plane
+	if (distance0 * distance1 < 0.0f)
+	{
+		// Find intersection point of edge and plane
+		btScalar interp = distance0 / (distance0 - distance1);
+		vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
+		if (distance0 > 0.0f)
+		{
+			vOut[numOut].id = vIn[0].id;
+		}
+		else
+		{
+			vOut[numOut].id = vIn[1].id;
+		}
+		++numOut;
+	}
+
+	return numOut;
+}
+
+// Find the separation between poly1 and poly2 for a give edge normal on poly1.
+static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1,
+							  const btBox2dShape* poly2, const btTransform& xf2)
+{
+	const btVector3* vertices1 = poly1->getVertices();
+	const btVector3* normals1 = poly1->getNormals();
+
+	int count2 = poly2->getVertexCount();
+	const btVector3* vertices2 = poly2->getVertices();
+
+	btAssert(0 <= edge1 && edge1 < poly1->getVertexCount());
+
+	// Convert normal from poly1's frame into poly2's frame.
+	btVector3 normal1World = b2Mul(xf1.getBasis(), normals1[edge1]);
+	btVector3 normal1 = b2MulT(xf2.getBasis(), normal1World);
+
+	// Find support vertex on poly2 for -normal.
+	int index = 0;
+	btScalar minDot = BT_LARGE_FLOAT;
+
+	for (int i = 0; i < count2; ++i)
+	{
+		btScalar dot = b2Dot(vertices2[i], normal1);
+		if (dot < minDot)
+		{
+			minDot = dot;
+			index = i;
+		}
+	}
+
+	btVector3 v1 = b2Mul(xf1, vertices1[edge1]);
+	btVector3 v2 = b2Mul(xf2, vertices2[index]);
+	btScalar separation = b2Dot(v2 - v1, normal1World);
+	return separation;
+}
+
+// Find the max separation between poly1 and poly2 using edge normals from poly1.
+static btScalar FindMaxSeparation(int* edgeIndex,
+								 const btBox2dShape* poly1, const btTransform& xf1,
+								 const btBox2dShape* poly2, const btTransform& xf2)
+{
+	int count1 = poly1->getVertexCount();
+	const btVector3* normals1 = poly1->getNormals();
+
+	// Vector pointing from the centroid of poly1 to the centroid of poly2.
+	btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid());
+	btVector3 dLocal1 = b2MulT(xf1.getBasis(), d);
+
+	// Find edge normal on poly1 that has the largest projection onto d.
+	int edge = 0;
+	btScalar maxDot = -BT_LARGE_FLOAT;
+	for (int i = 0; i < count1; ++i)
+	{
+		btScalar dot = b2Dot(normals1[i], dLocal1);
+		if (dot > maxDot)
+		{
+			maxDot = dot;
+			edge = i;
+		}
+	}
+
+	// Get the separation for the edge normal.
+	btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+	if (s > 0.0f)
+	{
+		return s;
+	}
+
+	// Check the separation for the previous edge normal.
+	int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
+	btScalar sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
+	if (sPrev > 0.0f)
+	{
+		return sPrev;
+	}
+
+	// Check the separation for the next edge normal.
+	int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
+	btScalar sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
+	if (sNext > 0.0f)
+	{
+		return sNext;
+	}
+
+	// Find the best edge and the search direction.
+	int bestEdge;
+	btScalar bestSeparation;
+	int increment;
+	if (sPrev > s && sPrev > sNext)
+	{
+		increment = -1;
+		bestEdge = prevEdge;
+		bestSeparation = sPrev;
+	}
+	else if (sNext > s)
+	{
+		increment = 1;
+		bestEdge = nextEdge;
+		bestSeparation = sNext;
+	}
+	else
+	{
+		*edgeIndex = edge;
+		return s;
+	}
+
+	// Perform a local search for the best edge normal.
+	for ( ; ; )
+	{
+		if (increment == -1)
+			edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
+		else
+			edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
+
+		s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+		if (s > 0.0f)
+		{
+			return s;
+		}
+
+		if (s > bestSeparation)
+		{
+			bestEdge = edge;
+			bestSeparation = s;
+		}
+		else
+		{
+			break;
+		}
+	}
+
+	*edgeIndex = bestEdge;
+	return bestSeparation;
+}
+
+static void FindIncidentEdge(ClipVertex c[2],
+							 const btBox2dShape* poly1, const btTransform& xf1, int edge1,
+							 const btBox2dShape* poly2, const btTransform& xf2)
+{
+	const btVector3* normals1 = poly1->getNormals();
+
+	int count2 = poly2->getVertexCount();
+	const btVector3* vertices2 = poly2->getVertices();
+	const btVector3* normals2 = poly2->getNormals();
+
+	btAssert(0 <= edge1 && edge1 < poly1->getVertexCount());
+
+	// Get the normal of the reference edge in poly2's frame.
+	btVector3 normal1 = b2MulT(xf2.getBasis(), b2Mul(xf1.getBasis(), normals1[edge1]));
+
+	// Find the incident edge on poly2.
+	int index = 0;
+	btScalar minDot = BT_LARGE_FLOAT;
+	for (int i = 0; i < count2; ++i)
+	{
+		btScalar dot = b2Dot(normal1, normals2[i]);
+		if (dot < minDot)
+		{
+			minDot = dot;
+			index = i;
+		}
+	}
+
+	// Build the clip vertices for the incident edge.
+	int i1 = index;
+	int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
+
+	c[0].v = b2Mul(xf2, vertices2[i1]);
+//	c[0].id.features.referenceEdge = (unsigned char)edge1;
+//	c[0].id.features.incidentEdge = (unsigned char)i1;
+//	c[0].id.features.incidentVertex = 0;
+
+	c[1].v = b2Mul(xf2, vertices2[i2]);
+//	c[1].id.features.referenceEdge = (unsigned char)edge1;
+//	c[1].id.features.incidentEdge = (unsigned char)i2;
+//	c[1].id.features.incidentVertex = 1;
+}
+
+// Find edge normal of max separation on A - return if separating axis is found
+// Find edge normal of max separation on B - return if separation axis is found
+// Choose reference edge as min(minA, minB)
+// Find incident edge
+// Clip
+
+// The normal points from 1 to 2
+void b2CollidePolygons(btManifoldResult* manifold,
+					  const btBox2dShape* polyA, const btTransform& xfA,
+					  const btBox2dShape* polyB, const btTransform& xfB)
+{
+
+	int edgeA = 0;
+	btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
+	if (separationA > 0.0f)
+		return;
+
+	int edgeB = 0;
+	btScalar separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
+	if (separationB > 0.0f)
+		return;
+
+	const btBox2dShape* poly1;	// reference poly
+	const btBox2dShape* poly2;	// incident poly
+	btTransform xf1, xf2;
+	int edge1;		// reference edge
+	unsigned char flip;
+	const btScalar k_relativeTol = 0.98f;
+	const btScalar k_absoluteTol = 0.001f;
+
+	// TODO_ERIN use "radius" of poly for absolute tolerance.
+	if (separationB > k_relativeTol * separationA + k_absoluteTol)
+	{
+		poly1 = polyB;
+		poly2 = polyA;
+		xf1 = xfB;
+		xf2 = xfA;
+		edge1 = edgeB;
+		flip = 1;
+	}
+	else
+	{
+		poly1 = polyA;
+		poly2 = polyB;
+		xf1 = xfA;
+		xf2 = xfB;
+		edge1 = edgeA;
+		flip = 0;
+	}
+
+	ClipVertex incidentEdge[2];
+	FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
+
+	int count1 = poly1->getVertexCount();
+	const btVector3* vertices1 = poly1->getVertices();
+
+	btVector3 v11 = vertices1[edge1];
+	btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
+
+	btVector3 dv = v12 - v11;
+	btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11);
+	sideNormal.normalize();
+	btVector3 frontNormal = btCrossS(sideNormal, 1.0f);
+	
+	
+	v11 = b2Mul(xf1, v11);
+	v12 = b2Mul(xf1, v12);
+
+	btScalar frontOffset = b2Dot(frontNormal, v11);
+	btScalar sideOffset1 = -b2Dot(sideNormal, v11);
+	btScalar sideOffset2 = b2Dot(sideNormal, v12);
+
+	// Clip incident edge against extruded edge1 side edges.
+	ClipVertex clipPoints1[2];
+	clipPoints1[0].v.setValue(0,0,0);
+	clipPoints1[1].v.setValue(0,0,0);
+
+	ClipVertex clipPoints2[2];
+	clipPoints2[0].v.setValue(0,0,0);
+	clipPoints2[1].v.setValue(0,0,0);
+
+
+	int np;
+
+	// Clip to box side 1
+	np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
+
+	if (np < 2)
+		return;
+
+	// Clip to negative box side 1
+	np = ClipSegmentToLine(clipPoints2, clipPoints1,  sideNormal, sideOffset2);
+
+	if (np < 2)
+	{
+		return;
+	}
+
+	// Now clipPoints2 contains the clipped points.
+	btVector3 manifoldNormal = flip ? -frontNormal : frontNormal;
+
+	int pointCount = 0;
+	for (int i = 0; i < b2_maxManifoldPoints; ++i)
+	{
+		btScalar separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
+
+		if (separation <= 0.0f)
+		{
+			
+			//b2ManifoldPoint* cp = manifold->points + pointCount;
+			//btScalar separation = separation;
+			//cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
+			//cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
+
+			manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation);
+
+//			cp->id = clipPoints2[i].id;
+//			cp->id.features.flip = flip;
+			++pointCount;
+		}
+	}
+
+//	manifold->pointCount = pointCount;}
+}

libs/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h

@@ -0,0 +1,66 @@
+/*
+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 BOX_2D_BOX_2D__COLLISION_ALGORITHM_H
+#define BOX_2D_BOX_2D__COLLISION_ALGORITHM_H
+
+#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+
+class btPersistentManifold;
+
+///box-box collision detection
+class btBox2dBox2dCollisionAlgorithm : public btActivatingCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	
+public:
+	btBox2dBox2dCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+		: btActivatingCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+	virtual ~btBox2dBox2dCollisionAlgorithm();
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
+
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			int bbsize = sizeof(btBox2dBox2dCollisionAlgorithm);
+			void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
+			return new(ptr) btBox2dBox2dCollisionAlgorithm(0,ci,body0,body1);
+		}
+	};
+
+};
+
+#endif //BOX_2D_BOX_2D__COLLISION_ALGORITHM_H
+

libs/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp

@@ -0,0 +1,85 @@
+/*
+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 "btBoxBoxCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "btBoxBoxDetector.h"
+
+#define USE_PERSISTENT_CONTACTS 1
+
+btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1)
+: btActivatingCollisionAlgorithm(ci,obj0,obj1),
+m_ownManifold(false),
+m_manifoldPtr(mf)
+{
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1))
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1);
+		m_ownManifold = true;
+	}
+}
+
+btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void btBoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	if (!m_manifoldPtr)
+		return;
+
+	btCollisionObject*	col0 = body0;
+	btCollisionObject*	col1 = body1;
+	btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape();
+	btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape();
+
+
+
+	/// report a contact. internally this will be kept persistent, and contact reduction is done
+	resultOut->setPersistentManifold(m_manifoldPtr);
+#ifndef USE_PERSISTENT_CONTACTS	
+	m_manifoldPtr->clearManifold();
+#endif //USE_PERSISTENT_CONTACTS
+
+	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
+	input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
+	input.m_transformA = body0->getWorldTransform();
+	input.m_transformB = body1->getWorldTransform();
+
+	btBoxBoxDetector detector(box0,box1);
+	detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+
+#ifdef USE_PERSISTENT_CONTACTS
+	//  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
+	if (m_ownManifold)
+	{
+		resultOut->refreshContactPoints();
+	}
+#endif //USE_PERSISTENT_CONTACTS
+
+}
+
+btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
+{
+	//not yet
+	return 1.f;
+}

libs/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h

@@ -0,0 +1,66 @@
+/*
+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 BOX_BOX__COLLISION_ALGORITHM_H
+#define BOX_BOX__COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+
+class btPersistentManifold;
+
+///box-box collision detection
+class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	
+public:
+	btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+		: btActivatingCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+	virtual ~btBoxBoxCollisionAlgorithm();
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
+
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			int bbsize = sizeof(btBoxBoxCollisionAlgorithm);
+			void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
+			return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0,body1);
+		}
+	};
+
+};
+
+#endif //BOX_BOX__COLLISION_ALGORITHM_H
+

libs/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp

@@ -0,0 +1,718 @@
+/*
+ * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
+ * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org
+ Bullet Continuous Collision Detection and Physics Library
+ Bullet is 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.
+*/
+
+///ODE box-box collision detection is adapted to work with Bullet
+
+#include "btBoxBoxDetector.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+
+#include <float.h>
+#include <string.h>
+
+btBoxBoxDetector::btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2)
+: m_box1(box1),
+m_box2(box2)
+{
+
+}
+
+
+// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
+// generate contact points. this returns 0 if there is no contact otherwise
+// it returns the number of contacts generated.
+// `normal' returns the contact normal.
+// `depth' returns the maximum penetration depth along that normal.
+// `return_code' returns a number indicating the type of contact that was
+// detected:
+//        1,2,3 = box 2 intersects with a face of box 1
+//        4,5,6 = box 1 intersects with a face of box 2
+//        7..15 = edge-edge contact
+// `maxc' is the maximum number of contacts allowed to be generated, i.e.
+// the size of the `contact' array.
+// `contact' and `skip' are the contact array information provided to the
+// collision functions. this function only fills in the position and depth
+// fields.
+struct dContactGeom;
+#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
+#define dInfinity FLT_MAX
+
+
+/*PURE_INLINE btScalar dDOT   (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
+PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); }
+PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); }
+PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); }
+*/
+static btScalar dDOT   (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
+static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); }
+static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); }
+static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); }
+#define dMULTIPLYOP1_331(A,op,B,C) \
+{\
+  (A)[0] op dDOT41((B),(C)); \
+  (A)[1] op dDOT41((B+1),(C)); \
+  (A)[2] op dDOT41((B+2),(C)); \
+}
+
+#define dMULTIPLYOP0_331(A,op,B,C) \
+{ \
+  (A)[0] op dDOT((B),(C)); \
+  (A)[1] op dDOT((B+4),(C)); \
+  (A)[2] op dDOT((B+8),(C)); \
+} 
+
+#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C)
+#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)
+
+typedef btScalar dMatrix3[4*3];
+
+void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
+			   const btVector3& pb, const btVector3& ub,
+			   btScalar *alpha, btScalar *beta);
+void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
+			   const btVector3& pb, const btVector3& ub,
+			   btScalar *alpha, btScalar *beta)
+{
+  btVector3 p;
+  p[0] = pb[0] - pa[0];
+  p[1] = pb[1] - pa[1];
+  p[2] = pb[2] - pa[2];
+  btScalar uaub = dDOT(ua,ub);
+  btScalar q1 =  dDOT(ua,p);
+  btScalar q2 = -dDOT(ub,p);
+  btScalar d = 1-uaub*uaub;
+  if (d <= btScalar(0.0001f)) {
+    // @@@ this needs to be made more robust
+    *alpha = 0;
+    *beta  = 0;
+  }
+  else {
+    d = 1.f/d;
+    *alpha = (q1 + uaub*q2)*d;
+    *beta  = (uaub*q1 + q2)*d;
+  }
+}
+
+
+
+// find all the intersection points between the 2D rectangle with vertices
+// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
+// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
+//
+// the intersection points are returned as x,y pairs in the 'ret' array.
+// the number of intersection points is returned by the function (this will
+// be in the range 0 to 8).
+
+static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16])
+{
+  // q (and r) contain nq (and nr) coordinate points for the current (and
+  // chopped) polygons
+  int nq=4,nr=0;
+  btScalar buffer[16];
+  btScalar *q = p;
+  btScalar *r = ret;
+  for (int dir=0; dir <= 1; dir++) {
+    // direction notation: xy[0] = x axis, xy[1] = y axis
+    for (int sign=-1; sign <= 1; sign += 2) {
+      // chop q along the line xy[dir] = sign*h[dir]
+      btScalar *pq = q;
+      btScalar *pr = r;
+      nr = 0;
+      for (int i=nq; i > 0; i--) {
+	// go through all points in q and all lines between adjacent points
+	if (sign*pq[dir] < h[dir]) {
+	  // this point is inside the chopping line
+	  pr[0] = pq[0];
+	  pr[1] = pq[1];
+	  pr += 2;
+	  nr++;
+	  if (nr & 8) {
+	    q = r;
+	    goto done;
+	  }
+	}
+	btScalar *nextq = (i > 1) ? pq+2 : q;
+	if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) {
+	  // this line crosses the chopping line
+	  pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
+	    (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
+	  pr[dir] = sign*h[dir];
+	  pr += 2;
+	  nr++;
+	  if (nr & 8) {
+	    q = r;
+	    goto done;
+	  }
+	}
+	pq += 2;
+      }
+      q = r;
+      r = (q==ret) ? buffer : ret;
+      nq = nr;
+    }
+  }
+ done:
+  if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar));
+  return nr;
+}
+
+
+#define M__PI 3.14159265f
+
+// given n points in the plane (array p, of size 2*n), generate m points that
+// best represent the whole set. the definition of 'best' here is not
+// predetermined - the idea is to select points that give good box-box
+// collision detection behavior. the chosen point indexes are returned in the
+// array iret (of size m). 'i0' is always the first entry in the array.
+// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
+// in the range [0..n-1].
+
+void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]);
+void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[])
+{
+  // compute the centroid of the polygon in cx,cy
+  int i,j;
+  btScalar a,cx,cy,q;
+  if (n==1) {
+    cx = p[0];
+    cy = p[1];
+  }
+  else if (n==2) {
+    cx = btScalar(0.5)*(p[0] + p[2]);
+    cy = btScalar(0.5)*(p[1] + p[3]);
+  }
+  else {
+    a = 0;
+    cx = 0;
+    cy = 0;
+    for (i=0; i<(n-1); i++) {
+      q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
+      a += q;
+      cx += q*(p[i*2]+p[i*2+2]);
+      cy += q*(p[i*2+1]+p[i*2+3]);
+    }
+    q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
+	if (btFabs(a+q) > SIMD_EPSILON)
+	{
+		a = 1.f/(btScalar(3.0)*(a+q));
+	} else
+	{
+		a=BT_LARGE_FLOAT;
+	}
+    cx = a*(cx + q*(p[n*2-2]+p[0]));
+    cy = a*(cy + q*(p[n*2-1]+p[1]));
+  }
+
+  // compute the angle of each point w.r.t. the centroid
+  btScalar A[8];
+  for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx);
+
+  // search for points that have angles closest to A[i0] + i*(2*pi/m).
+  int avail[8];
+  for (i=0; i<n; i++) avail[i] = 1;
+  avail[i0] = 0;
+  iret[0] = i0;
+  iret++;
+  for (j=1; j<m; j++) {
+    a = btScalar(j)*(2*M__PI/m) + A[i0];
+    if (a > M__PI) a -= 2*M__PI;
+    btScalar maxdiff=1e9,diff;
+
+    *iret = i0;			// iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
+
+    for (i=0; i<n; i++) {
+      if (avail[i]) {
+	diff = btFabs (A[i]-a);
+	if (diff > M__PI) diff = 2*M__PI - diff;
+	if (diff < maxdiff) {
+	  maxdiff = diff;
+	  *iret = i;
+	}
+      }
+    }
+#if defined(DEBUG) || defined (_DEBUG)
+    btAssert (*iret != i0);	// ensure iret got set
+#endif
+    avail[*iret] = 0;
+    iret++;
+  }
+}
+
+
+
+int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
+	     const btVector3& side1, const btVector3& p2,
+	     const dMatrix3 R2, const btVector3& side2,
+	     btVector3& normal, btScalar *depth, int *return_code,
+		 int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output);
+int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
+	     const btVector3& side1, const btVector3& p2,
+	     const dMatrix3 R2, const btVector3& side2,
+	     btVector3& normal, btScalar *depth, int *return_code,
+		 int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output)
+{
+  const btScalar fudge_factor = btScalar(1.05);
+  btVector3 p,pp,normalC(0.f,0.f,0.f);
+  const btScalar *normalR = 0;
+  btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
+    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
+  int i,j,invert_normal,code;
+
+  // get vector from centers of box 1 to box 2, relative to box 1
+  p = p2 - p1;
+  dMULTIPLY1_331 (pp,R1,p);		// get pp = p relative to body 1
+
+  // get side lengths / 2
+  A[0] = side1[0]*btScalar(0.5);
+  A[1] = side1[1]*btScalar(0.5);
+  A[2] = side1[2]*btScalar(0.5);
+  B[0] = side2[0]*btScalar(0.5);
+  B[1] = side2[1]*btScalar(0.5);
+  B[2] = side2[2]*btScalar(0.5);
+
+  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
+  R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
+  R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
+  R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
+
+  Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13);
+  Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23);
+  Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33);
+
+  // for all 15 possible separating axes:
+  //   * see if the axis separates the boxes. if so, return 0.
+  //   * find the depth of the penetration along the separating axis (s2)
+  //   * if this is the largest depth so far, record it.
+  // the normal vector will be set to the separating axis with the smallest
+  // depth. note: normalR is set to point to a column of R1 or R2 if that is
+  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
+  // set to a vector relative to body 1. invert_normal is 1 if the sign of
+  // the normal should be flipped.
+
+#define TST(expr1,expr2,norm,cc) \
+  s2 = btFabs(expr1) - (expr2); \
+  if (s2 > 0) return 0; \
+  if (s2 > s) { \
+    s = s2; \
+    normalR = norm; \
+    invert_normal = ((expr1) < 0); \
+    code = (cc); \
+  }
+
+  s = -dInfinity;
+  invert_normal = 0;
+  code = 0;
+
+  // separating axis = u1,u2,u3
+  TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
+  TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
+  TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
+
+  // separating axis = v1,v2,v3
+  TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
+  TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
+  TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
+
+  // note: cross product axes need to be scaled when s is computed.
+  // normal (n1,n2,n3) is relative to box 1.
+#undef TST
+#define TST(expr1,expr2,n1,n2,n3,cc) \
+  s2 = btFabs(expr1) - (expr2); \
+  if (s2 > SIMD_EPSILON) return 0; \
+  l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
+  if (l > SIMD_EPSILON) { \
+    s2 /= l; \
+    if (s2*fudge_factor > s) { \
+      s = s2; \
+      normalR = 0; \
+      normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
+      invert_normal = ((expr1) < 0); \
+      code = (cc); \
+    } \
+  }
+
+  btScalar fudge2 (1.0e-5f);
+
+  Q11 += fudge2;
+  Q12 += fudge2;
+  Q13 += fudge2;
+
+  Q21 += fudge2;
+  Q22 += fudge2;
+  Q23 += fudge2;
+
+  Q31 += fudge2;
+  Q32 += fudge2;
+  Q33 += fudge2;
+
+  // separating axis = u1 x (v1,v2,v3)
+  TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
+  TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
+  TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
+
+  // separating axis = u2 x (v1,v2,v3)
+  TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
+  TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
+  TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
+
+  // separating axis = u3 x (v1,v2,v3)
+  TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
+  TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
+  TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
+
+#undef TST
+
+  if (!code) return 0;
+
+  // if we get to this point, the boxes interpenetrate. compute the normal
+  // in global coordinates.
+  if (normalR) {
+    normal[0] = normalR[0];
+    normal[1] = normalR[4];
+    normal[2] = normalR[8];
+  }
+  else {
+    dMULTIPLY0_331 (normal,R1,normalC);
+  }
+  if (invert_normal) {
+    normal[0] = -normal[0];
+    normal[1] = -normal[1];
+    normal[2] = -normal[2];
+  }
+  *depth = -s;
+
+  // compute contact point(s)
+
+  if (code > 6) {
+    // an edge from box 1 touches an edge from box 2.
+    // find a point pa on the intersecting edge of box 1
+    btVector3 pa;
+    btScalar sign;
+    for (i=0; i<3; i++) pa[i] = p1[i];
+    for (j=0; j<3; j++) {
+      sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0);
+      for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
+    }
+
+    // find a point pb on the intersecting edge of box 2
+    btVector3 pb;
+    for (i=0; i<3; i++) pb[i] = p2[i];
+    for (j=0; j<3; j++) {
+      sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0);
+      for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
+    }
+
+    btScalar alpha,beta;
+    btVector3 ua,ub;
+    for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
+    for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
+
+    dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
+    for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
+    for (i=0; i<3; i++) pb[i] += ub[i]*beta;
+
+	{
+		
+		//contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
+		//contact[0].depth = *depth;
+		btVector3 pointInWorld;
+
+#ifdef USE_CENTER_POINT
+	    for (i=0; i<3; i++) 
+			pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5);
+		output.addContactPoint(-normal,pointInWorld,-*depth);
+#else
+		output.addContactPoint(-normal,pb,-*depth);
+
+#endif //
+		*return_code = code;
+	}
+    return 1;
+  }
+
+  // okay, we have a face-something intersection (because the separating
+  // axis is perpendicular to a face). define face 'a' to be the reference
+  // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
+  // the incident face (the closest face of the other box).
+
+  const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb;
+  if (code <= 3) {
+    Ra = R1;
+    Rb = R2;
+    pa = p1;
+    pb = p2;
+    Sa = A;
+    Sb = B;
+  }
+  else {
+    Ra = R2;
+    Rb = R1;
+    pa = p2;
+    pb = p1;
+    Sa = B;
+    Sb = A;
+  }
+
+  // nr = normal vector of reference face dotted with axes of incident box.
+  // anr = absolute values of nr.
+  btVector3 normal2,nr,anr;
+  if (code <= 3) {
+    normal2[0] = normal[0];
+    normal2[1] = normal[1];
+    normal2[2] = normal[2];
+  }
+  else {
+    normal2[0] = -normal[0];
+    normal2[1] = -normal[1];
+    normal2[2] = -normal[2];
+  }
+  dMULTIPLY1_331 (nr,Rb,normal2);
+  anr[0] = btFabs (nr[0]);
+  anr[1] = btFabs (nr[1]);
+  anr[2] = btFabs (nr[2]);
+
+  // find the largest compontent of anr: this corresponds to the normal
+  // for the indident face. the other axis numbers of the indicent face
+  // are stored in a1,a2.
+  int lanr,a1,a2;
+  if (anr[1] > anr[0]) {
+    if (anr[1] > anr[2]) {
+      a1 = 0;
+      lanr = 1;
+      a2 = 2;
+    }
+    else {
+      a1 = 0;
+      a2 = 1;
+      lanr = 2;
+    }
+  }
+  else {
+    if (anr[0] > anr[2]) {
+      lanr = 0;
+      a1 = 1;
+      a2 = 2;
+    }
+    else {
+      a1 = 0;
+      a2 = 1;
+      lanr = 2;
+    }
+  }
+
+  // compute center point of incident face, in reference-face coordinates
+  btVector3 center;
+  if (nr[lanr] < 0) {
+    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
+  }
+  else {
+    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
+  }
+
+  // find the normal and non-normal axis numbers of the reference box
+  int codeN,code1,code2;
+  if (code <= 3) codeN = code-1; else codeN = code-4;
+  if (codeN==0) {
+    code1 = 1;
+    code2 = 2;
+  }
+  else if (codeN==1) {
+    code1 = 0;
+    code2 = 2;
+  }
+  else {
+    code1 = 0;
+    code2 = 1;
+  }
+
+  // find the four corners of the incident face, in reference-face coordinates
+  btScalar quad[8];	// 2D coordinate of incident face (x,y pairs)
+  btScalar c1,c2,m11,m12,m21,m22;
+  c1 = dDOT14 (center,Ra+code1);
+  c2 = dDOT14 (center,Ra+code2);
+  // optimize this? - we have already computed this data above, but it is not
+  // stored in an easy-to-index format. for now it's quicker just to recompute
+  // the four dot products.
+  m11 = dDOT44 (Ra+code1,Rb+a1);
+  m12 = dDOT44 (Ra+code1,Rb+a2);
+  m21 = dDOT44 (Ra+code2,Rb+a1);
+  m22 = dDOT44 (Ra+code2,Rb+a2);
+  {
+    btScalar k1 = m11*Sb[a1];
+    btScalar k2 = m21*Sb[a1];
+    btScalar k3 = m12*Sb[a2];
+    btScalar k4 = m22*Sb[a2];
+    quad[0] = c1 - k1 - k3;
+    quad[1] = c2 - k2 - k4;
+    quad[2] = c1 - k1 + k3;
+    quad[3] = c2 - k2 + k4;
+    quad[4] = c1 + k1 + k3;
+    quad[5] = c2 + k2 + k4;
+    quad[6] = c1 + k1 - k3;
+    quad[7] = c2 + k2 - k4;
+  }
+
+  // find the size of the reference face
+  btScalar rect[2];
+  rect[0] = Sa[code1];
+  rect[1] = Sa[code2];
+
+  // intersect the incident and reference faces
+  btScalar ret[16];
+  int n = intersectRectQuad2 (rect,quad,ret);
+  if (n < 1) return 0;		// this should never happen
+
+  // convert the intersection points into reference-face coordinates,
+  // and compute the contact position and depth for each point. only keep
+  // those points that have a positive (penetrating) depth. delete points in
+  // the 'ret' array as necessary so that 'point' and 'ret' correspond.
+  btScalar point[3*8];		// penetrating contact points
+  btScalar dep[8];			// depths for those points
+  btScalar det1 = 1.f/(m11*m22 - m12*m21);
+  m11 *= det1;
+  m12 *= det1;
+  m21 *= det1;
+  m22 *= det1;
+  int cnum = 0;			// number of penetrating contact points found
+  for (j=0; j < n; j++) {
+    btScalar k1 =  m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
+    btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
+    for (i=0; i<3; i++) point[cnum*3+i] =
+			  center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
+    dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
+    if (dep[cnum] >= 0) {
+      ret[cnum*2] = ret[j*2];
+      ret[cnum*2+1] = ret[j*2+1];
+      cnum++;
+    }
+  }
+  if (cnum < 1) return 0;	// this should never happen
+
+  // we can't generate more contacts than we actually have
+  if (maxc > cnum) maxc = cnum;
+  if (maxc < 1) maxc = 1;
+
+  if (cnum <= maxc) {
+
+	  if (code<4) 
+	  {
+    // we have less contacts than we need, so we use them all
+    for (j=0; j < cnum; j++) 
+	{
+		btVector3 pointInWorld;
+		for (i=0; i<3; i++) 
+			pointInWorld[i] = point[j*3+i] + pa[i];
+		output.addContactPoint(-normal,pointInWorld,-dep[j]);
+
+    }
+	  } else
+	  {
+		  // we have less contacts than we need, so we use them all
+		for (j=0; j < cnum; j++) 
+		{
+			btVector3 pointInWorld;
+			for (i=0; i<3; i++) 
+				pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j];
+				//pointInWorld[i] = point[j*3+i] + pa[i];
+			output.addContactPoint(-normal,pointInWorld,-dep[j]);
+		}
+	  }
+  }
+  else {
+    // we have more contacts than are wanted, some of them must be culled.
+    // find the deepest point, it is always the first contact.
+    int i1 = 0;
+    btScalar maxdepth = dep[0];
+    for (i=1; i<cnum; i++) {
+      if (dep[i] > maxdepth) {
+	maxdepth = dep[i];
+	i1 = i;
+      }
+    }
+
+    int iret[8];
+    cullPoints2 (cnum,ret,maxc,i1,iret);
+
+    for (j=0; j < maxc; j++) {
+//      dContactGeom *con = CONTACT(contact,skip*j);
+  //    for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
+    //  con->depth = dep[iret[j]];
+
+		btVector3 posInWorld;
+		for (i=0; i<3; i++) 
+			posInWorld[i] = point[iret[j]*3+i] + pa[i];
+		if (code<4) 
+	   {
+			output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
+		} else
+		{
+			output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]);
+		}
+    }
+    cnum = maxc;
+  }
+
+  *return_code = code;
+  return cnum;
+}
+
+void	btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/)
+{
+	
+	const btTransform& transformA = input.m_transformA;
+	const btTransform& transformB = input.m_transformB;
+	
+	int skip = 0;
+	dContactGeom *contact = 0;
+
+	dMatrix3 R1;
+	dMatrix3 R2;
+
+	for (int j=0;j<3;j++)
+	{
+		R1[0+4*j] = transformA.getBasis()[j].x();
+		R2[0+4*j] = transformB.getBasis()[j].x();
+
+		R1[1+4*j] = transformA.getBasis()[j].y();
+		R2[1+4*j] = transformB.getBasis()[j].y();
+
+
+		R1[2+4*j] = transformA.getBasis()[j].z();
+		R2[2+4*j] = transformB.getBasis()[j].z();
+
+	}
+
+	
+
+	btVector3 normal;
+	btScalar depth;
+	int return_code;
+	int maxc = 4;
+
+
+	dBoxBox2 (transformA.getOrigin(), 
+	R1,
+	2.f*m_box1->getHalfExtentsWithMargin(),
+	transformB.getOrigin(),
+	R2, 
+	2.f*m_box2->getHalfExtentsWithMargin(),
+	normal, &depth, &return_code,
+	maxc, contact, skip,
+	output
+	);
+
+}

libs/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.h

@@ -0,0 +1,44 @@
+/*
+ * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
+ * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org
+
+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 BOX_BOX_DETECTOR_H
+#define BOX_BOX_DETECTOR_H
+
+
+class btBoxShape;
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+
+
+/// btBoxBoxDetector wraps the ODE box-box collision detector
+/// re-distributed under the Zlib license with permission from Russell L. Smith
+struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface
+{
+	btBoxShape* m_box1;
+	btBoxShape* m_box2;
+
+public:
+
+	btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2);
+
+	virtual ~btBoxBoxDetector() {};
+
+	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
+
+};
+
+#endif //BT_BOX_BOX_DETECTOR_H

libs/bullet/BulletCollision/CollisionDispatch/btCollisionConfiguration.h

@@ -0,0 +1,47 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_COLLISION_CONFIGURATION
+#define BT_COLLISION_CONFIGURATION
+struct btCollisionAlgorithmCreateFunc;
+
+class btStackAlloc;
+class btPoolAllocator;
+
+///btCollisionConfiguration allows to configure Bullet collision detection
+///stack allocator size, default collision algorithms and persistent manifold pool size
+///@todo: describe the meaning
+class	btCollisionConfiguration
+{
+
+public:
+
+	virtual ~btCollisionConfiguration()
+	{
+	}
+
+	///memory pools
+	virtual btPoolAllocator* getPersistentManifoldPool() = 0;
+
+	virtual btPoolAllocator* getCollisionAlgorithmPool() = 0;
+
+	virtual btStackAlloc*	getStackAllocator() = 0;
+
+	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0;
+
+};
+
+#endif //BT_COLLISION_CONFIGURATION
+

libs/bullet/BulletCollision/CollisionDispatch/btCollisionCreateFunc.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 COLLISION_CREATE_FUNC
+#define COLLISION_CREATE_FUNC
+
+#include "LinearMath/btAlignedObjectArray.h"
+class btCollisionAlgorithm;
+class btCollisionObject;
+
+struct btCollisionAlgorithmConstructionInfo;
+
+///Used by the btCollisionDispatcher to register and create instances for btCollisionAlgorithm
+struct btCollisionAlgorithmCreateFunc
+{
+	bool m_swapped;
+	
+	btCollisionAlgorithmCreateFunc()
+		:m_swapped(false)
+	{
+	}
+	virtual ~btCollisionAlgorithmCreateFunc(){};
+
+	virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& , btCollisionObject* body0,btCollisionObject* body1)
+	{
+		
+		(void)body0;
+		(void)body1;
+		return 0;
+	}
+};
+#endif //COLLISION_CREATE_FUNC
+

libs/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -0,0 +1,303 @@
+/*
+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 "btCollisionDispatcher.h"
+
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "LinearMath/btPoolAllocator.h"
+#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
+
+int gNumManifold = 0;
+
+#ifdef BT_DEBUG
+#include <stdio.h>
+#endif
+
+
+btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): 
+m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD),
+	m_collisionConfiguration(collisionConfiguration)
+{
+	int i;
+
+	setNearCallback(defaultNearCallback);
+	
+	m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool();
+
+	m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool();
+
+	for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
+	{
+		for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
+		{
+			m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
+			btAssert(m_doubleDispatch[i][j]);
+		}
+	}
+	
+	
+}
+
+
+void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
+{
+	m_doubleDispatch[proxyType0][proxyType1] = createFunc;
+}
+
+btCollisionDispatcher::~btCollisionDispatcher()
+{
+}
+
+btPersistentManifold*	btCollisionDispatcher::getNewManifold(void* b0,void* b1) 
+{ 
+	gNumManifold++;
+	
+	//btAssert(gNumManifold < 65535);
+	
+
+	btCollisionObject* body0 = (btCollisionObject*)b0;
+	btCollisionObject* body1 = (btCollisionObject*)b1;
+
+	//optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
+	
+	btScalar contactBreakingThreshold =  (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? 
+		btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold) , body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold))
+		: gContactBreakingThreshold ;
+
+	btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold());
+		
+	void* mem = 0;
+	
+	if (m_persistentManifoldPoolAllocator->getFreeCount())
+	{
+		mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
+	} else
+	{
+		mem = btAlignedAlloc(sizeof(btPersistentManifold),16);
+
+	}
+	btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold);
+	manifold->m_index1a = m_manifoldsPtr.size();
+	m_manifoldsPtr.push_back(manifold);
+
+	return manifold;
+}
+
+void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold)
+{
+	manifold->clearManifold();
+}
+
+	
+void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
+{
+	
+	gNumManifold--;
+
+	//printf("releaseManifold: gNumManifold %d\n",gNumManifold);
+	clearManifold(manifold);
+
+	int findIndex = manifold->m_index1a;
+	btAssert(findIndex < m_manifoldsPtr.size());
+	m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1);
+	m_manifoldsPtr[findIndex]->m_index1a = findIndex;
+	m_manifoldsPtr.pop_back();
+
+	manifold->~btPersistentManifold();
+	if (m_persistentManifoldPoolAllocator->validPtr(manifold))
+	{
+		m_persistentManifoldPoolAllocator->freeMemory(manifold);
+	} else
+	{
+		btAlignedFree(manifold);
+	}
+	
+}
+
+	
+
+btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold)
+{
+	
+	btCollisionAlgorithmConstructionInfo ci;
+
+	ci.m_dispatcher1 = this;
+	ci.m_manifold = sharedManifold;
+	btCollisionAlgorithm* algo = m_doubleDispatch[body0->getCollisionShape()->getShapeType()][body1->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0,body1);
+
+	return algo;
+}
+
+
+
+
+bool	btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionObject* body1)
+{
+	//here you can do filtering
+	bool hasResponse = 
+		(body0->hasContactResponse() && body1->hasContactResponse());
+	//no response between two static/kinematic bodies:
+	hasResponse = hasResponse &&
+		((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject()));
+	return hasResponse;
+}
+
+bool	btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionObject* body1)
+{
+	btAssert(body0);
+	btAssert(body1);
+
+	bool needsCollision = true;
+
+#ifdef BT_DEBUG
+	if (!(m_dispatcherFlags & btCollisionDispatcher::CD_STATIC_STATIC_REPORTED))
+	{
+		//broadphase filtering already deals with this
+		if ((body0->isStaticObject() || body0->isKinematicObject()) &&
+			(body1->isStaticObject() || body1->isKinematicObject()))
+		{
+			m_dispatcherFlags |= btCollisionDispatcher::CD_STATIC_STATIC_REPORTED;
+			printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
+		}
+	}
+#endif //BT_DEBUG
+
+	if ((!body0->isActive()) && (!body1->isActive()))
+		needsCollision = false;
+	else if (!body0->checkCollideWith(body1))
+		needsCollision = false;
+	
+	return needsCollision ;
+
+}
+
+
+
+///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc)
+///this is useful for the collision dispatcher.
+class btCollisionPairCallback : public btOverlapCallback
+{
+	const btDispatcherInfo& m_dispatchInfo;
+	btCollisionDispatcher*	m_dispatcher;
+
+public:
+
+	btCollisionPairCallback(const btDispatcherInfo& dispatchInfo,btCollisionDispatcher*	dispatcher)
+	:m_dispatchInfo(dispatchInfo),
+	m_dispatcher(dispatcher)
+	{
+	}
+
+	/*btCollisionPairCallback& operator=(btCollisionPairCallback& other)
+	{
+		m_dispatchInfo = other.m_dispatchInfo;
+		m_dispatcher = other.m_dispatcher;
+		return *this;
+	}
+	*/
+
+
+	virtual ~btCollisionPairCallback() {}
+
+
+	virtual bool	processOverlap(btBroadphasePair& pair)
+	{
+		(*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
+
+		return false;
+	}
+};
+
+
+
+void	btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) 
+{
+	//m_blockedForChanges = true;
+
+	btCollisionPairCallback	collisionCallback(dispatchInfo,this);
+
+	pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher);
+
+	//m_blockedForChanges = false;
+
+}
+
+
+
+
+//by default, Bullet will use this near callback
+void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo)
+{
+		btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
+		btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+
+		if (dispatcher.needsCollision(colObj0,colObj1))
+		{
+			//dispatcher will keep algorithms persistent in the collision pair
+			if (!collisionPair.m_algorithm)
+			{
+				collisionPair.m_algorithm = dispatcher.findAlgorithm(colObj0,colObj1);
+			}
+
+			if (collisionPair.m_algorithm)
+			{
+				btManifoldResult contactPointResult(colObj0,colObj1);
+				
+				if (dispatchInfo.m_dispatchFunc == 		btDispatcherInfo::DISPATCH_DISCRETE)
+				{
+					//discrete collision detection query
+					collisionPair.m_algorithm->processCollision(colObj0,colObj1,dispatchInfo,&contactPointResult);
+				} else
+				{
+					//continuous collision detection query, time of impact (toi)
+					btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult);
+					if (dispatchInfo.m_timeOfImpact > toi)
+						dispatchInfo.m_timeOfImpact = toi;
+
+				}
+			}
+		}
+
+}
+
+
+void* btCollisionDispatcher::allocateCollisionAlgorithm(int size)
+{
+	if (m_collisionAlgorithmPoolAllocator->getFreeCount())
+	{
+		return m_collisionAlgorithmPoolAllocator->allocate(size);
+	}
+	
+	//warn user for overflow?
+	return	btAlignedAlloc(static_cast<size_t>(size), 16);
+}
+
+void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr)
+{
+	if (m_collisionAlgorithmPoolAllocator->validPtr(ptr))
+	{
+		m_collisionAlgorithmPoolAllocator->freeMemory(ptr);
+	} else
+	{
+		btAlignedFree(ptr);
+	}
+}

libs/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h

@@ -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.
+*/
+
+#ifndef COLLISION__DISPATCHER_H
+#define COLLISION__DISPATCHER_H
+
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+class btIDebugDraw;
+class btOverlappingPairCache;
+class btPoolAllocator;
+class btCollisionConfiguration;
+
+#include "btCollisionCreateFunc.h"
+
+#define USE_DISPATCH_REGISTRY_ARRAY 1
+
+class btCollisionDispatcher;
+///user can override this nearcallback for collision filtering and more finegrained control over collision detection
+typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo);
+
+
+///btCollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs.
+///Time of Impact, Closest Points and Penetration Depth.
+class btCollisionDispatcher : public btDispatcher
+{
+	int		m_dispatcherFlags;
+	
+	btAlignedObjectArray<btPersistentManifold*>	m_manifoldsPtr;
+
+	btManifoldResult	m_defaultManifoldResult;
+
+	btNearCallback		m_nearCallback;
+	
+	btPoolAllocator*	m_collisionAlgorithmPoolAllocator;
+
+	btPoolAllocator*	m_persistentManifoldPoolAllocator;
+
+	btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
+
+	btCollisionConfiguration*	m_collisionConfiguration;
+
+
+public:
+
+	enum DispatcherFlags
+	{
+		CD_STATIC_STATIC_REPORTED = 1,
+		CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD = 2
+	};
+
+	int	getDispatcherFlags() const
+	{
+		return m_dispatcherFlags;
+	}
+
+	void	setDispatcherFlags(int flags)
+	{
+        (void) flags;
+		m_dispatcherFlags = 0;
+	}
+
+	///registerCollisionCreateFunc allows registration of custom/alternative collision create functions
+	void	registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc);
+
+	int	getNumManifolds() const
+	{ 
+		return int( m_manifoldsPtr.size());
+	}
+
+	btPersistentManifold**	getInternalManifoldPointer()
+	{
+		return &m_manifoldsPtr[0];
+	}
+
+	 btPersistentManifold* getManifoldByIndexInternal(int index)
+	{
+		return m_manifoldsPtr[index];
+	}
+
+	 const btPersistentManifold* getManifoldByIndexInternal(int index) const
+	{
+		return m_manifoldsPtr[index];
+	}
+
+	btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration);
+
+	virtual ~btCollisionDispatcher();
+
+	virtual btPersistentManifold*	getNewManifold(void* b0,void* b1);
+	
+	virtual void releaseManifold(btPersistentManifold* manifold);
+
+
+	virtual void clearManifold(btPersistentManifold* manifold);
+
+			
+	btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold = 0);
+		
+	virtual bool	needsCollision(btCollisionObject* body0,btCollisionObject* body1);
+	
+	virtual bool	needsResponse(btCollisionObject* body0,btCollisionObject* body1);
+	
+	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ;
+
+	void	setNearCallback(btNearCallback	nearCallback)
+	{
+		m_nearCallback = nearCallback; 
+	}
+
+	btNearCallback	getNearCallback() const
+	{
+		return m_nearCallback;
+	}
+
+	//by default, Bullet will use this near callback
+	static void  defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo);
+
+	virtual	void* allocateCollisionAlgorithm(int size);
+
+	virtual	void freeCollisionAlgorithm(void* ptr);
+
+	btCollisionConfiguration*	getCollisionConfiguration()
+	{
+		return m_collisionConfiguration;
+	}
+
+	const btCollisionConfiguration*	getCollisionConfiguration() const
+	{
+		return m_collisionConfiguration;
+	}
+
+	void	setCollisionConfiguration(btCollisionConfiguration* config)
+	{
+		m_collisionConfiguration = config;
+	}
+
+};
+
+#endif //COLLISION__DISPATCHER_H
+

libs/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -0,0 +1,116 @@
+/*
+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 "btCollisionObject.h"
+#include "LinearMath/btSerializer.h"
+
+btCollisionObject::btCollisionObject()
+	:	m_anisotropicFriction(1.f,1.f,1.f),
+	m_hasAnisotropicFriction(false),
+	m_contactProcessingThreshold(BT_LARGE_FLOAT),
+		m_broadphaseHandle(0),
+		m_collisionShape(0),
+		m_extensionPointer(0),
+		m_rootCollisionShape(0),
+		m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT),
+		m_islandTag1(-1),
+		m_companionId(-1),
+		m_activationState1(1),
+		m_deactivationTime(btScalar(0.)),
+		m_friction(btScalar(0.5)),
+		m_restitution(btScalar(0.)),
+		m_internalType(CO_COLLISION_OBJECT),
+		m_userObjectPointer(0),
+		m_hitFraction(btScalar(1.)),
+		m_ccdSweptSphereRadius(btScalar(0.)),
+		m_ccdMotionThreshold(btScalar(0.)),
+		m_checkCollideWith(false)
+{
+	m_worldTransform.setIdentity();
+}
+
+btCollisionObject::~btCollisionObject()
+{
+}
+
+void btCollisionObject::setActivationState(int newState) 
+{ 
+	if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION))
+		m_activationState1 = newState;
+}
+
+void btCollisionObject::forceActivationState(int newState)
+{
+	m_activationState1 = newState;
+}
+
+void btCollisionObject::activate(bool forceActivation)
+{
+	if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT)))
+	{
+		setActivationState(ACTIVE_TAG);
+		m_deactivationTime = btScalar(0.);
+	}
+}
+
+const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+
+	btCollisionObjectData* dataOut = (btCollisionObjectData*)dataBuffer;
+
+	m_worldTransform.serialize(dataOut->m_worldTransform);
+	m_interpolationWorldTransform.serialize(dataOut->m_interpolationWorldTransform);
+	m_interpolationLinearVelocity.serialize(dataOut->m_interpolationLinearVelocity);
+	m_interpolationAngularVelocity.serialize(dataOut->m_interpolationAngularVelocity);
+	m_anisotropicFriction.serialize(dataOut->m_anisotropicFriction);
+	dataOut->m_hasAnisotropicFriction = m_hasAnisotropicFriction;
+	dataOut->m_contactProcessingThreshold = m_contactProcessingThreshold;
+	dataOut->m_broadphaseHandle = 0;
+	dataOut->m_collisionShape = serializer->getUniquePointer(m_collisionShape);
+	dataOut->m_rootCollisionShape = 0;//@todo
+	dataOut->m_collisionFlags = m_collisionFlags;
+	dataOut->m_islandTag1 = m_islandTag1;
+	dataOut->m_companionId = m_companionId;
+	dataOut->m_activationState1 = m_activationState1;
+	dataOut->m_activationState1 = m_activationState1;
+	dataOut->m_deactivationTime = m_deactivationTime;
+	dataOut->m_friction = m_friction;
+	dataOut->m_restitution = m_restitution;
+	dataOut->m_internalType = m_internalType;
+	
+	char* name = (char*) serializer->findNameForPointer(this);
+	dataOut->m_name = (char*)serializer->getUniquePointer(name);
+	if (dataOut->m_name)
+	{
+		serializer->serializeName(name);
+	}
+	dataOut->m_hitFraction = m_hitFraction;
+	dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius;
+	dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold;
+	dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold;
+	dataOut->m_checkCollideWith = m_checkCollideWith;
+
+	return btCollisionObjectDataName;
+}
+
+
+void btCollisionObject::serializeSingleObject(class btSerializer* serializer) const
+{
+	int len = calculateSerializeBufferSize();
+	btChunk* chunk = serializer->allocate(len,1);
+	const char* structType = serialize(chunk->m_oldPtr, serializer);
+	serializer->finalizeChunk(chunk,structType,BT_COLLISIONOBJECT_CODE,(void*)this);
+}

libs/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -0,0 +1,524 @@
+/*
+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_OBJECT_H
+#define COLLISION_OBJECT_H
+
+#include "LinearMath/btTransform.h"
+
+//island management, m_activationState1
+#define ACTIVE_TAG 1
+#define ISLAND_SLEEPING 2
+#define WANTS_DEACTIVATION 3
+#define DISABLE_DEACTIVATION 4
+#define DISABLE_SIMULATION 5
+
+struct	btBroadphaseProxy;
+class	btCollisionShape;
+struct btCollisionShapeData;
+#include "LinearMath/btMotionState.h"
+#include "LinearMath/btAlignedAllocator.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btCollisionObjectData btCollisionObjectDoubleData
+#define btCollisionObjectDataName "btCollisionObjectDoubleData"
+#else
+#define btCollisionObjectData btCollisionObjectFloatData
+#define btCollisionObjectDataName "btCollisionObjectFloatData"
+#endif
+
+
+/// btCollisionObject can be used to manage collision detection objects. 
+/// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
+/// They can be added to the btCollisionWorld.
+ATTRIBUTE_ALIGNED16(class)	btCollisionObject
+{
+
+protected:
+
+	btTransform	m_worldTransform;
+
+	///m_interpolationWorldTransform is used for CCD and interpolation
+	///it can be either previous or future (predicted) transform
+	btTransform	m_interpolationWorldTransform;
+	//those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) 
+	//without destroying the continuous interpolated motion (which uses this interpolation velocities)
+	btVector3	m_interpolationLinearVelocity;
+	btVector3	m_interpolationAngularVelocity;
+	
+	btVector3	m_anisotropicFriction;
+	int			m_hasAnisotropicFriction;
+	btScalar	m_contactProcessingThreshold;	
+
+	btBroadphaseProxy*		m_broadphaseHandle;
+	btCollisionShape*		m_collisionShape;
+	///m_extensionPointer is used by some internal low-level Bullet extensions.
+	void*					m_extensionPointer;
+	
+	///m_rootCollisionShape is temporarily used to store the original collision shape
+	///The m_collisionShape might be temporarily replaced by a child collision shape during collision detection purposes
+	///If it is NULL, the m_collisionShape is not temporarily replaced.
+	btCollisionShape*		m_rootCollisionShape;
+
+	int				m_collisionFlags;
+
+	int				m_islandTag1;
+	int				m_companionId;
+
+	int				m_activationState1;
+	btScalar			m_deactivationTime;
+
+	btScalar		m_friction;
+	btScalar		m_restitution;
+
+	///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc.
+	///do not assign your own m_internalType unless you write a new dynamics object class.
+	int				m_internalType;
+
+	///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
+	void*			m_userObjectPointer;
+
+	///time of impact calculation
+	btScalar		m_hitFraction; 
+	
+	///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+	btScalar		m_ccdSweptSphereRadius;
+
+	/// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
+	btScalar		m_ccdMotionThreshold;
+	
+	/// If some object should have elaborate collision filtering by sub-classes
+	int			m_checkCollideWith;
+
+	virtual bool	checkCollideWithOverride(btCollisionObject* /* co */)
+	{
+		return true;
+	}
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	enum CollisionFlags
+	{
+		CF_STATIC_OBJECT= 1,
+		CF_KINEMATIC_OBJECT= 2,
+		CF_NO_CONTACT_RESPONSE = 4,
+		CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
+		CF_CHARACTER_OBJECT = 16,
+		CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
+		CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing
+	};
+
+	enum	CollisionObjectTypes
+	{
+		CO_COLLISION_OBJECT =1,
+		CO_RIGID_BODY=2,
+		///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter
+		///It is useful for collision sensors, explosion objects, character controller etc.
+		CO_GHOST_OBJECT=4,
+		CO_SOFT_BODY=8,
+		CO_HF_FLUID=16,
+		CO_USER_TYPE=32
+	};
+
+	SIMD_FORCE_INLINE bool mergesSimulationIslands() const
+	{
+		///static objects, kinematic and object without contact response don't merge islands
+		return  ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0);
+	}
+
+	const btVector3& getAnisotropicFriction() const
+	{
+		return m_anisotropicFriction;
+	}
+	void	setAnisotropicFriction(const btVector3& anisotropicFriction)
+	{
+		m_anisotropicFriction = anisotropicFriction;
+		m_hasAnisotropicFriction = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f);
+	}
+	bool	hasAnisotropicFriction() const
+	{
+		return m_hasAnisotropicFriction!=0;
+	}
+
+	///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default.
+	///Note that using contacts with positive distance can improve stability. It increases, however, the chance of colliding with degerate contacts, such as 'interior' triangle edges
+	void	setContactProcessingThreshold( btScalar contactProcessingThreshold)
+	{
+		m_contactProcessingThreshold = contactProcessingThreshold;
+	}
+	btScalar	getContactProcessingThreshold() const
+	{
+		return m_contactProcessingThreshold;
+	}
+
+	SIMD_FORCE_INLINE bool		isStaticObject() const {
+		return (m_collisionFlags & CF_STATIC_OBJECT) != 0;
+	}
+
+	SIMD_FORCE_INLINE bool		isKinematicObject() const
+	{
+		return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0;
+	}
+
+	SIMD_FORCE_INLINE bool		isStaticOrKinematicObject() const
+	{
+		return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ;
+	}
+
+	SIMD_FORCE_INLINE bool		hasContactResponse() const {
+		return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0;
+	}
+
+	
+	btCollisionObject();
+
+	virtual ~btCollisionObject();
+
+	virtual void	setCollisionShape(btCollisionShape* collisionShape)
+	{
+		m_collisionShape = collisionShape;
+		m_rootCollisionShape = collisionShape;
+	}
+
+	SIMD_FORCE_INLINE const btCollisionShape*	getCollisionShape() const
+	{
+		return m_collisionShape;
+	}
+
+	SIMD_FORCE_INLINE btCollisionShape*	getCollisionShape()
+	{
+		return m_collisionShape;
+	}
+
+	SIMD_FORCE_INLINE const btCollisionShape*	getRootCollisionShape() const
+	{
+		return m_rootCollisionShape;
+	}
+
+	SIMD_FORCE_INLINE btCollisionShape*	getRootCollisionShape()
+	{
+		return m_rootCollisionShape;
+	}
+
+	///Avoid using this internal API call
+	///internalSetTemporaryCollisionShape is used to temporary replace the actual collision shape by a child collision shape.
+	void	internalSetTemporaryCollisionShape(btCollisionShape* collisionShape)
+	{
+		m_collisionShape = collisionShape;
+	}
+
+	///Avoid using this internal API call, the extension pointer is used by some Bullet extensions. 
+	///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead.
+	void*		internalGetExtensionPointer() const
+	{
+		return m_extensionPointer;
+	}
+	///Avoid using this internal API call, the extension pointer is used by some Bullet extensions
+	///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead.
+	void	internalSetExtensionPointer(void* pointer)
+	{
+		m_extensionPointer = pointer;
+	}
+
+	SIMD_FORCE_INLINE	int	getActivationState() const { return m_activationState1;}
+	
+	void setActivationState(int newState);
+
+	void	setDeactivationTime(btScalar time)
+	{
+		m_deactivationTime = time;
+	}
+	btScalar	getDeactivationTime() const
+	{
+		return m_deactivationTime;
+	}
+
+	void forceActivationState(int newState);
+
+	void	activate(bool forceActivation = false);
+
+	SIMD_FORCE_INLINE bool isActive() const
+	{
+		return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION));
+	}
+
+	void	setRestitution(btScalar rest)
+	{
+		m_restitution = rest;
+	}
+	btScalar	getRestitution() const
+	{
+		return m_restitution;
+	}
+	void	setFriction(btScalar frict)
+	{
+		m_friction = frict;
+	}
+	btScalar	getFriction() const
+	{
+		return m_friction;
+	}
+
+	///reserved for Bullet internal usage
+	int	getInternalType() const
+	{
+		return m_internalType;
+	}
+
+	btTransform&	getWorldTransform()
+	{
+		return m_worldTransform;
+	}
+
+	const btTransform&	getWorldTransform() const
+	{
+		return m_worldTransform;
+	}
+
+	void	setWorldTransform(const btTransform& worldTrans)
+	{
+		m_worldTransform = worldTrans;
+	}
+
+
+	SIMD_FORCE_INLINE btBroadphaseProxy*	getBroadphaseHandle()
+	{
+		return m_broadphaseHandle;
+	}
+
+	SIMD_FORCE_INLINE const btBroadphaseProxy*	getBroadphaseHandle() const
+	{
+		return m_broadphaseHandle;
+	}
+
+	void	setBroadphaseHandle(btBroadphaseProxy* handle)
+	{
+		m_broadphaseHandle = handle;
+	}
+
+
+	const btTransform&	getInterpolationWorldTransform() const
+	{
+		return m_interpolationWorldTransform;
+	}
+
+	btTransform&	getInterpolationWorldTransform()
+	{
+		return m_interpolationWorldTransform;
+	}
+
+	void	setInterpolationWorldTransform(const btTransform&	trans)
+	{
+		m_interpolationWorldTransform = trans;
+	}
+
+	void	setInterpolationLinearVelocity(const btVector3& linvel)
+	{
+		m_interpolationLinearVelocity = linvel;
+	}
+
+	void	setInterpolationAngularVelocity(const btVector3& angvel)
+	{
+		m_interpolationAngularVelocity = angvel;
+	}
+
+	const btVector3&	getInterpolationLinearVelocity() const
+	{
+		return m_interpolationLinearVelocity;
+	}
+
+	const btVector3&	getInterpolationAngularVelocity() const
+	{
+		return m_interpolationAngularVelocity;
+	}
+
+	SIMD_FORCE_INLINE int getIslandTag() const
+	{
+		return	m_islandTag1;
+	}
+
+	void	setIslandTag(int tag)
+	{
+		m_islandTag1 = tag;
+	}
+
+	SIMD_FORCE_INLINE int getCompanionId() const
+	{
+		return	m_companionId;
+	}
+
+	void	setCompanionId(int id)
+	{
+		m_companionId = id;
+	}
+
+	SIMD_FORCE_INLINE btScalar			getHitFraction() const
+	{
+		return m_hitFraction; 
+	}
+
+	void	setHitFraction(btScalar hitFraction)
+	{
+		m_hitFraction = hitFraction;
+	}
+
+	
+	SIMD_FORCE_INLINE int	getCollisionFlags() const
+	{
+		return m_collisionFlags;
+	}
+
+	void	setCollisionFlags(int flags)
+	{
+		m_collisionFlags = flags;
+	}
+	
+	///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+	btScalar			getCcdSweptSphereRadius() const
+	{
+		return m_ccdSweptSphereRadius;
+	}
+
+	///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+	void	setCcdSweptSphereRadius(btScalar radius)
+	{
+		m_ccdSweptSphereRadius = radius;
+	}
+
+	btScalar 	getCcdMotionThreshold() const
+	{
+		return m_ccdMotionThreshold;
+	}
+
+	btScalar 	getCcdSquareMotionThreshold() const
+	{
+		return m_ccdMotionThreshold*m_ccdMotionThreshold;
+	}
+
+
+
+	/// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
+	void	setCcdMotionThreshold(btScalar ccdMotionThreshold)
+	{
+		m_ccdMotionThreshold = ccdMotionThreshold;
+	}
+
+	///users can point to their objects, userPointer is not used by Bullet
+	void*	getUserPointer() const
+	{
+		return m_userObjectPointer;
+	}
+	
+	///users can point to their objects, userPointer is not used by Bullet
+	void	setUserPointer(void* userPointer)
+	{
+		m_userObjectPointer = userPointer;
+	}
+
+
+	inline bool checkCollideWith(btCollisionObject* co)
+	{
+		if (m_checkCollideWith)
+			return checkCollideWithOverride(co);
+
+		return true;
+	}
+
+	virtual	int	calculateSerializeBufferSize()	const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, class btSerializer* serializer) const;
+
+	virtual void serializeSingleObject(class btSerializer* serializer) const;
+
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btCollisionObjectDoubleData
+{
+	void					*m_broadphaseHandle;
+	void					*m_collisionShape;
+	btCollisionShapeData	*m_rootCollisionShape;
+	char					*m_name;
+
+	btTransformDoubleData	m_worldTransform;
+	btTransformDoubleData	m_interpolationWorldTransform;
+	btVector3DoubleData		m_interpolationLinearVelocity;
+	btVector3DoubleData		m_interpolationAngularVelocity;
+	btVector3DoubleData		m_anisotropicFriction;
+	double					m_contactProcessingThreshold;	
+	double					m_deactivationTime;
+	double					m_friction;
+	double					m_restitution;
+	double					m_hitFraction; 
+	double					m_ccdSweptSphereRadius;
+	double					m_ccdMotionThreshold;
+
+	int						m_hasAnisotropicFriction;
+	int						m_collisionFlags;
+	int						m_islandTag1;
+	int						m_companionId;
+	int						m_activationState1;
+	int						m_internalType;
+	int						m_checkCollideWith;
+
+	char	m_padding[4];
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btCollisionObjectFloatData
+{
+	void					*m_broadphaseHandle;
+	void					*m_collisionShape;
+	btCollisionShapeData	*m_rootCollisionShape;
+	char					*m_name;
+
+	btTransformFloatData	m_worldTransform;
+	btTransformFloatData	m_interpolationWorldTransform;
+	btVector3FloatData		m_interpolationLinearVelocity;
+	btVector3FloatData		m_interpolationAngularVelocity;
+	btVector3FloatData		m_anisotropicFriction;
+	float					m_contactProcessingThreshold;	
+	float					m_deactivationTime;
+	float					m_friction;
+	float					m_restitution;
+	float					m_hitFraction; 
+	float					m_ccdSweptSphereRadius;
+	float					m_ccdMotionThreshold;
+
+	int						m_hasAnisotropicFriction;
+	int						m_collisionFlags;
+	int						m_islandTag1;
+	int						m_companionId;
+	int						m_activationState1;
+	int						m_internalType;
+	int						m_checkCollideWith;
+};
+
+
+
+SIMD_FORCE_INLINE	int	btCollisionObject::calculateSerializeBufferSize() const
+{
+	return sizeof(btCollisionObjectData);
+}
+
+
+
+#endif //COLLISION_OBJECT_H

libs/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h

@@ -0,0 +1,509 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://bulletphysics.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.
+*/
+
+
+/**
+ * @mainpage Bullet Documentation
+ *
+ * @section intro_sec Introduction
+ * Bullet Collision Detection & Physics SDK
+ *
+ * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ).
+ *
+ * The main documentation is Bullet_User_Manual.pdf, included in the source code distribution.
+ * There is the Physics Forum for feedback and general Collision Detection and Physics discussions.
+ * Please visit http://www.bulletphysics.com
+ *
+ * @section install_sec Installation
+ *
+ * @subsection step1 Step 1: Download
+ * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list
+ *
+ * @subsection step2 Step 2: Building
+ * Bullet main build system for all platforms is cmake, you can download http://www.cmake.org
+ * cmake can autogenerate projectfiles for Microsoft Visual Studio, Apple Xcode, KDevelop and Unix Makefiles.
+ * The easiest is to run the CMake cmake-gui graphical user interface and choose the options and generate projectfiles.
+ * You can also use cmake in the command-line. Here are some examples for various platforms:
+ * cmake . -G "Visual Studio 9 2008"
+ * cmake . -G Xcode
+ * cmake . -G "Unix Makefiles"
+ * Although cmake is recommended, you can also use autotools for UNIX: ./autogen.sh ./configure to create a Makefile and then run make.
+ * 
+ * @subsection step3 Step 3: Testing demos
+ * Try to run and experiment with BasicDemo executable as a starting point.
+ * Bullet can be used in several ways, as Full Rigid Body simulation, as Collision Detector Library or Low Level / Snippets like the GJK Closest Point calculation.
+ * The Dependencies can be seen in this documentation under Directories
+ * 
+ * @subsection step4 Step 4: Integrating in your application, full Rigid Body and Soft Body simulation
+ * Check out BasicDemo how to create a btDynamicsWorld, btRigidBody and btCollisionShape, Stepping the simulation and synchronizing your graphics object transform.
+ * Check out SoftDemo how to use soft body dynamics, using btSoftRigidDynamicsWorld.
+ * @subsection step5 Step 5 : Integrate the Collision Detection Library (without Dynamics and other Extras)
+ * Bullet Collision Detection can also be used without the Dynamics/Extras.
+ * Check out btCollisionWorld and btCollisionObject, and the CollisionInterfaceDemo.
+ * @subsection step6 Step 6 : Use Snippets like the GJK Closest Point calculation.
+ * Bullet has been designed in a modular way keeping dependencies to a minimum. The ConvexHullDistance demo demonstrates direct use of btGjkPairDetector.
+ *
+ * @section copyright Copyright
+ * For up-to-data information and copyright and contributors list check out the Bullet_User_Manual.pdf
+ * 
+ */
+ 
+ 
+
+#ifndef COLLISION_WORLD_H
+#define COLLISION_WORLD_H
+
+class btStackAlloc;
+class btCollisionShape;
+class btConvexShape;
+class btBroadphaseInterface;
+class btSerializer;
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "btCollisionObject.h"
+#include "btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+///CollisionWorld is interface and container for the collision detection
+class btCollisionWorld
+{
+
+	
+protected:
+
+	btAlignedObjectArray<btCollisionObject*>	m_collisionObjects;
+	
+	btDispatcher*	m_dispatcher1;
+
+	btDispatcherInfo	m_dispatchInfo;
+
+	btStackAlloc*	m_stackAlloc;
+
+	btBroadphaseInterface*	m_broadphasePairCache;
+
+	btIDebugDraw*	m_debugDrawer;
+
+	///m_forceUpdateAllAabbs can be set to false as an optimization to only update active object AABBs
+	///it is true by default, because it is error-prone (setting the position of static objects wouldn't update their AABB)
+	bool m_forceUpdateAllAabbs;
+
+	void	serializeCollisionObjects(btSerializer* serializer);
+
+public:
+
+	//this constructor doesn't own the dispatcher and paircache/broadphase
+	btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration);
+
+	virtual ~btCollisionWorld();
+
+	void	setBroadphase(btBroadphaseInterface*	pairCache)
+	{
+		m_broadphasePairCache = pairCache;
+	}
+
+	const btBroadphaseInterface*	getBroadphase() const
+	{
+		return m_broadphasePairCache;
+	}
+
+	btBroadphaseInterface*	getBroadphase()
+	{
+		return m_broadphasePairCache;
+	}
+
+	btOverlappingPairCache*	getPairCache()
+	{
+		return m_broadphasePairCache->getOverlappingPairCache();
+	}
+
+
+	btDispatcher*	getDispatcher()
+	{
+		return m_dispatcher1;
+	}
+
+	const btDispatcher*	getDispatcher() const
+	{
+		return m_dispatcher1;
+	}
+
+	void	updateSingleAabb(btCollisionObject* colObj);
+
+	virtual void	updateAabbs();
+	
+	virtual void	setDebugDrawer(btIDebugDraw*	debugDrawer)
+	{
+			m_debugDrawer = debugDrawer;
+	}
+
+	virtual btIDebugDraw*	getDebugDrawer()
+	{
+		return m_debugDrawer;
+	}
+
+	virtual void	debugDrawWorld();
+
+	virtual void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
+
+
+	///LocalShapeInfo gives extra information for complex shapes
+	///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart
+	struct	LocalShapeInfo
+	{
+		int	m_shapePart;
+		int	m_triangleIndex;
+		
+		//const btCollisionShape*	m_shapeTemp;
+		//const btTransform*	m_shapeLocalTransform;
+	};
+
+	struct	LocalRayResult
+	{
+		LocalRayResult(btCollisionObject*	collisionObject, 
+			LocalShapeInfo*	localShapeInfo,
+			const btVector3&		hitNormalLocal,
+			btScalar hitFraction)
+		:m_collisionObject(collisionObject),
+		m_localShapeInfo(localShapeInfo),
+		m_hitNormalLocal(hitNormalLocal),
+		m_hitFraction(hitFraction)
+		{
+		}
+
+		btCollisionObject*		m_collisionObject;
+		LocalShapeInfo*			m_localShapeInfo;
+		btVector3				m_hitNormalLocal;
+		btScalar				m_hitFraction;
+
+	};
+
+	///RayResultCallback is used to report new raycast results
+	struct	RayResultCallback
+	{
+		btScalar	m_closestHitFraction;
+		btCollisionObject*		m_collisionObject;
+		short int	m_collisionFilterGroup;
+		short int	m_collisionFilterMask;
+      //@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback
+      unsigned int m_flags;
+
+		virtual ~RayResultCallback()
+		{
+		}
+		bool	hasHit() const
+		{
+			return (m_collisionObject != 0);
+		}
+
+		RayResultCallback()
+			:m_closestHitFraction(btScalar(1.)),
+			m_collisionObject(0),
+			m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
+			m_collisionFilterMask(btBroadphaseProxy::AllFilter),
+         //@BP Mod
+         m_flags(0)
+		{
+		}
+
+		virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+		{
+			bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
+			collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+			return collides;
+		}
+
+
+		virtual	btScalar	addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) = 0;
+	};
+
+	struct	ClosestRayResultCallback : public RayResultCallback
+	{
+		ClosestRayResultCallback(const btVector3&	rayFromWorld,const btVector3&	rayToWorld)
+		:m_rayFromWorld(rayFromWorld),
+		m_rayToWorld(rayToWorld)
+		{
+		}
+
+		btVector3	m_rayFromWorld;//used to calculate hitPointWorld from hitFraction
+		btVector3	m_rayToWorld;
+
+		btVector3	m_hitNormalWorld;
+		btVector3	m_hitPointWorld;
+			
+		virtual	btScalar	addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace)
+		{
+			//caller already does the filter on the m_closestHitFraction
+			btAssert(rayResult.m_hitFraction <= m_closestHitFraction);
+			
+			m_closestHitFraction = rayResult.m_hitFraction;
+			m_collisionObject = rayResult.m_collisionObject;
+			if (normalInWorldSpace)
+			{
+				m_hitNormalWorld = rayResult.m_hitNormalLocal;
+			} else
+			{
+				///need to transform normal into worldspace
+				m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal;
+			}
+			m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
+			return rayResult.m_hitFraction;
+		}
+	};
+
+	struct	AllHitsRayResultCallback : public RayResultCallback
+	{
+		AllHitsRayResultCallback(const btVector3&	rayFromWorld,const btVector3&	rayToWorld)
+		:m_rayFromWorld(rayFromWorld),
+		m_rayToWorld(rayToWorld)
+		{
+		}
+
+		btAlignedObjectArray<btCollisionObject*>		m_collisionObjects;
+
+		btVector3	m_rayFromWorld;//used to calculate hitPointWorld from hitFraction
+		btVector3	m_rayToWorld;
+
+		btAlignedObjectArray<btVector3>	m_hitNormalWorld;
+		btAlignedObjectArray<btVector3>	m_hitPointWorld;
+		btAlignedObjectArray<btScalar> m_hitFractions;
+			
+		virtual	btScalar	addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace)
+		{
+			m_collisionObject = rayResult.m_collisionObject;
+			m_collisionObjects.push_back(rayResult.m_collisionObject);
+			btVector3 hitNormalWorld;
+			if (normalInWorldSpace)
+			{
+				hitNormalWorld = rayResult.m_hitNormalLocal;
+			} else
+			{
+				///need to transform normal into worldspace
+				hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal;
+			}
+			m_hitNormalWorld.push_back(hitNormalWorld);
+			btVector3 hitPointWorld;
+			hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
+			m_hitPointWorld.push_back(hitPointWorld);
+			m_hitFractions.push_back(rayResult.m_hitFraction);
+			return m_closestHitFraction;
+		}
+	};
+
+
+	struct LocalConvexResult
+	{
+		LocalConvexResult(btCollisionObject*	hitCollisionObject, 
+			LocalShapeInfo*	localShapeInfo,
+			const btVector3&		hitNormalLocal,
+			const btVector3&		hitPointLocal,
+			btScalar hitFraction
+			)
+		:m_hitCollisionObject(hitCollisionObject),
+		m_localShapeInfo(localShapeInfo),
+		m_hitNormalLocal(hitNormalLocal),
+		m_hitPointLocal(hitPointLocal),
+		m_hitFraction(hitFraction)
+		{
+		}
+
+		btCollisionObject*		m_hitCollisionObject;
+		LocalShapeInfo*			m_localShapeInfo;
+		btVector3				m_hitNormalLocal;
+		btVector3				m_hitPointLocal;
+		btScalar				m_hitFraction;
+	};
+
+	///RayResultCallback is used to report new raycast results
+	struct	ConvexResultCallback
+	{
+		btScalar	m_closestHitFraction;
+		short int	m_collisionFilterGroup;
+		short int	m_collisionFilterMask;
+		
+		ConvexResultCallback()
+			:m_closestHitFraction(btScalar(1.)),
+			m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
+			m_collisionFilterMask(btBroadphaseProxy::AllFilter)
+		{
+		}
+
+		virtual ~ConvexResultCallback()
+		{
+		}
+		
+		bool	hasHit() const
+		{
+			return (m_closestHitFraction < btScalar(1.));
+		}
+
+		
+
+		virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+		{
+			bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
+			collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+			return collides;
+		}
+
+		virtual	btScalar	addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) = 0;
+	};
+
+	struct	ClosestConvexResultCallback : public ConvexResultCallback
+	{
+		ClosestConvexResultCallback(const btVector3&	convexFromWorld,const btVector3&	convexToWorld)
+		:m_convexFromWorld(convexFromWorld),
+		m_convexToWorld(convexToWorld),
+		m_hitCollisionObject(0)
+		{
+		}
+
+		btVector3	m_convexFromWorld;//used to calculate hitPointWorld from hitFraction
+		btVector3	m_convexToWorld;
+
+		btVector3	m_hitNormalWorld;
+		btVector3	m_hitPointWorld;
+		btCollisionObject*	m_hitCollisionObject;
+		
+		virtual	btScalar	addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace)
+		{
+//caller already does the filter on the m_closestHitFraction
+			btAssert(convexResult.m_hitFraction <= m_closestHitFraction);
+						
+			m_closestHitFraction = convexResult.m_hitFraction;
+			m_hitCollisionObject = convexResult.m_hitCollisionObject;
+			if (normalInWorldSpace)
+			{
+				m_hitNormalWorld = convexResult.m_hitNormalLocal;
+			} else
+			{
+				///need to transform normal into worldspace
+				m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
+			}
+			m_hitPointWorld = convexResult.m_hitPointLocal;
+			return convexResult.m_hitFraction;
+		}
+	};
+
+	///ContactResultCallback is used to report contact points
+	struct	ContactResultCallback
+	{
+		short int	m_collisionFilterGroup;
+		short int	m_collisionFilterMask;
+		
+		ContactResultCallback()
+			:m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
+			m_collisionFilterMask(btBroadphaseProxy::AllFilter)
+		{
+		}
+
+		virtual ~ContactResultCallback()
+		{
+		}
+		
+		virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+		{
+			bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
+			collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+			return collides;
+		}
+
+		virtual	btScalar	addSingleResult(btManifoldPoint& cp,	const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1) = 0;
+	};
+
+
+
+	int	getNumCollisionObjects() const
+	{
+		return int(m_collisionObjects.size());
+	}
+
+	/// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback
+	/// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback.
+	virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; 
+
+	/// convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback
+	/// This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
+	void    convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback,  btScalar allowedCcdPenetration = btScalar(0.)) const;
+
+	///contactTest performs a discrete collision test between colObj against all objects in the btCollisionWorld, and calls the resultCallback.
+	///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
+	void	contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback);
+
+	///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
+	///it reports one or more contact points (including the one with deepest penetration)
+	void	contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback);
+
+
+	/// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
+	/// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape.
+	/// This allows more customization.
+	static void	rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
+					  btCollisionObject* collisionObject,
+					  const btCollisionShape* collisionShape,
+					  const btTransform& colObjWorldTransform,
+					  RayResultCallback& resultCallback);
+
+	/// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
+	static void	objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans,const btTransform& rayToTrans,
+					  btCollisionObject* collisionObject,
+					  const btCollisionShape* collisionShape,
+					  const btTransform& colObjWorldTransform,
+					  ConvexResultCallback& resultCallback, btScalar	allowedPenetration);
+
+	virtual void	addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter);
+
+	btCollisionObjectArray& getCollisionObjectArray()
+	{
+		return m_collisionObjects;
+	}
+
+	const btCollisionObjectArray& getCollisionObjectArray() const
+	{
+		return m_collisionObjects;
+	}
+
+
+	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
+
+	virtual void	performDiscreteCollisionDetection();
+
+	btDispatcherInfo& getDispatchInfo()
+	{
+		return m_dispatchInfo;
+	}
+
+	const btDispatcherInfo& getDispatchInfo() const
+	{
+		return m_dispatchInfo;
+	}
+	
+	bool	getForceUpdateAllAabbs() const
+	{
+		return m_forceUpdateAllAabbs;
+	}
+	void setForceUpdateAllAabbs( bool forceUpdateAllAabbs)
+	{
+		m_forceUpdateAllAabbs = forceUpdateAllAabbs;
+	}
+
+	///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (Bullet/Demos/SerializeDemo)
+	virtual	void	serialize(btSerializer* serializer);
+
+};
+
+
+#endif //COLLISION_WORLD_H

libs/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

@@ -0,0 +1,353 @@
+/*
+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 "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "btManifoldResult.h"
+
+btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
+:btActivatingCollisionAlgorithm(ci,body0,body1),
+m_isSwapped(isSwapped),
+m_sharedManifold(ci.m_manifold)
+{
+	m_ownsManifold = false;
+
+	btCollisionObject* colObj = m_isSwapped? body1 : body0;
+	btAssert (colObj->getCollisionShape()->isCompound());
+	
+	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+	m_compoundShapeRevision = compoundShape->getUpdateRevision();
+	
+	preallocateChildAlgorithms(body0,body1);
+}
+
+void	btCompoundCollisionAlgorithm::preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1)
+{
+	btCollisionObject* colObj = m_isSwapped? body1 : body0;
+	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+	btAssert (colObj->getCollisionShape()->isCompound());
+	
+	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+
+	int numChildren = compoundShape->getNumChildShapes();
+	int i;
+	
+	m_childCollisionAlgorithms.resize(numChildren);
+	for (i=0;i<numChildren;i++)
+	{
+		if (compoundShape->getDynamicAabbTree())
+		{
+			m_childCollisionAlgorithms[i] = 0;
+		} else
+		{
+			btCollisionShape* tmpShape = colObj->getCollisionShape();
+			btCollisionShape* childShape = compoundShape->getChildShape(i);
+			colObj->internalSetTemporaryCollisionShape( childShape );
+			m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(colObj,otherObj,m_sharedManifold);
+			colObj->internalSetTemporaryCollisionShape( tmpShape );
+		}
+	}
+}
+
+void	btCompoundCollisionAlgorithm::removeChildAlgorithms()
+{
+	int numChildren = m_childCollisionAlgorithms.size();
+	int i;
+	for (i=0;i<numChildren;i++)
+	{
+		if (m_childCollisionAlgorithms[i])
+		{
+			m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
+			m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
+		}
+	}
+}
+
+btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
+{
+	removeChildAlgorithms();
+}
+
+
+
+
+struct	btCompoundLeafCallback : btDbvt::ICollide
+{
+
+public:
+
+	btCollisionObject* m_compoundColObj;
+	btCollisionObject* m_otherObj;
+	btDispatcher* m_dispatcher;
+	const btDispatcherInfo& m_dispatchInfo;
+	btManifoldResult*	m_resultOut;
+	btCollisionAlgorithm**	m_childCollisionAlgorithms;
+	btPersistentManifold*	m_sharedManifold;
+
+
+
+
+	btCompoundLeafCallback (btCollisionObject* compoundObj,btCollisionObject* otherObj,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult*	resultOut,btCollisionAlgorithm**	childCollisionAlgorithms,btPersistentManifold*	sharedManifold)
+		:m_compoundColObj(compoundObj),m_otherObj(otherObj),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
+		m_childCollisionAlgorithms(childCollisionAlgorithms),
+		m_sharedManifold(sharedManifold)
+	{
+
+	}
+
+
+	void	ProcessChildShape(btCollisionShape* childShape,int index)
+	{
+		btAssert(index>=0);
+		btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
+		btAssert(index<compoundShape->getNumChildShapes());
+
+
+		//backup
+		btTransform	orgTrans = m_compoundColObj->getWorldTransform();
+		btTransform	orgInterpolationTrans = m_compoundColObj->getInterpolationWorldTransform();
+		const btTransform& childTrans = compoundShape->getChildTransform(index);
+		btTransform	newChildWorldTrans = orgTrans*childTrans ;
+
+		//perform an AABB check first
+		btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
+		childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
+		m_otherObj->getCollisionShape()->getAabb(m_otherObj->getWorldTransform(),aabbMin1,aabbMax1);
+
+		if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+		{
+
+			m_compoundColObj->setWorldTransform( newChildWorldTrans);
+			m_compoundColObj->setInterpolationWorldTransform(newChildWorldTrans);
+
+			//the contactpoint is still projected back using the original inverted worldtrans
+			btCollisionShape* tmpShape = m_compoundColObj->getCollisionShape();
+			m_compoundColObj->internalSetTemporaryCollisionShape( childShape );
+
+			if (!m_childCollisionAlgorithms[index])
+				m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold);
+
+			///detect swapping case
+			if (m_resultOut->getBody0Internal() == m_compoundColObj)
+			{
+				m_resultOut->setShapeIdentifiersA(-1,index);
+			} else
+			{
+				m_resultOut->setShapeIdentifiersB(-1,index);
+			}
+
+			m_childCollisionAlgorithms[index]->processCollision(m_compoundColObj,m_otherObj,m_dispatchInfo,m_resultOut);
+			if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+			{
+				btVector3 worldAabbMin,worldAabbMax;
+				m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1));
+				m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1));
+			}
+			
+			//revert back transform
+			m_compoundColObj->internalSetTemporaryCollisionShape( tmpShape);
+			m_compoundColObj->setWorldTransform(  orgTrans );
+			m_compoundColObj->setInterpolationWorldTransform(orgInterpolationTrans);
+		}
+	}
+	void		Process(const btDbvtNode* leaf)
+	{
+		int index = leaf->dataAsInt;
+
+		btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
+		btCollisionShape* childShape = compoundShape->getChildShape(index);
+		if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+		{
+			btVector3 worldAabbMin,worldAabbMax;
+			btTransform	orgTrans = m_compoundColObj->getWorldTransform();
+			btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
+			m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
+		}
+		ProcessChildShape(childShape,index);
+
+	}
+};
+
+
+
+
+
+
+void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	btCollisionObject* colObj = m_isSwapped? body1 : body0;
+	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+
+	
+
+	btAssert (colObj->getCollisionShape()->isCompound());
+	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+
+	///btCompoundShape might have changed:
+	////make sure the internal child collision algorithm caches are still valid
+	if (compoundShape->getUpdateRevision() != m_compoundShapeRevision)
+	{
+		///clear and update all
+		removeChildAlgorithms();
+		
+		preallocateChildAlgorithms(body0,body1);
+	}
+
+
+	btDbvt* tree = compoundShape->getDynamicAabbTree();
+	//use a dynamic aabb tree to cull potential child-overlaps
+	btCompoundLeafCallback  callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
+
+	///we need to refresh all contact manifolds
+	///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
+	///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
+	{
+		int i;
+		btManifoldArray manifoldArray;
+		for (i=0;i<m_childCollisionAlgorithms.size();i++)
+		{
+			if (m_childCollisionAlgorithms[i])
+			{
+				m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
+				for (int m=0;m<manifoldArray.size();m++)
+				{
+					if (manifoldArray[m]->getNumContacts())
+					{
+						resultOut->setPersistentManifold(manifoldArray[m]);
+						resultOut->refreshContactPoints();
+						resultOut->setPersistentManifold(0);//??necessary?
+					}
+				}
+				manifoldArray.clear();
+			}
+		}
+	}
+
+	if (tree)
+	{
+
+		btVector3 localAabbMin,localAabbMax;
+		btTransform otherInCompoundSpace;
+		otherInCompoundSpace = colObj->getWorldTransform().inverse() * otherObj->getWorldTransform();
+		otherObj->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
+
+		const ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+		//process all children, that overlap with  the given AABB bounds
+		tree->collideTV(tree->m_root,bounds,callback);
+
+	} else
+	{
+		//iterate over all children, perform an AABB check inside ProcessChildShape
+		int numChildren = m_childCollisionAlgorithms.size();
+		int i;
+		for (i=0;i<numChildren;i++)
+		{
+			callback.ProcessChildShape(compoundShape->getChildShape(i),i);
+		}
+	}
+
+	{
+				//iterate over all children, perform an AABB check inside ProcessChildShape
+		int numChildren = m_childCollisionAlgorithms.size();
+		int i;
+		btManifoldArray	manifoldArray;
+        btCollisionShape* childShape = 0;
+        btTransform	orgTrans;
+        btTransform	orgInterpolationTrans;
+        btTransform	newChildWorldTrans;
+        btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;        
+        
+		for (i=0;i<numChildren;i++)
+		{
+			if (m_childCollisionAlgorithms[i])
+			{
+				childShape = compoundShape->getChildShape(i);
+			//if not longer overlapping, remove the algorithm
+                orgTrans = colObj->getWorldTransform();
+                orgInterpolationTrans = colObj->getInterpolationWorldTransform();
+				const btTransform& childTrans = compoundShape->getChildTransform(i);
+                newChildWorldTrans = orgTrans*childTrans ;
+
+				//perform an AABB check first
+				childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
+				otherObj->getCollisionShape()->getAabb(otherObj->getWorldTransform(),aabbMin1,aabbMax1);
+
+				if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+				{
+					m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
+					m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
+					m_childCollisionAlgorithms[i] = 0;
+				}
+			}
+		}
+	}
+}
+
+btScalar	btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+	btCollisionObject* colObj = m_isSwapped? body1 : body0;
+	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+
+	btAssert (colObj->getCollisionShape()->isCompound());
+	
+	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+
+	//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
+	//If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
+	//given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
+	//determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
+	//then use each overlapping node AABB against Tree0
+	//and vise versa.
+
+	btScalar hitFraction = btScalar(1.);
+
+	int numChildren = m_childCollisionAlgorithms.size();
+	int i;
+    btTransform	orgTrans;
+    btScalar frac;
+	for (i=0;i<numChildren;i++)
+	{
+		//temporarily exchange parent btCollisionShape with childShape, and recurse
+		btCollisionShape* childShape = compoundShape->getChildShape(i);
+
+		//backup
+        orgTrans = colObj->getWorldTransform();
+	
+		const btTransform& childTrans = compoundShape->getChildTransform(i);
+		//btTransform	newChildWorldTrans = orgTrans*childTrans ;
+		colObj->setWorldTransform( orgTrans*childTrans );
+
+		btCollisionShape* tmpShape = colObj->getCollisionShape();
+		colObj->internalSetTemporaryCollisionShape( childShape );
+        frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
+		if (frac<hitFraction)
+		{
+			hitFraction = frac;
+		}
+		//revert back
+		colObj->internalSetTemporaryCollisionShape( tmpShape);
+		colObj->setWorldTransform( orgTrans);
+	}
+	return hitFraction;
+
+}
+
+
+

libs/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.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 COMPOUND_COLLISION_ALGORITHM_H
+#define COMPOUND_COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btDispatcher;
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "btCollisionCreateFunc.h"
+#include "LinearMath/btAlignedObjectArray.h"
+class btDispatcher;
+class btCollisionObject;
+
+/// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
+class btCompoundCollisionAlgorithm  : public btActivatingCollisionAlgorithm
+{
+	btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
+	bool m_isSwapped;
+
+	class btPersistentManifold*	m_sharedManifold;
+	bool					m_ownsManifold;
+
+	int	m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated
+	
+	void	removeChildAlgorithms();
+	
+	void	preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1);
+
+public:
+
+	btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+	virtual ~btCompoundCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		int i;
+		for (i=0;i<m_childCollisionAlgorithms.size();i++)
+		{
+			if (m_childCollisionAlgorithms[i])
+				m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
+		}
+	}
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm));
+			return new(mem) btCompoundCollisionAlgorithm(ci,body0,body1,false);
+		}
+	};
+
+	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm));
+			return new(mem) btCompoundCollisionAlgorithm(ci,body0,body1,true);
+		}
+	};
+
+};
+
+#endif //COMPOUND_COLLISION_ALGORITHM_H

libs/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp

@@ -0,0 +1,247 @@
+/*
+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 "btConvex2dConvex2dAlgorithm.h"
+
+//#include <stdio.h>
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+
+
+
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+
+
+btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+{
+	m_numPerturbationIterations = 0;
+	m_minimumPointsPerturbationThreshold = 3;
+	m_simplexSolver = simplexSolver;
+	m_pdSolver = pdSolver;
+}
+
+btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() 
+{ 
+}
+
+btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+: btActivatingCollisionAlgorithm(ci,body0,body1),
+m_simplexSolver(simplexSolver),
+m_pdSolver(pdSolver),
+m_ownManifold (false),
+m_manifoldPtr(mf),
+m_lowLevelOfDetail(false),
+ m_numPerturbationIterations(numPerturbationIterations),
+m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
+{
+	(void)body0;
+	(void)body1;
+}
+
+
+
+
+btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void	btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
+{
+	m_lowLevelOfDetail = useLowLevel;
+}
+
+
+
+extern btScalar gContactBreakingThreshold;
+
+
+//
+// Convex-Convex collision algorithm
+//
+void btConvex2dConvex2dAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+	if (!m_manifoldPtr)
+	{
+		//swapped?
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
+		m_ownManifold = true;
+	}
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
+	//comment-out next line to test multi-contact generation
+	//resultOut->getPersistentManifold()->clearManifold();
+
+
+	btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
+	btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
+
+	btVector3  normalOnB;
+	btVector3  pointOnBWorld;
+
+	{
+
+
+		btGjkPairDetector::ClosestPointInput input;
+
+		btGjkPairDetector	gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
+		//TODO: if (dispatchInfo.m_useContinuous)
+		gjkPairDetector.setMinkowskiA(min0);
+		gjkPairDetector.setMinkowskiB(min1);
+
+		{
+			input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
+			input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
+		}
+
+		input.m_stackAlloc = dispatchInfo.m_stackAllocator;
+		input.m_transformA = body0->getWorldTransform();
+		input.m_transformB = body1->getWorldTransform();
+
+		gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+
+		btVector3 v0,v1;
+		btVector3 sepNormalWorldSpace;
+
+	}
+
+	if (m_ownManifold)
+	{
+		resultOut->refreshContactPoints();
+	}
+
+}
+
+
+
+
+btScalar	btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
+
+	///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
+	///col0->m_worldTransform,
+	btScalar resultFraction = btScalar(1.);
+
+
+	btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
+	btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
+
+	if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
+		squareMot1 < col1->getCcdSquareMotionThreshold())
+		return resultFraction;
+
+
+	//An adhoc way of testing the Continuous Collision Detection algorithms
+	//One object is approximated as a sphere, to simplify things
+	//Starting in penetration should report no time of impact
+	//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
+	//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
+
+
+	/// Convex0 against sphere for Convex1
+	{
+		btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
+
+		btSphereShape	sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+		btConvexCast::CastResult result;
+		btVoronoiSimplexSolver voronoiSimplex;
+		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+		///Simplification, one object is simplified as a sphere
+		btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
+		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+			col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+		{
+
+			//store result.m_fraction in both bodies
+
+			if (col0->getHitFraction()> result.m_fraction)
+				col0->setHitFraction( result.m_fraction );
+
+			if (col1->getHitFraction() > result.m_fraction)
+				col1->setHitFraction( result.m_fraction);
+
+			if (resultFraction > result.m_fraction)
+				resultFraction = result.m_fraction;
+
+		}
+
+
+
+
+	}
+
+	/// Sphere (for convex0) against Convex1
+	{
+		btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
+
+		btSphereShape	sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+		btConvexCast::CastResult result;
+		btVoronoiSimplexSolver voronoiSimplex;
+		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+		///Simplification, one object is simplified as a sphere
+		btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
+		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+			col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+		{
+
+			//store result.m_fraction in both bodies
+
+			if (col0->getHitFraction()	> result.m_fraction)
+				col0->setHitFraction( result.m_fraction);
+
+			if (col1->getHitFraction() > result.m_fraction)
+				col1->setHitFraction( result.m_fraction);
+
+			if (resultFraction > result.m_fraction)
+				resultFraction = result.m_fraction;
+
+		}
+	}
+
+	return resultFraction;
+
+}
+

libs/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h

@@ -0,0 +1,95 @@
+/*
+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_2D_CONVEX_2D_ALGORITHM_H
+#define CONVEX_2D_CONVEX_2D_ALGORITHM_H
+
+#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
+
+class btConvexPenetrationDepthSolver;
+
+
+///The convex2dConvex2dAlgorithm collision algorithm support 2d collision detection for btConvex2dShape
+///Currently it requires the btMinkowskiPenetrationDepthSolver, it has support for 2d penetration depth computation
+class btConvex2dConvex2dAlgorithm : public btActivatingCollisionAlgorithm
+{
+	btSimplexSolverInterface*		m_simplexSolver;
+	btConvexPenetrationDepthSolver* m_pdSolver;
+
+	
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool			m_lowLevelOfDetail;
+	
+	int m_numPerturbationIterations;
+	int m_minimumPointsPerturbationThreshold;
+
+public:
+
+	btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
+
+
+	virtual ~btConvex2dConvex2dAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		///should we use m_ownManifold to avoid adding duplicates?
+		if (m_manifoldPtr && m_ownManifold)
+			manifoldArray.push_back(m_manifoldPtr);
+	}
+
+
+	void	setLowLevelOfDetail(bool useLowLevel);
+
+
+	const btPersistentManifold*	getManifold()
+	{
+		return m_manifoldPtr;
+	}
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+
+		btConvexPenetrationDepthSolver*		m_pdSolver;
+		btSimplexSolverInterface*			m_simplexSolver;
+		int m_numPerturbationIterations;
+		int m_minimumPointsPerturbationThreshold;
+
+		CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+		
+		virtual ~CreateFunc();
+
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvex2dConvex2dAlgorithm));
+			return new(mem) btConvex2dConvex2dAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+		}
+	};
+
+
+};
+
+#endif //CONVEX_2D_CONVEX_2D_ALGORITHM_H

libs/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -0,0 +1,321 @@
+/*
+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 "btConvexConcaveCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionShapes/btConcaveShape.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+
+btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
+: btActivatingCollisionAlgorithm(ci,body0,body1),
+m_isSwapped(isSwapped),
+m_btConvexTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped)
+{
+}
+
+btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
+{
+}
+
+void	btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray&	manifoldArray)
+{
+	if (m_btConvexTriangleCallback.m_manifoldPtr)
+	{
+		manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr);
+	}
+}
+
+
+btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher*  dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
+	  m_dispatcher(dispatcher),
+	m_dispatchInfoPtr(0)
+{
+	m_convexBody = isSwapped? body1:body0;
+	m_triBody = isSwapped? body0:body1;
+	
+	  //
+	  // create the manifold from the dispatcher 'manifold pool'
+	  //
+	  m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
+
+  	  clearCache();
+}
+
+btConvexTriangleCallback::~btConvexTriangleCallback()
+{
+	clearCache();
+	m_dispatcher->releaseManifold( m_manifoldPtr );
+  
+}
+  
+
+void	btConvexTriangleCallback::clearCache()
+{
+	m_dispatcher->clearManifold(m_manifoldPtr);
+}
+
+
+
+void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
+{
+ 
+	//just for debugging purposes
+	//printf("triangle %d",m_triangleCount++);
+
+
+	//aabb filter is already applied!	
+
+	btCollisionAlgorithmConstructionInfo ci;
+	ci.m_dispatcher1 = m_dispatcher;
+
+	btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
+
+
+	
+	///debug drawing of the overlapping triangles
+	if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
+	{
+		btVector3 color(1,1,0);
+		btTransform& tr = ob->getWorldTransform();
+		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
+		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
+		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
+
+		//btVector3 center = triangle[0] + triangle[1]+triangle[2];
+		//center *= btScalar(0.333333);
+		//m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color);
+		//m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color);
+		//m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color);
+
+	}
+
+
+	//btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
+	
+	if (m_convexBody->getCollisionShape()->isConvex())
+	{
+		btTriangleShape tm(triangle[0],triangle[1],triangle[2]);	
+		tm.setMargin(m_collisionMarginTriangle);
+		
+		btCollisionShape* tmpShape = ob->getCollisionShape();
+		ob->internalSetTemporaryCollisionShape( &tm );
+		
+		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr);
+
+		if (m_resultOut->getBody0Internal() == m_triBody)
+		{
+			m_resultOut->setShapeIdentifiersA(partId,triangleIndex);
+		}
+		else
+		{
+			m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
+		}
+	
+		colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+		colAlgo->~btCollisionAlgorithm();
+		ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
+		ob->internalSetTemporaryCollisionShape( tmpShape);
+	}
+
+
+}
+
+
+
+void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	m_dispatchInfoPtr = &dispatchInfo;
+	m_collisionMarginTriangle = collisionMarginTriangle;
+	m_resultOut = resultOut;
+
+	//recalc aabbs
+	btTransform convexInTriangleSpace;
+	convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * m_convexBody->getWorldTransform();
+	btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->getCollisionShape());
+	//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
+	convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
+	btScalar extraMargin = collisionMarginTriangle;
+	btVector3 extra(extraMargin,extraMargin,extraMargin);
+
+	m_aabbMax += extra;
+	m_aabbMin -= extra;
+	
+}
+
+void btConvexConcaveCollisionAlgorithm::clearCache()
+{
+	m_btConvexTriangleCallback.clearCache();
+
+}
+
+void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	
+	
+	btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
+	btCollisionObject* triBody = m_isSwapped ? body0 : body1;
+
+	if (triBody->getCollisionShape()->isConcave())
+	{
+
+
+		btCollisionObject*	triOb = triBody;
+		btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
+		
+		if (convexBody->getCollisionShape()->isConvex())
+		{
+			btScalar collisionMarginTriangle = concaveShape->getMargin();
+					
+			resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+			m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
+
+			//Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
+			//m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+
+			m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
+
+			concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
+			
+			resultOut->refreshContactPoints();
+	
+		}
+	
+	}
+
+}
+
+
+btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
+	btCollisionObject* triBody = m_isSwapped ? body0 : body1;
+
+
+	//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
+
+	//only perform CCD above a certain threshold, this prevents blocking on the long run
+	//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
+	btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
+	if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
+	{
+		return btScalar(1.);
+	}
+
+	//const btVector3& from = convexbody->m_worldTransform.getOrigin();
+	//btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
+	//todo: only do if the motion exceeds the 'radius'
+
+	btTransform triInv = triBody->getWorldTransform().inverse();
+	btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
+	btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
+
+	struct LocalTriangleSphereCastCallback	: public btTriangleCallback
+	{
+		btTransform m_ccdSphereFromTrans;
+		btTransform m_ccdSphereToTrans;
+		btTransform	m_meshTransform;
+
+		btScalar	m_ccdSphereRadius;
+		btScalar	m_hitFraction;
+	
+
+		LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
+			:m_ccdSphereFromTrans(from),
+			m_ccdSphereToTrans(to),
+			m_ccdSphereRadius(ccdSphereRadius),
+			m_hitFraction(hitFraction)
+		{			
+		}
+		
+		
+		virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+		{
+			(void)partId;
+			(void)triangleIndex;
+			//do a swept sphere for now
+			btTransform ident;
+			ident.setIdentity();
+			btConvexCast::CastResult castResult;
+			castResult.m_fraction = m_hitFraction;
+			btSphereShape	pointShape(m_ccdSphereRadius);
+			btTriangleShape	triShape(triangle[0],triangle[1],triangle[2]);
+			btVoronoiSimplexSolver	simplexSolver;
+			btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
+			//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
+			//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
+			//local space?
+
+			if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
+				ident,ident,castResult))
+			{
+				if (m_hitFraction > castResult.m_fraction)
+					m_hitFraction = castResult.m_fraction;
+			}
+
+		}
+
+	};
+
+
+	
+
+	
+	if (triBody->getCollisionShape()->isConcave())
+	{
+		btVector3 rayAabbMin = convexFromLocal.getOrigin();
+		rayAabbMin.setMin(convexToLocal.getOrigin());
+		btVector3 rayAabbMax = convexFromLocal.getOrigin();
+		rayAabbMax.setMax(convexToLocal.getOrigin());
+		btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
+		rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+		rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+
+		btScalar curHitFraction = btScalar(1.); //is this available?
+		LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
+			convexbody->getCcdSweptSphereRadius(),curHitFraction);
+
+		raycastCallback.m_hitFraction = convexbody->getHitFraction();
+
+		btCollisionObject* concavebody = triBody;
+
+		btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
+		
+		if (triangleMesh)
+		{
+			triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
+		}
+	
+
+
+		if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
+		{
+			convexbody->setHitFraction( raycastCallback.m_hitFraction);
+			return raycastCallback.m_hitFraction;
+		}
+	}
+
+	return btScalar(1.);
+
+}

libs/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h

@@ -0,0 +1,116 @@
+/*
+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_CONCAVE_COLLISION_ALGORITHM_H
+#define CONVEX_CONCAVE_COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btDispatcher;
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "btCollisionCreateFunc.h"
+
+///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called.
+class btConvexTriangleCallback : public btTriangleCallback
+{
+	btCollisionObject* m_convexBody;
+	btCollisionObject* m_triBody;
+
+	btVector3	m_aabbMin;
+	btVector3	m_aabbMax ;
+
+
+	btManifoldResult* m_resultOut;
+	btDispatcher*	m_dispatcher;
+	const btDispatcherInfo* m_dispatchInfoPtr;
+	btScalar m_collisionMarginTriangle;
+	
+public:
+int	m_triangleCount;
+	
+	btPersistentManifold*	m_manifoldPtr;
+
+	btConvexTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+	void	setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual ~btConvexTriangleCallback();
+
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
+	
+	void clearCache();
+
+	SIMD_FORCE_INLINE const btVector3& getAabbMin() const
+	{
+		return m_aabbMin;
+	}
+	SIMD_FORCE_INLINE const btVector3& getAabbMax() const
+	{
+		return m_aabbMax;
+	}
+
+};
+
+
+
+
+/// btConvexConcaveCollisionAlgorithm  supports collision between convex shapes and (concave) trianges meshes.
+class btConvexConcaveCollisionAlgorithm  : public btActivatingCollisionAlgorithm
+{
+
+	bool	m_isSwapped;
+
+	btConvexTriangleCallback m_btConvexTriangleCallback;
+
+
+
+public:
+
+	btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+	virtual ~btConvexConcaveCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray);
+	
+	void	clearCache();
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm));
+			return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0,body1,false);
+		}
+	};
+
+	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm));
+			return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0,body1,true);
+		}
+	};
+
+};
+
+#endif //CONVEX_CONCAVE_COLLISION_ALGORITHM_H

libs/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -0,0 +1,580 @@
+/*
+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.
+*/
+
+///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance
+///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums
+///with reproduction case
+//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
+
+#include "btConvexConvexAlgorithm.h"
+
+//#include <stdio.h>
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+
+
+
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+
+
+
+///////////
+
+
+
+static SIMD_FORCE_INLINE void segmentsClosestPoints(
+	btVector3& ptsVector,
+	btVector3& offsetA,
+	btVector3& offsetB,
+	btScalar& tA, btScalar& tB,
+	const btVector3& translation,
+	const btVector3& dirA, btScalar hlenA,
+	const btVector3& dirB, btScalar hlenB )
+{
+	// compute the parameters of the closest points on each line segment
+
+	btScalar dirA_dot_dirB = btDot(dirA,dirB);
+	btScalar dirA_dot_trans = btDot(dirA,translation);
+	btScalar dirB_dot_trans = btDot(dirB,translation);
+
+	btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
+
+	if ( denom == 0.0f ) {
+		tA = 0.0f;
+	} else {
+		tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	}
+
+	tB = tA * dirA_dot_dirB - dirB_dot_trans;
+
+	if ( tB < -hlenB ) {
+		tB = -hlenB;
+		tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	} else if ( tB > hlenB ) {
+		tB = hlenB;
+		tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	}
+
+	// compute the closest points relative to segment centers.
+
+	offsetA = dirA * tA;
+	offsetB = dirB * tB;
+
+	ptsVector = translation - offsetA + offsetB;
+}
+
+
+static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
+	btVector3& normalOnB,
+	btVector3& pointOnB,
+	btScalar capsuleLengthA,
+	btScalar	capsuleRadiusA,
+	btScalar capsuleLengthB,
+	btScalar	capsuleRadiusB,
+	int capsuleAxisA,
+	int capsuleAxisB,
+	const btTransform& transformA,
+	const btTransform& transformB,
+	btScalar distanceThreshold )
+{
+	btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA);
+	btVector3 translationA = transformA.getOrigin();
+	btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB);
+	btVector3 translationB = transformB.getOrigin();
+
+	// translation between centers
+
+	btVector3 translation = translationB - translationA;
+
+	// compute the closest points of the capsule line segments
+
+	btVector3 ptsVector;           // the vector between the closest points
+	
+	btVector3 offsetA, offsetB;    // offsets from segment centers to their closest points
+	btScalar tA, tB;              // parameters on line segment
+
+	segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
+						   directionA, capsuleLengthA, directionB, capsuleLengthB );
+
+	btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
+
+	if ( distance > distanceThreshold )
+		return distance;
+
+	btScalar lenSqr = ptsVector.length2();
+	if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON))
+	{
+		//degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
+		btVector3 q;
+		btPlaneSpace1(directionA,normalOnB,q);
+	} else
+	{
+		// compute the contact normal
+		normalOnB = ptsVector*-btRecipSqrt(lenSqr);
+	}
+	pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB;
+
+	return distance;
+}
+
+
+
+
+
+
+
+//////////
+
+
+
+
+
+btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+{
+	m_numPerturbationIterations = 0;
+	m_minimumPointsPerturbationThreshold = 3;
+	m_simplexSolver = simplexSolver;
+	m_pdSolver = pdSolver;
+}
+
+btConvexConvexAlgorithm::CreateFunc::~CreateFunc() 
+{ 
+}
+
+btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+: btActivatingCollisionAlgorithm(ci,body0,body1),
+m_simplexSolver(simplexSolver),
+m_pdSolver(pdSolver),
+m_ownManifold (false),
+m_manifoldPtr(mf),
+m_lowLevelOfDetail(false),
+#ifdef USE_SEPDISTANCE_UTIL2
+m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
+			  (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
+#endif
+m_numPerturbationIterations(numPerturbationIterations),
+m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
+{
+	(void)body0;
+	(void)body1;
+}
+
+
+
+
+btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void	btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
+{
+	m_lowLevelOfDetail = useLowLevel;
+}
+
+
+struct btPerturbedContactResult : public btManifoldResult
+{
+	btManifoldResult* m_originalManifoldResult;
+	btTransform m_transformA;
+	btTransform m_transformB;
+	btTransform	m_unPerturbedTransform;
+	bool	m_perturbA;
+	btIDebugDraw*	m_debugDrawer;
+
+
+	btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer)
+		:m_originalManifoldResult(originalResult),
+		m_transformA(transformA),
+		m_transformB(transformB),
+		m_unPerturbedTransform(unPerturbedTransform),
+		m_perturbA(perturbA),
+		m_debugDrawer(debugDrawer)
+	{
+	}
+	virtual ~ btPerturbedContactResult()
+	{
+	}
+
+	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
+	{
+		btVector3 endPt,startPt;
+		btScalar newDepth;
+		btVector3 newNormal;
+
+		if (m_perturbA)
+		{
+			btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
+			endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
+			newDepth = (endPt -  pointInWorld).dot(normalOnBInWorld);
+			startPt = endPt+normalOnBInWorld*newDepth;
+		} else
+		{
+			endPt = pointInWorld + normalOnBInWorld*orgDepth;
+			startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
+			newDepth = (endPt -  startPt).dot(normalOnBInWorld);
+			
+		}
+
+//#define DEBUG_CONTACTS 1
+#ifdef DEBUG_CONTACTS
+		m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0));
+		m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0));
+		m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1));
+#endif //DEBUG_CONTACTS
+
+		
+		m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
+	}
+
+};
+
+extern btScalar gContactBreakingThreshold;
+
+
+//
+// Convex-Convex collision algorithm
+//
+void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+	if (!m_manifoldPtr)
+	{
+		//swapped?
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
+		m_ownManifold = true;
+	}
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
+	//comment-out next line to test multi-contact generation
+	//resultOut->getPersistentManifold()->clearManifold();
+	
+
+	btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
+	btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
+
+	btVector3  normalOnB;
+		btVector3  pointOnBWorld;
+#ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
+	if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
+	{
+		btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
+		btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
+		btVector3 localScalingA = capsuleA->getLocalScaling();
+		btVector3 localScalingB = capsuleB->getLocalScaling();
+		
+		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+
+		btScalar dist = capsuleCapsuleDistance(normalOnB,	pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
+			capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
+			body0->getWorldTransform(),body1->getWorldTransform(),threshold);
+
+		if (dist<threshold)
+		{
+			btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
+			resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);	
+		}
+		resultOut->refreshContactPoints();
+		return;
+	}
+#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
+
+
+#ifdef USE_SEPDISTANCE_UTIL2
+	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
+	{
+		m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
+	}
+
+	if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
+#endif //USE_SEPDISTANCE_UTIL2
+
+	{
+
+	
+	btGjkPairDetector::ClosestPointInput input;
+
+	btGjkPairDetector	gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
+	//TODO: if (dispatchInfo.m_useContinuous)
+	gjkPairDetector.setMinkowskiA(min0);
+	gjkPairDetector.setMinkowskiB(min1);
+
+#ifdef USE_SEPDISTANCE_UTIL2
+	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
+	{
+		input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
+	} else
+#endif //USE_SEPDISTANCE_UTIL2
+	{
+		if (dispatchInfo.m_convexMaxDistanceUseCPT)
+		{
+			input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
+		} else
+		{
+			input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
+		}
+		input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
+	}
+
+	input.m_stackAlloc = dispatchInfo.m_stackAllocator;
+	input.m_transformA = body0->getWorldTransform();
+	input.m_transformB = body1->getWorldTransform();
+
+	gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+
+	
+
+#ifdef USE_SEPDISTANCE_UTIL2
+	btScalar sepDist = 0.f;
+	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
+	{
+		sepDist = gjkPairDetector.getCachedSeparatingDistance();
+		if (sepDist>SIMD_EPSILON)
+		{
+			sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
+			//now perturbe directions to get multiple contact points
+			
+		}
+	}
+#endif //USE_SEPDISTANCE_UTIL2
+
+	//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
+	
+	//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
+	if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
+	{
+		
+		int i;
+		btVector3 v0,v1;
+		btVector3 sepNormalWorldSpace;
+	
+		sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
+		btPlaneSpace1(sepNormalWorldSpace,v0,v1);
+
+
+		bool perturbeA = true;
+		const btScalar angleLimit = 0.125f * SIMD_PI;
+		btScalar perturbeAngle;
+		btScalar radiusA = min0->getAngularMotionDisc();
+		btScalar radiusB = min1->getAngularMotionDisc();
+		if (radiusA < radiusB)
+		{
+			perturbeAngle = gContactBreakingThreshold /radiusA;
+			perturbeA = true;
+		} else
+		{
+			perturbeAngle = gContactBreakingThreshold / radiusB;
+			perturbeA = false;
+		}
+		if ( perturbeAngle > angleLimit ) 
+				perturbeAngle = angleLimit;
+
+		btTransform unPerturbedTransform;
+		if (perturbeA)
+		{
+			unPerturbedTransform = input.m_transformA;
+		} else
+		{
+			unPerturbedTransform = input.m_transformB;
+		}
+		
+		for ( i=0;i<m_numPerturbationIterations;i++)
+		{
+			if (v0.length2()>SIMD_EPSILON)
+			{
+			btQuaternion perturbeRot(v0,perturbeAngle);
+			btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
+			btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
+			
+			
+			if (perturbeA)
+			{
+				input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis());
+				input.m_transformB = body1->getWorldTransform();
+#ifdef DEBUG_CONTACTS
+				dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
+#endif //DEBUG_CONTACTS
+			} else
+			{
+				input.m_transformA = body0->getWorldTransform();
+				input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis());
+#ifdef DEBUG_CONTACTS
+				dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
+#endif
+			}
+			
+			btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
+			gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
+			}
+			
+		}
+	}
+
+	
+
+#ifdef USE_SEPDISTANCE_UTIL2
+	if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
+	{
+		m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
+	}
+#endif //USE_SEPDISTANCE_UTIL2
+
+
+	}
+
+	if (m_ownManifold)
+	{
+		resultOut->refreshContactPoints();
+	}
+
+}
+
+
+
+bool disableCcd = false;
+btScalar	btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
+    
+	///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
+	///col0->m_worldTransform,
+	btScalar resultFraction = btScalar(1.);
+
+
+	btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
+	btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
+    
+	if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
+		squareMot1 < col1->getCcdSquareMotionThreshold())
+		return resultFraction;
+
+	if (disableCcd)
+		return btScalar(1.);
+
+
+	//An adhoc way of testing the Continuous Collision Detection algorithms
+	//One object is approximated as a sphere, to simplify things
+	//Starting in penetration should report no time of impact
+	//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
+	//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
+
+		
+	/// Convex0 against sphere for Convex1
+	{
+		btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
+
+		btSphereShape	sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+		btConvexCast::CastResult result;
+		btVoronoiSimplexSolver voronoiSimplex;
+		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+		///Simplification, one object is simplified as a sphere
+		btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
+		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+			col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+		{
+		
+			//store result.m_fraction in both bodies
+		
+			if (col0->getHitFraction()> result.m_fraction)
+				col0->setHitFraction( result.m_fraction );
+
+			if (col1->getHitFraction() > result.m_fraction)
+				col1->setHitFraction( result.m_fraction);
+
+			if (resultFraction > result.m_fraction)
+				resultFraction = result.m_fraction;
+
+		}
+		
+		
+
+
+	}
+
+	/// Sphere (for convex0) against Convex1
+	{
+		btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
+
+		btSphereShape	sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+		btConvexCast::CastResult result;
+		btVoronoiSimplexSolver voronoiSimplex;
+		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+		///Simplification, one object is simplified as a sphere
+		btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
+		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+			col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+		{
+		
+			//store result.m_fraction in both bodies
+		
+			if (col0->getHitFraction()	> result.m_fraction)
+				col0->setHitFraction( result.m_fraction);
+
+			if (col1->getHitFraction() > result.m_fraction)
+				col1->setHitFraction( result.m_fraction);
+
+			if (resultFraction > result.m_fraction)
+				resultFraction = result.m_fraction;
+
+		}
+	}
+	
+	return resultFraction;
+
+}
+

libs/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h

@@ -0,0 +1,109 @@
+/*
+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_CONVEX_ALGORITHM_H
+#define CONVEX_CONVEX_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "btCollisionCreateFunc.h"
+#include "btCollisionDispatcher.h"
+#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
+
+class btConvexPenetrationDepthSolver;
+
+///Enabling USE_SEPDISTANCE_UTIL2 requires 100% reliable distance computation. However, when using large size ratios GJK can be imprecise
+///so the distance is not conservative. In that case, enabling this USE_SEPDISTANCE_UTIL2 would result in failing/missing collisions.
+///Either improve GJK for large size ratios (testing a 100 units versus a 0.1 unit object) or only enable the util
+///for certain pairs that have a small size ratio
+
+//#define USE_SEPDISTANCE_UTIL2 1
+
+///The convexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations between two convex objects.
+///Multiple contact points are calculated by perturbing the orientation of the smallest object orthogonal to the separating normal.
+///This idea was described by Gino van den Bergen in this forum topic http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=4&t=288&p=888#p888
+class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
+{
+#ifdef USE_SEPDISTANCE_UTIL2
+	btConvexSeparatingDistanceUtil	m_sepDistance;
+#endif
+	btSimplexSolverInterface*		m_simplexSolver;
+	btConvexPenetrationDepthSolver* m_pdSolver;
+
+	
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool			m_lowLevelOfDetail;
+	
+	int m_numPerturbationIterations;
+	int m_minimumPointsPerturbationThreshold;
+
+
+	///cache separating vector to speedup collision detection
+	
+
+public:
+
+	btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
+
+
+	virtual ~btConvexConvexAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		///should we use m_ownManifold to avoid adding duplicates?
+		if (m_manifoldPtr && m_ownManifold)
+			manifoldArray.push_back(m_manifoldPtr);
+	}
+
+
+	void	setLowLevelOfDetail(bool useLowLevel);
+
+
+	const btPersistentManifold*	getManifold()
+	{
+		return m_manifoldPtr;
+	}
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+
+		btConvexPenetrationDepthSolver*		m_pdSolver;
+		btSimplexSolverInterface*			m_simplexSolver;
+		int m_numPerturbationIterations;
+		int m_minimumPointsPerturbationThreshold;
+
+		CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+		
+		virtual ~CreateFunc();
+
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm));
+			return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+		}
+	};
+
+
+};
+
+#endif //CONVEX_CONVEX_ALGORITHM_H

libs/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp

@@ -0,0 +1,155 @@
+/*
+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 "btConvexPlaneCollisionAlgorithm.h"
+
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+
+//#include <stdio.h>
+
+btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold)
+: btCollisionAlgorithm(ci),
+m_ownManifold(false),
+m_manifoldPtr(mf),
+m_isSwapped(isSwapped),
+m_numPerturbationIterations(numPerturbationIterations),
+m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
+{
+	btCollisionObject* convexObj = m_isSwapped? col1 : col0;
+	btCollisionObject* planeObj = m_isSwapped? col0 : col1;
+
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj))
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(convexObj,planeObj);
+		m_ownManifold = true;
+	}
+}
+
+
+btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+    btCollisionObject* convexObj = m_isSwapped? body1 : body0;
+	btCollisionObject* planeObj = m_isSwapped? body0: body1;
+
+	btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape();
+	btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();
+
+    bool hasCollision = false;
+	const btVector3& planeNormal = planeShape->getPlaneNormal();
+	const btScalar& planeConstant = planeShape->getPlaneConstant();
+	
+	btTransform convexWorldTransform = convexObj->getWorldTransform();
+	btTransform convexInPlaneTrans;
+	convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexWorldTransform;
+	//now perturbe the convex-world transform
+	convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot);
+	btTransform planeInConvex;
+	planeInConvex= convexWorldTransform.inverse() * planeObj->getWorldTransform();
+	
+	btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
+
+	btVector3 vtxInPlane = convexInPlaneTrans(vtx);
+	btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
+
+	btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
+	btVector3 vtxInPlaneWorld = planeObj->getWorldTransform() * vtxInPlaneProjected;
+
+	hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
+	resultOut->setPersistentManifold(m_manifoldPtr);
+	if (hasCollision)
+	{
+		/// report a contact. internally this will be kept persistent, and contact reduction is done
+		btVector3 normalOnSurfaceB = planeObj->getWorldTransform().getBasis() * planeNormal;
+		btVector3 pOnB = vtxInPlaneWorld;
+		resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
+	}
+}
+
+
+void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)dispatchInfo;
+	if (!m_manifoldPtr)
+		return;
+
+    btCollisionObject* convexObj = m_isSwapped? body1 : body0;
+	btCollisionObject* planeObj = m_isSwapped? body0: body1;
+
+	btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape();
+	btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();
+
+    
+	const btVector3& planeNormal = planeShape->getPlaneNormal();
+	//const btScalar& planeConstant = planeShape->getPlaneConstant();
+
+	//first perform a collision query with the non-perturbated collision objects
+	{
+		btQuaternion rotq(0,0,0,1);
+		collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut);
+	}
+
+	if (resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold)
+	{
+		btVector3 v0,v1;
+		btPlaneSpace1(planeNormal,v0,v1);
+		//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
+
+		const btScalar angleLimit = 0.125f * SIMD_PI;
+		btScalar perturbeAngle;
+		btScalar radius = convexShape->getAngularMotionDisc();
+		perturbeAngle = gContactBreakingThreshold / radius;
+		if ( perturbeAngle > angleLimit ) 
+				perturbeAngle = angleLimit;
+
+		btQuaternion perturbeRot(v0,perturbeAngle);
+		for (int i=0;i<m_numPerturbationIterations;i++)
+		{
+			btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
+			btQuaternion rotq(planeNormal,iterationAngle);
+			collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0,body1,dispatchInfo,resultOut);
+		}
+	}
+
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr->getNumContacts())
+		{
+			resultOut->refreshContactPoints();
+		}
+	}
+}
+
+btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	(void)col0;
+	(void)col1;
+
+	//not yet
+	return btScalar(1.);
+}

libs/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h

@@ -0,0 +1,84 @@
+/*
+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_PLANE_COLLISION_ALGORITHM_H
+#define CONVEX_PLANE_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btPersistentManifold;
+#include "btCollisionDispatcher.h"
+
+#include "LinearMath/btVector3.h"
+
+/// btSphereBoxCollisionAlgorithm  provides sphere-box collision detection.
+/// Other features are frame-coherency (persistent data) and collision response.
+class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm
+{
+	bool		m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool		m_isSwapped;
+	int			m_numPerturbationIterations;
+	int			m_minimumPointsPerturbationThreshold;
+
+public:
+
+	btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold);
+
+	virtual ~btConvexPlaneCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	void collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		int	m_numPerturbationIterations;
+		int m_minimumPointsPerturbationThreshold;
+			
+		CreateFunc() 
+			: m_numPerturbationIterations(1),
+			m_minimumPointsPerturbationThreshold(1)
+		{
+		}
+		
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm));
+			if (!m_swapped)
+			{
+				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+			} else
+			{
+				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+			}
+		}
+	};
+
+};
+
+#endif //CONVEX_PLANE_COLLISION_ALGORITHM_H
+

libs/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp

@@ -0,0 +1,298 @@
+/*
+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 "btDefaultCollisionConfiguration.h"
+
+#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h"
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+#include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+
+
+
+#include "LinearMath/btStackAlloc.h"
+#include "LinearMath/btPoolAllocator.h"
+
+
+
+
+
+btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo)
+//btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc*	stackAlloc,btPoolAllocator*	persistentManifoldPool,btPoolAllocator*	collisionAlgorithmPool)
+{
+
+	void* mem = btAlignedAlloc(sizeof(btVoronoiSimplexSolver),16);
+	m_simplexSolver = new (mem)btVoronoiSimplexSolver();
+
+	if (constructionInfo.m_useEpaPenetrationAlgorithm)
+	{
+		mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16);
+		m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver;
+	}else
+	{
+		mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16);
+		m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver;
+	}
+	
+	//default CreationFunctions, filling the m_doubleDispatch table
+	mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16);
+	m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver);
+	mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
+	m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
+	m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc;
+	mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16);
+	m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16);
+	m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc;
+	mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16);
+	m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc;
+	
+	mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16);
+	m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc;
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+	mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16);
+	m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16);
+	m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc;
+	m_boxSphereCF->m_swapped = true;
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
+	mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16);
+	m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16);
+	m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
+	m_triangleSphereCF->m_swapped = true;
+	
+	mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc),16);
+	m_boxBoxCF = new(mem)btBoxBoxCollisionAlgorithm::CreateFunc;
+
+	//convex versus plane
+	mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16);
+	m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16);
+	m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc;
+	m_planeConvexCF->m_swapped = true;
+	
+	///calculate maximum element size, big enough to fit any collision algorithm in the memory pool
+	int maxSize = sizeof(btConvexConvexAlgorithm);
+	int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm);
+	int maxSize3 = sizeof(btCompoundCollisionAlgorithm);
+	int sl = sizeof(btConvexSeparatingDistanceUtil);
+	sl = sizeof(btGjkPairDetector);
+	int	collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize);
+	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
+	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);
+
+	if (constructionInfo.m_stackAlloc)
+	{
+		m_ownsStackAllocator = false;
+		this->m_stackAlloc = constructionInfo.m_stackAlloc;
+	} else
+	{
+		m_ownsStackAllocator = true;
+		void* mem = btAlignedAlloc(sizeof(btStackAlloc),16);
+		m_stackAlloc = new(mem)btStackAlloc(constructionInfo.m_defaultStackAllocatorSize);
+	}
+		
+	if (constructionInfo.m_persistentManifoldPool)
+	{
+		m_ownsPersistentManifoldPool = false;
+		m_persistentManifoldPool = constructionInfo.m_persistentManifoldPool;
+	} else
+	{
+		m_ownsPersistentManifoldPool = true;
+		void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
+		m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize);
+	}
+	
+	if (constructionInfo.m_collisionAlgorithmPool)
+	{
+		m_ownsCollisionAlgorithmPool = false;
+		m_collisionAlgorithmPool = constructionInfo.m_collisionAlgorithmPool;
+	} else
+	{
+		m_ownsCollisionAlgorithmPool = true;
+		void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
+		m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize);
+	}
+
+
+}
+
+btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
+{
+	if (m_ownsStackAllocator)
+	{
+		m_stackAlloc->destroy();
+		m_stackAlloc->~btStackAlloc();
+		btAlignedFree(m_stackAlloc);
+	}
+	if (m_ownsCollisionAlgorithmPool)
+	{
+		m_collisionAlgorithmPool->~btPoolAllocator();
+		btAlignedFree(m_collisionAlgorithmPool);
+	}
+	if (m_ownsPersistentManifoldPool)
+	{
+		m_persistentManifoldPool->~btPoolAllocator();
+		btAlignedFree(m_persistentManifoldPool);
+	}
+
+	m_convexConvexCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree(	m_convexConvexCreateFunc);
+
+	m_convexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_convexConcaveCreateFunc);
+	m_swappedConvexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_swappedConvexConcaveCreateFunc);
+
+	m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_compoundCreateFunc);
+
+	m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_swappedCompoundCreateFunc);
+
+	m_emptyCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_emptyCreateFunc);
+
+	m_sphereSphereCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_sphereSphereCF);
+
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+	m_sphereBoxCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_sphereBoxCF);
+	m_boxSphereCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_boxSphereCF);
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
+	m_sphereTriangleCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_sphereTriangleCF);
+	m_triangleSphereCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_triangleSphereCF);
+	m_boxBoxCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_boxBoxCF);
+
+	m_convexPlaneCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_convexPlaneCF);
+	m_planeConvexCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_planeConvexCF);
+
+	m_simplexSolver->~btVoronoiSimplexSolver();
+	btAlignedFree(m_simplexSolver);
+
+	m_pdSolver->~btConvexPenetrationDepthSolver();
+	
+	btAlignedFree(m_pdSolver);
+
+
+}
+
+
+btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)
+{
+
+
+
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+	{
+		return	m_sphereSphereCF;
+	}
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE))
+	{
+		return	m_sphereBoxCF;
+	}
+
+	if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+	{
+		return	m_boxSphereCF;
+	}
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
+
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE))
+	{
+		return	m_sphereTriangleCF;
+	}
+
+	if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE  ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+	{
+		return	m_triangleSphereCF;
+	} 
+
+	if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE))
+	{
+		return m_boxBoxCF;
+	}
+	
+	if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE))
+	{
+		return m_convexPlaneCF;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE))
+	{
+		return m_planeConvexCF;
+	}
+	
+
+
+	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
+	{
+		return m_convexConvexCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1))
+	{
+		return m_convexConcaveCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0))
+	{
+		return m_swappedConvexConcaveCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isCompound(proxyType0))
+	{
+		return m_compoundCreateFunc;
+	} else
+	{
+		if (btBroadphaseProxy::isCompound(proxyType1))
+		{
+			return m_swappedCompoundCreateFunc;
+		}
+	}
+
+	//failed to find an algorithm
+	return m_emptyCreateFunc;
+}
+
+void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+{
+	btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*) m_convexConvexCreateFunc;
+	convexConvex->m_numPerturbationIterations = numPerturbationIterations;
+	convexConvex->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
+}

libs/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h

@@ -0,0 +1,135 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_DEFAULT_COLLISION_CONFIGURATION
+#define BT_DEFAULT_COLLISION_CONFIGURATION
+
+#include "btCollisionConfiguration.h"
+class btVoronoiSimplexSolver;
+class btConvexPenetrationDepthSolver;
+
+struct	btDefaultCollisionConstructionInfo
+{
+	btStackAlloc*		m_stackAlloc;
+	btPoolAllocator*	m_persistentManifoldPool;
+	btPoolAllocator*	m_collisionAlgorithmPool;
+	int					m_defaultMaxPersistentManifoldPoolSize;
+	int					m_defaultMaxCollisionAlgorithmPoolSize;
+	int					m_customCollisionAlgorithmMaxElementSize;
+	int					m_defaultStackAllocatorSize;
+	int					m_useEpaPenetrationAlgorithm;
+
+	btDefaultCollisionConstructionInfo()
+		:m_stackAlloc(0),
+		m_persistentManifoldPool(0),
+		m_collisionAlgorithmPool(0),
+		m_defaultMaxPersistentManifoldPoolSize(4096),
+		m_defaultMaxCollisionAlgorithmPoolSize(4096),
+		m_customCollisionAlgorithmMaxElementSize(0),
+		m_defaultStackAllocatorSize(0),
+		m_useEpaPenetrationAlgorithm(true)
+	{
+	}
+};
+
+
+
+///btCollisionConfiguration allows to configure Bullet collision detection
+///stack allocator, pool memory allocators
+///@todo: describe the meaning
+class	btDefaultCollisionConfiguration : public btCollisionConfiguration
+{
+
+protected:
+
+	int	m_persistentManifoldPoolSize;
+	
+	btStackAlloc*	m_stackAlloc;
+	bool	m_ownsStackAllocator;
+
+	btPoolAllocator*	m_persistentManifoldPool;
+	bool	m_ownsPersistentManifoldPool;
+
+
+	btPoolAllocator*	m_collisionAlgorithmPool;
+	bool	m_ownsCollisionAlgorithmPool;
+
+	//default simplex/penetration depth solvers
+	btVoronoiSimplexSolver*	m_simplexSolver;
+	btConvexPenetrationDepthSolver*	m_pdSolver;
+	
+	//default CreationFunctions, filling the m_doubleDispatch table
+	btCollisionAlgorithmCreateFunc*	m_convexConvexCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_convexConcaveCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_swappedConvexConcaveCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_compoundCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_swappedCompoundCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_emptyCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_sphereSphereCF;
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+	btCollisionAlgorithmCreateFunc* m_sphereBoxCF;
+	btCollisionAlgorithmCreateFunc* m_boxSphereCF;
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
+	btCollisionAlgorithmCreateFunc* m_boxBoxCF;
+	btCollisionAlgorithmCreateFunc*	m_sphereTriangleCF;
+	btCollisionAlgorithmCreateFunc*	m_triangleSphereCF;
+	btCollisionAlgorithmCreateFunc*	m_planeConvexCF;
+	btCollisionAlgorithmCreateFunc*	m_convexPlaneCF;
+	
+public:
+
+
+	btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo());
+
+	virtual ~btDefaultCollisionConfiguration();
+
+		///memory pools
+	virtual btPoolAllocator* getPersistentManifoldPool()
+	{
+		return m_persistentManifoldPool;
+	}
+
+	virtual btPoolAllocator* getCollisionAlgorithmPool()
+	{
+		return m_collisionAlgorithmPool;
+	}
+
+	virtual btStackAlloc*	getStackAllocator()
+	{
+		return m_stackAlloc;
+	}
+
+	virtual	btVoronoiSimplexSolver*	getSimplexSolver()
+	{
+		return m_simplexSolver;
+	}
+
+
+	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
+
+	///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm.
+	///By default, this feature is disabled for best performance.
+	///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature.
+	///@param minimumPointsPerturbationThreshold is the minimum number of points in the contact cache, above which the feature is disabled
+	///3 is a good value for both params, if you want to enable the feature. This is because the default contact cache contains a maximum of 4 points, and one collision query at the unperturbed orientation is performed first.
+	///See Bullet/Demos/CollisionDemo for an example how this feature gathers multiple points.
+	///@todo we could add a per-object setting of those parameters, for level-of-detail collision detection.
+	void	setConvexConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3);
+
+};
+
+#endif //BT_DEFAULT_COLLISION_CONFIGURATION
+

libs/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp

@@ -0,0 +1,34 @@
+/*
+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 "btEmptyCollisionAlgorithm.h"
+
+
+
+btEmptyAlgorithm::btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+	: btCollisionAlgorithm(ci)
+{
+}
+
+void btEmptyAlgorithm::processCollision (btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* )
+{
+}
+
+btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* )
+{
+	return btScalar(1.);
+}
+
+

libs/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.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 EMPTY_ALGORITH
+#define EMPTY_ALGORITH
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "btCollisionCreateFunc.h"
+#include "btCollisionDispatcher.h"
+
+#define ATTRIBUTE_ALIGNED(a)
+
+///EmptyAlgorithm is a stub for unsupported collision pairs.
+///The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame.
+class btEmptyAlgorithm : public btCollisionAlgorithm
+{
+
+public:
+	
+	btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci);
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+	}
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			(void)body0;
+			(void)body1;
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btEmptyAlgorithm));
+			return new(mem) btEmptyAlgorithm(ci);
+		}
+	};
+
+} ATTRIBUTE_ALIGNED(16);
+
+#endif //EMPTY_ALGORITH

libs/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp

@@ -0,0 +1,171 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans  http://bulletphysics.com
+
+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 "btGhostObject.h"
+#include "btCollisionWorld.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "LinearMath/btAabbUtil2.h"
+
+btGhostObject::btGhostObject()
+{
+	m_internalType = CO_GHOST_OBJECT;
+}
+
+btGhostObject::~btGhostObject()
+{
+	///btGhostObject should have been removed from the world, so no overlapping objects
+	btAssert(!m_overlappingObjects.size());
+}
+
+
+void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy)
+{
+	btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
+	btAssert(otherObject);
+	///if this linearSearch becomes too slow (too many overlapping objects) we should add a more appropriate data structure
+	int index = m_overlappingObjects.findLinearSearch(otherObject);
+	if (index==m_overlappingObjects.size())
+	{
+		//not found
+		m_overlappingObjects.push_back(otherObject);
+	}
+}
+
+void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy)
+{
+	btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
+	btAssert(otherObject);
+	int index = m_overlappingObjects.findLinearSearch(otherObject);
+	if (index<m_overlappingObjects.size())
+	{
+		m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1];
+		m_overlappingObjects.pop_back();
+	}
+}
+
+
+btPairCachingGhostObject::btPairCachingGhostObject()
+{
+	m_hashPairCache = new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
+}
+
+btPairCachingGhostObject::~btPairCachingGhostObject()
+{
+	m_hashPairCache->~btHashedOverlappingPairCache();
+	btAlignedFree( m_hashPairCache );
+}
+
+void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy)
+{
+	btBroadphaseProxy*actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle();
+	btAssert(actualThisProxy);
+
+	btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
+	btAssert(otherObject);
+	int index = m_overlappingObjects.findLinearSearch(otherObject);
+	if (index==m_overlappingObjects.size())
+	{
+		m_overlappingObjects.push_back(otherObject);
+		m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy);
+	}
+}
+
+void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1)
+{
+	btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
+	btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle();
+	btAssert(actualThisProxy);
+
+	btAssert(otherObject);
+	int index = m_overlappingObjects.findLinearSearch(otherObject);
+	if (index<m_overlappingObjects.size())
+	{
+		m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1];
+		m_overlappingObjects.pop_back();
+		m_hashPairCache->removeOverlappingPair(actualThisProxy,otherProxy,dispatcher);
+	}
+}
+
+
+void	btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
+{
+	btTransform	convexFromTrans,convexToTrans;
+	convexFromTrans = convexFromWorld;
+	convexToTrans = convexToWorld;
+	btVector3 castShapeAabbMin, castShapeAabbMax;
+	/* Compute AABB that encompasses angular movement */
+	{
+		btVector3 linVel, angVel;
+		btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
+		btTransform R;
+		R.setIdentity ();
+		R.setRotation (convexFromTrans.getRotation());
+		castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
+	}
+
+	/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
+	// do a ray-shape query using convexCaster (CCD)
+	int i;
+	for (i=0;i<m_overlappingObjects.size();i++)
+	{
+		btCollisionObject*	collisionObject= m_overlappingObjects[i];
+		//only perform raycast if filterMask matches
+		if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
+			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+			btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+			AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
+			btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
+			btVector3 hitNormal;
+			if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
+			{
+				btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans,
+					collisionObject,
+						collisionObject->getCollisionShape(),
+						collisionObject->getWorldTransform(),
+						resultCallback,
+						allowedCcdPenetration);
+			}
+		}
+	}
+
+}
+
+void	btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const
+{
+	btTransform rayFromTrans;
+	rayFromTrans.setIdentity();
+	rayFromTrans.setOrigin(rayFromWorld);
+	btTransform  rayToTrans;
+	rayToTrans.setIdentity();
+	rayToTrans.setOrigin(rayToWorld);
+
+
+	int i;
+	for (i=0;i<m_overlappingObjects.size();i++)
+	{
+		btCollisionObject*	collisionObject= m_overlappingObjects[i];
+		//only perform raycast if filterMask matches
+		if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
+		{
+			btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,
+							collisionObject,
+								collisionObject->getCollisionShape(),
+								collisionObject->getWorldTransform(),
+								resultCallback);
+		}
+	}
+}
+

libs/bullet/BulletCollision/CollisionDispatch/btGhostObject.h

@@ -0,0 +1,175 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_GHOST_OBJECT_H
+#define BT_GHOST_OBJECT_H
+
+
+#include "btCollisionObject.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h"
+#include "LinearMath/btAlignedAllocator.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "btCollisionWorld.h"
+
+class btConvexShape;
+
+class btDispatcher;
+
+///The btGhostObject can keep track of all objects that are overlapping
+///By default, this overlap is based on the AABB
+///This is useful for creating a character controller, collision sensors/triggers, explosions etc.
+///We plan on adding rayTest and other queries for the btGhostObject
+ATTRIBUTE_ALIGNED16(class) btGhostObject : public btCollisionObject
+{
+protected:
+
+	btAlignedObjectArray<btCollisionObject*> m_overlappingObjects;
+
+public:
+
+	btGhostObject();
+
+	virtual ~btGhostObject();
+
+	void	convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const;
+
+	void	rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const; 
+
+	///this method is mainly for expert/internal use only.
+	virtual void	addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0);
+	///this method is mainly for expert/internal use only.
+	virtual void	removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0);
+
+	int	getNumOverlappingObjects() const
+	{
+		return m_overlappingObjects.size();
+	}
+
+	btCollisionObject*	getOverlappingObject(int index)
+	{
+		return m_overlappingObjects[index];
+	}
+
+	const btCollisionObject*	getOverlappingObject(int index) const
+	{
+		return m_overlappingObjects[index];
+	}
+
+	btAlignedObjectArray<btCollisionObject*>&	getOverlappingPairs()
+	{
+		return m_overlappingObjects;
+	}
+
+	const btAlignedObjectArray<btCollisionObject*>	getOverlappingPairs() const
+	{
+		return m_overlappingObjects;
+	}
+
+	//
+	// internal cast
+	//
+
+	static const btGhostObject*	upcast(const btCollisionObject* colObj)
+	{
+		if (colObj->getInternalType()==CO_GHOST_OBJECT)
+			return (const btGhostObject*)colObj;
+		return 0;
+	}
+	static btGhostObject*			upcast(btCollisionObject* colObj)
+	{
+		if (colObj->getInternalType()==CO_GHOST_OBJECT)
+			return (btGhostObject*)colObj;
+		return 0;
+	}
+
+};
+
+class	btPairCachingGhostObject : public btGhostObject
+{
+	btHashedOverlappingPairCache*	m_hashPairCache;
+
+public:
+
+	btPairCachingGhostObject();
+
+	virtual ~btPairCachingGhostObject();
+
+	///this method is mainly for expert/internal use only.
+	virtual void	addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0);
+
+	virtual void	removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0);
+
+	btHashedOverlappingPairCache*	getOverlappingPairCache()
+	{
+		return m_hashPairCache;
+	}
+
+};
+
+
+
+///The btGhostPairCallback interfaces and forwards adding and removal of overlapping pairs from the btBroadphaseInterface to btGhostObject.
+class btGhostPairCallback : public btOverlappingPairCallback
+{
+	
+public:
+	btGhostPairCallback()
+	{
+	}
+
+	virtual ~btGhostPairCallback()
+	{
+		
+	}
+
+	virtual btBroadphasePair*	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+	{
+		btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject;
+		btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject;
+		btGhostObject* ghost0 = 		btGhostObject::upcast(colObj0);
+		btGhostObject* ghost1 = 		btGhostObject::upcast(colObj1);
+		if (ghost0)
+			ghost0->addOverlappingObjectInternal(proxy1, proxy0);
+		if (ghost1)
+			ghost1->addOverlappingObjectInternal(proxy0, proxy1);
+		return 0;
+	}
+
+	virtual void*	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
+	{
+		btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject;
+		btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject;
+		btGhostObject* ghost0 = 		btGhostObject::upcast(colObj0);
+		btGhostObject* ghost1 = 		btGhostObject::upcast(colObj1);
+		if (ghost0)
+			ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0);
+		if (ghost1)
+			ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1);
+		return 0;
+	}
+
+	virtual void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/)
+	{
+		btAssert(0);
+		//need to keep track of all ghost objects and call them here
+		//m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher);
+	}
+
+	
+
+};
+
+#endif
+

libs/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp

@@ -0,0 +1,772 @@
+#include "btInternalEdgeUtility.h"
+
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+#include "LinearMath/btIDebugDraw.h"
+
+
+//#define DEBUG_INTERNAL_EDGE
+
+
+#ifdef DEBUG_INTERNAL_EDGE
+#include <stdio.h>
+#endif //DEBUG_INTERNAL_EDGE
+
+
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+static btIDebugDraw* gDebugDrawer = 0;
+
+void	btSetDebugDrawer(btIDebugDraw* debugDrawer)
+{
+	gDebugDrawer = debugDrawer;
+}
+
+static void    btDebugDrawLine(const btVector3& from,const btVector3& to, const btVector3& color)
+{
+	if (gDebugDrawer)
+		gDebugDrawer->drawLine(from,to,color);
+}
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+
+static int	btGetHash(int partId, int triangleIndex)
+{
+	int hash = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
+	return hash;
+}
+
+
+
+static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA,const btVector3& normalB)
+{
+	const btVector3 refAxis0  = edgeA;
+	const btVector3 refAxis1  = normalA;
+	const btVector3 swingAxis = normalB;
+	btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+	return  angle;
+}
+
+
+struct btConnectivityProcessor : public btTriangleCallback
+{
+	int				m_partIdA;
+	int				m_triangleIndexA;
+	btVector3*		m_triangleVerticesA;
+	btTriangleInfoMap*	m_triangleInfoMap;
+
+
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+	{
+		//skip self-collisions
+		if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
+			return;
+
+		//skip duplicates (disabled for now)
+		//if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))
+		//	return;
+
+		//search for shared vertices and edges
+		int numshared = 0;
+		int sharedVertsA[3]={-1,-1,-1};
+		int sharedVertsB[3]={-1,-1,-1};
+
+		///skip degenerate triangles
+		btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2();
+		if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold)
+			return;
+
+
+		btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2();
+		///skip degenerate triangles
+		if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold)
+			return;
+
+#if 0
+		printf("triangle A[0]	=	(%f,%f,%f)\ntriangle A[1]	=	(%f,%f,%f)\ntriangle A[2]	=	(%f,%f,%f)\n",
+			m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(),
+			m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(),
+			m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ());
+
+		printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex);
+		printf("triangle B[0]	=	(%f,%f,%f)\ntriangle B[1]	=	(%f,%f,%f)\ntriangle B[2]	=	(%f,%f,%f)\n",
+			triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(),
+			triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(),
+			triangle[2].getX(),triangle[2].getY(),triangle[2].getZ());
+#endif
+
+		for (int i=0;i<3;i++)
+		{
+			for (int j=0;j<3;j++)
+			{
+				if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold)
+				{
+					sharedVertsA[numshared] = i;
+					sharedVertsB[numshared] = j;
+					numshared++;
+					///degenerate case
+					if(numshared >= 3)
+						return;
+				}
+			}
+			///degenerate case
+			if(numshared >= 3)
+				return;
+		}
+		switch (numshared)
+		{
+		case 0:
+			{
+				break;
+			}
+		case 1:
+			{
+				//shared vertex
+				break;
+			}
+		case 2:
+			{
+				//shared edge
+				//we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
+				if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
+				{
+					sharedVertsA[0] = 2;
+					sharedVertsA[1] = 0;
+					int tmp = sharedVertsB[1];
+					sharedVertsB[1] = sharedVertsB[0];
+					sharedVertsB[0] = tmp;
+				}
+
+				int hash = btGetHash(m_partIdA,m_triangleIndexA);
+
+				btTriangleInfo* info = m_triangleInfoMap->find(hash);
+				if (!info)
+				{
+					btTriangleInfo tmp;
+					m_triangleInfoMap->insert(hash,tmp);
+					info = m_triangleInfoMap->find(hash);
+				}
+
+				int sumvertsA = sharedVertsA[0]+sharedVertsA[1];
+				int otherIndexA = 3-sumvertsA;
+
+				
+				btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]);
+
+				btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]);
+				int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]);
+
+				btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]);
+				//btTriangleShape tB(triangle[0],triangle[1],triangle[2]);
+
+				btVector3 normalA;
+				btVector3 normalB;
+				tA.calcNormal(normalA);
+				tB.calcNormal(normalB);
+				edge.normalize();
+				btVector3 edgeCrossA = edge.cross(normalA).normalize();
+
+				{
+					btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]];
+					if (edgeCrossA.dot(tmp) < 0)
+					{
+						edgeCrossA*=-1;
+					}
+				}
+
+				btVector3 edgeCrossB = edge.cross(normalB).normalize();
+
+				{
+					btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]];
+					if (edgeCrossB.dot(tmp) < 0)
+					{
+						edgeCrossB*=-1;
+					}
+				}
+
+				btScalar	angle2 = 0;
+				btScalar	ang4 = 0.f;
+
+
+				btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB);
+				btScalar len2 = calculatedEdge.length2();
+
+				btScalar correctedAngle(0);
+				btVector3 calculatedNormalB = normalA;
+				bool isConvex = false;
+
+				if (len2<m_triangleInfoMap->m_planarEpsilon)
+				{
+					angle2 = 0.f;
+					ang4 = 0.f;
+				} else
+				{
+
+					calculatedEdge.normalize();
+					btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA);
+					calculatedNormalA.normalize();
+					angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB);
+					ang4 = SIMD_PI-angle2;
+					btScalar dotA = normalA.dot(edgeCrossB);
+					///@todo: check if we need some epsilon, due to floating point imprecision
+					isConvex = (dotA<0.);
+
+					correctedAngle = isConvex ? ang4 : -ang4;
+					btQuaternion orn2(calculatedEdge,-correctedAngle);
+					calculatedNormalB = btMatrix3x3(orn2)*normalA;
+
+
+				}
+
+				
+
+				
+							
+				//alternatively use 
+				//btVector3 calculatedNormalB2 = quatRotate(orn,normalA);
+
+
+				switch (sumvertsA)
+				{
+				case 1:
+					{
+						btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1];
+						btQuaternion orn(edge,-correctedAngle);
+						btVector3 computedNormalB = quatRotate(orn,normalA);
+						btScalar bla = computedNormalB.dot(normalB);
+						if (bla<0)
+						{
+							computedNormalB*=-1;
+							info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB;
+						}
+#ifdef DEBUG_INTERNAL_EDGE
+						if ((computedNormalB-normalB).length()>0.0001)
+						{
+							printf("warning: normals not identical\n");
+						}
+#endif//DEBUG_INTERNAL_EDGE
+
+						info->m_edgeV0V1Angle = -correctedAngle;
+
+						if (isConvex)
+							info->m_flags |= TRI_INFO_V0V1_CONVEX;
+						break;
+					}
+				case 2:
+					{
+						btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0];
+						btQuaternion orn(edge,-correctedAngle);
+						btVector3 computedNormalB = quatRotate(orn,normalA);
+						if (computedNormalB.dot(normalB)<0)
+						{
+							computedNormalB*=-1;
+							info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB;
+						}
+
+#ifdef DEBUG_INTERNAL_EDGE
+						if ((computedNormalB-normalB).length()>0.0001)
+						{
+							printf("warning: normals not identical\n");
+						}
+#endif //DEBUG_INTERNAL_EDGE
+						info->m_edgeV2V0Angle = -correctedAngle;
+						if (isConvex)
+							info->m_flags |= TRI_INFO_V2V0_CONVEX;
+						break;	
+					}
+				case 3:
+					{
+						btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2];
+						btQuaternion orn(edge,-correctedAngle);
+						btVector3 computedNormalB = quatRotate(orn,normalA);
+						if (computedNormalB.dot(normalB)<0)
+						{
+							info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB;
+							computedNormalB*=-1;
+						}
+#ifdef DEBUG_INTERNAL_EDGE
+						if ((computedNormalB-normalB).length()>0.0001)
+						{
+							printf("warning: normals not identical\n");
+						}
+#endif //DEBUG_INTERNAL_EDGE
+						info->m_edgeV1V2Angle = -correctedAngle;
+
+						if (isConvex)
+							info->m_flags |= TRI_INFO_V1V2_CONVEX;
+						break;
+					}
+				}
+
+				break;
+			}
+		default:
+			{
+				//				printf("warning: duplicate triangle\n");
+			}
+
+		}
+	}
+};
+/////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////
+
+void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap)
+{
+	//the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
+	if (trimeshShape->getTriangleInfoMap())
+		return;
+
+	trimeshShape->setTriangleInfoMap(triangleInfoMap);
+
+	btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface();
+	const btVector3& meshScaling = meshInterface->getScaling();
+
+	for (int partId = 0; partId< meshInterface->getNumSubParts();partId++)
+	{
+		const unsigned char *vertexbase = 0;
+		int numverts = 0;
+		PHY_ScalarType type = PHY_INTEGER;
+		int stride = 0;
+		const unsigned char *indexbase = 0;
+		int indexstride = 0;
+		int numfaces = 0;
+		PHY_ScalarType indicestype = PHY_INTEGER;
+		//PHY_ScalarType indexType=0;
+
+		btVector3 triangleVerts[3];
+		meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,	type,stride,&indexbase,indexstride,numfaces,indicestype,partId);
+		btVector3 aabbMin,aabbMax;
+
+		for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++)
+		{
+			unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride);
+
+			for (int j=2;j>=0;j--)
+			{
+
+				int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
+				if (type == PHY_FLOAT)
+				{
+					float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
+					triangleVerts[j] = btVector3(
+						graphicsbase[0]*meshScaling.getX(),
+						graphicsbase[1]*meshScaling.getY(),
+						graphicsbase[2]*meshScaling.getZ());
+				}
+				else
+				{
+					double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
+					triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
+				}
+			}
+			aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+			aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); 
+			aabbMin.setMin(triangleVerts[0]);
+			aabbMax.setMax(triangleVerts[0]);
+			aabbMin.setMin(triangleVerts[1]);
+			aabbMax.setMax(triangleVerts[1]);
+			aabbMin.setMin(triangleVerts[2]);
+			aabbMax.setMax(triangleVerts[2]);
+
+			btConnectivityProcessor connectivityProcessor;
+			connectivityProcessor.m_partIdA = partId;
+			connectivityProcessor.m_triangleIndexA = triangleIndex;
+			connectivityProcessor.m_triangleVerticesA = &triangleVerts[0];
+			connectivityProcessor.m_triangleInfoMap  = triangleInfoMap;
+
+			trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax);
+		}
+
+	}
+
+}
+
+
+
+
+// Given a point and a line segment (defined by two points), compute the closest point
+// in the line.  Cap the point at the endpoints of the line segment.
+void btNearestPointInLineSegment(const btVector3 &point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint)
+{
+	btVector3 lineDelta     = line1 - line0;
+
+	// Handle degenerate lines
+	if ( lineDelta.fuzzyZero())
+	{
+		nearestPoint = line0;
+	}
+	else
+	{
+		btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta);
+
+		// Clamp the point to conform to the segment's endpoints
+		if ( delta < 0 )
+			delta = 0;
+		else if ( delta > 1 )
+			delta = 1;
+
+		nearestPoint = line0 + lineDelta*delta;
+	}
+}
+
+
+
+
+bool	btClampNormal(const btVector3& edge,const btVector3& tri_normal_org,const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 & clampedLocalNormal)
+{
+	btVector3 tri_normal = tri_normal_org;
+	//we only have a local triangle normal, not a local contact normal -> only normal in world space...
+	//either compute the current angle all in local space, or all in world space
+
+	btVector3 edgeCross = edge.cross(tri_normal).normalize();
+	btScalar curAngle = btGetAngle(edgeCross,tri_normal,localContactNormalOnB);
+
+	if (correctedEdgeAngle<0)
+	{
+		if (curAngle < correctedEdgeAngle)
+		{
+			btScalar diffAngle = correctedEdgeAngle-curAngle;
+			btQuaternion rotation(edge,diffAngle );
+			clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
+			return true;
+		}
+	}
+
+	if (correctedEdgeAngle>=0)
+	{
+		if (curAngle > correctedEdgeAngle)
+		{
+			btScalar diffAngle = correctedEdgeAngle-curAngle;
+			btQuaternion rotation(edge,diffAngle );
+			clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
+			return true;
+		}
+	}
+	return false;
+}
+
+
+
+/// Changes a btManifoldPoint collision normal to the normal from the mesh.
+void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* colObj0,const btCollisionObject* colObj1, int partId0, int index0, int normalAdjustFlags)
+{
+	//btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
+	if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
+		return;
+
+	btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape();
+	btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
+	if (!triangleInfoMapPtr)
+		return;
+
+	int hash = btGetHash(partId0,index0);
+
+
+	btTriangleInfo* info = triangleInfoMapPtr->find(hash);
+	if (!info)
+		return;
+
+	btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f;
+	
+	const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0->getCollisionShape());
+	btVector3 v0,v1,v2;
+	tri_shape->getVertex(0,v0);
+	tri_shape->getVertex(1,v1);
+	tri_shape->getVertex(2,v2);
+
+	btVector3 center = (v0+v1+v2)*btScalar(1./3.);
+
+	btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);
+	btVector3 tri_normal;
+	tri_shape->calcNormal(tri_normal);
+
+	//btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
+	btVector3 nearest;
+	btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest);
+
+	btVector3 contact = cp.m_localPointB;
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+	const btTransform& tr = colObj0->getWorldTransform();
+	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+
+
+	bool isNearEdge = false;
+
+	int numConcaveEdgeHits = 0;
+	int numConvexEdgeHits = 0;
+
+	btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
+	localContactNormalOnB.normalize();//is this necessary?
+
+	if ((info->m_edgeV0V1Angle)< SIMD_2_PI)
+	{
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
+#endif
+		btScalar len = (contact-nearest).length();
+		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
+		{
+			btVector3 edge(v0-v1);
+			isNearEdge = true;
+
+			if (info->m_edgeV0V1Angle==btScalar(0))
+			{
+				numConcaveEdgeHits++;
+			} else
+			{
+
+				bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);
+				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
+	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
+	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+				btVector3 nA = swapFactor * tri_normal;
+
+				btQuaternion orn(edge,info->m_edgeV0V1Angle);
+				btVector3 computedNormalB = quatRotate(orn,tri_normal);
+				if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)
+					computedNormalB*=-1;
+				btVector3 nB = swapFactor*computedNormalB;
+
+				btScalar	NdotA = localContactNormalOnB.dot(nA);
+				btScalar	NdotB = localContactNormalOnB.dot(nB);
+				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
+
+#ifdef DEBUG_INTERNAL_EDGE
+				{
+					
+					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
+				}
+#endif //DEBUG_INTERNAL_EDGE
+
+
+				if (backFacingNormal)
+				{
+					numConcaveEdgeHits++;
+				}
+				else
+				{
+					numConvexEdgeHits++;
+					btVector3 clampedLocalNormal;
+					bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal);
+					if (isClamped)
+					{
+						if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
+						{
+							btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal;
+							//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
+							cp.m_normalWorldOnB = newNormal;
+							// Reproject collision point along normal. (what about cp.m_distance1?)
+							cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
+							cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
+							
+						}
+					}
+				}
+			}
+		}
+	}
+
+	btNearestPointInLineSegment(contact,v1,v2,nearest);
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+	if ((info->m_edgeV1V2Angle)< SIMD_2_PI)
+	{
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+
+
+		btScalar len = (contact-nearest).length();
+		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
+		{
+			isNearEdge = true;
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+			btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+			btVector3 edge(v1-v2);
+
+			isNearEdge = true;
+
+			if (info->m_edgeV1V2Angle == btScalar(0))
+			{
+				numConcaveEdgeHits++;
+			} else
+			{
+				bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0;
+				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
+	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
+	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+				btVector3 nA = swapFactor * tri_normal;
+				
+				btQuaternion orn(edge,info->m_edgeV1V2Angle);
+				btVector3 computedNormalB = quatRotate(orn,tri_normal);
+				if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB)
+					computedNormalB*=-1;
+				btVector3 nB = swapFactor*computedNormalB;
+
+#ifdef DEBUG_INTERNAL_EDGE
+				{
+					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
+				}
+#endif //DEBUG_INTERNAL_EDGE
+
+
+				btScalar	NdotA = localContactNormalOnB.dot(nA);
+				btScalar	NdotB = localContactNormalOnB.dot(nB);
+				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
+
+				if (backFacingNormal)
+				{
+					numConcaveEdgeHits++;
+				}
+				else
+				{
+					numConvexEdgeHits++;
+					btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
+					btVector3 clampedLocalNormal;
+					bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal);
+					if (isClamped)
+					{
+						if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
+						{
+							btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal;
+							//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
+							cp.m_normalWorldOnB = newNormal;
+							// Reproject collision point along normal.
+							cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
+							cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
+						}
+					}
+				}
+			}
+		}
+	}
+
+	btNearestPointInLineSegment(contact,v2,v0,nearest);
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+	if ((info->m_edgeV2V0Angle)< SIMD_2_PI)
+	{
+
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+		btScalar len = (contact-nearest).length();
+		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
+		{
+			isNearEdge = true;
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+			btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+			btVector3 edge(v2-v0);
+
+			if (info->m_edgeV2V0Angle==btScalar(0))
+			{
+				numConcaveEdgeHits++;
+			} else
+			{
+
+				bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0;
+				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
+	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
+	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+				btVector3 nA = swapFactor * tri_normal;
+				btQuaternion orn(edge,info->m_edgeV2V0Angle);
+				btVector3 computedNormalB = quatRotate(orn,tri_normal);
+				if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB)
+					computedNormalB*=-1;
+				btVector3 nB = swapFactor*computedNormalB;
+
+#ifdef DEBUG_INTERNAL_EDGE
+				{
+					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
+				}
+#endif //DEBUG_INTERNAL_EDGE
+
+				btScalar	NdotA = localContactNormalOnB.dot(nA);
+				btScalar	NdotB = localContactNormalOnB.dot(nB);
+				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
+
+				if (backFacingNormal)
+				{
+					numConcaveEdgeHits++;
+				}
+				else
+				{
+					numConvexEdgeHits++;
+					//				printf("hitting convex edge\n");
+
+
+					btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
+					btVector3 clampedLocalNormal;
+					bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal);
+					if (isClamped)
+					{
+						if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
+						{
+							btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal;
+							//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
+							cp.m_normalWorldOnB = newNormal;
+							// Reproject collision point along normal.
+							cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
+							cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
+						}
+					}
+				} 
+			}
+			
+
+		}
+	}
+
+#ifdef DEBUG_INTERNAL_EDGE
+	{
+		btVector3 color(0,1,1);
+		btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color);
+	}
+#endif //DEBUG_INTERNAL_EDGE
+
+	if (isNearEdge)
+	{
+
+		if (numConcaveEdgeHits>0)
+		{
+			if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0)
+			{
+				//fix tri_normal so it pointing the same direction as the current local contact normal
+				if (tri_normal.dot(localContactNormalOnB) < 0)
+				{
+					tri_normal *= -1;
+				}
+				cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis()*tri_normal;
+			} else
+			{
+				//modify the normal to be the triangle normal (or backfacing normal)
+				cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *(tri_normal *frontFacing);
+			}
+			
+			
+			// Reproject collision point along normal.
+			cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
+			cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
+		}
+	}
+}

libs/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h

@@ -0,0 +1,46 @@
+
+#ifndef BT_INTERNAL_EDGE_UTILITY_H
+#define BT_INTERNAL_EDGE_UTILITY_H
+
+#include "LinearMath/btHashMap.h"
+#include "LinearMath/btVector3.h"
+
+#include "BulletCollision/CollisionShapes/btTriangleInfoMap.h"
+
+///The btInternalEdgeUtility helps to avoid or reduce artifacts due to wrong collision normals caused by internal edges.
+///See also http://code.google.com/p/bullet/issues/detail?id=27
+
+class btBvhTriangleMeshShape;
+class btCollisionObject;
+class btManifoldPoint;
+class btIDebugDraw;
+
+
+
+enum btInternalEdgeAdjustFlags
+{
+	BT_TRIANGLE_CONVEX_BACKFACE_MODE = 1,
+	BT_TRIANGLE_CONCAVE_DOUBLE_SIDED = 2, //double sided options are experimental, single sided is recommended
+	BT_TRIANGLE_CONVEX_DOUBLE_SIDED = 4
+};
+
+
+///Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo'
+void	btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap);
+
+
+///Call the btFixMeshNormal to adjust the collision normal, using the triangle info map (generated using btGenerateInternalEdgeInfo)
+///If this info map is missing, or the triangle is not store in this map, nothing will be done
+void	btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* trimeshColObj0,const btCollisionObject* otherColObj1, int partId0, int index0, int normalAdjustFlags = 0);
+
+///Enable the BT_INTERNAL_EDGE_DEBUG_DRAW define and call btSetDebugDrawer, to get visual info to see if the internal edge utility works properly.
+///If the utility doesn't work properly, you might have to adjust the threshold values in btTriangleInfoMap
+//#define BT_INTERNAL_EDGE_DEBUG_DRAW
+
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+void	btSetDebugDrawer(btIDebugDraw* debugDrawer);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+
+#endif //BT_INTERNAL_EDGE_UTILITY_H
+

libs/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp

@@ -0,0 +1,134 @@
+/*
+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 "btManifoldResult.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+
+///This is to allow MaterialCombiner/Custom Friction/Restitution values
+ContactAddedCallback		gContactAddedCallback=0;
+
+///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
+inline btScalar	calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+	btScalar friction = body0->getFriction() * body1->getFriction();
+
+	const btScalar MAX_FRICTION  = btScalar(10.);
+	if (friction < -MAX_FRICTION)
+		friction = -MAX_FRICTION;
+	if (friction > MAX_FRICTION)
+		friction = MAX_FRICTION;
+	return friction;
+
+}
+
+inline btScalar	calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+	return body0->getRestitution() * body1->getRestitution();
+}
+
+
+
+btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* body1)
+		:m_manifoldPtr(0),
+		m_body0(body0),
+		m_body1(body1)
+#ifdef DEBUG_PART_INDEX
+		,m_partId0(-1),
+	m_partId1(-1),
+	m_index0(-1),
+	m_index1(-1)
+#endif //DEBUG_PART_INDEX
+{
+	m_rootTransA = body0->getWorldTransform();
+	m_rootTransB = body1->getWorldTransform();
+}
+
+
+void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+{
+	btAssert(m_manifoldPtr);
+	//order in manifold needs to match
+	
+	if (depth > m_manifoldPtr->getContactBreakingThreshold())
+		return;
+
+	bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
+
+	btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
+
+	btVector3 localA;
+	btVector3 localB;
+	
+	if (isSwapped)
+	{
+		localA = m_rootTransB.invXform(pointA );
+		localB = m_rootTransA.invXform(pointInWorld);
+	} else
+	{
+		localA = m_rootTransA.invXform(pointA );
+		localB = m_rootTransB.invXform(pointInWorld);
+	}
+
+	btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
+	newPt.m_positionWorldOnA = pointA;
+	newPt.m_positionWorldOnB = pointInWorld;
+	
+	int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
+
+	newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1);
+	newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1);
+
+   //BP mod, store contact triangles.
+	if (isSwapped)
+	{
+		newPt.m_partId0 = m_partId1;
+		newPt.m_partId1 = m_partId0;
+		newPt.m_index0  = m_index1;
+		newPt.m_index1  = m_index0;
+	} else
+	{
+		newPt.m_partId0 = m_partId0;
+		newPt.m_partId1 = m_partId1;
+		newPt.m_index0  = m_index0;
+		newPt.m_index1  = m_index1;
+	}
+	//printf("depth=%f\n",depth);
+	///@todo, check this for any side effects
+	if (insertIndex >= 0)
+	{
+		//const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
+		m_manifoldPtr->replaceContactPoint(newPt,insertIndex);
+	} else
+	{
+		insertIndex = m_manifoldPtr->addManifoldPoint(newPt);
+	}
+	
+	//User can override friction and/or restitution
+	if (gContactAddedCallback &&
+		//and if either of the two bodies requires custom material
+		 ((m_body0->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
+		   (m_body1->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
+	{
+		//experimental feature info, for per-triangle material etc.
+		btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
+		btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
+		(*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1);
+	}
+
+}
+

libs/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h

@@ -0,0 +1,128 @@
+/*
+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_RESULT_H
+#define MANIFOLD_RESULT_H
+
+class btCollisionObject;
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btManifoldPoint;
+
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+
+#include "LinearMath/btTransform.h"
+
+typedef bool (*ContactAddedCallback)(btManifoldPoint& cp,	const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1);
+extern ContactAddedCallback		gContactAddedCallback;
+
+//#define DEBUG_PART_INDEX 1
+
+
+///btManifoldResult is a helper class to manage  contact results.
+class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result
+{
+protected:
+
+	btPersistentManifold* m_manifoldPtr;
+
+	//we need this for compounds
+	btTransform	m_rootTransA;
+	btTransform	m_rootTransB;
+
+	btCollisionObject* m_body0;
+	btCollisionObject* m_body1;
+	int	m_partId0;
+	int m_partId1;
+	int m_index0;
+	int m_index1;
+	
+
+public:
+
+	btManifoldResult()
+#ifdef DEBUG_PART_INDEX
+		:
+	m_partId0(-1),
+	m_partId1(-1),
+	m_index0(-1),
+	m_index1(-1)
+#endif //DEBUG_PART_INDEX
+	{
+	}
+
+	btManifoldResult(btCollisionObject* body0,btCollisionObject* body1);
+
+	virtual ~btManifoldResult() {};
+
+	void	setPersistentManifold(btPersistentManifold* manifoldPtr)
+	{
+		m_manifoldPtr = manifoldPtr;
+	}
+
+	const btPersistentManifold*	getPersistentManifold() const
+	{
+		return m_manifoldPtr;
+	}
+	btPersistentManifold*	getPersistentManifold()
+	{
+		return m_manifoldPtr;
+	}
+
+	virtual void setShapeIdentifiersA(int partId0,int index0)
+	{
+		m_partId0=partId0;
+		m_index0=index0;
+	}
+
+	virtual void setShapeIdentifiersB(	int partId1,int index1)
+	{
+		m_partId1=partId1;
+		m_index1=index1;
+	}
+
+
+	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth);
+
+	SIMD_FORCE_INLINE	void refreshContactPoints()
+	{
+		btAssert(m_manifoldPtr);
+		if (!m_manifoldPtr->getNumContacts())
+			return;
+
+		bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
+
+		if (isSwapped)
+		{
+			m_manifoldPtr->refreshContactPoints(m_rootTransB,m_rootTransA);
+		} else
+		{
+			m_manifoldPtr->refreshContactPoints(m_rootTransA,m_rootTransB);
+		}
+	}
+
+	const btCollisionObject* getBody0Internal() const
+	{
+		return m_body0;
+	}
+
+	const btCollisionObject* getBody1Internal() const
+	{
+		return m_body1;
+	}
+	
+};
+
+#endif //MANIFOLD_RESULT_H

libs/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ -0,0 +1,443 @@
+
+/*
+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 "LinearMath/btScalar.h"
+#include "btSimulationIslandManager.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+
+//#include <stdio.h>
+#include "LinearMath/btQuickprof.h"
+
+btSimulationIslandManager::btSimulationIslandManager():
+m_splitIslands(true)
+{
+}
+
+btSimulationIslandManager::~btSimulationIslandManager()
+{
+}
+
+
+void btSimulationIslandManager::initUnionFind(int n)
+{
+		m_unionFind.reset(n);
+}
+		
+
+void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld)
+{
+	
+	{
+		btOverlappingPairCache* pairCachePtr = colWorld->getPairCache();
+		const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs();
+		btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
+		
+		for (int i=0;i<numOverlappingPairs;i++)
+		{
+			const btBroadphasePair& collisionPair = pairPtr[i];
+			btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
+			btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+
+			if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
+				((colObj1) && ((colObj1)->mergesSimulationIslands())))
+			{
+
+				m_unionFind.unite((colObj0)->getIslandTag(),
+					(colObj1)->getIslandTag());
+			}
+		}
+	}
+}
+
+#ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
+void   btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
+{
+
+	// put the index into m_controllers into m_tag   
+	int index = 0;
+	{
+
+		int i;
+		for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
+		{
+			btCollisionObject*   collisionObject= colWorld->getCollisionObjectArray()[i];
+			//Adding filtering here
+			if (!collisionObject->isStaticOrKinematicObject())
+			{
+				collisionObject->setIslandTag(index++);
+			}
+			collisionObject->setCompanionId(-1);
+			collisionObject->setHitFraction(btScalar(1.));
+		}
+	}
+	// do the union find
+
+	initUnionFind( index );
+
+	findUnions(dispatcher,colWorld);
+}
+
+void   btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
+{
+	// put the islandId ('find' value) into m_tag   
+	{
+		int index = 0;
+		int i;
+		for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
+		{
+			btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+			if (!collisionObject->isStaticOrKinematicObject())
+			{
+				collisionObject->setIslandTag( m_unionFind.find(index) );
+				//Set the correct object offset in Collision Object Array
+				m_unionFind.getElement(index).m_sz = i;
+				collisionObject->setCompanionId(-1);
+				index++;
+			} else
+			{
+				collisionObject->setIslandTag(-1);
+				collisionObject->setCompanionId(-2);
+			}
+		}
+	}
+}
+
+
+#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
+void	btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
+{
+
+	initUnionFind( int (colWorld->getCollisionObjectArray().size()));
+
+	// put the index into m_controllers into m_tag	
+	{
+
+		int index = 0;
+		int i;
+		for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
+		{
+			btCollisionObject*	collisionObject= colWorld->getCollisionObjectArray()[i];
+			collisionObject->setIslandTag(index);
+			collisionObject->setCompanionId(-1);
+			collisionObject->setHitFraction(btScalar(1.));
+			index++;
+
+		}
+	}
+	// do the union find
+
+	findUnions(dispatcher,colWorld);
+}
+
+void	btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
+{
+	// put the islandId ('find' value) into m_tag	
+	{
+
+
+		int index = 0;
+		int i;
+		for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
+		{
+			btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+			if (!collisionObject->isStaticOrKinematicObject())
+			{
+				collisionObject->setIslandTag( m_unionFind.find(index) );
+				collisionObject->setCompanionId(-1);
+			} else
+			{
+				collisionObject->setIslandTag(-1);
+				collisionObject->setCompanionId(-2);
+			}
+			index++;
+		}
+	}
+}
+
+#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
+
+inline	int	getIslandId(const btPersistentManifold* lhs)
+{
+	int islandId;
+	const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
+	const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
+	islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
+	return islandId;
+
+}
+
+
+
+/// function object that routes calls to operator<
+class btPersistentManifoldSortPredicate
+{
+	public:
+
+		SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs )
+		{
+			return getIslandId(lhs) < getIslandId(rhs);
+		}
+};
+
+
+void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
+{
+
+	BT_PROFILE("islandUnionFindAndQuickSort");
+	
+	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+
+	m_islandmanifold.resize(0);
+
+	//we are going to sort the unionfind array, and store the element id in the size
+	//afterwards, we clean unionfind, to make sure no-one uses it anymore
+	
+	getUnionFind().sortIslands();
+	int numElem = getUnionFind().getNumElements();
+
+	int endIslandIndex=1;
+	int startIslandIndex;
+
+
+	//update the sleeping state for bodies, if all are sleeping
+	for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
+	{
+		int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+		for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
+		{
+		}
+
+		//int numSleeping = 0;
+
+		bool allSleeping = true;
+
+		int idx;
+		for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+		{
+			int i = getUnionFind().getElement(idx).m_sz;
+
+			btCollisionObject* colObj0 = collisionObjects[i];
+			if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+			{
+//				printf("error in island management\n");
+			}
+
+			btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+			if (colObj0->getIslandTag() == islandId)
+			{
+				if (colObj0->getActivationState()== ACTIVE_TAG)
+				{
+					allSleeping = false;
+				}
+				if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
+				{
+					allSleeping = false;
+				}
+			}
+		}
+			
+
+		if (allSleeping)
+		{
+			int idx;
+			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+			{
+				int i = getUnionFind().getElement(idx).m_sz;
+				btCollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+				{
+//					printf("error in island management\n");
+				}
+
+				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+				if (colObj0->getIslandTag() == islandId)
+				{
+					colObj0->setActivationState( ISLAND_SLEEPING );
+				}
+			}
+		} else
+		{
+
+			int idx;
+			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+			{
+				int i = getUnionFind().getElement(idx).m_sz;
+
+				btCollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+				{
+//					printf("error in island management\n");
+				}
+
+				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+				if (colObj0->getIslandTag() == islandId)
+				{
+					if ( colObj0->getActivationState() == ISLAND_SLEEPING)
+					{
+						colObj0->setActivationState( WANTS_DEACTIVATION);
+						colObj0->setDeactivationTime(0.f);
+					}
+				}
+			}
+		}
+	}
+
+	
+	int i;
+	int maxNumManifolds = dispatcher->getNumManifolds();
+
+//#define SPLIT_ISLANDS 1
+//#ifdef SPLIT_ISLANDS
+
+	
+//#endif //SPLIT_ISLANDS
+
+	
+	for (i=0;i<maxNumManifolds ;i++)
+	{
+		 btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
+		 
+		 btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0());
+		 btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
+		
+		 ///@todo: check sleeping conditions!
+		 if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
+			((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
+		{
+		
+			//kinematic objects don't merge islands, but wake up all connected objects
+			if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
+			{
+				colObj1->activate();
+			}
+			if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
+			{
+				colObj0->activate();
+			}
+			if(m_splitIslands)
+			{ 
+				//filtering for response
+				if (dispatcher->needsResponse(colObj0,colObj1))
+					m_islandmanifold.push_back(manifold);
+			}
+		}
+	}
+}
+
+
+
+///@todo: this is random access, it can be walked 'cache friendly'!
+void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback)
+{
+	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+
+	buildIslands(dispatcher,collisionWorld);
+
+	int endIslandIndex=1;
+	int startIslandIndex;
+	int numElem = getUnionFind().getNumElements();
+
+	BT_PROFILE("processIslands");
+
+	if(!m_splitIslands)
+	{
+		btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
+		int maxNumManifolds = dispatcher->getNumManifolds();
+		callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
+	}
+	else
+	{
+		// Sort manifolds, based on islands
+		// Sort the vector using predicate and std::sort
+		//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
+
+		int numManifolds = int (m_islandmanifold.size());
+
+		//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
+		m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
+
+		//now process all active islands (sets of manifolds for now)
+
+		int startManifoldIndex = 0;
+		int endManifoldIndex = 1;
+
+		//int islandId;
+
+		
+
+	//	printf("Start Islands\n");
+
+		//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
+		for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
+		{
+			int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+
+
+			   bool islandSleeping = false;
+	                
+					for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
+					{
+							int i = getUnionFind().getElement(endIslandIndex).m_sz;
+							btCollisionObject* colObj0 = collisionObjects[i];
+							m_islandBodies.push_back(colObj0);
+							if (!colObj0->isActive())
+									islandSleeping = true;
+					}
+	                
+
+			//find the accompanying contact manifold for this islandId
+			int numIslandManifolds = 0;
+			btPersistentManifold** startManifold = 0;
+
+			if (startManifoldIndex<numManifolds)
+			{
+				int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
+				if (curIslandId == islandId)
+				{
+					startManifold = &m_islandmanifold[startManifoldIndex];
+				
+					for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
+					{
+
+					}
+					/// Process the actual simulation, only if not sleeping/deactivated
+					numIslandManifolds = endManifoldIndex-startManifoldIndex;
+				}
+
+			}
+
+			if (!islandSleeping)
+			{
+				callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
+	//			printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
+			}
+			
+			if (numIslandManifolds)
+			{
+				startManifoldIndex = endManifoldIndex;
+			}
+
+			m_islandBodies.resize(0);
+		}
+	} // else if(!splitIslands) 
+
+}

libs/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h

@@ -0,0 +1,81 @@
+/*
+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 SIMULATION_ISLAND_MANAGER_H
+#define SIMULATION_ISLAND_MANAGER_H
+
+#include "BulletCollision/CollisionDispatch/btUnionFind.h"
+#include "btCollisionCreateFunc.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btCollisionObject.h"
+
+class btCollisionObject;
+class btCollisionWorld;
+class btDispatcher;
+class btPersistentManifold;
+
+
+///SimulationIslandManager creates and handles simulation islands, using btUnionFind
+class btSimulationIslandManager
+{
+	btUnionFind m_unionFind;
+
+	btAlignedObjectArray<btPersistentManifold*>  m_islandmanifold;
+	btAlignedObjectArray<btCollisionObject* >  m_islandBodies;
+	
+	bool m_splitIslands;
+	
+public:
+	btSimulationIslandManager();
+	virtual ~btSimulationIslandManager();
+
+
+	void initUnionFind(int n);	
+	
+		
+	btUnionFind& getUnionFind() { return m_unionFind;}
+
+	virtual	void	updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher);
+	virtual	void	storeIslandActivationState(btCollisionWorld* world);
+
+
+	void	findUnions(btDispatcher* dispatcher,btCollisionWorld* colWorld);
+
+	
+
+	struct	IslandCallback
+	{
+		virtual ~IslandCallback() {};
+
+		virtual	void	ProcessIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold**	manifolds,int numManifolds, int islandId) = 0;
+	};
+
+	void	buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback);
+
+	void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
+
+	bool getSplitIslands()
+	{
+		return m_splitIslands;
+	}
+	void setSplitIslands(bool doSplitIslands)
+	{
+		m_splitIslands = doSplitIslands;
+	}
+
+};
+
+#endif //SIMULATION_ISLAND_MANAGER_H
+

libs/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp

@@ -0,0 +1,260 @@
+/*
+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 "btSphereBoxCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+//#include <stdio.h>
+
+btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
+: btActivatingCollisionAlgorithm(ci,col0,col1),
+m_ownManifold(false),
+m_manifoldPtr(mf),
+m_isSwapped(isSwapped)
+{
+	btCollisionObject* sphereObj = m_isSwapped? col1 : col0;
+	btCollisionObject* boxObj = m_isSwapped? col0 : col1;
+	
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObj,boxObj))
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(sphereObj,boxObj);
+		m_ownManifold = true;
+	}
+}
+
+
+btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+
+
+void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)dispatchInfo;
+	(void)resultOut;
+	if (!m_manifoldPtr)
+		return;
+
+	btCollisionObject* sphereObj = m_isSwapped? body1 : body0;
+	btCollisionObject* boxObj = m_isSwapped? body0 : body1;
+
+
+	btSphereShape* sphere0 = (btSphereShape*)sphereObj->getCollisionShape();
+
+	btVector3 normalOnSurfaceB;
+	btVector3 pOnBox,pOnSphere;
+	btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin();
+	btScalar radius = sphere0->getRadius();
+	
+	btScalar dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius);
+
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
+	if (dist < SIMD_EPSILON)
+	{
+		btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize();
+
+		/// report a contact. internally this will be kept persistent, and contact reduction is done
+
+		resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist);
+		
+	}
+
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr->getNumContacts())
+		{
+			resultOut->refreshContactPoints();
+		}
+	}
+
+}
+
+btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	(void)col0;
+	(void)col1;
+
+	//not yet
+	return btScalar(1.);
+}
+
+
+btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* boxObj, btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius ) 
+{
+
+	btScalar margins;
+	btVector3 bounds[2];
+	btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape();
+	
+	bounds[0] = -boxShape->getHalfExtentsWithoutMargin();
+	bounds[1] = boxShape->getHalfExtentsWithoutMargin();
+
+	margins = boxShape->getMargin();//also add sphereShape margin?
+
+	const btTransform&	m44T = boxObj->getWorldTransform();
+
+	btVector3	boundsVec[2];
+	btScalar	fPenetration;
+
+	boundsVec[0] = bounds[0];
+	boundsVec[1] = bounds[1];
+
+	btVector3	marginsVec( margins, margins, margins );
+
+	// add margins
+	bounds[0] += marginsVec;
+	bounds[1] -= marginsVec;
+
+	/////////////////////////////////////////////////
+
+	btVector3	tmp, prel, n[6], normal, v3P;
+	btScalar   fSep = btScalar(10000000.0), fSepThis;
+
+	n[0].setValue( btScalar(-1.0),  btScalar(0.0),  btScalar(0.0) );
+	n[1].setValue(  btScalar(0.0), btScalar(-1.0),  btScalar(0.0) );
+	n[2].setValue(  btScalar(0.0),  btScalar(0.0), btScalar(-1.0) );
+	n[3].setValue(  btScalar(1.0),  btScalar(0.0),  btScalar(0.0) );
+	n[4].setValue(  btScalar(0.0),  btScalar(1.0),  btScalar(0.0) );
+	n[5].setValue(  btScalar(0.0),  btScalar(0.0),  btScalar(1.0) );
+
+	// convert  point in local space
+	prel = m44T.invXform( sphereCenter);
+	
+	bool	bFound = false;
+
+	v3P = prel;
+
+	for (int i=0;i<6;i++)
+	{
+		int j = i<3? 0:1;
+		if ( (fSepThis = ((v3P-bounds[j]) .dot(n[i]))) > btScalar(0.0) )
+		{
+			v3P = v3P - n[i]*fSepThis;		
+			bFound = true;
+		}
+	}
+	
+	//
+
+	if ( bFound )
+	{
+		bounds[0] = boundsVec[0];
+		bounds[1] = boundsVec[1];
+
+		normal = (prel - v3P).normalize();
+		pointOnBox = v3P + normal*margins;
+		v3PointOnSphere = prel - normal*fRadius;
+
+		if ( ((v3PointOnSphere - pointOnBox) .dot (normal)) > btScalar(0.0) )
+		{
+			return btScalar(1.0);
+		}
+
+		// transform back in world space
+		tmp = m44T( pointOnBox);
+		pointOnBox    = tmp;
+		tmp  = m44T( v3PointOnSphere);		
+		v3PointOnSphere = tmp;
+		btScalar fSeps2 = (pointOnBox-v3PointOnSphere).length2();
+		
+		//if this fails, fallback into deeper penetration case, below
+		if (fSeps2 > SIMD_EPSILON)
+		{
+			fSep = - btSqrt(fSeps2);
+			normal = (pointOnBox-v3PointOnSphere);
+			normal *= btScalar(1.)/fSep;
+		}
+
+		return fSep;
+	}
+
+	//////////////////////////////////////////////////
+	// Deep penetration case
+
+	fPenetration = getSpherePenetration( boxObj,pointOnBox, v3PointOnSphere, sphereCenter, fRadius,bounds[0],bounds[1] );
+
+	bounds[0] = boundsVec[0];
+	bounds[1] = boundsVec[1];
+
+	if ( fPenetration <= btScalar(0.0) )
+		return (fPenetration-margins);
+	else
+		return btScalar(1.0);
+}
+
+btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* boxObj,btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax) 
+{
+
+	btVector3 bounds[2];
+
+	bounds[0] = aabbMin;
+	bounds[1] = aabbMax;
+
+	btVector3	p0, tmp, prel, n[6], normal;
+	btScalar   fSep = btScalar(-10000000.0), fSepThis;
+
+	// set p0 and normal to a default value to shup up GCC
+	p0.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+	normal.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+
+	n[0].setValue( btScalar(-1.0),  btScalar(0.0),  btScalar(0.0) );
+	n[1].setValue(  btScalar(0.0), btScalar(-1.0),  btScalar(0.0) );
+	n[2].setValue(  btScalar(0.0),  btScalar(0.0), btScalar(-1.0) );
+	n[3].setValue(  btScalar(1.0),  btScalar(0.0),  btScalar(0.0) );
+	n[4].setValue(  btScalar(0.0),  btScalar(1.0),  btScalar(0.0) );
+	n[5].setValue(  btScalar(0.0),  btScalar(0.0),  btScalar(1.0) );
+
+	const btTransform&	m44T = boxObj->getWorldTransform();
+
+	// convert  point in local space
+	prel = m44T.invXform( sphereCenter);
+
+	///////////
+
+	for (int i=0;i<6;i++)
+	{
+		int j = i<3 ? 0:1;
+		if ( (fSepThis = ((prel-bounds[j]) .dot( n[i]))-fRadius) > btScalar(0.0) )	return btScalar(1.0);
+		if ( fSepThis > fSep )
+		{
+			p0 = bounds[j];	normal = (btVector3&)n[i];
+			fSep = fSepThis;
+		}
+	}
+
+	pointOnBox = prel - normal*(normal.dot((prel-p0)));
+	v3PointOnSphere = pointOnBox + normal*fSep;
+
+	// transform back in world space
+	tmp  = m44T( pointOnBox);		
+	pointOnBox    = tmp;
+	tmp  = m44T( v3PointOnSphere);		v3PointOnSphere = tmp;
+	normal = (pointOnBox-v3PointOnSphere).normalize();
+
+	return fSep;
+
+}
+

libs/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h

@@ -0,0 +1,75 @@
+/*
+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 SPHERE_BOX_COLLISION_ALGORITHM_H
+#define SPHERE_BOX_COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btPersistentManifold;
+#include "btCollisionDispatcher.h"
+
+#include "LinearMath/btVector3.h"
+
+/// btSphereBoxCollisionAlgorithm  provides sphere-box collision detection.
+/// Other features are frame-coherency (persistent data) and collision response.
+class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool	m_isSwapped;
+	
+public:
+
+	btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
+
+	virtual ~btSphereBoxCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
+
+	btScalar getSphereDistance( btCollisionObject* boxObj,btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius );
+
+	btScalar getSpherePenetration( btCollisionObject* boxObj, btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax);
+	
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm));
+			if (!m_swapped)
+			{
+				return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false);
+			} else
+			{
+				return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true);
+			}
+		}
+	};
+
+};
+
+#endif //SPHERE_BOX_COLLISION_ALGORITHM_H
+

libs/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp

@@ -0,0 +1,105 @@
+/*
+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 "btSphereSphereCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1)
+: btActivatingCollisionAlgorithm(ci,col0,col1),
+m_ownManifold(false),
+m_manifoldPtr(mf)
+{
+	if (!m_manifoldPtr)
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1);
+		m_ownManifold = true;
+	}
+}
+
+btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)dispatchInfo;
+
+	if (!m_manifoldPtr)
+		return;
+
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
+	btSphereShape* sphere0 = (btSphereShape*)col0->getCollisionShape();
+	btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape();
+
+	btVector3 diff = col0->getWorldTransform().getOrigin()-  col1->getWorldTransform().getOrigin();
+	btScalar len = diff.length();
+	btScalar radius0 = sphere0->getRadius();
+	btScalar radius1 = sphere1->getRadius();
+
+#ifdef CLEAR_MANIFOLD
+	m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting
+#endif
+
+	///iff distance positive, don't generate a new contact
+	if ( len > (radius0+radius1))
+	{
+#ifndef CLEAR_MANIFOLD
+		resultOut->refreshContactPoints();
+#endif //CLEAR_MANIFOLD
+		return;
+	}
+	///distance (negative means penetration)
+	btScalar dist = len - (radius0+radius1);
+
+	btVector3 normalOnSurfaceB(1,0,0);
+	if (len > SIMD_EPSILON)
+	{
+		normalOnSurfaceB = diff / len;
+	}
+
+	///point on A (worldspace)
+	///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
+	///point on B (worldspace)
+	btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB;
+
+	/// report a contact. internally this will be kept persistent, and contact reduction is done
+	
+	
+	resultOut->addContactPoint(normalOnSurfaceB,pos1,dist);
+
+#ifndef CLEAR_MANIFOLD
+	resultOut->refreshContactPoints();
+#endif //CLEAR_MANIFOLD
+
+}
+
+btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)col0;
+	(void)col1;
+	(void)dispatchInfo;
+	(void)resultOut;
+
+	//not yet
+	return btScalar(1.);
+}

libs/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h

@@ -0,0 +1,66 @@
+/*
+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 SPHERE_SPHERE_COLLISION_ALGORITHM_H
+#define SPHERE_SPHERE_COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "btCollisionDispatcher.h"
+
+class btPersistentManifold;
+
+/// btSphereSphereCollisionAlgorithm  provides sphere-sphere collision detection.
+/// Other features are frame-coherency (persistent data) and collision response.
+/// Also provides the most basic sample for custom/user btCollisionAlgorithm
+class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	
+public:
+	btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+	btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+		: btActivatingCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
+	
+	virtual ~btSphereSphereCollisionAlgorithm();
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm));
+			return new(mem) btSphereSphereCollisionAlgorithm(0,ci,body0,body1);
+		}
+	};
+
+};
+
+#endif //SPHERE_SPHERE_COLLISION_ALGORITHM_H
+

libs/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp

@@ -0,0 +1,84 @@
+/*
+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 "btSphereTriangleCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "SphereTriangleDetector.h"
+
+
+btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1,bool swapped)
+: btActivatingCollisionAlgorithm(ci,col0,col1),
+m_ownManifold(false),
+m_manifoldPtr(mf),
+m_swapped(swapped)
+{
+	if (!m_manifoldPtr)
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1);
+		m_ownManifold = true;
+	}
+}
+
+btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	if (!m_manifoldPtr)
+		return;
+
+	btCollisionObject* sphereObj = m_swapped? col1 : col0;
+	btCollisionObject* triObj = m_swapped? col0 : col1;
+
+	btSphereShape* sphere = (btSphereShape*)sphereObj->getCollisionShape();
+	btTriangleShape* triangle = (btTriangleShape*)triObj->getCollisionShape();
+	
+	/// report a contact. internally this will be kept persistent, and contact reduction is done
+	resultOut->setPersistentManifold(m_manifoldPtr);
+	SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold());
+	
+	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
+	input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds
+	input.m_transformA = sphereObj->getWorldTransform();
+	input.m_transformB = triObj->getWorldTransform();
+
+	bool swapResults = m_swapped;
+
+	detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults);
+
+	if (m_ownManifold)
+		resultOut->refreshContactPoints();
+	
+}
+
+btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	(void)col0;
+	(void)col1;
+
+	//not yet
+	return btScalar(1.);
+}

libs/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h

@@ -0,0 +1,69 @@
+/*
+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 SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+#define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btPersistentManifold;
+#include "btCollisionDispatcher.h"
+
+/// btSphereSphereCollisionAlgorithm  provides sphere-sphere collision detection.
+/// Other features are frame-coherency (persistent data) and collision response.
+/// Also provides the most basic sample for custom/user btCollisionAlgorithm
+class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool	m_swapped;
+	
+public:
+	btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool swapped);
+
+	btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+		: btActivatingCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
+	
+	virtual ~btSphereTriangleCollisionAlgorithm();
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm));
+
+			return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped);
+		}
+	};
+
+};
+
+#endif //SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+

libs/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp

@@ -0,0 +1,82 @@
+/*
+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 "btUnionFind.h"
+
+
+
+btUnionFind::~btUnionFind()
+{
+	Free();
+
+}
+
+btUnionFind::btUnionFind()
+{ 
+
+}
+
+void	btUnionFind::allocate(int N)
+{
+	m_elements.resize(N);
+}
+void	btUnionFind::Free()
+{
+	m_elements.clear();
+}
+
+
+void	btUnionFind::reset(int N)
+{
+	allocate(N);
+
+	for (int i = 0; i < N; i++) 
+	{ 
+		m_elements[i].m_id = i; m_elements[i].m_sz = 1; 
+	} 
+}
+
+
+class btUnionFindElementSortPredicate
+{
+	public:
+
+		bool operator() ( const btElement& lhs, const btElement& rhs )
+		{
+			return lhs.m_id < rhs.m_id;
+		}
+};
+
+///this is a special operation, destroying the content of btUnionFind.
+///it sorts the elements, based on island id, in order to make it easy to iterate over islands
+void	btUnionFind::sortIslands()
+{
+
+	//first store the original body index, and islandId
+	int numElements = m_elements.size();
+	
+	for (int i=0;i<numElements;i++)
+	{
+		m_elements[i].m_id = find(i);
+#ifndef STATIC_SIMULATION_ISLAND_OPTIMIZATION
+		m_elements[i].m_sz = i;
+#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
+	}
+	
+	 // Sort the vector using predicate and std::sort
+	  //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate);
+	  m_elements.quickSort(btUnionFindElementSortPredicate());
+
+}

libs/bullet/BulletCollision/CollisionDispatch/btUnionFind.h

@@ -0,0 +1,129 @@
+/*
+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 UNION_FIND_H
+#define UNION_FIND_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#define USE_PATH_COMPRESSION 1
+
+///see for discussion of static island optimizations by Vroonsh here: http://code.google.com/p/bullet/issues/detail?id=406
+#define STATIC_SIMULATION_ISLAND_OPTIMIZATION 1
+
+struct	btElement
+{
+	int	m_id;
+	int	m_sz;
+};
+
+///UnionFind calculates connected subsets
+// Implements weighted Quick Union with path compression
+// optimization: could use short ints instead of ints (halving memory, would limit the number of rigid bodies to 64k, sounds reasonable)
+class btUnionFind
+  {
+    private:
+		btAlignedObjectArray<btElement>	m_elements;
+
+    public:
+	  
+		btUnionFind();
+		~btUnionFind();
+
+	
+		//this is a special operation, destroying the content of btUnionFind.
+		//it sorts the elements, based on island id, in order to make it easy to iterate over islands
+		void	sortIslands();
+
+	  void	reset(int N);
+
+	  SIMD_FORCE_INLINE int	getNumElements() const
+	  {
+		  return int(m_elements.size());
+	  }
+	  SIMD_FORCE_INLINE bool  isRoot(int x) const
+	  {
+		  return (x == m_elements[x].m_id);
+	  }
+
+	  btElement&	getElement(int index)
+	  {
+		  return m_elements[index];
+	  }
+	  const btElement& getElement(int index) const
+	  {
+		  return m_elements[index];
+	  }
+   
+	  void	allocate(int N);
+	  void	Free();
+
+
+
+
+	  int find(int p, int q)
+		{ 
+			return (find(p) == find(q)); 
+		}
+
+		void unite(int p, int q)
+		{
+			int i = find(p), j = find(q);
+			if (i == j) 
+				return;
+
+#ifndef USE_PATH_COMPRESSION
+			//weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) )
+			if (m_elements[i].m_sz < m_elements[j].m_sz)
+			{ 
+				m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; 
+			}
+			else 
+			{ 
+				m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz; 
+			}
+#else
+			m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; 
+#endif //USE_PATH_COMPRESSION
+		}
+
+		int find(int x)
+		{ 
+			//btAssert(x < m_N);
+			//btAssert(x >= 0);
+
+			while (x != m_elements[x].m_id) 
+			{
+		//not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically
+	
+		#ifdef USE_PATH_COMPRESSION
+				const btElement* elementPtr = &m_elements[m_elements[x].m_id];
+				m_elements[x].m_id = elementPtr->m_id;
+				x = elementPtr->m_id;			
+		#else//
+				x = m_elements[x].m_id;
+		#endif		
+				//btAssert(x < m_N);
+				//btAssert(x >= 0);
+
+			}
+			return x; 
+		}
+
+
+  };
+
+
+#endif //UNION_FIND_H