public void Distance()
{
DistanceInput input = new DistanceInput();
input.ProxyA = new DistanceProxy(_polygonA, 0);
input.ProxyB = new DistanceProxy(_polygonB, 0);
input.TransformA = _transformA;
input.TransformB = _transformB;
input.UseRadii = true;
DistanceGJK.ComputeDistance(ref input, out _, out _);
}
public override void Update(GameSettings settings, GameTime gameTime)
{
base.Update(settings, gameTime);
DistanceInput input = new DistanceInput();
input.ProxyA = new DistanceProxy(_polygonA, 0);
input.ProxyB = new DistanceProxy(_polygonB, 0);
input.TransformA = _transformA;
input.TransformB = _transformB;
input.UseRadii = true;
SimplexCache cache = new SimplexCache();
cache.Count = 0;
DistanceOutput output;
DistanceGJK.ComputeDistance(ref input, out output, out cache);
DrawString($"distance = {output.Distance}");
DrawString($"iterations = {output.Iterations}");
DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View);
{
Color color = new Color(0.9f, 0.9f, 0.9f);
Vector2[] v = new Vector2[Settings.MaxPolygonVertices];
for (int i = 0; i < _polygonA.Vertices.Count; ++i)
{
v[i] = MathUtils.Mul(ref _transformA, _polygonA.Vertices[i]);
}
DebugView.DrawPolygon(v, _polygonA.Vertices.Count, color);
for (int i = 0; i < _polygonB.Vertices.Count; ++i)
{
v[i] = MathUtils.Mul(ref _transformB, _polygonB.Vertices[i]);
}
DebugView.DrawPolygon(v, _polygonB.Vertices.Count, color);
}
Vector2 x1 = output.PointA;
Vector2 x2 = output.PointB;
Color c1 = new Color(1.0f, 0.0f, 0.0f);
DebugView.DrawPoint(x1, 4.0f, c1);
Color c2 = new Color(1.0f, 1.0f, 0.0f);
DebugView.DrawPoint(x2, 4.0f, c2);
DebugView.EndCustomDraw();
}
/// <summary>
/// Test overlap between the two shapes.
/// </summary>
/// <param name="shapeA">The first shape.</param>
/// <param name="indexA">The index for the first shape.</param>
/// <param name="shapeB">The second shape.</param>
/// <param name="indexB">The index for the second shape.</param>
/// <param name="xfA">The transform for the first shape.</param>
/// <param name="xfB">The transform for the seconds shape.</param>
/// <returns></returns>
public static bool TestOverlap(Shape shapeA, int indexA, Shape shapeB, int indexB, ref Transform xfA, ref Transform xfB)
{
DistanceInput input = new DistanceInput();
input.ProxyA = new DistanceProxy(shapeA, indexA);
input.ProxyB = new DistanceProxy(shapeB, indexB);
input.TransformA = xfA;
input.TransformB = xfB;
input.UseRadii = true;
SimplexCache cache;
DistanceOutput output;
DistanceGJK.ComputeDistance(ref input, out output, out cache);
return(output.Distance < 10.0f * Settings.Epsilon);
}
/// <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)
{
if (Settings.EnableDiagnostics) //Velcro: We only gather diagnostics when enabled
{
++TOICalls;
}
output = new TOIOutput();
output.State = TOIOutputState.Unknown;
output.T = input.TMax;
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();
GGame.Math.Fix64 tMax = input.TMax;
GGame.Math.Fix64 totalRadius = input.ProxyA.Radius + input.ProxyB.Radius;
GGame.Math.Fix64 target = Math.Max((float)Settings.LinearSlop, (float)(totalRadius - 3.0f * Settings.LinearSlop));
GGame.Math.Fix64 tolerance = 0.25f * Settings.LinearSlop;
Debug.Assert(target > tolerance);
GGame.Math.Fix64 t1 = 0.0f;
const int k_maxIterations = 20;
int iter = 0;
// Prepare input for distance query.
DistanceInput distanceInput = new DistanceInput();
distanceInput.ProxyA = input.ProxyA;
distanceInput.ProxyB = input.ProxyB;
distanceInput.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 (;;)
{
Transform xfA, xfB;
sweepA.GetTransform(out xfA, t1);
sweepB.GetTransform(out xfB, t1);
// Get the distance between shapes. We can also use the results
// to get a separating axis.
distanceInput.TransformA = xfA;
distanceInput.TransformB = xfB;
DistanceOutput distanceOutput;
SimplexCache cache;
DistanceGJK.ComputeDistance(ref distanceInput, out distanceOutput, out 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;
GGame.Math.Fix64 t2 = tMax;
int pushBackIter = 0;
for (;;)
{
// Find the deepest point at t2. Store the witness point indices.
int indexA, indexB;
GGame.Math.Fix64 s2 = SeparationFunction.FindMinSeparation(out indexA, out 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.
GGame.Math.Fix64 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;
GGame.Math.Fix64 a1 = t1, a2 = t2;
for (;;)
{
// Use a mix of the secant rule and bisection.
GGame.Math.Fix64 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;
if (Settings.EnableDiagnostics) //Velcro: We only gather diagnostics when enabled
{
++TOIRootIters;
}
GGame.Math.Fix64 s = SeparationFunction.Evaluate(indexA, indexB, t, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, ref axis, ref localPoint, type);
if (Fix64.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;
}
}
if (Settings.EnableDiagnostics) //Velcro: We only gather diagnostics when enabled
{
TOIMaxRootIters = Math.Max(TOIMaxRootIters, rootIterCount);
}
++pushBackIter;
if (pushBackIter == Settings.MaxPolygonVertices)
{
break;
}
}
++iter;
if (Settings.EnableDiagnostics) //Velcro: We only gather diagnostics when enabled
{
++TOIIters;
}
if (done)
{
break;
}
if (iter == k_maxIterations)
{
// Root finder got stuck. Semi-victory.
output.State = TOIOutputState.Failed;
output.T = t1;
break;
}
}
if (Settings.EnableDiagnostics) //Velcro: We only gather diagnostics when enabled
{
TOIMaxIters = Math.Max(TOIMaxIters, iter);
}
}
public override void Update(GameSettings settings, GameTime gameTime)
{
base.Update(settings, gameTime);
ShapeCastInput input = new ShapeCastInput();
input.ProxyA = new DistanceProxy(_vAs, _radiusA);
input.ProxyB = new DistanceProxy(_vBs, _radiusB);
input.TransformA = _transformA;
input.TransformB = _transformB;
input.TranslationB = _translationB;
ShapeCastOutput output;
bool hit = DistanceGJK.ShapeCast(ref input, out output);
Transform transformB2;
transformB2.q = _transformB.q;
transformB2.p = _transformB.p + output.Lambda * input.TranslationB;
DistanceInput distanceInput = new DistanceInput();
distanceInput.ProxyA = new DistanceProxy(_vAs, _radiusA);
distanceInput.ProxyB = new DistanceProxy(_vBs, _radiusB);
distanceInput.TransformA = _transformA;
distanceInput.TransformB = transformB2;
distanceInput.UseRadii = false;
SimplexCache simplexCache;
DistanceOutput distanceOutput;
DistanceGJK.ComputeDistance(ref distanceInput, out distanceOutput, out simplexCache);
DrawString($"hit = {(hit ? "true" : "false")}, iters = {output.Iterations}, lambda = {output.Lambda}, distance = {distanceOutput.Distance}");
Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices];
for (int i = 0; i < _countA; ++i)
{
vertices[i] = MathUtils.Mul(ref _transformA, _vAs[i]);
}
DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View);
if (_countA == 1)
{
DebugView.DrawCircle(vertices[0], _radiusA, new Color(0.9f, 0.9f, 0.9f));
}
else
{
DebugView.DrawPolygon(vertices, _countA, new Color(0.9f, 0.9f, 0.9f));
}
for (int i = 0; i < _countB; ++i)
{
vertices[i] = MathUtils.Mul(ref _transformB, _vBs[i]);
}
if (_countB == 1)
{
DebugView.DrawCircle(vertices[0], _radiusB, new Color(0.5f, 0.9f, 0.5f));
}
else
{
DebugView.DrawPolygon(vertices, _countB, new Color(0.5f, 0.9f, 0.5f));
}
for (int i = 0; i < _countB; ++i)
{
vertices[i] = MathUtils.Mul(ref transformB2, _vBs[i]);
}
if (_countB == 1)
{
DebugView.DrawCircle(vertices[0], _radiusB, new Color(0.5f, 0.7f, 0.9f));
}
else
{
DebugView.DrawPolygon(vertices, _countB, new Color(0.5f, 0.7f, 0.9f));
}
if (hit)
{
Vector2 p1 = output.Point;
DebugView.DrawPoint(p1, 10.0f, new Color(0.9f, 0.3f, 0.3f));
Vector2 p2 = p1 + output.Normal;
DebugView.DrawSegment(p1, p2, new Color(0.9f, 0.3f, 0.3f));
}
DebugView.EndCustomDraw();
}
