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