// Find TOI contacts and solve them. public void SolveTOI(b2TimeStep step) { b2Island island = new b2Island(2 * b2Settings.b2_maxTOIContacts, b2Settings.b2_maxTOIContacts, 0, m_contactManager.ContactListener); if (m_stepComplete) { for (b2Body b = m_bodyList; b; b = b.Next) { b.BodyFlags &= ~b2Body.e_islandFlag; b.m_sweep.alpha0 = 0.0f; } for (b2Contact c = m_contactManager.ContactList; c; c = c.Next) { // Invalidate TOI c.ContactFlags &= ~(b2ContactType.e_toiFlag | b2ContactType.e_islandFlag); c.m_toiCount = 0; c.m_toi = 1.0f; } } // Find TOI events and solve them. for (; ;) { // Find the first TOI. b2Contact minContact = null; float minAlpha = 1.0f; for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next) { // Is this contact disabled? if (c.IsEnabled() == false) { continue; } // Prevent excessive sub-stepping. if (c.m_toiCount > b2Settings.b2_maxSubSteps) { continue; } float alpha = 1.0f; if (c.ContactFlags.HasFlag(b2ContactFlags.e_toiFlag)) { // This contact has a valid cached TOI. alpha = c.m_toi; } else { b2Fixture fA = c.GetFixtureA(); b2Fixture fB = c.GetFixtureB(); // Is there a sensor? if (fA.IsSensor || fB.IsSensor) { continue; } b2Body bA = fA.Body; b2Body bB = fB.Body; b2BodyType typeA = bA.BodyType; b2BodyType typeB = bB.BodyType; bool activeA = bA.IsAwake() && typeA != b2BodyType.b2_staticBody; bool activeB = bB.IsAwake() && typeB != b2BodyType.b2_staticBody; // Is at least one body active (awake and dynamic or kinematic)? if (activeA == false && activeB == false) { continue; } bool collideA = bA.IsBullet() || typeA != b2BodyType.b2_dynamicBody; bool collideB = bB.IsBullet() || typeB != b2BodyType.b2_dynamicBody; // 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); } int indexA = c.GetChildIndexA(); int indexB = c.GetChildIndexB(); // Compute the time of impact in interval [0, minTOI] b2TOIInput input = new b2TOIInput(); input.proxyA.Set(fA.Shape, indexA); input.proxyB.Set(fB.Shape, indexB); input.sweepA = bA.Sweep; input.sweepB = bB.Sweep; input.tMax = 1.0f; b2TOIOutput output = b2TimeOfImpact(input); // Beta is the fraction of the remaining portion of the . float beta = output.t; if (output.state == b2TOIOutputType.e_touching) { alpha = b2Math.b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); } else { alpha = 1.0f; } c.m_toi = alpha; c.ContactFlags |= b2ContactFlags.e_toiFlag; } if (alpha < minAlpha) { // This is the minimum TOI found so far. minContact = c; minAlpha = alpha; } } if (minContact == null || 1.0f - 10.0f * b2Settings.b2_epsilon < minAlpha) { // No more TOI events. Done! m_stepComplete = true; break; } { // Advance the bodies to the TOI. b2Fixture fA = minContact.GetFixtureA(); b2Fixture fB = minContact.GetFixtureB(); b2Body bA = fA.Body; b2Body bB = fB.Body; b2Sweep backup1 = bA.Sweep; b2Sweep backup2 = bB.Sweep; bA.Advance(minAlpha); bB.Advance(minAlpha); // The TOI contact likely has some new contact points. minContact.Update(m_contactManager.ContactListener); minContact.ContactFlags &= ~b2ContactFlags.e_toiFlag; ++minContact.m_toiCount; // Is the contact solid? if (minContact.IsEnabled() == false || minContact.IsTouching() == false) { // Restore the sweeps. minContact.SetEnabled(false); bA.Sweep = backup1; bB.Sweep = backup2; bA.SynchronizeTransform(); bB.SynchronizeTransform(); continue; } bA.SetAwake(true); bB.SetAwake(true); // Build the island island.Clear(); island.Add(bA); island.Add(bB); island.Add(minContact); bA.BodyFlags |= b2BodyFlags.e_islandFlag; bB.BodyFlags |= b2BodyFlags.e_islandFlag; minContact.ContentType |= b2ContactFlags.e_islandFlag; // Get contacts on bodyA and bodyB. b2Body[] bodies = new b2Body[] { bA, bB }; for (int i = 0; i < 2; ++i) { b2Body body = bodies[i]; if (body.BodyType == b2BodyType.b2_dynamicBody) { for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next) { if (island.BodyCount == island.BodyCapacity) { break; } if (island.ContactCount == island.ContactCapacity) { break; } b2Contact contact = ce.contact; // Has this contact already been added to the island? if (contact.ContactType & b2ContactType.e_islandFlag) { continue; } // Only add static, kinematic, or bullet bodies. b2Body other = ce.other; if (other.BodyType == b2BodyType.b2_dynamicBody && body.IsBullet() == false && other.IsBullet() == false) { continue; } // Skip sensors. bool sensorA = contact.m_fixtureA.m_isSensor; bool sensorB = contact.m_fixtureB.m_isSensor; if (sensorA || sensorB) { continue; } // Tentatively advance the body to the TOI. b2Sweep backup = other.Sweep; if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag)) { other.Advance(minAlpha); } // Update the contact points contact.Update(m_contactManager.ContactListener); // Was the contact disabled by the user? if (contact.IsEnabled() == 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.ContactFlags |= b2ContactFlags.e_islandFlag; island.Add(contact); // Has the other body already been added to the island? if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag)) { continue; } // Add the other body to the island. other.BodyFlags |= b2BodyFlags.e_islandFlag; if (other.BodyType != b2BodyType.b2_staticBody) { other.SetAwake(true); } island.Add(other); } } } b2TimeStep 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(subStep, bA.m_islandIndex, bB.m_islandIndex); // Reset island flags and synchronize broad-phase proxies. for (int i = 0; i < island.m_bodyCount; ++i) { b2Body body = island.m_bodies[i]; body.BodyFlags &= ~b2BodyFlags.e_islandFlag; if (body.BodyType != b2BodyType.b2_dynamicBody) { continue; } body.SynchronizeFixtures(); // Invalidate all contact TOIs on this displaced body. for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next) { ce.Contact.ContactFlags &= ~(b2ContactFlags.e_toiFlag | b2ContactFlags.e_islandFlag); } } // Commit fixture proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. m_contactManager.FindNewContacts(); if (m_subStepping) { m_stepComplete = false; break; } } } }
protected override void Draw(Settings settings) { base.Draw(settings); b2Sweep sweepA = new b2Sweep(); sweepA.alpha0 = 0; sweepA.c0.Set(24.0f, -60.0f); sweepA.a0 = 2.95f; sweepA.c = sweepA.c0; sweepA.a = sweepA.a0; sweepA.localCenter.SetZero(); b2Sweep sweepB = new b2Sweep(); sweepB.alpha0 = 0; sweepB.c0.Set(53.474274f, -50.252514f); sweepB.a0 = 513.36676f; // - 162.0f * b2_pi; sweepB.c.Set(54.595478f, -51.083473f); sweepB.a = 513.62781f; // - 162.0f * b2_pi; sweepB.localCenter.SetZero(); //sweepB.a0 -= 300.0f * b2_pi; //sweepB.a -= 300.0f * b2_pi; b2TOIInput input = b2TOIInput.Create(); input.proxyA.Set(m_shapeA, 0); input.proxyB.Set(m_shapeB, 0); input.sweepA = sweepA; input.sweepB = sweepB; input.tMax = 1.0f; b2TOIOutput output = b2TimeOfImpact.Compute(input); m_debugDraw.DrawString(5, m_textLine, "toi = {0}", output.t); m_textLine += 15; m_debugDraw.DrawString(5, m_textLine, "max toi iters = {0}, max root iters = {1}", b2TimeOfImpact.b2_toiMaxIters, b2TimeOfImpact.b2_toiMaxRootIters); m_textLine += 15; b2Vec2[] vertices = new b2Vec2[b2Settings.b2_maxPolygonVertices]; b2Transform transformA = new b2Transform(); sweepA.GetTransform(ref transformA, 0.0f); for (int i = 0; i < m_shapeA.VertexCount; ++i) { vertices[i] = b2Math.b2Mul(transformA, m_shapeA.Vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeA.VertexCount, new b2Color(0.9f, 0.9f, 0.9f)); b2Transform transformB = new b2Transform(); sweepB.GetTransform(ref transformB, 0.0f); b2Vec2 localPoint = new b2Vec2(2.0f, -0.1f); for (int i = 0; i < m_shapeB.VertexCount; ++i) { vertices[i] = b2Math.b2Mul(transformB, m_shapeB.Vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.VertexCount, new b2Color(0.5f, 0.9f, 0.5f)); sweepB.GetTransform(ref transformB, output.t); for (int i = 0; i < m_shapeB.VertexCount; ++i) { vertices[i] = b2Math.b2Mul(transformB, m_shapeB.Vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.VertexCount, new b2Color(0.5f, 0.7f, 0.9f)); sweepB.GetTransform(ref transformB, 1.0f); for (int i = 0; i < m_shapeB.VertexCount; ++i) { vertices[i] = b2Math.b2Mul(transformB, m_shapeB.Vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.VertexCount, new b2Color(0.9f, 0.5f, 0.5f)); #if false for (float t = 0.0f; t < 1.0f; t += 0.1f) { sweepB.GetTransform(ref transformB, t); for (int i = 0; i < m_shapeB.VertexCount; ++i) { vertices[i] = b2Math.b2Mul(transformB, m_shapeB.Vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.VertexCount, new b2Color(0.9f, 0.5f, 0.5f)); } #endif }