// 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();
}
}
// Tries to generate a manifold between a pair of faces. It can fail in some cases due to numerical accuracy:
// 1) both faces are nearly perpendicular to the normal
// 2) the closest features on the shapes are vertices, so that the intersection of the projection of the faces to the plane perpendicular to the normal contains only one point
// In those cases, FaceFace() returns false and the caller should generate a contact from the closest points on the shapes.
private static unsafe bool FaceFace(
ref ConvexHull convexA, ref ConvexHull convexB, int faceIndexA, int faceIndexB, MTransform worldFromA, MTransform aFromB,
float3 normal, float distance, ref Manifold manifold)
{
// Get the plane of each face
Plane planeA = convexA.Planes[faceIndexA];
Plane planeB = TransformPlane(aFromB, convexB.Planes[faceIndexB]);
// Handle cases where one of the faces is nearly perpendicular to the contact normal
// This gets around divide by zero / numerical problems from dividing collider planes which often contain some error by a very small number, amplifying that error
const float cosMaxAngle = 0.05f;
float dotA = math.dot(planeA.Normal, normal);
float dotB = math.dot(planeB.Normal, normal);
bool acceptB = true; // true if vertices of B projected onto the face of A are accepted
if (dotA > -cosMaxAngle)
{
// Handle cases where both faces are nearly perpendicular to the contact normal.
if (dotB < cosMaxAngle)
{
// Both faces are nearly perpendicular to the contact normal, let the caller generate a single contact
return(false);
}
// Face of A is nearly perpendicular to the contact normal, don't try to project vertices onto it
acceptB = false;
}
else if (dotB < cosMaxAngle)
{
// Face of B is nearly perpendicular to the normal, so we need to clip the edges of B against face A instead
MTransform bFromA = Inverse(aFromB);
float3 normalInB = math.mul(bFromA.Rotation, -normal);
MTransform worldFromB = Mul(worldFromA, aFromB);
bool result = FaceFace(ref convexB, ref convexA, faceIndexB, faceIndexA, worldFromB, bFromA, normalInB, distance, ref manifold);
manifold.Normal = -manifold.Normal;
manifold.Flip();
return(result);
}
// Check if the manifold gets a point roughly as close as the closest
distance += closestDistanceTolerance;
bool foundClosestPoint = false;
// Transform vertices of B into A-space
// Initialize validB, which is true for each vertex of B that is inside face A
ConvexHull.Face faceA = convexA.Faces[faceIndexA];
ConvexHull.Face faceB = convexB.Faces[faceIndexB];
bool * validB = stackalloc bool[faceB.NumVertices];
float3 * verticesBinA = stackalloc float3[faceB.NumVertices];
{
byte * indicesB = convexB.FaceVertexIndicesPtr + faceB.FirstIndex;
float3 *verticesB = convexB.VerticesPtr;
for (int i = 0; i < faceB.NumVertices; i++)
{
validB[i] = acceptB;
verticesBinA[i] = Mul(aFromB, verticesB[indicesB[i]]);
}
}
// For each edge of A
float invDotB = math.rcp(dotB);
float sumRadii = convexA.ConvexRadius + convexB.ConvexRadius;
byte * indicesA = convexA.FaceVertexIndicesPtr + faceA.FirstIndex;
float3 *verticesA = convexA.VerticesPtr;
for (EdgeIterator edgeA = EdgeIterator.Begin(verticesA, indicesA, -normal, faceA.NumVertices); edgeA.Valid(); edgeA.Advance())
{
float fracEnterA = 0.0f;
float fracExitA = 1.0f;
// For each edge of B
for (EdgeIterator edgeB = EdgeIterator.Begin(verticesBinA, null, normal, faceB.NumVertices); edgeB.Valid(); edgeB.Advance())
{
// Cast edge A against plane B and test if vertex B is inside plane A
castRayPlane(edgeA.Vertex0, edgeA.Edge, edgeB.Perp, edgeB.Offset, ref fracEnterA, ref fracExitA);
validB[edgeB.Index] &= (math.dot(edgeB.Vertex1, edgeA.Perp) < edgeA.Offset);
}
// If edge A hits B, add a contact points
if (fracEnterA < fracExitA)
{
float distance0 = (math.dot(edgeA.Vertex0, planeB.Normal) + planeB.Distance) * invDotB;
float deltaDistance = math.dot(edgeA.Edge, planeB.Normal) * invDotB;
float3 vertexAOnB = edgeA.Vertex0 - normal * distance0;
float3 edgeAOnB = edgeA.Edge - normal * deltaDistance;
foundClosestPoint |= AddEdgeContact(vertexAOnB, edgeAOnB, distance0, deltaDistance, fracEnterA, normal, convexB.ConvexRadius, sumRadii, worldFromA, distance, ref manifold);
if (fracExitA < 1.0f) // If the exit fraction is 1, then the next edge has the same contact point with enter fraction 0
{
foundClosestPoint |= AddEdgeContact(vertexAOnB, edgeAOnB, distance0, deltaDistance, fracExitA, normal, convexB.ConvexRadius, sumRadii, worldFromA, distance, ref manifold);
}
}
}
// For each vertex of B
float invDotA = math.rcp(dotA);
for (int i = 0; i < faceB.NumVertices; i++)
{
if (validB[i] && manifold.NumContacts < Manifold.k_MaxNumContacts)
{
float3 vertexB = verticesBinA[i];
float distanceB = (math.dot(vertexB, planeA.Normal) + planeA.Distance) * -invDotA;
manifold[manifold.NumContacts++] = new ContactPoint
{
Position = Mul(worldFromA, vertexB) + manifold.Normal * convexB.ConvexRadius,
Distance = distanceB - sumRadii
};
foundClosestPoint |= distanceB <= distance;
}
}
return(foundClosestPoint);
}
// 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();
}
}