// Add a new contact constraint for a pair of objects // unless the contact constraint already exists public void AddContact(Box A, Box B) { Body bodyA = A.body; Body bodyB = B.body; if (!bodyA.CanCollide(bodyB)) { return; } // Search for existing matching contact // Return if found duplicate to avoid duplicate constraints // Mark pre-existing duplicates as active foreach (var edge in A.body.ContactList) { if (edge.other == bodyB) { Box shapeA = edge.constraint.A; Box shapeB = edge.constraint.B; // @TODO: Verify this against Box2D; not sure if this is all we need here if ((A == shapeA) && (B == shapeB)) { return; } } } // Create new contact ContactConstraint contact = new ContactConstraint() { A = A, B = B, bodyA = A.body, bodyB = B.body, Flags = 0, friction = MixFriction(A, B), restitution = MixRestitution(A, B), }; contact.manifold.SetPair(A, B); ContactList.Add(contact); // Connect A contact.edgeA.constraint = contact; contact.edgeA.other = bodyB; bodyA.ContactList.Add(contact.edgeA); // Connect B contact.edgeB.constraint = contact; contact.edgeB.other = bodyA; bodyB.ContactList.Add(contact.edgeB); bodyA.SetToAwake(); bodyB.SetToAwake(); }
// Remove a specific contact public void RemoveContact(ContactConstraint contact) { Body A = contact.bodyA; Body B = contact.bodyB; // Remove from A A.ContactList.Remove(contact.edgeA); // Remove from B B.ContactList.Remove(contact.edgeB); A.SetToAwake(); B.SetToAwake(); // Remove contact from the manager ContactList.Remove(contact); }
// Run the simulation forward in time by dt (fixed timestep). Variable // timestep is not supported. public void Step(double Dt) { if (NewBox) { ContactManager.Broadphase.UpdatePairs(); NewBox = false; } ContactManager.TestCollisions(); foreach (var body in Bodies) { body.Flags &= ~BodyFlags.eIsland; } Island.AllowSleep = AllowSleep; Island.EnableFriction = EnableFriction; Island.Dt = Dt; Island.Gravity = Gravity; Island.Iterations = Iterations; // Build each active Island and then solve each built Island // int stackSize = Bodies.Count; foreach (var seed in Bodies) { // Seed cannot be apart of an Island already if ((seed.Flags & BodyFlags.eIsland) > 0) { continue; } // Seed must be awake if ((seed.Flags & BodyFlags.eAwake) == 0) { continue; } // Seed cannot be a static body in order to keep islands // as small as possible if ((seed.Flags & BodyFlags.eStatic) > 0) { continue; } int stackCount = 0; stack[stackCount++] = seed; Island.Clear(); // Mark seed as apart of Island seed.Flags |= BodyFlags.eIsland; // Perform DFS on constraint graph while (stackCount > 0) { // Decrement stack to implement iterative backtracking Body body = stack[--stackCount]; Island.Add(body); // Awaken all bodies connected to the Island body.SetToAwake(); // Do not search across static bodies to keep Island // formations as small as possible, however the static // body itself should be apart of the Island in order // to properly represent a full contact if ((body.Flags & BodyFlags.eStatic) > 0) { continue; } // Search all contacts connected to this body foreach (var edge in body.ContactList) { ContactConstraint contact = edge.constraint; // Skip contacts that have been added to an Island already if ((contact.Flags & ContactFlags.eIsland) > 0) { continue; } // Can safely skip this contact if it didn't actually collide with anything if ((contact.Flags & ContactFlags.eColliding) == 0) { continue; } // Skip sensors if (contact.A.sensor || contact.B.sensor) { continue; } // Mark Island flag and add to Island contact.Flags |= ContactFlags.eIsland; Island.Add(contact); // Attempt to add the other body in the contact to the Island // to simulate contact awakening propogation Body other = edge.other; if ((other.Flags & BodyFlags.eIsland) > 0) { continue; } Assert(stackCount < 256); stack[stackCount++] = other; other.Flags |= BodyFlags.eIsland; } } Assert(Island.Bodies.Count != 0); Island.Initialize(); Island.Solve(); // Reset all static Island flags // This allows static bodies to participate in other Island formations foreach (var body in Island.Bodies) { if ((body.Flags & BodyFlags.eStatic) > 0) { body.Flags &= ~BodyFlags.eIsland; } } } // Update the broadphase AABBs foreach (var body in Bodies) { if ((body.Flags & BodyFlags.eStatic) > 0) { continue; } body.SynchronizeProxies(); } // Look for new contacts ContactManager.FindNewContacts(); // Clear all forces foreach (var body in Bodies) { Vec3.Identity(ref body.Force); Vec3.Identity(ref body.Torque); } }