/// Destroy a joint. This may cause the connected bodies to begin colliding.
/// @warning This function is locked during callbacks.
public void DestroyJoint(Joint j)
{
Debug.Assert(!IsLocked);
if (IsLocked)
{
return;
}
bool collideConnected = j._collideConnected;
// Remove from the doubly linked list.
if (j._prev != null)
{
j._prev._next = j._next;
}
if (j._next != null)
{
j._next._prev = j._prev;
}
if (j == _jointList)
{
_jointList = j._next;
}
// Disconnect from island graph.
Body bodyA = j._bodyA;
Body bodyB = j._bodyB;
// Wake up connected bodies.
bodyA.WakeUp();
bodyB.WakeUp();
// Remove from body 1.
if (j._edgeA.Prev != null)
{
j._edgeA.Prev.Next = j._edgeA.Next;
}
if (j._edgeA.Next != null)
{
j._edgeA.Next.Prev = j._edgeA.Prev;
}
if (j._edgeA == bodyA._jointList)
{
bodyA._jointList = j._edgeA.Next;
}
j._edgeA.Prev = null;
j._edgeA.Next = null;
// Remove from body 2
if (j._edgeB.Prev != null)
{
j._edgeB.Prev.Next = j._edgeB.Next;
}
if (j._edgeB.Next != null)
{
j._edgeB.Next.Prev = j._edgeB.Prev;
}
if (j._edgeB == bodyB._jointList)
{
bodyB._jointList = j._edgeB.Next;
}
j._edgeB.Prev = null;
j._edgeB.Next = null;
Debug.Assert(_jointCount > 0);
--_jointCount;
// If the joint prevents collisions, then flag any contacts for filtering.
if (collideConnected == false)
{
ContactEdge edge = bodyB.GetConactList();
while (edge != null)
{
if (edge.Other == bodyA)
{
// Flag the contact for filtering at the next time step (where either
// body is awake).
edge.Contact.FlagForFiltering();
}
edge = edge.Next;
}
}
}
void SolveTOI(ref TimeStep step)
{
// Reserve an island and a queue for TOI island solution.
_island.Reset(_bodyCount,
Settings.b2_maxTOIContactsPerIsland,
Settings.b2_maxTOIJointsPerIsland,
_contactManager.ContactListener);
//Simple one pass queue
//Relies on the fact that we're only making one pass
//through and each body can only be pushed/popped once.
//To push:
// queue[queueStart+queueSize++] = newElement;
//To pop:
// poppedElement = queue[queueStart++];
// --queueSize;
//#warning More Body array Allocs
int queueCapacity = _bodyCount;
Body[] queue = new Body[_bodyCount];
for (Body b = _bodyList; b != null; b = b._next)
{
b._flags &= ~BodyFlags.Island;
b._sweep.t0 = 0.0f;
}
for (Contact c = _contactManager._contactList; c != null; c = c._next)
{
// Invalidate TOI
c._flags &= ~(ContactFlags.Toi | ContactFlags.Island);
}
for (Joint j = _jointList; j != null; j = j._next)
{
j._islandFlag = false;
}
// Find TOI events and solve them.
for (;;)
{
// Find the first TOI.
Contact minContact = null;
float minTOI = 1.0f;
for (Contact c = _contactManager._contactList; c != null; c = c._next)
{
if ((c._flags & (ContactFlags.Slow | ContactFlags.NonSolid)) != ContactFlags.None)
{
continue;
}
// TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.
float toi = 1.0f;
if ((c._flags & ContactFlags.Toi) != ContactFlags.None)
{
// This contact has a valid cached TOI.
toi = c._toi;
}
else
{
// Compute the TOI for this contact.
Fixture s1 = c.GetFixtureA();
Fixture s2 = c.GetFixtureB();
Body b1 = s1.GetBody();
Body b2 = s2.GetBody();
if ((b1.IsStatic || b1.IsSleeping) && (b2.IsStatic || b2.IsSleeping))
{
continue;
}
// Put the sweeps onto the same time interval.
float t0 = b1._sweep.t0;
if (b1._sweep.t0 < b2._sweep.t0)
{
t0 = b2._sweep.t0;
b1._sweep.Advance(t0);
}
else if (b2._sweep.t0 < b1._sweep.t0)
{
t0 = b1._sweep.t0;
b2._sweep.Advance(t0);
}
Debug.Assert(t0 < 1.0f);
// Compute the time of impact.
toi = c.ComputeTOI(ref b1._sweep, ref b2._sweep);
//CalculateTimeOfImpact(c._fixtureA.GetShape(), b1._sweep, c._fixtureB.GetShape(), b2._sweep);
Debug.Assert(0.0f <= toi && toi <= 1.0f);
// If the TOI is in range ...
if (0.0f < toi && toi < 1.0f)
{
// Interpolate on the actual range.
toi = Math.Min((1.0f - toi) * t0 + toi, 1.0f);
}
c._toi = toi;
c._flags |= ContactFlags.Toi;
}
if (Settings.b2_FLT_EPSILON < toi && toi < minTOI)
{
// This is the minimum TOI found so far.
minContact = c;
minTOI = toi;
}
}
if (minContact == null || 1.0f - 100.0f * Settings.b2_FLT_EPSILON < minTOI)
{
// No more TOI events. Done!
break;
}
// Advance the bodies to the TOI.
Fixture s1_2 = minContact.GetFixtureA();
Fixture s2_2 = minContact.GetFixtureB();
Body b1_2 = s1_2.GetBody();
Body b2_2 = s2_2.GetBody();
b1_2.Advance(minTOI);
b2_2.Advance(minTOI);
// The TOI contact likely has some new contact points.
minContact.Update(_contactManager.ContactListener);
minContact._flags &= ~ContactFlags.Toi;
if ((minContact._flags & ContactFlags.Touch) == 0)
{
// This shouldn't happen. Numerical error?
//Debug.Assert(false);
continue;
}
// Build the TOI island. We need a dynamic seed.
Body seed = b1_2;
if (seed.IsStatic)
{
seed = b2_2;
}
// Reset island and queue.
_island.Clear();
int queueStart = 0; // starting index for queue
int queueSize = 0; // elements in queue
queue[queueStart + queueSize++] = seed;
seed._flags |= BodyFlags.Island;
// Perform a breadth first search (BFS) on the contact/joint graph.
while (queueSize > 0)
{
// Grab the next body off the stack and add it to the island.
Body b = queue[queueStart++];
--queueSize;
_island.Add(b);
// Make sure the body is awake.
b._flags &= ~BodyFlags.Sleep;
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b.IsStatic)
{
continue;
}
// Search all contacts connected to this body.
for (ContactEdge cEdge = b._contactList; cEdge != null; cEdge = cEdge.Next)
{
// Does the TOI island still have space for contacts?
if (_island._contacts.Count == _island._contactCapacity)
{
continue;
}
// Has this contact already been added to an island? Skip slow or non-solid contacts.
if ((cEdge.Contact._flags & (ContactFlags.Island | ContactFlags.Slow | ContactFlags.NonSolid)) != ContactFlags.None)
{
continue;
}
// Is this contact touching? For performance we are not updating this contact.
if ((cEdge.Contact._flags & ContactFlags.Touch) == 0)
{
continue;
}
_island.Add(cEdge.Contact);
cEdge.Contact._flags |= ContactFlags.Island;
// Update other body.
Body other = cEdge.Other;
// Was the other body already added to this island?
if ((other._flags & BodyFlags.Island) != BodyFlags.None)
{
continue;
}
// March forward, this can do no harm since this is the min TOI.
if (other.IsStatic == false)
{
other.Advance(minTOI);
other.WakeUp();
}
Debug.Assert(queueStart + queueSize < queueCapacity);
queue[queueStart + queueSize] = other;
++queueSize;
other._flags |= BodyFlags.Island;
}
for (JointEdge jEdge = b._jointList; jEdge != null; jEdge = jEdge.Next)
{
if (_island._jointCount == _island._jointCapacity)
{
continue;
}
if (jEdge.Joint._islandFlag == true)
{
continue;
}
_island.Add(jEdge.Joint);
jEdge.Joint._islandFlag = true;
Body other = jEdge.Other;
if ((other._flags & BodyFlags.Island) != BodyFlags.None)
{
continue;
}
if (!other.IsStatic)
{
other.Advance(minTOI);
other.WakeUp();
}
Debug.Assert(queueStart + queueSize < queueCapacity);
queue[queueStart + queueSize] = other;
++queueSize;
other._flags |= BodyFlags.Island;
}
}
TimeStep subStep;
subStep.warmStarting = false;
subStep.dt = (1.0f - minTOI) * step.dt;
subStep.inv_dt = 1.0f / subStep.dt;
subStep.dtRatio = 0.0f;
subStep.velocityIterations = step.velocityIterations;
subStep.positionIterations = step.positionIterations;
_island.SolveTOI(ref subStep);
// Post solve cleanup.
for (int i = 0; i < _island._bodyCount; ++i)
{
// Allow bodies to participate in future TOI islands.
Body b = _island._bodies[i];
b._flags &= ~BodyFlags.Island;
if ((b._flags & (BodyFlags.Sleep | BodyFlags.Frozen)) != BodyFlags.None)
{
continue;
}
if (b.IsStatic)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
// Invalidate all contact TOIs associated with this body. Some of these
// may not be in the island because they were not touching.
for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next)
{
ce.Contact._flags &= ~ContactFlags.Toi;
}
}
int contactCount = _island._contacts.Count;
for (int i = 0; i < contactCount; ++i)
{
// Allow contacts to participate in future TOI islands.
Contact c = _island._contacts[i];
c._flags &= ~(ContactFlags.Toi | ContactFlags.Island);
}
for (int i = 0; i < _island._jointCount; ++i)
{
// Allow joints to participate in future TOI islands.
Joint j = _island._joints[i];
j._islandFlag = false;
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
_contactManager.FindNewContacts();
}
}
internal void Update(IContactListener listener)
{
Manifold oldManifold = _manifold;
Evaluate();
Body bodyA = _fixtureA.GetBody();
Body bodyB = _fixtureB.GetBody();
int oldCount = oldManifold._pointCount;
int newCount = _manifold._pointCount;
if (newCount == 0 && oldCount > 0)
{
bodyA.WakeUp();
bodyB.WakeUp();
}
// Slow contacts don't generate TOI events.
if (bodyA.IsStatic || bodyA.IsBullet || bodyB.IsStatic || bodyB.IsBullet)
{
_flags &= ~ContactFlags.Slow;
}
else
{
_flags |= ContactFlags.Slow;
}
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < _manifold._pointCount; ++i)
{
ManifoldPoint mp2 = _manifold._points[i];
mp2.NormalImpulse = 0.0f;
mp2.TangentImpulse = 0.0f;
ContactID id2 = mp2.Id;
for (int j = 0; j < oldManifold._pointCount; ++j)
{
ManifoldPoint mp1 = oldManifold._points[j];
if (mp1.Id.Key == id2.Key)
{
mp2.NormalImpulse = mp1.NormalImpulse;
mp2.TangentImpulse = mp1.TangentImpulse;
break;
}
}
_manifold._points[i] = mp2;
}
if (oldCount == 0 && newCount > 0)
{
_flags |= ContactFlags.Touch;
listener.BeginContact(this);
}
if (oldCount > 0 && newCount == 0)
{
_flags &= ~ContactFlags.Touch;
listener.EndContact(this);
}
if ((_flags & ContactFlags.NonSolid) == 0)
{
listener.PreSolve(this, ref oldManifold);
// The user may have disabled contact.
if (_manifold._pointCount == 0)
{
_flags &= ~ContactFlags.Touch;
}
}
}
