// This algorithm uses conservative advancement to compute the time of
// impact (TOI) of two shapes.
// Refs: Bullet, Young Kim
/// <summary>
/// Compute the time when two shapes begin to touch or touch at a closer distance.
/// warning the sweeps must have the same time interval.
/// </summary>
/// <param name="shape1"></param>
/// <param name="sweep1"></param>
/// <param name="shape2"></param>
/// <param name="sweep2"></param>
/// <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>
#warning: "check params"
public static float TimeOfImpact(Shape shape1, Sweep sweep1, Shape shape2, Sweep sweep2)
{
float r1 = shape1.GetSweepRadius();
float r2 = shape2.GetSweepRadius();
Box2DXDebug.Assert(sweep1.T0 == sweep2.T0);
Box2DXDebug.Assert(1.0f - sweep1.T0 > Common.Settings.FLT_EPSILON);
float t0 = sweep1.T0;
Vec2 v1 = sweep1.C - sweep1.C0;
Vec2 v2 = sweep2.C - sweep2.C0;
float omega1 = sweep1.A - sweep1.A0;
float omega2 = sweep2.A - sweep2.A0;
float alpha = 0.0f;
Vec2 p1, p2;
int k_maxIterations = 20; // TODO_ERIN b2Settings
int iter = 0;
Vec2 normal = Vec2.Zero;
float distance = 0.0f;
float targetDistance = 0.0f;
for (; ; )
{
float t = (1.0f - alpha) * t0 + alpha;
XForm xf1, xf2;
sweep1.GetXForm(out xf1, t);
sweep2.GetXForm(out xf2, t);
// Get the distance between shapes.
distance = Collision.Distance(out p1, out p2, shape1, xf1, shape2, xf2);
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement.
if (distance > 2.0f * Settings.ToiSlop)
{
targetDistance = 1.5f * Settings.ToiSlop;
}
else
{
targetDistance = Common.Math.Max(0.05f * Settings.ToiSlop, distance - 0.5f * Settings.ToiSlop);
}
}
if (distance - targetDistance < 0.05f * Settings.ToiSlop || iter == k_maxIterations)
{
break;
}
normal = p2 - p1;
normal.Normalize();
// Compute upper bound on remaining movement.
float approachVelocityBound = Vec2.Dot(normal, v1 - v2) +
Common.Math.Abs(omega1) * r1 + Common.Math.Abs(omega2) * r2;
if (Common.Math.Abs(approachVelocityBound) < Common.Settings.FLT_EPSILON)
{
alpha = 1.0f;
break;
}
// Get the conservative time increment. Don't advance all the way.
float dAlpha = (distance - targetDistance) / approachVelocityBound;
//float dt = (distance - 0.5f * Settings.LinearSlop) / approachVelocityBound;
float newAlpha = alpha + dAlpha;
// The shapes may be moving apart or a safe distance apart.
if (newAlpha < 0.0f || 1.0f < newAlpha)
{
alpha = 1.0f;
break;
}
// Ensure significant advancement.
if (newAlpha < (1.0f + 100.0f * Common.Settings.FLT_EPSILON) * alpha)
{
break;
}
alpha = newAlpha;
++iter;
}
return alpha;
}
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();
float toi = Collision.TimeOfImpact(_shape1, sweep1, _shape2, sweep2);
OpenGLDebugDraw.DrawString(5, _textLine, "toi = " + toi.ToString());
_textLine += 15;
XForm xf2 = new XForm();
sweep2.GetXForm(out xf2, toi);
int vertexCount = _shape2.VertexCount;
Vec2[] vertices = new Vec2[Box2DX.Common.Settings.MaxPolygonVertices];
Vec2[] localVertices = _shape2.GetVertices();
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = Box2DX.Common.Math.Mul(xf2, localVertices[i]);
}
_debugDraw.DrawPolygon(vertices, vertexCount, new Color(0.5f, 0.7f, 0.9f));
localVertices = _shape2.GetCoreVertices();
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = Box2DX.Common.Math.Mul(xf2, localVertices[i]);
}
_debugDraw.DrawPolygon(vertices, vertexCount, new Color(0.5f, 0.7f, 0.9f));
}