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.IslandFlag;
BodyList[i]._sweep.Alpha0 = 0.0f;
}
for (int i = 0; i < ContactManager.ContactList.Count; i++)
{
Contact 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;
float minAlpha = 1.0f;
for (int i = 0; i < ContactManager.ContactList.Count; i++)
{
Contact 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
{
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 activeA = bA.Awake && typeA != BodyType.Static;
bool activeB = bB.Awake && typeB != BodyType.Static;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false)
{
continue;
}
bool collideA = (bA.IsBullet || typeA != BodyType.Dynamic) && (fA.IgnoreCCDWith & fB.CollisionCategories) == 0 && !bA.IgnoreCCD;
bool 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.
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]
_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, _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.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.
Fixture fA1 = minContact.FixtureA;
Fixture fB1 = minContact.FixtureB;
Body bA0 = fA1.Body;
Body bB0 = fB1.Body;
Sweep backup1 = bA0._sweep;
Sweep 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 (int i = 0; i < 2; ++i)
{
Body body = bodies[i];
if (body.BodyType == BodyType.Dynamic)
{
for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
{
Contact 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.
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.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 (int i = 0; i < Island.BodyCount; ++i)
{
Body 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 (ContactEdge 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 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 = new DistanceProxy(_shapeA, 0);
input.ProxyB = new DistanceProxy(_shapeB, 0);
input.SweepA = sweepA;
input.SweepB = sweepB;
input.TMax = 1.0f;
TOIOutput output;
TimeOfImpact.CalculateTimeOfImpact(out output, ref 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] = Transform.Multiply(_shapeA.Vertices[i], ref transformA);
}
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] = Transform.Multiply(_shapeB.Vertices[i], ref transformB);
}
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] = Transform.Multiply(_shapeB.Vertices[i], ref transformB);
}
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] = Transform.Multiply(_shapeB.Vertices[i], ref transformB);
}
DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.9f, 0.5f, 0.5f));
DebugView.EndCustomDraw();
}
