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(); }