Ejemplo n.º 1
0
        /// Destroy a fixture. This removes the fixture from the broad-phase and
        /// destroys all contacts associated with this fixture. This will
        /// automatically adjust the mass of the body if the body is dynamic and the
        /// fixture has positive density.
        /// All fixtures attached to a body are implicitly destroyed when the body is destroyed.
        /// @param fixture the fixture to be removed.
        /// @warning This function is locked during callbacks.
        public void DestroyFixture(Fixture fixture)
        {
            Debug.Assert(_world.IsLocked == false);
            if (_world.IsLocked == true)
            {
                return;
            }

            Debug.Assert(fixture._body == this);

            // Remove the fixture from this body's singly linked list.
            Debug.Assert(_fixtureCount > 0);
            Fixture node  = _fixtureList;
            bool    found = false;

            while (node != null)
            {
                if (node == fixture)
                {
                    _fixtureList = fixture._next;
                    found        = true;
                    break;
                }

                node = node._next;
            }

            // You tried to remove a shape that is not attached to this body.
            Debug.Assert(found);

            // Destroy any contacts associated with the fixture.
            ContactEdge edge = _contactList;

            while (edge != null)
            {
                Contact c = edge.Contact;
                edge = edge.Next;

                Fixture fixtureA = c.GetFixtureA();
                Fixture fixtureB = c.GetFixtureB();

                if (fixture == fixtureA || fixture == fixtureB)
                {
                    // This destroys the contact and removes it from
                    // this body's contact list.
                    _world._contactManager.Destroy(c);
                }
            }

            if ((_flags & BodyFlags.Active) == BodyFlags.Active)
            {
                Debug.Assert(fixture._proxyId != BroadPhase.NullProxy);

                BroadPhase broadPhase = _world._contactManager._broadPhase;
                fixture.DestroyProxies(broadPhase);
            }

            fixture.Destroy();
            fixture._body = null;
            fixture._next = null;

            --_fixtureCount;


            ResetMassData();
        }
Ejemplo n.º 2
0
        void Solve(ref TimeStep step)
        {
            // Size the island for the worst case.
            _island.Reset(_bodyCount,
                          _contactManager._contactCount,
                          _jointCount,
                          _contactManager.ContactListener);

            // Clear all the island flags.
            for (Body b = _bodyList; b != null; b = b._next)
            {
                b._flags &= ~BodyFlags.Island;
            }
            for (Contact c = _contactManager._contactList; c != null; c = c._next)
            {
                c._flags &= ~ContactFlags.Island;
            }
            for (Joint j = _jointList; j != null; j = j._next)
            {
                j._islandFlag = false;
            }

            // Build and simulate all awake islands.
            int stackSize = _bodyCount;

            if (stackSize > stack.Length)
            {
                stack = new Body[Math.Max(stack.Length * 2, stackSize)];
            }

            for (Body seed = _bodyList; seed != null; seed = seed._next)
            {
                if ((seed._flags & (BodyFlags.Island)) != BodyFlags.None)
                {
                    continue;
                }

                if (seed.IsAwake() == false || seed.IsActive() == false)
                {
                    continue;
                }

                // The seed can be dynamic or kinematic.
                if (seed.GetType() == BodyType.Static)
                {
                    continue;
                }

                // Reset island and stack.
                _island.Clear();
                int stackCount = 0;
                stack[stackCount++] = seed;
                seed._flags        |= BodyFlags.Island;

                // Perform a depth first search (DFS) on the constraint graph.
                while (stackCount > 0)
                {
                    // Grab the next body off the stack and add it to the island.
                    Body b = stack[--stackCount];
                    Debug.Assert(b.IsActive() == true);
                    _island.Add(b);

                    // Make sure the body is awake.
                    b.SetAwake(true);

                    // To keep islands as small as possible, we don't
                    // propagate islands across static bodies.
                    if (b.GetType() == BodyType.Static)
                    {
                        continue;
                    }

                    // Search all contacts connected to this body.
                    for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next)
                    {
                        Contact contact = ce.Contact;

                        // Has this contact already been added to an island?
                        if ((contact._flags & ContactFlags.Island) != ContactFlags.None)
                        {
                            continue;
                        }

                        // Is this contact solid and touching?
                        if (!ce.Contact.IsEnabled() || !ce.Contact.IsTouching())
                        {
                            continue;
                        }

                        // Skip sensors.
                        bool sensorA = contact._fixtureA._isSensor;
                        bool sensorB = contact._fixtureB._isSensor;
                        if (sensorA || sensorB)
                        {
                            continue;
                        }

                        _island.Add(contact);
                        contact._flags |= ContactFlags.Island;

                        Body other = ce.Other;

                        // Was the other body already added to this island?
                        if ((other._flags & BodyFlags.Island) != BodyFlags.None)
                        {
                            continue;
                        }

                        Debug.Assert(stackCount < stackSize);
                        stack[stackCount++] = other;
                        other._flags       |= BodyFlags.Island;
                    }

                    // Search all joints connect to this body.
                    for (JointEdge je = b._jointList; je != null; je = je.Next)
                    {
                        if (je.Joint._islandFlag == true)
                        {
                            continue;
                        }

                        Body other = je.Other;

                        // Don't simulate joints connected to inactive bodies.
                        if (other.IsActive() == false)
                        {
                            continue;
                        }

                        _island.Add(je.Joint);
                        je.Joint._islandFlag = true;

                        if ((other._flags & BodyFlags.Island) != BodyFlags.None)
                        {
                            continue;
                        }

                        Debug.Assert(stackCount < stackSize);
                        stack[stackCount++] = other;
                        other._flags       |= BodyFlags.Island;
                    }
                }

                _island.Solve(ref step, Gravity, _allowSleep);

                // Post solve cleanup.
                for (int i = 0; i < _island._bodyCount; ++i)
                {
                    // Allow static bodies to participate in other islands.
                    Body b = _island._bodies[i];
                    if (b.GetType() == BodyType.Static)
                    {
                        b._flags &= ~BodyFlags.Island;
                    }
                }
            }

            // Synchronize fixtures, check for out of range bodies.
            for (Body b = _bodyList; b != null; b = b.GetNext())
            {
                if (!b.IsAwake() || !b.IsActive())
                {
                    continue;
                }

                if (b.GetType() == BodyType.Static)
                {
                    continue;
                }

                // Update fixtures (for broad-phase).
                b.SynchronizeFixtures();
            }

            // Look for new contacts.
            _contactManager.FindNewContacts();
        }
Ejemplo n.º 3
0
        // Advance a dynamic body to its first time of contact
        // and adjust the position to ensure clearance.
        void SolveTOI(Body body)
        {
            // Find the minimum contact.
            Contact toiContact = null;
            float   toi        = 1.0f;
            bool    found;
            int     count;
            int     iter = 0;

            bool bullet = body.IsBullet;

            // Iterate until all contacts agree on the minimum TOI. We have
            // to iterate because the TOI algorithm may skip some intermediate
            // collisions when objects rotate through each other.
            do
            {
                count = 0;
                found = false;
                for (ContactEdge ce = body._contactList; ce != null; ce = ce.Next)
                {
                    Body     other = ce.Other;
                    BodyType type  = other.GetType();

                    // Only bullets perform TOI with dynamic bodies.
                    if (bullet == true)
                    {
                        // Bullets only perform TOI with bodies that have their TOI resolved.
                        if ((other._flags & BodyFlags.Toi) == 0)
                        {
                            continue;
                        }
                    }
                    else if (type == BodyType.Dynamic)
                    {
                        continue;
                    }

                    // Check for a disabled contact.
                    Contact contact = ce.Contact;
                    if (contact.IsEnabled() == false)
                    {
                        continue;
                    }

                    // Prevent infinite looping.
                    if (contact._toiCount > 10)
                    {
                        continue;
                    }

                    Fixture fixtureA = contact._fixtureA;
                    Fixture fixtureB = contact._fixtureB;

                    // Cull sensors.
                    if (fixtureA.IsSensor() || fixtureB.IsSensor())
                    {
                        continue;
                    }

                    Body bodyA = fixtureA._body;
                    Body bodyB = fixtureB._body;

                    // Compute the time of impact in interval [0, minTOI]
                    TOIInput input = new TOIInput();
                    input.proxyA.Set(fixtureA.GetShape());
                    input.proxyB.Set(fixtureB.GetShape());
                    input.sweepA = bodyA._sweep;
                    input.sweepB = bodyB._sweep;
                    input.tMax   = toi;

                    TOIOutput output;
                    TimeOfImpact.CalculateTimeOfImpact(out output, ref input);

                    if (output.State == TOIOutputState.Touching && output.t < toi)
                    {
                        toiContact = contact;
                        toi        = output.t;
                        found      = true;
                    }

                    ++count;
                }

                ++iter;
            } while (found && count > 1 && iter < 50);

            if (toiContact == null)
            {
                return;
            }

            // Advance the body to its safe time.
            Sweep backup = body._sweep;

            body.Advance(toi);

            ++toiContact._toiCount;

            // Update all the valid contacts on this body and build a contact island.
            count = 0;
            for (ContactEdge ce = body._contactList; (ce != null) && (count < Settings.b2_maxTOIContacts); ce = ce.Next)
            {
                Body     other = ce.Other;
                BodyType type  = other.GetType();

                // Only perform correction with static bodies, so the
                // body won't get pushed out of the world.
                if (type != BodyType.Static)
                {
                    continue;
                }

                // Check for a disabled contact.
                Contact contact = ce.Contact;
                if (contact.IsEnabled() == false)
                {
                    continue;
                }

                Fixture fixtureA = contact._fixtureA;
                Fixture fixtureB = contact._fixtureB;

                // Cull sensors.
                if (fixtureA.IsSensor() || fixtureB.IsSensor())
                {
                    continue;
                }

                // The contact likely has some new contact points. The listener
                // gives the user a chance to disable the contact;
                contact.Update(_contactManager.ContactListener);

                // Did the user disable the contact?
                if (contact.IsEnabled() == false)
                {
                    if (contact == toiContact)
                    {
                        // Restore the body's sweep.
                        body._sweep = backup;
                        body.SynchronizeTransform();

                        // Recurse because the TOI has been invalidated.
                        SolveTOI(body);
                        return;
                    }

                    // Skip this contact.
                    continue;
                }

                if (contact.IsTouching() == false)
                {
                    continue;
                }

                _toiContacts[count] = contact;
                ++count;
            }

            // Reduce the TOI body's overlap with the contact island.
            _toiSolver.Initialize(_toiContacts, count, body);

            const float k_toiBaumgarte = 0.75f;

            //bool solved = false;
            for (int i = 0; i < 20; ++i)
            {
                bool contactsOkay = _toiSolver.Solve(k_toiBaumgarte);
                if (contactsOkay)
                {
                    //solved = true;
                    break;
                }
            }
        }
Ejemplo n.º 4
0
        /// Destroy a joint. This may cause the connected bodies to begin colliding.
        /// @warning This function is locked during callbacks.
        public void DestroyJoint(Joint j)
        {
            Debug.Assert(!IsLocked);
            if (IsLocked)
            {
                return;
            }

            bool collideConnected = j._collideConnected;

            // Remove from the doubly linked list.
            if (j._prev != null)
            {
                j._prev._next = j._next;
            }

            if (j._next != null)
            {
                j._next._prev = j._prev;
            }

            if (j == _jointList)
            {
                _jointList = j._next;
            }

            // Disconnect from island graph.
            Body bodyA = j._bodyA;
            Body bodyB = j._bodyB;

            // Wake up connected bodies.
            bodyA.SetAwake(true);
            bodyB.SetAwake(true);

            // Remove from body 1.
            if (j._edgeA.Prev != null)
            {
                j._edgeA.Prev.Next = j._edgeA.Next;
            }

            if (j._edgeA.Next != null)
            {
                j._edgeA.Next.Prev = j._edgeA.Prev;
            }

            if (j._edgeA == bodyA._jointList)
            {
                bodyA._jointList = j._edgeA.Next;
            }

            j._edgeA.Prev = null;
            j._edgeA.Next = null;

            // Remove from body 2
            if (j._edgeB.Prev != null)
            {
                j._edgeB.Prev.Next = j._edgeB.Next;
            }

            if (j._edgeB.Next != null)
            {
                j._edgeB.Next.Prev = j._edgeB.Prev;
            }

            if (j._edgeB == bodyB._jointList)
            {
                bodyB._jointList = j._edgeB.Next;
            }

            j._edgeB.Prev = null;
            j._edgeB.Next = null;

            Debug.Assert(_jointCount > 0);
            --_jointCount;

            // If the joint prevents collisions, then flag any contacts for filtering.
            if (collideConnected == false)
            {
                ContactEdge edge = bodyB.GetContactList();
                while (edge != null)
                {
                    if (edge.Other == bodyA)
                    {
                        // Flag the contact for filtering at the next time step (where either
                        // body is awake).
                        edge.Contact.FlagForFiltering();
                    }

                    edge = edge.Next;
                }
            }
        }
Ejemplo n.º 5
0
        /// Create a joint to rain bodies together. No reference to the definition
        /// is retained. This may cause the connected bodies to cease colliding.
        /// @warning This function is locked during callbacks.
        public Joint CreateJoint(JointDef def)
        {
            Debug.Assert(!IsLocked);
            if (IsLocked)
            {
                return(null);
            }

            Joint j = Joint.Create(def);

            // Connect to the world list.
            j._prev = null;
            j._next = _jointList;
            if (_jointList != null)
            {
                _jointList._prev = j;
            }
            _jointList = j;
            ++_jointCount;

            // Connect to the bodies' doubly linked lists.
            j._edgeA.Joint = j;
            j._edgeA.Other = j._bodyB;
            j._edgeA.Prev  = null;
            j._edgeA.Next  = j._bodyA._jointList;

            if (j._bodyA._jointList != null)
            {
                j._bodyA._jointList.Prev = j._edgeA;
            }

            j._bodyA._jointList = j._edgeA;

            j._edgeB.Joint = j;
            j._edgeB.Other = j._bodyA;
            j._edgeB.Prev  = null;
            j._edgeB.Next  = j._bodyB._jointList;

            if (j._bodyB._jointList != null)
            {
                j._bodyB._jointList.Prev = j._edgeB;
            }

            j._bodyB._jointList = j._edgeB;

            Body bodyA = def.bodyA;
            Body bodyB = def.bodyB;

            bool staticA = bodyA.GetType() == BodyType.Static;
            bool staticB = bodyB.GetType() == BodyType.Static;

            // If the joint prevents collisions, then flag any contacts for filtering.
            if (def.collideConnected == false)
            {
                ContactEdge edge = bodyB.GetContactList();
                while (edge != null)
                {
                    if (edge.Other == bodyA)
                    {
                        // Flag the contact for filtering at the next time step (where either
                        // body is awake).
                        edge.Contact.FlagForFiltering();
                    }

                    edge = edge.Next;
                }
            }

            // Note: creating a joint doesn't wake the bodies.

            return(j);
        }
Ejemplo n.º 6
0
        /// Destroy a rigid body given a definition. No reference to the definition
        /// is retained. This function is locked during callbacks.
        /// @warning This automatically deletes all associated shapes and joints.
        /// @warning This function is locked during callbacks.
        public void DestroyBody(Body b)
        {
            Debug.Assert(_bodyCount > 0);
            Debug.Assert(!IsLocked);
            if (IsLocked)
            {
                return;
            }

            // Delete the attached joints.
            JointEdge je = b._jointList;

            while (je != null)
            {
                JointEdge je0 = je;
                je = je.Next;

                if (DestructionListener != null)
                {
                    DestructionListener.SayGoodbye(je0.Joint);
                }

                DestroyJoint(je0.Joint);
            }
            b._jointList = null;

            // Delete the attached contacts.
            ContactEdge ce = b._contactList;

            while (ce != null)
            {
                ContactEdge ce0 = ce;
                ce = ce.Next;
                _contactManager.Destroy(ce0.Contact);
            }
            b._contactList = null;

            // Delete the attached fixtures. This destroys broad-phase proxies.
            Fixture f = b._fixtureList;

            while (f != null)
            {
                Fixture f0 = f;
                f = f._next;

                if (DestructionListener != null)
                {
                    DestructionListener.SayGoodbye(f0);
                }

                f0.DestroyProxy(_contactManager._broadPhase);
                f0.Destroy();
            }
            b._fixtureList  = null;
            b._fixtureCount = 0;

            // Remove world body list.
            if (b._prev != null)
            {
                b._prev._next = b._next;
            }

            if (b._next != null)
            {
                b._next._prev = b._prev;
            }

            if (b == _bodyList)
            {
                _bodyList = b._next;
            }

            --_bodyCount;
        }
Ejemplo n.º 7
0
        /// <summary>
        /// Set the active state of the body. An inactive body is not
        /// simulated and cannot be collided with or woken up.
        /// If you pass a flag of true, all fixtures will be added to the
        /// broad-phase.
        /// If you pass a flag of false, all fixtures will be removed from
        /// the broad-phase and all contacts will be destroyed.
        /// Fixtures and joints are otherwise unaffected. You may continue
        /// to create/destroy fixtures and joints on inactive bodies.
        /// Fixtures on an inactive body are implicitly inactive and will
        /// not participate in collisions, ray-casts, or queries.
        /// Joints connected to an inactive body are implicitly inactive.
        /// An inactive body is still owned by a b2World object and remains
        /// in the body list.
        /// </summary>
        /// <param name="flag"></param>
        public void SetActive(bool flag)
        {
            if (flag == IsActive())
            {
                return;
            }

            if (flag)
            {
                _flags |= BodyFlags.Active;

                // Create all proxies.
                BroadPhase broadPhase = _world._contactManager._broadPhase;
                for (Fixture f = _fixtureList; f != null; f = f._next)
                {
                    f.CreateProxy(broadPhase, ref _xf);
                }

                // Contacts are created the next time step.
            }
            else
            {
                _flags &= ~BodyFlags.Active;

                // Destroy all proxies.
                BroadPhase broadPhase = _world._contactManager._broadPhase;
                for (Fixture f = _fixtureList; f != null; f = f._next)
                {
                    f.DestroyProxy(broadPhase);
                }

                // Destroy the attached contacts.
                ContactEdge ce = _contactList;
                while (ce != null)
                {
                    ContactEdge ce0 = ce;
                    ce = ce.Next;
                    _world._contactManager.Destroy(ce0.Contact);
                }
                _contactList = null;
            }
        }
Ejemplo n.º 8
0
        // Broad-phase callback.
        internal void AddPair(FixtureProxy proxyA, FixtureProxy proxyB)
        {
            Fixture fixtureA = proxyA.fixture;
            Fixture fixtureB = proxyB.fixture;

            int indexA = proxyA.childIndex;
            int indexB = proxyB.childIndex;

            Body bodyA = fixtureA.GetBody();
            Body bodyB = fixtureB.GetBody();

            // Are the fixtures on the same body?
            if (bodyA == bodyB)
            {
                return;
            }

            // Does a contact already exist?
            ContactEdge edge = bodyB.GetContactList();

            while (edge != null)
            {
                if (edge.Other == bodyA)
                {
                    Fixture fA = edge.Contact.GetFixtureA();
                    Fixture fB = edge.Contact.GetFixtureB();
                    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 (ContactFilter != null && ContactFilter.ShouldCollide(fixtureA, fixtureB) == false)
            {
                return;
            }

            // Call the factory.
            Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB);

            // 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._prev = null;
            c._next = _contactList;
            if (_contactList != null)
            {
                _contactList._prev = c;
            }
            _contactList = c;

            // Connect to island graph.

            // Connect to body A
            c._nodeA.Contact = c;
            c._nodeA.Other   = bodyB;

            c._nodeA.Prev = null;
            c._nodeA.Next = bodyA._contactList;
            if (bodyA._contactList != null)
            {
                bodyA._contactList.Prev = c._nodeA;
            }
            bodyA._contactList = c._nodeA;

            // Connect to body B
            c._nodeB.Contact = c;
            c._nodeB.Other   = bodyA;

            c._nodeB.Prev = null;
            c._nodeB.Next = bodyB._contactList;
            if (bodyB._contactList != null)
            {
                bodyB._contactList.Prev = c._nodeB;
            }
            bodyB._contactList = c._nodeB;

            ++_contactCount;
        }