+ fixed btMinkowskiSumShape Sa+b(v) = Sa(v)-Sb(-v) instead of Sa(v)+Sb(v)

+ fix related btMinkowskiSumShape issue in btSubsimplexConvexCast and btGjkConvexCast
+ add hitpoint for btSubsimplexConvexCast
+ reduce maximum number of iterations in conservative advancement/CCD implementations

Demos/GjkConvexCastDemo/LinearConvexCastDemo.cpp

@@ -84,6 +84,7 @@ void LinearConvexCastDemo::initPhysics()
 	shapeA->addPoint( btVector3(   -r, -0.25f * h,   -r ) );
 
 
+
 	// Triangle
 	btConvexHullShape* shapeB = new btConvexHullShape;
 	shapeB->addPoint( btVector3(  0.0f,  1.0f, 0.0f ) );
@@ -118,25 +119,44 @@ void LinearConvexCastDemo::displayCallback(void)
 
 	GL_ShapeDrawer::drawCoordSystem();
 
+	
+
+	static btScalar angle = 0.f;
+	angle+=getDeltaTimeMicroseconds()/1000000.0;
+
+	tr[1].setRotation(btQuaternion(btVector3(1,0,0),angle));
+
 	btTransform toA, toB;
-	toA.setIdentity();
+	toA = tr[0];
-	toA.setOrigin( btVector3( 0.0f, 1.5f, 0.0f  ) );
+	toA.setOrigin( btVector3( 0.0f, 0.f, 0.0f  ) );
-	toB.setIdentity();
+	toB = tr[1];
-	toB.setOrigin( btVector3( 0.0f, 4.0f, 0.0f  ) );
+	toB.setOrigin( btVector3( 0.0f, 0.0f, 0.0f  ) );
 
 
 	gGjkSimplexSolver.reset();
 	
-	btGjkConvexCast convexCaster(shapePtr[ 0 ], shapePtr[ 1 ], &gGjkSimplexSolver );
+
-	//btSubsimplexConvexCast convexCaster( shapePtr[ 0 ], shapePtr[ 1 ], &gGjkSimplexSolver );
+	//btGjkConvexCast convexCaster(shapePtr[ 0 ], shapePtr[ 1 ], &gGjkSimplexSolver );
+	btSubsimplexConvexCast convexCaster( shapePtr[ 0 ], shapePtr[ 1 ], &gGjkSimplexSolver );
 
 	btConvexCast::CastResult result;
+
+	result.m_hitPoint.setValue(0,0,0);
+
 	convexCaster.calcTimeOfImpact( tr[ 0 ], toA, tr[ 1 ], toB, result );
 
-	btScalar m1[16], m2[16];
+	btScalar m1[16], m2[16],m3[16];
 	tr[ 0 ].getOpenGLMatrix( m1 );
 	tr[ 1 ].getOpenGLMatrix( m2 );
 
+	btSphereShape	sphere(0.2);
+
+	btTransform tmp = tr[0];
+	tmp.setOrigin(result.m_hitPoint);
+	tmp.getOpenGLMatrix(m3);
+	m_shapeDrawer.drawOpenGL( m3, &sphere, btVector3( 1, 0, 1 ), getDebugMode() );
+
+
 	m_shapeDrawer.drawOpenGL( m1, shapePtr[ 0 ], btVector3( 1, 0, 0 ), getDebugMode() );
 	m_shapeDrawer.drawOpenGL( m2, shapePtr[ 1 ], btVector3( 1, 0, 0 ), getDebugMode() );
 

Demos/Raytracer/Raytracer.cpp

@@ -101,6 +101,7 @@ void	Raytracer::initPhysics()
 	{
 		transforms[i].setIdentity();
 		btVector3	pos(0.f,0.f,-(2.5* numObjects * 0.5)+i*2.5f);
+		transforms[i].setIdentity();
 		transforms[i].setOrigin( pos );
 		btQuaternion orn;
 		if (i < 2)

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -33,7 +33,6 @@ subject to the following restrictions:
 
 
 
-#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
 

src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp

@@ -26,9 +26,9 @@ m_shapeB(shapeB)
 
 btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
 {
-	btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec*m_transA.getBasis()));
+	btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(-vec*m_transA.getBasis()));
 	btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(vec*m_transB.getBasis()));
-	return supVertexA + supVertexB;
+	return  supVertexA - supVertexB;
 }
 
 void	btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const

src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp

@@ -16,7 +16,6 @@ subject to the following restrictions:
 
 #include "btContinuousConvexCollision.h"
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
-#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
 #include "LinearMath/btTransformUtil.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
@@ -35,7 +34,7 @@ m_convexA(convexA),m_convexB(convexB)
 
 /// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
 /// You don't want your game ever to lock-up.
-#define MAX_ITERATIONS 1000
+#define MAX_ITERATIONS 64
 
 bool	btContinuousConvexCollision::calcTimeOfImpact(
 				const btTransform& fromA,

src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp

@@ -17,10 +17,15 @@ subject to the following restrictions:
 
 #include "btGjkConvexCast.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
-#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
 #include "btGjkPairDetector.h"
 #include "btPointCollector.h"
+#include "LinearMath/btTransformUtil.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define MAX_ITERATIONS 64
+#else
+#define MAX_ITERATIONS 32
+#endif
 
 btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
 :m_simplexSolver(simplexSolver),
@@ -38,121 +43,113 @@ bool	btGjkConvexCast::calcTimeOfImpact(
 {
 
 
-	btMinkowskiSumShape combi(m_convexA,m_convexB);
+	m_simplexSolver->reset();
-	btMinkowskiSumShape* convex = &combi;
 
-	btTransform	rayFromLocalA;
+	/// compute linear velocity for this interval, to interpolate
-	btTransform	rayToLocalA;
+	//assume no rotation/angular velocity, assert here?
-
+	btVector3 linVelA,linVelB;
-	rayFromLocalA = fromA.inverse()* fromB;
+	linVelA = toA.getOrigin()-fromA.getOrigin();
-	rayToLocalA = toA.inverse()* toB;
+	linVelB = toB.getOrigin()-fromB.getOrigin();
-
-
-	btTransform trA,trB;
-	trA = btTransform(fromA);
-	trB = btTransform(fromB);
-	trA.setOrigin(btPoint3(0,0,0));
-	trB.setOrigin(btPoint3(0,0,0));
-
-	convex->setTransformA(trA);
-	convex->setTransformB(trB);
-
-
-
-
-	btScalar radius = btScalar(0.01);
 
+	btScalar radius = btScalar(0.001);
 	btScalar lambda = btScalar(0.);
-	btVector3 s = rayFromLocalA.getOrigin();
+	btVector3 v(1,0,0);
-	btVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin();
+
-	btVector3 x = s;
+	int maxIter = MAX_ITERATIONS;
+
 	btVector3 n;
-	n.setValue(0,0,0);
+	n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
 	bool hasResult = false;
 	btVector3 c;
+	btVector3 r = (linVelA-linVelB);
 
 	btScalar lastLambda = lambda;
+	//btScalar epsilon = btScalar(0.001);
 
+	int numIter = 0;
 	//first solution, using GJK
 
-	//no penetration support for now, perhaps pass a pointer when we really want it
-	btConvexPenetrationDepthSolver* penSolverPtr = 0;
 
 	btTransform identityTrans;
 	identityTrans.setIdentity();
 
-	btSphereShape	raySphere(btScalar(0.0));
-	raySphere.setMargin(btScalar(0.));
 
-	btTransform sphereTr;
+//	result.drawCoordSystem(sphereTr);
-	sphereTr.setIdentity();
-	sphereTr.setOrigin( rayFromLocalA.getOrigin());
 
-	result.drawCoordSystem(sphereTr);
+	btPointCollector	pointCollector;
-	{
-		btPointCollector	pointCollector1;
-		btGjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr);		
 
-		btGjkPairDetector::ClosestPointInput input;
+		
-		input.m_transformA = sphereTr;
+	btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver);		
-		input.m_transformB = identityTrans;
+	btGjkPairDetector::ClosestPointInput input;
-		gjk.getClosestPoints(input,pointCollector1,0);
 
-		hasResult = pointCollector1.m_hasResult;
+	//we don't use margins during CCD
-		c = pointCollector1.m_pointInWorld;
+	//	gjk.setIgnoreMargin(true);
-		n = pointCollector1.m_normalOnBInWorld;
-	}
 
-	
+	input.m_transformA = fromA;
+	input.m_transformB = fromB;
+	gjk.getClosestPoints(input,pointCollector,0);
+
+	hasResult = pointCollector.m_hasResult;
+	c = pointCollector.m_pointInWorld;
 
 	if (hasResult)
 	{
 		btScalar dist;
-		dist = (c-x).length();
+		dist = pointCollector.m_distance;
-		if (dist < radius)
+		n = pointCollector.m_normalOnBInWorld;
-		{
+
-			//penetration
+	
-			lastLambda = btScalar(1.);
-		}
 
 		//not close enough
 		while (dist > radius)
 		{
-			
+			numIter++;
-			n = x - c;
+			if (numIter > maxIter)
-			btScalar nDotr = n.dot(r);
+			{
+				return false; //todo: report a failure
+			}
+			btScalar dLambda = btScalar(0.);
 
-			if (nDotr >= -(SIMD_EPSILON*SIMD_EPSILON))
+			btScalar projectedLinearVelocity = r.dot(n);
+			
+			dLambda = dist / (projectedLinearVelocity);
+
+			lambda = lambda - dLambda;
+
+			if (lambda > btScalar(1.))
 				return false;
-			
-			lambda = lambda - n.dot(n) / nDotr;
-			if (lambda <= lastLambda)
-				break;
 
+			if (lambda < btScalar(0.))
+				return false;
+
+			//todo: next check with relative epsilon
+			if (lambda <= lastLambda)
+			{
+				return false;
+				//n.setValue(0,0,0);
+				break;
+			}
 			lastLambda = lambda;
 
-			x = s + lambda * r;
+			//interpolate to next lambda
-
+			result.DebugDraw( lambda );
-			sphereTr.setOrigin( x );
+			input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
-			result.drawCoordSystem(sphereTr);
+			input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
-			btPointCollector	pointCollector;
+			
-			btGjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr);
-			btGjkPairDetector::ClosestPointInput input;
-			input.m_transformA = sphereTr;
-			input.m_transformB = identityTrans;
 			gjk.getClosestPoints(input,pointCollector,0);
 			if (pointCollector.m_hasResult)
 			{
 				if (pointCollector.m_distance < btScalar(0.))
 				{
-					//degeneracy, report a hit
 					result.m_fraction = lastLambda;
-					result.m_normal = n;
+					n = pointCollector.m_normalOnBInWorld;
+					result.m_normal=n;
 					result.m_hitPoint = pointCollector.m_pointInWorld;
 					return true;
 				}
-				c = pointCollector.m_pointInWorld;			
+				c = pointCollector.m_pointInWorld;		
-				dist = (c-x).length();
+				n = pointCollector.m_normalOnBInWorld;
+				dist = pointCollector.m_distance;
 			} else
 			{
 				//??
@@ -161,16 +158,14 @@ bool	btGjkConvexCast::calcTimeOfImpact(
 
 		}
 
-		if (lastLambda < btScalar(1.))
+		result.m_fraction = lambda;
-		{
+		result.m_normal = n;
-		
+		result.m_hitPoint = c;
-			result.m_fraction = lastLambda;
+		return true;
-			result.m_normal = n;
-			result.m_hitPoint = c;
-			return true;
-		}
 	}
 
 	return false;
+
+
 }
 

src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp

@@ -14,11 +14,10 @@ subject to the following restrictions:
 */
 
 #include "btMinkowskiPenetrationDepthSolver.h"
-#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
-
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
 
 
 

src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp

@@ -16,9 +16,11 @@ subject to the following restrictions:
 
 #include "btSubSimplexConvexCast.h"
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
+
 #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
-
+#include "btPointCollector.h"
+#include "LinearMath/btTransformUtil.h"
 
 btSubsimplexConvexCast::btSubsimplexConvexCast (const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
 :m_simplexSolver(simplexSolver),
@@ -41,34 +43,24 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 		CastResult& result)
 {
 
-	btMinkowskiSumShape combi(m_convexA,m_convexB);
-	btMinkowskiSumShape* convex = &combi;
-
-	btTransform	rayFromLocalA;
-	btTransform	rayToLocalA;
-
-	rayFromLocalA = fromA.inverse()* fromB;
-	rayToLocalA = toA.inverse()* toB;
-
-
 	m_simplexSolver->reset();
 
-	convex->setTransformB(btTransform(rayFromLocalA.getBasis()));
+	btVector3 linVelA,linVelB;
-
+	linVelA = toA.getOrigin()-fromA.getOrigin();
-	//btScalar radius = btScalar(0.01);
+	linVelB = toB.getOrigin()-fromB.getOrigin();
 
 	btScalar lambda = btScalar(0.);
-	//todo: need to verify this:
-	//because of minkowski difference, we need the inverse direction
-	
-	btVector3 s = -rayFromLocalA.getOrigin();
-	btVector3 r = -(rayToLocalA.getOrigin()-rayFromLocalA.getOrigin());
-	btVector3 x = s;
-	btVector3 v;
-	btVector3 arbitraryPoint = convex->localGetSupportingVertex(r);
-	
-	v = x - arbitraryPoint;
 
+	btTransform interpolatedTransA = fromA;
+	btTransform interpolatedTransB = fromB;
+
+	///take relative motion
+	btVector3 r = (linVelA-linVelB);
+	btVector3 v;
+	
+	btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis()));
+	btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis()));
+	v = supVertexA-supVertexB;
 	int maxIter = MAX_ITERATIONS;
 
 	btVector3 n;
@@ -90,8 +82,9 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 	
 	while ( (dist2 > epsilon) && maxIter--)
 	{
-		p = convex->localGetSupportingVertex( v);
+		supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis()));
-		 w = x - p;
+		supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis()));
+		w = supVertexA-supVertexB;
 
 		btScalar VdotW = v.dot(w);
 
@@ -104,16 +97,19 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 			else
 			{
 				lambda = lambda - VdotW / VdotR;
-				x = s + lambda * r;
+				//interpolate to next lambda
-				m_simplexSolver->reset();
+				//	x = s + lambda * r;
+				interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
+				interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
+				//m_simplexSolver->reset();
 				//check next line
-				w = x-p;
+				 w = supVertexA-supVertexB;
 				lastLambda = lambda;
 				n = v;
 				hasResult = true;
 			}
 		} 
-		m_simplexSolver->addVertex( w, x , p);
+		m_simplexSolver->addVertex( w, supVertexA , supVertexB);
 		if (m_simplexSolver->closest(v))
 		{
 			dist2 = v.length2();
@@ -130,8 +126,10 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 	//int numiter = MAX_ITERATIONS - maxIter;
 //	printf("number of iterations: %d", numiter);
 	result.m_fraction = lambda;
-	result.m_normal = n;
+	result.m_normal = n.normalized();
-
+	btVector3 hitA,hitB;
+	m_simplexSolver->compute_points(hitA,hitB);
+	result.m_hitPoint=hitB;
 	return true;
 }