public static void AppendCapsule(CapsuleCollider *capsule, RigidTransform worldFromCollider, ref List <DisplayResult> results)
{
float r = capsule->Radius;
results.Add(new DisplayResult
{
Mesh = ReferenceSphere,
Scale = new Vector4(r * 2.0f, r * 2.0f, r * 2.0f),
Position = math.transform(worldFromCollider, capsule->Vertex0),
Orientation = worldFromCollider.rot
});
results.Add(new DisplayResult
{
Mesh = ReferenceSphere,
Scale = new Vector4(r * 2.0f, r * 2.0f, r * 2.0f),
Position = math.transform(worldFromCollider, capsule->Vertex1),
Orientation = worldFromCollider.rot
});
results.Add(new DisplayResult
{
Mesh = ReferenceCylinder,
Scale = new Vector4(r * 2.0f, math.length(capsule->Vertex1 - capsule->Vertex0) * 0.5f, r * 2.0f),
Position = math.transform(worldFromCollider, (capsule->Vertex0 + capsule->Vertex1) * 0.5f),
Orientation = math.mul(worldFromCollider.rot, Quaternion.FromToRotation(new float3(0, 1, 0), math.normalizesafe(capsule->Vertex1 - capsule->Vertex0)))
});
}
// 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 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();
}
}
public static unsafe Result CapsuleCapsule(CapsuleCollider *capsuleA, CapsuleCollider *capsuleB, MTransform aFromB)
{
// Transform capsule B into A-space
float3 pointB = Mul(aFromB, capsuleB->Vertex0);
float3 edgeB = math.mul(aFromB.Rotation, capsuleB->Vertex1 - capsuleB->Vertex0);
// Get point and edge of A
float3 pointA = capsuleA->Vertex0;
float3 edgeA = capsuleA->Vertex1 - capsuleA->Vertex0;
// Get the closest points on the capsules
SegmentSegment(pointA, edgeA, pointB, edgeB, out float3 closestA, out float3 closestB);
float3 diff = closestA - closestB;
float coreDistanceSq = math.lengthsq(diff);
if (coreDistanceSq < distanceEpsSq)
{
// Special case for extremely small distances, should be rare
float3 normal = math.cross(edgeA, edgeB);
if (math.lengthsq(normal) < 1e-5f)
{
float3 edge = math.normalizesafe(edgeA, math.normalizesafe(edgeB, new float3(1, 0, 0))); // edges are parallel or one of the capsules is a sphere
Math.CalculatePerpendicularNormalized(edge, out normal, out float3 unused); // normal is anything perpendicular to edge
}
else
{
normal = math.normalize(normal); // normal is cross of edges, sign doesn't matter
}
return(new Result
{
NormalInA = normal,
PositionOnAinA = pointA - normal * capsuleA->Radius,
Distance = -capsuleA->Radius - capsuleB->Radius
});
}
return(PointPoint(closestA, closestB, capsuleA->Radius, capsuleA->Radius + capsuleB->Radius));
}
// 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
});
}
