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();
}
}
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);
}
}
/// Call this to draw shapes and other debug draw data.
public void DrawDebugData()
{
if (DebugDraw == null)
{
return;
}
DebugDrawFlags flags = DebugDraw.Flags;
if ((flags & DebugDrawFlags.Shape) == DebugDrawFlags.Shape)
{
for (Body b = _bodyList; b != null; b = b.GetNext())
{
Transform xf;
b.GetTransform(out xf);
for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
{
if (b.IsActive() == false)
{
DrawShape(f, xf, new Color(0.5f, 0.5f, 0.3f));
}
else if (b.GetType() == BodyType.Static)
{
DrawShape(f, xf, new Color(0.5f, 0.9f, 0.5f));
}
else if (b.GetType() == BodyType.Kinematic)
{
DrawShape(f, xf, new Color(0.5f, 0.5f, 0.9f));
}
else if (b.IsAwake() == false)
{
DrawShape(f, xf, new Color(0.6f, 0.6f, 0.6f));
}
else
{
DrawShape(f, xf, new Color(0.9f, 0.7f, 0.7f));
}
}
}
}
if ((flags & DebugDrawFlags.Joint) == DebugDrawFlags.Joint)
{
for (Joint j = _jointList; j != null; j = j.GetNext())
{
DrawJoint(j);
}
}
if ((flags & DebugDrawFlags.Pair) == DebugDrawFlags.Pair)
{
Color color = new Color(0.3f, 0.9f, 0.9f);
for (Contact c = _contactManager._contactList; c != null; c = c.GetNext())
{
/*
* Fixture fixtureA = c.GetFixtureA();
* Fixture fixtureB = c.GetFixtureB();
*
* AABB aabbA;
* AABB aabbB;
* fixtureA.GetAABB(out aabbA);
* fixtureB.GetAABB(out aabbB);
*
* Vector2 cA = aabbA.GetCenter();
* Vector2 cB = aabbB.GetCenter();
*
* DebugDraw.DrawSegment(cA, cB, color);
*/
}
}
if ((flags & DebugDrawFlags.AABB) == DebugDrawFlags.AABB)
{
Color color = new Color(0.9f, 0.3f, 0.9f);
BroadPhase bp = _contactManager._broadPhase;
for (Body b = _bodyList; b != null; b = b.GetNext())
{
if (b.IsActive() == false)
{
continue;
}
for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
{
for (int i = 0; i < f._proxyCount; ++i)
{
FixtureProxy proxy = f._proxies[i];
AABB aabb;
bp.GetFatAABB(proxy.proxyId, out aabb);
FixedArray8 <Vector2> vs = new FixedArray8 <Vector2>();
vs[0] = new Vector2(aabb.lowerBound.x, aabb.lowerBound.y);
vs[1] = new Vector2(aabb.upperBound.x, aabb.lowerBound.y);
vs[2] = new Vector2(aabb.upperBound.x, aabb.upperBound.y);
vs[3] = new Vector2(aabb.lowerBound.x, aabb.upperBound.y);
DebugDraw.DrawPolygon(ref vs, 4, color);
}
}
}
}
if ((flags & DebugDrawFlags.CenterOfMass) == DebugDrawFlags.CenterOfMass)
{
for (Body b = _bodyList; b != null; b = b.GetNext())
{
Transform xf;
b.GetTransform(out xf);
xf.Position = b.GetWorldCenter();
DebugDraw.DrawTransform(ref xf);
}
}
}
void Solve(ref TimeStep step)
{
// Size the island for the worst case.
_island.Reset(_bodyCount,
_contactManager._contactCount,
_jointCount,
_contactManager.ContactListener);
// Clear all the island flags.
for (Body b = _bodyList; b != null; b = b._next)
{
b._flags &= ~BodyFlags.Island;
}
for (Contact c = _contactManager._contactList; c != null; c = c._next)
{
c._flags &= ~ContactFlags.Island;
}
for (Joint j = _jointList; j != null; j = j._next)
{
j._islandFlag = false;
}
// Build and simulate all awake islands.
int stackSize = _bodyCount;
if (stackSize > stack.Length)
{
stack = new Body[Math.Max(stack.Length * 2, stackSize)];
}
for (Body seed = _bodyList; seed != null; seed = seed._next)
{
if ((seed._flags & (BodyFlags.Island)) != BodyFlags.None)
{
continue;
}
if (seed.IsAwake() == false || seed.IsActive() == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed.GetType() == BodyType.Static)
{
continue;
}
// Reset island and stack.
_island.Clear();
int stackCount = 0;
stack[stackCount++] = seed;
seed._flags |= BodyFlags.Island;
// Perform a depth first search (DFS) on the raint graph.
while (stackCount > 0)
{
// Grab the next body off the stack and add it to the island.
Body b = stack[--stackCount];
//Debug.Assert(b.IsActive() == true);
_island.Add(b);
// Make sure the body is awake.
b.SetAwake(true);
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b.GetType() == BodyType.Static)
{
continue;
}
// Search all contacts connected to this body.
for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next)
{
Contact contact = ce.Contact;
// Has this contact already been added to an island?
if ((contact._flags & ContactFlags.Island) != ContactFlags.None)
{
continue;
}
// Is this contact solid and touching?
if (!ce.Contact.IsEnabled() || !ce.Contact.IsTouching())
{
continue;
}
// Skip sensors.
bool sensorA = contact._fixtureA._isSensor;
bool sensorB = contact._fixtureB._isSensor;
if (sensorA || sensorB)
{
continue;
}
_island.Add(contact);
contact._flags |= ContactFlags.Island;
Body other = ce.Other;
// Was the other body already added to this island?
if ((other._flags & BodyFlags.Island) != BodyFlags.None)
{
continue;
}
//Debug.Assert(stackCount < stackSize);
stack[stackCount++] = other;
other._flags |= BodyFlags.Island;
}
// Search all joints connect to this body.
for (JointEdge je = b._jointList; je != null; je = je.Next)
{
if (je.Joint._islandFlag == true)
{
continue;
}
Body other = je.Other;
// Don't simulate joints connected to inactive bodies.
if (other.IsActive() == false)
{
continue;
}
_island.Add(je.Joint);
je.Joint._islandFlag = true;
if ((other._flags & BodyFlags.Island) != BodyFlags.None)
{
continue;
}
//Debug.Assert(stackCount < stackSize);
stack[stackCount++] = other;
other._flags |= BodyFlags.Island;
}
}
_island.Solve(ref step, Gravity, _allowSleep);
// Post solve cleanup.
for (int i = 0; i < _island._bodyCount; ++i)
{
// Allow static bodies to participate in other islands.
Body b = _island._bodies[i];
if (b.GetType() == BodyType.Static)
{
b._flags &= ~BodyFlags.Island;
}
}
}
// Synchronize fixtures, check for out of range bodies.
for (Body b = _bodyList; b != null; b = b.GetNext())
{
// If a body was not in an island then it did not move.
if ((b._flags & BodyFlags.Island) != BodyFlags.Island)
{
continue;
}
if (b.GetType() == BodyType.Static)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
_contactManager.FindNewContacts();
}
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;
}
}
}
