| public GetClosestPoints ( Microsoft.Xna.Framework.Vector3 &closestPointA, Microsoft.Xna.Framework.Vector3 &closestPointB ) : void | ||
| closestPointA | Microsoft.Xna.Framework.Vector3 | Closest point on shape A. |
| closestPointB | Microsoft.Xna.Framework.Vector3 | Closest point on shape B. |
| return | void | |
public void GetClosestPoints(out Vector3 closestPointA, out Vector3 closestPointB)
{
//A * U + B * V + C * W
switch (State)
{
case SimplexState.Point:
closestPointA = SimplexA.A;
closestPointB = SimplexB.A;
return;
case SimplexState.Segment:
Vector3 temp;
Vector3.Multiply(ref SimplexA.A, U, out closestPointA);
Vector3.Multiply(ref SimplexA.B, V, out temp);
Vector3.Add(ref closestPointA, ref temp, out closestPointA);
Vector3.Multiply(ref SimplexB.A, U, out closestPointB);
Vector3.Multiply(ref SimplexB.B, V, out temp);
Vector3.Add(ref closestPointB, ref temp, out closestPointB);
return;
case SimplexState.Triangle:
Vector3.Multiply(ref SimplexA.A, U, out closestPointA);
Vector3.Multiply(ref SimplexA.B, V, out temp);
Vector3.Add(ref closestPointA, ref temp, out closestPointA);
Vector3.Multiply(ref SimplexA.C, W, out temp);
Vector3.Add(ref closestPointA, ref temp, out closestPointA);
Vector3.Multiply(ref SimplexB.A, U, out closestPointB);
Vector3.Multiply(ref SimplexB.B, V, out temp);
Vector3.Add(ref closestPointB, ref temp, out closestPointB);
Vector3.Multiply(ref SimplexB.C, W, out temp);
Vector3.Add(ref closestPointB, ref temp, out closestPointB);
return;
}
closestPointA = Toolbox.ZeroVector;
closestPointB = Toolbox.ZeroVector;
}
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);
}
