public void Clone(TOIInput input)
{
this.ProxyA.Radius = input.ProxyA.Radius;
this.ProxyA.Vertices.Clear();
this.ProxyA.Vertices.AddRange(input.ProxyA.Vertices);
this.ProxyB.Radius = input.ProxyB.Radius;
this.ProxyB.Vertices.Clear();
this.ProxyB.Vertices.AddRange(input.ProxyB.Vertices);
this.SweepA = input.SweepA;
this.SweepB = input.SweepB;
this.TMax = input.TMax;
}
public void Restore(TOIInput input)
{
input.ProxyA.Radius = this.ProxyA.Radius;
input.ProxyA.Vertices.Clear();
input.ProxyA.Vertices.AddRange(this.ProxyA.Vertices);
input.ProxyB.Radius = this.ProxyB.Radius;
input.ProxyB.Vertices.Clear();
input.ProxyB.Vertices.AddRange(this.ProxyB.Vertices);
input.SweepA = this.SweepA;
input.SweepB = this.SweepB;
input.TMax = this.TMax;
}
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 float ComputeTOI(Sweep sweepA, Sweep sweepB)
{
TOIInput input = new TOIInput();
input.SweepA = sweepA;
input.SweepB = sweepB;
input.SweepRadiusA = _fixtureA.ComputeSweepRadius(sweepA.LocalCenter);
input.SweepRadiusB = _fixtureB.ComputeSweepRadius(sweepB.LocalCenter);
input.Tolerance = Common.Settings.LinearSlop;
return(Collision.Collision.TimeOfImpact(input, _fixtureA.Shape, _fixtureB.Shape));
}
public void Distance()
{
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;
TimeOfImpact.CalculateTimeOfImpact(ref input, out _);
}
public void Distance()
{
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);
}
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"></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);
}
/// <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);
}
/// <summary>
/// Find TOI contacts and solve them.
/// </summary>
/// <param name="step">The step.</param>
private void SolveTOI(ref TimeStep step)
{
Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, ContactManager);
if (_stepComplete)
{
for (int i = 0; i < BodyList.Count; i++)
{
BodyList[i].Flags &= ~BodyFlags.Island;
BodyList[i].Sweep.Alpha0 = 0.0f;
}
for (Contact c = ContactManager.ContactList; c != null; c = c.Next)
{
// Invalidate TOI
c.Flags &= ~(ContactFlags.TOI | ContactFlags.Island);
c.TOICount = 0;
c.TOI = 1.0f;
}
}
// Find TOI events and solve them.
for (;;)
{
// Find the first TOI.
Contact minContact = null;
float minAlpha = 1.0f;
for (Contact c = ContactManager.ContactList; c != null; c = c.Next)
{
// Is this contact disabled?
if (c.Enabled == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c.TOICount > Settings.MaxSubSteps)
{
continue;
}
float alpha;
if ((c.Flags & ContactFlags.TOI) == ContactFlags.TOI)
{
// This contact has a valid cached TOI.
alpha = c.TOI;
}
else
{
Fixture fA = c.FixtureA;
Fixture fB = c.FixtureB;
// Is there a sensor?
if (fA.IsSensor || fB.IsSensor)
{
continue;
}
Body bA = fA.Body;
Body bB = fB.Body;
BodyType typeA = bA.BodyType;
BodyType typeB = bB.BodyType;
Debug.Assert(typeA == BodyType.Dynamic || typeB == BodyType.Dynamic);
bool awakeA = bA.Awake && typeA != BodyType.Static;
bool awakeB = bB.Awake && typeB != BodyType.Static;
// Is at least one body awake?
if (awakeA == false && awakeB == false)
{
continue;
}
bool collideA = bA.IsBullet || typeA != BodyType.Dynamic;
bool collideB = bB.IsBullet || typeB != BodyType.Dynamic;
// Are these two non-bullet dynamic bodies?
if (collideA == false && collideB == false)
{
continue;
}
// Compute the TOI for this contact.
// Put the sweeps onto the same time interval.
float alpha0 = bA.Sweep.Alpha0;
if (bA.Sweep.Alpha0 < bB.Sweep.Alpha0)
{
alpha0 = bB.Sweep.Alpha0;
bA.Sweep.Advance(alpha0);
}
else if (bB.Sweep.Alpha0 < bA.Sweep.Alpha0)
{
alpha0 = bA.Sweep.Alpha0;
bB.Sweep.Advance(alpha0);
}
Debug.Assert(alpha0 < 1.0f);
// Compute the time of impact in interval [0, minTOI]
TOIInput input = new TOIInput();
input.ProxyA.Set(fA.Shape, c.ChildIndexA);
input.ProxyB.Set(fB.Shape, c.ChildIndexB);
input.SweepA = bA.Sweep;
input.SweepB = bB.Sweep;
input.TMax = 1.0f;
TOIOutput output;
TimeOfImpact.CalculateTimeOfImpact(out output, ref input);
// Beta is the fraction of the remaining portion of the .
float beta = output.T;
if (output.State == TOIOutputState.Touching)
{
alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c.TOI = alpha;
c.Flags |= ContactFlags.TOI;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == null || 1.0f - 10.0f * Settings.Epsilon < minAlpha)
{
// No more TOI events. Done!
_stepComplete = true;
break;
}
// Advance the bodies to the TOI.
Fixture fA1 = minContact.FixtureA;
Fixture fB1 = minContact.FixtureB;
Body bA1 = fA1.Body;
Body bB1 = fB1.Body;
Sweep backup1 = bA1.Sweep;
Sweep backup2 = bB1.Sweep;
bA1.Advance(minAlpha);
bB1.Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.Update(ContactManager);
minContact.Flags &= ~ContactFlags.TOI;
++minContact.TOICount;
// Is the contact solid?
if (minContact.Enabled == false || minContact.IsTouching() == false)
{
// Restore the sweeps.
minContact.Enabled = false;
bA1.Sweep = backup1;
bB1.Sweep = backup2;
bA1.SynchronizeTransform();
bB1.SynchronizeTransform();
continue;
}
bA1.Awake = true;
bB1.Awake = true;
// Build the island
Island.Clear();
Island.Add(bA1);
Island.Add(bB1);
Island.Add(minContact);
bA1.Flags |= BodyFlags.Island;
bB1.Flags |= BodyFlags.Island;
minContact.Flags |= ContactFlags.Island;
// Get contacts on bodyA and bodyB.
Body[] bodies = { bA1, bB1 };
for (int i = 0; i < 2; ++i)
{
Body body = bodies[i];
if (body.BodyType == BodyType.Dynamic)
{
// for (ContactEdge ce = body.ContactList; ce && Island.BodyCount < Settings.MaxTOIContacts; ce = ce.Next)
for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
{
Contact contact = ce.Contact;
// Has this contact already been added to the island?
if ((contact.Flags & ContactFlags.Island) == ContactFlags.Island)
{
continue;
}
// Only add static, kinematic, or bullet bodies.
Body other = ce.Other;
if (other.BodyType == BodyType.Dynamic &&
body.IsBullet == false && other.IsBullet == false)
{
continue;
}
// Skip sensors.
if (contact.FixtureA.IsSensor || contact.FixtureB.IsSensor)
{
continue;
}
// Tentatively advance the body to the TOI.
Sweep backup = other.Sweep;
if ((other.Flags & BodyFlags.Island) == 0)
{
other.Advance(minAlpha);
}
// Update the contact points
contact.Update(ContactManager);
// Was the contact disabled by the user?
if (contact.Enabled == false)
{
other.Sweep = backup;
other.SynchronizeTransform();
continue;
}
// Are there contact points?
if (contact.IsTouching() == false)
{
other.Sweep = backup;
other.SynchronizeTransform();
continue;
}
// Add the contact to the island
contact.Flags |= ContactFlags.Island;
Island.Add(contact);
// Has the other body already been added to the island?
if ((other.Flags & BodyFlags.Island) == BodyFlags.Island)
{
continue;
}
// Add the other body to the island.
other.Flags |= BodyFlags.Island;
if (other.BodyType != BodyType.Static)
{
other.Awake = true;
}
Island.Add(other);
}
}
}
TimeStep subStep;
subStep.dt = (1.0f - minAlpha) * step.dt;
subStep.inv_dt = 1.0f / subStep.dt;
subStep.dtRatio = 1.0f;
//subStep.positionIterations = 20;
//subStep.velocityIterations = step.velocityIterations;
//subStep.warmStarting = false;
Island.SolveTOI(ref subStep);
// Reset island flags and synchronize broad-phase proxies.
for (int i = 0; i < Island.BodyCount; ++i)
{
Body body = Island.Bodies[i];
body.Flags &= ~BodyFlags.Island;
if (body.BodyType != BodyType.Dynamic)
{
continue;
}
body.SynchronizeFixtures();
// Invalidate all contact TOIs on this displaced body.
for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
{
ce.Contact.Flags &= ~(ContactFlags.TOI | ContactFlags.Island);
}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
ContactManager.FindNewContacts();
if (_subStepping)
{
_stepComplete = false;
break;
}
}
}
/// <summary>
// Advance a dynamic body to its first time of contact
// and adjust the position to ensure clearance.
/// </summary>
/// <param name="body">The body.</param>
private void SolveTOI(Body body)
{
// Find the minimum contact.
Contact toiContact = null;
float toi = 1.0f;
Body toiOther = null;
bool found;
int count;
int iter = 0;
bool bullet = body.IsBullet;
// Iterate until all contacts agree on the minimum TOI. We have
// to iterate because the TOI algorithm may skip some intermediate
// collisions when objects rotate through each other.
do
{
count = 0;
found = false;
for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
{
if (ce.Contact == toiContact)
{
continue;
}
Body other = ce.Other;
BodyType type = other.BodyType;
// Only bullets perform TOI with dynamic bodies.
if (bullet)
{
// Bullets only perform TOI with bodies that have their TOI resolved.
if ((other.Flags & BodyFlags.Toi) == 0)
{
continue;
}
// No repeated hits on non-static bodies
if (type != BodyType.Static && (ce.Contact.Flags & ContactFlags.BulletHit) != 0)
{
continue;
}
}
else if (type == BodyType.Dynamic)
{
continue;
}
// Check for a disabled contact.
Contact contact = ce.Contact;
if (contact.Enabled == false)
{
continue;
}
// Prevent infinite looping.
if (contact.TOICount > 10)
{
continue;
}
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
int indexA = contact.ChildIndexA;
int indexB = contact.ChildIndexB;
// Cull sensors.
if (fixtureA.IsSensor || fixtureB.IsSensor)
{
continue;
}
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
// Compute the time of impact in interval [0, minTOI]
TOIInput input = new TOIInput();
input.ProxyA.Set(fixtureA.Shape, indexA);
input.ProxyB.Set(fixtureB.Shape, indexB);
input.SweepA = bodyA.Sweep;
input.SweepB = bodyB.Sweep;
input.TMax = toi;
TOIOutput output;
TimeOfImpact.CalculateTimeOfImpact(out output, ref input);
if (output.State == TOIOutputState.Touching && output.T < toi)
{
toiContact = contact;
toi = output.T;
toiOther = other;
found = true;
}
++count;
}
++iter;
} while (found && count > 1 && iter < 50);
if (toiContact == null)
{
body.Advance(1.0f);
return;
}
Sweep backup = body.Sweep;
body.Advance(toi);
toiContact.Update(ContactManager);
if (toiContact.Enabled == false)
{
// Contact disabled. Backup and recurse.
body.Sweep = backup;
SolveTOI(body);
}
++toiContact.TOICount;
// Update all the valid contacts on this body and build a contact island.
count = 0;
for (ContactEdge ce = body.ContactList; (ce != null) && (count < Settings.MaxTOIContacts); ce = ce.Next)
{
Body other = ce.Other;
BodyType type = other.BodyType;
// Only perform correction with static bodies, so the
// body won't get pushed out of the world.
if (type == BodyType.Dynamic)
{
continue;
}
// Check for a disabled contact.
Contact contact = ce.Contact;
if (contact.Enabled == false)
{
continue;
}
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
// Cull sensors.
if (fixtureA.IsSensor || fixtureB.IsSensor)
{
continue;
}
// The contact likely has some new contact points. The listener
// gives the user a chance to disable the contact.
if (contact != toiContact)
{
contact.Update(ContactManager);
}
// Did the user disable the contact?
if (contact.Enabled == false)
{
// Skip this contact.
continue;
}
if (contact.IsTouching() == false)
{
continue;
}
_toiContacts[count] = contact;
++count;
}
// Reduce the TOI body's overlap with the contact island.
_toiSolver.Initialize(_toiContacts, count, body);
const float k_toiBaumgarte = 0.75f;
// bool solved = false;
for (int i = 0; i < 20; ++i)
{
bool contactsOkay = _toiSolver.Solve(k_toiBaumgarte);
if (contactsOkay)
{
// solved = true;
break;
}
}
if (toiOther.BodyType != BodyType.Static)
{
toiContact.Flags |= ContactFlags.BulletHit;
}
}
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
}
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 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));
}
private void SolveTOI(ref TimeStep step)
{
Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, ContactManager);
if (_stepComplete)
{
for (var i = 0; i < BodyList.Count; i++)
{
BodyList[i]._flags &= ~BodyFlags.IslandFlag;
BodyList[i]._sweep.Alpha0 = 0.0f;
}
for (var i = 0; i < ContactManager.ContactList.Count; i++)
{
var c = ContactManager.ContactList[i];
// Invalidate TOI
c._flags &= ~ContactFlags.IslandFlag;
c._flags &= ~ContactFlags.TOIFlag;
c._toiCount = 0;
c._toi = 1.0f;
}
}
// Find TOI events and solve them.
for (;;)
{
// Find the first TOI.
Contact minContact = null;
var minAlpha = 1.0f;
for (var i = 0; i < ContactManager.ContactList.Count; i++)
{
var c = ContactManager.ContactList[i];
// Is this contact disabled?
if (c.Enabled == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c._toiCount > Settings.MaxSubSteps)
{
continue;
}
float alpha;
if (c.TOIFlag)
{
// This contact has a valid cached TOI.
alpha = c._toi;
}
else
{
var fA = c.FixtureA;
var fB = c.FixtureB;
// Is there a sensor?
if (fA.IsSensor || fB.IsSensor)
{
continue;
}
var bA = fA.Body;
var bB = fB.Body;
var typeA = bA.BodyType;
var typeB = bB.BodyType;
Debug.Assert(typeA == BodyType.Dynamic || typeB == BodyType.Dynamic);
var activeA = bA.Awake && typeA != BodyType.Static;
var activeB = bB.Awake && typeB != BodyType.Static;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false)
{
continue;
}
var collideA = (bA.IsBullet || typeA != BodyType.Dynamic) &&
(fA.IgnoreCCDWith & fB.CollisionCategories) == 0 && !bA.IgnoreCCD;
var collideB = (bB.IsBullet || typeB != BodyType.Dynamic) &&
(fB.IgnoreCCDWith & fA.CollisionCategories) == 0 && !bB.IgnoreCCD;
// Are these two non-bullet dynamic bodies?
if (collideA == false && collideB == false)
{
continue;
}
// Compute the TOI for this contact.
// Put the sweeps onto the same time interval.
var alpha0 = bA._sweep.Alpha0;
if (bA._sweep.Alpha0 < bB._sweep.Alpha0)
{
alpha0 = bB._sweep.Alpha0;
bA._sweep.Advance(alpha0);
}
else if (bB._sweep.Alpha0 < bA._sweep.Alpha0)
{
alpha0 = bA._sweep.Alpha0;
bB._sweep.Advance(alpha0);
}
Debug.Assert(alpha0 < 1.0f);
// Compute the time of impact in interval [0, minTOI]
var input = new TOIInput();
input.ProxyA = new DistanceProxy(fA.Shape, c.ChildIndexA);
input.ProxyB = new DistanceProxy(fB.Shape, c.ChildIndexB);
input.SweepA = bA._sweep;
input.SweepB = bB._sweep;
input.TMax = 1.0f;
TOIOutput output;
TimeOfImpact.CalculateTimeOfImpact(ref input, out output);
// Beta is the fraction of the remaining portion of the .
var beta = output.T;
if (output.State == TOIOutputState.Touching)
{
alpha = Mathf.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c._toi = alpha;
c._flags &= ~ContactFlags.TOIFlag;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == null || 1.0f - 10.0f * Settings.Epsilon < minAlpha)
{
// No more TOI events. Done!
_stepComplete = true;
break;
}
// Advance the bodies to the TOI.
var fA1 = minContact.FixtureA;
var fB1 = minContact.FixtureB;
var bA0 = fA1.Body;
var bB0 = fB1.Body;
var backup1 = bA0._sweep;
var backup2 = bB0._sweep;
bA0.Advance(minAlpha);
bB0.Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.Update(ContactManager);
minContact._flags &= ~ContactFlags.TOIFlag;
++minContact._toiCount;
// Is the contact solid?
if (minContact.Enabled == false || minContact.IsTouching == false)
{
// Restore the sweeps.
minContact._flags &= ~ContactFlags.EnabledFlag;
bA0._sweep = backup1;
bB0._sweep = backup2;
bA0.SynchronizeTransform();
bB0.SynchronizeTransform();
continue;
}
bA0.Awake = true;
bB0.Awake = true;
// Build the island
Island.Clear();
Island.Add(bA0);
Island.Add(bB0);
Island.Add(minContact);
bA0._flags |= BodyFlags.IslandFlag;
bB0._flags |= BodyFlags.IslandFlag;
minContact._flags &= ~ContactFlags.IslandFlag;
// Get contacts on bodyA and bodyB.
Body[] bodies = { bA0, bB0 };
for (var i = 0; i < 2; ++i)
{
var body = bodies[i];
if (body.BodyType == BodyType.Dynamic)
{
for (var ce = body.ContactList; ce != null; ce = ce.Next)
{
var contact = ce.Contact;
if (Island.BodyCount == Island.BodyCapacity)
{
break;
}
if (Island.ContactCount == Island.ContactCapacity)
{
break;
}
// Has this contact already been added to the island?
if (contact.IslandFlag)
{
continue;
}
// Only add static, kinematic, or bullet bodies.
var other = ce.Other;
if (other.BodyType == BodyType.Dynamic &&
body.IsBullet == false && other.IsBullet == false)
{
continue;
}
// Skip sensors.
if (contact.FixtureA.IsSensor || contact.FixtureB.IsSensor)
{
continue;
}
// Tentatively advance the body to the TOI.
var backup = other._sweep;
if (!other.IsIsland)
{
other.Advance(minAlpha);
}
// Update the contact points
contact.Update(ContactManager);
// Was the contact disabled by the user?
if (contact.Enabled == false)
{
other._sweep = backup;
other.SynchronizeTransform();
continue;
}
// Are there contact points?
if (contact.IsTouching == false)
{
other._sweep = backup;
other.SynchronizeTransform();
continue;
}
// Add the contact to the island
minContact._flags |= ContactFlags.IslandFlag;
Island.Add(contact);
// Has the other body already been added to the island?
if (other.IsIsland)
{
continue;
}
// Add the other body to the island.
other._flags |= BodyFlags.IslandFlag;
if (other.BodyType != BodyType.Static)
{
other.Awake = true;
}
Island.Add(other);
}
}
}
TimeStep subStep;
subStep.dt = (1.0f - minAlpha) * step.dt;
subStep.inv_dt = 1.0f / subStep.dt;
subStep.dtRatio = 1.0f;
Island.SolveTOI(ref subStep, bA0.IslandIndex, bB0.IslandIndex);
// Reset island flags and synchronize broad-phase proxies.
for (var i = 0; i < Island.BodyCount; ++i)
{
var body = Island.Bodies[i];
body._flags &= ~BodyFlags.IslandFlag;
if (body.BodyType != BodyType.Dynamic)
{
continue;
}
body.SynchronizeFixtures();
// Invalidate all contact TOIs on this displaced body.
for (var ce = body.ContactList; ce != null; ce = ce.Next)
{
ce.Contact._flags &= ~ContactFlags.TOIFlag;
ce.Contact._flags &= ~ContactFlags.IslandFlag;
}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
ContactManager.FindNewContacts();
if (Settings.EnableSubStepping)
{
_stepComplete = false;
break;
}
}
}
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));
}
