public static unsafe Result ConvexConvex(
float3 *verticesA, int numVerticesA, float convexRadiusA,
float3 *verticesB, int numVerticesB, float convexRadiusB,
MTransform aFromB)
{
ConvexConvexDistanceQueries.Result result = ConvexConvexDistanceQueries.ConvexConvex(
verticesA, numVerticesA, verticesB, numVerticesB, aFromB, ConvexConvexDistanceQueries.PenetrationHandling.Exact3D);
// Adjust for convex radii
result.ClosestPoints.Distance -= (convexRadiusA + convexRadiusB);
result.ClosestPoints.PositionOnAinA -= result.ClosestPoints.NormalInA * convexRadiusA;
return(result.ClosestPoints);
}
// Create contact points for a pair of generic convex hulls in world space.
public static unsafe void ConvexConvex(
ref ConvexHull hullA, ref ConvexHull hullB,
MTransform worldFromA, MTransform aFromB, float maxDistance,
out Manifold manifold)
{
// Get closest points on the hulls
ConvexConvexDistanceQueries.Result result = ConvexConvexDistanceQueries.ConvexConvex(
hullA.VerticesPtr, hullA.NumVertices, hullB.VerticesPtr, hullB.NumVertices, aFromB, ConvexConvexDistanceQueries.PenetrationHandling.Exact3D);
float sumRadii = hullB.ConvexRadius + hullA.ConvexRadius;
if (result.ClosestPoints.Distance < maxDistance + sumRadii)
{
float3 normal = result.ClosestPoints.NormalInA;
manifold = new Manifold
{
Normal = math.mul(worldFromA.Rotation, normal)
};
if (hullA.NumFaces > 0)
{
int faceIndexA = hullA.GetSupportingFace(-normal, result.SimplexVertexA(0));
if (hullB.NumFaces > 0)
{
// Convex vs convex
int faceIndexB = hullB.GetSupportingFace(math.mul(math.transpose(aFromB.Rotation), normal), result.SimplexVertexB(0));
if (FaceFace(ref hullA, ref hullB, faceIndexA, faceIndexB, worldFromA, aFromB, normal, result.ClosestPoints.Distance, ref manifold))
{
return;
}
}
else if (hullB.NumVertices == 2)
{
// Convex vs capsule
if (FaceEdge(ref hullA, ref hullB, faceIndexA, worldFromA, aFromB, normal, result.ClosestPoints.Distance, ref manifold))
{
return;
}
} // Else convex vs sphere
}
else if (hullA.NumVertices == 2)
{
if (hullB.NumFaces > 0)
{
// Capsule vs convex
manifold.Normal = math.mul(worldFromA.Rotation, -normal); // negate the normal because we are temporarily flipping to triangle A capsule B
MTransform worldFromB = Mul(worldFromA, aFromB);
MTransform bFromA = Inverse(aFromB);
float3 normalInB = math.mul(bFromA.Rotation, normal);
int faceIndexB = hullB.GetSupportingFace(normalInB, result.SimplexVertexB(0));
bool foundClosestPoint = FaceEdge(ref hullB, ref hullA, faceIndexB, worldFromB, bFromA, -normalInB, result.ClosestPoints.Distance, ref manifold);
manifold.Flip();
if (foundClosestPoint)
{
return;
}
} // Else capsule vs capsule or sphere
} // Else sphere vs something
// Either one of the shapes is a sphere, or both of the shapes are capsules, or both of the closest features are nearly perpendicular to the contact normal,
// or FaceFace()/FaceEdge() missed the closest point due to numerical error. In these cases, add the closest point directly to the manifold.
if (manifold.NumContacts < Manifold.k_MaxNumContacts)
{
DistanceQueries.Result convexDistance = result.ClosestPoints;
manifold[manifold.NumContacts++] = new ContactPoint
{
Position = Mul(worldFromA, convexDistance.PositionOnAinA) - manifold.Normal * (convexDistance.Distance - hullB.ConvexRadius),
Distance = convexDistance.Distance - sumRadii
};
}
}
else
{
manifold = new Manifold();
}
}
