static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance( btVector3& normalOnB, btVector3& pointOnB, btScalar capsuleLengthA, btScalar capsuleRadiusA, btScalar capsuleLengthB, btScalar capsuleRadiusB, int capsuleAxisA, int capsuleAxisB, const btTransform& transformA, const btTransform& transformB, btScalar distanceThreshold) { btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA); btVector3 translationA = transformA.getOrigin(); btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB); btVector3 translationB = transformB.getOrigin(); // translation between centers btVector3 translation = translationB - translationA; // compute the closest points of the capsule line segments btVector3 ptsVector; // the vector between the closest points btVector3 offsetA, offsetB; // offsets from segment centers to their closest points btScalar tA, tB; // parameters on line segment segmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB, translation, directionA, capsuleLengthA, directionB, capsuleLengthB); btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB; if (distance > distanceThreshold) return distance; btScalar lenSqr = ptsVector.length2(); if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON)) { //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA' btVector3 q; btPlaneSpace1(directionA, normalOnB, q); } else { // compute the contact normal normalOnB = ptsVector * -btRecipSqrt(lenSqr); } pointOnB = transformB.getOrigin() + offsetB + normalOnB * capsuleRadiusB; return distance; }
PfxFloat pfxContactCapsuleCapsule( PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB, void *shapeA,const PfxTransform3 &transformA, void *shapeB,const PfxTransform3 &transformB, PfxFloat distanceThreshold) { PfxCapsule &capsuleA = *((PfxCapsule*)shapeA); PfxCapsule &capsuleB = *((PfxCapsule*)shapeB); PfxVector3 directionA = transformA.getUpper3x3().getCol0(); PfxVector3 translationA = transformA.getTranslation(); PfxVector3 directionB = transformB.getUpper3x3().getCol0(); PfxVector3 translationB = transformB.getTranslation(); // translation between centers PfxVector3 translation = translationB - translationA; // compute the closest points of the capsule line segments PfxVector3 ptsVector; // the vector between the closest points PfxVector3 offsetA, offsetB; // offsets from segment centers to their closest points PfxFloat tA, tB; // parameters on line segment segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation, directionA, capsuleA.m_halfLen, directionB, capsuleB.m_halfLen ); PfxFloat distance = length(ptsVector) - capsuleA.m_radius - capsuleB.m_radius; if ( distance > distanceThreshold ) return distance; // compute the contact normal segmentsNormal( normal, ptsVector ); // compute points on capsules pointA = PfxPoint3( transpose(transformA.getUpper3x3()) * ( offsetA + normal * capsuleA.m_radius ) ); pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( offsetB - normal * capsuleB.m_radius ) ); return distance; }