///<summary>
/// Gets the closest point on the tetrahedron to the origin.
///</summary>
///<param name="point">Closest point.</param>
///<returns>Whether or not the simplex 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.
SimpleSimplex minimumSimplex = new SimpleSimplex();
point = new Vector3();
float minimumDistance = float.MaxValue;
SimpleSimplex candidate;
float candidateDistance;
Vector3 candidatePoint;
if (TryTetrahedronTriangle(ref A, ref C, ref D, ref B, out candidate, out candidatePoint))
{
point = candidatePoint;
minimumSimplex = candidate;
minimumDistance = candidatePoint.LengthSquared();
}
if (TryTetrahedronTriangle(ref C, ref B, ref D, ref A, out candidate, out candidatePoint) &&
(candidateDistance = candidatePoint.LengthSquared()) < minimumDistance)
{
point = candidatePoint;
minimumSimplex = candidate;
minimumDistance = candidateDistance;
}
if (TryTetrahedronTriangle(ref B, ref A, ref D, ref C, out candidate, out candidatePoint) &&
(candidateDistance = candidatePoint.LengthSquared()) < minimumDistance)
{
point = candidatePoint;
minimumSimplex = candidate;
minimumDistance = candidateDistance;
}
if (minimumDistance < float.MaxValue)
{
this = minimumSimplex;
return(false);
}
return(true);
}
///<summary>
/// Tests if the pair is intersecting.
///</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="localSeparatingAxis">Warmstartable separating axis used by the method to quickly early-out if possible. Updated to the latest separating axis after each run.</param>
///<returns>Whether or not the objects were intersecting.</returns>
public static bool AreShapesIntersecting(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB,
ref Vector3 localSeparatingAxis)
{
RigidTransform localtransformB;
MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB);
//Warm start the simplex.
var simplex = new SimpleSimplex();
Vector3 extremePoint;
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref localSeparatingAxis, ref localtransformB, out extremePoint);
simplex.AddNewSimplexPoint(ref extremePoint);
Vector3 closestPoint;
int count = 0;
while (count++ < MaximumGJKIterations)
{
if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex.
closestPoint.LengthSquared() <= simplex.GetErrorTolerance() * Toolbox.BigEpsilon)
{
//Intersecting, or so close to it that it will be difficult/expensive to figure out the separation.
return(true);
}
//Use the closest point as a direction.
Vector3 direction;
Vector3.Negate(ref closestPoint, out direction);
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref direction, ref localtransformB, out extremePoint);
//Since this is a boolean test, we don't need to refine the simplex if it becomes apparent that we cannot reach the origin.
//If the most extreme point at any given time does not go past the origin, then we can quit immediately.
float dot;
Vector3.Dot(ref extremePoint, ref closestPoint, out dot); //extreme point dotted against the direction pointing backwards towards the CSO.
if (dot > 0)
{
// If it's positive, that means that the direction pointing towards the origin produced an extreme point 'in front of' the origin, eliminating the possibility of any intersection.
localSeparatingAxis = direction;
return(false);
}
simplex.AddNewSimplexPoint(ref extremePoint);
}
return(false);
}
private static bool TryTetrahedronTriangle(ref Vector3 A, ref Vector3 B, ref Vector3 C,
ref Vector3 otherPoint, out SimpleSimplex 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 SimpleSimplex();
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 > 0)
{
//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 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;
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;
float V = AdotAC / (AdotAC - CdotAC);
Vector3.Multiply(ref ac, 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;
float V = d3d4 / (d3d4 + d6d5);
Vector3 bc;
Vector3.Subtract(ref C, ref B, out bc);
Vector3.Multiply(ref bc, V, out point);
Vector3.Add(ref point, ref B, out point);
return(true);
}
//On the face of the triangle.
float vc = AdotAB * BdotAC - BdotAB * AdotAC;
simplex.A = A;
simplex.B = B;
simplex.C = C;
simplex.State = SimplexState.Triangle;
float denom = 1f / (va + vb + vc);
float w = vc * denom;
float v = vb * 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);
return(true);
}
return(false);
}