protected Joint(JointDef def)
{
Debug.Assert(def.bodyA != def.bodyB);
_type = def.type;
_bodyA = def.bodyA;
_bodyB = def.bodyB;
_collideConnected = def.collideConnected;
_userData = def.userData;
_edgeA = new JointEdge();
_edgeB = new JointEdge();
}
// This is used to prevent connected bodies from colliding.
// It may lie, depending on the collideConnected flag.
internal bool ShouldCollide(Body other)
{
// At least one body should be dynamic.
if (_type != BodyType.Dynamic && other._type != BodyType.Dynamic)
{
return(false);
}
// Does a joint prevent collision?
for (JointEdge jn = _jointList; jn != null; jn = jn.Next)
{
if (jn.Other == other)
{
if (jn.Joint._collideConnected == false)
{
return(false);
}
}
}
return(true);
}
protected Joint(JointDef def)
{
Debug.Assert(def.bodyA != def.bodyB);
_type = def.type;
_bodyA = def.bodyA;
_bodyB = def.bodyB;
_collideConnected = def.collideConnected;
_userData = def.userData;
_edgeA = new JointEdge();
_edgeB = new JointEdge();
}
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 constraint 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 (!b.IsAwake() || !b.IsActive())
{
continue;
}
if (b.GetType() == BodyType.Static)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
_contactManager.FindNewContacts();
}
/// Destroy a rigid body given a definition. No reference to the definition
/// is retained. This function is locked during callbacks.
/// @warning This automatically deletes all associated shapes and joints.
/// @warning This function is locked during callbacks.
public void DestroyBody(Body b)
{
Debug.Assert(_bodyCount > 0);
Debug.Assert(!IsLocked);
if (IsLocked)
{
return;
}
// Delete the attached joints.
JointEdge je = b._jointList;
while (je != null)
{
JointEdge je0 = je;
je = je.Next;
if (DestructionListener != null)
{
DestructionListener.SayGoodbye(je0.Joint);
}
DestroyJoint(je0.Joint);
}
b._jointList = null;
// Delete the attached contacts.
ContactEdge ce = b._contactList;
while (ce != null)
{
ContactEdge ce0 = ce;
ce = ce.Next;
_contactManager.Destroy(ce0.Contact);
}
b._contactList = null;
// Delete the attached fixtures. This destroys broad-phase proxies.
Fixture f = b._fixtureList;
while (f != null)
{
Fixture f0 = f;
f = f._next;
if (DestructionListener != null)
{
DestructionListener.SayGoodbye(f0);
}
f0.DestroyProxy(_contactManager._broadPhase);
f0.Destroy();
}
b._fixtureList = null;
b._fixtureCount = 0;
// Remove world body list.
if (b._prev != null)
{
b._prev._next = b._next;
}
if (b._next != null)
{
b._next._prev = b._prev;
}
if (b == _bodyList)
{
_bodyList = b._next;
}
--_bodyCount;
}
private static Joint GetFirstGearableJoint(JointEdge je)
{
while (je != null && !(je.Joint is PrismaticJoint || je.Joint is RevoluteJoint))
{
je = je.Next;
}
if (je == null)
{
throw (new Exception("missing gear joint target"));
}
return je.Joint;
}