///<summary>
/// Gets the closest points between the shapes.
///</summary>
///<param name="shapeA">First shape of the pair.</param>
///<param name="shapeB">Second shape of the pair.</param>
///<param name="transformA">Transform to apply to the first shape.</param>
///<param name="transformB">Transform to apply to the second shape.</param>
/// <param name="cachedSimplex">Simplex from a previous updated used to warmstart the current attempt. Updated after each run.</param>
///<param name="closestPointA">Closest point on the first shape to the second shape.</param>
///<param name="closestPointB">Closest point on the second shape to the first shape.</param>
///<returns>Whether or not the objects were intersecting. If they are intersecting, then the closest points cannot be identified.</returns>
public static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB,
ref CachedSimplex cachedSimplex, out Vector3 closestPointA, out Vector3 closestPointB)
{
RigidTransform localtransformB;
MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB);
bool toReturn = GetClosestPoints(shapeA, shapeB, ref localtransformB, ref cachedSimplex, out closestPointA, out closestPointB);
RigidTransform.Transform(ref closestPointA, ref transformA, out closestPointA);
RigidTransform.Transform(ref closestPointB, ref transformA, out closestPointB);
return(toReturn);
}
///<summary>
/// Updates the cached simplex with the latest run's results.
///</summary>
///<param name="simplex">Simplex to update.</param>
public void UpdateCachedSimplex(ref CachedSimplex simplex)
{
simplex.LocalSimplexA = SimplexA;
switch (State)
{
case SimplexState.Point:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Quaternion conjugate;
Quaternion.Conjugate(ref LocalTransformB.Orientation, out conjugate);
Vector3.Transform(ref simplex.LocalSimplexB.A, ref conjugate, out simplex.LocalSimplexB.A);
break;
case SimplexState.Segment:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
Matrix3x3 transform;
Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B);
break;
case SimplexState.Triangle:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C);
Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C);
break;
case SimplexState.Tetrahedron:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C);
Vector3.Subtract(ref SimplexB.D, ref LocalTransformB.Position, out simplex.LocalSimplexB.D);
Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.D, ref transform, out simplex.LocalSimplexB.D);
break;
}
simplex.State = State;
}
///<summary>
/// Gets the closest points between the shapes.
///</summary>
///<param name="shapeA">First shape of the pair.</param>
///<param name="shapeB">Second shape of the pair.</param>
///<param name="transformA">Transform to apply to the first shape.</param>
///<param name="transformB">Transform to apply to the second shape.</param>
///<param name="closestPointA">Closest point on the first shape to the second shape.</param>
///<param name="closestPointB">Closest point on the second shape to the first shape.</param>
///<returns>Whether or not the objects were intersecting. If they are intersecting, then the closest points cannot be identified.</returns>
public static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB,
out Vector3 closestPointA, out Vector3 closestPointB)
{
//The cached simplex stores locations that are local to the shapes. A fairly decent initial state is between the centroids of the objects.
//In local space, the centroids are at the origins.
RigidTransform localtransformB;
MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB);
var simplex = new CachedSimplex {
State = SimplexState.Point
};
// new CachedSimplex(shapeA, shapeB, ref localtransformB);
bool toReturn = GetClosestPoints(shapeA, shapeB, ref localtransformB, ref simplex, out closestPointA, out closestPointB);
RigidTransform.Transform(ref closestPointA, ref transformA, out closestPointA);
RigidTransform.Transform(ref closestPointB, ref transformA, out closestPointB);
return(toReturn);
}
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);
}
///<summary>
/// Constructs a new pair simplex.
///</summary>
///<param name="cachedSimplex">Cached simplex to use to warmstart the simplex.</param>
///<param name="localTransformB">Transform of shape B in the local space of A.</param>
public PairSimplex(ref CachedSimplex cachedSimplex, ref RigidTransform localTransformB)
{
//NOTE:
//USING A CACHED SIMPLEX INVALIDATES ASSUMPTIONS THAT ALLOW SIMPLEX CASES TO BE IGNORED!
//To get those assumptions back, either DO NOT USE CACHED SIMPLEXES, or
//VERIFY THE SIMPLEXES.
//-A point requires no verification.
//-A segment needs verification that the origin is in front of A in the direction of B.
//-A triangle needs verification that the origin is within the edge planes and in the direction of C.
//-A tetrahedron needs verification that the origin is within the edge planes of triangle ABC and is in the direction of D.
//This simplex implementation will not ignore any cases, so we can warm start safely with one problem.
//Due to relative movement, the simplex may become degenerate. Edges could become points, etc.
//Some protections are built into the simplex cases, but keep an eye out for issues.
//Most dangerous degeneracy seen so far is tetrahedron. It fails to find any points on opposing sides due to numerical problems and returns intersection.
previousDistanceToClosest = float.MaxValue;
errorTolerance = 0;
LocalTransformB = localTransformB;
//Transform the SimplexB into the working space of the simplex and compute the working space simplex.
State = cachedSimplex.State;
SimplexA = cachedSimplex.LocalSimplexA;
SimplexB = new ContributingShapeSimplex();
U = 0;
V = 0;
W = 0;
switch (State)
{
case SimplexState.Point:
Vector3.Transform(ref cachedSimplex.LocalSimplexB.A, ref LocalTransformB.Orientation, out SimplexB.A);
Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
B = new Vector3();
C = new Vector3();
D = new Vector3();
break;
case SimplexState.Segment:
Matrix3x3 transform;
Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B);
Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B);
Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B);
C = new Vector3();
D = new Vector3();
////Test for degeneracy.
//float edgeLengthAB;
//Vector3.DistanceSquared(ref A, ref B, out edgeLengthAB);
//if (edgeLengthAB < Toolbox.Epsilon)
// State = SimplexState.Point;
break;
case SimplexState.Triangle:
Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C);
Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B);
Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C);
Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B);
Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C);
D = new Vector3();
////Test for degeneracy.
//Vector3 AB, AC;
//Vector3.Subtract(ref B, ref A, out AB);
//Vector3.Subtract(ref C, ref A, out AC);
//Vector3 cross;
//Vector3.Cross(ref AB, ref AC, out cross);
////If the area is small compared to a tolerance (adjusted by the partial perimeter), it's degenerate.
//if (cross.LengthSquared() < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared()))
// State = SimplexState.Point;
break;
case SimplexState.Tetrahedron:
Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.D, ref transform, out SimplexB.D);
Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B);
Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C);
Vector3.Add(ref SimplexB.D, ref LocalTransformB.Position, out SimplexB.D);
Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B);
Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C);
Vector3.Subtract(ref SimplexA.D, ref SimplexB.D, out D);
////Test for degeneracy.
//Vector3 AD;
//Vector3.Subtract(ref B, ref A, out AB);
//Vector3.Subtract(ref C, ref A, out AC);
//Vector3.Subtract(ref D, ref A, out AD);
//Vector3.Cross(ref AB, ref AC, out cross);
//float volume;
//Vector3.Dot(ref cross, ref AD, out volume);
////Volume is small compared to partial 'perimeter.'
//if (volume < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared() + AD.LengthSquared()))
// State = SimplexState.Point;
break;
default:
A = new Vector3();
B = new Vector3();
C = new Vector3();
D = new Vector3();
break;
}
}