diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp')
-rw-r--r-- | extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp | 264 |
1 files changed, 109 insertions, 155 deletions
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp index 8df876928c1..e8b567e0efc 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp @@ -18,20 +18,21 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btSphereShape.h" #include "BulletCollision/CollisionShapes/btBoxShape.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" //#include <stdio.h> -btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped) -: btActivatingCollisionAlgorithm(ci,col0,col1), +btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped) +: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap), m_ownManifold(false), m_manifoldPtr(mf), m_isSwapped(isSwapped) { - btCollisionObject* sphereObj = m_isSwapped? col1 : col0; - btCollisionObject* boxObj = m_isSwapped? col0 : col1; + const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? col1Wrap : col0Wrap; + const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? col0Wrap : col1Wrap; - if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObj,boxObj)) + if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject())) { - m_manifoldPtr = m_dispatcher->getNewManifold(sphereObj,boxObj); + m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject()); m_ownManifold = true; } } @@ -48,36 +49,31 @@ btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm() -void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, 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; + const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? body0Wrap : body1Wrap; - - btSphereShape* sphere0 = (btSphereShape*)sphereObj->getCollisionShape(); + btVector3 pOnBox; btVector3 normalOnSurfaceB; - btVector3 pOnBox,pOnSphere; - btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin(); + btScalar penetrationDepth; + btVector3 sphereCenter = sphereObjWrap->getWorldTransform().getOrigin(); + const btSphereShape* sphere0 = (const btSphereShape*)sphereObjWrap->getCollisionShape(); btScalar radius = sphere0->getRadius(); - - btScalar dist = getSphereDistance(boxObj,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) @@ -102,159 +98,117 @@ btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* } -btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* boxObj, 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]; - btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape(); + 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(); - - 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; + btScalar intersectionDist = fRadius + boxMargin; + btScalar contactDist = intersectionDist + maxContactDistance; + normal = sphereRelPos - closestPoint; - 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; + //if there is no penetration, we are done + btScalar dist2 = normal.length2(); + if (dist2 > contactDist * contactDist) + { + return false; + } - v3P = prel; + btScalar distance; - for (int i=0;i<6;i++) + //special case if the sphere center is inside the box + if (dist2 <= SIMD_EPSILON) { - int j = i<3? 0:1; - if ( (fSepThis = ((v3P-bounds[j]) .dot(n[i]))) > btScalar(0.0) ) - { - v3P = v3P - n[i]*fSepThis; - bFound = true; - } + distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal); } - - // - - if ( bFound ) + else //compute the penetration details { - 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; + distance = normal.length(); + normal /= distance; } - ////////////////////////////////////////////////// - // Deep penetration case - - fPenetration = getSpherePenetration( boxObj,pointOnBox, v3PointOnSphere, sphereCenter, fRadius,bounds[0],bounds[1] ); + pointOnBox = closestPoint + normal * boxMargin; +// v3PointOnSphere = sphereRelPos - (normal * fRadius); + penetrationDepth = distance - intersectionDist; - bounds[0] = boundsVec[0]; - bounds[1] = boundsVec[1]; - - if ( fPenetration <= btScalar(0.0) ) - return (fPenetration-margins); - else - return btScalar(1.0); + // transform back in world space + btVector3 tmp = m44T(pointOnBox); + pointOnBox = tmp; +// tmp = m44T(v3PointOnSphere); +// v3PointOnSphere = tmp; + tmp = m44T.getBasis() * normal; + normal = tmp; + + return true; } -btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* boxObj,btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax) +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)); + } - 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++) + faceDist = boxHalfExtent.getY() - sphereRelPos.getY(); + if (faceDist < minDist) { - 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; - } + minDist = faceDist; + closestPoint = sphereRelPos; + closestPoint.setY( boxHalfExtent.getY() ); + normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f)); } - pointOnBox = prel - normal*(normal.dot((prel-p0))); - v3PointOnSphere = pointOnBox + normal*fSep; + 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)); + } - // transform back in world space - tmp = m44T( pointOnBox); - pointOnBox = tmp; - tmp = m44T( v3PointOnSphere); v3PointOnSphere = tmp; - normal = (pointOnBox-v3PointOnSphere).normalize(); + 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 fSep; + 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; } - |