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;
}
