private static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB,
ref CachedSimplex cachedSimplex, out Vector3 localClosestPointA, out Vector3 localClosestPointB)
{
var simplex = new PairSimplex(ref cachedSimplex, ref localTransformB);
Vector3 closestPoint;
int count = 0;
while (true)
{
if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex and computes barycentric coordinates if necessary.
closestPoint.LengthSquared() <= Toolbox.Epsilon * simplex.errorTolerance)
{
//Intersecting.
localClosestPointA = Toolbox.ZeroVector;
localClosestPointB = Toolbox.ZeroVector;
simplex.UpdateCachedSimplex(ref cachedSimplex);
return(true);
}
if (++count > MaximumGJKIterations)
{
break; //Must break BEFORE a new vertex is added if we're over the iteration limit. This guarantees final simplex is not a tetrahedron.
}
if (simplex.GetNewSimplexPoint(shapeA, shapeB, count, ref closestPoint))
{
//No progress towards origin, not intersecting.
break;
}
}
//Compute closest points from the contributing simplexes and barycentric coordinates
simplex.GetClosestPoints(out localClosestPointA, out localClosestPointB);
//simplex.VerifyContributions();
//if (Vector3.Distance(localClosestPointA - localClosestPointB, closestPoint) > .00001f)
// Debug.WriteLine("break.");
simplex.UpdateCachedSimplex(ref cachedSimplex);
return(false);
}
private static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB,
ref CachedSimplex cachedSimplex, out Vector3 localClosestPointA, out Vector3 localClosestPointB)
{
var simplex = new PairSimplex(ref cachedSimplex, ref localTransformB);
Vector3 closestPoint;
int count = 0;
while (true)
{
if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex and computes barycentric coordinates if necessary.
closestPoint.LengthSquared() <= Toolbox.Epsilon * simplex.errorTolerance)
{
//Intersecting.
localClosestPointA = Toolbox.ZeroVector;
localClosestPointB = Toolbox.ZeroVector;
simplex.UpdateCachedSimplex(ref cachedSimplex);
return true;
}
if (++count > MaximumGJKIterations)
break; //Must break BEFORE a new vertex is added if we're over the iteration limit. This guarantees final simplex is not a tetrahedron.
if (simplex.GetNewSimplexPoint(shapeA, shapeB, count, ref closestPoint))
{
//No progress towards origin, not intersecting.
break;
}
}
//Compute closest points from the contributing simplexes and barycentric coordinates
simplex.GetClosestPoints(out localClosestPointA, out localClosestPointB);
//simplex.VerifyContributions();
//if (Vector3.Distance(localClosestPointA - localClosestPointB, closestPoint) > .00001f)
// Debug.WriteLine("break.");
simplex.UpdateCachedSimplex(ref cachedSimplex);
return false;
}
private static bool TryTetrahedronTriangle(ref Vector3 A, ref Vector3 B, ref Vector3 C,
ref Vector3 A1, ref Vector3 B1, ref Vector3 C1,
ref Vector3 A2, ref Vector3 B2, ref Vector3 C2,
Fix64 errorTolerance,
ref Vector3 otherPoint, out PairSimplex simplex, out Vector3 point)
{
//Note that there may be some extra terms that can be removed from this process.
//Some conditions could use less parameters, since it is known that the origin
//is not 'behind' BC or AC.
simplex = new PairSimplex();
point = new Vector3();
Vector3 ab, ac;
Vector3.Subtract(ref B, ref A, out ab);
Vector3.Subtract(ref C, ref A, out ac);
Vector3 normal;
Vector3.Cross(ref ab, ref ac, out normal);
Fix64 AdotN, ADdotN;
Vector3 AD;
Vector3.Subtract(ref otherPoint, ref A, out AD);
Vector3.Dot(ref A, ref normal, out AdotN);
Vector3.Dot(ref AD, ref normal, out ADdotN);
//If (-A * N) * (AD * N) < 0, D and O are on opposite sides of the triangle.
if (AdotN * ADdotN >= -Toolbox.Epsilon * errorTolerance)
{
//The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector,
//just use -A.
//Same for B->, C->P...
//Check to see if it's outside A.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A.
Fix64 AdotAB, AdotAC;
Vector3.Dot(ref ab, ref A, out AdotAB);
Vector3.Dot(ref ac, ref A, out AdotAC);
AdotAB = -AdotAB;
AdotAC = -AdotAC;
if (AdotAC <= F64.C0 && AdotAB <= F64.C0)
{
//It is A!
simplex.State = SimplexState.Point;
simplex.A = A;
simplex.U = F64.C1;
simplex.SimplexA.A = A1;
simplex.SimplexB.A = A2;
point = A;
return(true);
}
//Check to see if it's outside B.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B.
Fix64 BdotAB, BdotAC;
Vector3.Dot(ref ab, ref B, out BdotAB);
Vector3.Dot(ref ac, ref B, out BdotAC);
BdotAB = -BdotAB;
BdotAC = -BdotAC;
if (BdotAB >= F64.C0 && BdotAC <= BdotAB)
{
//It is B!
simplex.State = SimplexState.Point;
simplex.A = B;
simplex.U = F64.C1;
simplex.SimplexA.A = B1;
simplex.SimplexB.A = B2;
point = B;
return(true);
}
//Check to see if it's outside AB.
Fix64 vc = AdotAB * BdotAC - BdotAB * AdotAC;
if (vc <= F64.C0 && AdotAB > F64.C0 && BdotAB < F64.C0) //Note > and < instead of => <=; avoids possibly division by zero
{
simplex.State = SimplexState.Segment;
simplex.V = AdotAB / (AdotAB - BdotAB);
simplex.U = F64.C1 - simplex.V;
simplex.A = A;
simplex.B = B;
simplex.SimplexA.A = A1;
simplex.SimplexB.A = A2;
simplex.SimplexA.B = B1;
simplex.SimplexB.B = B2;
Vector3.Multiply(ref ab, simplex.V, out point);
Vector3.Add(ref point, ref A, out point);
return(true);
}
//Check to see if it's outside C.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C.
Fix64 CdotAB, CdotAC;
Vector3.Dot(ref ab, ref C, out CdotAB);
Vector3.Dot(ref ac, ref C, out CdotAC);
CdotAB = -CdotAB;
CdotAC = -CdotAC;
if (CdotAC >= F64.C0 && CdotAB <= CdotAC)
{
//It is C!
simplex.State = SimplexState.Point;
simplex.A = C;
simplex.U = F64.C1;
simplex.SimplexA.A = C1;
simplex.SimplexB.A = C2;
point = C;
return(true);
}
//Check if it's outside AC.
//Fix64 AdotAB, AdotAC;
//Vector3.Dot(ref ab, ref A, out AdotAB);
//Vector3.Dot(ref ac, ref A, out AdotAC);
//AdotAB = -AdotAB;
//AdotAC = -AdotAC;
Fix64 vb = CdotAB * AdotAC - AdotAB * CdotAC;
if (vb <= F64.C0 && AdotAC > F64.C0 && CdotAC < F64.C0) //Note > instead of >= and < instead of <=; prevents bad denominator
{
simplex.State = SimplexState.Segment;
simplex.A = A;
simplex.B = C;
simplex.SimplexA.A = A1;
simplex.SimplexA.B = C1;
simplex.SimplexB.A = A2;
simplex.SimplexB.B = C2;
simplex.V = AdotAC / (AdotAC - CdotAC);
simplex.U = F64.C1 - simplex.V;
Vector3.Multiply(ref ac, simplex.V, out point);
Vector3.Add(ref point, ref A, out point);
return(true);
}
//Check if it's outside BC.
//Fix64 BdotAB, BdotAC;
//Vector3.Dot(ref ab, ref B, out BdotAB);
//Vector3.Dot(ref ac, ref B, out BdotAC);
//BdotAB = -BdotAB;
//BdotAC = -BdotAC;
Fix64 va = BdotAB * CdotAC - CdotAB * BdotAC;
Fix64 d3d4;
Fix64 d6d5;
if (va <= F64.C0 && (d3d4 = BdotAC - BdotAB) > F64.C0 && (d6d5 = CdotAB - CdotAC) > F64.C0)//Note > instead of >= and < instead of <=; prevents bad denominator
{
simplex.State = SimplexState.Segment;
simplex.A = B;
simplex.B = C;
simplex.SimplexA.A = B1;
simplex.SimplexA.B = C1;
simplex.SimplexB.A = B2;
simplex.SimplexB.B = C2;
simplex.V = d3d4 / (d3d4 + d6d5);
simplex.U = F64.C1 - simplex.V;
Vector3 bc;
Vector3.Subtract(ref C, ref B, out bc);
Vector3.Multiply(ref bc, simplex.V, out point);
Vector3.Add(ref point, ref B, out point);
return(true);
}
//On the face of the triangle.
simplex.A = A;
simplex.B = B;
simplex.C = C;
simplex.SimplexA.A = A1;
simplex.SimplexA.B = B1;
simplex.SimplexA.C = C1;
simplex.SimplexB.A = A2;
simplex.SimplexB.B = B2;
simplex.SimplexB.C = C2;
simplex.State = SimplexState.Triangle;
Fix64 denom = (va + vb + vc);
simplex.W = vc / denom;
simplex.V = vb / denom;
simplex.U = F64.C1 - simplex.V - simplex.W;
Vector3.Multiply(ref ab, simplex.V, out point);
Vector3 acw;
Vector3.Multiply(ref ac, simplex.W, out acw);
Vector3.Add(ref A, ref point, out point);
Vector3.Add(ref point, ref acw, out point);
return(true);
}
return(false);
}
///<summary>
/// Gets the point on the tetrahedron closest to the origin.
///</summary>
///<param name="point">Closest point to the origin.</param>
///<returns>Whether or not the tetrahedron encloses the origin.</returns>
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();
Fix64 minimumDistance = Fix64.MaxValue;
PairSimplex candidate;
Fix64 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 < Fix64.MaxValue)
{
minimumSimplex.LocalTransformB = LocalTransformB;
minimumSimplex.previousDistanceToClosest = previousDistanceToClosest;
minimumSimplex.errorTolerance = errorTolerance;
this = minimumSimplex;
return(false);
}
return(true);
}
private static bool TryTetrahedronTriangle(ref Vector3 A, ref Vector3 B, ref Vector3 C,
ref Vector3 A1, ref Vector3 B1, ref Vector3 C1,
ref Vector3 A2, ref Vector3 B2, ref Vector3 C2,
float errorTolerance,
ref Vector3 otherPoint, out PairSimplex simplex, out Vector3 point)
{
//Note that there may be some extra terms that can be removed from this process.
//Some conditions could use less parameters, since it is known that the origin
//is not 'behind' BC or AC.
simplex = new PairSimplex();
point = new Vector3();
Vector3 ab, ac;
Vector3.Subtract(ref B, ref A, out ab);
Vector3.Subtract(ref C, ref A, out ac);
Vector3 normal;
Vector3.Cross(ref ab, ref ac, out normal);
float AdotN, ADdotN;
Vector3 AD;
Vector3.Subtract(ref otherPoint, ref A, out AD);
Vector3.Dot(ref A, ref normal, out AdotN);
Vector3.Dot(ref AD, ref normal, out ADdotN);
//If (-A * N) * (AD * N) < 0, D and O are on opposite sides of the triangle.
if (AdotN * ADdotN >= -Toolbox.Epsilon * errorTolerance)
{
//The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector,
//just use -A.
//Same for B->, C->P...
//Check to see if it's outside A.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A.
float AdotAB, AdotAC;
Vector3.Dot(ref ab, ref A, out AdotAB);
Vector3.Dot(ref ac, ref A, out AdotAC);
AdotAB = -AdotAB;
AdotAC = -AdotAC;
if (AdotAC <= 0f && AdotAB <= 0)
{
//It is A!
simplex.State = SimplexState.Point;
simplex.A = A;
simplex.U = 1;
simplex.SimplexA.A = A1;
simplex.SimplexB.A = A2;
point = A;
return true;
}
//Check to see if it's outside B.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B.
float BdotAB, BdotAC;
Vector3.Dot(ref ab, ref B, out BdotAB);
Vector3.Dot(ref ac, ref B, out BdotAC);
BdotAB = -BdotAB;
BdotAC = -BdotAC;
if (BdotAB >= 0f && BdotAC <= BdotAB)
{
//It is B!
simplex.State = SimplexState.Point;
simplex.A = B;
simplex.U = 1;
simplex.SimplexA.A = B1;
simplex.SimplexB.A = B2;
point = B;
return true;
}
//Check to see if it's outside AB.
float vc = AdotAB * BdotAC - BdotAB * AdotAC;
if (vc <= 0 && AdotAB > 0 && BdotAB < 0) //Note > and < instead of => <=; avoids possibly division by zero
{
simplex.State = SimplexState.Segment;
simplex.V = AdotAB / (AdotAB - BdotAB);
simplex.U = 1 - simplex.V;
simplex.A = A;
simplex.B = B;
simplex.SimplexA.A = A1;
simplex.SimplexB.A = A2;
simplex.SimplexA.B = B1;
simplex.SimplexB.B = B2;
Vector3.Multiply(ref ab, simplex.V, out point);
Vector3.Add(ref point, ref A, out point);
return true;
}
//Check to see if it's outside C.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C.
float CdotAB, CdotAC;
Vector3.Dot(ref ab, ref C, out CdotAB);
Vector3.Dot(ref ac, ref C, out CdotAC);
CdotAB = -CdotAB;
CdotAC = -CdotAC;
if (CdotAC >= 0f && CdotAB <= CdotAC)
{
//It is C!
simplex.State = SimplexState.Point;
simplex.A = C;
simplex.U = 1;
simplex.SimplexA.A = C1;
simplex.SimplexB.A = C2;
point = C;
return true;
}
//Check if it's outside AC.
//float AdotAB, AdotAC;
//Vector3.Dot(ref ab, ref A, out AdotAB);
//Vector3.Dot(ref ac, ref A, out AdotAC);
//AdotAB = -AdotAB;
//AdotAC = -AdotAC;
float vb = CdotAB * AdotAC - AdotAB * CdotAC;
if (vb <= 0f && AdotAC > 0f && CdotAC < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator
{
simplex.State = SimplexState.Segment;
simplex.A = A;
simplex.B = C;
simplex.SimplexA.A = A1;
simplex.SimplexA.B = C1;
simplex.SimplexB.A = A2;
simplex.SimplexB.B = C2;
simplex.V = AdotAC / (AdotAC - CdotAC);
simplex.U = 1 - simplex.V;
Vector3.Multiply(ref ac, simplex.V, out point);
Vector3.Add(ref point, ref A, out point);
return true;
}
//Check if it's outside BC.
//float BdotAB, BdotAC;
//Vector3.Dot(ref ab, ref B, out BdotAB);
//Vector3.Dot(ref ac, ref B, out BdotAC);
//BdotAB = -BdotAB;
//BdotAC = -BdotAC;
float va = BdotAB * CdotAC - CdotAB * BdotAC;
float d3d4;
float d6d5;
if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator
{
simplex.State = SimplexState.Segment;
simplex.A = B;
simplex.B = C;
simplex.SimplexA.A = B1;
simplex.SimplexA.B = C1;
simplex.SimplexB.A = B2;
simplex.SimplexB.B = C2;
simplex.V = d3d4 / (d3d4 + d6d5);
simplex.U = 1 - simplex.V;
Vector3 bc;
Vector3.Subtract(ref C, ref B, out bc);
Vector3.Multiply(ref bc, simplex.V, out point);
Vector3.Add(ref point, ref B, out point);
return true;
}
//On the face of the triangle.
simplex.A = A;
simplex.B = B;
simplex.C = C;
simplex.SimplexA.A = A1;
simplex.SimplexA.B = B1;
simplex.SimplexA.C = C1;
simplex.SimplexB.A = A2;
simplex.SimplexB.B = B2;
simplex.SimplexB.C = C2;
simplex.State = SimplexState.Triangle;
float denom = 1f / (va + vb + vc);
simplex.W = vc * denom;
simplex.V = vb * denom;
simplex.U = 1 - simplex.V - simplex.W;
Vector3.Multiply(ref ab, simplex.V, out point);
Vector3 acw;
Vector3.Multiply(ref ac, simplex.W, out acw);
Vector3.Add(ref A, ref point, out point);
Vector3.Add(ref point, ref acw, out point);
return true;
}
return false;
}
///<summary>
/// Gets the point on the tetrahedron closest to the origin.
///</summary>
///<param name="point">Closest point to the origin.</param>
///<returns>Whether or not the tetrahedron encloses the origin.</returns>
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;
}