public void Destroy(Contact c)
{
Fixture fixtureA = c.FixtureA;
Fixture fixtureB = c.FixtureB;
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
if (m_contactListener != null && c.IsTouching())
{
m_contactListener.EndContact(c);
}
// Remove from the world.
m_contactList.Remove(c);
// Remove from body 1
c.m_nodeA.other.m_contactList.Remove(c.m_nodeA);
// Remove from body 2
c.m_nodeB.other.m_contactList.Remove(c.m_nodeA);
// Call the factory.
}
internal void Destroy(Contact c)
{
Fixture fixtureA = c.GetFixtureA();
Fixture fixtureB = c.GetFixtureB();
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
if (ContactListener != null && c.IsTouching())
{
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 == bodyA._contactList)
{
bodyA._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 == bodyB._contactList)
{
bodyB._contactList = c._nodeB.Next;
}
c.Destroy();
--_contactCount;
}
// Advance a dynamic body to its first time of contact
// and adjust the position to ensure clearance.
void SolveTOI(Body body)
{
// Find the minimum contact.
Contact toiContact = null;
float toi = 1.0f;
Body toiOther = null;
bool found;
int count;
int iter = 0;
bool bullet = body.IsBullet;
// Iterate until all contacts agree on the minimum TOI. We have
// to iterate because the TOI algorithm may skip some intermediate
// collisions when objects rotate through each other.
do
{
count = 0;
found = false;
for (ContactEdge ce = body._contactList; ce != null; ce = ce.Next)
{
if (ce.Contact == toiContact)
{
continue;
}
Body other = ce.Other;
BodyType type = other.GetType();
// Only bullets perform TOI with dynamic bodies.
if (bullet == true)
{
// Bullets only perform TOI with bodies that have their TOI resolved.
if ((other._flags & BodyFlags.Toi) == 0)
{
continue;
}
// No repeated hits on non-static bodies
if (type != BodyType.Static && (ce.Contact._flags & ContactFlags.BulletHit) != 0)
{
continue;
}
}
else if (type == BodyType.Dynamic)
{
continue;
}
// Check for a disabled contact.
Contact contact = ce.Contact;
if (contact.IsEnabled() == false)
{
continue;
}
// Prevent infinite looping.
if (contact._toiCount > 10)
{
continue;
}
Fixture fixtureA = contact._fixtureA;
Fixture fixtureB = contact._fixtureB;
int indexA = contact._indexA;
int indexB = contact._indexB;
// Cull sensors.
if (fixtureA.IsSensor() || fixtureB.IsSensor())
{
continue;
}
Body bodyA = fixtureA._body;
Body bodyB = fixtureB._body;
// Compute the time of impact in interval [0, minTOI]
TOIInput input = new TOIInput();
input.proxyA.Set(fixtureA.GetShape(), indexA);
input.proxyB.Set(fixtureB.GetShape(), indexB);
input.sweepA = bodyA._sweep;
input.sweepB = bodyB._sweep;
input.tMax = toi;
TOIOutput output;
TimeOfImpact.CalculateTimeOfImpact(out output, ref input);
if (output.State == TOIOutputState.Touching && output.t < toi)
{
toiContact = contact;
toi = output.t;
toiOther = other;
found = true;
}
++count;
}
++iter;
} while (found && count > 1 && iter < 50);
if (toiContact == null)
{
body.Advance(1.0f);
return;
}
Sweep backup = body._sweep;
body.Advance(toi);
toiContact.Update(_contactManager.ContactListener);
if (toiContact.IsEnabled() == false)
{
// Contact disabled. Backup and recurse.
body._sweep = backup;
SolveTOI(body);
}
++toiContact._toiCount;
// Update all the valid contacts on this body and build a contact island.
count = 0;
for (ContactEdge ce = body._contactList; (ce != null) && (count < Settings.b2_maxTOIContacts); ce = ce.Next)
{
Body other = ce.Other;
BodyType type = other.GetType();
// Only perform correction with static bodies, so the
// body won't get pushed out of the world.
if (type == BodyType.Dynamic)
{
continue;
}
// Check for a disabled contact.
Contact contact = ce.Contact;
if (contact.IsEnabled() == false)
{
continue;
}
Fixture fixtureA = contact._fixtureA;
Fixture fixtureB = contact._fixtureB;
// Cull sensors.
if (fixtureA.IsSensor() || fixtureB.IsSensor())
{
continue;
}
// The contact likely has some new contact points. The listener
// gives the user a chance to disable the contact.
if (contact != toiContact)
{
contact.Update(_contactManager.ContactListener);
}
// Did the user disable the contact?
if (contact.IsEnabled() == false)
{
// Skip this contact.
continue;
}
if (contact.IsTouching() == false)
{
continue;
}
_toiContacts[count] = contact;
++count;
}
// Reduce the TOI body's overlap with the contact island.
_toiSolver.Initialize(_toiContacts, count, body);
float k_toiBaumgarte = 0.75f;
//bool solved = false;
for (int i = 0; i < 20; ++i)
{
bool contactsOkay = _toiSolver.Solve(k_toiBaumgarte);
if (contactsOkay)
{
//solved = true;
break;
}
}
if (toiOther.GetType() != BodyType.Static)
{
toiContact._flags |= ContactFlags.BulletHit;
}
}
public void Destroy(Contact c) {
Fixture fixtureA = c.FixtureA;
Fixture fixtureB = c.FixtureB;
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
if (m_contactListener != null && c.IsTouching())
{
m_contactListener.EndContact(c);
}
// Remove from the world.
m_contactList.Remove(c);
// Remove from body 1
c.m_nodeA.other.m_contactList.Remove(c.m_nodeA);
// Remove from body 2
c.m_nodeB.other.m_contactList.Remove(c.m_nodeA);
// Call the factory.
}
private void Solve(TimeStep step)
{
m_profile.solveInit = 0.0f;
m_profile.solveVelocity = 0.0f;
m_profile.solvePosition = 0.0f;
// Size the island for the worst case.
Island island = new Island(m_contactManager.m_contactListener);
// Clear all the island flags.
foreach (Body b in m_bodyList)
{
b.m_flags &= ~Body.BodyFlags.e_islandFlag;
}
foreach (Contact c in m_contactManager.m_contactList)
{
c.m_flags &= ~ContactFlags.e_islandFlag;
}
foreach (Joint j in m_jointList)
{
j.m_islandFlag = false;
}
// Build and simulate all awake islands.
List <Body> stack = new List <Body>(m_bodyList.Count());
foreach (Body seed in m_bodyList)
{
if (seed.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
{
continue;
}
if (seed.IsAwake() == false || seed.IsActive() == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed.GetBodyType() == BodyType._staticBody)
{
continue;
}
// Reset island and stack.
island.Clear();
int stackCount = 0;
stack.Add(seed); stackCount++;
seed.m_flags |= Body.BodyFlags.e_islandFlag;
// Perform a depth first search (DFS) on the constraint graph.
while (stackCount > 0)
{
// Grab the next body off the stack and add it to the island.
Body b = stack[--stackCount];
Utilities.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.GetBodyType() == BodyType._staticBody)
{
continue;
}
// Search all contacts connected to this body.
foreach (ContactEdge ce in b.m_contactList)
{
Contact contact = ce.contact;
// Has this contact already been added to an island?
if (contact.m_flags.HasFlag(ContactFlags.e_islandFlag))
{
continue;
}
// Is this contact solid and touching?
if (contact.IsEnabled() == false ||
contact.IsTouching() == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact.m_fixtureA.m_isSensor;
bool sensorB = contact.m_fixtureB.m_isSensor;
if (sensorA || sensorB)
{
continue;
}
island.Add(contact);
contact.m_flags |= ContactFlags.e_islandFlag;
Body other = ce.other;
// Was the other body already added to this island?
if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
{
continue;
}
Utilities.Assert(stackCount < m_bodyList.Count());
stack.Add(other); stackCount++;
other.m_flags |= Body.BodyFlags.e_islandFlag;
}
// Search all joints connect to this body.
foreach (JointEdge je in b.m_jointList)
{
if (je.joint.m_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.m_islandFlag = true;
if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
{
continue;
}
stack.Add(other); stackCount++;
other.m_flags |= Body.BodyFlags.e_islandFlag;
}
}
Profile profile = new Profile();
island.Solve(profile, step, m_gravity, m_allowSleep);
m_profile.solveInit += profile.solveInit;
m_profile.solveVelocity += profile.solveVelocity;
m_profile.solvePosition += profile.solvePosition;
// Post solve cleanup.
for (int i = 0; i < island.m_bodies.Count(); ++i)
{
// Allow static bodies to participate in other islands.
Body b = island.m_bodies[i];
if (b.GetBodyType() == BodyType._staticBody)
{
b.m_flags &= ~Body.BodyFlags.e_islandFlag;
}
}
}
{
Timer timer = new Timer();
// Synchronize fixtures, check for out of range bodies.
foreach (Body b in m_bodyList)
{
// If a body was not in an island then it did not move.
if ((b.m_flags & Body.BodyFlags.e_islandFlag) == 0)
{
continue;
}
if (b.GetBodyType() == BodyType._staticBody)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
m_contactManager.FindNewContacts();
m_profile.broadphase = timer.GetMilliseconds();
}
}
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;
}
}
}
