use Dispatcher in ConcaveConvexCollisionAlgorithm (so it uses the registered collision algorithm, not hardcoded convexconcave)

improved performance of constraint solver by precalculating the cross product/impulse arm
added collision comparison code: ODE box-box, also sphere-triangle
added safety check into GJK, and an assert for AABB's that are very large
write partid/triangle index outside of GJK

Demos/BasicDemo/BasicDemo.cpp

@@ -15,7 +15,7 @@ subject to the following restrictions:
 
 
 //#define USE_GROUND_BOX 1
-#define PRINT_CONTACT_STATISTICS 1
+//#define PRINT_CONTACT_STATISTICS 1
 //#define CHECK_MEMORY_LEAKS 1
 
 int gNumObjects = 120;
@@ -120,13 +120,12 @@ void	BasicDemo::initPhysics()
 	m_sphereSphereCF = new btSphereSphereCollisionAlgorithm::CreateFunc;
 	m_dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,m_sphereSphereCF);
 
-#ifdef USE_GROUND_BOX
+
 	m_sphereBoxCF = new btSphereBoxCollisionAlgorithm::CreateFunc;
 	m_boxSphereCF = new btSphereBoxCollisionAlgorithm::CreateFunc;
 	m_boxSphereCF->m_swapped = true;
 	m_dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,BOX_SHAPE_PROXYTYPE,m_sphereBoxCF);
 	m_dispatcher->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,m_boxSphereCF);
-#endif //USE_GROUND_BOX
 
 	m_solver = new btSequentialImpulseConstraintSolver;
 
@@ -207,10 +206,10 @@ void	BasicDemo::exitPhysics()
 	//delete collision algorithms creation functions
 	delete m_sphereSphereCF;
 	
-#ifdef USE_GROUND_BOX
+
 	delete m_sphereBoxCF;
 	delete m_boxSphereCF;
-#endif// USE_GROUND_BOX
+
 	//delete solver
 	delete m_solver;
 

Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp

@@ -19,11 +19,14 @@ subject to the following restrictions:
 //#define USER_DEFINED_FRICTION_MODEL 1
 
 //following define allows to compare/replace Bullet's constraint solver with ODE quickstep
-//this define requires to either add the libquickstep library (win32, see msvc/8/libquickstep.vcproj) or manually add the files in Extras/quickstep
+//this define requires to either add the libquickstep library (win32, see msvc/8/libquickstep.vcproj) or manually add the files from Extras/quickstep
 //#define COMPARE_WITH_QUICKSTEP 1
 
 
 #include "btBulletDynamicsCommon.h"
+#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/BoxBoxCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h"
 
 #ifdef COMPARE_WITH_QUICKSTEP
 #include "../Extras/quickstep/OdeConstraintSolver.h"
@@ -79,7 +82,7 @@ btCollisionShape* shapePtr[numShapes] =
 	///Please don't make the box sizes larger then 1000: the collision detection will be inaccurate.
 	///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=346
 
-//#define USE_GROUND_PLANE 1
+#define USE_GROUND_PLANE 1
 #ifdef USE_GROUND_PLANE
 	new btStaticPlaneShape(btVector3(0,1,0),10),
 #else
@@ -89,9 +92,10 @@ btCollisionShape* shapePtr[numShapes] =
 		new btCylinderShape (btVector3(CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin)),
 		//new btCylinderShape (btVector3(1-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin,1-gCollisionMargin)),
 		//new btBoxShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)),
-		new btSphereShape (CUBE_HALF_EXTENTS- 0.05f),
+		//new btConeShape(CUBE_HALF_EXTENTS-gCollisionMargin,2.f*CUBE_HALF_EXTENTS-gCollisionMargin),
-
+		
-		//new btConeShape(CUBE_HALF_EXTENTS,2.f*CUBE_HALF_EXTENTS),
+		new btSphereShape (CUBE_HALF_EXTENTS),
+		
 		//new btBU_Simplex1to4(btPoint3(-1,-1,-1),btPoint3(1,-1,-1),btPoint3(-1,1,-1),btPoint3(0,0,1)),
 
 		//new btEmptyShape(),
@@ -242,6 +246,8 @@ void	CcdPhysicsDemo::initPhysics()
 	
 #ifdef REGISTER_CUSTOM_COLLISION_ALGORITHM
 	dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,new btSphereSphereCollisionAlgorithm::CreateFunc);
+	dispatcher->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE,BOX_SHAPE_PROXYTYPE,new BoxBoxCollisionAlgorithm::CreateFunc);
+	dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,TRIANGLE_SHAPE_PROXYTYPE,new btSphereTriangleCollisionAlgorithm::CreateFunc);
 #endif //REGISTER_CUSTOM_COLLISION_ALGORITHM
 
 #ifdef COMPARE_WITH_QUICKSTEP

Demos/OpenGL/DemoApplication.cpp

@@ -21,6 +21,7 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"//picking
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
 #include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
 
 #include "GL_ShapeDrawer.h"
@@ -426,8 +427,8 @@ void	DemoApplication::shootBox(const btVector3& destination)
 		startTransform.setIdentity();
 		btVector3 camPos = getCameraPosition();
 		startTransform.setOrigin(camPos);
+		//btCollisionShape* boxShape = new btSphereShape(1);
 		btCollisionShape* boxShape = new btBoxShape(btVector3(1.f,1.f,1.f));
-
 		btRigidBody* body = this->localCreateRigidBody(mass, startTransform,boxShape);
 
 		btVector3 linVel(destination[0]-camPos[0],destination[1]-camPos[1],destination[2]-camPos[2]);

Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.cpp

@@ -56,7 +56,7 @@ void BoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btColl
 	resultOut->setPersistentManifold(m_manifoldPtr);
 	
 	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
-	input.m_maximumDistanceSquared;
+	input.m_maximumDistanceSquared = 1e30f;
 	input.m_transformA = body0->m_worldTransform;
 	input.m_transformB = body1->m_worldTransform;
 

src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h

@@ -103,6 +103,10 @@ struct btBroadphaseProxy
 	{
 		return (proxyType == COMPOUND_SHAPE_PROXYTYPE);
 	}
+	static inline bool isInfinite(int proxyType)
+	{
+		return (proxyType == STATIC_PLANE_PROXYTYPE);
+	}
 	
 };
 

src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h

@@ -21,11 +21,14 @@ class btDispatcher;
 class btManifoldResult;
 struct btCollisionObject;
 struct btDispatcherInfo;
+class	btPersistentManifold;
+
 
 struct btCollisionAlgorithmConstructionInfo
 {
 	btCollisionAlgorithmConstructionInfo()
-		:m_dispatcher(0)
+		:m_dispatcher(0),
+		m_manifold(0)
 	{
 	}
 	btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp)
@@ -34,6 +37,7 @@ struct btCollisionAlgorithmConstructionInfo
 	}
 
 	btDispatcher*	m_dispatcher;
+	btPersistentManifold*	m_manifold;
 
 	int	getDispatcherId();
 

src/BulletCollision/BroadphaseCollision/btDispatcher.h

@@ -65,7 +65,7 @@ class btDispatcher
 public:
 	virtual ~btDispatcher() ;
 
-	virtual btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1) = 0;
+	virtual btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold=0) = 0;
 
 	//
 	// asume dispatchers to have unique id's in the range [0..max dispacher]

src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp

@@ -0,0 +1,201 @@
+/*
+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 "SphereTriangleDetector.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+
+SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle)
+:m_sphere(sphere),
+m_triangle(triangle)
+{
+
+}
+
+void	SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+{
+
+	const btTransform& transformA = input.m_transformA;
+	const btTransform& transformB = input.m_transformB;
+
+	btVector3 point,normal;
+	btScalar timeOfImpact = 1.f;
+	btScalar depth = 0.f;
+//	output.m_distance = 1e30f;
+	//move sphere into triangle space
+	btTransform	sphereInTr = transformB.inverseTimes(transformA);
+
+	if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact))
+	{
+		output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
+	}
+
+}
+
+#define MAX_OVERLAP 0.f
+
+
+
+// See also geometrictools.com
+// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
+float SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
+	btVector3 diff = p - from;
+	btVector3 v = to - from;
+	float t = v.dot(diff);
+	
+	if (t > 0) {
+		float 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, float &timeOfImpact)
+{
+
+	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];
+	float distanceFromPlane = p1ToCentre.dot(normal);
+
+	if (distanceFromPlane < 0.f)
+	{
+		//triangle facing the other way
+	
+		distanceFromPlane *= -1.f;
+		normal *= -1.f;
+	}
+
+	///todo: move this gContactBreakingTreshold into a proper structure
+	extern float gContactBreakingTreshold;
+
+	float contactMargin = gContactBreakingTreshold;
+	bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
+	bool isInsideShellPlane = distanceFromPlane < r;
+	
+	float deltaDotNormal = delta.dot(normal);
+	if (!isInsideShellPlane && deltaDotNormal >= 0.0f)
+		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
+			float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
+			btVector3 nearestOnEdge;
+			for (int i = 0; i < m_triangle->getNumEdges(); i++) {
+				
+				btPoint3 pa;
+				btPoint3 pb;
+				
+				m_triangle->getEdge(i,pa,pb);
+
+				float 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;
+		float distanceSqr = contactToCentre.length2();
+		if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) {
+			float distance = sqrtf(distanceSqr);
+			if (1)
+			{
+				resultNormal = contactToCentre;
+				resultNormal.normalize();
+			}
+			point = contactPoint;
+			depth = -(r-distance);
+			return true;
+		}
+
+		if (delta.dot(contactToCentre) >= 0.0f) 
+			return false;
+		
+		// Moving towards the contact point -> collision
+		point = contactPoint;
+		timeOfImpact = 0.0f;
+		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));
+	
+	float 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;
+
+}

src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h

@@ -0,0 +1,48 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPHERE_TRIANGLE_DETECTOR_H
+#define SPHERE_TRIANGLE_DETECTOR_H
+
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+#include "LinearMath/btPoint3.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);
+
+	SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle);
+
+	virtual ~SphereTriangleDetector() {};
+
+private:
+
+	bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, float &timeOfImpact);
+	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;
+
+	
+};
+#endif //SPHERE_TRIANGLE_DETECTOR_H

src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -142,13 +142,14 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
 
 	
 
-btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* body0,btCollisionObject* body1)
+btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold)
 {
 #define USE_DISPATCH_REGISTRY_ARRAY 1
 #ifdef USE_DISPATCH_REGISTRY_ARRAY
 	
 	btCollisionAlgorithmConstructionInfo ci;
 	ci.m_dispatcher = this;
+	ci.m_manifold = sharedManifold;
 	btCollisionAlgorithm* algo = m_doubleDispatch[body0->m_collisionShape->getShapeType()][body1->m_collisionShape->getShapeType()]
 	->CreateCollisionAlgorithm(ci,body0,body1);
 #else
@@ -193,7 +194,7 @@ btCollisionAlgorithmCreateFunc* btCollisionDispatcher::internalFindCreateFunc(in
 
 
 
-btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1)
+btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold)
 {
 	m_count++;
 	
@@ -202,7 +203,7 @@ btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionOb
 	
 	if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConvex() )
 	{
-		return new btConvexConvexAlgorithm(0,ci,body0,body1);
+		return new btConvexConvexAlgorithm(sharedManifold,ci,body0,body1);
 	}
 
 	if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConcave())

src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h

@@ -56,7 +56,7 @@ class btCollisionDispatcher : public btDispatcher
 	btCollisionAlgorithmCreateFunc*	m_swappedCompoundCreateFunc;
 	btCollisionAlgorithmCreateFunc*   m_emptyCreateFunc;
 
-	btCollisionAlgorithm* internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1);
+	btCollisionAlgorithm* internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold = 0);
 
 public:
 
@@ -114,7 +114,7 @@ public:
 	virtual void clearManifold(btPersistentManifold* manifold);
 
 			
-	btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1);
+	btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold = 0);
 		
 	virtual bool	needsCollision(btCollisionObject* body0,btCollisionObject* body1);
 	

src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -17,7 +17,6 @@ subject to the following restrictions:
 #include "btConvexConcaveCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
-#include "btConvexConvexAlgorithm.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 #include "BulletCollision/CollisionShapes/btConcaveShape.h"
 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
@@ -115,10 +114,16 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i
 		btCollisionShape* tmpShape = ob->m_collisionShape;
 		ob->m_collisionShape = &tm;
 		
+
+		btCollisionAlgorithm* colAlgo = ci.m_dispatcher->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr);
 		///this should use the btDispatcher, so the actual registered algorithm is used
-		btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
+		//		btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
-		cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
+
-		cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+		m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex);
+	//	cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
+//		cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+		colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+		delete colAlgo;
 		ob->m_collisionShape = tmpShape;
 
 	}

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -172,7 +172,7 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 	
 	input.m_transformA = body0->m_worldTransform;
 	input.m_transformB = body1->m_worldTransform;
-    
+	
 	resultOut->setPersistentManifold(m_manifoldPtr);
 	m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
 

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h

@@ -54,13 +54,6 @@ public:
 
 	void	setLowLevelOfDetail(bool useLowLevel);
 
-	virtual void setShapeIdentifiers(int partId0,int index0,	int partId1,int index1)
-	{
-			m_gjkPairDetector.m_partId0=partId0;
-			m_gjkPairDetector.m_partId1=partId1;
-			m_gjkPairDetector.m_index0=index0;
-			m_gjkPairDetector.m_index1=index1;		
-	}
 
 	const btPersistentManifold*	getManifold()
 	{
@@ -71,7 +64,7 @@ public:
 	{
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
-			return new btConvexConvexAlgorithm(0,ci,body0,body1);
+			return new btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1);
 		}
 	};
 

src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp

@@ -0,0 +1,71 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#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)
+: btCollisionAlgorithm(ci),
+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;
+
+	btSphereShape* sphere = (btSphereShape*)col0->m_collisionShape;
+	btTriangleShape* triangle = (btTriangleShape*)col1->m_collisionShape;
+	
+	/// report a contact. internally this will be kept persistent, and contact reduction is done
+	resultOut->setPersistentManifold(m_manifoldPtr);
+	SphereTriangleDetector detector(sphere,triangle);
+	
+	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
+	input.m_maximumDistanceSquared = 1e30f;//todo: tighter bounds
+	input.m_transformA = col0->m_worldTransform;
+	input.m_transformB = col1->m_worldTransform;
+
+	detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+
+}
+
+float btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	//not yet
+	return 1.f;
+}

src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h

@@ -0,0 +1,59 @@
+/*
+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 "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.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 btSphereTriangleCollisionAlgorithm : public btCollisionAlgorithm
+{
+	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)
+		: btCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	
+	virtual ~btSphereTriangleCollisionAlgorithm();
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			
+				return new btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped);
+		}
+	};
+
+};
+
+#endif //SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+

src/BulletCollision/CollisionShapes/btCollisionShape.h

@@ -66,6 +66,12 @@ public:
 		return btBroadphaseProxy::isCompound(getShapeType());
 	}
 
+	///isInfinite is used to catch simulation error (aabb check)
+	inline bool isInfinite() const
+	{
+		return btBroadphaseProxy::isInfinite(getShapeType());
+	}
+
 	virtual void	setLocalScaling(const btVector3& scaling) =0;
 	virtual const btVector3& getLocalScaling() const =0;
 

src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

@@ -36,11 +36,7 @@ m_penetrationDepthSolver(penetrationDepthSolver),
 m_simplexSolver(simplexSolver),
 m_minkowskiA(objectA),
 m_minkowskiB(objectB),
-m_ignoreMargin(false),
+m_ignoreMargin(false)
-m_partId0(-1),
-m_index0(-1),
-m_partId1(-1),
-m_index1(-1)
 {
 }
 
@@ -60,7 +56,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 		marginB = 0.f;
 	}
 
-int curIter = 0;
+	int curIter = 0;
+	int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
 
 	bool isValid = false;
 	bool checkSimplex = false;
@@ -131,6 +128,25 @@ int curIter = 0;
 				checkSimplex = true;
 				break;
 			}
+
+			  //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
+              if (curIter++ > gGjkMaxIter)   
+              {   
+                      #if defined(DEBUG) || defined (_DEBUG)   
+                              printf("btGjkPairDetector maxIter exceeded:%i\n",curIter);   
+                              printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",   
+                              m_cachedSeparatingAxis.getX(),   
+                              m_cachedSeparatingAxis.getY(),   
+                              m_cachedSeparatingAxis.getZ(),   
+                              squaredDistance,   
+                              m_minkowskiA->getShapeType(),   
+                              m_minkowskiB->getShapeType());   
+                      #endif   
+                      break;   
+
+              } 
+
+
 			bool check = (!m_simplexSolver->fullSimplex());
 			//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
 
@@ -200,7 +216,6 @@ int curIter = 0;
 		//spu_printf("distance\n");
 #endif //__CELLOS_LV2__
 
-		output.setShapeIdentifiers(m_partId0,m_index0,m_partId1,m_index1);
 
 		output.addContactPoint(
 			normalInB,

src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h

@@ -43,12 +43,6 @@ class btGjkPairDetector : public btDiscreteCollisionDetectorInterface
 
 public:
 
-	//experimental feature information, per triangle, per convex etc.
-	//'material combiner' / contact added callback
-	int	m_partId0;
-	int	m_index0;
-	int	m_partId1;
-	int	m_index1;
 
 	btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver);
 	virtual ~btGjkPairDetector() {};

src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

@@ -24,16 +24,7 @@ subject to the following restrictions:
 
 #define ASSERT2 assert
 
-//some values to find stable tresholds
+#define USE_INTERNAL_APPLY_IMPULSE 1
-
-float useGlobalSettingContacts = false;//true;
-btScalar contactDamping = 0.2f;
-btScalar contactTau = .02f;//0.02f;//*0.02f;
-
-
-
-
-
 
 
 //bilateral constraint between two dynamic objects
@@ -75,7 +66,9 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
 
 
 	rel_vel = normal.dot(vel);
-		
+	
+	//todo: move this into proper structure
+	btScalar contactDamping = 0.2f;
 
 #ifdef ONLY_USE_LINEAR_MASS
 	btScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
@@ -88,25 +81,17 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
 
 
 
-
-//velocity + friction
 //response  between two dynamic objects with friction
 float resolveSingleCollision(
 	btRigidBody& body1,
 	btRigidBody& body2,
 	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo
+	const btContactSolverInfo& solverInfo)
-
-		)
 {
 
 	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
 	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-    
+   	const btVector3& normal = contactPoint.m_normalWorldOnB;
-	
-//	printf("distance=%f\n",distance);
-
-	const btVector3& normal = contactPoint.m_normalWorldOnB;
 
 	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
 	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
@@ -117,34 +102,18 @@ float resolveSingleCollision(
 	btScalar rel_vel;
 	rel_vel = normal.dot(vel);
 	
-
 	btScalar Kfps = 1.f / solverInfo.m_timeStep ;
 
 	float damping = solverInfo.m_damping ;
 	float Kerp = solverInfo.m_erp;
-	
-	if (useGlobalSettingContacts)
-	{
-		damping = contactDamping;
-		Kerp = contactTau;
-	} 
-
 	float Kcor = Kerp *Kfps;
 
-	//printf("dist=%f\n",distance);
+	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-
-		btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
 	assert(cpd);
-
+	btScalar distance = cpd->m_penetration;
-	btScalar distance = cpd->m_penetration;//contactPoint.getDistance();
-	
-
-	//distance = 0.f;
 	btScalar positionalError = Kcor *-distance;
-	//jacDiagABInv;
 	btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
 
-	
 	btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
 
 	btScalar	velocityImpulse = velocityError * cpd->m_jacDiagABInv;
@@ -158,9 +127,20 @@ float resolveSingleCollision(
 
 	normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
 
+#ifdef USE_INTERNAL_APPLY_IMPULSE
+	if (body1.getInvMass())
+	{
+		body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
+	}
+	if (body2.getInvMass())
+	{
+		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
+	}
+#else USE_INTERNAL_APPLY_IMPULSE
 	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
 	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
-	
+#endif //USE_INTERNAL_APPLY_IMPULSE
+
 	return normalImpulse;
 }
 
@@ -169,9 +149,86 @@ float resolveSingleFriction(
 	btRigidBody& body1,
 	btRigidBody& body2,
 	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo
+	const btContactSolverInfo& solverInfo)
+{
 
-		)
+	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
+	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
+
+	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	
+	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+
+	float combinedFriction = cpd->m_friction;
+	
+	btScalar limit = cpd->m_appliedImpulse * combinedFriction;
+	//if (contactPoint.m_appliedImpulse>0.f)
+	//friction
+	{
+		//apply friction in the 2 tangential directions
+		
+		// 1st tangent
+		btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+		btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+		btVector3 vel = vel1 - vel2;
+	
+		btScalar j1,j2;
+
+		{
+				
+			btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
+
+			// calculate j that moves us to zero relative velocity
+			j1 = -vrel * cpd->m_jacDiagABInvTangent0;
+			float total = cpd->m_accumulatedTangentImpulse0 + j1;
+			GEN_set_min(total, limit);
+			GEN_set_max(total, -limit);
+			j1 = total - cpd->m_accumulatedTangentImpulse0;
+			cpd->m_accumulatedTangentImpulse0 = total;
+		}
+		{
+			// 2nd tangent
+
+			btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
+			
+			// calculate j that moves us to zero relative velocity
+			j2 = -vrel * cpd->m_jacDiagABInvTangent1;
+			float total = cpd->m_accumulatedTangentImpulse1 + j2;
+			GEN_set_min(total, limit);
+			GEN_set_max(total, -limit);
+			j2 = total - cpd->m_accumulatedTangentImpulse1;
+			cpd->m_accumulatedTangentImpulse1 = total;
+		}
+
+#ifdef USE_INTERNAL_APPLY_IMPULSE
+	if (body1.getInvMass())
+	{
+		body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
+		body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
+	}
+	if (body2.getInvMass())
+	{
+		body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
+		body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);	
+	}
+#else USE_INTERNAL_APPLY_IMPULSE
+	body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
+	body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
+#endif //USE_INTERNAL_APPLY_IMPULSE
+
+
+	} 
+	return cpd->m_appliedImpulse;
+}
+
+
+float resolveSingleFrictionOriginal(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
 {
 
 	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
@@ -232,3 +289,110 @@ float resolveSingleFriction(
 	} 
 	return cpd->m_appliedImpulse;
 }
+
+
+//velocity + friction
+//response  between two dynamic objects with friction
+float resolveSingleCollisionCombined(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
+{
+
+	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
+	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
+   	const btVector3& normal = contactPoint.m_normalWorldOnB;
+
+	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	
+	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+	btVector3 vel = vel1 - vel2;
+	btScalar rel_vel;
+	rel_vel = normal.dot(vel);
+	
+	btScalar Kfps = 1.f / solverInfo.m_timeStep ;
+
+	float damping = solverInfo.m_damping ;
+	float Kerp = solverInfo.m_erp;
+	float Kcor = Kerp *Kfps;
+
+	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+	btScalar distance = cpd->m_penetration;
+	btScalar positionalError = Kcor *-distance;
+	btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
+
+	btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
+
+	btScalar	velocityImpulse = velocityError * cpd->m_jacDiagABInv;
+
+	btScalar normalImpulse = penetrationImpulse+velocityImpulse;
+	
+	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+	float oldNormalImpulse = cpd->m_appliedImpulse;
+	float sum = oldNormalImpulse + normalImpulse;
+	cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum;
+
+	normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
+
+
+#ifdef USE_INTERNAL_APPLY_IMPULSE
+	if (body1.getInvMass())
+	{
+		body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
+	}
+	if (body2.getInvMass())
+	{
+		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
+	}
+#else USE_INTERNAL_APPLY_IMPULSE
+	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
+	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
+#endif //USE_INTERNAL_APPLY_IMPULSE
+
+	{
+		//friction
+		btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+		btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+		btVector3 vel = vel1 - vel2;
+	  
+		rel_vel = normal.dot(vel);
+
+
+		btVector3 lat_vel = vel - normal * rel_vel;
+		btScalar lat_rel_vel = lat_vel.length();
+
+		float combinedFriction = cpd->m_friction;
+
+		if (cpd->m_appliedImpulse > 0)
+		if (lat_rel_vel > SIMD_EPSILON)
+		{
+			lat_vel /= lat_rel_vel;
+			btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
+			btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
+			btScalar friction_impulse = lat_rel_vel /
+				(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
+			btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
+
+			GEN_set_min(friction_impulse, normal_impulse);
+			GEN_set_max(friction_impulse, -normal_impulse);
+			body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
+			body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
+		}
+	}
+
+
+
+	return normalImpulse;
+}
+float resolveSingleFrictionEmpty(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
+{
+	return 0.f;
+};

src/BulletDynamics/ConstraintSolver/btContactConstraint.h

@@ -72,6 +72,15 @@ struct btConstraintPersistentData
 			float	m_penetration;
 			btVector3	m_frictionWorldTangential0;
 			btVector3	m_frictionWorldTangential1;
+
+			btVector3	m_frictionAngularComponent0A;
+			btVector3	m_frictionAngularComponent0B;
+			btVector3	m_frictionAngularComponent1A;
+			btVector3	m_frictionAngularComponent1B;
+
+			//some data doesn't need to be persistent over frames: todo: clean/reuse this
+			btVector3	m_angularComponentA;
+			btVector3	m_angularComponentB;
 		
 			ContactSolverFunc	m_contactSolverFunc;
 			ContactSolverFunc	m_frictionSolverFunc;

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -74,8 +74,6 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
 		for (j=0;j<numManifolds;j++)
 		{
 			int k=j;
-			//interleaving the preparation with solving impacts the behaviour a lot, todo: find out why
-
 			prepareConstraints(manifoldPtr[k],info,debugDrawer);
 			solve(manifoldPtr[k],info,0,debugDrawer);
 		}
@@ -232,6 +230,7 @@ void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 					cpd->m_penetration = 0.f;
 				} 				
 				
+				
 
 				float relaxation = info.m_damping;
 				cpd->m_appliedImpulse *= relaxation;
@@ -266,6 +265,36 @@ void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 	#endif //NO_FRICTION_WARMSTART
 					cp.m_normalWorldOnB*cpd->m_appliedImpulse;
 
+
+
+				///
+				{
+				btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+				cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
+				btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
+				cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
+				}
+				{
+					btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
+					cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
+				}
+				{
+					btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
+					cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
+				}
+				{
+					btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
+					cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
+				}
+				{
+					btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
+					cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
+				}
+				
+				///
+
+
+
 				//apply previous frames impulse on both bodies
 				body0->applyImpulse(totalImpulse, rel_pos1);
 				body1->applyImpulse(-totalImpulse, rel_pos2);

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -502,7 +502,28 @@ void	btDiscreteDynamicsWorld::updateAabbs()
 				btPoint3 minAabb,maxAabb;
 				colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb);
 				btSimpleBroadphase* bp = (btSimpleBroadphase*)m_broadphasePairCache;
-				bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb);
+				if ( colObj->m_collisionShape->isInfinite() || ((maxAabb-minAabb).length2() < 1e12f))
+				{
+					bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb);
+				} else
+				{
+					//something went wrong, investigate
+					//this assert is unwanted in 3D modelers (danger of loosing work)
+					assert(0);
+					body->SetActivationState(DISABLE_SIMULATION);
+					
+					static bool reportMe = true;
+					if (reportMe)
+					{
+						reportMe = false;
+						printf("Overflow in AABB, object removed from simulation \n");
+						printf("If you can reproduce this, please email bugs@continuousphysics.com\n");
+						printf("Please include above information, your Platform, version of OS.\n");
+						printf("Thanks.\n");
+					}
+
+
+				}
 				if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
 				{
 					DrawAabb(m_debugDrawer,minAabb,maxAabb,colorvec);

src/BulletDynamics/Dynamics/btRigidBody.h

@@ -158,6 +158,16 @@ public:
 			applyTorqueImpulse(rel_pos.cross(impulse));
 		}
 	}
+
+	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+	inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,float impulseMagnitude)
+	{
+		if (m_inverseMass != 0.f)
+		{
+			m_linearVelocity += linearComponent*impulseMagnitude;
+			m_angularVelocity += angularComponent*impulseMagnitude;
+		}
+	}
 	
 	void clearForces() 
 	{