///<summary>
/// Adds a new point to the simplex.
///</summary>
///<param name="point">Point to add.</param>
public void AddNewSimplexPoint(ref Vector3 point)
{
switch (State)
{
case SimplexState.Empty:
State = SimplexState.Point;
A = point;
break;
case SimplexState.Point:
State = SimplexState.Segment;
B = point;
break;
case SimplexState.Segment:
State = SimplexState.Triangle;
C = point;
break;
case SimplexState.Triangle:
State = SimplexState.Tetrahedron;
D = point;
break;
}
}
///<summary>
/// Gets the point on the segment closest to the origin.
///</summary>
///<param name="point">Point closest to origin.</param>
public void GetPointOnSegmentClosestToOrigin(out Vector3 point)
{
Vector3 segmentDisplacement;
Vector3.Subtract(ref B, ref A, out segmentDisplacement);
float dotA;
Vector3.Dot(ref segmentDisplacement, ref A, out dotA);
if (dotA > 0)
{
//'Behind' segment. This can't happen in a boolean version,
//but with closest points warmstarting or raycasts, it will.
State = SimplexState.Point;
U = 1;
point = A;
return;
}
float dotB;
Vector3.Dot(ref segmentDisplacement, ref B, out dotB);
if (dotB > 0)
{
//Inside segment.
U = dotB / segmentDisplacement.LengthSquared();
V = 1 - U;
Vector3.Multiply(ref segmentDisplacement, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//It should be possible in the warmstarted closest point calculation/raycasting to be outside B.
//It is not possible in a 'boolean' GJK, where it early outs as soon as a separating axis is found.
//Outside B.
//Remove current A; we're becoming a point.
A = B;
SimplexA.A = SimplexA.B;
SimplexB.A = SimplexB.B;
State = SimplexState.Point;
U = 1;
point = A;
}
///<summary>
/// Adds a new point to the simplex.
///</summary>
///<param name="point">Point to add.</param>
///<param name="hitLocation">Current ray hit location.</param>
///<param name="shiftedSimplex">Simplex shifted with the hit location.</param>
public void AddNewSimplexPoint(ref Vector3 point, ref Vector3 hitLocation, out RaySimplex shiftedSimplex)
{
shiftedSimplex = new RaySimplex();
switch (State)
{
case SimplexState.Empty:
State = SimplexState.Point;
A = point;
Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
break;
case SimplexState.Point:
State = SimplexState.Segment;
B = point;
Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
Vector3.Subtract(ref hitLocation, ref B, out shiftedSimplex.B);
break;
case SimplexState.Segment:
State = SimplexState.Triangle;
C = point;
Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
Vector3.Subtract(ref hitLocation, ref B, out shiftedSimplex.B);
Vector3.Subtract(ref hitLocation, ref C, out shiftedSimplex.C);
break;
case SimplexState.Triangle:
State = SimplexState.Tetrahedron;
D = point;
Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
Vector3.Subtract(ref hitLocation, ref B, out shiftedSimplex.B);
Vector3.Subtract(ref hitLocation, ref C, out shiftedSimplex.C);
Vector3.Subtract(ref hitLocation, ref D, out shiftedSimplex.D);
break;
}
shiftedSimplex.State = State;
}
private PairSimplex(ref RigidTransform localTransformB)
{
//This isn't a very good approach since the transform position is not guaranteed to be within the object. Would have to use the GetNewSimplexPoint to make it valid.
previousDistanceToClosest = Fix64.MaxValue;
errorTolerance = F64.C0;
LocalTransformB = localTransformB;
//Warm up the simplex using the centroids.
//Could also use the GetNewSimplexPoint if it had a Empty case, but test before choosing.
State = SimplexState.Point;
SimplexA = new ContributingShapeSimplex();
SimplexB = new ContributingShapeSimplex {
A = localTransformB.Position
};
//minkowski space support = shapeA-shapeB = 0,0,0 - positionB
Vector3.Negate(ref localTransformB.Position, out A);
B = new Vector3();
C = new Vector3();
D = new Vector3();
U = F64.C0;
V = F64.C0;
W = F64.C0;
}
///<summary>
/// Adds a new point to the simplex.
///</summary>
///<param name="point">Point to add.</param>
public void AddNewSimplexPoint(ref System.Numerics.Vector3 point)
{
switch (State)
{
case SimplexState.Empty:
State = SimplexState.Point;
A = point;
break;
case SimplexState.Point:
State = SimplexState.Segment;
B = point;
break;
case SimplexState.Segment:
State = SimplexState.Triangle;
C = point;
break;
case SimplexState.Triangle:
State = SimplexState.Tetrahedron;
D = point;
break;
}
}
///<summary>
/// Adds a new point to the simplex.
///</summary>
///<param name="point">Point to add.</param>
///<param name="hitLocation">Current ray hit location.</param>
///<param name="shiftedSimplex">Simplex shifted with the hit location.</param>
public void AddNewSimplexPoint(ref Vector3 point, ref Vector3 hitLocation, out RaySimplex shiftedSimplex)
{
shiftedSimplex = new RaySimplex();
switch (State)
{
case SimplexState.Empty:
State = SimplexState.Point;
A = point;
Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
break;
case SimplexState.Point:
State = SimplexState.Segment;
B = point;
Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
Vector3.Subtract(ref hitLocation, ref B, out shiftedSimplex.B);
break;
case SimplexState.Segment:
State = SimplexState.Triangle;
C = point;
Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
Vector3.Subtract(ref hitLocation, ref B, out shiftedSimplex.B);
Vector3.Subtract(ref hitLocation, ref C, out shiftedSimplex.C);
break;
case SimplexState.Triangle:
State = SimplexState.Tetrahedron;
D = point;
Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
Vector3.Subtract(ref hitLocation, ref B, out shiftedSimplex.B);
Vector3.Subtract(ref hitLocation, ref C, out shiftedSimplex.C);
Vector3.Subtract(ref hitLocation, ref D, out shiftedSimplex.D);
break;
}
shiftedSimplex.State = State;
}
///<summary>
/// Adds a new point to the simplex.
///</summary>
///<param name="shapeA">First shape in the pair.</param>
///<param name="shapeB">Second shape in the pair.</param>
///<param name="iterationCount">Current iteration count.</param>
///<param name="closestPoint">Current point on simplex closest to origin.</param>
///<returns>Whether or not GJK should exit due to a lack of progression.</returns>
public bool GetNewSimplexPoint(ConvexShape shapeA, ConvexShape shapeB, int iterationCount, ref Vector3 closestPoint)
{
Vector3 negativeDirection;
Vector3.Negate(ref closestPoint, out negativeDirection);
Vector3 sa, sb;
shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa);
shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb);
Vector3 S;
Vector3.Subtract(ref sa, ref sb, out S);
//If S is not further towards the origin along negativeDirection than closestPoint, then we're done.
Fix64 dotS;
Vector3.Dot(ref S, ref negativeDirection, out dotS); //-P * S
Fix64 distanceToClosest = closestPoint.LengthSquared();
Fix64 progression = dotS + distanceToClosest;
//It's likely that the system is oscillating between two or more states, usually because of a degenerate simplex.
//Rather than detect specific problem cases, this approach just lets it run and catches whatever falls through.
//During oscillation, one of the states is usually just BARELY outside of the numerical tolerance.
//After a bunch of iterations, the system lets it pick the 'better' one.
if (iterationCount > GJKToolbox.HighGJKIterations && distanceToClosest - previousDistanceToClosest < DistanceConvergenceEpsilon * errorTolerance)
{
return(true);
}
if (distanceToClosest < previousDistanceToClosest)
{
previousDistanceToClosest = distanceToClosest;
}
//If "A" is the new point always, then the switch statement can be removed
//in favor of just pushing three points up.
switch (State)
{
case SimplexState.Point:
if (progression <= (errorTolerance = MathHelper.Max(A.LengthSquared(), S.LengthSquared())) * ProgressionEpsilon)
{
return(true);
}
State = SimplexState.Segment;
B = S;
SimplexA.B = sa;
SimplexB.B = sb;
return(false);
case SimplexState.Segment:
if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), S.LengthSquared())) * ProgressionEpsilon)
{
return(true);
}
State = SimplexState.Triangle;
C = S;
SimplexA.C = sa;
SimplexB.C = sb;
return(false);
case SimplexState.Triangle:
if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), MathHelper.Max(C.LengthSquared(), S.LengthSquared()))) * ProgressionEpsilon)
{
return(true);
}
State = SimplexState.Tetrahedron;
D = S;
SimplexA.D = sa;
SimplexB.D = sb;
return(false);
}
return(false);
}
///<summary>
/// Gets the point on the triangle closest to the origin.
///</summary>
///<param name="point">Point closest to origin.</param>
public void GetPointOnTriangleClosestToOrigin(out Vector3 point)
{
Vector3 ab, ac;
Vector3.Subtract(ref B, ref A, out ab);
Vector3.Subtract(ref C, ref A, out ac);
//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->P, 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!
State = SimplexState.Point;
U = F64.C1;
point = A;
return;
}
//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!
State = SimplexState.Point;
A = B;
U = F64.C1;
SimplexA.A = SimplexA.B;
SimplexB.A = SimplexB.B;
point = B;
return;
}
//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
{
State = SimplexState.Segment;
V = AdotAB / (AdotAB - BdotAB);
U = F64.C1 - V;
Vector3.Multiply(ref ab, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//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!
State = SimplexState.Point;
A = C;
SimplexA.A = SimplexA.C;
SimplexB.A = SimplexB.C;
U = F64.C1;
point = A;
return;
}
//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
{
//Get rid of B. Compress C into B.
State = SimplexState.Segment;
B = C;
SimplexA.B = SimplexA.C;
SimplexB.B = SimplexB.C;
V = AdotAC / (AdotAC - CdotAC);
U = F64.C1 - V;
Vector3.Multiply(ref ac, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//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
{
//Throw away A. C->A.
//TODO: Does B->A, C->B work better?
State = SimplexState.Segment;
A = C;
SimplexA.A = SimplexA.C;
SimplexB.A = SimplexB.C;
U = d3d4 / (d3d4 + d6d5);
V = F64.C1 - U;
Vector3 bc;
Vector3.Subtract(ref C, ref B, out bc);
Vector3.Multiply(ref bc, U, out point);
Vector3.Add(ref point, ref B, out point);
return;
}
//On the face of the triangle.
Fix64 denom = F64.C1 / (va + vb + vc);
V = vb * denom;
W = vc * denom;
U = F64.C1 - V - W;
Vector3.Multiply(ref ab, V, out point);
Vector3 acw;
Vector3.Multiply(ref ac, W, out acw);
Vector3.Add(ref A, ref point, out point);
Vector3.Add(ref point, ref acw, out point);
}
///<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>
/// Adds a new point to the simplex.
///</summary>
///<param name="shapeA">First shape in the pair.</param>
///<param name="shapeB">Second shape in the pair.</param>
///<param name="iterationCount">Current iteration count.</param>
///<param name="closestPoint">Current point on simplex closest to origin.</param>
///<returns>Whether or not GJK should exit due to a lack of progression.</returns>
public bool GetNewSimplexPoint(ConvexShape shapeA, ConvexShape shapeB, int iterationCount, ref Vector3 closestPoint)
{
Vector3 negativeDirection;
Vector3.Negate(ref closestPoint, out negativeDirection);
Vector3 sa, sb;
shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa);
shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb);
Vector3 S;
Vector3.Subtract(ref sa, ref sb, out S);
//If S is not further towards the origin along negativeDirection than closestPoint, then we're done.
float dotS;
Vector3.Dot(ref S, ref negativeDirection, out dotS); //-P * S
float distanceToClosest = closestPoint.LengthSquared();
float progression = dotS + distanceToClosest;
//It's likely that the system is oscillating between two or more states, usually because of a degenerate simplex.
//Rather than detect specific problem cases, this approach just lets it run and catches whatever falls through.
//During oscillation, one of the states is usually just BARELY outside of the numerical tolerance.
//After a bunch of iterations, the system lets it pick the 'better' one.
if (iterationCount > GJKToolbox.HighGJKIterations && distanceToClosest - previousDistanceToClosest < DistanceConvergenceEpsilon * errorTolerance)
return true;
if (distanceToClosest < previousDistanceToClosest)
previousDistanceToClosest = distanceToClosest;
//If "A" is the new point always, then the switch statement can be removed
//in favor of just pushing three points up.
switch (State)
{
case SimplexState.Point:
if (progression <= (errorTolerance = MathHelper.Max(A.LengthSquared(), S.LengthSquared())) * ProgressionEpsilon)
return true;
State = SimplexState.Segment;
B = S;
SimplexA.B = sa;
SimplexB.B = sb;
return false;
case SimplexState.Segment:
if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), S.LengthSquared())) * ProgressionEpsilon)
return true;
State = SimplexState.Triangle;
C = S;
SimplexA.C = sa;
SimplexB.C = sb;
return false;
case SimplexState.Triangle:
if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), MathHelper.Max(C.LengthSquared(), S.LengthSquared()))) * ProgressionEpsilon)
return true;
State = SimplexState.Tetrahedron;
D = S;
SimplexA.D = sa;
SimplexB.D = sb;
return false;
}
return false;
}
///<summary>
/// Gets the point on the triangle closest to the origin.
///</summary>
///<param name="point">Point closest to origin.</param>
public void GetPointOnTriangleClosestToOrigin(out Vector3 point)
{
Vector3 ab, ac;
Vector3.Subtract(ref B, ref A, out ab);
Vector3.Subtract(ref C, ref A, out ac);
//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->P, 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!
State = SimplexState.Point;
U = 1;
point = A;
return;
}
//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!
State = SimplexState.Point;
A = B;
U = 1;
SimplexA.A = SimplexA.B;
SimplexB.A = SimplexB.B;
point = B;
return;
}
//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
{
State = SimplexState.Segment;
V = AdotAB / (AdotAB - BdotAB);
U = 1 - V;
Vector3.Multiply(ref ab, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//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!
State = SimplexState.Point;
A = C;
SimplexA.A = SimplexA.C;
SimplexB.A = SimplexB.C;
U = 1;
point = A;
return;
}
//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
{
//Get rid of B. Compress C into B.
State = SimplexState.Segment;
B = C;
SimplexA.B = SimplexA.C;
SimplexB.B = SimplexB.C;
V = AdotAC / (AdotAC - CdotAC);
U = 1 - V;
Vector3.Multiply(ref ac, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//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
{
//Throw away A. C->A.
//TODO: Does B->A, C->B work better?
State = SimplexState.Segment;
A = C;
SimplexA.A = SimplexA.C;
SimplexB.A = SimplexB.C;
U = d3d4 / (d3d4 + d6d5);
V = 1 - U;
Vector3 bc;
Vector3.Subtract(ref C, ref B, out bc);
Vector3.Multiply(ref bc, U, out point);
Vector3.Add(ref point, ref B, out point);
return;
}
//On the face of the triangle.
float denom = 1f / (va + vb + vc);
V = vb * denom;
W = vc * denom;
U = 1 - V - W;
Vector3.Multiply(ref ab, V, out point);
Vector3 acw;
Vector3.Multiply(ref ac, W, out acw);
Vector3.Add(ref A, ref point, out point);
Vector3.Add(ref point, ref acw, out point);
}
///<summary>
/// Gets the point on the segment closest to the origin.
///</summary>
///<param name="point">Point closest to origin.</param>
public void GetPointOnSegmentClosestToOrigin(out Vector3 point)
{
Vector3 segmentDisplacement;
Vector3.Subtract(ref B, ref A, out segmentDisplacement);
float dotA;
Vector3.Dot(ref segmentDisplacement, ref A, out dotA);
if (dotA > 0)
{
//'Behind' segment. This can't happen in a boolean version,
//but with closest points warmstarting or raycasts, it will.
State = SimplexState.Point;
U = 1;
point = A;
return;
}
float dotB;
Vector3.Dot(ref segmentDisplacement, ref B, out dotB);
if (dotB > 0)
{
//Inside segment.
U = dotB / segmentDisplacement.LengthSquared();
V = 1 - U;
Vector3.Multiply(ref segmentDisplacement, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//It should be possible in the warmstarted closest point calculation/raycasting to be outside B.
//It is not possible in a 'boolean' GJK, where it early outs as soon as a separating axis is found.
//Outside B.
//Remove current A; we're becoming a point.
A = B;
SimplexA.A = SimplexA.B;
SimplexB.A = SimplexB.B;
State = SimplexState.Point;
U = 1;
point = A;
}
///<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;
}
}
private PairSimplex(ref RigidTransform localTransformB)
{
//This isn't a very good approach since the transform position is not guaranteed to be within the object. Would have to use the GetNewSimplexPoint to make it valid.
previousDistanceToClosest = float.MaxValue;
errorTolerance = 0;
LocalTransformB = localTransformB;
//Warm up the simplex using the centroids.
//Could also use the GetNewSimplexPoint if it had a Empty case, but test before choosing.
State = SimplexState.Point;
SimplexA = new ContributingShapeSimplex();
SimplexB = new ContributingShapeSimplex {A = localTransformB.Position};
//minkowski space support = shapeA-shapeB = 0,0,0 - positionB
Vector3.Negate(ref localTransformB.Position, out A);
B = new Vector3();
C = new Vector3();
D = new Vector3();
U = 0;
V = 0;
W = 0;
}
///<summary>
/// Gets the closest point on the triangle to the origin.
///</summary>
///<param name="point">Closest point.</param>
public void GetPointOnTriangleClosestToOrigin(out Vector3 point)
{
Vector3 ab, ac;
Vector3.Subtract(ref B, ref A, out ab);
Vector3.Subtract(ref C, ref A, out ac);
//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...
//CAN'T BE IN A'S REGION.
//CAN'T BE IN B'S REGION.
//CAN'T BE IN AB'S REGION.
//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 d5, d6;
Vector3.Dot(ref ab, ref C, out d5);
Vector3.Dot(ref ac, ref C, out d6);
d5 = -d5;
d6 = -d6;
if (d6 >= 0f && d5 <= d6)
{
//It is C!
State = SimplexState.Point;
A = C;
point = A;
return;
}
//Check if it's outside AC.
float d1, d2;
Vector3.Dot(ref ab, ref A, out d1);
Vector3.Dot(ref ac, ref A, out d2);
d1 = -d1;
d2 = -d2;
float vb = d5 * d2 - d1 * d6;
if (vb <= 0f && d2 > 0f && d6 < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator
{
//Get rid of B. Compress C into B.
State = SimplexState.Segment;
B = C;
float V = d2 / (d2 - d6);
Vector3.Multiply(ref ac, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//Check if it's outside BC.
float d3, d4;
Vector3.Dot(ref ab, ref B, out d3);
Vector3.Dot(ref ac, ref B, out d4);
d3 = -d3;
d4 = -d4;
float va = d3 * d6 - d5 * d4;
float d3d4;
float d6d5;
if (va <= 0f && (d3d4 = d4 - d3) > 0f && (d6d5 = d5 - d6) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator
{
//Throw away A. C->A.
//TODO: Does B->A, C->B work better?
State = SimplexState.Segment;
A = C;
float U = d3d4 / (d3d4 + d6d5);
Vector3 bc;
Vector3.Subtract(ref C, ref B, out bc);
Vector3.Multiply(ref bc, U, out point);
Vector3.Add(ref point, ref B, out point);
return;
}
//On the face of the triangle.
float vc = d1 * d4 - d3 * d2;
float denom = 1f / (va + vb + vc);
float v = vb * denom;
float w = vc * denom;
Vector3.Multiply(ref ab, v, out point);
Vector3 acw;
Vector3.Multiply(ref ac, w, out acw);
Vector3.Add(ref A, ref point, out point);
Vector3.Add(ref point, ref acw, out point);
}
///<summary>
/// Gets the closest point on the triangle to the origin.
///</summary>
///<param name="point">Closest point.</param>
public void GetPointOnTriangleClosestToOrigin(out System.Numerics.Vector3 point)
{
System.Numerics.Vector3 ab, ac;
Vector3Ex.Subtract(ref B, ref A, out ab);
Vector3Ex.Subtract(ref C, ref A, out ac);
//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...
//CAN'T BE IN A'S REGION.
//CAN'T BE IN B'S REGION.
//CAN'T BE IN AB'S REGION.
//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 d5, d6;
Vector3Ex.Dot(ref ab, ref C, out d5);
Vector3Ex.Dot(ref ac, ref C, out d6);
d5 = -d5;
d6 = -d6;
if (d6 >= 0f && d5 <= d6)
{
//It is C!
State = SimplexState.Point;
A = C;
point = A;
return;
}
//Check if it's outside AC.
float d1, d2;
Vector3Ex.Dot(ref ab, ref A, out d1);
Vector3Ex.Dot(ref ac, ref A, out d2);
d1 = -d1;
d2 = -d2;
float vb = d5 * d2 - d1 * d6;
if (vb <= 0f && d2 > 0f && d6 < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator
{
//Get rid of B. Compress C into B.
State = SimplexState.Segment;
B = C;
float V = d2 / (d2 - d6);
Vector3Ex.Multiply(ref ac, V, out point);
Vector3Ex.Add(ref point, ref A, out point);
return;
}
//Check if it's outside BC.
float d3, d4;
Vector3Ex.Dot(ref ab, ref B, out d3);
Vector3Ex.Dot(ref ac, ref B, out d4);
d3 = -d3;
d4 = -d4;
float va = d3 * d6 - d5 * d4;
float d3d4;
float d6d5;
if (va <= 0f && (d3d4 = d4 - d3) > 0f && (d6d5 = d5 - d6) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator
{
//Throw away A. C->A.
//TODO: Does B->A, C->B work better?
State = SimplexState.Segment;
A = C;
float U = d3d4 / (d3d4 + d6d5);
System.Numerics.Vector3 bc;
Vector3Ex.Subtract(ref C, ref B, out bc);
Vector3Ex.Multiply(ref bc, U, out point);
Vector3Ex.Add(ref point, ref B, out point);
return;
}
//On the face of the triangle.
float vc = d1 * d4 - d3 * d2;
float denom = 1f / (va + vb + vc);
float v = vb * denom;
float w = vc * denom;
Vector3Ex.Multiply(ref ab, v, out point);
System.Numerics.Vector3 acw;
Vector3Ex.Multiply(ref ac, w, out acw);
Vector3Ex.Add(ref A, ref point, out point);
Vector3Ex.Add(ref point, ref acw, out point);
}
///<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:
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();
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();
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);
break;
default:
A = new Vector3();
B = new Vector3();
C = new Vector3();
D = new Vector3();
break;
}
}