예제 #1
0
파일: Contact.cs 프로젝트: vb0067/LGame
        /// Get the world manifold.
        public void GetWorldManifold(out WorldManifold worldManifold)
        {
            worldManifold = new WorldManifold();

            Body  bodyA  = _fixtureA.GetBody();
            Body  bodyB  = _fixtureB.GetBody();
            Shape shapeA = _fixtureA.GetShape();
            Shape shapeB = _fixtureB.GetShape();

            worldManifold.Initialize(Manifold, bodyA.GetTransform(), shapeA._radius, bodyB.GetTransform(), shapeB._radius);
        }
예제 #2
0
파일: Contact.cs 프로젝트: vb0067/LGame
        public Contact(Fixture fixtureA, Fixture fixtureB)
        {
            Flags = 0;

            if (fixtureA.IsSensor || fixtureB.IsSensor)
            {
                Flags |= ContactFlag.SensorFlag;
            }

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

            if (bodyA.IsStatic() || bodyA.IsBullet() || bodyB.IsStatic() || bodyB.IsBullet())
            {
                Flags |= ContactFlag.ContinuousFlag;
            }
            else
            {
                Flags &= ~ContactFlag.ContinuousFlag;
            }

            _fixtureA = fixtureA;
            _fixtureB = fixtureB;

            Manifold            = new Manifold();
            Manifold.PointCount = 0;

            Prev = null;
            Next = null;

            NodeA         = new ContactEdge();
            NodeA.Contact = null;
            NodeA.Prev    = null;
            NodeA.Next    = null;
            NodeA.Other   = null;

            NodeB         = new ContactEdge();
            NodeB.Contact = null;
            NodeB.Prev    = null;
            NodeB.Next    = null;
            NodeB.Other   = null;
        }
예제 #3
0
        public void Destroy(Contact c)
        {
            Fixture fixtureA = c.GetFixtureA();
            Fixture fixtureB = c.GetFixtureB();
            Body    body1    = fixtureA.GetBody();
            Body    body2    = fixtureB.GetBody();

            if (c.Manifold.PointCount > 0)
            {
                _contactListener.EndContact(c);
            }

            // Remove from the world.
            if (c.Prev != null)
            {
                c.Prev.Next = c.Next;
            }

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

            if (c == _contactList)
            {
                _contactList = c.Next;
            }

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

            if (c.NodeA.Next != null)
            {
                c.NodeA.Next.Prev = c.NodeA.Prev;
            }

            if (c.NodeA == body1._contactList)
            {
                body1._contactList = c.NodeA.Next;
            }

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

            if (c.NodeB.Next != null)
            {
                c.NodeB.Next.Prev = c.NodeB.Prev;
            }

            if (c.NodeB == body2._contactList)
            {
                body2._contactList = c.NodeB.Next;
            }

            // Call the factory.
            Contact.Destroy(c);
            --_contactCount;
        }
예제 #4
0
        public void AddPair(object proxyUserDataA, object proxyUserDataB)
        {
            Fixture fixtureA = (Fixture)proxyUserDataA;
            Fixture fixtureB = (Fixture)proxyUserDataB;

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

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

            // Are both bodies static?
            if (bodyA.IsStatic() && bodyB.IsStatic())
            {
                return;
            }

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

            while (edge != null)
            {
                if (edge.Other == bodyA)
                {
                    Fixture fA = edge.Contact.GetFixtureA();
                    Fixture fB = edge.Contact.GetFixtureB();
                    if (fA == fixtureA && fB == fixtureB)
                    {
                        // A contact already exists.
                        return;
                    }

                    if (fA == fixtureB && fB == fixtureA)
                    {
                        // A contact already exists.
                        return;
                    }
                }

                edge = edge.Next;
            }

            // Does a joint override collision?
            if (bodyB.IsConnected(bodyA))
            {
                return;
            }

            // Check user filtering.
            if (_contactFilter.ShouldCollide(fixtureA, fixtureB) == false)
            {
                return;
            }

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

            // Contact creation may swap fixtures.
            fixtureA = c.GetFixtureA();
            fixtureB = c.GetFixtureB();
            bodyA    = fixtureA.GetBody();
            bodyB    = fixtureB.GetBody();

            // Insert into the world.
            c.Prev = null;
            c.Next = _contactList;
            if (_contactList != null)
            {
                _contactList.Prev = c;
            }
            _contactList = c;

            // Connect to island graph.

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

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

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

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

            ++_contactCount;
        }
예제 #5
0
        // This is the top level collision call for the time step. Here
        // all the narrow phase collision is processed for the world
        // contact list.
        public void Collide()
        {
            // Update awake contacts.
            Contact c = _contactList;

            while (c != null)
            {
                Fixture fixtureA = c.GetFixtureA();
                Fixture fixtureB = c.GetFixtureB();
                Body    bodyA    = fixtureA.GetBody();
                Body    bodyB    = fixtureB.GetBody();

                if (bodyA.IsSleeping() && bodyB.IsSleeping())
                {
                    c = c.GetNext();
                    continue;
                }

                // Is this contact flagged for filtering?
                if ((c.Flags & ContactFlag.FilterFlag) == ContactFlag.FilterFlag)
                {
                    //TODO: The following code (next 4 if blocks) use a class and thus copy by ref. It might expect to copy by value
                    // Are both bodies static?
                    if (bodyA.IsStatic() && bodyB.IsStatic())
                    {
                        Contact cNuke = c;
                        c = cNuke.GetNext();
                        Destroy(cNuke);
                        continue;
                    }

                    // Does a joint override collision?
                    if (bodyB.IsConnected(bodyA))
                    {
                        Contact cNuke = c;
                        c = cNuke.GetNext();
                        Destroy(cNuke);
                        continue;
                    }

                    // Check user filtering.
                    if (_contactFilter.ShouldCollide(fixtureA, fixtureB) == false)
                    {
                        Contact cNuke = c;
                        c = cNuke.GetNext();
                        Destroy(cNuke);
                        continue;
                    }

                    // Clear the filtering flag.
                    c.Flags &= ~ContactFlag.FilterFlag;
                }

                int  proxyIdA = fixtureA.ProxyId;
                int  proxyIdB = fixtureB.ProxyId;
                bool overlap  = _broadPhase.TestOverlap(proxyIdA, proxyIdB);

                // Here we destroy contacts that cease to overlap in the broad-phase.
                if (overlap == false)
                {
                    Contact cNuke = c;
                    c = cNuke.GetNext();
                    Destroy(cNuke);
                    continue;
                }

                // The contact persists.
                c.Update(_contactListener);
                c = c.GetNext();
            }
        }
예제 #6
0
        public Contact(Fixture fixtureA, Fixture fixtureB)
        {
            Flags = 0;

            if (fixtureA.IsSensor || fixtureB.IsSensor)
            {
                Flags |= ContactFlag.SensorFlag;
            }

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

            if (bodyA.IsStatic() || bodyA.IsBullet() || bodyB.IsStatic() || bodyB.IsBullet())
            {
                Flags |= ContactFlag.ContinuousFlag;
            }
            else
            {
                Flags &= ~ContactFlag.ContinuousFlag;
            }

            _fixtureA = fixtureA;
            _fixtureB = fixtureB;

            Manifold = new Manifold();
            Manifold.PointCount = 0;

            Prev = null;
            Next = null;

            NodeA = new ContactEdge();
            NodeA.Contact = null;
            NodeA.Prev = null;
            NodeA.Next = null;
            NodeA.Other = null;

            NodeB = new ContactEdge();
            NodeB.Contact = null;
            NodeB.Prev = null;
            NodeB.Next = null;
            NodeB.Other = null;
        }
예제 #7
0
파일: World.cs 프로젝트: vb0067/LGame
        // Find TOI contacts and solve them.
        private void SolveTOI(TimeStep step)
        {
            // Reserve an island and a queue for TOI island solution.
            Island island = new Island(_bodyCount,
                                       Settings.MaxTOIContactsPerIsland,
                                       Settings.MaxTOIJointsPerIsland,
                                       _contactManager._contactListener);

            //Simple one pass queue
            //Relies on the fact that we're only making one pass
            //through and each body can only be pushed/popped once.
            //To push:
            //  queue[queueStart+queueSize++] = newElement;
            //To pop:
            //	poppedElement = queue[queueStart++];
            //  --queueSize;
            int queueCapacity = _bodyCount;

            Body[] queue = new Body[queueCapacity];

            for (Body b = _bodyList; b != null; b = b._next)
            {
                b._flags   &= ~Body.BodyFlags.Island;
                b._sweep.T0 = 0.0f;
            }

            for (Contact c = _contactManager._contactList; c != null; c = c.Next)
            {
                // Invalidate TOI
                c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag);
            }

            for (Joint j = _jointList; j != null; j = j._next)
            {
                j._islandFlag = false;
            }

            // Find TOI events and solve them.
            for (; ;)
            {
                // Find the first TOI.
                Contact minContact = null;
                float   minTOI     = 1.0f;

                for (Contact c = _contactManager._contactList; c != null; c = c.Next)
                {
                    // Can this contact generate a solid TOI contact?
                    if (c.IsSolid() == false || c.IsContinuous() == false)
                    {
                        continue;
                    }

                    // TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.

                    float toi = 1.0f;
                    if ((c.Flags & ContactFlag.ToiFlag) != 0)
                    {
                        // This contact has a valid cached TOI.
                        toi = c.Toi;
                    }
                    else
                    {
                        // Compute the TOI for this contact.
                        Fixture s1 = c.GetFixtureA();
                        Fixture s2 = c.GetFixtureB();
                        Body    b1 = s1.GetBody();
                        Body    b2 = s2.GetBody();

                        if ((b1.IsStatic() || b1.IsSleeping()) && (b2.IsStatic() || b2.IsSleeping()))
                        {
                            continue;
                        }

                        // Put the sweeps onto the same time interval.
                        float t0 = b1._sweep.T0;

                        if (b1._sweep.T0 < b2._sweep.T0)
                        {
                            t0 = b2._sweep.T0;
                            b1._sweep.Advance(t0);
                        }
                        else if (b2._sweep.T0 < b1._sweep.T0)
                        {
                            t0 = b1._sweep.T0;
                            b2._sweep.Advance(t0);
                        }

                        Box2DXDebug.Assert(t0 < 1.0f);

                        // Compute the time of impact.
                        toi = c.ComputeTOI(b1._sweep, b2._sweep);

                        Box2DXDebug.Assert(0.0f <= toi && toi <= 1.0f);

                        // If the TOI is in range ...
                        if (0.0f < toi && toi < 1.0f)
                        {
                            // Interpolate on the actual range.
                            toi = Math.Min((1.0f - toi) * t0 + toi, 1.0f);
                        }


                        c.Toi    = toi;
                        c.Flags |= ContactFlag.ToiFlag;
                    }

                    if (Settings.FLT_EPSILON < toi && toi < minTOI)
                    {
                        // This is the minimum TOI found so far.
                        minContact = c;
                        minTOI     = toi;
                    }
                }

                if (minContact == null || 1.0f - 100.0f * Settings.FLT_EPSILON < minTOI)
                {
                    // No more TOI events. Done!
                    break;
                }

                // Advance the bodies to the TOI.
                Fixture f1 = minContact.GetFixtureA();
                Fixture f2 = minContact.GetFixtureB();
                Body    b3 = f1.GetBody();
                Body    b4 = f2.GetBody();

                Sweep backup1 = b3._sweep;
                Sweep backup2 = b4._sweep;

                b3.Advance(minTOI);
                b4.Advance(minTOI);

                // The TOI contact likely has some new contact points.
                minContact.Update(_contactManager._contactListener);
                minContact.Flags &= ~ContactFlag.ToiFlag;

                // Is the contact solid?
                if (minContact.IsSolid() == false)
                {
                    // Restore the sweeps.
                    b3._sweep = backup1;
                    b4._sweep = backup2;
                    b3.SynchronizeTransform();
                    b4.SynchronizeTransform();
                    continue;
                }

                // Did numerical issues prevent a contact point from being generated?
                if (minContact.IsTouching() == false)
                {
                    // Give up on this TOI.
                    continue;
                }

                // Build the TOI island. We need a dynamic seed.
                Body seed = b3;
                if (seed.IsStatic())
                {
                    seed = b4;
                }

                // Reset island and queue.
                island.Clear();

                int queueStart = 0; // starting index for queue
                int queueSize  = 0; // elements in queue
                queue[queueStart + queueSize++] = seed;
                seed._flags |= Body.BodyFlags.Island;

                // Perform a breadth first search (BFS) on the contact/joint graph.
                while (queueSize > 0)
                {
                    // Grab the next body off the stack and add it to the island.
                    Body b = queue[queueStart++];
                    --queueSize;

                    island.Add(ref b);

                    // Make sure the body is awake.
                    b._flags &= ~Body.BodyFlags.Sleep;

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

                    // Search all contacts connected to this body.
                    for (ContactEdge cEdge = b._contactList; cEdge != null; cEdge = cEdge.Next)
                    {
                        // Does the TOI island still have space for contacts?
                        if (island.ContactCount == island.ContactCapacity)
                        {
                            break;
                        }

                        // Has this contact already been added to an island? Skip slow or non-solid contacts.
                        if ((cEdge.Contact.Flags & ContactFlag.IslandFlag) != 0)
                        {
                            continue;
                        }

                        // Is this contact touching? For performance we are not updating this contact.
                        if (cEdge.Contact.IsSolid() == false || cEdge.Contact.IsTouching() == false)
                        {
                            continue;
                        }

                        island.Add(ref cEdge.Contact);
                        cEdge.Contact.Flags |= ContactFlag.IslandFlag;

                        // Update other body.
                        Body other = cEdge.Other;

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

                        // March forward, this can do no harm since this is the min TOI.
                        if (other.IsStatic() == false)
                        {
                            other.Advance(minTOI);
                            other.WakeUp();
                        }

                        Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
                        queue[queueStart + queueSize] = other;
                        ++queueSize;
                        other._flags |= Body.BodyFlags.Island;
                    }

                    for (JointEdge jEdge = b._jointList; jEdge != null; jEdge = jEdge.Next)
                    {
                        if (island.JointCount == island.JointCapacity)
                        {
                            continue;
                        }

                        if (jEdge.Joint._islandFlag == true)
                        {
                            continue;
                        }

                        island.Add(jEdge.Joint);

                        jEdge.Joint._islandFlag = true;

                        Body other = jEdge.Other;

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

                        if (!other.IsStatic())
                        {
                            other.Advance(minTOI);
                            other.WakeUp();
                        }

                        Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
                        queue[queueStart + queueSize] = other;
                        ++queueSize;
                        other._flags |= Body.BodyFlags.Island;
                    }
                }

                TimeStep subStep;
                subStep.WarmStarting       = false;
                subStep.Dt                 = (1.0f - minTOI) * step.Dt;
                subStep.Inv_Dt             = 1.0f / subStep.Dt;
                subStep.DtRatio            = 0.0f;
                subStep.VelocityIterations = step.VelocityIterations;
                subStep.PositionIterations = step.PositionIterations;

                island.SolveTOI(ref subStep);

                // Post solve cleanup.
                for (int i = 0; i < island.BodyCount; ++i)
                {
                    // Allow bodies to participate in future TOI islands.
                    Body b = island.Bodies[i];
                    b._flags &= ~Body.BodyFlags.Island;

                    if ((b._flags & Body.BodyFlags.Sleep) != 0)
                    {
                        continue;
                    }

                    if (b.IsStatic())
                    {
                        continue;
                    }

                    b.SynchronizeFixtures();

                    // Invalidate all contact TOIs associated with this body. Some of these
                    // may not be in the island because they were not touching.
                    for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next)
                    {
                        ce.Contact.Flags &= ~ContactFlag.ToiFlag;
                    }
                }

                for (int i = 0; i < island.ContactCount; ++i)
                {
                    // Allow contacts to participate in future TOI islands.
                    Contact c = island.Contacts[i];
                    c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag);
                }

                for (int i = 0; i < island.JointCount; ++i)
                {
                    // Allow joints to participate in future TOI islands.
                    Joint j = island.Joints[i];
                    j._islandFlag = false;
                }

                // Commit fixture proxy movements to the broad-phase so that new contacts are created.
                // Also, some contacts can be destroyed.
                _contactManager.FindNewContacts();
            }

            queue = null;
        }
예제 #8
0
        public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
        {
            Step = step;

            ConstraintCount = contactCount;
            Constraints     = new ContactConstraint[ConstraintCount];

            for (int i = 0; i < ConstraintCount; ++i)
            {
                Contact contact = contacts[i];

                Fixture  fixtureA = contact.GetFixtureA();
                Fixture  fixtureB = contact.GetFixtureB();
                Shape    shapeA   = fixtureA.GetShape();
                Shape    shapeB   = fixtureB.GetShape();
                float    radiusA  = shapeA._radius;
                float    radiusB  = shapeB._radius;
                Body     bodyA    = fixtureA.GetBody();
                Body     bodyB    = fixtureB.GetBody();
                Manifold manifold = contact.GetManifold();

                float friction    = Settings.MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction());
                float restitution = Settings.MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution());

                Vec2  vA = bodyA._linearVelocity;
                Vec2  vB = bodyB._linearVelocity;
                float wA = bodyA._angularVelocity;
                float wB = bodyB._angularVelocity;

                Box2DXDebug.Assert(manifold.PointCount > 0);

                WorldManifold worldManifold = new WorldManifold();
                worldManifold.Initialize(manifold, bodyA.GetTransform(), radiusA, bodyB.GetTransform(), radiusB);

                ContactConstraint cc = new ContactConstraint();
                Constraints[i] = cc;
                cc.BodyA       = bodyA;
                cc.BodyB       = bodyB;
                cc.Manifold    = manifold;
                cc.Normal      = worldManifold.Normal;
                cc.PointCount  = manifold.PointCount;
                cc.Friction    = friction;
                cc.Restitution = restitution;

                cc.LocalPlaneNormal = manifold.LocalPlaneNormal;
                cc.LocalPoint       = manifold.LocalPoint;
                cc.Radius           = radiusA + radiusB;
                cc.Type             = manifold.Type;

                for (int j = 0; j < cc.PointCount; ++j)
                {
                    ManifoldPoint          cp  = manifold.Points[j];
                    ContactConstraintPoint ccp = cc.Points[j];

                    ccp.NormalImpulse  = cp.NormalImpulse;
                    ccp.TangentImpulse = cp.TangentImpulse;

                    ccp.LocalPoint = cp.LocalPoint;

                    ccp.RA = worldManifold.Points[j] - bodyA._sweep.C;
                    ccp.RB = worldManifold.Points[j] - bodyB._sweep.C;

                    float rnA = Vec2.Cross(ccp.RA, cc.Normal);
                    float rnB = Vec2.Cross(ccp.RB, cc.Normal);
                    rnA *= rnA;
                    rnB *= rnB;

                    float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;

                    Box2DXDebug.Assert(kNormal > Settings.FLT_EPSILON);
                    ccp.NormalMass = 1.0f / kNormal;

                    float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass;
                    kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB;

                    Box2DXDebug.Assert(kEqualized > Settings.FLT_EPSILON);
                    ccp.EqualizedMass = 1.0f / kEqualized;

                    Vec2 tangent = Vec2.Cross(cc.Normal, 1.0f);

                    float rtA = Vec2.Cross(ccp.RA, tangent);
                    float rtB = Vec2.Cross(ccp.RB, tangent);
                    rtA *= rtA;
                    rtB *= rtB;

                    float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;

                    Box2DXDebug.Assert(kTangent > Settings.FLT_EPSILON);
                    ccp.TangentMass = 1.0f / kTangent;

                    // Setup a velocity bias for restitution.
                    ccp.VelocityBias = 0.0f;
                    float vRel = Vec2.Dot(cc.Normal, vB + Vec2.Cross(wB, ccp.RB) - vA - Vec2.Cross(wA, ccp.RA));
                    if (vRel < -Settings.VelocityThreshold)
                    {
                        ccp.VelocityBias = -cc.Restitution * vRel;
                    }
                }

                // If we have two points, then prepare the block solver.
                if (cc.PointCount == 2)
                {
                    ContactConstraintPoint ccp1 = cc.Points[0];
                    ContactConstraintPoint ccp2 = cc.Points[1];

                    float invMassA = bodyA._invMass;
                    float invIA    = bodyA._invI;
                    float invMassB = bodyB._invMass;
                    float invIB    = bodyB._invI;

                    float rn1A = Vec2.Cross(ccp1.RA, cc.Normal);
                    float rn1B = Vec2.Cross(ccp1.RB, cc.Normal);
                    float rn2A = Vec2.Cross(ccp2.RA, cc.Normal);
                    float rn2B = Vec2.Cross(ccp2.RB, cc.Normal);

                    float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
                    float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
                    float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;

                    // Ensure a reasonable condition number.
                    const float k_maxConditionNumber = 100.0f;
                    if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
                    {
                        // K is safe to invert.
                        cc.K.Col1.Set(k11, k12);
                        cc.K.Col2.Set(k12, k22);
                        cc.NormalMass = cc.K.Invert();
                    }
                    else
                    {
                        // The constraints are redundant, just use one.
                        // TODO_ERIN use deepest?
                        cc.PointCount = 1;
                    }
                }
            }
        }