public b2ContactManager()
 {
     m_contactList = null;
     m_contactCount = 0;
     m_contactFilter = b2ContactFilter.b2_defaultFilter;
     m_contactListener = b2ContactListener.b2_defaultListener;
 }
Esempio n. 2
0
        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);
            }
        }
Esempio n. 3
0
        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);
            }
        }
Esempio n. 5
0
 public b2ContactManager()
 {
     m_contactList     = null;
     m_contactCount    = 0;
     m_contactFilter   = b2ContactFilter.b2_defaultFilter;
     m_contactListener = b2ContactListener.b2_defaultListener;
 }
Esempio n. 6
0
        /**
         * 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;
            }
        }
Esempio n. 7
0
        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);
        }
Esempio n. 8
0
        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;
            }
        }
Esempio n. 9
0
 public b2ContactManager()
 {
     m_contactList     = null;
     m_contactCount    = 0;
     m_contactFilter   = b2ContactFilter.b2_defaultFilter;
     m_contactListener = b2ContactListener.b2_defaultListener;
     m_broadPhase      = new b2BroadPhase();
 }
Esempio n. 10
0
        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);
        }
Esempio n. 11
0
 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;
         }
     }
 }
Esempio n. 12
0
 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));
     }
 }
Esempio n. 13
0
 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));
     }
 }
Esempio n. 14
0
 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));
     }
 }
Esempio n. 15
0
        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();
            }
        }
Esempio n. 16
0
 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;
         }
     }
 }
Esempio n. 17
0
 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;
 }
Esempio n. 18
0
        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));
        }
Esempio n. 19
0
    /*
     * 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);
    }
Esempio n. 20
0
        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);
            }
        }
Esempio n. 21
0
        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);
            }
        }
Esempio n. 22
0
        // 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;
                }
            }
        }
Esempio n. 23
0
        /**
         * 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;
            }
        }
Esempio n. 24
0
    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);
    }
Esempio n. 25
0
        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;
            }
        }
Esempio n. 26
0
        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;
        }
Esempio n. 30
0
        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();
        }
Esempio n. 31
0
        // 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();
            }
        }
Esempio n. 32
0
        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) { }
Esempio n. 34
0
        // 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) { }
Esempio n. 36
0
 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)
 {
 }