public b2ContactManager()
{
m_contactList = null;
m_contactCount = 0;
m_contactFilter = b2ContactFilter.b2_defaultFilter;
m_contactListener = b2ContactListener.b2_defaultListener;
}
public void Report(b2ContactConstraint[] constraints)
{
if (m_listener == null)
{
return;
}
//b2ContactImpulse impulse = b2ContactImpulse.Create();
var normals = _impulse.normalImpulses;
var tangens = _impulse.tangentImpulses;
for (int i = 0, count = m_contactCount; i < count; ++i)
{
b2Contact c = m_contacts[i];
var vc = constraints[i];
_impulse.count = vc.pointCount;
for (int j = 0; j < vc.pointCount; ++j)
{
normals[j] = vc.points[j].normalImpulse;
tangens[j] = vc.points[j].tangentImpulse;
}
m_listener.PostSolve(c, ref _impulse);
}
}
public override void PreSolve(b2Contact contact, b2Manifold oldManifold)
{
ICollisionDataExtend collisionData = GetCollisionData(contact);
_Physics2D.ExecutePreCollision(collisionData);
contact.SetEnabled(!collisionData.ContactDisabled);
}
public void Report(b2ContactVelocityConstraint[] constraints)
{
if (m_listener == null)
{
return;
}
b2ContactImpulse impulse = b2ContactImpulse.Create();
for (int i = 0; i < m_contactCount; ++i)
{
b2Contact c = m_contacts[i];
b2ContactVelocityConstraint vc = constraints[i];
impulse.count = vc.pointCount;
for (int j = 0; j < vc.pointCount; ++j)
{
impulse.normalImpulses[j] = vc.points[j].normalImpulse;
impulse.tangentImpulses[j] = vc.points[j].tangentImpulse;
}
m_listener.PostSolve(c, ref impulse);
}
}
public b2ContactManager()
{
m_contactList = null;
m_contactCount = 0;
m_contactFilter = b2ContactFilter.b2_defaultFilter;
m_contactListener = b2ContactListener.b2_defaultListener;
}
/**
* Set if this fixture is a sensor.
*/
public void SetSensor(bool sensor)
{
if (m_isSensor == sensor)
{
return;
}
m_isSensor = sensor;
if (m_body == null)
{
return;
}
b2ContactEdge edge = m_body.GetContactList();
while (edge != null)
{
b2Contact contact = edge.contact;
b2Fixture fixtureA = contact.GetFixtureA();
b2Fixture fixtureB = contact.GetFixtureB();
if (fixtureA == this || fixtureB == this)
{
contact.SetSensor(fixtureA.IsSensor() || fixtureB.IsSensor());
}
edge = edge.next;
}
}
public b2Contact GetContactList()
{
global::System.IntPtr cPtr = Box2DPINVOKE.b2World_GetContactList__SWIG_0(swigCPtr);
b2Contact ret = (cPtr == global::System.IntPtr.Zero) ? null : new b2Contact(cPtr, false);
return(ret);
}
public override void PreSolve(b2Contact contact, b2Manifold oldManifold)
{
b2Manifold manifold = contact.GetManifold();
if (manifold.pointCount == 0)
{
return;
}
b2Fixture fixtureA = contact.GetFixtureA();
b2Fixture fixtureB = contact.GetFixtureB();
b2Collision.b2GetPointStates(state1, state2, oldManifold, manifold);
contact.GetWorldManifold(ref worldManifold);
for (int i = 0; i < manifold.pointCount && m_pointCount < k_maxContactPoints; ++i)
{
ContactPoint cp = m_points[m_pointCount];
if (cp == null)
{
cp = new ContactPoint();
m_points[m_pointCount] = cp;
}
cp.fixtureA = fixtureA;
cp.fixtureB = fixtureB;
cp.position = worldManifold.points[i];
cp.normal = worldManifold.normal;
cp.state = state2[i];
++m_pointCount;
}
}
public b2ContactManager()
{
m_contactList = null;
m_contactCount = 0;
m_contactFilter = b2ContactFilter.b2_defaultFilter;
m_contactListener = b2ContactListener.b2_defaultListener;
m_broadPhase = new b2BroadPhase();
}
public override void EndContact(b2Contact contact)
{
ICollisionData collisionData = GetCollisionData(contact);
_Physics2D.ExecuteOnCollisionExit(collisionData);
((ColliderNative)collisionData.ColliderA).ExecuteOnCollisionExit(collisionData);
((CollisionDataNative)collisionData).Swap();
((ColliderNative)collisionData.ColliderA).ExecuteOnCollisionExit(collisionData);
}
public override void EndContact(b2Contact contact)
{
foreach (var leg in new[] { Env.legs[1], Env.legs[3] })
{
if (leg == contact.GetFixtureA().GetBody() || leg == contact.GetFixtureB().GetBody())
{
(leg.GetUserData() as CustomBodyData).GroundContact = false;
}
}
}
public virtual void PostSolve(b2Contact contact, b2ContactImpulse impulse)
{
if (SwigDerivedClassHasMethod("PostSolve", swigMethodTypes3))
{
Box2dPINVOKE.b2ContactListener_PostSolveSwigExplicitb2ContactListener(swigCPtr, b2Contact.getCPtr(contact), b2ContactImpulse.getCPtr(impulse));
}
else
{
Box2dPINVOKE.b2ContactListener_PostSolve(swigCPtr, b2Contact.getCPtr(contact), b2ContactImpulse.getCPtr(impulse));
}
}
public virtual void PreSolve(b2Contact contact, b2Manifold oldManifold)
{
if (SwigDerivedClassHasMethod("PreSolve", swigMethodTypes2))
{
Box2dPINVOKE.b2ContactListener_PreSolveSwigExplicitb2ContactListener(swigCPtr, b2Contact.getCPtr(contact), b2Manifold.getCPtr(oldManifold));
}
else
{
Box2dPINVOKE.b2ContactListener_PreSolve(swigCPtr, b2Contact.getCPtr(contact), b2Manifold.getCPtr(oldManifold));
}
}
public virtual void EndContact(b2Contact contact)
{
if (SwigDerivedClassHasMethod("EndContact", swigMethodTypes1))
{
Box2dPINVOKE.b2ContactListener_EndContactSwigExplicitb2ContactListener(swigCPtr, b2Contact.getCPtr(contact));
}
else
{
Box2dPINVOKE.b2ContactListener_EndContact(swigCPtr, b2Contact.getCPtr(contact));
}
}
public override void BeginContact(b2Contact contact)
{
base.BeginContact(contact);
b2Body bodyA = contact.GetFixtureA().Body;
b2Body bodyB = contact.GetFixtureB().Body;
if (((bodyA == Ball) || (bodyB == Ball)) && ((bodyA == Floor) || (bodyB == Floor)))
{
_layer.RequestStopGame();
}
}
public override void BeginContact(b2Contact contact)
{
if (Env.hull == contact.GetFixtureA().GetBody() || Env.hull == contact.GetFixtureB().GetBody())
{
Env.game_over = true;
}
foreach (var leg in new[] { Env.legs[1], Env.legs[3] })
{
if (leg == contact.GetFixtureA().GetBody() || leg == contact.GetFixtureB().GetBody())
{
(leg.GetUserData() as CustomBodyData).GroundContact = true;
}
}
}
internal CollisionDataNative(b2Contact contact, IPhysicsObject physicsObjectA, ICollider colliderA, int childIndexA,
IPhysicsObject physicsObjectB, ICollider colliderB, int childIndexB)
{
_Contact = contact;
_PhysicsObjectA = physicsObjectA;
_ColliderA = colliderA;
_ChildIndexA = childIndexA;
_PhysicsObjectB = physicsObjectB;
_ColliderB = colliderB;
_ChildIndexB = childIndexB;
_DetailsCalculated = false;
_Swapped = false;
ContactDisabled = false;
}
private CollisionDataNative GetCollisionData(b2Contact contact)
{
int colliderAId = contact.GetFixtureA().GetUserData().data;
int colliderBId = contact.GetFixtureB().GetUserData().data;
int physicsObjectAId = contact.GetFixtureA().GetBody().GetUserData().data;
int physicsObjectBId = contact.GetFixtureB().GetBody().GetUserData().data;
int childIndexA = contact.GetChildIndexA();
int childIndexB = contact.GetChildIndexB();
IPhysicsObject physicsObjectA = _Physics2D.GetPhysicsObject(physicsObjectAId);
ICollider colliderA = physicsObjectA.GetCollider(colliderAId);
IPhysicsObject physicsObjectB = _Physics2D.GetPhysicsObject(physicsObjectBId);
ICollider colliderB = physicsObjectB.GetCollider(colliderBId);
return(new CollisionDataNative(contact, physicsObjectA, colliderA, childIndexA, physicsObjectB, colliderB, childIndexB));
}
/*
* Position Correction Notes
* =========================
* I tried the several algorithms for position correction of the 2D revolute joint.
* I looked at these systems:
* - simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.
* - suspension bridge with 30 1m long planks of length 1m.
* - multi-link chain with 30 1m long links.
*
* Here are the algorithms:
*
* Baumgarte - A fraction of the position error is added to the velocity error. There is no
* separate position solver.
*
* Pseudo Velocities - After the velocity solver and position integration,
* the position error, Jacobian, and effective mass are recomputed. Then
* the velocity constraints are solved with pseudo velocities and a fraction
* of the position error is added to the pseudo velocity error. The pseudo
* velocities are initialized to zero and there is no warm-starting. After
* the position solver, the pseudo velocities are added to the positions.
* This is also called the First Order World method or the Position LCP method.
*
* Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the
* position error is re-computed for each constraint and the positions are updated
* after the constraint is solved. The radius vectors (aka Jacobians) are
* re-computed too (otherwise the algorithm has horrible instability). The pseudo
* velocity states are not needed because they are effectively zero at the beginning
* of each iteration. Since we have the current position error, we allow the
* iterations to terminate early if the error becomes smaller than b2_linearSlop.
*
* Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed
* each time a constraint is solved.
*
* Here are the results:
* Baumgarte - this is the cheapest algorithm but it has some stability problems,
* especially with the bridge. The chain links separate easily close to the root
* and they jitter as they struggle to pull together. This is one of the most common
* methods in the field. The big drawback is that the position correction artificially
* affects the momentum, thus leading to instabilities and false bounce. I used a
* bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller
* factor makes joints and contacts more spongy.
*
* Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is
* stable. However, joints still separate with large angular velocities. Drag the
* simple pendulum in a circle quickly and the joint will separate. The chain separates
* easily and does not recover. I used a bias factor of 0.2. A larger value lead to
* the bridge collapsing when a heavy cube drops on it.
*
* Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo
* Velocities, but in other ways it is worse. The bridge and chain are much more
* stable, but the simple pendulum goes unstable at high angular velocities.
*
* Full NGS - stable in all tests. The joints display good stiffness. The bridge
* still sags, but this is better than infinite forces.
*
* Recommendations
* Pseudo Velocities are not really worthwhile because the bridge and chain cannot
* recover from joint separation. In other cases the benefit over Baumgarte is small.
*
* Modified NGS is not a robust method for the revolute joint due to the violent
* instability seen in the simple pendulum. Perhaps it is viable with other constraint
* types, especially scalar constraints where the effective mass is a scalar.
*
* This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities
* and is very fast. I don't think we can escape Baumgarte, especially in highly
* demanding cases where high constraint fidelity is not needed.
*
* Full NGS is robust and easy on the eyes. I recommend this as an option for
* higher fidelity simulation and certainly for suspension bridges and long chains.
* Full NGS might be a good choice for ragdolls, especially motorized ragdolls where
* joint separation can be problematic. The number of NGS iterations can be reduced
* for better performance without harming robustness much.
*
* Each joint in a can be handled differently in the position solver. So I recommend
* a system where the user can select the algorithm on a per joint basis. I would
* probably default to the slower Full NGS and let the user select the faster
* Baumgarte method in performance critical scenarios.
*/
/*
* Cache Performance
*
* The Box2D solvers are dominated by cache misses. Data structures are designed
* to increase the number of cache hits. Much of misses are due to random access
* to body data. The constraint structures are iterated over linearly, which leads
* to few cache misses.
*
* The bodies are not accessed during iteration. Instead read only data, such as
* the mass values are stored with the constraints. The mutable data are the constraint
* impulses and the bodies velocities/positions. The impulses are held inside the
* constraint structures. The body velocities/positions are held in compact, temporary
* arrays to increase the number of cache hits. Linear and angular velocity are
* stored in a single array since multiple arrays lead to multiple misses.
*/
/*
* 2D Rotation
*
* R = [cos(theta) -sin(theta)]
* [sin(theta) cos(theta) ]
*
* thetaDot = omega
*
* Let q1 = cos(theta), q2 = sin(theta).
* R = [q1 -q2]
* [q2 q1]
*
* q1Dot = -thetaDot * q2
* q2Dot = thetaDot * q1
*
* q1_new = q1_old - dt * w * q2
* q2_new = q2_old + dt * w * q1
* then normalize.
*
* This might be faster than computing sin+cos.
* However, we can compute sin+cos of the same angle fast.
*/
public b2Island(int bodyCapacity, int contactCapacity, int jointCapacity, b2ContactListener listener)
{
m_bodyCapacity = bodyCapacity;
m_contactCapacity = contactCapacity;
m_jointCapacity = jointCapacity;
m_bodyCount = 0;
m_contactCount = 0;
m_jointCount = 0;
m_listener = listener;
m_bodies = new b2Body[bodyCapacity];
m_contacts = new b2Contact[contactCapacity];
m_joints = new b2Joint[jointCapacity];
m_velocities = Arrays.InitializeWithDefaultInstances <b2Velocity>(m_bodyCapacity);
m_positions = Arrays.InitializeWithDefaultInstances <b2Position>(m_bodyCapacity);
}
public void Report(List <b2ContactConstraint> constraints)
{
if (m_listener == null)
{
return;
}
for (int i = 0; i < m_contactCount; ++i)
{
b2Contact c = m_contacts[i];
b2ContactConstraint cc = constraints[i];
for (int j = 0; j < cc.pointCount; ++j)
{
s_impulse.normalImpulses[j] = cc.points[j].normalImpulse;
s_impulse.tangentImpulses[j] = cc.points[j].tangentImpulse;
}
m_listener.PostSolve(c, s_impulse);
}
}
public virtual void Refilter()
{
if (m_body == null)
{
return;
}
// Flag associated contacts for filtering.
b2ContactEdge edge = m_body.ContactList;
while (edge != null)
{
b2Contact contact = edge.Contact;
b2Fixture fixtureA = contact.GetFixtureA();
b2Fixture fixtureB = contact.GetFixtureB();
if (fixtureA == this || fixtureB == this)
{
contact.FlagForFiltering();
}
edge = edge.Next;
}
b2World world = m_body.World;
if (world == null)
{
return;
}
// Touch each proxy so that new pairs may be created
b2BroadPhase broadPhase = world.ContactManager.BroadPhase;
for (int i = 0; i < m_proxyCount; ++i)
{
broadPhase.TouchProxy(m_proxies[i].proxyId);
}
}
// Implement contact listener.
public override void EndContact(b2Contact contact)
{
b2Fixture fixtureA = contact.GetFixtureA();
b2Fixture fixtureB = contact.GetFixtureB();
if (fixtureA == m_sensor)
{
object userData = fixtureB.Body.UserData;
if (userData != null)
{
m_touching[(int)userData] = false;
}
}
if (fixtureB == m_sensor)
{
object userData = fixtureA.Body.UserData;
if (userData != null)
{
m_touching[(int)userData] = false;
}
}
}
/**
* Set the contact filtering data. This will not update contacts until the next time
* step when either parent body is active and awake.
*/
public void SetFilterData(b2FilterData filter)
{
m_filter = filter.Copy();
if (m_body != null)
{
return;
}
b2ContactEdge edge = m_body.GetContactList();
while (edge != null)
{
b2Contact contact = edge.contact;
b2Fixture fixtureA = contact.GetFixtureA();
b2Fixture fixtureB = contact.GetFixtureB();
if (fixtureA == this || fixtureB == this)
{
contact.FlagForFiltering();
}
edge = edge.next;
}
}
internal static void Destroy(ref b2Contact contact)
{
Debug.Assert(s_initialized == true);
b2Fixture fixtureA = contact.m_fixtureA;
b2Fixture fixtureB = contact.m_fixtureB;
if (contact.m_manifold.pointCount > 0 && fixtureA.IsSensor() == false && fixtureB.IsSensor() == false)
{
fixtureA.GetBody().SetAwake(true);
fixtureB.GetBody().SetAwake(true);
}
b2Shape.Type typeA = fixtureA.GetType();
b2Shape.Type typeB = fixtureB.GetType();
Debug.Assert(0 <= ((int)typeA) && typeB < b2Shape.Type.e_typeCount);
Debug.Assert(0 <= ((int)typeA) && typeB < b2Shape.Type.e_typeCount);
b2ContactDestroyFcn destroyFcn = s_registers[(int)typeA, (int)typeB].destroyFcn;
destroyFcn(ref contact);
}
public override void PostSolve(b2Contact contact, ref b2ContactImpulse impulse)
{
if (m_broke)
{
// The body already broke.
return;
}
// Should the body break?
int count = contact.GetManifold().pointCount;
float maxImpulse = 0.0f;
for (int i = 0; i < count; ++i)
{
maxImpulse = Math.Max(maxImpulse, impulse.normalImpulses[i]);
}
if (maxImpulse > 40.0f)
{
// Flag the body for breaking.
m_break = true;
}
}
public override void PreSolve(b2Contact contact, b2Manifold oldManifold)
{
base.PreSolve(contact, oldManifold);
b2Fixture fixtureA = contact.GetFixtureA();
b2Fixture fixtureB = contact.GetFixtureB();
if (fixtureA != m_platform && fixtureA != m_character)
{
return;
}
if (fixtureB != m_platform && fixtureB != m_character)
{
return;
}
b2Vec2 position = m_character.Body.Position;
if (position.y < m_top + m_radius - 3.0f * b2Settings.b2_linearSlop)
{
contact.SetEnabled(false);
}
}
public void Destroy(b2Contact c)
{
b2Fixture fixtureA = c.GetFixtureA();
b2Fixture fixtureB = c.GetFixtureB();
b2Body bodyA = fixtureA.GetBody();
b2Body bodyB = fixtureB.GetBody();
if (m_contactListener && c.IsTouching())
{
m_contactListener.EndContact(c);
}
// Remove from the world.
if (c.m_prev)
{
c.m_prev.m_next = c.m_next;
}
if (c.m_next)
{
c.m_next.m_prev = c.m_prev;
}
if (c == m_contactList)
{
m_contactList = c.m_next;
}
// Remove from body 1
if (c.m_nodeA.prev)
{
c.m_nodeA.prev.next = c.m_nodeA.next;
}
if (c.m_nodeA.next)
{
c.m_nodeA.next.prev = c.m_nodeA.prev;
}
if (c.m_nodeA == bodyA.m_contactList)
{
bodyA.m_contactList = c.m_nodeA.next;
}
// Remove from body 2
if (c.m_nodeB.prev)
{
c.m_nodeB.prev.next = c.m_nodeB.next;
}
if (c.m_nodeB.next)
{
c.m_nodeB.next.prev = c.m_nodeB.prev;
}
if (c.m_nodeB == bodyB.m_contactList)
{
bodyB.m_contactList = c.m_nodeB.next;
}
// Call the factory.
b2Contact.Destroy(c);
--m_contactCount;
}
/// <summary>
/// This lets you inspect a contact after the solver is finished. This is useful
/// for inspecting impulses.
/// Note: the contact manifold does not include time of impact impulses, which can be
/// arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly
/// in a separate data structure.
/// Note: this is only called for contacts that are touching, solid, and awake.
/// </summary>
public virtual void PostSolve(b2Contact contact, b2ContactImpulse impulse)
{
}
public void AddPair(object proxyUserDataA, object proxyUserDataB)
{
b2FixtureProxy proxyA = (b2FixtureProxy)proxyUserDataA;
b2FixtureProxy proxyB = (b2FixtureProxy)proxyUserDataB;
b2Fixture fixtureA = proxyA.fixture;
b2Fixture fixtureB = proxyB.fixture;
int indexA = proxyA.childIndex;
int indexB = proxyB.childIndex;
b2Body bodyA = fixtureA.GetBody();
b2Body bodyB = fixtureB.GetBody();
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// TODO_ERIN use a hash table to remove a potential bottleneck when both
// bodies have a lot of contacts.
// Does a contact already exist?
b2ContactEdge edge = bodyB.GetContactList();
while (edge)
{
if (edge.other == bodyA)
{
b2Fixture fA = edge.contact.GetFixtureA();
b2Fixture fB = edge.contact.GetFixtureB();
int iA = edge.contact.GetChildIndexA();
int iB = edge.contact.GetChildIndexB();
if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
{
// A contact already exists.
return;
}
if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
{
// A contact already exists.
return;
}
}
edge = edge.next;
}
// Does a joint override collision? Is at least one body dynamic?
if (bodyB.ShouldCollide(bodyA) == false)
{
return;
}
// Check user filtering.
if (m_contactFilter && m_contactFilter.ShouldCollide(fixtureA, fixtureB) == false)
{
return;
}
// Call the factory.
b2Contact c = b2Contact.Create(fixtureA, indexA, fixtureB, indexB, m_allocator);
if (c == null)
{
return;
}
// Contact creation may swap fixtures.
fixtureA = c.GetFixtureA();
fixtureB = c.GetFixtureB();
indexA = c.GetChildIndexA();
indexB = c.GetChildIndexB();
bodyA = fixtureA.GetBody();
bodyB = fixtureB.GetBody();
// Insert into the world.
c.m_prev = null;
c.m_next = m_contactList;
if (m_contactList != null)
{
m_contactList.m_prev = c;
}
m_contactList = c;
// Connect to island graph.
// Connect to body A
c.m_nodeA.contact = c;
c.m_nodeA.other = bodyB;
c.m_nodeA.prev = null;
c.m_nodeA.next = bodyA.m_contactList;
if (bodyA.m_contactList != null)
{
bodyA.m_contactList.prev = &c.m_nodeA;
}
bodyA.m_contactList = &c.m_nodeA;
// Connect to body B
c.m_nodeB.contact = c;
c.m_nodeB.other = bodyA;
c.m_nodeB.prev = null;
c.m_nodeB.next = bodyB.m_contactList;
if (bodyB.m_contactList != null)
{
bodyB.m_contactList.prev = &c.m_nodeB;
}
bodyB.m_contactList = &c.m_nodeB;
// Wake up the bodies
bodyA.SetAwake(true);
bodyB.SetAwake(true);
++m_contactCount;
}
public virtual void DestroyFixture(b2Fixture fixture)
{
Debug.Assert(m_world.IsLocked == false);
if (m_world.IsLocked)
{
return;
}
Debug.Assert(fixture.Body == this);
// Remove the fixture from this body's singly linked list.
Debug.Assert(m_fixtureCount > 0);
b2Fixture node = m_fixtureList;
bool found = false;
while (node != null)
{
if (node == fixture)
{
node = fixture.Next;
found = true;
break;
}
node = node.Next;
}
// You tried to remove a shape that is not attached to this body.
Debug.Assert(found);
// Destroy any contacts associated with the fixture.
b2ContactEdge edge = m_contactList;
while (edge != null)
{
b2Contact c = edge.Contact;
edge = edge.Next;
b2Fixture fixtureA = c.FixtureA;
b2Fixture fixtureB = c.FixtureB;
if (fixture == fixtureA || fixture == fixtureB)
{
// This destroys the contact and removes it from
// this body's contact list.
m_world.ContactManager.Destroy(c);
}
}
if (m_flags.HasFlag(b2BodyFlags.e_activeFlag))
{
b2BroadPhase broadPhase = m_world.ContactManager.BroadPhase;
fixture.DestroyProxies(broadPhase);
}
fixture.Body = null;
fixture.Next = null;
--m_fixtureCount;
// Reset the mass data.
ResetMassData();
}
// Find islands, integrate and solveraints, solve positionraints
public void Solve(b2TimeStep step)
{
m_profile.solveInit = 0.0f;
m_profile.solveVelocity = 0.0f;
m_profile.solvePosition = 0.0f;
// Size the island for the worst case.
b2Island island = new b2Island(m_bodyCount,
m_contactManager.ContactCount,
m_jointCount,
m_contactManager.ContactListener);
// Clear all the island flags.
for (b2Body b = m_bodyList; b != null; b = b.Next)
{
b.BodyFlags &= ~b2BodyFlags.e_islandFlag;
}
for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next)
{
c.ContactFlags &= ~b2ContactFlags.e_islandFlag;
}
for (b2Joint j = m_jointList; j; j = j.Next)
{
j.m_islandFlag = false;
}
// Build and simulate all awake islands.
int stackSize = m_bodyCount;
b2Body[] stack = new b2Body[stackSize];
for (b2Body seed = m_bodyList; seed != null; seed = seed.Next)
{
if (seed.BodyFlags & b2BodyFlags.e_islandFlag)
{
continue;
}
if (seed.IsAwake() == false || seed.IsActive() == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed.BodyType == b2BodyType.b2_staticBody)
{
continue;
}
// Reset island and stack.
island.Clear();
int stackCount = 0;
stack[stackCount++] = seed;
seed.BodyFlags |= b2BodyFlags.e_islandFlag;
// Perform a depth first search (DFS) on theraint graph.
while (stackCount > 0)
{
// Grab the next body off the stack and add it to the island.
b2Body b = stack[--stackCount];
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.BodyType == b2BodyType.b2_staticBody)
{
continue;
}
// Search all contacts connected to this body.
for (b2ContactEdge ce = b.ContactList; ce != null; ce = ce.next)
{
b2Contact contact = ce.contact;
// Has this contact already been added to an island?
if (contact.ContactFlags & b2ContactFlags.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.ContactFlags |= b2ContactType.e_islandFlag;
b2Body other = ce.other;
// Was the other body already added to this island?
if ((other.BodyFlags & b2BodyFlags.e_islandFlag) > 0)
{
continue;
}
stack[stackCount++] = other;
other.BodyFlags |= b2BodyFlags.e_islandFlag;
}
// Search all joints connect to this body.
for (b2JointEdge je = b.JointList; je; je = je.next)
{
if (je.joint.IslandFlag == true)
{
continue;
}
b2Body 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.BodyFlags & b2BodyFlags.e_islandFlag) > 0)
{
continue;
}
stack[stackCount++] = other;
other.BodyFlags |= b2BodyFlags.e_islandFlag;
}
}
b2Profile profile = island.Solve(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_bodyCount; ++i)
{
// Allow static bodies to participate in other islands.
b2Body b = island.m_bodies[i];
if (b.BodyType == b2BodyType.b2_staticBody)
{
b.BodyFlags &= ~b2BodyFlags.e_islandFlag;
}
}
}
{
b2Timer timer;
// Synchronize fixtures, check for out of range bodies.
for (b2Body b = m_bodyList; b != null; b = b.Next)
{
// If a body was not in an island then it did not move.
if ((b.BodyFlags & b2BodyType.e_islandFlag) == 0)
{
continue;
}
if (b.GetBodyType() == b2BodyType.b2_staticBody)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
m_contactManager.FindNewContacts();
m_profile.broadphase = timer.GetMilliseconds();
}
}
public void DrawDebugData()
{
if (m_debugDraw == null)
{
return;
}
b2DrawFlags flags = m_debugDraw.GetFlags();
if (flags & b2DrawFlags.e_shapeBit)
{
for (b2Body b = m_bodyList; b; b = b.Next)
{
b2Transform xf = b.Transform;
for (b2Fixture f = b.FixtureList; f != null; f = f.Next)
{
if (b.IsActive() == false)
{
DrawShape(f, xf, new b2Color(0.5f, 0.5f, 0.3f));
}
else if (b.GetType() == b2BodyType.b2_staticBody)
{
DrawShape(f, xf, new b2Color(0.5f, 0.9f, 0.5f));
}
else if (b.GetType() == b2BodyType.b2_kinematicBody)
{
DrawShape(f, xf, new b2Color(0.5f, 0.5f, 0.9f));
}
else if (b.IsAwake() == false)
{
DrawShape(f, xf, new b2Color(0.6f, 0.6f, 0.6f));
}
else
{
DrawShape(f, xf, new b2Color(0.9f, 0.7f, 0.7f));
}
}
}
}
if (flags.HasFlag(b2DrawFlags.e_jointBit))
{
for (b2Joint j = m_jointList; j != null; j = j.GetNext())
{
DrawJoint(j);
}
}
if (flags.HasFlag(b2DrawFlags.e_pairBit))
{
b2Color color = new b2Color(0.3f, 0.9f, 0.9f);
for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next)
{
//b2Fixture fixtureA = c.GetFixtureA();
//b2Fixture fixtureB = c.GetFixtureB();
//b2Vec2 cA = fixtureA.GetAABB().GetCenter();
//b2Vec2 cB = fixtureB.GetAABB().GetCenter();
//m_debugDraw.DrawSegment(cA, cB, color);
}
}
if (flags.HasFlag(b2DrawFlags.e_aabbBit))
{
b2Color color(0.9f, 0.3f, 0.9f);
b2BroadPhase bp = m_contactManager.BroadPhase;
for (b2Body b = m_bodyList; b != null; b = b.Next)
{
if (b.IsActive() == false)
{
continue;
}
for (b2Fixture f = b.FixtureList; f != null; f = f.Next)
{
for (int i = 0; i < f.ProxyCount; ++i)
{
b2FixtureProxy proxy = f.Proxies[i];
b2AABB aabb = bp.GetFatAABB(proxy.proxyId);
b2Vec2[] vs = new b2Vec2[4];
vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y);
vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y);
vs[2].Set(aabb.upperBound.x, aabb.upperBound.y);
vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y);
m_debugDraw.DrawPolygon(vs, 4, color);
}
}
}
}
if (flags.HasFlag(b2DrawFlags.e_centerOfMassBit))
{
for (b2Body b = m_bodyList; b != null; b = b.Next)
{
b2Transform xf = b.Transform;
xf.p = b.WorldCenter;
m_debugDraw.DrawTransform(xf);
}
}
}
/// <summary>
/// Called when two fixtures begin to touch.
/// </summary>
public virtual void BeginContact(b2Contact contact) { }
// Find TOI contacts and solve them.
public void SolveTOI(b2TimeStep step)
{
b2Island island = new b2Island(2 * b2Settings.b2_maxTOIContacts, b2Settings.b2_maxTOIContacts, 0, m_contactManager.ContactListener);
if (m_stepComplete)
{
for (b2Body b = m_bodyList; b; b = b.Next)
{
b.BodyFlags &= ~b2Body.e_islandFlag;
b.m_sweep.alpha0 = 0.0f;
}
for (b2Contact c = m_contactManager.ContactList; c; c = c.Next)
{
// Invalidate TOI
c.ContactFlags &= ~(b2ContactType.e_toiFlag | b2ContactType.e_islandFlag);
c.m_toiCount = 0;
c.m_toi = 1.0f;
}
}
// Find TOI events and solve them.
for (; ;)
{
// Find the first TOI.
b2Contact minContact = null;
float minAlpha = 1.0f;
for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next)
{
// Is this contact disabled?
if (c.IsEnabled() == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c.m_toiCount > b2Settings.b2_maxSubSteps)
{
continue;
}
float alpha = 1.0f;
if (c.ContactFlags.HasFlag(b2ContactFlags.e_toiFlag))
{
// This contact has a valid cached TOI.
alpha = c.m_toi;
}
else
{
b2Fixture fA = c.GetFixtureA();
b2Fixture fB = c.GetFixtureB();
// Is there a sensor?
if (fA.IsSensor || fB.IsSensor)
{
continue;
}
b2Body bA = fA.Body;
b2Body bB = fB.Body;
b2BodyType typeA = bA.BodyType;
b2BodyType typeB = bB.BodyType;
bool activeA = bA.IsAwake() && typeA != b2BodyType.b2_staticBody;
bool activeB = bB.IsAwake() && typeB != b2BodyType.b2_staticBody;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false)
{
continue;
}
bool collideA = bA.IsBullet() || typeA != b2BodyType.b2_dynamicBody;
bool collideB = bB.IsBullet() || typeB != b2BodyType.b2_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.Sweep.alpha0;
if (bA.Sweep.alpha0 < bB.Sweep.alpha0)
{
alpha0 = bB.Sweep.alpha0;
bA.Sweep.Advance(alpha0);
}
else if (bB.Sweep.alpha0 < bA.Sweep.alpha0)
{
alpha0 = bA.Sweep.alpha0;
bB.Sweep.Advance(alpha0);
}
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
// Compute the time of impact in interval [0, minTOI]
b2TOIInput input = new b2TOIInput();
input.proxyA.Set(fA.Shape, indexA);
input.proxyB.Set(fB.Shape, indexB);
input.sweepA = bA.Sweep;
input.sweepB = bB.Sweep;
input.tMax = 1.0f;
b2TOIOutput output = b2TimeOfImpact(input);
// Beta is the fraction of the remaining portion of the .
float beta = output.t;
if (output.state == b2TOIOutputType.e_touching)
{
alpha = b2Math.b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c.m_toi = alpha;
c.ContactFlags |= b2ContactFlags.e_toiFlag;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == null || 1.0f - 10.0f * b2Settings.b2_epsilon < minAlpha)
{
// No more TOI events. Done!
m_stepComplete = true;
break;
}
{
// Advance the bodies to the TOI.
b2Fixture fA = minContact.GetFixtureA();
b2Fixture fB = minContact.GetFixtureB();
b2Body bA = fA.Body;
b2Body bB = fB.Body;
b2Sweep backup1 = bA.Sweep;
b2Sweep backup2 = bB.Sweep;
bA.Advance(minAlpha);
bB.Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.Update(m_contactManager.ContactListener);
minContact.ContactFlags &= ~b2ContactFlags.e_toiFlag;
++minContact.m_toiCount;
// Is the contact solid?
if (minContact.IsEnabled() == false || minContact.IsTouching() == false)
{
// Restore the sweeps.
minContact.SetEnabled(false);
bA.Sweep = backup1;
bB.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.BodyFlags |= b2BodyFlags.e_islandFlag;
bB.BodyFlags |= b2BodyFlags.e_islandFlag;
minContact.ContentType |= b2ContactFlags.e_islandFlag;
// Get contacts on bodyA and bodyB.
b2Body[] bodies = new b2Body[] { bA, bB };
for (int i = 0; i < 2; ++i)
{
b2Body body = bodies[i];
if (body.BodyType == b2BodyType.b2_dynamicBody)
{
for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next)
{
if (island.BodyCount == island.BodyCapacity)
{
break;
}
if (island.ContactCount == island.ContactCapacity)
{
break;
}
b2Contact contact = ce.contact;
// Has this contact already been added to the island?
if (contact.ContactType & b2ContactType.e_islandFlag)
{
continue;
}
// Only add static, kinematic, or bullet bodies.
b2Body other = ce.other;
if (other.BodyType == b2BodyType.b2_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.
b2Sweep backup = other.Sweep;
if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag))
{
other.Advance(minAlpha);
}
// Update the contact points
contact.Update(m_contactManager.ContactListener);
// Was the contact disabled by the user?
if (contact.IsEnabled() == false)
{
other.Sweep = backup;
other.SynchronizeTransform();
continue;
}
// Are there contact points?
if (contact.IsTouching() == false)
{
other.Sweep = backup;
other.SynchronizeTransform();
continue;
}
// Add the contact to the island
contact.ContactFlags |= b2ContactFlags.e_islandFlag;
island.Add(contact);
// Has the other body already been added to the island?
if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag))
{
continue;
}
// Add the other body to the island.
other.BodyFlags |= b2BodyFlags.e_islandFlag;
if (other.BodyType != b2BodyType.b2_staticBody)
{
other.SetAwake(true);
}
island.Add(other);
}
}
}
b2TimeStep 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_bodyCount; ++i)
{
b2Body body = island.m_bodies[i];
body.BodyFlags &= ~b2BodyFlags.e_islandFlag;
if (body.BodyType != b2BodyType.b2_dynamicBody)
{
continue;
}
body.SynchronizeFixtures();
// Invalidate all contact TOIs on this displaced body.
for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next)
{
ce.Contact.ContactFlags &= ~(b2ContactFlags.e_toiFlag | b2ContactFlags.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;
}
}
}
}
/// <summary>
/// Called when two fixtures cease to touch.
/// </summary>
public virtual void EndContact(b2Contact contact) { }
public new static void Destroy(ref b2Contact contact)
{
contact = null;
}
/// <summary>
/// This is called after a contact is updated. This allows you to inspect a
/// contact before it goes to the solver. If you are careful, you can modify the
/// contact manifold (e.g. disable contact).
/// A copy of the old manifold is provided so that you can detect changes.
/// Note: this is called only for awake bodies.
/// Note: this is called even when the number of contact points is zero.
/// Note: this is not called for sensors.
/// Note: if you set the number of contact points to zero, you will not
/// get an EndContact callback. However, you may get a BeginContact callback
/// the next step.
/// </summary>
public virtual void PreSolve(b2Contact contact, b2Manifold oldManifold)
{
}
