// 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();
}
}
public void Execute(ref ModifiableContactHeader contactHeader, ref ModifiableContactPoint contactPoint)
{
bool isBodyA = (contactHeader.EntityA == SurfaceEntity);
bool isBodyB = (contactHeader.EntityB == SurfaceEntity);
if (isBodyA || isBodyB)
{
if (contactPoint.Index == 0)
{
// if we have a mesh surface we can get the surface normal from the plane of the polygon
var rbIdx = CollisionWorld.GetRigidBodyIndex(SurfaceEntity);
var body = CollisionWorld.Bodies[rbIdx];
if (body.Collider.Value.CollisionType == CollisionType.Composite)
{
unsafe
{
body.Collider.Value.GetLeaf(isBodyA ? contactHeader.ColliderKeyA : contactHeader.ColliderKeyB, out ChildCollider leafCollider);
if (leafCollider.Collider->Type == ColliderType.Triangle || leafCollider.Collider->Type == ColliderType.Quad)
{
PolygonCollider *polygonCollider = (PolygonCollider *)leafCollider.Collider;
// Potential optimization: If TransformFromChild has no rotation just use body.WorldFromBody.rot
// This is likely if you only have a MeshCollider with no hierarchy.
quaternion rotation = math.mul(body.WorldFromBody.rot, leafCollider.TransformFromChild.rot);
float3 surfaceNormal = math.rotate(rotation, polygonCollider->Planes[0].Normal);
distanceScale = math.dot(surfaceNormal, contactHeader.Normal);
contactHeader.Normal = surfaceNormal;
}
}
}
}
contactPoint.Distance *= distanceScale;
}
}
// 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 box and triangle in world space.
public static unsafe void BoxTriangle(
BoxCollider *boxA, PolygonCollider *triangleB,
MTransform worldFromA, MTransform aFromB, float maxDistance,
out Manifold manifold)
{
Assert.IsTrue(triangleB->Vertices.Length == 3);
// Get triangle in box space
MTransform aFromBoxA = new MTransform(boxA->Orientation, boxA->Center);
MTransform boxAFromB = Mul(Inverse(aFromBoxA), aFromB);
float3 t0 = Mul(boxAFromB, triangleB->ConvexHull.Vertices[0]);
float3 t1 = Mul(boxAFromB, triangleB->ConvexHull.Vertices[1]);
float3 t2 = Mul(boxAFromB, triangleB->ConvexHull.Vertices[2]);
Plane triPlane = triangleB->ConvexHull.Planes[0];
float3 triangleNormal = math.mul(boxAFromB.Rotation, triPlane.Normal);
FourTransposedPoints vertsB;
FourTransposedPoints edgesB;
FourTransposedPoints perpsB;
CalcTrianglePlanes(t0, t1, t2, triangleNormal, out vertsB, out edgesB, out perpsB);
float3 halfExtents = boxA->Size * 0.5f + maxDistance;
// find the closest minkowski plane
float4 plane;
{
// Box face vs triangle vertex
float4 planeFaceVertex;
{
// get aabb of minkowski diff
float3 tMin = math.min(math.min(t0, t1), t2) - halfExtents;
float3 tMax = math.max(math.max(t0, t1), t2) + halfExtents;
// find the aabb face closest to the origin
float3 axis0 = new float3(1, 0, 0);
float3 axis1 = axis0.zxy; // 010
float3 axis2 = axis0.yzx; // 001
float4 planeX = SelectMaxW(new float4(axis0, -tMax.x), new float4(-axis0, tMin.x));
float4 planeY = SelectMaxW(new float4(axis1, -tMax.y), new float4(-axis1, tMin.y));
float4 planeZ = SelectMaxW(new float4(axis2, -tMax.z), new float4(-axis2, tMin.z));
planeFaceVertex = SelectMaxW(planeX, planeY);
planeFaceVertex = SelectMaxW(planeFaceVertex, planeZ);
}
// Box vertex vs triangle face
float4 planeVertexFace;
{
// Calculate the triangle normal
float triangleOffset = math.dot(triangleNormal, t0);
float expansionOffset = math.dot(math.abs(triangleNormal), halfExtents);
planeVertexFace = SelectMaxW(
new float4(triangleNormal, -triangleOffset - expansionOffset),
new float4(-triangleNormal, triangleOffset - expansionOffset));
}
// Edge planes
float4 planeEdgeEdge = new float4(0, 0, 0, -float.MaxValue);
{
// Test the planes from crossing axis i with each edge of the triangle, for example if i = 1 then n0 is from (0, 1, 0) x (t1 - t0).
for (int i = 0, j = 1, k = 2; i < 3; j = k, k = i, i++)
{
// Normalize the cross product and flip it to point outward from the edge
float4 lengthsSq = edgesB.GetComponent(j) * edgesB.GetComponent(j) + edgesB.GetComponent(k) * edgesB.GetComponent(k);
float4 invLengths = math.rsqrt(lengthsSq);
float4 dots = edgesB.GetComponent(j) * perpsB.GetComponent(k) - edgesB.GetComponent(k) * perpsB.GetComponent(j);
float4 factors = invLengths * math.sign(dots);
float4 nj = -edgesB.GetComponent(k) * factors;
float4 nk = edgesB.GetComponent(j) * factors;
float4 distances = -nj *vertsB.GetComponent(j) - nk * vertsB.GetComponent(k) - math.abs(nj) * halfExtents[j] - math.abs(nk) * halfExtents[k];
// If the box edge is parallel to the triangle face then skip it, the plane is redundant with a vertex-face plane
bool4 valid = dots != float4.zero;
distances = math.select(Constants.Min4F, distances, valid);
float3 n0 = new float3(); n0[i] = 0.0f; n0[j] = nj[0]; n0[k] = nk[0];
float3 n1 = new float3(); n1[i] = 0.0f; n1[j] = nj[1]; n1[k] = nk[1];
float3 n2 = new float3(); n2[i] = 0.0f; n2[j] = nj[2]; n2[k] = nk[2];
float4 temp = SelectMaxW(SelectMaxW(new float4(n0, distances.x), new float4(n1, distances.y)), new float4(n2, distances.z));
planeEdgeEdge = SelectMaxW(planeEdgeEdge, temp);
}
}
plane = SelectMaxW(SelectMaxW(planeFaceVertex, planeVertexFace), planeEdgeEdge);
}
manifold = new Manifold();
// Check for a separating plane TODO.ma could early out as soon as any plane with w>0 is found
if (plane.w <= 0.0f)
{
// Get the normal and supporting faces
float3 normalInA = math.mul(boxA->Orientation, plane.xyz);
manifold.Normal = math.mul(worldFromA.Rotation, normalInA);
int faceIndexA = boxA->ConvexHull.GetSupportingFace(-normalInA);
int faceIndexB = triangleB->ConvexHull.GetSupportingFace(math.mul(math.transpose(aFromB.Rotation), normalInA));
// Build manifold
if (!FaceFace(ref boxA->ConvexHull, ref triangleB->ConvexHull, faceIndexA, faceIndexB, worldFromA, aFromB, normalInA, float.MaxValue, ref manifold))
{
// The closest points are vertices, we need GJK to find them
ConvexConvex(
ref ((ConvexCollider *)boxA)->ConvexHull, ref ((ConvexCollider *)triangleB)->ConvexHull,
worldFromA, aFromB, maxDistance, out manifold);
}
}
}
// Dispatch any pair of convex colliders
public static unsafe Result ConvexConvex(Collider *convexA, Collider *convexB, MTransform aFromB)
{
Result result;
bool flip = false;
switch (convexA->Type)
{
case ColliderType.Sphere:
SphereCollider *sphereA = (SphereCollider *)convexA;
switch (convexB->Type)
{
case ColliderType.Sphere:
result = SphereSphere(sphereA, (SphereCollider *)convexB, aFromB);
break;
case ColliderType.Capsule:
CapsuleCollider *capsuleB = (CapsuleCollider *)convexB;
result = CapsuleSphere(capsuleB->Vertex0, capsuleB->Vertex1, capsuleB->Radius, sphereA->Center, sphereA->Radius, Inverse(aFromB));
flip = true;
break;
case ColliderType.Triangle:
PolygonCollider *triangleB = (PolygonCollider *)convexB;
result = TriangleSphere(
triangleB->Vertices[0], triangleB->Vertices[1], triangleB->Vertices[2], triangleB->Planes[0].Normal,
sphereA->Center, sphereA->Radius, Inverse(aFromB));
flip = true;
break;
case ColliderType.Quad:
PolygonCollider *quadB = (PolygonCollider *)convexB;
result = QuadSphere(
quadB->Vertices[0], quadB->Vertices[1], quadB->Vertices[2], quadB->Vertices[3], quadB->Planes[0].Normal,
sphereA->Center, sphereA->Radius, Inverse(aFromB));
flip = true;
break;
case ColliderType.Box:
result = BoxSphere((BoxCollider *)convexB, sphereA, Inverse(aFromB));
flip = true;
break;
case ColliderType.Cylinder:
case ColliderType.Convex:
result = ConvexConvex(ref sphereA->ConvexHull, ref ((ConvexCollider *)convexB)->ConvexHull, aFromB);
break;
default:
throw new NotImplementedException();
}
break;
case ColliderType.Capsule:
CapsuleCollider *capsuleA = (CapsuleCollider *)convexA;
switch (convexB->Type)
{
case ColliderType.Sphere:
SphereCollider *sphereB = (SphereCollider *)convexB;
result = CapsuleSphere(capsuleA->Vertex0, capsuleA->Vertex1, capsuleA->Radius, sphereB->Center, sphereB->Radius, aFromB);
break;
case ColliderType.Capsule:
result = CapsuleCapsule(capsuleA, (CapsuleCollider *)convexB, aFromB);
break;
case ColliderType.Triangle:
result = CapsuleTriangle(capsuleA, (PolygonCollider *)convexB, aFromB);
break;
case ColliderType.Box:
case ColliderType.Quad:
case ColliderType.Cylinder:
case ColliderType.Convex:
result = ConvexConvex(ref capsuleA->ConvexHull, ref ((ConvexCollider *)convexB)->ConvexHull, aFromB);
break;
default:
throw new NotImplementedException();
}
break;
case ColliderType.Triangle:
PolygonCollider *triangleA = (PolygonCollider *)convexA;
switch (convexB->Type)
{
case ColliderType.Sphere:
SphereCollider *sphereB = (SphereCollider *)convexB;
result = TriangleSphere(
triangleA->Vertices[0], triangleA->Vertices[1], triangleA->Vertices[2], triangleA->Planes[0].Normal,
sphereB->Center, sphereB->Radius, aFromB);
break;
case ColliderType.Capsule:
result = CapsuleTriangle((CapsuleCollider *)convexB, triangleA, Inverse(aFromB));
flip = true;
break;
case ColliderType.Box:
case ColliderType.Triangle:
case ColliderType.Quad:
case ColliderType.Cylinder:
case ColliderType.Convex:
result = ConvexConvex(ref triangleA->ConvexHull, ref ((ConvexCollider *)convexB)->ConvexHull, aFromB);
break;
default:
throw new NotImplementedException();
}
break;
case ColliderType.Box:
BoxCollider *boxA = (BoxCollider *)convexA;
switch (convexB->Type)
{
case ColliderType.Sphere:
result = BoxSphere(boxA, (SphereCollider *)convexB, aFromB);
break;
case ColliderType.Capsule:
case ColliderType.Box:
case ColliderType.Triangle:
case ColliderType.Quad:
case ColliderType.Cylinder:
case ColliderType.Convex:
result = ConvexConvex(ref boxA->ConvexHull, ref ((ConvexCollider *)convexB)->ConvexHull, aFromB);
break;
default:
throw new NotImplementedException();
}
break;
case ColliderType.Quad:
case ColliderType.Cylinder:
case ColliderType.Convex:
result = ConvexConvex(ref ((ConvexCollider *)convexA)->ConvexHull, ref ((ConvexCollider *)convexB)->ConvexHull, aFromB);
break;
default:
throw new NotImplementedException();
}
if (flip)
{
result.PositionOnAinA = Mul(aFromB, result.PositionOnBinA);
result.NormalInA = math.mul(aFromB.Rotation, -result.NormalInA);
}
return(result);
}
public static unsafe Result CapsuleTriangle(CapsuleCollider *capsuleA, PolygonCollider *triangleB, MTransform aFromB)
{
// Get vertices
float3 c0 = capsuleA->Vertex0;
float3 c1 = capsuleA->Vertex1;
float3 t0 = Mul(aFromB, triangleB->ConvexHull.Vertices[0]);
float3 t1 = Mul(aFromB, triangleB->ConvexHull.Vertices[1]);
float3 t2 = Mul(aFromB, triangleB->ConvexHull.Vertices[2]);
float3 direction;
float distanceSq;
float3 pointCapsule;
float sign = 1.0f; // negated if penetrating
{
// Calculate triangle edges and edge planes
float3 faceNormal = math.mul(aFromB.Rotation, triangleB->ConvexHull.Planes[0].Normal);
FourTransposedPoints vertsB;
FourTransposedPoints edgesB;
FourTransposedPoints perpsB;
CalcTrianglePlanes(t0, t1, t2, faceNormal, out vertsB, out edgesB, out perpsB);
// c0 against triangle face
{
FourTransposedPoints rels = new FourTransposedPoints(c0) - vertsB;
PointTriangleFace(c0, t0, faceNormal, vertsB, edgesB, perpsB, rels, out float signedDistance);
distanceSq = signedDistance * signedDistance;
if (distanceSq > distanceEpsSq)
{
direction = -faceNormal * signedDistance;
}
else
{
direction = math.select(faceNormal, -faceNormal, math.dot(c1 - c0, faceNormal) < 0); // rare case, capsule point is exactly on the triangle face
}
pointCapsule = c0;
}
// c1 against triangle face
{
FourTransposedPoints rels = new FourTransposedPoints(c1) - vertsB;
PointTriangleFace(c1, t0, faceNormal, vertsB, edgesB, perpsB, rels, out float signedDistance);
float distanceSq1 = signedDistance * signedDistance;
float3 direction1;
if (distanceSq1 > distanceEpsSq)
{
direction1 = -faceNormal * signedDistance;
}
else
{
direction1 = math.select(faceNormal, -faceNormal, math.dot(c0 - c1, faceNormal) < 0); // rare case, capsule point is exactly on the triangle face
}
SelectMin(ref direction, ref distanceSq, ref pointCapsule, direction1, distanceSq1, c1);
}
// axis against triangle edges
float3 axis = c1 - c0;
for (int i = 0; i < 3; i++)
{
float3 closestOnCapsule, closestOnTriangle;
SegmentSegment(c0, axis, vertsB.GetPoint(i), edgesB.GetPoint(i), out closestOnCapsule, out closestOnTriangle);
float3 edgeDiff = closestOnCapsule - closestOnTriangle;
float edgeDistanceSq = math.lengthsq(edgeDiff);
edgeDiff = math.select(edgeDiff, perpsB.GetPoint(i), edgeDistanceSq < distanceEpsSq); // use edge plane if the capsule axis intersects the edge
SelectMin(ref direction, ref distanceSq, ref pointCapsule, edgeDiff, edgeDistanceSq, closestOnCapsule);
}
// axis against triangle face
{
// Find the intersection of the axis with the triangle plane
float axisDot = math.dot(axis, faceNormal);
float dist0 = math.dot(t0 - c0, faceNormal); // distance from c0 to the plane along the normal
float t = dist0 * math.select(math.rcp(axisDot), 0.0f, axisDot == 0.0f);
if (t > 0.0f && t < 1.0f)
{
// If they intersect, check if the intersection is inside the triangle
FourTransposedPoints rels = new FourTransposedPoints(c0 + axis * t) - vertsB;
float4 dots = perpsB.Dot(rels);
if (math.all(dots <= float4.zero))
{
// Axis intersects the triangle, choose the separating direction
float dist1 = axisDot - dist0;
bool use1 = math.abs(dist1) < math.abs(dist0);
float dist = math.select(-dist0, dist1, use1);
float3 closestOnCapsule = math.select(c0, c1, use1);
SelectMin(ref direction, ref distanceSq, ref pointCapsule, dist * faceNormal, dist * dist, closestOnCapsule);
// Even if the edge is closer than the face, we now know that the edge hit was penetrating
sign = -1.0f;
}
}
}
}
float invDistance = math.rsqrt(distanceSq);
float distance;
float3 normal;
if (distanceSq < distanceEpsSq)
{
normal = math.normalize(direction); // rare case, capsule axis almost exactly touches the triangle
distance = 0.0f;
}
else
{
normal = direction * invDistance * sign; // common case, distanceSq = lengthsq(direction)
distance = distanceSq * invDistance * sign;
}
return(new Result
{
NormalInA = normal,
PositionOnAinA = pointCapsule - normal * capsuleA->Radius,
Distance = distance - capsuleA->Radius
});
}
