diff --git a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index 7faee6faf..e81a2578f 100644 --- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/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)) { diff --git a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 81ed424a3..474785bfc 100644 --- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/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; diff --git a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp index 46b5a9811..0626cbd7a 100644 --- a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp +++ b/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; - - const btSphereShape* sphere0 = (const btSphereShape*)sphereObjWrap->getCollisionShape(); + btVector3 pOnBox; btVector3 normalOnSurfaceB; - btVector3 pOnBox,pOnSphere; + btScalar penetrationDepth; btVector3 sphereCenter = sphereObjWrap->getWorldTransform().getOrigin(); + const btSphereShape* sphere0 = (const btSphereShape*)sphereObjWrap->getCollisionShape(); btScalar radius = sphere0->getRadius(); - - btScalar dist = getSphereDistance(boxObjWrap,pOnBox,pOnSphere,sphereCenter,radius); + btScalar maxContactDistance = m_manifoldPtr->getContactBreakingThreshold(); 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,dist); - + resultOut->addContactPoint(normalOnSurfaceB, pOnBox, penetrationDepth); } 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(); - bounds[1] = boxShape->getHalfExtentsWithoutMargin(); + btScalar intersectionDist = fRadius + boxMargin; + btScalar contactDist = intersectionDist + maxContactDistance; + normal = sphereRelPos - closestPoint; - margins = boxShape->getMargin();//also add sphereShape margin? - - const btTransform& m44T = boxObjWrap->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++) + //if there is no penetration, we are done + btScalar dist2 = normal.length2(); + if (dist2 > contactDist * contactDist) { - 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; + return false; } - ////////////////////////////////////////////////// - // Deep penetration case + btScalar distance; - fPenetration = getSpherePenetration( boxObjWrap,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( 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++) + //special case if the sphere center is inside the box + if (dist2 == 0.0f) { - 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; - } + distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal); + } + else //compute the penetration details + { + distance = normal.length(); + normal /= distance; } - pointOnBox = prel - normal*(normal.dot((prel-p0))); - v3PointOnSphere = pointOnBox + normal*fSep; + pointOnBox = closestPoint + normal * boxMargin; +// v3PointOnSphere = sphereRelPos - (normal * fRadius); + penetrationDepth = distance - intersectionDist; // transform back in world space - tmp = m44T( pointOnBox); - pointOnBox = tmp; - tmp = m44T( v3PointOnSphere); v3PointOnSphere = tmp; - normal = (pointOnBox-v3PointOnSphere).normalize(); - - return fSep; + btVector3 tmp = m44T(pointOnBox); + pointOnBox = tmp; +// tmp = m44T(v3PointOnSphere); +// v3PointOnSphere = tmp; + tmp = m44T.getBasis() * normal; + 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; +} diff --git a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h index 0b67a8b35..eefaedc9e 100644 --- a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ b/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 { diff --git a/src/btBulletCollisionCommon.h b/src/btBulletCollisionCommon.h index 472690c15..af981b5d3 100644 --- a/src/btBulletCollisionCommon.h +++ b/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"