/// <summary>
/// Evaluates the index a
/// </summary>
/// <param name="indexA">The index</param>
/// <param name="indexB">The index</param>
/// <param name="t">The </param>
/// <param name="proxyA">The proxy</param>
/// <param name="sweepA">The sweep</param>
/// <param name="proxyB">The proxy</param>
/// <param name="sweepB">The sweep</param>
/// <param name="axis">The axis</param>
/// <param name="localPoint">The local point</param>
/// <param name="type">The type</param>
/// <returns>The float</returns>
public static float Evaluate(int indexA, int indexB, float t, DistanceProxy proxyA, ref Sweep sweepA,
DistanceProxy proxyB, ref Sweep sweepB, ref Vector2 axis, ref Vector2 localPoint,
SeparationFunctionType type)
{
sweepA.GetTransform(out Transform xfA, t);
sweepB.GetTransform(out Transform xfB, t);
switch (type)
{
case SeparationFunctionType.Points:
{
Vector2 localPointA = proxyA.Vertices[indexA];
Vector2 localPointB = proxyB.Vertices[indexB];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
float separation = Vector2.Dot(pointB - pointA, axis);
return(separation);
}
case SeparationFunctionType.FaceA:
{
Vector2 normal = MathUtils.Mul(ref xfA.Q, axis);
Vector2 pointA = MathUtils.Mul(ref xfA, localPoint);
Vector2 localPointB = proxyB.Vertices[indexB];
Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
float separation = Vector2.Dot(pointB - pointA, normal);
return(separation);
}
case SeparationFunctionType.FaceB:
{
Vector2 normal = MathUtils.Mul(ref xfB.Q, axis);
Vector2 pointB = MathUtils.Mul(ref xfB, localPoint);
Vector2 localPointA = proxyA.Vertices[indexA];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
float separation = Vector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Debug.Assert(false);
return(0.0f);
}
}
/// <summary>
/// Initializes the cache
/// </summary>
/// <param name="cache">The cache</param>
/// <param name="proxyA">The proxy</param>
/// <param name="sweepA">The sweep</param>
/// <param name="proxyB">The proxy</param>
/// <param name="sweepB">The sweep</param>
/// <param name="t1">The </param>
/// <param name="axis">The axis</param>
/// <param name="localPoint">The local point</param>
/// <param name="type">The type</param>
public static void Initialize(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA,
DistanceProxy proxyB, ref Sweep sweepB, float t1, out Vector2 axis, out Vector2 localPoint,
out SeparationFunctionType type)
{
int count = cache.Count;
Debug.Assert(0 < count && count < 3);
sweepA.GetTransform(out Transform xfA, t1);
sweepB.GetTransform(out Transform xfB, t1);
if (count == 1)
{
localPoint = Vector2.Zero;
type = SeparationFunctionType.Points;
Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
Vector2 localPointB = proxyB.Vertices[cache.IndexB[0]];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
axis = pointB - pointA;
axis = Vector2.Normalize(axis);
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
type = SeparationFunctionType.FaceB;
Vector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]];
Vector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]];
Vector2 a = localPointB2 - localPointB1;
axis = new Vector2(a.Y, -a.X);
axis = Vector2.Normalize(axis);
Vector2 normal = MathUtils.Mul(ref xfB.Q, axis);
localPoint = 0.5f * (localPointB1 + localPointB2);
Vector2 pointB = MathUtils.Mul(ref xfB, localPoint);
Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
axis = -axis;
}
}
else
{
// Two points on A and one or two points on B.
type = SeparationFunctionType.FaceA;
Vector2 localPointA1 = proxyA.Vertices[cache.IndexA[0]];
Vector2 localPointA2 = proxyA.Vertices[cache.IndexA[1]];
Vector2 a = localPointA2 - localPointA1;
axis = new Vector2(a.Y, -a.X);
axis = Vector2.Normalize(axis);
Vector2 normal = MathUtils.Mul(ref xfA.Q, axis);
localPoint = 0.5f * (localPointA1 + localPointA2);
Vector2 pointA = MathUtils.Mul(ref xfA, localPoint);
Vector2 localPointB = proxyB.Vertices[cache.IndexB[0]];
Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
axis = -axis;
}
}
//Velcro note: the returned value that used to be here has been removed, as it was not used.
}
/// <summary>
/// Compute the upper bound on time before two shapes penetrate. Time is represented as a fraction between
/// [0,tMax]. This uses a swept separating axis and may miss some intermediate, non-tunneling collision. If you change
/// the
/// time interval, you should call this function again. Note: use Distance() to compute the contact point and normal at
/// the
/// time of impact.
/// </summary>
/// <param name="input">The input.</param>
/// <param name="output">The output.</param>
public static void CalculateTimeOfImpact(ref ToiInput input, out ToiOutput output)
{
++ToiCalls;
output = new ToiOutput
{
State = ToiOutputState.Unknown,
T = input.Max
};
Sweep sweepA = input.SweepA;
Sweep sweepB = input.SweepB;
// Large rotations can make the root finder fail, so we normalize the
// sweep angles.
sweepA.Normalize();
sweepB.Normalize();
float tMax = input.Max;
float totalRadius = input.ProxyA.Radius + input.ProxyB.Radius;
float target = Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop);
float tolerance = 0.25f * Settings.LinearSlop;
Debug.Assert(target > tolerance);
float t1 = 0.0f;
const int kMaxIterations = 20;
int iter = 0;
// Prepare input for distance query.
DistanceInput distanceInput = new DistanceInput
{
ProxyA = input.ProxyA,
ProxyB = input.ProxyB,
UseRadii = false
};
// The outer loop progressively attempts to compute new separating axes.
// This loop terminates when an axis is repeated (no progress is made).
for (;;)
{
sweepA.GetTransform(out Transform xfA, t1);
sweepB.GetTransform(out Transform xfB, t1);
// Get the distance between shapes. We can also use the results
// to get a separating axis.
distanceInput.TransformA = xfA;
distanceInput.TransformB = xfB;
DistanceGjk.ComputeDistance(ref distanceInput, out DistanceOutput distanceOutput,
out SimplexCache cache);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.Distance <= 0.0f)
{
// Failure!
output.State = ToiOutputState.Overlapped;
output.T = 0.0f;
break;
}
if (distanceOutput.Distance < target + tolerance)
{
// Victory!
output.State = ToiOutputState.Touching;
output.T = t1;
break;
}
SeparationFunction.Initialize(ref cache, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, t1,
out Vector2 axis, out Vector2 localPoint, out SeparationFunctionType type);
// Compute the TOI on the separating axis. We do this by successively
// resolving the deepest point. This loop is bounded by the number of vertices.
bool done = false;
float t2 = tMax;
int pushBackIter = 0;
for (;;)
{
// Find the deepest point at t2. Store the witness point indices.
float s2 = SeparationFunction.FindMinSeparation(out int indexA, out int indexB, t2, input.ProxyA,
ref sweepA, input.ProxyB, ref sweepB, ref axis, ref localPoint, type);
// Is the final configuration separated?
if (s2 > target + tolerance)
{
// Victory!
output.State = ToiOutputState.Seperated;
output.T = tMax;
done = true;
break;
}
// Has the separation reached tolerance?
if (s2 > target - tolerance)
{
// Advance the sweeps
t1 = t2;
break;
}
// Compute the initial separation of the witness points.
float s1 = SeparationFunction.Evaluate(indexA, indexB, t1, input.ProxyA, ref sweepA, input.ProxyB,
ref sweepB, ref axis, ref localPoint, type);
// Check for initial overlap. This might happen if the root finder
// runs out of iterations.
if (s1 < target - tolerance)
{
output.State = ToiOutputState.Failed;
output.T = t1;
done = true;
break;
}
// Check for touching
if (s1 <= target + tolerance)
{
// Victory! t1 should hold the TOI (could be 0.0).
output.State = ToiOutputState.Touching;
output.T = t1;
done = true;
break;
}
// Compute 1D root of: f(x) - target = 0
int rootIterCount = 0;
float a1 = t1, a2 = t2;
for (;;)
{
// Use a mix of the secant rule and bisection.
float t;
if ((rootIterCount & 1) != 0)
{
// Secant rule to improve convergence.
t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
}
else
{
// Bisection to guarantee progress.
t = 0.5f * (a1 + a2);
}
++rootIterCount;
++ToiRootIters;
float s = SeparationFunction.Evaluate(indexA, indexB, t, input.ProxyA, ref sweepA, input.ProxyB,
ref sweepB, ref axis, ref localPoint, type);
if (Math.Abs(s - target) < tolerance)
{
// t2 holds a tentative value for t1
t2 = t;
break;
}
// Ensure we continue to bracket the root.
if (s > target)
{
a1 = t;
s1 = s;
}
else
{
a2 = t;
s2 = s;
}
if (rootIterCount == 50)
{
break;
}
}
ToiMaxRootIters = Math.Max(ToiMaxRootIters, rootIterCount);
++pushBackIter;
if (pushBackIter == Settings.MaxPolygonVertices)
{
break;
}
}
++iter;
++ToiIters;
if (done)
{
break;
}
if (iter == kMaxIterations)
{
// Root finder got stuck. Semi-victory.
output.State = ToiOutputState.Failed;
output.T = t1;
break;
}
}
ToiMaxIters = Math.Max(ToiMaxIters, iter);
}
