public void Initialize(Contact[] contacts, int count, Body toiBody) { _count = count; _toiBody = toiBody; if (_constraints.Length < _count) { _constraints = new TOIConstraint[Math.Max(_constraints.Length * 2, _count)]; } for (int i = 0; i < _count; ++i) { Contact contact = contacts[i]; Fixture fixtureA = contact.GetFixtureA(); Fixture fixtureB = contact.GetFixtureB(); Shape shapeA = fixtureA.GetShape(); Shape shapeB = fixtureB.GetShape(); float radiusA = shapeA._radius; float radiusB = shapeB._radius; Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); Manifold manifold; contact.GetManifold(out manifold); //Debug.Assert(manifold._pointCount > 0); TOIConstraint constraint = _constraints[i]; constraint.bodyA = bodyA; constraint.bodyB = bodyB; constraint.localNormal = manifold._localNormal; constraint.localPoint = manifold._localPoint; constraint.type = manifold._type; constraint.pointCount = manifold._pointCount; constraint.radius = radiusA + radiusB; for (int j = 0; j < constraint.pointCount; ++j) { constraint.localPoints[j] = manifold._points[j].LocalPoint; } _constraints[i] = constraint; } }
/// Called for each fixture found in the query AABB. /// @return false to terminate the query. public override bool ReportFixture(Fixture fixture) { if (m_count == e_maxCount) { return false; } Body body = fixture.GetBody(); Shape shape = fixture.GetShape(); bool overlap = Collision.TestOverlap(shape, 0, m_circle, 0, body.GetTransform(), m_transform); if (overlap) { DrawFixture(fixture); ++m_count; } return true; }
public override float ReportFixture(Fixture fixture, Vec2 point, Vec2 normal, float fraction) { Body body = fixture.GetBody(); if (body.UserData != null) { int index = (int)body.UserData; if (index == 0) { // By returning -1, we instruct the calling code to ignore this fixture // and continue the ray-cast to the next fixture. return -1.0f; } } m_hit = true; m_point = point; m_normal = normal; // At this point we have a hit, so we know the ray is obstructed. // By returning 0, we instruct the calling code to terminate the ray-cast. return 0.0f; }
public void DrawFixture(Fixture fixture) { Color color = Color.FromArgb(245, 245, 150); Transform xf = fixture.GetBody().GetTransform(); switch (fixture.GetShapeType()) { case ShapeType.Circle: { CircleShape circle = (CircleShape)fixture.GetShape(); Vec2 center = Utilities.Mul(xf, circle.m_p); float radius = circle.m_radius; m_debugDraw.DrawCircle(center, radius, color); } break; case ShapeType.Polygon: { PolygonShape poly = (PolygonShape)fixture.GetShape(); int vertexCount = poly.m_count; Utilities.Assert(vertexCount <= Settings._maxPolygonVertices); Vec2[] vertices = new Vec2[Settings._maxPolygonVertices]; for (int i = 0; i < vertexCount; ++i) { vertices[i] = Utilities.Mul(xf, poly.m_vertices[i]); } m_debugDraw.DrawPolygon(vertices, vertexCount, color); } break; default: break; } }
public void Destroy(Contact c) { Fixture fixtureA = c.FixtureA; Fixture fixtureB = c.FixtureB; Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); if (m_contactListener != null && c.IsTouching()) { m_contactListener.EndContact(c); } // Remove from the world. m_contactList.Remove(c); // Remove from body 1 c.m_nodeA.other.m_contactList.Remove(c.m_nodeA); // Remove from body 2 c.m_nodeB.other.m_contactList.Remove(c.m_nodeA); // Call the factory. }
public override float ReportFixture(Fixture fixture, Vec2 point, Vec2 normal, float fraction) { Body body = fixture.GetBody(); if (body.UserData != null) { int index = (int)body.UserData; if (index == 0) { // By returning -1, we instruct the calling code to ignore this fixture and // continue the ray-cast to the next fixture. return -1.0f; } } m_hit = true; m_point = point; m_normal = normal; // By returning the current fraction, we instruct the calling code to clip the ray and // continue the ray-cast to the next fixture. WARNING: do not assume that fixtures // are reported in order. However, by clipping, we can always get the closest fixture. return fraction; }
internal void Collide() { // Update awake contacts. Contact c = _contactList; while (c != null) { Fixture fixtureA = c.GetFixtureA(); Fixture fixtureB = c.GetFixtureB(); int indexA = c.GetChildIndexA(); int indexB = c.GetChildIndexB(); Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); if (bodyA.IsAwake() == false && bodyB.IsAwake() == false) { c = c.GetNext(); continue; } // Is this contact flagged for filtering? if ((c._flags & ContactFlags.Filter) == ContactFlags.Filter) { // Should these bodies collide? if (bodyB.ShouldCollide(bodyA) == false) { Contact cNuke = c; c = cNuke.GetNext(); Destroy(cNuke); continue; } // Check user filtering. if (ContactFilter != null && ContactFilter.ShouldCollide(fixtureA, fixtureB) == false) { Contact cNuke = c; c = cNuke.GetNext(); Destroy(cNuke); continue; } // Clear the filtering flag. c._flags &= ~ContactFlags.Filter; } int proxyIdA = fixtureA._proxies[indexA].proxyId; int proxyIdB = fixtureB._proxies[indexB].proxyId; bool overlap = _broadPhase.TestOverlap(proxyIdA, proxyIdB); // Here we destroy contacts that cease to overlap in the broad-phase. if (overlap == false) { Contact cNuke = c; c = cNuke.GetNext(); Destroy(cNuke); continue; } // The contact persists. c.Update(ContactListener); c = c.GetNext(); } }
internal void Destroy(Contact c) { Fixture fixtureA = c.GetFixtureA(); Fixture fixtureB = c.GetFixtureB(); Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); if (ContactListener != null && c.IsTouching()) { ContactListener.EndContact(c); } // Remove from the world. if (c._prev != null) { c._prev._next = c._next; } if (c._next != null) { c._next._prev = c._prev; } if (c == _contactList) { _contactList = c._next; } // Remove from body 1 if (c._nodeA.Prev != null) { c._nodeA.Prev.Next = c._nodeA.Next; } if (c._nodeA.Next != null) { c._nodeA.Next.Prev = c._nodeA.Prev; } if (c._nodeA == bodyA._contactList) { bodyA._contactList = c._nodeA.Next; } // Remove from body 2 if (c._nodeB.Prev != null) { c._nodeB.Prev.Next = c._nodeB.Next; } if (c._nodeB.Next != null) { c._nodeB.Next.Prev = c._nodeB.Prev; } if (c._nodeB == bodyB._contactList) { bodyB._contactList = c._nodeB.Next; } c.Destroy(); --_contactCount; }
// Broad-phase callback. public void AddPair(object proxyUserDataA, object proxyUserDataB) { FixtureProxy proxyA = (FixtureProxy)proxyUserDataA; FixtureProxy proxyB = (FixtureProxy)proxyUserDataB; Fixture fixtureA = proxyA.fixture; Fixture fixtureB = proxyB.fixture; int indexA = proxyA.childIndex; int indexB = proxyB.childIndex; Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); // Are the fixtures on the same body? if (bodyA == bodyB) { return; } // TODO_ERIN use a hash table to remove a potential bottleneck when both // bodies have a lot of contacts. // Does a contact already exist? List <ContactEdge> edges = bodyB.GetContactList(); foreach (ContactEdge edge in edges) { if (edge.other == bodyA) { Fixture fA = edge.contact.FixtureA; Fixture fB = edge.contact.FixtureB; int iA = edge.contact.GetChildIndexA(); int iB = edge.contact.GetChildIndexB(); if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB) { // A contact already exists. return; } if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA) { // A contact already exists. return; } } } // Does a joint override collision? Is at least one body dynamic? if (bodyB.ShouldCollide(bodyA) == false) { return; } // Check user filtering. if ((m_contactFilter != null) && m_contactFilter.ShouldCollide(fixtureA, fixtureB) == false) { return; } // Call the factory. Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB); if (c == null) { return; } // Contact creation may swap fixtures. fixtureA = c.FixtureA; fixtureB = c.FixtureB; indexA = c.GetChildIndexA(); indexB = c.GetChildIndexB(); bodyA = fixtureA.GetBody(); bodyB = fixtureB.GetBody(); // Insert into the world. m_contactList.Add(c); // Connect to island graph. // Connect to body A c.m_nodeA.contact = c; c.m_nodeA.other = bodyB; bodyA.m_contactList.Add(c.m_nodeA); // Connect to body B c.m_nodeB.contact = c; c.m_nodeB.other = bodyA; bodyB.m_contactList.Add(c.m_nodeB); // Wake up the bodies if (fixtureA.IsSensor == false && fixtureB.IsSensor == false) { bodyA.SetAwake(true); bodyB.SetAwake(true); } }
public void Collide() { // Update awake contacts. for (int i = 0; i < m_contactList.Count(); i++) { Contact c = m_contactList[i]; Fixture fixtureA = c.FixtureA; Fixture fixtureB = c.FixtureB; int indexA = c.GetChildIndexA(); int indexB = c.GetChildIndexB(); Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); // Is this contact flagged for filtering? if (c.m_flags.HasFlag(ContactFlags.e_filterFlag)) { throw new NotImplementedException(); //// Should these bodies collide? //if (bodyB.ShouldCollide(bodyA) == false) //{ // Contact cNuke = c; // c = cNuke.GetNext(); // Destroy(cNuke); // continue; //} //// Check user filtering. //if (m_contactFilter && m_contactFilter.ShouldCollide(fixtureA, fixtureB) == false) //{ // Contact* cNuke = c; // c = cNuke.GetNext(); // Destroy(cNuke); // continue; //} //// Clear the filtering flag. //c.m_flags &= ~ContactFlags.e_filterFlag; } bool activeA = bodyA.IsAwake() && bodyA.m_type != BodyType._staticBody; bool activeB = bodyB.IsAwake() && bodyB.m_type != BodyType._staticBody; // At least one body must be awake and it must be dynamic or kinematic. if (activeA == false && activeB == false) { continue; } int proxyIdA = fixtureA.m_proxies[indexA].proxyId; int proxyIdB = fixtureB.m_proxies[indexB].proxyId; bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB); // Here we destroy contacts that cease to overlap in the broad-phase. if (overlap == false) { Destroy(c); continue; } // The contact persists. c.Update(m_contactListener); } }
// Update the contact manifold and touching status. // Note: do not assume the fixture AABBs are overlapping or are valid. internal void Update(ContactListener listener) { Manifold oldManifold = m_manifold; // Re-enable this contact. m_flags |= ContactFlags.e_enabledFlag; bool touching = false; bool wasTouching = (m_flags & ContactFlags.e_touchingFlag) == ContactFlags.e_touchingFlag; bool sensorA = m_fixtureA.IsSensor; bool sensorB = m_fixtureB.IsSensor; bool sensor = sensorA || sensorB; Body bodyA = m_fixtureA.GetBody(); Body bodyB = m_fixtureB.GetBody(); Transform xfA = bodyA.GetTransform(); Transform xfB = bodyB.GetTransform(); // Is this contact a sensor? if (sensor) { Shape shapeA = m_fixtureA.GetShape(); Shape shapeB = m_fixtureB.GetShape(); touching = Collision.TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB); // Sensors don't generate manifolds. m_manifold.points.Clear(); } else { Evaluate(out m_manifold, xfA, xfB); touching = m_manifold.points.Count() > 0; // Match old contact ids to new contact ids and copy the // stored impulses to warm start the solver. for (int i = 0; i < m_manifold.points.Count(); ++i) { ManifoldPoint mp2 = m_manifold.points[i]; mp2.normalImpulse = 0.0f; mp2.tangentImpulse = 0.0f; ContactID id2 = mp2.id; for (int j = 0; j < oldManifold.points.Count(); ++j) { ManifoldPoint mp1 = oldManifold.points[j]; if (mp1.id.key == id2.key) { mp2.normalImpulse = mp1.normalImpulse; mp2.tangentImpulse = mp1.tangentImpulse; break; } } } if (touching != wasTouching) { bodyA.SetAwake(true); bodyB.SetAwake(true); } } if (touching) { m_flags |= ContactFlags.e_touchingFlag; } else { m_flags &= ~ContactFlags.e_touchingFlag; } if (wasTouching == false && touching == true && listener != null) { listener.BeginContact(this); } if (wasTouching == true && touching == false && listener != null) { listener.EndContact(this); } if (sensor == false && touching && listener != null) { listener.PreSolve(this, oldManifold); } }
public void Solve(ref TimeStep step, Vector2 gravity, bool allowSleep) { // Integrate velocities and apply damping. for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; if (b.GetType() != BodyType.Dynamic) { continue; } // Integrate velocities. b._linearVelocity += step.dt * (gravity + b._invMass * b._force); b._angularVelocity += step.dt * b._invI * b._torque; // Apply damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) // v2 = exp(-c * dt) * v1 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 b._linearVelocity *= MathUtils.Clamp(1.0f - step.dt * b._linearDamping, 0.0f, 1.0f); b._angularVelocity *= MathUtils.Clamp(1.0f - step.dt * b._angularDamping, 0.0f, 1.0f); } // Partition contacts so that contacts with static bodies are solved last. int i1 = -1; for (int i2 = 0; i2 < _contactCount; ++i2) { Fixture fixtureA = _contacts[i2].GetFixtureA(); Fixture fixtureB = _contacts[i2].GetFixtureB(); Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); bool nonStatic = bodyA.GetType() != BodyType.Static && bodyB.GetType() != BodyType.Static; if (nonStatic) { ++i1; //b2Swap(_contacts[i1], _contacts[i2]); Contact temp = _contacts[i1]; _contacts[i1] = _contacts[i2]; _contacts[i2] = temp; } } // Initialize velocity constraints. _contactSolver.Reset(_contacts, _contactCount, step.dtRatio); _contactSolver.WarmStart(); for (int i = 0; i < _jointCount; ++i) { _joints[i].InitVelocityConstraints(ref step); } // Solve velocity constraints. for (int i = 0; i < step.velocityIterations; ++i) { for (int j = 0; j < _jointCount; ++j) { _joints[j].SolveVelocityConstraints(ref step); } _contactSolver.SolveVelocityConstraints(); } // Post-solve (store impulses for warm starting). _contactSolver.StoreImpulses(); // Integrate positions. for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; if (b.GetType() == BodyType.Static) { continue; } // Check for large velocities. Vector2 translation = step.dt * b._linearVelocity; if (Vector2.Dot(translation, translation) > Settings.b2_maxTranslationSquared) { float ratio = Settings.b2_maxTranslation / translation.magnitude; b._linearVelocity *= ratio; } float rotation = step.dt * b._angularVelocity; if (rotation * rotation > Settings.b2_maxRotationSquared) { float ratio = Settings.b2_maxRotation / Math.Abs(rotation); b._angularVelocity *= ratio; } // Store positions for continuous collision. b._sweep.c0 = b._sweep.c; b._sweep.a0 = b._sweep.a; // Integrate b._sweep.c += step.dt * b._linearVelocity; b._sweep.a += step.dt * b._angularVelocity; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } // Iterate over constraints. for (int i = 0; i < step.positionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.b2_contactBaumgarte); bool jointsOkay = true; for (int j = 0; j < _jointCount; ++j) { bool jointOkay = _joints[j].SolvePositionConstraints(Settings.b2_contactBaumgarte); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. break; } } Report(_contactSolver._constraints); if (allowSleep) { float minSleepTime = Settings.b2_maxFloat; const float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance; const float angTolSqr = Settings.b2_angularSleepTolerance * Settings.b2_angularSleepTolerance; for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; if (b.GetType() == BodyType.Static) { continue; } if ((b._flags & BodyFlags.AutoSleep) == 0) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } if ((b._flags & BodyFlags.AutoSleep) == 0 || b._angularVelocity * b._angularVelocity > angTolSqr || Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } else { b._sleepTime += step.dt; minSleepTime = Math.Min(minSleepTime, b._sleepTime); } } if (minSleepTime >= Settings.b2_timeToSleep) { for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; b.SetAwake(false); } } } }
public override float ReportFixture(Fixture fixture, Vec2 point, Vec2 normal, float fraction) { Body body = fixture.GetBody(); if (body.UserData != null) { int index = (int)body.UserData; if (index == 0) { // By returning -1, we instruct the calling code to ignore this fixture // and continue the ray-cast to the next fixture. return -1.0f; } } Utilities.Assert(m_count < e_maxCount); m_points[m_count] = point; m_normals[m_count] = normal; ++m_count; if (m_count == e_maxCount) { // At this point the buffer is full. // By returning 0, we instruct the calling code to terminate the ray-cast. return 0.0f; } // By returning 1, we instruct the caller to continue without clipping the ray. return 1.0f; }
private void SolveTOI(TimeStep step) { Island island = new Island(m_contactManager.m_contactListener); if (m_stepComplete) { foreach (Body b in m_bodyList) { b.m_flags &= ~Body.BodyFlags.e_islandFlag; b.m_sweep.alpha0 = 0.0f; } foreach (Contact c in m_contactManager.m_contactList) { // Invalidate TOI c.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag); c.m_toiCount = 0; c.m_toi = 1.0f; } } Fixture fA = null; Fixture fB = null; Body bA = null; Body bB = null; // Find TOI events and solve them. for (;;) { // Find the first TOI. Contact minContact = null; float minAlpha = 1.0f; foreach (Contact c in m_contactManager.m_contactList) { // Is this contact disabled? if (c.IsEnabled() == false) { continue; } // Prevent excessive sub-stepping. if (c.m_toiCount > Settings._maxSubSteps) { continue; } float alpha = 1.0f; if (c.m_flags.HasFlag(ContactFlags.e_toiFlag)) { // This contact has a valid cached TOI. alpha = c.m_toi; } else { fA = c.FixtureA; fB = c.FixtureB; // Is there a sensor? if (fA.IsSensor || fB.IsSensor) { continue; } bA = fA.GetBody(); bB = fB.GetBody(); BodyType typeA = bA.m_type; BodyType typeB = bB.m_type; Utilities.Assert(typeA == BodyType._dynamicBody || typeB == BodyType._dynamicBody); bool activeA = bA.IsAwake() && typeA != BodyType._staticBody; bool activeB = bB.IsAwake() && typeB != BodyType._staticBody; // Is at least one body active (awake and dynamic or kinematic)? if (activeA == false && activeB == false) { continue; } bool collideA = bA.IsBullet() || typeA != BodyType._dynamicBody; bool collideB = bB.IsBullet() || typeB != BodyType._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.m_sweep.alpha0; if (bA.m_sweep.alpha0 < bB.m_sweep.alpha0) { alpha0 = bB.m_sweep.alpha0; bA.m_sweep.Advance(alpha0); } else if (bB.m_sweep.alpha0 < bA.m_sweep.alpha0) { alpha0 = bA.m_sweep.alpha0; bB.m_sweep.Advance(alpha0); } Utilities.Assert(alpha0 < 1.0f); int indexA = c.GetChildIndexA(); int indexB = c.GetChildIndexB(); // Compute the time of impact in interval [0, minTOI] TOIInput input = new TOIInput(); input.proxyA.Set(fA.GetShape(), indexA); input.proxyB.Set(fB.GetShape(), indexB); input.sweepA = bA.m_sweep; input.sweepB = bB.m_sweep; input.tMax = 1.0f; TOIOutput output; Utilities.TimeOfImpact(out output, input); // Beta is the fraction of the remaining portion of the . float beta = output.t; if (output.state == TOIOutput.State.e_touching) { alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); } else { alpha = 1.0f; } c.m_toi = alpha; c.m_flags |= ContactFlags.e_toiFlag; } if (alpha < minAlpha) { // This is the minimum TOI found so far. minContact = c; minAlpha = alpha; } } if (minContact == null || 1.0f - 10.0f * Single.Epsilon < minAlpha) { // No more TOI events. Done! m_stepComplete = true; break; } // Advance the bodies to the TOI. fA = minContact.FixtureA; fB = minContact.FixtureB; bA = fA.GetBody(); bB = fB.GetBody(); Sweep backup1 = bA.m_sweep; Sweep backup2 = bB.m_sweep; bA.Advance(minAlpha); bB.Advance(minAlpha); // The TOI contact likely has some new contact points. minContact.Update(m_contactManager.m_contactListener); minContact.m_flags &= ~ContactFlags.e_toiFlag; ++minContact.m_toiCount; // Is the contact solid? if (minContact.IsEnabled() == false || minContact.IsTouching() == false) { // Restore the sweeps. minContact.SetEnabled(false); bA.m_sweep = backup1; bB.m_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.m_flags |= Body.BodyFlags.e_islandFlag; bB.m_flags |= Body.BodyFlags.e_islandFlag; minContact.m_flags |= ContactFlags.e_islandFlag; // Get contacts on bodyA and bodyB. Body[] bodies = { bA, bB }; for (int i = 0; i < 2; ++i) { Body body = bodies[i]; if (body.m_type == BodyType._dynamicBody) { foreach (ContactEdge ce in body.m_contactList) { throw new NotImplementedException(); //if (island.m_bodies.Count() == island.m_bodyCapacity) //{ // break; //} //if (island.m_bodies.Count() == island.m_contactCapacity) //{ // break; //} //Contact* contact = ce.contact; //// Has this contact already been added to the island? //if (contact.m_flags & ContactFlags.e_islandFlag) //{ // continue; //} //// Only add static, kinematic, or bullet bodies. //Body* other = ce.other; //if (other.m_type == _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. //Sweep backup = other.m_sweep; //if ((other.m_flags & Body.BodyFlags.e_islandFlag) == 0) //{ // other.Advance(minAlpha); //} //// Update the contact points //contact.Update(m_contactManager.m_contactListener); //// Was the contact disabled by the user? //if (contact.IsEnabled() == false) //{ // other.m_sweep = backup; // other.SynchronizeTransform(); // continue; //} //// Are there contact points? //if (contact.IsTouching() == false) //{ // other.m_sweep = backup; // other.SynchronizeTransform(); // continue; //} //// Add the contact to the island //contact.m_flags |= ContactFlags.e_islandFlag; //island.Add(contact); //// Has the other body already been added to the island? //if (other.m_flags & Body.BodyFlags.e_islandFlag) //{ // continue; //} //// Add the other body to the island. //other.m_flags |= Body.BodyFlags.e_islandFlag; //if (other.m_type != _staticBody) //{ // other.SetAwake(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(subStep, bA.m_islandIndex, bB.m_islandIndex); // Reset island flags and synchronize broad-phase proxies. for (int i = 0; i < island.m_bodies.Count(); ++i) { throw new NotImplementedException(); //Body* body = island.m_bodies[i]; //body.m_flags &= ~Body.BodyFlags.e_islandFlag; //if (body.m_type != _dynamicBody) //{ // continue; //} //body.SynchronizeFixtures(); //// Invalidate all contact TOIs on this displaced body. //for (ContactEdge* ce = body.m_contactList; ce; ce = ce.next) //{ // ce.contact.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.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; } } }
// Broad-phase callback. internal void AddPair(FixtureProxy proxyA, FixtureProxy proxyB) { Fixture fixtureA = proxyA.fixture; Fixture fixtureB = proxyB.fixture; int indexA = proxyA.childIndex; int indexB = proxyB.childIndex; Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); // Are the fixtures on the same body? if (bodyA == bodyB) { return; } // Does a contact already exist? ContactEdge edge = bodyB.GetContactList(); while (edge != null) { if (edge.Other == bodyA) { Fixture fA = edge.Contact.GetFixtureA(); Fixture fB = edge.Contact.GetFixtureB(); int iA = edge.Contact.GetChildIndexA(); int iB = edge.Contact.GetChildIndexB(); if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB) { // A contact already exists. return; } if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA) { // A contact already exists. return; } } edge = edge.Next; } // Does a joint override collision? Is at least one body dynamic? if (bodyB.ShouldCollide(bodyA) == false) { return; } // Check user filtering. if (ContactFilter != null && ContactFilter.ShouldCollide(fixtureA, fixtureB) == false) { return; } // Call the factory. Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB); // Contact creation may swap fixtures. fixtureA = c.GetFixtureA(); fixtureB = c.GetFixtureB(); indexA = c.GetChildIndexA(); indexB = c.GetChildIndexB(); bodyA = fixtureA.GetBody(); bodyB = fixtureB.GetBody(); // Insert into the world. c._prev = null; c._next = _contactList; if (_contactList != null) { _contactList._prev = c; } _contactList = c; // Connect to island graph. // Connect to body A c._nodeA.Contact = c; c._nodeA.Other = bodyB; c._nodeA.Prev = null; c._nodeA.Next = bodyA._contactList; if (bodyA._contactList != null) { bodyA._contactList.Prev = c._nodeA; } bodyA._contactList = c._nodeA; // Connect to body B c._nodeB.Contact = c; c._nodeB.Other = bodyA; c._nodeB.Prev = null; c._nodeB.Next = bodyB._contactList; if (bodyB._contactList != null) { bodyB._contactList.Prev = c._nodeB; } bodyB._contactList = c._nodeB; ++_contactCount; }
public void Reset(Contact[] contacts, int contactCount, float impulseRatio) { _contacts = contacts; _constraintCount = contactCount; // grow the array if (_constraints == null || _constraints.Length < _constraintCount) { _constraints = new ContactConstraint[_constraintCount * 2]; } for (int i = 0; i < _constraintCount; ++i) { Contact contact = contacts[i]; Fixture fixtureA = contact._fixtureA; Fixture fixtureB = contact._fixtureB; Shape shapeA = fixtureA.GetShape(); Shape shapeB = fixtureB.GetShape(); float radiusA = shapeA._radius; float radiusB = shapeB._radius; Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); Manifold manifold; contact.GetManifold(out manifold); float friction = Settings.b2MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction()); float restitution = Settings.b2MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution()); Vector2 vA = bodyA._linearVelocity; Vector2 vB = bodyB._linearVelocity; float wA = bodyA._angularVelocity; float wB = bodyB._angularVelocity; //Debug.Assert(manifold._pointCount > 0); WorldManifold worldManifold = new WorldManifold(ref manifold, ref bodyA._xf, radiusA, ref bodyB._xf, radiusB); ContactConstraint cc = _constraints[i]; cc.bodyA = bodyA; cc.bodyB = bodyB; cc.manifold = manifold; cc.normal = worldManifold._normal; cc.pointCount = manifold._pointCount; cc.friction = friction; cc.localNormal = manifold._localNormal; cc.localPoint = manifold._localPoint; cc.radius = radiusA + radiusB; cc.type = manifold._type; for (int j = 0; j < cc.pointCount; ++j) { ManifoldPoint cp = manifold._points[j]; ContactConstraintPoint ccp = cc.points[j]; ccp.normalImpulse = impulseRatio * cp.NormalImpulse; ccp.tangentImpulse = impulseRatio * cp.TangentImpulse; ccp.localPoint = cp.LocalPoint; ccp.rA = worldManifold._points[j] - bodyA._sweep.c; ccp.rB = worldManifold._points[j] - bodyB._sweep.c; #if MATH_OVERLOADS float rnA = MathUtils.Cross(ccp.rA, cc.normal); float rnB = MathUtils.Cross(ccp.rB, cc.normal); #else float rnA = ccp.rA.x * cc.normal.y - ccp.rA.y * cc.normal.x; float rnB = ccp.rB.x * cc.normal.y - ccp.rB.y * cc.normal.x; #endif rnA *= rnA; rnB *= rnB; float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB; //Debug.Assert(kNormal > Settings.b2_epsilon); ccp.normalMass = 1.0f / kNormal; #if MATH_OVERLOADS Vector2 tangent = MathUtils.Cross(cc.normal, 1.0f); float rtA = MathUtils.Cross(ccp.rA, tangent); float rtB = MathUtils.Cross(ccp.rB, tangent); #else Vector2 tangent = new Vector2(cc.normal.y, -cc.normal.x); float rtA = ccp.rA.x * tangent.y - ccp.rA.y * tangent.x; float rtB = ccp.rB.x * tangent.y - ccp.rB.y * tangent.x; #endif rtA *= rtA; rtB *= rtB; float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB; //Debug.Assert(kTangent > Settings.b2_epsilon); ccp.tangentMass = 1.0f / kTangent; // Setup a velocity bias for restitution. ccp.velocityBias = 0.0f; float vRel = Vector2.Dot(cc.normal, vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA)); if (vRel < -Settings.b2_velocityThreshold) { ccp.velocityBias = -restitution * vRel; } cc.points[j] = ccp; } // If we have two points, then prepare the block solver. if (cc.pointCount == 2) { ContactConstraintPoint ccp1 = cc.points[0]; ContactConstraintPoint ccp2 = cc.points[1]; float invMassA = bodyA._invMass; float invIA = bodyA._invI; float invMassB = bodyB._invMass; float invIB = bodyB._invI; float rn1A = MathUtils.Cross(ccp1.rA, cc.normal); float rn1B = MathUtils.Cross(ccp1.rB, cc.normal); float rn2A = MathUtils.Cross(ccp2.rA, cc.normal); float rn2B = MathUtils.Cross(ccp2.rB, cc.normal); float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; // Ensure a reasonable condition number. const float k_maxConditionNumber = 100.0f; if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. cc.K = new Mat22(new Vector2(k11, k12), new Vector2(k12, k22)); cc.normalMass = cc.K.GetInverse(); } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? cc.pointCount = 1; } } _constraints[i] = cc; } }
public ContactSolver(ContactSolverDef def) { m_step = def.step; m_positionConstraints = new List <ContactPositionConstraint>(); m_velocityConstraints = new List <ContactVelocityConstraint>(); m_positions = def.positions; m_velocities = def.velocities; m_contacts = def.contacts; // Initialize position independent portions of the constraints. for (int i = 0; i < def.contacts.Count(); ++i) { Contact contact = m_contacts[i]; Fixture fixtureA = contact.m_fixtureA; Fixture fixtureB = contact.m_fixtureB; Shape shapeA = fixtureA.GetShape(); Shape shapeB = fixtureB.GetShape(); float radiusA = shapeA.m_radius; float radiusB = shapeB.m_radius; Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); Manifold manifold = contact.GetManifold(); int pointCount = manifold.points.Count(); Utilities.Assert(pointCount > 0); ContactVelocityConstraint vc = new ContactVelocityConstraint(); vc.friction = contact.m_friction; vc.restitution = contact.m_restitution; vc.tangentSpeed = contact.m_tangentSpeed; vc.indexA = bodyA.m_islandIndex; vc.indexB = bodyB.m_islandIndex; vc.invMassA = bodyA.m_invMass; vc.invMassB = bodyB.m_invMass; vc.invIA = bodyA.m_invI; vc.invIB = bodyB.m_invI; vc.contactIndex = i; //vc.points.Count() = pointCount; vc.K.SetZero(); vc.normalMass.SetZero(); ContactPositionConstraint pc = new ContactPositionConstraint(); pc.indexA = bodyA.m_islandIndex; pc.indexB = bodyB.m_islandIndex; pc.invMassA = bodyA.m_invMass; pc.invMassB = bodyB.m_invMass; pc.localCenterA = bodyA.m_sweep.localCenter; pc.localCenterB = bodyB.m_sweep.localCenter; pc.invIA = bodyA.m_invI; pc.invIB = bodyB.m_invI; pc.localNormal = manifold.localNormal; pc.localPoint = manifold.localPoint; pc.pointCount = pointCount; pc.radiusA = radiusA; pc.radiusB = radiusB; pc.type = manifold.type; for (int j = 0; j < pointCount; ++j) { ManifoldPoint cp = manifold.points[j]; VelocityConstraintPoint vcp = new VelocityConstraintPoint(); if (m_step.warmStarting) { vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse; vcp.tangentImpulse = m_step.dtRatio * cp.tangentImpulse; } else { vcp.normalImpulse = 0.0f; vcp.tangentImpulse = 0.0f; } vcp.rA.SetZero(); vcp.rB.SetZero(); vcp.normalMass = 0.0f; vcp.tangentMass = 0.0f; vcp.velocityBias = 0.0f; vc.points.Add(vcp); pc.localPoints[j] = cp.localPoint; } m_velocityConstraints.Add(vc); m_positionConstraints.Add(pc); } }