fix and re-enable sphere-box collision algorithm, Issue 643

Thanks to Laurent for the fix and Thomas for adapting the fix to latest trunk revision!

src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp

@@ -22,9 +22,7 @@ subject to the following restrictions:
 #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"
@@ -72,13 +70,11 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault
 	
 	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;
@@ -181,12 +177,10 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
 	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);
@@ -220,7 +214,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg
 	{
 		return	m_sphereSphereCF;
 	}
-#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
 	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE))
 	{
 		return	m_sphereBoxCF;
@@ -230,8 +223,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg
 	{
 		return	m_boxSphereCF;
 	}
-#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
-
 
 	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE))
 	{

src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h

@@ -78,10 +78,8 @@ protected:
 	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;

src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp

@@ -49,7 +49,7 @@ btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
 
 
 
-void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	(void)dispatchInfo;
 	(void)resultOut;
@@ -59,26 +59,21 @@ void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWra
 	const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? body1Wrap : body0Wrap;
 	const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? body0Wrap : body1Wrap;
 
-
+	btVector3 pOnBox;
-	const btSphereShape* sphere0 = (const btSphereShape*)sphereObjWrap->getCollisionShape();
 
 	btVector3 normalOnSurfaceB;
-	btVector3 pOnBox,pOnSphere;
+	btScalar penetrationDepth;
 	btVector3 sphereCenter = sphereObjWrap->getWorldTransform().getOrigin();
+	const btSphereShape* sphere0 = (const btSphereShape*)sphereObjWrap->getCollisionShape();
 	btScalar radius = sphere0->getRadius();
-	
+	btScalar maxContactDistance = m_manifoldPtr->getContactBreakingThreshold();
-	btScalar dist = getSphereDistance(boxObjWrap,pOnBox,pOnSphere,sphereCenter,radius);
 
 	resultOut->setPersistentManifold(m_manifoldPtr);
 
-	if (dist < SIMD_EPSILON)
+	if (getSphereDistance(boxObjWrap, pOnBox, normalOnSurfaceB, penetrationDepth, sphereCenter, radius, maxContactDistance))
 	{
-		btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize();
-
 		/// report a contact. internally this will be kept persistent, and contact reduction is done
-
+		resultOut->addContactPoint(normalOnSurfaceB, pOnBox, penetrationDepth);
-		resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist);
-		
 	}
 
 	if (m_ownManifold)
@@ -103,159 +98,117 @@ btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject*
 }
 
 
-btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius ) 
+bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance ) 
 {
-
-	btScalar margins;
-	btVector3 bounds[2];
 	const btBoxShape* boxShape= (const btBoxShape*)boxObjWrap->getCollisionShape();
+	btVector3 const &boxHalfExtent = boxShape->getHalfExtentsWithoutMargin();
+	btScalar boxMargin = boxShape->getMargin();
+	penetrationDepth = 1.0f;
+
+	// convert the sphere position to the box's local space
+	btTransform const &m44T = boxObjWrap->getWorldTransform();
+	btVector3 sphereRelPos = m44T.invXform(sphereCenter);
+
+	// Determine the closest point to the sphere center in the box
+	btVector3 closestPoint = sphereRelPos;
+	closestPoint.setX( btMin(boxHalfExtent.getX(), closestPoint.getX()) );
+	closestPoint.setX( btMax(-boxHalfExtent.getX(), closestPoint.getX()) );
+	closestPoint.setY( btMin(boxHalfExtent.getY(), closestPoint.getY()) );
+	closestPoint.setY( btMax(-boxHalfExtent.getY(), closestPoint.getY()) );
+	closestPoint.setZ( btMin(boxHalfExtent.getZ(), closestPoint.getZ()) );
+	closestPoint.setZ( btMax(-boxHalfExtent.getZ(), closestPoint.getZ()) );
 	
-	bounds[0] = -boxShape->getHalfExtentsWithoutMargin();
+	btScalar intersectionDist = fRadius + boxMargin;
-	bounds[1] = boxShape->getHalfExtentsWithoutMargin();
+	btScalar contactDist = intersectionDist + maxContactDistance;
+	normal = sphereRelPos - closestPoint;
 
-	margins = boxShape->getMargin();//also add sphereShape margin?
+	//if there is no penetration, we are done
-
+	btScalar dist2 = normal.length2();
-	const btTransform&	m44T = boxObjWrap->getWorldTransform();
+	if (dist2 > contactDist * contactDist)
-
-	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;
+		return false;
-		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;
 	}
 
-	//////////////////////////////////////////////////
+	btScalar distance;
-	// Deep penetration case
 
-	fPenetration = getSpherePenetration( boxObjWrap,pointOnBox, v3PointOnSphere, sphereCenter, fRadius,bounds[0],bounds[1] );
+	//special case if the sphere center is inside the box
-
+	if (dist2 == 0.0f)
-	bounds[0] = boundsVec[0];
-	bounds[1] = boundsVec[1];
-
-	if ( fPenetration <= btScalar(0.0) )
-		return (fPenetration-margins);
-	else
-		return btScalar(1.0);
-}
-
-btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( const btCollisionObjectWrapper* boxObjWrap,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 = boxObjWrap->getWorldTransform();
-
-	// convert  point in local space
-	prel = m44T.invXform( sphereCenter);
-
-	///////////
-
-	for (int i=0;i<6;i++)
 	{
-		int j = i<3 ? 0:1;
+		distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal);
-		if ( (fSepThis = ((prel-bounds[j]) .dot( n[i]))-fRadius) > btScalar(0.0) )	return btScalar(1.0);
+	}
-		if ( fSepThis > fSep )
+	else //compute the penetration details
-		{
+	{
-			p0 = bounds[j];	normal = (btVector3&)n[i];
+		distance = normal.length();
-			fSep = fSepThis;
+		normal /= distance;
-		}
 	}
 
-	pointOnBox = prel - normal*(normal.dot((prel-p0)));
+	pointOnBox = closestPoint + normal * boxMargin;
-	v3PointOnSphere = pointOnBox + normal*fSep;
+//	v3PointOnSphere = sphereRelPos - (normal * fRadius);	
+	penetrationDepth = distance - intersectionDist;
 
 	// transform back in world space
-	tmp  = m44T( pointOnBox);		
+	btVector3 tmp = m44T(pointOnBox);
-	pointOnBox    = tmp;
+	pointOnBox = tmp;
-	tmp  = m44T( v3PointOnSphere);		v3PointOnSphere = tmp;
+//	tmp = m44T(v3PointOnSphere);
-	normal = (pointOnBox-v3PointOnSphere).normalize();
+//	v3PointOnSphere = tmp;
-
+	tmp = m44T.getBasis() * normal;
-	return fSep;
+	normal = tmp;
 
+	return true;
 }
 
+btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal ) 
+{
+	//project the center of the sphere on the closest face of the box
+	btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX();
+	btScalar minDist = faceDist;
+	closestPoint.setX( boxHalfExtent.getX() );
+	normal.setValue(btScalar(1.0f),  btScalar(0.0f),  btScalar(0.0f));
+
+	faceDist = boxHalfExtent.getX() + sphereRelPos.getX();
+	if (faceDist < minDist)
+	{
+		minDist = faceDist;
+		closestPoint = sphereRelPos;
+		closestPoint.setX( -boxHalfExtent.getX() );
+		normal.setValue(btScalar(-1.0f),  btScalar(0.0f),  btScalar(0.0f));
+	}
+
+	faceDist = boxHalfExtent.getY() - sphereRelPos.getY();
+	if (faceDist < minDist)
+	{
+		minDist = faceDist;
+		closestPoint = sphereRelPos;
+		closestPoint.setY( boxHalfExtent.getY() );
+		normal.setValue(btScalar(0.0f),  btScalar(1.0f),  btScalar(0.0f));
+	}
+
+	faceDist = boxHalfExtent.getY() + sphereRelPos.getY();
+	if (faceDist < minDist)
+	{
+		minDist = faceDist;
+		closestPoint = sphereRelPos;
+		closestPoint.setY( -boxHalfExtent.getY() );
+		normal.setValue(btScalar(0.0f),  btScalar(-1.0f),  btScalar(0.0f));
+	}
+
+	faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ();
+	if (faceDist < minDist)
+	{
+		minDist = faceDist;
+		closestPoint = sphereRelPos;
+		closestPoint.setZ( boxHalfExtent.getZ() );
+		normal.setValue(btScalar(0.0f),  btScalar(0.0f),  btScalar(1.0f));
+	}
+
+	faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ();
+	if (faceDist < minDist)
+	{
+		minDist = faceDist;
+		closestPoint = sphereRelPos;
+		closestPoint.setZ( -boxHalfExtent.getZ() );
+		normal.setValue(btScalar(0.0f),  btScalar(0.0f),  btScalar(-1.0f));
+	}
+
+	return minDist;
+}

src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h

@@ -50,9 +50,9 @@ public:
 		}
 	}
 
-	btScalar getSphereDistance( const btCollisionObjectWrapper* boxObjWrap,btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius );
+	bool getSphereDistance( const btCollisionObjectWrapper* boxObjWrap, btVector3& v3PointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& v3SphereCenter, btScalar fRadius, btScalar maxContactDistance );
 
-	btScalar getSpherePenetration( const btCollisionObjectWrapper* boxObjWrap, btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax);
+	btScalar getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal );
 	
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{

src/btBulletCollisionCommon.h

@@ -45,7 +45,6 @@ subject to the following restrictions:
 ///Narrowphase Collision Detector
 #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
 
-//btSphereBoxCollisionAlgorithm is broken, use gjk for now
 //#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h"