/// Get the world manifold.
public void GetWorldManifold(out WorldManifold worldManifold)
{
worldManifold = new WorldManifold();
Body bodyA = _fixtureA.GetBody();
Body bodyB = _fixtureB.GetBody();
Shape shapeA = _fixtureA.GetShape();
Shape shapeB = _fixtureB.GetShape();
worldManifold.Initialize(Manifold, bodyA.GetTransform(), shapeA._radius, bodyB.GetTransform(), shapeB._radius);
}
public Contact(Fixture fixtureA, Fixture fixtureB)
{
Flags = 0;
if (fixtureA.IsSensor || fixtureB.IsSensor)
{
Flags |= ContactFlag.SensorFlag;
}
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
if (bodyA.IsStatic() || bodyA.IsBullet() || bodyB.IsStatic() || bodyB.IsBullet())
{
Flags |= ContactFlag.ContinuousFlag;
}
else
{
Flags &= ~ContactFlag.ContinuousFlag;
}
_fixtureA = fixtureA;
_fixtureB = fixtureB;
Manifold = new Manifold();
Manifold.PointCount = 0;
Prev = null;
Next = null;
NodeA = new ContactEdge();
NodeA.Contact = null;
NodeA.Prev = null;
NodeA.Next = null;
NodeA.Other = null;
NodeB = new ContactEdge();
NodeB.Contact = null;
NodeB.Prev = null;
NodeB.Next = null;
NodeB.Other = null;
}
public void Destroy(Contact c)
{
Fixture fixtureA = c.GetFixtureA();
Fixture fixtureB = c.GetFixtureB();
Body body1 = fixtureA.GetBody();
Body body2 = fixtureB.GetBody();
if (c.Manifold.PointCount > 0)
{
_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 == body1._contactList)
{
body1._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 == body2._contactList)
{
body2._contactList = c.NodeB.Next;
}
// Call the factory.
Contact.Destroy(c);
--_contactCount;
}
public void AddPair(object proxyUserDataA, object proxyUserDataB)
{
Fixture fixtureA = (Fixture)proxyUserDataA;
Fixture fixtureB = (Fixture)proxyUserDataB;
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// Are both bodies static?
if (bodyA.IsStatic() && bodyB.IsStatic())
{
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();
if (fA == fixtureA && fB == fixtureB)
{
// A contact already exists.
return;
}
if (fA == fixtureB && fB == fixtureA)
{
// A contact already exists.
return;
}
}
edge = edge.Next;
}
// Does a joint override collision?
if (bodyB.IsConnected(bodyA))
{
return;
}
// Check user filtering.
if (_contactFilter.ShouldCollide(fixtureA, fixtureB) == false)
{
return;
}
// Call the factory.
Contact c = Contact.Create(fixtureA, fixtureB);
// Contact creation may swap fixtures.
fixtureA = c.GetFixtureA();
fixtureB = c.GetFixtureB();
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;
}
// This is the top level collision call for the time step. Here
// all the narrow phase collision is processed for the world
// contact list.
public void Collide()
{
// Update awake contacts.
Contact c = _contactList;
while (c != null)
{
Fixture fixtureA = c.GetFixtureA();
Fixture fixtureB = c.GetFixtureB();
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
if (bodyA.IsSleeping() && bodyB.IsSleeping())
{
c = c.GetNext();
continue;
}
// Is this contact flagged for filtering?
if ((c.Flags & ContactFlag.FilterFlag) == ContactFlag.FilterFlag)
{
//TODO: The following code (next 4 if blocks) use a class and thus copy by ref. It might expect to copy by value
// Are both bodies static?
if (bodyA.IsStatic() && bodyB.IsStatic())
{
Contact cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
// Does a joint override collision?
if (bodyB.IsConnected(bodyA))
{
Contact cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
// Check user filtering.
if (_contactFilter.ShouldCollide(fixtureA, fixtureB) == false)
{
Contact cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
// Clear the filtering flag.
c.Flags &= ~ContactFlag.FilterFlag;
}
int proxyIdA = fixtureA.ProxyId;
int proxyIdB = fixtureB.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();
}
}
public Contact(Fixture fixtureA, Fixture fixtureB)
{
Flags = 0;
if (fixtureA.IsSensor || fixtureB.IsSensor)
{
Flags |= ContactFlag.SensorFlag;
}
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
if (bodyA.IsStatic() || bodyA.IsBullet() || bodyB.IsStatic() || bodyB.IsBullet())
{
Flags |= ContactFlag.ContinuousFlag;
}
else
{
Flags &= ~ContactFlag.ContinuousFlag;
}
_fixtureA = fixtureA;
_fixtureB = fixtureB;
Manifold = new Manifold();
Manifold.PointCount = 0;
Prev = null;
Next = null;
NodeA = new ContactEdge();
NodeA.Contact = null;
NodeA.Prev = null;
NodeA.Next = null;
NodeA.Other = null;
NodeB = new ContactEdge();
NodeB.Contact = null;
NodeB.Prev = null;
NodeB.Next = null;
NodeB.Other = null;
}
// Find TOI contacts and solve them.
private void SolveTOI(TimeStep step)
{
// Reserve an island and a queue for TOI island solution.
Island island = new Island(_bodyCount,
Settings.MaxTOIContactsPerIsland,
Settings.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;
int queueCapacity = _bodyCount;
Body[] queue = new Body[queueCapacity];
for (Body b = _bodyList; b != null; b = b._next)
{
b._flags &= ~Body.BodyFlags.Island;
b._sweep.T0 = 0.0f;
}
for (Contact c = _contactManager._contactList; c != null; c = c.Next)
{
// Invalidate TOI
c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag);
}
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)
{
// Can this contact generate a solid TOI contact?
if (c.IsSolid() == false || c.IsContinuous() == false)
{
continue;
}
// TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.
float toi = 1.0f;
if ((c.Flags & ContactFlag.ToiFlag) != 0)
{
// 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);
}
Box2DXDebug.Assert(t0 < 1.0f);
// Compute the time of impact.
toi = c.ComputeTOI(b1._sweep, b2._sweep);
Box2DXDebug.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 |= ContactFlag.ToiFlag;
}
if (Settings.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.FLT_EPSILON < minTOI)
{
// No more TOI events. Done!
break;
}
// Advance the bodies to the TOI.
Fixture f1 = minContact.GetFixtureA();
Fixture f2 = minContact.GetFixtureB();
Body b3 = f1.GetBody();
Body b4 = f2.GetBody();
Sweep backup1 = b3._sweep;
Sweep backup2 = b4._sweep;
b3.Advance(minTOI);
b4.Advance(minTOI);
// The TOI contact likely has some new contact points.
minContact.Update(_contactManager._contactListener);
minContact.Flags &= ~ContactFlag.ToiFlag;
// Is the contact solid?
if (minContact.IsSolid() == false)
{
// Restore the sweeps.
b3._sweep = backup1;
b4._sweep = backup2;
b3.SynchronizeTransform();
b4.SynchronizeTransform();
continue;
}
// Did numerical issues prevent a contact point from being generated?
if (minContact.IsTouching() == false)
{
// Give up on this TOI.
continue;
}
// Build the TOI island. We need a dynamic seed.
Body seed = b3;
if (seed.IsStatic())
{
seed = b4;
}
// 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 |= Body.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(ref b);
// Make sure the body is awake.
b._flags &= ~Body.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.ContactCount == island.ContactCapacity)
{
break;
}
// Has this contact already been added to an island? Skip slow or non-solid contacts.
if ((cEdge.Contact.Flags & ContactFlag.IslandFlag) != 0)
{
continue;
}
// Is this contact touching? For performance we are not updating this contact.
if (cEdge.Contact.IsSolid() == false || cEdge.Contact.IsTouching() == false)
{
continue;
}
island.Add(ref cEdge.Contact);
cEdge.Contact.Flags |= ContactFlag.IslandFlag;
// Update other body.
Body other = cEdge.Other;
// Was the other body already added to this island?
if ((other._flags & Body.BodyFlags.Island) != 0)
{
continue;
}
// March forward, this can do no harm since this is the min TOI.
if (other.IsStatic() == false)
{
other.Advance(minTOI);
other.WakeUp();
}
Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
queue[queueStart + queueSize] = other;
++queueSize;
other._flags |= Body.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 & Body.BodyFlags.Island) != 0)
{
continue;
}
if (!other.IsStatic())
{
other.Advance(minTOI);
other.WakeUp();
}
Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
queue[queueStart + queueSize] = other;
++queueSize;
other._flags |= Body.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 &= ~Body.BodyFlags.Island;
if ((b._flags & Body.BodyFlags.Sleep) != 0)
{
continue;
}
if (b.IsStatic())
{
continue;
}
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 &= ~ContactFlag.ToiFlag;
}
}
for (int i = 0; i < island.ContactCount; ++i)
{
// Allow contacts to participate in future TOI islands.
Contact c = island.Contacts[i];
c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag);
}
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();
}
queue = null;
}
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
{
Step = step;
ConstraintCount = contactCount;
Constraints = new ContactConstraint[ConstraintCount];
for (int i = 0; i < ConstraintCount; ++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();
float friction = Settings.MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction());
float restitution = Settings.MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution());
Vec2 vA = bodyA._linearVelocity;
Vec2 vB = bodyB._linearVelocity;
float wA = bodyA._angularVelocity;
float wB = bodyB._angularVelocity;
Box2DXDebug.Assert(manifold.PointCount > 0);
WorldManifold worldManifold = new WorldManifold();
worldManifold.Initialize(manifold, bodyA.GetTransform(), radiusA, bodyB.GetTransform(), radiusB);
ContactConstraint cc = new ContactConstraint();
Constraints[i] = cc;
cc.BodyA = bodyA;
cc.BodyB = bodyB;
cc.Manifold = manifold;
cc.Normal = worldManifold.Normal;
cc.PointCount = manifold.PointCount;
cc.Friction = friction;
cc.Restitution = restitution;
cc.LocalPlaneNormal = manifold.LocalPlaneNormal;
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 = cp.NormalImpulse;
ccp.TangentImpulse = cp.TangentImpulse;
ccp.LocalPoint = cp.LocalPoint;
ccp.RA = worldManifold.Points[j] - bodyA._sweep.C;
ccp.RB = worldManifold.Points[j] - bodyB._sweep.C;
float rnA = Vec2.Cross(ccp.RA, cc.Normal);
float rnB = Vec2.Cross(ccp.RB, cc.Normal);
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;
Box2DXDebug.Assert(kNormal > Settings.FLT_EPSILON);
ccp.NormalMass = 1.0f / kNormal;
float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass;
kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB;
Box2DXDebug.Assert(kEqualized > Settings.FLT_EPSILON);
ccp.EqualizedMass = 1.0f / kEqualized;
Vec2 tangent = Vec2.Cross(cc.Normal, 1.0f);
float rtA = Vec2.Cross(ccp.RA, tangent);
float rtB = Vec2.Cross(ccp.RB, tangent);
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;
Box2DXDebug.Assert(kTangent > Settings.FLT_EPSILON);
ccp.TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.VelocityBias = 0.0f;
float vRel = Vec2.Dot(cc.Normal, vB + Vec2.Cross(wB, ccp.RB) - vA - Vec2.Cross(wA, ccp.RA));
if (vRel < -Settings.VelocityThreshold)
{
ccp.VelocityBias = -cc.Restitution * vRel;
}
}
// 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 = Vec2.Cross(ccp1.RA, cc.Normal);
float rn1B = Vec2.Cross(ccp1.RB, cc.Normal);
float rn2A = Vec2.Cross(ccp2.RA, cc.Normal);
float rn2B = Vec2.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.Col1.Set(k11, k12);
cc.K.Col2.Set(k12, k22);
cc.NormalMass = cc.K.Invert();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
}
}
