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); } }
/** * 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) { }