bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance ) { 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()) ); btScalar intersectionDist = fRadius + boxMargin; btScalar contactDist = intersectionDist + maxContactDistance; normal = sphereRelPos - closestPoint; //if there is no penetration, we are done btScalar dist2 = normal.length2(); if (dist2 > contactDist * contactDist) { return false; } btScalar distance; //special case if the sphere center is inside the box if (dist2 == 0.0f) { distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal); } else //compute the penetration details { distance = normal.length(); normal /= distance; } pointOnBox = closestPoint + normal * boxMargin; // v3PointOnSphere = sphereRelPos - (normal * fRadius); penetrationDepth = distance - intersectionDist; // 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::getSphereDistance(btCollisionObject* boxObj, btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius ) { btScalar margins; btVector3 bounds[2]; btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape(); 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; 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; 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; } ////////////////////////////////////////////////// // Deep penetration case fPenetration = getSpherePenetration( boxObj,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); }