public Contact(Shape s1, Shape s2)
{
this._flags = (Contact.CollisionFlags) 0;
if (s1.IsSensor || s2.IsSensor)
{
this._flags |= Contact.CollisionFlags.NonSolid;
}
this._shape1 = s1;
this._shape2 = s2;
this._manifoldCount = 0;
this._prev = null;
this._next = null;
this._node1 = new ContactEdge();
this._node1.Contact = null;
this._node1.Prev = null;
this._node1.Next = null;
this._node1.Other = null;
this._node2 = new ContactEdge();
this._node2.Contact = null;
this._node2.Prev = null;
this._node2.Next = null;
this._node2.Other = null;
}
public Contact(Shape s1, Shape s2)
{
this._flags = (Contact.CollisionFlags)0;
if (s1.IsSensor || s2.IsSensor)
{
this._flags |= Contact.CollisionFlags.NonSolid;
}
this._shape1 = s1;
this._shape2 = s2;
this._manifoldCount = 0;
this._prev = null;
this._next = null;
this._node1 = new ContactEdge();
this._node1.Contact = null;
this._node1.Prev = null;
this._node1.Next = null;
this._node1.Other = null;
this._node2 = new ContactEdge();
this._node2.Contact = null;
this._node2.Prev = null;
this._node2.Next = null;
this._node2.Other = null;
}
public void SetFilterData(Filter filter)
{
Filter = filter;
if (Body == null)
{
return;
}
// Flag associated contacts for filtering.
ContactEdge edge = Body.GetContactList();
while (edge != null)
{
Contact contact = edge.Contact;
Fixture fixtureA = contact.GetFixtureA();
Fixture fixtureB = contact.GetFixtureB();
if (fixtureA == this || fixtureB == this)
{
contact.FlagForFiltering();
}
}
}
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;
}
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 Contact(Shape s1, Shape s2)
{
_flags = 0;
if (s1.IsSensor || s2.IsSensor)
{
_flags |= CollisionFlags.NonSolid;
}
_shape1 = s1;
_shape2 = s2;
_manifoldCount = 0;
_prev = null;
_next = null;
_node1 = new ContactEdge();
_node1.Contact = null;
_node1.Prev = null;
_node1.Next = null;
_node1.Other = null;
_node2 = new ContactEdge();
_node2.Contact = null;
_node2.Prev = null;
_node2.Next = null;
_node2.Other = null;
}
public Contact(Fixture fA, Fixture fB)
{
_flags = 0;
if (fA.IsSensor || fB.IsSensor)
{
_flags |= CollisionFlags.NonSolid;
}
_fixtureA = fA;
_fixtureB = fB;
_manifold.PointCount = 0;
_prev = null;
_next = null;
_nodeA = new ContactEdge();
_nodeB = new ContactEdge();
}
internal bool _useGravity; // STEVE Added
internal Body(BodyDef bd, World world)
{
Box2DXDebug.Assert(!world._lock);
this._flags = (Body.BodyFlags) 0;
if (bd.IsBullet)
{
this._flags |= Body.BodyFlags.Bullet;
}
if (bd.FixedRotation)
{
this._flags |= Body.BodyFlags.FixedRotation;
}
if (bd.AllowSleep)
{
this._flags |= Body.BodyFlags.AllowSleep;
}
if (bd.IsSleeping)
{
this._flags |= Body.BodyFlags.Sleep;
}
this._world = world;
this._xf.Position = bd.Position;
this._xf.R.Set(bd.Angle);
this._sweep.LocalCenter = bd.MassData.Center;
this._sweep.T0 = 1f;
this._sweep.A0 = (this._sweep.A = bd.Angle);
this._sweep.C0 = (this._sweep.C = Box2DX.Common.Math.Mul(this._xf, this._sweep.LocalCenter));
this._jointList = null;
this._contactList = null;
this._prev = null;
this._next = null;
this._linearDamping = bd.LinearDamping;
this._angularDamping = bd.AngularDamping;
this._force.Set(0f, 0f);
this._torque = 0f;
this._linearVelocity.SetZero();
this._angularVelocity = 0f;
this._sleepTime = 0f;
this._invMass = 0f;
this._I = 0f;
this._invI = 0f;
this._mass = bd.MassData.Mass;
if (this._mass > 0f)
{
this._invMass = 1f / this._mass;
}
if ((this._flags & Body.BodyFlags.FixedRotation) == (Body.BodyFlags) 0)
{
this._I = bd.MassData.I;
}
if (this._I > 0f)
{
this._invI = 1f / this._I;
}
if (this._invMass == 0f && this._invI == 0f)
{
this._type = Body.BodyType.Static;
}
else
{
this._type = Body.BodyType.Dynamic;
}
this._userData = bd.UserData;
this._shapeList = null;
this._shapeCount = 0;
this._gravity = _world.Gravity; // STEVE Added
this._useGravity = false; // STEVE Added
}
/// Destroy a fixture. This removes the fixture from the broad-phase and
/// therefore destroys any contacts associated with this fixture. All fixtures
/// attached to a body are implicitly destroyed when the body is destroyed.
/// @param fixture the fixture to be removed.
/// @warning This function is locked during callbacks.
public void DestroyFixture(ref Fixture fixture)
{
Box2DXDebug.Assert(_world.IsLocked() == false);
if (_world.IsLocked() == true)
{
return;
}
Box2DXDebug.Assert(fixture.Body == this);
// Remove the fixture from this body's singly linked list.
Box2DXDebug.Assert(_fixtureCount > 0);
Fixture node = _fixtureList;
bool found = false;
while (node != null)
{
if (node == fixture)
{
_fixtureList = fixture._next;
found = true;
break;
}
node = node._next;
}
// You tried to remove a shape that is not attached to this body.
Box2DXDebug.Assert(found);
// Destroy any contacts associated with the fixture.
ContactEdge edge = _contactList;
while (edge != null)
{
Contact c = edge.Contact;
edge = edge.Next;
Fixture fixtureA = c.GetFixtureA();
Fixture fixtureB = c.GetFixtureB();
if (fixture == fixtureA || fixture == fixtureB)
{
// This destroys the contact and removes it from
// this body's contact list.
_world._contactManager.Destroy(c);
}
}
bool needMassUpdate = fixture._massData.Mass > 0.0f || fixture._massData.I > 0.0f;
BroadPhase broadPhase = _world._contactManager._broadPhase;
fixture.Destroy(broadPhase);
fixture.Body = null;
fixture._next = null;
--_fixtureCount;
// Adjust mass properties if needed.
if (needMassUpdate)
{
ResetMass();
}
}
/// <summary>
/// Destroy a joint. This may cause the connected bodies to begin colliding.
/// @warning This function is locked during callbacks.
/// </summary>
/// <param name="j"></param>
public void DestroyJoint(Joint j)
{
Box2DXDebug.Assert(IsLocked() == false);
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;
Joint.Destroy(j);
Box2DXDebug.Assert(_jointCount > 0);
--_jointCount;
// If the joint prevents collisions, then flag any contacts for filtering.
if (collideConnected == false)
{
ContactEdge edge = bodyB.GetContactList();
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;
}
}
}
/// <summary>
/// Create a joint to constrain bodies together. No reference to the definition
/// is retained. This may cause the connected bodies to cease colliding.
/// @warning This function is locked during callbacks.
/// </summary>
/// <param name="def"></param>
/// <returns></returns>
public Joint CreateJoint(JointDef def)
{
Box2DXDebug.Assert(IsLocked() == false);
if (IsLocked())
{
return(null);
}
Joint j = Joint.Create(def);
// Connect to the world list.
j._prev = null;
j._next = _jointList;
if (_jointList != null)
{
_jointList._prev = j;
}
_jointList = j;
++_jointCount;
// Connect to the bodies' doubly linked lists.
j._edgeA.Joint = j;
j._edgeA.Other = j._bodyB;
j._edgeA.Prev = null;
j._edgeA.Next = j._bodyA._jointList;
if (j._bodyA._jointList != null)
{
j._bodyA._jointList.Prev = j._edgeA;
}
j._bodyA._jointList = j._edgeA;
j._edgeB.Joint = j;
j._edgeB.Other = j._bodyA;
j._edgeB.Prev = null;
j._edgeB.Next = j._bodyB._jointList;
if (j._bodyB._jointList != null)
{
j._bodyB._jointList.Prev = j._edgeB;
}
j._bodyB._jointList = j._edgeB;
Body bodyA = def.Body1;
Body bodyB = def.Body2;
bool staticA = bodyA.IsStatic();
bool staticB = bodyB.IsStatic();
// If the joint prevents collisions, then flag any contacts for filtering.
if (def.CollideConnected == false && (staticA == false || staticB == false))
{
// Ensure we iterate over contacts on a dynamic body (usually have less contacts
// than a static body). Ideally we will have a contact count on both bodies.
if (staticB)
{
Math.Swap(ref bodyA, ref bodyB);
}
ContactEdge edge = bodyB.GetContactList();
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;
}
}
// Note: creating a joint doesn't wake the bodies.
return(j);
}
public void DestroyBody(Body b)
{
Box2DXDebug.Assert(_bodyCount > 0);
Box2DXDebug.Assert(IsLocked() == false);
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.Destroy(_contactManager._broadPhase);
f0 = null;
}
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;
b = null;
}
internal Body(BodyDef bd, World world)
{
Box2DXDebug.Assert(world._lock == false);
_flags = 0;
if (bd.IsBullet)
{
_flags |= BodyFlags.Bullet;
}
if (bd.FixedRotation)
{
_flags |= BodyFlags.FixedRotation;
}
if (bd.AllowSleep)
{
_flags |= BodyFlags.AllowSleep;
}
if (bd.IsSleeping)
{
_flags |= BodyFlags.Sleep;
}
_world = world;
_xf.Position = bd.Position;
_xf.R.Set(bd.Angle);
_sweep.LocalCenter = bd.MassData.Center;
_sweep.T0 = 1.0f;
_sweep.A0 = _sweep.A = bd.Angle;
_sweep.C0 = _sweep.C = Common.Math.Mul(_xf, _sweep.LocalCenter);
_jointList = null;
_contactList = null;
_prev = null;
_next = null;
_linearDamping = bd.LinearDamping;
_angularDamping = bd.AngularDamping;
_force.Set(0.0f, 0.0f);
_torque = 0.0f;
_linearVelocity.SetZero();
_angularVelocity = 0.0f;
_sleepTime = 0.0f;
_invMass = 0.0f;
_I = 0.0f;
_invI = 0.0f;
_mass = bd.MassData.Mass;
if (_mass > 0.0f)
{
_invMass = 1.0f / _mass;
}
if ((_flags & BodyFlags.FixedRotation) == 0)
{
_I = bd.MassData.I;
}
if (_I > 0.0f)
{
_invI = 1.0f / _I;
}
if (_invMass == 0.0f && _invI == 0.0f)
{
_type = BodyType.Static;
}
else
{
_type = BodyType.Dynamic;
}
_userData = bd.UserData;
_shapeList = null;
_shapeCount = 0;
}
internal Body(BodyDef bd, World world)
{
Box2DXDebug.Assert(world._lock == false);
_flags = 0;
if (bd.IsBullet)
{
_flags |= BodyFlags.Bullet;
}
if (bd.FixedRotation)
{
_flags |= BodyFlags.FixedRotation;
}
if (bd.AllowSleep)
{
_flags |= BodyFlags.AllowSleep;
}
if (bd.IsSleeping)
{
_flags |= BodyFlags.Sleep;
}
_world = world;
_xf.Position = bd.Position;
_xf.R.Set(bd.Angle);
_sweep.LocalCenter = bd.MassData.Center;
_sweep.T0 = 1.0f;
_sweep.A0 = _sweep.A = bd.Angle;
_sweep.C0 = _sweep.C = Common.Math.Mul(_xf, _sweep.LocalCenter);
_jointList = null;
_contactList = null;
_prev = null;
_next = null;
_linearDamping = bd.LinearDamping;
_angularDamping = bd.AngularDamping;
_force.Set(0.0f, 0.0f);
_torque = 0.0f;
_linearVelocity.SetZero();
_angularVelocity = 0.0f;
_sleepTime = 0.0f;
_invMass = 0.0f;
_I = 0.0f;
_invI = 0.0f;
_mass = bd.MassData.Mass;
if (_mass > 0.0f)
{
_invMass = 1.0f / _mass;
}
if ((_flags & BodyFlags.FixedRotation) == 0)
{
_I = bd.MassData.I;
}
if (_I > 0.0f)
{
_invI = 1.0f / _I;
}
if (_invMass == 0.0f && _invI == 0.0f)
{
_type = BodyType.Static;
}
else
{
_type = BodyType.Dynamic;
}
_userData = bd.UserData;
_shapeList = null;
_shapeCount = 0;
}
// Find islands, integrate and solve constraints, solve position constraints
private void Solve(TimeStep step)
{
// Size the island for the worst case.
Island island = new Island(_bodyCount,
_contactManager._contactCount,
_jointCount,
_contactManager._contactListener);
// Clear all the island flags.
for (Body b = _bodyList; b != null; b = b._next)
{
b._flags &= ~Body.BodyFlags.Island;
}
for (Contact c = _contactManager._contactList; c != null; c = c.Next)
{
c.Flags &= ~ContactFlag.IslandFlag;
}
for (Joint j = _jointList; j != null; j = j._next)
{
j._islandFlag = false;
}
// Build and simulate all awake islands.
int stackSize = _bodyCount;
Body[] stack = new Body[stackSize];
for (Body seed = _bodyList; seed != null; seed = seed._next)
{
if ((seed._flags & (Body.BodyFlags.Island | Body.BodyFlags.Sleep)) != 0)
{
continue;
}
if (seed.IsStatic())
{
continue;
}
// Reset island and stack.
island.Clear();
int stackCount = 0;
stack[stackCount++] = seed;
seed._flags |= Body.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];
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 ce = b._contactList; ce != null; ce = ce.Next)
{
// Has this contact already been added to an island?
if ((ce.Contact.Flags & ContactFlag.IslandFlag) != 0)
{
continue;
}
// Is this contact touching?
if (ce.Contact.IsSolid() == false || ce.Contact.IsTouching() == false)
{
continue;
}
island.Add(ref ce.Contact);
ce.Contact.Flags |= ContactFlag.IslandFlag;
Body other = ce.Other;
// Was the other body already added to this island?
if ((other._flags & Body.BodyFlags.Island) != 0)
{
continue;
}
Box2DXDebug.Assert(stackCount < stackSize);
stack[stackCount++] = other;
other._flags |= Body.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;
}
island.Add(je.Joint);
je.Joint._islandFlag = true;
Body other = je.Other;
if ((other._flags & Body.BodyFlags.Island) != 0)
{
continue;
}
Box2DXDebug.Assert(stackCount < stackSize);
stack[stackCount++] = other;
other._flags |= Body.BodyFlags.Island;
}
}
island.Solve(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.IsStatic())
{
b._flags &= ~Body.BodyFlags.Island;
}
}
}
stack = null;
// Synchronize shapes, check for out of range bodies.
for (Body b = _bodyList; b != null; b = b.GetNext())
{
if ((b._flags & Body.BodyFlags.Sleep) != 0)
{
continue;
}
if (b.IsStatic())
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
_contactManager.FindNewContacts();
}
private Transform _xf; // the body origin transform
#endregion Fields
#region Constructors
internal Body(BodyDef bd, World world)
{
_flags = 0;
if (bd.IsBullet)
{
_flags |= BodyFlags.Bullet;
}
if (bd.FixedRotation)
{
_flags |= BodyFlags.FixedRotation;
}
if (bd.AllowSleep)
{
_flags |= BodyFlags.AllowSleep;
}
if (bd.IsSleeping)
{
_flags |= BodyFlags.Sleep;
}
_world = world;
_xf.Position = bd.Position;
_xf.R.Set(bd.Angle);
_sweep.LocalCenter.SetZero();
_sweep.T0 = 1.0f;
_sweep.A0 = _sweep.A = bd.Angle;
_sweep.C0 = _sweep.C = Math.Mul(_xf, _sweep.LocalCenter);
_jointList = null;
_contactList = null;
_prev = null;
_next = null;
_linearVelocity = bd.LinearVelocity;
_angularVelocity = bd.AngularVelocity;
_linearDamping = bd.LinearDamping;
_angularDamping = bd.AngularDamping;
_force.Set(0.0f, 0.0f);
_torque = 0.0f;
_sleepTime = 0.0f;
_mass = 0;
_invMass = 0.0f;
_I = 0.0f;
_invI = 0.0f;
_type = BodyType.Static;
_userData = bd.UserData;
_fixtureList = null;
_fixtureCount = 0;
}
// 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;
}
/// This resets the mass properties to the sum of the mass properties of the fixtures.
/// This normally does not need to be called unless you called SetMassData to override
/// the mass and you later want to reset the mass.
public void ResetMass()
{
// Compute mass data from shapes. Each shape has its own density.
_mass = 0.0f;
_invMass = 0.0f;
_I = 0.0f;
_invI = 0.0f;
Vec2 center = Vec2.Zero;
for (Fixture f = _fixtureList; f != null; f = f._next)
{
MassData massData = f.GetMassData();
_mass += massData.Mass;
center += massData.Mass * massData.Center;
_I += massData.I;
}
// Compute center of mass.
if (_mass > 0.0f)
{
_invMass = 1.0f / _mass;
center *= _invMass;
}
if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0)
{
// Center the inertia about the center of mass.
_I -= _mass * Vec2.Dot(center, center);
Box2DXDebug.Assert(_I > 0.0f);
_invI = 1.0f / _I;
}
else
{
_I = 0.0f;
_invI = 0.0f;
}
// Move center of mass.
Vec2 oldCenter = _sweep.C;
_sweep.LocalCenter = center;
_sweep.C0 = _sweep.C = Math.Mul(_xf, _sweep.LocalCenter);
// Update center of mass velocity.
_linearVelocity += Vec2.Cross(_angularVelocity, _sweep.C - oldCenter);
// Determine the new body type.
BodyType oldType = _type;
if (_invMass == 0.0f && _invI == 0.0f)
{
_type = BodyType.Static;
}
else
{
_type = BodyType.Dynamic;
}
// If the body type changed, we need to flag contacts for filtering.
if (oldType != _type)
{
for (ContactEdge ce = _contactList; ce != null; ce = ce.Next)
{
ce.Contact.FlagForFiltering();
}
}
}
internal Body(BodyDef bd, World world)
{
Box2DXDebug.Assert(!world._lock);
this._flags = (Body.BodyFlags)0;
if (bd.IsBullet)
{
this._flags |= Body.BodyFlags.Bullet;
}
if (bd.FixedRotation)
{
this._flags |= Body.BodyFlags.FixedRotation;
}
if (bd.AllowSleep)
{
this._flags |= Body.BodyFlags.AllowSleep;
}
if (bd.IsSleeping)
{
this._flags |= Body.BodyFlags.Sleep;
}
this._world = world;
this._xf.Position = bd.Position;
this._xf.R.Set(bd.Angle);
this._sweep.LocalCenter = bd.MassData.Center;
this._sweep.T0 = 1f;
this._sweep.A0 = (this._sweep.A = bd.Angle);
this._sweep.C0 = (this._sweep.C = Box2DX.Common.Math.Mul(this._xf, this._sweep.LocalCenter));
this._jointList = null;
this._contactList = null;
this._prev = null;
this._next = null;
this._linearDamping = bd.LinearDamping;
this._angularDamping = bd.AngularDamping;
this._force.Set(0f, 0f);
this._torque = 0f;
this._linearVelocity.SetZero();
this._angularVelocity = 0f;
this._sleepTime = 0f;
this._invMass = 0f;
this._I = 0f;
this._invI = 0f;
this._mass = bd.MassData.Mass;
if (this._mass > 0f)
{
this._invMass = 1f / this._mass;
}
if ((this._flags & Body.BodyFlags.FixedRotation) == (Body.BodyFlags)0)
{
this._I = bd.MassData.I;
}
if (this._I > 0f)
{
this._invI = 1f / this._I;
}
if (this._invMass == 0f && this._invI == 0f)
{
this._type = Body.BodyType.Static;
}
else
{
this._type = Body.BodyType.Dynamic;
}
this._userData = bd.UserData;
this._shapeList = null;
this._shapeCount = 0;
this._gravity = _world.Gravity; // STEVE Added
this._useGravity = false; // STEVE Added
}
