// 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 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));
}
/// <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;
}
}
}