///<summary>
/// Cleans up the pair tester.
///</summary>
public void CleanUp()
{
state = CollisionState.Separated;
previousState = CollisionState.Separated;
cachedSimplex = new CachedSimplex();
localSeparatingAxis = new Vector3();
collidableA = null;
collidableB = null;
}
///<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);
Quaternion.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();// new CachedSimplex(shapeA, shapeB, ref localtransformB);
simplex.State = SimplexState.Point;
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);
}
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>
/// 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>
/// 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;
}
///<summary>
/// Initializes the pair tester.
///</summary>
///<param name="shapeA">First shape in the pair.</param>
///<param name="shapeB">Second shape in the pair.</param>
public void Initialize(Collidable shapeA, Collidable shapeB)
{
collidableA = (ConvexCollidable)shapeA;
collidableB = (ConvexCollidable)shapeB;
cachedSimplex = new CachedSimplex { State = SimplexState.Point };// new CachedSimplex(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform);
}
private bool DoExternalNear(out TinyStructList<ContactData> contactList)
{
Vector3 closestA, closestB;
//Don't bother trying to do any clever caching. The continually transforming simplex makes it very rarely useful.
//TODO: Initialize the simplex of the GJK method using the 'true' center of the triangle.
//If left unmodified, the simplex that is used in GJK will just be a point at 0,0,0, which of course is at the origin.
//This causes an instant-out, always. Not good!
//By giving the contributing simplex the average centroid, it has a better guess.
Vector3 triangleCentroid;
Vector3.Add(ref triangle.vA, ref triangle.vB, out triangleCentroid);
Vector3.Add(ref triangleCentroid, ref triangle.vC, out triangleCentroid);
Vector3.Multiply(ref triangleCentroid, .33333333f, out triangleCentroid);
var initialSimplex = new CachedSimplex { State = SimplexState.Point, LocalSimplexB = { A = triangleCentroid } };
if (GJKToolbox.GetClosestPoints(convex, triangle, ref Toolbox.RigidIdentity, ref Toolbox.RigidIdentity, ref initialSimplex, out closestA, out closestB))
{
state = CollisionState.Deep;
return DoDeepContact(out contactList);
}
Vector3 displacement;
Vector3.Subtract(ref closestB, ref closestA, out displacement);
float distanceSquared = displacement.LengthSquared();
float margin = convex.collisionMargin + triangle.collisionMargin;
contactList = new TinyStructList<ContactData>();
if (distanceSquared < margin * margin)
{
//Try to generate a contact.
var contact = new ContactData();
//Determine if the normal points in the appropriate direction given the sidedness of the triangle.
if (triangle.sidedness != TriangleSidedness.DoubleSided)
{
Vector3 triangleNormal, ab, ac;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3.Cross(ref ab, ref ac, out triangleNormal);
float dot;
Vector3.Dot(ref triangleNormal, ref displacement, out dot);
if (triangle.sidedness == TriangleSidedness.Clockwise && dot > 0)
return false;
if (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)
return false;
}
//Displacement is from A to B. point = A + t * AB, where t = marginA / margin.
if (margin > Toolbox.Epsilon) //This can be zero! It would cause a NaN if unprotected.
Vector3.Multiply(ref displacement, convex.collisionMargin / margin, out contact.Position); //t * AB
else contact.Position = new Vector3();
Vector3.Add(ref closestA, ref contact.Position, out contact.Position); //A + t * AB.
contact.Normal = displacement;
float distance = (float)Math.Sqrt(distanceSquared);
Vector3.Divide(ref contact.Normal, distance, out contact.Normal);
contact.PenetrationDepth = margin - distance;
contactList.Add(ref contact);
TryToEscape(ref contact.Position);
return true;
}
//Too far to make a contact- move back to separation.
state = CollisionState.ExternalSeparated;
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 = Fix64.MaxValue;
errorTolerance = F64.C0;
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 = F64.C0;
V = F64.C0;
W = F64.C0;
switch (State)
{
case SimplexState.Point:
Quaternion.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.
//Fix64 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);
//Fix64 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;
}
}
///<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>
/// 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;
}
}
