public bool GetPointOnTetrahedronClosestToOrigin(out Vector3 point)
{
//Thanks to the fact that D is new and that we know that the origin is within the extruded
//triangular prism of ABC (and on the "D" side of ABC),
//we can immediately ignore voronoi regions:
//A, B, C, AC, AB, BC, ABC
//and only consider:
//D, DA, DB, DC, DAC, DCB, DBA
//There is some overlap of calculations in this method, since DAC, DCB, and DBA are tested fully.
PairSimplex minimumSimplex = new PairSimplex();
point = new Vector3();
float minimumDistance = float.MaxValue;
PairSimplex candidate;
float candidateDistance;
Vector3 candidatePoint;
if (TryTetrahedronTriangle(ref A, ref C, ref D,
ref SimplexA.A, ref SimplexA.C, ref SimplexA.D,
ref SimplexB.A, ref SimplexB.C, ref SimplexB.D,
errorTolerance,
ref B, out candidate, out candidatePoint))
{
point = candidatePoint;
minimumSimplex = candidate;
minimumDistance = candidatePoint.LengthSquared();
}
//Try BDC instead of CBD
if (TryTetrahedronTriangle(ref B, ref D, ref C,
ref SimplexA.B, ref SimplexA.D, ref SimplexA.C,
ref SimplexB.B, ref SimplexB.D, ref SimplexB.C,
errorTolerance,
ref A, out candidate, out candidatePoint) &&
(candidateDistance = candidatePoint.LengthSquared()) < minimumDistance)
{
point = candidatePoint;
minimumSimplex = candidate;
minimumDistance = candidateDistance;
}
//Try ADB instead of BAD
if (TryTetrahedronTriangle(ref A, ref D, ref B,
ref SimplexA.A, ref SimplexA.D, ref SimplexA.B,
ref SimplexB.A, ref SimplexB.D, ref SimplexB.B,
errorTolerance,
ref C, out candidate, out candidatePoint) &&
(candidateDistance = candidatePoint.LengthSquared()) < minimumDistance)
{
point = candidatePoint;
minimumSimplex = candidate;
minimumDistance = candidateDistance;
}
if (TryTetrahedronTriangle(ref A, ref B, ref C,
ref SimplexA.A, ref SimplexA.B, ref SimplexA.C,
ref SimplexB.A, ref SimplexB.B, ref SimplexB.C,
errorTolerance,
ref D, out candidate, out candidatePoint) &&
(candidateDistance = candidatePoint.LengthSquared()) < minimumDistance)
{
point = candidatePoint;
minimumSimplex = candidate;
minimumDistance = candidateDistance;
}
if (minimumDistance < float.MaxValue)
{
minimumSimplex.LocalTransformB = LocalTransformB;
minimumSimplex.previousDistanceToClosest = previousDistanceToClosest;
minimumSimplex.errorTolerance = errorTolerance;
this = minimumSimplex;
return false;
}
return true;
}
