// Create contact points for a capsule and triangle in world space.
public static unsafe void CapsuleTriangle(
CapsuleCollider *capsuleA, PolygonCollider *triangleB,
MTransform worldFromA, MTransform aFromB, float maxDistance,
out Manifold manifold)
{
Assert.IsTrue(triangleB->Vertices.Length == 3);
DistanceQueries.Result convexDistance = DistanceQueries.CapsuleTriangle(capsuleA, triangleB, aFromB);
if (convexDistance.Distance < maxDistance)
{
// Build manifold
manifold = new Manifold
{
Normal = math.mul(worldFromA.Rotation, -convexDistance.NormalInA) // 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, convexDistance.NormalInA);
int faceIndexB = triangleB->ConvexHull.GetSupportingFace(normalInB);
if (FaceEdge(ref triangleB->ConvexHull, ref capsuleA->ConvexHull, faceIndexB, worldFromB, bFromA, -normalInB, convexDistance.Distance + capsuleA->Radius, ref manifold))
{
manifold.Flip();
}
else
{
manifold = new Manifold(convexDistance, worldFromA);
}
}
else
{
manifold = new Manifold();
}
}
// Create a single point manifold from a distance query result
public Manifold(DistanceQueries.Result convexDistance, MTransform worldFromA)
{
NumContacts = 1;
Normal = math.mul(worldFromA.Rotation, convexDistance.NormalInA);
this[0] = new ContactPoint
{
Distance = convexDistance.Distance,
Position = Mul(worldFromA, convexDistance.PositionOnBinA)
};
}
// Create a contact point for a pair of spheres in world space.
public static unsafe void SphereSphere(
SphereCollider *sphereA, SphereCollider *sphereB,
MTransform worldFromA, MTransform aFromB, float maxDistance,
out Manifold manifold)
{
DistanceQueries.Result convexDistance = DistanceQueries.SphereSphere(sphereA, sphereB, aFromB);
if (convexDistance.Distance < maxDistance)
{
manifold = new Manifold(convexDistance, worldFromA);
}
else
{
manifold = new Manifold();
}
}
// Create a contact point for a pair of capsules in world space.
public static unsafe void CapsuleCapsule(
CapsuleCollider *capsuleA, CapsuleCollider *capsuleB,
MTransform worldFromA, MTransform aFromB, float maxDistance,
out Manifold manifold)
{
// TODO: Should produce a multi-point manifold
DistanceQueries.Result convexDistance = DistanceQueries.CapsuleCapsule(capsuleA, capsuleB, aFromB);
if (convexDistance.Distance < maxDistance)
{
manifold = new Manifold(convexDistance, worldFromA);
}
else
{
manifold = new Manifold();
}
}
// Create a single point manifold between a capsule and a sphere in world space.
public static unsafe void CapsuleSphere(
CapsuleCollider *capsuleA, SphereCollider *sphereB,
MTransform worldFromA, MTransform aFromB, float maxDistance,
out Manifold manifold)
{
DistanceQueries.Result convexDistance = DistanceQueries.CapsuleSphere(
capsuleA->Vertex0, capsuleA->Vertex1, capsuleA->Radius, sphereB->Center, sphereB->Radius, aFromB);
if (convexDistance.Distance < maxDistance)
{
manifold = new Manifold(convexDistance, worldFromA);
}
else
{
manifold = new Manifold();
}
}
// Create a single point manifold between a triangle and sphere in world space.
public static unsafe void TriangleSphere(
PolygonCollider *triangleA, SphereCollider *sphereB,
MTransform worldFromA, MTransform aFromB, float maxDistance,
out Manifold manifold)
{
Assert.IsTrue(triangleA->Vertices.Length == 3);
DistanceQueries.Result convexDistance = DistanceQueries.TriangleSphere(
triangleA->Vertices[0], triangleA->Vertices[1], triangleA->Vertices[2], triangleA->Planes[0].Normal,
sphereB->Center, sphereB->Radius, aFromB);
if (convexDistance.Distance < maxDistance)
{
manifold = new Manifold(convexDistance, worldFromA);
}
else
{
manifold = new Manifold();
}
}
// 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();
}
}