public void Sweep()
{
// From issue #447
Sweep sweep = new Sweep();
sweep.LocalCenter.SetZero();
sweep.C0.Set(-2.0f, 4.0f);
sweep.C.Set(3.0f, 8.0f);
sweep.A0 = 0.5f;
sweep.A = 5.0f;
sweep.Alpha0 = 0.0f;
Transform transform;
sweep.GetTransform(out transform, 0.0f);
transform.Position.X.ShouldBe(sweep.C0.X);
transform.Position.Y.ShouldBe(sweep.C0.Y);
transform.Rotation.Cos.ShouldBe((float)Math.Cos(sweep.A0));
transform.Rotation.Sin.ShouldBe((float)Math.Sin(sweep.A0));
sweep.GetTransform(out transform, 1.0f);
transform.Position.X.ShouldBe(sweep.C.X);
transform.Position.Y.ShouldBe(sweep.C.Y);
transform.Rotation.Cos.ShouldBe((float)Math.Cos(sweep.A));
transform.Rotation.Sin.ShouldBe((float)Math.Sin(sweep.A));
}
public override void Step(Settings settings)
{
settings.pause = 1;
base.Step(settings);
settings.pause = 0;
Sweep sweep1 = new Sweep();
sweep1.C0.Set(0.0f, 20.0f);
sweep1.A0 = 0.0f;
sweep1.C = sweep1.C0;
sweep1.A = sweep1.A0;
sweep1.T0 = 0.0f;
sweep1.LocalCenter = _body1.GetLocalCenter();
Sweep sweep2 = new Sweep();
sweep2.C0.Set(9.6363468f, 28.050615f);
sweep2.A0 = 1.6408679f;
sweep2.C = sweep2.C0 + new Vec2(-0.075121880f, 0.27358246f);
sweep2.A = sweep2.A0 - 10.434675f;
sweep2.T0 = 0.0f;
sweep2.LocalCenter = _body2.GetLocalCenter();
TOIInput input = new TOIInput
{
SweepA = sweep1,
SweepB = sweep2
};
float toi = Collision.TimeOfImpact(input, _shape1, _shape2);
OpenGLDebugDraw.DrawString(5, _textLine, "toi = " + toi.ToString());
_textLine += 15;
XForm xf2 = new XForm();
sweep2.GetTransform(out xf2, toi);
int vertexCount = _shape2.VertexCount;
Vec2[] vertices = new Vec2[Box2DNet.Common.Settings.MaxPolygonVertices];
Vec2[] localVertices = _shape2.Vertices;
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = Box2DNet.Common.Math.Mul(xf2, localVertices[i]);
}
_debugDraw.DrawPolygon(vertices, vertexCount, new Color(0.5f, 0.7f, 0.9f));
localVertices = _shape2.CoreVertices;
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = Box2DNet.Common.Math.Mul(xf2, localVertices[i]);
}
_debugDraw.DrawPolygon(vertices, vertexCount, new Color(0.5f, 0.7f, 0.9f));
}
public void SweepGetTransform()
{
// From issue https://github.com/erincatto/box2d/issues/447
Sweep sweep = new Sweep();
sweep.LocalCenter = Vector2.Zero;
sweep.C0 = new Vector2(-2.0f, 4.0f);
sweep.C = new Vector2(3.0f, 8.0f);
sweep.A0 = 0.5f;
sweep.A = 5.0f;
sweep.Alpha0 = 0.0f;
sweep.GetTransform(out Transform transform, 0.0f);
Assert.Equal(transform.p.X, sweep.C0.X);
Assert.Equal(transform.p.Y, sweep.C0.Y);
Assert.Equal(transform.q.c, (float)Math.Cos(sweep.A0));
Assert.Equal(transform.q.s, (float)Math.Sin(sweep.A0));
sweep.GetTransform(out transform, 1.0f);
Assert.Equal(transform.p.X, sweep.C.X);
Assert.Equal(transform.p.Y, sweep.C.Y);
Assert.Equal(transform.q.c, (float)Math.Cos(sweep.A));
Assert.Equal(transform.q.s, (float)Math.Sin(sweep.A));
}
public override void Step(TestSettings settings)
{
base.Step(settings);
Sweep sweepA = new Sweep();
sweepA.c0.Set(24.0f, -60.0f);
sweepA.a0 = 2.95f;
sweepA.c = sweepA.c0;
sweepA.a = sweepA.a0;
sweepA.localCenter.SetZero();
Sweep sweepB = new Sweep();
sweepB.c0.Set(53.474274f, -50.252514f);
sweepB.a0 = 513.36676f; // - 162.0f * (float)Math.PI;
sweepB.c.Set(54.595478f, -51.083473f);
sweepB.a = 513.62781f; // - 162.0f * (float)Math.PI;
sweepB.localCenter.SetZero();
//sweepB.a0 -= 300.0f * (float)Math.PI;
//sweepB.a -= 300.0f * (float)Math.PI;
TOIInput input = new TOIInput();
input.proxyA.Set(m_shapeA, 0);
input.proxyB.Set(m_shapeB, 0);
input.sweepA = sweepA;
input.sweepB = sweepB;
input.tMax = 1.0f;
TOIOutput output;
Utilities.TimeOfImpact(out output, input);
m_debugDraw.DrawString("toi = {0}", output.t);
m_debugDraw.DrawString("max toi iters = {0}, max root iters = {1}", Utilities._toiMaxIters, Utilities._toiMaxRootIters);
Vec2[] vertices = new Vec2[Settings._maxPolygonVertices];
Transform transformA;
sweepA.GetTransform(out transformA, 0.0f);
for (int i = 0; i < m_shapeA.m_count; ++i)
{
vertices[i] = Utilities.Mul(transformA, m_shapeA.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, m_shapeA.m_count, Color.FromArgb(225, 225, 225));
Transform transformB;
sweepB.GetTransform(out transformB, 0.0f);
//Vec2 localPoint(2.0f, -0.1f);
for (int i = 0; i < m_shapeB.m_count; ++i)
{
vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(128, 225, 128));
sweepB.GetTransform(out transformB, output.t);
for (int i = 0; i < m_shapeB.m_count; ++i)
{
vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(128, 175, 225));
sweepB.GetTransform(out transformB, 1.0f);
for (int i = 0; i < m_shapeB.m_count; ++i)
{
vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(225, 128, 128));
#if ZERO
for (float t = 0.0f; t < 1.0f; t += 0.1f)
{
sweepB.GetTransform(out transformB, t);
for (int i = 0; i < m_shapeB.m_count; ++i)
{
vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(225, 0.5f, 0.5f));
}
#endif
}
public static void Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, float t1)
{
_localPoint = Vector2.Zero;
_proxyA = proxyA;
_proxyB = proxyB;
int count = cache.Count;
Debug.Assert(0 < count && count < 3);
_sweepA = sweepA;
_sweepB = sweepB;
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t1);
_sweepB.GetTransform(out xfB, t1);
if (count == 1)
{
_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.Normalize();
}
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.Normalize();
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.Normalize();
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;
}
}
//FPE note: the returned value that used to be here has been removed, as it was not used.
}
public override void Step(Framework.Settings settings)
{
base.Step(settings);
Sweep sweepA = new Sweep();
sweepA.c0 = new Vector2(24.0f, -60.0f);
sweepA.a0 = 2.95f;
sweepA.c = sweepA.c0;
sweepA.a = sweepA.a0;
sweepA.localCenter = Vector2.Zero;
Sweep sweepB = new Sweep();
sweepB.c0 = new Vector2(53.474274f, -50.252514f);
sweepB.a0 = 513.36676f;
sweepB.c = new Vector2(54.595478f, -51.083473f);
sweepB.a = 513.62781f;
sweepB.localCenter = Vector2.Zero;
TOIInput input = new TOIInput();
input.proxyA.Set(_shapeA, 0);
input.proxyB.Set(_shapeB, 0);
input.sweepA = sweepA;
input.sweepB = sweepB;
input.tMax = 1.0f;
TOIOutput output;
Alt.Box2D.TimeOfImpact.CalculateTimeOfImpact(out output, ref input);
_debugDraw.DrawString(50, _textLine, "toi = {0:n}", output.t);
_textLine += 15;
_debugDraw.DrawString(50, _textLine, "max toi iters = {0:n}, max root iters = {1:n}", Alt.Box2D.TimeOfImpact.b2_toiMaxIters, Alt.Box2D.TimeOfImpact.b2_toiMaxRootIters);
_textLine += 15;
FixedArray8 <Vector2> vertices = new FixedArray8 <Vector2>();
Alt.Box2D.Transform transformA;
sweepA.GetTransform(out transformA, 0.0f);
for (int i = 0; i < _shapeA._vertexCount; ++i)
{
vertices[i] = MathUtils.Multiply(ref transformA, _shapeA._vertices[i]);
}
_debugDraw.DrawPolygon(ref vertices, _shapeA._vertexCount, new ColorR(0.9, 0.9, 0.9));
Alt.Box2D.Transform transformB;
sweepB.GetTransform(out transformB, 0.0f);
//NoNeed Vector2 localPoint = new Vector2(2.0f, -0.1f);
//NoNeed Vector2 rB = MathUtils.Multiply(ref transformB, localPoint) - sweepB.c0;
//NoNeed double wB = sweepB.a - sweepB.a0;
//NoNeed Vector2 vB = sweepB.c - sweepB.c0;
//Vector2 v = vB + MathUtils.Cross(wB, rB);
for (int i = 0; i < _shapeB._vertexCount; ++i)
{
vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]);
}
_debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new ColorR(0.5, 0.9, 0.5));
sweepB.GetTransform(out transformB, output.t);
for (int i = 0; i < _shapeB._vertexCount; ++i)
{
vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]);
}
_debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new ColorR(0.5f, 0.7f, 0.9f));
sweepB.GetTransform(out transformB, 1.0f);
for (int i = 0; i < _shapeB._vertexCount; ++i)
{
vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]);
}
_debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new ColorR(0.9, 0.5, 0.5));
}
public override void Update(GameSettings settings, GameTime gameTime)
{
base.Update(settings, gameTime);
Sweep sweepA = new Sweep();
sweepA.C0 = new Vector2(24.0f, -60.0f);
sweepA.A0 = 2.95f;
sweepA.C = sweepA.C0;
sweepA.A = sweepA.A0;
sweepA.LocalCenter = Vector2.Zero;
Sweep sweepB = new Sweep();
sweepB.C0 = new Vector2(53.474274f, -50.252514f);
sweepB.A0 = 513.36676f; // - 162.0f * b2_pi;
sweepB.C = new Vector2(54.595478f, -51.083473f);
sweepB.A = 513.62781f; // - 162.0f * b2_pi;
sweepB.LocalCenter = Vector2.Zero;
//sweepB.a0 -= 300.0f * b2_pi;
//sweepB.a -= 300.0f * b2_pi;
TOIInput input = new TOIInput();
input.ProxyA.Set(_shapeA, 0);
input.ProxyB.Set(_shapeB, 0);
input.SweepA = sweepA;
input.SweepB = sweepB;
input.TMax = 1.0f;
TOIOutput output;
TimeOfImpact.CalculateTimeOfImpact(out output, input);
DrawString("TOI = " + output.T);
DrawString(string.Format("Max TOI iters = {0:n}, Max root iters = {1:n}", TimeOfImpact.TOIMaxIters, TimeOfImpact.TOIMaxRootIters));
Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices];
DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View);
Transform transformA;
sweepA.GetTransform(out transformA, 0.0f);
for (int i = 0; i < _shapeA.Vertices.Count; ++i)
{
vertices[i] = MathUtils.Mul(ref transformA, _shapeA.Vertices[i]);
}
DebugView.DrawPolygon(vertices, _shapeA.Vertices.Count, new Color(0.9f, 0.9f, 0.9f));
Transform transformB;
sweepB.GetTransform(out transformB, 0.0f);
for (int i = 0; i < _shapeB.Vertices.Count; ++i)
{
vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]);
}
DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.5f, 0.9f, 0.5f));
sweepB.GetTransform(out transformB, output.T);
for (int i = 0; i < _shapeB.Vertices.Count; ++i)
{
vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]);
}
DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.5f, 0.7f, 0.9f));
sweepB.GetTransform(out transformB, 1.0f);
for (int i = 0; i < _shapeB.Vertices.Count; ++i)
{
vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]);
}
DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.9f, 0.5f, 0.5f));
DebugView.EndCustomDraw();
}
/// <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="output">The output.</param>
/// <param name="input">The input.</param>
public static void CalculateTimeOfImpact(out TOIOutput output, ref TOIInput input)
{
++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();
float tMax = input.TMax;
float totalRadius = input.ProxyA.Radius + input.ProxyB.Radius;
float target = Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop);
const float tolerance = 0.25f * Settings.LinearSlop;
Debug.Assert(target > tolerance);
float t1 = 0.0f;
const int k_maxIterations = 20;
int iter = 0;
// Prepare input for distance query.
SimplexCache cache;
DistanceInput 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;
Distance.ComputeDistance(out distanceOutput, out cache, ref distanceInput);
// 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 fcn = new SeparationFunction(ref cache, ref input.ProxyA, ref sweepA,
ref input.ProxyB, ref sweepB, t1);
// 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.
int indexA, indexB;
float s2 = fcn.FindMinSeparation(out indexA, out indexB, t2);
// 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 = fcn.Evaluate(indexA, indexB, t1);
// 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);
}
float s = fcn.Evaluate(indexA, indexB, t);
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;
}
++rootIterCount;
++TOIRootIters;
if (rootIterCount == 50)
{
break;
}
}
TOIMaxRootIters = Math.Max(TOIMaxRootIters, rootIterCount);
++pushBackIter;
if (pushBackIter == Settings.MaxPolygonVertices)
{
break;
}
}
++iter;
++TOIIters;
if (done)
{
break;
}
if (iter == k_maxIterations)
{
// Root finder got stuck. Semi-victory.
output.State = TOIOutputState.Failed;
output.T = t1;
break;
}
}
TOIMaxIters = Math.Max(TOIMaxIters, iter);
}
// CCD via the secant method.
/// <summary>
/// Compute the time when two shapes begin to touch or touch at a closer distance.
/// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance.
/// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be
/// smaller than sum of the shape radii.
/// Warning the sweeps must have the same time interval.
/// </summary>
/// <returns>
/// The fraction between [0,1] in which the shapes first touch.
/// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
/// </returns>
public static float TimeOfImpact(TOIInput input, Shape shapeA, Shape shapeB)
{
Sweep sweepA = input.SweepA;
Sweep sweepB = input.SweepB;
Box2DNetDebug.Assert(sweepA.T0 == sweepB.T0);
Box2DNetDebug.Assert(1.0f - sweepA.T0 > Common.Settings.FLT_EPSILON);
float radius = shapeA._radius + shapeB._radius;
float tolerance = input.Tolerance;
float alpha = 0.0f;
const int k_maxIterations = 1000; // TODO_ERIN b2Settings
int iter = 0;
float target = 0.0f;
// Prepare input for distance query.
SimplexCache cache = new SimplexCache {
Count = 0
};
DistanceInput distanceInput;
distanceInput.UseRadii = false;
for (; ;)
{
XForm xfA, xfB;
sweepA.GetTransform(out xfA, alpha);
sweepB.GetTransform(out xfB, alpha);
// Get the distance between shapes.
distanceInput.TransformA = xfA;
distanceInput.TransformB = xfB;
DistanceOutput distanceOutput;
Distance(out distanceOutput, ref cache, ref distanceInput, shapeA, shapeB);
if (distanceOutput.Distance <= 0.0f)
{
alpha = 1.0f;
break;
}
SeparationFunction fcn = new SeparationFunction();
unsafe
{
fcn.Initialize(&cache, shapeA, xfA, shapeB, xfB);
}
float separation = fcn.Evaluate(xfA, xfB);
if (separation <= 0.0f)
{
alpha = 1.0f;
break;
}
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement. We take advantage of the shape radii
// to create additional clearance.
target = separation > radius?Common.Math.Max(radius - tolerance, 0.75f *radius) : Common.Math.Max(separation - tolerance, 0.02f * radius);
}
if (separation - target < 0.5f * tolerance)
{
if (iter == 0)
{
alpha = 1.0f;
break;
}
break;
}
#if _FALSE
// Dump the curve seen by the root finder
{
const int32 N = 100;
float32 dx = 1.0f / N;
float32 xs[N + 1];
public static void Set(ref SimplexCache cache, ref DistanceProxy proxyA, ref Sweep sweepA, ref DistanceProxy proxyB, ref Sweep sweepB, float t1)
{
_localPoint = Vector2.Zero;
_proxyA = proxyA;
_proxyB = proxyB;
int count = cache.Count;
Debug.Assert(0 < count && count < 3);
_sweepA = sweepA;
_sweepB = sweepB;
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t1);
_sweepB.GetTransform(out xfB, t1);
if (count == 1)
{
_type = SeparationFunctionType.Points;
Vector2 localPointA = _proxyA.Vertices[cache.IndexA[0]];
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
_axis = pointB - pointA;
_axis.Normalize();
}
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.Normalize();
Vector2 normal = Complex.Multiply(ref _axis, ref xfB.q);
_localPoint = 0.5f * (localPointB1 + localPointB2);
Vector2 pointB = Transform.Multiply(ref _localPoint, ref xfB);
Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
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.Normalize();
Vector2 normal = Complex.Multiply(ref _axis, ref xfA.q);
_localPoint = 0.5f * (localPointA1 + localPointA2);
Vector2 pointA = Transform.Multiply(ref _localPoint, ref xfA);
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
}
// TODO_ERIN might not need to return the separation
public float Initialize(Distance.SimplexCache cache, Distance.DistanceProxy proxyA, Sweep sweepA, Distance.DistanceProxy proxyB, Sweep sweepB, float t1)
{
ProxyA = proxyA;
ProxyB = proxyB;
int count = cache.Count;
Debug.Assert(0 < count && count < 3);
SweepA = sweepA;
SweepB = sweepB;
SweepA.GetTransform(xfa, t1);
SweepB.GetTransform(xfb, t1);
// log.debug("initializing separation.\n" +
// "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n"
// "distance: "+proxyA.
if (count == 1)
{
Type = Type.Points;
/*
* Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB =
* m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2
* pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize();
*/
localPointA.Set(ProxyA.GetVertex(cache.IndexA[0]));
localPointB.Set(ProxyB.GetVertex(cache.IndexB[0]));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
Axis.Set(pointB).SubLocal(pointA);
float s = Axis.Normalize();
return(s);
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
Type = Type.FaceB;
localPointB1.Set(ProxyB.GetVertex(cache.IndexB[0]));
localPointB2.Set(ProxyB.GetVertex(cache.IndexB[1]));
temp.Set(localPointB2).SubLocal(localPointB1);
Vec2.CrossToOutUnsafe(temp, 1f, Axis);
Axis.Normalize();
Rot.MulToOutUnsafe(xfb.Q, Axis, normal);
LocalPoint.Set(localPointB1).AddLocal(localPointB2).MulLocal(.5f);
Transform.MulToOutUnsafe(xfb, LocalPoint, pointB);
localPointA.Set(proxyA.GetVertex(cache.IndexA[0]));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
temp.Set(pointA).SubLocal(pointB);
float s = Vec2.Dot(temp, normal);
if (s < 0.0f)
{
Axis.NegateLocal();
s = -s;
}
return(s);
}
else
{
// Two points on A and one or two points on B.
Type = Type.FaceA;
localPointA1.Set(ProxyA.GetVertex(cache.IndexA[0]));
localPointA2.Set(ProxyA.GetVertex(cache.IndexA[1]));
temp.Set(localPointA2).SubLocal(localPointA1);
Vec2.CrossToOutUnsafe(temp, 1.0f, Axis);
Axis.Normalize();
Rot.MulToOutUnsafe(xfa.Q, Axis, normal);
LocalPoint.Set(localPointA1).AddLocal(localPointA2).MulLocal(.5f);
Transform.MulToOutUnsafe(xfa, LocalPoint, pointA);
localPointB.Set(ProxyB.GetVertex(cache.IndexB[0]));
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
temp.Set(pointB).SubLocal(pointA);
float s = Vec2.Dot(temp, normal);
if (s < 0.0f)
{
Axis.NegateLocal();
s = -s;
}
return(s);
}
}
/// <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="output"></param>
/// <param name="input"></param>
public void GetTimeOfImpact(TOIOutput output, TOIInput input)
{
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
++ToiCalls;
output.State = TOIOutputState.Unknown;
output.T = input.tMax;
Distance.DistanceProxy proxyA = input.ProxyA;
Distance.DistanceProxy proxyB = input.ProxyB;
sweepA.Set(input.SweepA);
sweepB.Set(input.SweepB);
// Large rotations can make the root finder fail, so we normalize the
// sweep angles.
sweepA.Normalize();
sweepB.Normalize();
float tMax = input.tMax;
float totalRadius = proxyA.Radius + proxyB.Radius;
// djm: whats with all these constants?
float target = MathUtils.Max(Settings.LINEAR_SLOP, totalRadius - 3.0f * Settings.LINEAR_SLOP);
const float tolerance = 0.25f * Settings.LINEAR_SLOP;
Debug.Assert(target > tolerance);
float t1 = 0f;
int iter = 0;
cache.Count = 0;
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 (; ;)
{
sweepA.GetTransform(xfA, t1);
sweepB.GetTransform(xfB, t1);
// System.out.printf("sweepA: %f, %f, sweepB: %f, %f\n",
// sweepA.c.x, sweepA.c.y, sweepB.c.x, sweepB.c.y);
// Get the distance between shapes. We can also use the results
// to get a separating axis
distanceInput.TransformA = xfA;
distanceInput.TransformB = xfB;
pool.GetDistance().GetDistance(distanceOutput, cache, distanceInput);
// System.out.printf("Dist: %f at points %f, %f and %f, %f. %d iterations\n",
// distanceOutput.distance, distanceOutput.pointA.x, distanceOutput.pointA.y,
// distanceOutput.pointB.x, distanceOutput.pointB.y,
// distanceOutput.iterations);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.Distance <= 0f)
{
// System.out.println("failure, overlapped");
// Failure!
output.State = TOIOutputState.Overlapped;
output.T = 0f;
break;
}
if (distanceOutput.Distance < target + tolerance)
{
// System.out.println("touching, victory");
// Victory!
output.State = TOIOutputState.Touching;
output.T = t1;
break;
}
// Initialize the separating axis.
fcn.Initialize(cache, proxyA, sweepA, proxyB, sweepB, t1);
// 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 = fcn.FindMinSeparation(indexes, t2);
// System.out.printf("s2: %f\n", s2);
// Is the final configuration separated?
if (s2 > target + tolerance)
{
// Victory!
// System.out.println("separated");
output.State = TOIOutputState.Separated;
output.T = tMax;
done = true;
break;
}
// Has the separation reached tolerance?
if (s2 > target - tolerance)
{
// System.out.println("advancing");
// Advance the sweeps
t1 = t2;
break;
}
// Compute the initial separation of the witness points.
float s1 = fcn.Evaluate(indexes[0], indexes[1], t1);
// Check for initial overlap. This might happen if the root finder
// runs out of iterations.
// System.out.printf("s1: %f, target: %f, tolerance: %f\n", s1, target,
// tolerance);
if (s1 < target - tolerance)
{
// System.out.println("failed?");
output.State = TOIOutputState.Failed;
output.T = t1;
done = true;
break;
}
// Check for touching
if (s1 <= target + tolerance)
{
// System.out.println("touching?");
// 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) == 1)
{
// Secant rule to improve convergence.
t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
}
else
{
// Bisection to guarantee progress.
t = 0.5f * (a1 + a2);
}
float s = fcn.Evaluate(indexes[0], indexes[1], t);
if (MathUtils.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;
}
++rootIterCount;
++ToiRootIters;
// djm: whats with this? put in settings?
if (rootIterCount == 50)
{
break;
}
}
ToiMaxRootIters = MathUtils.Max(ToiMaxRootIters, rootIterCount);
++pushBackIter;
if (pushBackIter == Settings.MAX_POLYGON_VERTICES)
{
break;
}
}
++iter;
++ToiIters;
if (done)
{
// System.out.println("done");
break;
}
if (iter == MAX_ITERATIONS)
{
// System.out.println("failed, root finder stuck");
// Root finder got stuck. Semi-victory.
output.State = TOIOutputState.Failed;
output.T = t1;
break;
}
}
// System.out.printf("final sweeps: %f, %f, %f; %f, %f, %f", input.s)
ToiMaxIters = MathUtils.Max(ToiMaxIters, iter);
}
public static float FindMinSeparation(out int indexA, out int indexB, float t)
{
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t);
_sweepB.GetTransform(out xfB, t);
switch (_type)
{
case SeparationFunctionType.Points:
{
var axisA = MathUtils.MulT(ref xfA.Q, _axis);
var axisB = MathUtils.MulT(ref xfB.Q, -_axis);
indexA = _proxyA.GetSupport(axisA);
indexB = _proxyB.GetSupport(axisB);
var localPointA = _proxyA.vertices[indexA];
var localPointB = _proxyB.vertices[indexB];
var pointA = MathUtils.Mul(ref xfA, localPointA);
var pointB = MathUtils.Mul(ref xfB, localPointB);
var separation = Vector2.Dot(pointB - pointA, _axis);
return(separation);
}
case SeparationFunctionType.FaceA:
{
var normal = MathUtils.Mul(ref xfA.Q, _axis);
var pointA = MathUtils.Mul(ref xfA, _localPoint);
var axisB = MathUtils.MulT(ref xfB.Q, -normal);
indexA = -1;
indexB = _proxyB.GetSupport(axisB);
var localPointB = _proxyB.vertices[indexB];
var pointB = MathUtils.Mul(ref xfB, localPointB);
var separation = Vector2.Dot(pointB - pointA, normal);
return(separation);
}
case SeparationFunctionType.FaceB:
{
var normal = MathUtils.Mul(ref xfB.Q, _axis);
var pointB = MathUtils.Mul(ref xfB, _localPoint);
var axisA = MathUtils.MulT(ref xfA.Q, -normal);
indexB = -1;
indexA = _proxyA.GetSupport(axisA);
var localPointA = _proxyA.vertices[indexA];
var pointA = MathUtils.Mul(ref xfA, localPointA);
var separation = Vector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Debug.Assert(false);
indexA = -1;
indexB = -1;
return(0.0f);
}
}
public override void Step(Framework.Settings settings)
{
base.Step(settings);
Sweep sweepA = new Sweep();
sweepA.c0 = Vector2.Zero;
sweepA.a0 = 0.0f;
sweepA.c = sweepA.c0;
sweepA.a = sweepA.a0;
sweepA.localCenter = Vector2.Zero;
Sweep sweepB = new Sweep();
sweepB.c0 = new Vector2(-0.20382018f, 2.1368704f);
sweepB.a0 = -3.1664171f;
sweepB.c = new Vector2(-0.26699525f, 2.3552670f);
sweepB.a = -3.3926492f;
sweepB.localCenter = Vector2.Zero;
TOIInput input = new TOIInput();
input.proxyA.Set(_shapeA);
input.proxyB.Set(_shapeB);
input.sweepA = sweepA;
input.sweepB = sweepB;
input.tMax = 1.0f;
TOIOutput output;
XNA.TimeOfImpact.CalculateTimeOfImpact(out output, ref input);
_debugDraw.DrawString(50, _textLine, "toi = {0:n}", output.t);
_textLine += 15;
_debugDraw.DrawString(50, _textLine, "max toi iters = {0:n}, max root iters = {1:n}", XNA.TimeOfImpact.b2_toiMaxIters, XNA.TimeOfImpact.b2_toiMaxRootIters);
_textLine += 15;
FixedArray8 <Vector2> vertices = new FixedArray8 <Vector2>();
Transform transformA;
sweepA.GetTransform(out transformA, 0.0f);
for (int i = 0; i < _shapeA._vertexCount; ++i)
{
vertices[i] = MathUtils.Multiply(ref transformA, _shapeA._vertices[i]);
}
_debugDraw.DrawPolygon(ref vertices, _shapeA._vertexCount, new Color(0.9f, 0.9f, 0.9f));
Transform transformB;
sweepB.GetTransform(out transformB, 0.0f);
Vector2 localPoint = new Vector2(2.0f, -0.1f);
Vector2 rB = MathUtils.Multiply(ref transformB, localPoint) - sweepB.c0;
float wB = sweepB.a - sweepB.a0;
Vector2 vB = sweepB.c - sweepB.c0;
Vector2 v = vB + MathUtils.Cross(wB, rB);
for (int i = 0; i < _shapeB._vertexCount; ++i)
{
vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]);
}
_debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new Color(0.5f, 0.9f, 0.5f));
sweepB.GetTransform(out transformB, output.t);
for (int i = 0; i < _shapeB._vertexCount; ++i)
{
vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]);
}
_debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new Color(0.5f, 0.7f, 0.9f));
sweepB.GetTransform(out transformB, 1.0f);
for (int i = 0; i < _shapeB._vertexCount; ++i)
{
vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]);
}
_debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new Color(0.9f, 0.5f, 0.5f));
}
public override void Update(GameSettings settings, GameTime gameTime)
{
base.Update(settings, gameTime);
Sweep sweepA = new Sweep();
sweepA.C0 = new Vector2(24.0f, -60.0f);
sweepA.A0 = 2.95f;
sweepA.C = sweepA.C0;
sweepA.A = sweepA.A0;
sweepA.LocalCenter = Vector2.Zero;
Sweep sweepB = new Sweep();
sweepB.C0 = new Vector2(53.474274f, -50.252514f);
sweepB.A0 = 513.36676f; // - 162.0f * MathConstants.Pi;
sweepB.C = new Vector2(54.595478f, -51.083473f);
sweepB.A = 513.62781f; // - 162.0f * MathConstants.Pi;
sweepB.LocalCenter = Vector2.Zero;
//sweepB.a0 -= 300.0f * MathConstants.Pi;
//sweepB.a -= 300.0f * MathConstants.Pi;
TOIInput input = new TOIInput();
input.ProxyA = new DistanceProxy(_shapeA, 0);
input.ProxyB = new DistanceProxy(_shapeB, 0);
input.SweepA = sweepA;
input.SweepB = sweepB;
input.TMax = 1.0f;
TOIOutput output;
TimeOfImpact.CalculateTimeOfImpact(ref input, out output);
DrawString("toi = " + output.T);
DrawString($"max toi iters = {TimeOfImpact.TOIMaxIters}, max root iters = {TimeOfImpact.TOIMaxRootIters}");
Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices];
DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View);
Transform transformA;
sweepA.GetTransform(out transformA, 0.0f);
for (int i = 0; i < _shapeA.Vertices.Count; ++i)
{
vertices[i] = MathUtils.Mul(ref transformA, _shapeA.Vertices[i]);
}
DebugView.DrawPolygon(vertices, _shapeA.Vertices.Count, new Color(0.9f, 0.9f, 0.9f));
Transform transformB;
sweepB.GetTransform(out transformB, 0.0f);
//b2Vec2 localPoint(2.0f, -0.1f);
for (int i = 0; i < _shapeB.Vertices.Count; ++i)
{
vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]);
}
DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.5f, 0.9f, 0.5f));
sweepB.GetTransform(out transformB, output.T);
for (int i = 0; i < _shapeB.Vertices.Count; ++i)
{
vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]);
}
DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.5f, 0.7f, 0.9f));
sweepB.GetTransform(out transformB, 1.0f);
for (int i = 0; i < _shapeB.Vertices.Count; ++i)
{
vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]);
}
DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.9f, 0.5f, 0.5f));
DebugView.EndCustomDraw();
#if false
for (float t = 0.0f; t < 1.0f; t += 0.1f)
{
sweepB.GetTransform(&transformB, t);
for (int i = 0; i < _shapeB.m_count; ++i)
{
vertices[i] = MathUtils.Mul(transformB, _shapeB.m_vertices[i]);
}
DebugView.DrawPolygon(vertices, _shapeB.m_count, b2Color(0.9f, 0.5f, 0.5f));
}
#endif
}
protected override void OnRender()
{
var sweepA = new Sweep();
sweepA.C0.Set(24.0f, -60.0f);
sweepA.A0 = 2.95f;
sweepA.C = sweepA.C0;
sweepA.A = sweepA.A0;
sweepA.LocalCenter.SetZero();
var sweepB = new Sweep();
sweepB.C0.Set(53.474274f, -50.252514f);
sweepB.A0 = 513.36676f; // - 162.0f * _pi;
sweepB.C.Set(54.595478f, -51.083473f);
sweepB.A = 513.62781f; // - 162.0f * _pi;
sweepB.LocalCenter.SetZero();
//sweepB.A0 -= 300.0f * _pi;
//sweepB.A -= 300.0f * _pi;
var input = new ToiInput();
input.ProxyA.Set(_shapeA, 0);
input.ProxyB.Set(_shapeB, 0);
input.SweepA = sweepA;
input.SweepB = sweepB;
input.Tmax = 1.0f;
TimeOfImpact.ComputeTimeOfImpact(out var output, input, World.ToiProfile, World.GJkProfile);
DrawString($"toi = {output.Time}");
DrawString($"max toi iters = {ToiMaxIters}, max root iters = {ToiMaxRootIters}");
var vertices = new Vector2[Settings.MaxPolygonVertices];
sweepA.GetTransform(out var transformA, 0.0f);
for (var i = 0; i < _shapeA.Count; ++i)
{
vertices[i] = MathUtils.Mul(transformA, _shapeA.Vertices[i]);
}
Drawer.DrawPolygon(vertices, _shapeA.Count, Color.FromArgb(230, 230, 230));
sweepB.GetTransform(out var transformB, 0.0f);
//Vec2 localPoint(2.0f, -0.1f);
for (var i = 0; i < _shapeB.Count; ++i)
{
vertices[i] = MathUtils.Mul(transformB, _shapeB.Vertices[i]);
}
Drawer.DrawPolygon(vertices, _shapeB.Count, Color.FromArgb(127, 230, 127));
sweepB.GetTransform(out transformB, output.Time);
for (var i = 0; i < _shapeB.Count; ++i)
{
vertices[i] = MathUtils.Mul(transformB, _shapeB.Vertices[i]);
}
Drawer.DrawPolygon(vertices, _shapeB.Count, Color.FromArgb(127, 178, 230));
sweepB.GetTransform(out transformB, 1.0f);
for (var i = 0; i < _shapeB.Count; ++i)
{
vertices[i] = MathUtils.Mul(transformB, _shapeB.Vertices[i]);
}
Drawer.DrawPolygon(vertices, _shapeB.Count, Color.FromArgb(230, 127, 127));
}
public static float FindMinSeparation(out int indexA, out int indexB, float t)
{
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t);
_sweepB.GetTransform(out xfB, t);
switch (_type)
{
case SeparationFunctionType.Points:
{
Vector2 axisA = Complex.Divide(ref _axis, ref xfA.q);
Vector2 axisB = -Complex.Divide(ref _axis, ref xfB.q);
indexA = _proxyA.GetSupport(axisA);
indexB = _proxyB.GetSupport(axisB);
Vector2 localPointA = _proxyA.Vertices[indexA];
Vector2 localPointB = _proxyB.Vertices[indexB];
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
float separation = Vector2.Dot(pointB - pointA, _axis);
return(separation);
}
case SeparationFunctionType.FaceA:
{
Vector2 normal = Complex.Multiply(ref _axis, ref xfA.q);
Vector2 pointA = Transform.Multiply(ref _localPoint, ref xfA);
Vector2 axisB = -Complex.Divide(ref normal, ref xfB.q);
indexA = -1;
indexB = _proxyB.GetSupport(axisB);
Vector2 localPointB = _proxyB.Vertices[indexB];
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
float separation = Vector2.Dot(pointB - pointA, normal);
return(separation);
}
case SeparationFunctionType.FaceB:
{
Vector2 normal = Complex.Multiply(ref _axis, ref xfB.q);
Vector2 pointB = Transform.Multiply(ref _localPoint, ref xfB);
Vector2 axisA = -Complex.Divide(ref normal, ref xfA.q);
indexB = -1;
indexA = _proxyA.GetSupport(axisA);
Vector2 localPointA = _proxyA.Vertices[indexA];
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
float separation = Vector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Debug.Assert(false);
indexA = -1;
indexB = -1;
return(0.0f);
}
}
// float FindMinSeparation(int* indexA, int* indexB, float t) const
public float FindMinSeparation(int[] indexes, float t)
{
SweepA.GetTransform(xfa, t);
SweepB.GetTransform(xfb, t);
switch (Type)
{
case Type.Points:
{
Rot.MulTransUnsafe(xfa.Q, Axis, axisA);
Rot.MulTransUnsafe(xfb.Q, Axis.NegateLocal(), axisB);
Axis.NegateLocal();
indexes[0] = ProxyA.GetSupport(axisA);
indexes[1] = ProxyB.GetSupport(axisB);
localPointA.Set(ProxyA.GetVertex(indexes[0]));
localPointB.Set(ProxyB.GetVertex(indexes[1]));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
float separation = Vec2.Dot(pointB.SubLocal(pointA), Axis);
return(separation);
}
case Type.FaceA:
{
Rot.MulToOutUnsafe(xfa.Q, Axis, normal);
Transform.MulToOutUnsafe(xfa, LocalPoint, pointA);
Rot.MulTransUnsafe(xfb.Q, normal.NegateLocal(), axisB);
normal.NegateLocal();
indexes[0] = -1;
indexes[1] = ProxyB.GetSupport(axisB);
localPointB.Set(ProxyB.GetVertex(indexes[1]));
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
float separation = Vec2.Dot(pointB.SubLocal(pointA), normal);
return(separation);
}
case Type.FaceB:
{
Rot.MulToOutUnsafe(xfb.Q, Axis, normal);
Transform.MulToOutUnsafe(xfb, LocalPoint, pointB);
Rot.MulTransUnsafe(xfa.Q, normal.NegateLocal(), axisA);
normal.NegateLocal();
indexes[1] = -1;
indexes[0] = ProxyA.GetSupport(axisA);
localPointA.Set(ProxyA.GetVertex(indexes[0]));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
float separation = Vec2.Dot(pointA.SubLocal(pointB), normal);
return(separation);
}
default:
Debug.Assert(false);
indexes[0] = -1;
indexes[1] = -1;
return(0f);
}
}
public override void Step(Framework.Settings settings)
{
base.Step(settings);
Sweep sweepA = new Sweep();
sweepA.c0 = new Vector2(24.0f, -60.0f);
sweepA.a0 = 2.95f;
sweepA.c = sweepA.c0;
sweepA.a = sweepA.a0;
sweepA.localCenter = Vector2.Zero;
Sweep sweepB = new Sweep();
sweepB.c0 = new Vector2(53.474274f, -50.252514f);
sweepB.a0 = 513.36676f;
sweepB.c = new Vector2(54.595478f, -51.083473f);
sweepB.a = 513.62781f;
sweepB.localCenter = Vector2.Zero;
TOIInput input = new TOIInput();
input.proxyA.Set(_shapeA, 0);
input.proxyB.Set(_shapeB, 0);
input.sweepA = sweepA;
input.sweepB = sweepB;
input.tMax = 1.0f;
TOIOutput output;
XNA.TimeOfImpact.CalculateTimeOfImpact(out output, ref input);
_debugDraw.DrawString(50, _textLine, "toi = {0:n}", output.t);
_textLine += 15;
_debugDraw.DrawString(50, _textLine, "max toi iters = {0:n}, max root iters = {1:n}", XNA.TimeOfImpact.b2_toiMaxIters, XNA.TimeOfImpact.b2_toiMaxRootIters);
_textLine += 15;
FixedArray8<Vector2> vertices = new FixedArray8<Vector2>();
Transform transformA;
sweepA.GetTransform(out transformA, 0.0f);
for (int i = 0; i < _shapeA._vertexCount; ++i)
{
vertices[i] = MathUtils.Multiply(ref transformA, _shapeA._vertices[i]);
}
_debugDraw.DrawPolygon(ref vertices, _shapeA._vertexCount, new Color(0.9f, 0.9f, 0.9f));
Transform transformB;
sweepB.GetTransform(out transformB, 0.0f);
Vector2 localPoint = new Vector2(2.0f, -0.1f);
Vector2 rB = MathUtils.Multiply(ref transformB, localPoint) - sweepB.c0;
float wB = sweepB.a - sweepB.a0;
Vector2 vB = sweepB.c - sweepB.c0;
Vector2 v = vB + MathUtils.Cross(wB, rB);
for (int i = 0; i < _shapeB._vertexCount; ++i)
{
vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]);
}
_debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new Color(0.5f, 0.9f, 0.5f));
sweepB.GetTransform(out transformB, output.t);
for (int i = 0; i < _shapeB._vertexCount; ++i)
{
vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]);
}
_debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new Color(0.5f, 0.7f, 0.9f));
sweepB.GetTransform(out transformB, 1.0f);
for (int i = 0; i < _shapeB._vertexCount; ++i)
{
vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]);
}
_debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new Color(0.9f, 0.5f, 0.5f));
}
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
public static bool TimeOfImpact(
DistanceProxy proxyA,
DistanceProxy proxyB,
Sweep sweepA,
Sweep sweepB,
float tMax,
out float t)
{
// Large rotations can make the root finder fail, so we normalize the
// sweep angles.
sweepA.Normalize();
sweepB.Normalize();
var totalRadius = proxyA.Radius + proxyB.Radius;
var target = System.Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop);
const float tolerance = 0.25f * Settings.LinearSlop;
System.Diagnostics.Debug.Assert(target > tolerance);
// Prepare input for distance query.
var cache = new SimplexCache {
Count = 0
};
// The outer loop progressively attempts to compute new separating axes.
// This loop terminates when an axis is repeated (no progress is made).
var t1 = 0.0f;
for (var iter = 0; iter < 20; ++iter)
{
WorldTransform 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.
var distance = Distance(ref cache, proxyA, xfA, proxyB, xfB);
// If the shapes are overlapped, we give up on continuous collision.
if (distance <= 0.0f)
{
// Failure!
t = 0.0f;
return(false);
}
if (distance < target + tolerance)
{
// Victory!
t = t1;
return(true);
}
// Initialize the separating axis.
SeparationFunction fcn;
SeparationFunction.Initialize(out fcn, cache, proxyA, proxyB, sweepA, sweepB, t1);
// 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.
var t2 = tMax;
for (var pushBackIter = 0; pushBackIter < Settings.MaxPolygonVertices; ++pushBackIter)
{
// Find the deepest point at t2. Store the witness point indices.
int indexA, indexB;
var s2 = fcn.FindMinSeparation(out indexA, out indexB, t2);
// Is the final configuration separated?
if (s2 > target + tolerance)
{
// Victory!
t = tMax;
return(false);
}
// Has the separation reached tolerance?
if (s2 > target - tolerance)
{
// Advance the sweeps
t1 = t2;
break;
}
// Compute the initial separation of the witness points.
var s1 = fcn.Evaluate(indexA, indexB, t1);
// Check for initial overlap. This might happen if the root finder
// runs out of iterations.
if (s1 < target - tolerance)
{
t = t1;
return(false);
}
// Check for touching
if (s1 <= target + tolerance)
{
// Victory! t1 should hold the TOI (could be 0.0).
t = t1;
return(true);
}
// Compute 1D root of: f(x) - target = 0
float a1 = t1, a2 = t2;
for (var rootIterCount = 0; rootIterCount < 50; ++rootIterCount)
{
// Use a mix of the secant rule and bisection.
float u;
if ((rootIterCount & 1) != 0)
{
// Secant rule to improve convergence.
u = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
}
else
{
// Bisection to guarantee progress.
u = 0.5f * (a1 + a2);
}
var s = fcn.Evaluate(indexA, indexB, u);
if (System.Math.Abs(s - target) < tolerance)
{
// t2 holds a tentative value for t1
t2 = u;
break;
}
// Ensure we continue to bracket the root.
if (s > target)
{
a1 = u;
s1 = s;
}
else
{
a2 = u;
s2 = s;
}
}
}
}
// Root finder got stuck. Semi-victory.
t = t1;
return(false);
}
public float FindMinSeparation(out int indexA, out int indexB, float t)
{
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t);
_sweepB.GetTransform(out xfB, t);
switch (_type)
{
case SeparationFunctionType.Points:
{
Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, _axis);
Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -_axis);
indexA = _proxyA.GetSupport(axisA);
indexB = _proxyB.GetSupport(axisB);
Vector2 localPointA = _proxyA.GetVertex(indexA);
Vector2 localPointB = _proxyB.GetVertex(indexB);
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
float separation = Vector2.Dot(pointB - pointA, _axis);
return(separation);
}
case SeparationFunctionType.FaceA:
{
Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);
Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -normal);
indexA = -1;
indexB = _proxyB.GetSupport(axisB);
Vector2 localPointB = _proxyB.GetVertex(indexB);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
float separation = Vector2.Dot(pointB - pointA, normal);
return(separation);
}
case SeparationFunctionType.FaceB:
{
Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);
Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, -normal);
indexB = -1;
indexA = _proxyA.GetSupport(axisA);
Vector2 localPointA = _proxyA.GetVertex(indexA);
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
float separation = Vector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Debug.Assert(false);
indexA = -1;
indexB = -1;
return(0.0f);
}
}
public float FindMinSeparation(out int indexA, out int indexB, float t)
{
WorldTransform xfA, xfB;
_sweepA.GetTransform(out xfA, t);
_sweepB.GetTransform(out xfB, t);
switch (_type)
{
case Type.Points:
{
var axisA = -xfA.Rotation * _axis;
var axisB = -xfB.Rotation * -_axis;
indexA = _proxyA.GetSupport(axisA);
indexB = _proxyB.GetSupport(axisB);
var localPointA = _proxyA.Vertices[indexA];
var localPointB = _proxyB.Vertices[indexB];
var pointA = xfA.ToGlobal(localPointA);
var pointB = xfB.ToGlobal(localPointB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
return(Vector2Util.Dot((Vector2)(pointB - pointA), _axis));
// ReSharper restore RedundantCast
}
case Type.FaceA:
{
var normal = xfA.Rotation * _axis;
var pointA = xfA.ToGlobal(_localPoint);
var axisB = -xfB.Rotation * -normal;
indexA = -1;
indexB = _proxyB.GetSupport(axisB);
var localPointB = _proxyB.Vertices[indexB];
var pointB = xfB.ToGlobal(localPointB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
return(Vector2Util.Dot((Vector2)(pointB - pointA), normal));
// ReSharper restore RedundantCast
}
case Type.FaceB:
{
var normal = xfB.Rotation * _axis;
var pointB = xfB.ToGlobal(_localPoint);
var axisA = -xfA.Rotation * -normal;
indexB = -1;
indexA = _proxyA.GetSupport(axisA);
var localPointA = _proxyA.Vertices[indexA];
var pointA = xfA.ToGlobal(localPointA);
// ReSharper disable RedundantCast Necessary for FarPhysics.
return(Vector2Util.Dot((Vector2)(pointA - pointB), normal));
// ReSharper restore RedundantCast
}
default:
throw new ArgumentOutOfRangeException();
}
}
public SeparationFunction(ref SimplexCache cache,
ref DistanceProxy proxyA, ref Sweep sweepA,
ref DistanceProxy proxyB, ref Sweep sweepB,
float t1)
{
_localPoint = Vector2.Zero;
_proxyA = proxyA;
_proxyB = proxyB;
int count = cache.Count;
Debug.Assert(0 < count && count < 3);
_sweepA = sweepA;
_sweepB = sweepB;
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t1);
_sweepB.GetTransform(out xfB, t1);
if (count == 1)
{
_type = SeparationFunctionType.Points;
Vector2 localPointA = _proxyA.GetVertex(cache.IndexA[0]);
Vector2 localPointB = _proxyB.GetVertex(cache.IndexB[0]);
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
_axis = pointB - pointA;
_axis.Normalize();
return;
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
_type = SeparationFunctionType.FaceB;
Vector2 localPointB1 = proxyB.GetVertex(cache.IndexB[0]);
Vector2 localPointB2 = proxyB.GetVertex(cache.IndexB[1]);
_axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
_localPoint = 0.5f * (localPointB1 + localPointB2);
Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);
Vector2 localPointA = proxyA.GetVertex(cache.IndexA[0]);
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
else
{
// Two points on A and one or two points on B.
_type = SeparationFunctionType.FaceA;
Vector2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]);
Vector2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]);
_axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
_localPoint = 0.5f * (localPointA1 + localPointA2);
Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);
Vector2 localPointB = _proxyB.GetVertex(cache.IndexB[0]);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
}
public override void Update(GameSettings settings, GameTime gameTime)
{
base.Update(settings, gameTime);
Sweep sweepA = new Sweep();
sweepA.C0 = new Vector2(24.0f, -60.0f);
sweepA.A0 = 2.95f;
sweepA.C = sweepA.C0;
sweepA.A = sweepA.A0;
sweepA.LocalCenter = Vector2.Zero;
Sweep sweepB = new Sweep();
sweepB.C0 = new Vector2(53.474274f, -50.252514f);
sweepB.A0 = 513.36676f; // - 162.0f * b2_pi;
sweepB.C = new Vector2(54.595478f, -51.083473f);
sweepB.A = 513.62781f; // - 162.0f * b2_pi;
sweepB.LocalCenter = Vector2.Zero;
//sweepB.a0 -= 300.0f * b2_pi;
//sweepB.a -= 300.0f * b2_pi;
TOIInput input = new TOIInput();
input.ProxyA.Set(_shapeA, 0);
input.ProxyB.Set(_shapeB, 0);
input.SweepA = sweepA;
input.SweepB = sweepB;
input.TMax = 1.0f;
TOIOutput output;
TimeOfImpact.CalculateTimeOfImpact(out output, input);
DrawString("TOI = " + output.T);
DrawString(string.Format("Max TOI iters = {0:n}, Max root iters = {1:n}", TimeOfImpact.TOIMaxIters, TimeOfImpact.TOIMaxRootIters));
Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices];
DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View);
Transform transformA;
sweepA.GetTransform(out transformA, 0.0f);
for (int i = 0; i < _shapeA.Vertices.Count; ++i)
{
vertices[i] = MathUtils.Mul(ref transformA, _shapeA.Vertices[i]);
}
DebugView.DrawPolygon(vertices, _shapeA.Vertices.Count, new Color(0.9f, 0.9f, 0.9f));
Transform transformB;
sweepB.GetTransform(out transformB, 0.0f);
for (int i = 0; i < _shapeB.Vertices.Count; ++i)
{
vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]);
}
DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.5f, 0.9f, 0.5f));
sweepB.GetTransform(out transformB, output.T);
for (int i = 0; i < _shapeB.Vertices.Count; ++i)
{
vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]);
}
DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.5f, 0.7f, 0.9f));
sweepB.GetTransform(out transformB, 1.0f);
for (int i = 0; i < _shapeB.Vertices.Count; ++i)
{
vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]);
}
DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.9f, 0.5f, 0.5f));
DebugView.EndCustomDraw();
}
public static void Set(ref SimplexCache cache,
DistanceProxy proxyA, ref Sweep sweepA,
DistanceProxy proxyB, ref Sweep sweepB,
float t1)
{
_localPoint = Vector2.Zero;
_proxyA = proxyA;
_proxyB = proxyB;
int count = cache.Count;
Debug.Assert(0 < count && count < 3);
_sweepA = sweepA;
_sweepB = sweepB;
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t1);
_sweepB.GetTransform(out xfB, t1);
if (count == 1)
{
_type = SeparationFunctionType.Points;
Vector2 localPointA = _proxyA.Vertices[cache.IndexA[0]];
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
_axis = pointB - pointA;
_axis.Normalize();
return;
}
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.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
_localPoint = 0.5f * (localPointB1 + localPointB2);
Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);
Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
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.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
_localPoint = 0.5f * (localPointA1 + localPointA2);
Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
}
/// <summary>
/// CCD via the secant method.
/// Compute the time when two shapes begin to touch or touch at a closer distance.
/// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance.
/// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be
/// smaller than sum of the shape radii.
/// @warning the sweeps must have the same time interval.
/// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
/// </summary>
/// <param name="input">The input.</param>
/// <param name="shapeA">The shape A.</param>
/// <param name="shapeB">The shape B.</param>
/// <returns>
/// fraction between [0,1] in which the shapes first touch.
/// </returns>
public static float TimeOfImpact(TOIInput input)
{
++ToiCalls;
DistanceProxy proxyA = input.ProxyA;
DistanceProxy proxyB = input.ProxyB;
Sweep sweepA = input.SweepA;
Sweep sweepB = input.SweepB;
Box2DXDebug.Assert(sweepA.T0 == sweepB.T0);
Box2DXDebug.Assert(1.0f - sweepA.T0 > Settings.FLT_EPSILON);
float radius = proxyA._radius + proxyB._radius;
float tolerance = input.Tolerance;
float alpha = 0.0f;
const int k_maxIterations = 1000; // TODO_ERIN b2Settings
int iter = 0;
float target = 0.0f;
// Prepare input for distance query.
SimplexCache cache = new SimplexCache();
cache.Count = 0;
DistanceInput distanceInput = new DistanceInput();
distanceInput.proxyA = input.ProxyA;
distanceInput.proxyB = input.ProxyB;
distanceInput.UseRadii = false;
for (;;)
{
Transform xfA, xfB;
sweepA.GetTransform(out xfA, alpha);
sweepB.GetTransform(out xfB, alpha);
// Get the distance between shapes.
distanceInput.TransformA = xfA;
distanceInput.TransformB = xfB;
DistanceOutput distanceOutput;
Distance(out distanceOutput, cache, distanceInput);
if (distanceOutput.Distance <= 0.0f)
{
alpha = 1.0f;
break;
}
SeparationFunction fcn = new SeparationFunction();
fcn.Initialize(cache, proxyA, xfA, proxyB, xfB);
float separation = fcn.Evaluate(xfA, xfB);
if (separation <= 0.0f)
{
alpha = 1.0f;
break;
}
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement. We take advantage of the shape radii
// to create additional clearance.
if (separation > radius)
{
target = Math.Max(radius - tolerance, 0.75f * radius);
}
else
{
target = Math.Max(separation - tolerance, 0.02f * radius);
}
}
if (separation - target < 0.5f * tolerance)
{
if (iter == 0)
{
alpha = 1.0f;
break;
}
break;
}
#if false
// Dump the curve seen by the root finder
{
const int N = 100;
float dx = 1.0f / N;
float xs[N + 1];
