// Removes this box from the body and broadphase. Forces the body
// to recompute its mass if the body is dynamic. Frees the memory
// pointed to by the box pointer.
public void RemoveBox(Box box)
{
Assert(box != null);
Assert(box.body == this);
bool found = Boxes.Remove(box);
// This shape was not connected to this body.
Assert(found);
// Remove all contacts associated with this shape
foreach (var edge in ContactList)
{
ContactConstraint contact = edge.constraint;
Box A = contact.A;
Box B = contact.B;
if (box == A || box == B)
{
Scene.ContactManager.RemoveContact(contact);
}
}
Scene.ContactManager.Broadphase.RemoveBox(box);
CalculateMassData();
// Scene.Heap.Free((void)box);
}
// 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();
}
public void ShutDown()
{
for (int i = 0; i < Contacts.Count; ++i)
{
ContactConstraintState c = Contacts[i];
ContactConstraint cc = Island.Contacts[i];
for (int j = 0; j < c.contactCount; ++j)
{
Contact oc = cc.manifold.contacts[j];
ContactState cs = c.contacts[j];
oc.normalImpulse = cs.normalImpulse;
oc.tangentImpulse = cs.tangentImpulse;
oc.bitangentImpulse = cs.bitangentImpulse;
}
}
}
// 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);
}
public void Initialize()
{
for (int i = 0; i < Contacts.Count; ++i)
{
ContactConstraint cc = Contacts[i];
ContactConstraintState c = ContactStates[i];
c.centerA = cc.bodyA.WorldCenter;
c.centerB = cc.bodyB.WorldCenter;
c.iA = cc.bodyA.InvInertiaWorld;
c.iB = cc.bodyB.InvInertiaWorld;
c.mA = cc.bodyA.InvMass;
c.mB = cc.bodyB.InvMass;
c.restitution = cc.restitution;
c.friction = cc.friction;
c.indexA = cc.bodyA.IslandIndex;
c.indexB = cc.bodyB.IslandIndex;
c.normal = cc.manifold.normal;
c.tangentVectors = cc.manifold.tangentVectors;
c.bitangentVectors = cc.manifold.bitangentVectors;
c.contactCount = cc.manifold.contactCount;
for (int j = 0; j < c.contactCount; ++j)
{
ContactState s = c.contacts[j] = ContactState.Allocate();
Contact cp = cc.manifold.contacts[j];
s.ra = cp.position - c.centerA;
s.rb = cp.position - c.centerB;
s.penetration = cp.penetration;
s.normalImpulse = cp.normalImpulse;
s.tangentImpulse = cp.tangentImpulse;
s.bitangentImpulse = cp.bitangentImpulse;
}
}
}
// 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);
}
}
public abstract void EndContact(ContactConstraint contact);
public abstract void BeginContact(ContactConstraint contact);
public void Add(ContactConstraint contact)
{
Contacts.Add(contact);
ContactStates.Add(ContactConstraintState.Allocate());
}
