public static bool InPoints(Vector2 w, Vector2[] points, int pointCount)
{
float k_tolerance = 100.0f * Settings.FLT_EPSILON;
for (int i = 0; i < pointCount; ++i)
{
Vector2 d = CommonMath.Abs(w - points[i]);
Vector2 m = CommonMath.Max(CommonMath.Abs(w), CommonMath.Abs(points[i]));
if (d.X < k_tolerance * (m.X + 1.0f) &&
d.Y < k_tolerance * (m.Y + 1.0f))
{
return(true);
}
}
return(false);
}
// 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>
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;
Vector2 v1 = sweep1.C - sweep1.C0;
Vector2 v2 = sweep2.C - sweep2.C0;
float omega1 = sweep1.A - sweep1.A0;
float omega2 = sweep2.A - sweep2.A0;
float alpha = 0.0f;
Vector2 p1, p2;
int k_maxIterations = 20; // TODO_ERIN b2Settings
int iter = 0;
Vector2 normal = Vector2.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 = 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 = CommonMath.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 = Vector2.Dot(normal, v1 - v2) +
CommonMath.Abs(omega1) * r1 + CommonMath.Abs(omega2) * r2;
if (CommonMath.Abs(approachVelocityBound) < 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 * Settings.FLT_EPSILON) * alpha)
{
break;
}
alpha = newAlpha;
++iter;
}
return(alpha);
}
