private void UpdateTriangleSimplex()
{
// The simplex is a triangle.
var info = new ClosestPointInfo();
GetClosestPointInTriangle(Vector3.Zero, _w[0], _w[1], _w[2], ref info);
ClosestPointOnA = _pointsA[0] * info.U
+ _pointsA[1] * info.V
+ _pointsA[2] * info.W;
ClosestPointOnB = _pointsB[0] * info.U
+ _pointsB[1] * info.V
+ _pointsB[2] * info.W;
ClosestPoint = info.ClosestPoint;
Reduce(ref info);
IsValid = true;
// Following assert can fail, for example, for ray convex test if objects
// are far away from the origin.
//Debug.Assert(Vector3.AreNumericallyEqual(ClosestPoint, ClosestPointOnA - ClosestPointOnB, Math.Max(1, ClosestPoint.Length) * 0.0001f), "ClosestPoint computed from barycentric coordinates must be equal to ClosestPointOnA - ClosestPointOnB.");
Debug.Assert(Numeric.IsGreaterOrEqual(info.U, 0));
Debug.Assert(Numeric.IsGreaterOrEqual(info.V, 0));
Debug.Assert(Numeric.IsGreaterOrEqual(info.W, 0));
Debug.Assert(Numeric.AreEqual(info.X, 0));
}
private void UpdateLineSimplex()
{
// The simplex is a line segment.
// Find closest point of line segment W[0] to W[1] to origin.
Vector3 segmentVector = _w[1] - _w[0];
Vector3 segmentStartToOrigin = Vector3.Zero - _w[0];
float param = Vector3.Dot(segmentVector, segmentStartToOrigin);
// Clamp parameter to [0,1].
if (param <= 0)
{
// Clamp to 0.
param = 0;
}
else
{
// Line parameter is > 0.
float lengthSquared = segmentVector.LengthSquared();
if (param > lengthSquared)
{
// Clamp to 1.
param = 1;
}
else
{
// Closest point is in the segment.
Debug.Assert(param > 0 && param <= lengthSquared);
// Normalize parameter;
param /= lengthSquared;
}
}
ClosestPointOnA = _pointsA[0] + param * (_pointsA[1] - _pointsA[0]);
ClosestPointOnB = _pointsB[0] + param * (_pointsB[1] - _pointsB[0]);
ClosestPoint = ClosestPointOnA - ClosestPointOnB;
var info = new ClosestPointInfo();
info.SetBarycentricCoordinates(1 - param, param, 0, 0);
Reduce(ref info);
IsValid = true;
Debug.Assert(
info.U >= 0 &&
info.V >= 0 &&
info.W >= 0 &&
info.X >= 0);
}
private void UpdateTetrahedronSimplex()
{
// Simplex is a tetrahedron.
ClosestPointInfo info = new ClosestPointInfo();
// Find closest point of tetrahedron to origin.
bool?containsOrigin = GetClosestPoints(Vector3.Zero, _w[0], _w[1], _w[2], _w[3], ref info);
// Origin is not contained.
ClosestPointOnA = _pointsA[0] * info.U
+ _pointsA[1] * info.V
+ _pointsA[2] * info.W
+ _pointsA[3] * info.X;
ClosestPointOnB = _pointsB[0] * info.U
+ _pointsB[1] * info.V
+ _pointsB[2] * info.W
+ _pointsB[3] * info.X;
ClosestPoint = info.ClosestPoint;
if (containsOrigin.HasValue)
{
IsValid = true;
if (!containsOrigin.Value)
{
Reduce(ref info);
}
}
else
{
// Degenerated.
IsValid = false;
}
// Following assert can fail, for example, for ray convex test if objects
// are far away from the origin.
//Debug.Assert(Vector3.AreNumericallyEqual(ClosestPoint, ClosestPointOnA - ClosestPointOnB, Math.Max(1, ClosestPoint.Length) * 0.0001f), "ClosestPoint computed from barycentric coordinates must be equal to ClosestPointOnA - ClosestPointOnB.");
Debug.Assert(Numeric.IsGreaterOrEqual(info.U, 0));
Debug.Assert(Numeric.IsGreaterOrEqual(info.V, 0));
Debug.Assert(Numeric.IsGreaterOrEqual(info.W, 0));
Debug.Assert(Numeric.IsGreaterOrEqual(info.X, 0));
}
/// <summary>
/// Removes unused entries from the arrays.
/// </summary>
private void Reduce(ref ClosestPointInfo info)
{
// We can remove an simplex vertices which do not contribute to the
// closest point. (If their barycentric coordinate/weight is 0, they do
// not contribute.)
if (NumberOfVertices >= 4 && info.X <= 0)
{
RemoveAt(3);
}
if (NumberOfVertices >= 3 && info.W <= 0)
{
RemoveAt(2);
}
if (NumberOfVertices >= 2 && info.V <= 0)
{
RemoveAt(1);
}
if (NumberOfVertices >= 1 && info.U <= 0)
{
RemoveAt(0);
}
}
private void UpdateTriangleSimplex()
{
// The simplex is a triangle.
var info = new ClosestPointInfo();
GetClosestPointInTriangle(Vector3F.Zero, _w[0], _w[1], _w[2], ref info);
ClosestPointOnA = _pointsA[0] * info.U
+ _pointsA[1] * info.V
+ _pointsA[2] * info.W;
ClosestPointOnB = _pointsB[0] * info.U
+ _pointsB[1] * info.V
+ _pointsB[2] * info.W;
ClosestPoint = info.ClosestPoint;
Reduce(ref info);
IsValid = true;
// Following assert can fail, for example, for ray convex test if objects
// are far away from the origin.
//Debug.Assert(Vector3F.AreNumericallyEqual(ClosestPoint, ClosestPointOnA - ClosestPointOnB, Math.Max(1, ClosestPoint.Length) * 0.0001f), "ClosestPoint computed from barycentric coordinates must be equal to ClosestPointOnA - ClosestPointOnB.");
Debug.Assert(Numeric.IsGreaterOrEqual(info.U, 0));
Debug.Assert(Numeric.IsGreaterOrEqual(info.V, 0));
Debug.Assert(Numeric.IsGreaterOrEqual(info.W, 0));
Debug.Assert(Numeric.AreEqual(info.X, 0));
}
private void UpdateTetrahedronSimplex()
{
// Simplex is a tetrahedron.
ClosestPointInfo info = new ClosestPointInfo();
// Find closest point of tetrahedron to origin.
bool? containsOrigin = GetClosestPoints(Vector3F.Zero, _w[0], _w[1], _w[2], _w[3], ref info);
// Origin is not contained.
ClosestPointOnA = _pointsA[0] * info.U
+ _pointsA[1] * info.V
+ _pointsA[2] * info.W
+ _pointsA[3] * info.X;
ClosestPointOnB = _pointsB[0] * info.U
+ _pointsB[1] * info.V
+ _pointsB[2] * info.W
+ _pointsB[3] * info.X;
ClosestPoint = info.ClosestPoint;
if (containsOrigin.HasValue)
{
IsValid = true;
if (!containsOrigin.Value)
Reduce(ref info);
}
else
{
// Degenerated.
IsValid = false;
}
// Following assert can fail, for example, for ray convex test if objects
// are far away from the origin.
//Debug.Assert(Vector3F.AreNumericallyEqual(ClosestPoint, ClosestPointOnA - ClosestPointOnB, Math.Max(1, ClosestPoint.Length) * 0.0001f), "ClosestPoint computed from barycentric coordinates must be equal to ClosestPointOnA - ClosestPointOnB.");
Debug.Assert(Numeric.IsGreaterOrEqual(info.U, 0));
Debug.Assert(Numeric.IsGreaterOrEqual(info.V, 0));
Debug.Assert(Numeric.IsGreaterOrEqual(info.W, 0));
Debug.Assert(Numeric.IsGreaterOrEqual(info.X, 0));
}
private void UpdateLineSimplex()
{
// The simplex is a line segment.
// Find closest point of line segment W[0] to W[1] to origin.
Vector3F segmentVector = _w[1] - _w[0];
Vector3F segmentStartToOrigin = Vector3F.Zero - _w[0];
float param = Vector3F.Dot(segmentVector, segmentStartToOrigin);
// Clamp parameter to [0,1].
if (param <= 0)
{
// Clamp to 0.
param = 0;
}
else
{
// Line parameter is > 0.
float lengthSquared = segmentVector.LengthSquared;
if (param > lengthSquared)
{
// Clamp to 1.
param = 1;
}
else
{
// Closest point is in the segment.
Debug.Assert(param > 0 && param <= lengthSquared);
// Normalize parameter;
param /= lengthSquared;
}
}
ClosestPointOnA = _pointsA[0] + param * (_pointsA[1] - _pointsA[0]);
ClosestPointOnB = _pointsB[0] + param * (_pointsB[1] - _pointsB[0]);
ClosestPoint = ClosestPointOnA - ClosestPointOnB;
var info = new ClosestPointInfo();
info.SetBarycentricCoordinates(1 - param, param, 0, 0);
Reduce(ref info);
IsValid = true;
Debug.Assert(
info.U >= 0
&& info.V >= 0
&& info.W >= 0
&& info.X >= 0);
}
/// <summary>
/// Removes unused entries from the arrays.
/// </summary>
private void Reduce(ref ClosestPointInfo info)
{
// We can remove an simplex vertices which do not contribute to the
// closest point. (If their barycentric coordinate/weight is 0, they do
// not contribute.)
if (NumberOfVertices >= 4 && info.X <= 0)
RemoveAt(3);
if (NumberOfVertices >= 3 && info.W <= 0)
RemoveAt(2);
if (NumberOfVertices >= 2 && info.V <= 0)
RemoveAt(1);
if (NumberOfVertices >= 1 && info.U <= 0)
RemoveAt(0);
}
private static bool? GetClosestPoints(Vector3F point, Vector3F tetrahedronVertexA, Vector3F tetrahedronVertexB, Vector3F tetrahedronVertexC, Vector3F tetrahedronVertexD, ref ClosestPointInfo info)
{
// Assume that the point is outside the tetrahedron. (This is the most likely case.)
bool? containsPoint = false;
// Check all tetrahedron faces to see if point is outside or inside the according plane.
bool? isPointOutsideABC = GeometryHelper.ArePointsOnOppositeSides(point, tetrahedronVertexD, tetrahedronVertexA, tetrahedronVertexB, tetrahedronVertexC);
bool? isPointOutsideACD = GeometryHelper.ArePointsOnOppositeSides(point, tetrahedronVertexB, tetrahedronVertexA, tetrahedronVertexC, tetrahedronVertexD);
bool? isPointOutsideADB = GeometryHelper.ArePointsOnOppositeSides(point, tetrahedronVertexC, tetrahedronVertexA, tetrahedronVertexD, tetrahedronVertexB);
bool? isPointOutsideBDC = GeometryHelper.ArePointsOnOppositeSides(point, tetrahedronVertexA, tetrahedronVertexB, tetrahedronVertexD, tetrahedronVertexC);
// The checks return null to indicate a degenerate/undecidable case. The origin
// touches a plane of a tetrahedron face or face of the tetrahedron are not valid triangles.
if (isPointOutsideABC == null || isPointOutsideACD == null || isPointOutsideADB == null || isPointOutsideBDC == null)
{
// Degenerate case.
containsPoint = null;
// Set all "undecided" face flags to true to test them for closest point.
if (isPointOutsideABC == null)
isPointOutsideABC = true;
if (isPointOutsideACD == null)
isPointOutsideACD = true;
if (isPointOutsideADB == null)
isPointOutsideADB = true;
if (isPointOutsideBDC == null)
isPointOutsideBDC = true;
}
else if (!(isPointOutsideABC.Value || isPointOutsideACD.Value || isPointOutsideADB.Value || isPointOutsideBDC.Value))
{
// The point is inside.
containsPoint = true;
// The GJK cannot compute the correct penetrating contact. We need EPA or MPR for this.
// But we can use the closest point of the tetrahedron as an approximate result for
// shallow contacts. --> Set all face flags to true to test them below.
isPointOutsideABC = true;
isPointOutsideACD = true;
isPointOutsideADB = true;
isPointOutsideBDC = true;
// Warning: It can happen that ABCD are all in a plane (e.g. when line segment vs line segment)
// is tested but the origin is outside! ArePointsOnOppositeSides does not detect all
// degenerate cases.
}
float bestDistanceSquared = float.MaxValue;
var tempInfo = new ClosestPointInfo();
// ----- ABC
// If points is outside of face ABC then compute closest point on ABC.
if (isPointOutsideABC.Value)
{
GetClosestPointInTriangle(point, tetrahedronVertexA, tetrahedronVertexB, tetrahedronVertexC, ref tempInfo);
Vector3F closestPoint = tempInfo.ClosestPoint;
// No comparison of actual distance with best distance required because this is the first test.
bestDistanceSquared = (point - closestPoint).LengthSquared;
info.ClosestPoint = closestPoint;
info.SetBarycentricCoordinates(tempInfo.U, tempInfo.V, tempInfo.W, 0);
}
// ----- ACD
// Repeat test for ACD.
if (isPointOutsideACD.Value)
{
GetClosestPointInTriangle(point, tetrahedronVertexA, tetrahedronVertexC, tetrahedronVertexD, ref tempInfo);
Vector3F closestPoint = tempInfo.ClosestPoint;
float distance = (point - closestPoint).LengthSquared;
if (distance < bestDistanceSquared)
{
bestDistanceSquared = distance;
info.ClosestPoint = closestPoint;
info.SetBarycentricCoordinates(tempInfo.U, 0, tempInfo.V, tempInfo.W);
}
}
// ----- ADB
// Repeat test for ADB.
if (isPointOutsideADB.Value)
{
GetClosestPointInTriangle(point, tetrahedronVertexA, tetrahedronVertexD, tetrahedronVertexB, ref tempInfo);
Vector3F closestPoint = tempInfo.ClosestPoint;
float distance = (point - closestPoint).LengthSquared;
if (distance < bestDistanceSquared)
{
bestDistanceSquared = distance;
info.ClosestPoint = closestPoint;
info.SetBarycentricCoordinates(tempInfo.U, tempInfo.W, 0, tempInfo.V);
}
}
// ----- BDC
// Repeat test for BDC.
if (isPointOutsideBDC.Value)
{
GetClosestPointInTriangle(point, tetrahedronVertexB, tetrahedronVertexD, tetrahedronVertexC, ref tempInfo);
Vector3F closestPoint = tempInfo.ClosestPoint;
float distance = (point - closestPoint).LengthSquared;
if (distance < bestDistanceSquared)
{
//bestDistanceSquared = distance; // Not needed anymore.
info.ClosestPoint = closestPoint;
info.SetBarycentricCoordinates(0, tempInfo.U, tempInfo.W, tempInfo.V);
}
}
// If we do not contain the point, then the simplex must not be full.
Debug.Assert(containsPoint == null
|| containsPoint == true
|| info.U <= 0
|| info.V <= 0
|| info.W <= 0
|| info.X <= 0);
return containsPoint;
}
/// <summary>
/// Computes the point in the triangle which is closest to <paramref name="point"/>
/// </summary>
/// <param name="point">The point.</param>
/// <param name="vertex0">The first vertex of the triangle.</param>
/// <param name="vertex1">The second vertex of the triangle.</param>
/// <param name="vertex2">The third vertex of the triangle.</param>
/// <param name="closestPointInfo">The closest-point information that will be set.</param>
private static void GetClosestPointInTriangle(Vector3F point, Vector3F vertex0, Vector3F vertex1, Vector3F vertex2, ref ClosestPointInfo closestPointInfo)
{
float u, v, w;
GeometryHelper.GetClosestPoint(new Triangle(vertex0, vertex1, vertex2), point, out u, out v, out w);
closestPointInfo.SetBarycentricCoordinates(u, v, w, 0);
closestPointInfo.ClosestPoint = u * vertex0 + v * vertex1 + w * vertex2;
}
private static bool?GetClosestPoints(Vector3 point, Vector3 tetrahedronVertexA, Vector3 tetrahedronVertexB, Vector3 tetrahedronVertexC, Vector3 tetrahedronVertexD, ref ClosestPointInfo info)
{
// Assume that the point is outside the tetrahedron. (This is the most likely case.)
bool?containsPoint = false;
// Check all tetrahedron faces to see if point is outside or inside the according plane.
bool?isPointOutsideABC = GeometryHelper.ArePointsOnOppositeSides(point, tetrahedronVertexD, tetrahedronVertexA, tetrahedronVertexB, tetrahedronVertexC);
bool?isPointOutsideACD = GeometryHelper.ArePointsOnOppositeSides(point, tetrahedronVertexB, tetrahedronVertexA, tetrahedronVertexC, tetrahedronVertexD);
bool?isPointOutsideADB = GeometryHelper.ArePointsOnOppositeSides(point, tetrahedronVertexC, tetrahedronVertexA, tetrahedronVertexD, tetrahedronVertexB);
bool?isPointOutsideBDC = GeometryHelper.ArePointsOnOppositeSides(point, tetrahedronVertexA, tetrahedronVertexB, tetrahedronVertexD, tetrahedronVertexC);
// The checks return null to indicate a degenerate/undecidable case. The origin
// touches a plane of a tetrahedron face or face of the tetrahedron are not valid triangles.
if (isPointOutsideABC == null || isPointOutsideACD == null || isPointOutsideADB == null || isPointOutsideBDC == null)
{
// Degenerate case.
containsPoint = null;
// Set all "undecided" face flags to true to test them for closest point.
if (isPointOutsideABC == null)
{
isPointOutsideABC = true;
}
if (isPointOutsideACD == null)
{
isPointOutsideACD = true;
}
if (isPointOutsideADB == null)
{
isPointOutsideADB = true;
}
if (isPointOutsideBDC == null)
{
isPointOutsideBDC = true;
}
}
else if (!(isPointOutsideABC.Value || isPointOutsideACD.Value || isPointOutsideADB.Value || isPointOutsideBDC.Value))
{
// The point is inside.
containsPoint = true;
// The GJK cannot compute the correct penetrating contact. We need EPA or MPR for this.
// But we can use the closest point of the tetrahedron as an approximate result for
// shallow contacts. --> Set all face flags to true to test them below.
isPointOutsideABC = true;
isPointOutsideACD = true;
isPointOutsideADB = true;
isPointOutsideBDC = true;
// Warning: It can happen that ABCD are all in a plane (e.g. when line segment vs line segment)
// is tested but the origin is outside! ArePointsOnOppositeSides does not detect all
// degenerate cases.
}
float bestDistanceSquared = float.MaxValue;
var tempInfo = new ClosestPointInfo();
// ----- ABC
// If points is outside of face ABC then compute closest point on ABC.
if (isPointOutsideABC.Value)
{
GetClosestPointInTriangle(point, tetrahedronVertexA, tetrahedronVertexB, tetrahedronVertexC, ref tempInfo);
Vector3 closestPoint = tempInfo.ClosestPoint;
// No comparison of actual distance with best distance required because this is the first test.
bestDistanceSquared = (point - closestPoint).LengthSquared();
info.ClosestPoint = closestPoint;
info.SetBarycentricCoordinates(tempInfo.U, tempInfo.V, tempInfo.W, 0);
}
// ----- ACD
// Repeat test for ACD.
if (isPointOutsideACD.Value)
{
GetClosestPointInTriangle(point, tetrahedronVertexA, tetrahedronVertexC, tetrahedronVertexD, ref tempInfo);
Vector3 closestPoint = tempInfo.ClosestPoint;
float distance = (point - closestPoint).LengthSquared();
if (distance < bestDistanceSquared)
{
bestDistanceSquared = distance;
info.ClosestPoint = closestPoint;
info.SetBarycentricCoordinates(tempInfo.U, 0, tempInfo.V, tempInfo.W);
}
}
// ----- ADB
// Repeat test for ADB.
if (isPointOutsideADB.Value)
{
GetClosestPointInTriangle(point, tetrahedronVertexA, tetrahedronVertexD, tetrahedronVertexB, ref tempInfo);
Vector3 closestPoint = tempInfo.ClosestPoint;
float distance = (point - closestPoint).LengthSquared();
if (distance < bestDistanceSquared)
{
bestDistanceSquared = distance;
info.ClosestPoint = closestPoint;
info.SetBarycentricCoordinates(tempInfo.U, tempInfo.W, 0, tempInfo.V);
}
}
// ----- BDC
// Repeat test for BDC.
if (isPointOutsideBDC.Value)
{
GetClosestPointInTriangle(point, tetrahedronVertexB, tetrahedronVertexD, tetrahedronVertexC, ref tempInfo);
Vector3 closestPoint = tempInfo.ClosestPoint;
float distance = (point - closestPoint).LengthSquared();
if (distance < bestDistanceSquared)
{
//bestDistanceSquared = distance; // Not needed anymore.
info.ClosestPoint = closestPoint;
info.SetBarycentricCoordinates(0, tempInfo.U, tempInfo.W, tempInfo.V);
}
}
// If we do not contain the point, then the simplex must not be full.
Debug.Assert(containsPoint == null ||
containsPoint == true ||
info.U <= 0 ||
info.V <= 0 ||
info.W <= 0 ||
info.X <= 0);
return(containsPoint);
}
/// <summary>
/// Computes the point in the triangle which is closest to <paramref name="point"/>
/// </summary>
/// <param name="point">The point.</param>
/// <param name="vertex0">The first vertex of the triangle.</param>
/// <param name="vertex1">The second vertex of the triangle.</param>
/// <param name="vertex2">The third vertex of the triangle.</param>
/// <param name="closestPointInfo">The closest-point information that will be set.</param>
private static void GetClosestPointInTriangle(Vector3 point, Vector3 vertex0, Vector3 vertex1, Vector3 vertex2, ref ClosestPointInfo closestPointInfo)
{
float u, v, w;
GeometryHelper.GetClosestPoint(new Triangle(vertex0, vertex1, vertex2), point, out u, out v, out w);
closestPointInfo.SetBarycentricCoordinates(u, v, w, 0);
closestPointInfo.ClosestPoint = u * vertex0 + v * vertex1 + w * vertex2;
}
