private static void CalcTrianglePlanes(float3 v0, float3 v1, float3 v2, float3 normalDirection,
out FourTransposedPoints verts, out FourTransposedPoints edges, out FourTransposedPoints perps)
{
verts = new FourTransposedPoints(v0, v1, v2, v0);
edges = verts.V1230 - verts;
perps = edges.Cross(new FourTransposedPoints(normalDirection));
}
public static Result TriangleSphere(
float3 vertex0, float3 vertex1, float3 vertex2, float3 normal,
float3 sphereCenter, float sphereRadius,
MTransform aFromB)
{
// Sphere center in A-space (TODO.ma should probably work in sphere space, typical for triangle verts to be far from the origin in its local space)
float3 pointB = Mul(aFromB, sphereCenter);
// Calculate triangle edges and edge planes
FourTransposedPoints vertsA;
FourTransposedPoints edgesA;
FourTransposedPoints perpsA;
CalcTrianglePlanes(vertex0, vertex1, vertex2, normal, out vertsA, out edgesA, out perpsA);
// Check if the closest point is on the triangle face
FourTransposedPoints rels = new FourTransposedPoints(pointB) - vertsA;
if (PointTriangleFace(pointB, vertex0, normal, vertsA, edgesA, perpsA, rels, out float signedDistance))
{
return(new Result
{
PositionOnAinA = pointB + normal * signedDistance,
NormalInA = math.select(normal, -normal, signedDistance < 0),
Distance = math.abs(signedDistance) - sphereRadius
});
}
// Find the closest point on the triangle edges - project point onto the line through each edge, then clamp to the edge
float4 nums = rels.Dot(edgesA);
float4 dens = edgesA.Dot(edgesA);
float4 sols = math.clamp(nums / dens, 0.0f, 1.0f); // fraction along the edge TODO.ma see how it handles inf/nan from divide by zero
FourTransposedPoints projs = edgesA.MulT(sols) - rels;
float4 distancesSq = projs.Dot(projs);
float3 proj0 = projs.GetPoint(0);
float3 proj1 = projs.GetPoint(1);
float3 proj2 = projs.GetPoint(2);
// Find the closest projected point
bool less1 = distancesSq.x < distancesSq.y;
float3 direction = math.select(proj1, proj0, less1);
float distanceSq = math.select(distancesSq.y, distancesSq.x, less1);
bool less2 = distanceSq < distancesSq.z;
direction = math.select(proj2, direction, less2);
distanceSq = math.select(distancesSq.z, distanceSq, less2);
const float triangleConvexRadius = 0.0f;
return(PointPoint(pointB, direction, distanceSq, triangleConvexRadius, sphereRadius));
}
// Checks if the closest point on the triangle is on its face. If so returns true and sets signedDistance to the distance along the normal, otherwise returns false
private static bool PointTriangleFace(float3 point, float3 v0, float3 normal,
FourTransposedPoints verts, FourTransposedPoints edges, FourTransposedPoints perps, FourTransposedPoints rels, out float signedDistance)
{
float4 dots = perps.Dot(rels);
float4 dotsSq = dots * math.abs(dots);
float4 perpLengthSq = perps.Dot(perps);
if (math.all(dotsSq <= perpLengthSq * distanceEpsSq))
{
// Closest point on face
signedDistance = math.dot(v0 - point, normal);
return(true);
}
signedDistance = float.MaxValue;
return(false);
}
public float4 Dot(FourTransposedPoints a) => X * a.X + Y * a.Y + Z * a.Z;
public FourTransposedPoints Cross(FourTransposedPoints a)
{
return(new FourTransposedPoints {
m_TransposedPoints = new float4x3(Y * a.Z - Z * a.Y, Z * a.X - X * a.Z, X * a.Y - Y * a.X)
});
}
/// <summary>
/// Compute the closest point on the simplex, returns true if the simplex contains a duplicate vertex
/// </summary>
public void SolveDistance()
{
int inputVertices = NumVertices;
switch (NumVertices)
{
// Point.
case 1:
Direction = A.Xyz;
ScaledDistance = math.lengthsq(Direction);
break;
// Line.
case 2:
{
float3 delta = B.Xyz - A.Xyz;
sfloat den = math.dot(delta, delta);
sfloat num = math.dot(-A.Xyz, delta);
// Reduce if closest point do not project on the line segment.
if (num >= den)
{
NumVertices = 1; A = B; goto case 1;
}
// Compute support direction
Direction = math.cross(math.cross(delta, A.Xyz), delta);
ScaledDistance = math.dot(Direction, A.Xyz);
}
break;
// Triangle.
case 3:
{
float3 ca = A.Xyz - C.Xyz;
float3 cb = B.Xyz - C.Xyz;
float3 n = math.cross(cb, ca);
// Reduce if closest point do not project in the triangle.
float3 crossA = math.cross(cb, n);
float3 crossB = math.cross(n, ca);
sfloat detA = math.dot(crossA, B.Xyz);
sfloat detB = math.dot(crossB, C.Xyz);
if (detA < sfloat.Zero)
{
if (detB >= sfloat.Zero || Det(n, crossA, C.Xyz) < sfloat.Zero)
{
A = B;
}
}
else if (detB >= sfloat.Zero)
{
sfloat dot = math.dot(C.Xyz, n);
if (dot < sfloat.Zero)
{
// Reorder vertices so that n points away from the origin
SupportVertex temp = A;
A = B;
B = temp;
n = -n;
dot = -dot;
}
Direction = n;
ScaledDistance = dot;
break;
}
B = C;
NumVertices = 2;
goto case 2;
}
// Tetrahedra.
case 4:
{
FourTransposedPoints tetra = new FourTransposedPoints(A.Xyz, B.Xyz, C.Xyz, D.Xyz);
FourTransposedPoints d = new FourTransposedPoints(D.Xyz);
// This routine finds the closest feature to the origin on the tetra by testing the origin against the planes of the
// voronoi diagram. If the origin is near the border of two regions in the diagram, then the plane tests might exclude
// it from both because of float rounding. To avoid this problem we use some tolerance testing the face planes and let
// EPA handle those border cases. 1e-5 is a somewhat arbitrary value and the actual distance scales with the tetra, so
// this might need to be tuned later!
float3 faceTest = tetra.Cross(tetra.V1203).Dot(d).xyz;
if (math.all(faceTest >= sfloat.FromRaw(0xb727c5ac)))
{
// Origin is inside the tetra
Direction = float3.zero;
break;
}
// Check if the closest point is on a face
bool3 insideFace = (faceTest >= sfloat.Zero).xyz;
FourTransposedPoints edges = d - tetra;
FourTransposedPoints normals = edges.Cross(edges.V1203);
bool3 insideEdge0 = (normals.Cross(edges).Dot(d) >= sfloat.Zero).xyz;
bool3 insideEdge1 = (edges.V1203.Cross(normals).Dot(d) >= sfloat.Zero).xyz;
bool3 onFace = (insideEdge0 & insideEdge1 & !insideFace);
if (math.any(onFace))
{
if (onFace.y)
{
A = B; B = C;
}
else if (onFace.z)
{
B = C;
}
}
else
{
// Check if the closest point is on an edge
// TODO maybe we can safely drop two vertices in this case
bool3 insideVertex = (edges.Dot(d) >= 0).xyz;
bool3 onEdge = (!insideEdge0 & !insideEdge1.zxy & insideVertex);
if (math.any(onEdge.yz))
{
A = B; B = C;
}
}
C = D;
NumVertices = 3;
goto case 3;
}
}
}
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
});
}
