Ejemplo n.º 1
0
        public void Distance()
        {
            TOIInput input = new TOIInput();

            input.ProxyA = new DistanceProxy(_shapeA, 0);
            input.ProxyB = new DistanceProxy(_shapeB, 0);
            input.SweepA = _sweepA;
            input.SweepB = _sweepB;
            input.TMax   = 1.0f;

            TimeOfImpact.CalculateTimeOfImpact(ref input, out _);
        }
Ejemplo n.º 2
0
        public void Distance()
        {
            TOIInput input = new TOIInput();

            input.ProxyA.Set(_shapeA, 0);
            input.ProxyB.Set(_shapeB, 0);
            input.SweepA = _sweepA;
            input.SweepB = _sweepB;
            input.TMax   = 1.0f;

            TOIOutput output;

            TimeOfImpact.CalculateTimeOfImpact(out output, input);
        }
Ejemplo n.º 3
0
        private void SolveTOI(ref TimeStep step)
        {
            Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, ContactManager);

            if (_stepComplete)
            {
                for (var i = 0; i < BodyList.Count; i++)
                {
                    BodyList[i]._flags       &= ~BodyFlags.IslandFlag;
                    BodyList[i]._sweep.Alpha0 = 0.0f;
                }

                for (var i = 0; i < ContactManager.ContactList.Count; i++)
                {
                    var c = ContactManager.ContactList[i];

                    // Invalidate TOI
                    c._flags   &= ~ContactFlags.IslandFlag;
                    c._flags   &= ~ContactFlags.TOIFlag;
                    c._toiCount = 0;
                    c._toi      = 1.0f;
                }
            }

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

                for (var i = 0; i < ContactManager.ContactList.Count; i++)
                {
                    var c = ContactManager.ContactList[i];

                    // Is this contact disabled?
                    if (c.Enabled == false)
                    {
                        continue;
                    }

                    // Prevent excessive sub-stepping.
                    if (c._toiCount > Settings.MaxSubSteps)
                    {
                        continue;
                    }

                    float alpha;
                    if (c.TOIFlag)
                    {
                        // This contact has a valid cached TOI.
                        alpha = c._toi;
                    }
                    else
                    {
                        var fA = c.FixtureA;
                        var fB = c.FixtureB;

                        // Is there a sensor?
                        if (fA.IsSensor || fB.IsSensor)
                        {
                            continue;
                        }

                        var bA = fA.Body;
                        var bB = fB.Body;

                        var typeA = bA.BodyType;
                        var typeB = bB.BodyType;
                        Debug.Assert(typeA == BodyType.Dynamic || typeB == BodyType.Dynamic);

                        var activeA = bA.Awake && typeA != BodyType.Static;
                        var activeB = bB.Awake && typeB != BodyType.Static;

                        // Is at least one body active (awake and dynamic or kinematic)?
                        if (activeA == false && activeB == false)
                        {
                            continue;
                        }

                        var collideA = (bA.IsBullet || typeA != BodyType.Dynamic) &&
                                       (fA.IgnoreCCDWith & fB.CollisionCategories) == 0 && !bA.IgnoreCCD;
                        var collideB = (bB.IsBullet || typeB != BodyType.Dynamic) &&
                                       (fB.IgnoreCCDWith & fA.CollisionCategories) == 0 && !bB.IgnoreCCD;

                        // 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.
                        var 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);
                        }

                        Debug.Assert(alpha0 < 1.0f);

                        // Compute the time of impact in interval [0, minTOI]
                        var input = new TOIInput();
                        input.ProxyA = new DistanceProxy(fA.Shape, c.ChildIndexA);
                        input.ProxyB = new DistanceProxy(fB.Shape, c.ChildIndexB);
                        input.SweepA = bA._sweep;
                        input.SweepB = bB._sweep;
                        input.TMax   = 1.0f;

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

                        // Beta is the fraction of the remaining portion of the .
                        var beta = output.T;
                        if (output.State == TOIOutputState.Touching)
                        {
                            alpha = Mathf.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
                        }
                        else
                        {
                            alpha = 1.0f;
                        }

                        c._toi    = alpha;
                        c._flags &= ~ContactFlags.TOIFlag;
                    }

                    if (alpha < minAlpha)
                    {
                        // This is the minimum TOI found so far.
                        minContact = c;
                        minAlpha   = alpha;
                    }
                }

                if (minContact == null || 1.0f - 10.0f * Settings.Epsilon < minAlpha)
                {
                    // No more TOI events. Done!
                    _stepComplete = true;
                    break;
                }

                // Advance the bodies to the TOI.
                var fA1 = minContact.FixtureA;
                var fB1 = minContact.FixtureB;
                var bA0 = fA1.Body;
                var bB0 = fB1.Body;

                var backup1 = bA0._sweep;
                var backup2 = bB0._sweep;

                bA0.Advance(minAlpha);
                bB0.Advance(minAlpha);

                // The TOI contact likely has some new contact points.
                minContact.Update(ContactManager);
                minContact._flags &= ~ContactFlags.TOIFlag;
                ++minContact._toiCount;

                // Is the contact solid?
                if (minContact.Enabled == false || minContact.IsTouching == false)
                {
                    // Restore the sweeps.
                    minContact._flags &= ~ContactFlags.EnabledFlag;
                    bA0._sweep         = backup1;
                    bB0._sweep         = backup2;
                    bA0.SynchronizeTransform();
                    bB0.SynchronizeTransform();
                    continue;
                }

                bA0.Awake = true;
                bB0.Awake = true;

                // Build the island
                Island.Clear();
                Island.Add(bA0);
                Island.Add(bB0);
                Island.Add(minContact);

                bA0._flags        |= BodyFlags.IslandFlag;
                bB0._flags        |= BodyFlags.IslandFlag;
                minContact._flags &= ~ContactFlags.IslandFlag;

                // Get contacts on bodyA and bodyB.
                Body[] bodies = { bA0, bB0 };
                for (var i = 0; i < 2; ++i)
                {
                    var body = bodies[i];
                    if (body.BodyType == BodyType.Dynamic)
                    {
                        for (var ce = body.ContactList; ce != null; ce = ce.Next)
                        {
                            var contact = ce.Contact;

                            if (Island.BodyCount == Island.BodyCapacity)
                            {
                                break;
                            }

                            if (Island.ContactCount == Island.ContactCapacity)
                            {
                                break;
                            }

                            // Has this contact already been added to the island?
                            if (contact.IslandFlag)
                            {
                                continue;
                            }

                            // Only add static, kinematic, or bullet bodies.
                            var other = ce.Other;
                            if (other.BodyType == BodyType.Dynamic &&
                                body.IsBullet == false && other.IsBullet == false)
                            {
                                continue;
                            }

                            // Skip sensors.
                            if (contact.FixtureA.IsSensor || contact.FixtureB.IsSensor)
                            {
                                continue;
                            }

                            // Tentatively advance the body to the TOI.
                            var backup = other._sweep;
                            if (!other.IsIsland)
                            {
                                other.Advance(minAlpha);
                            }

                            // Update the contact points
                            contact.Update(ContactManager);

                            // Was the contact disabled by the user?
                            if (contact.Enabled == 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
                            minContact._flags |= ContactFlags.IslandFlag;
                            Island.Add(contact);

                            // Has the other body already been added to the island?
                            if (other.IsIsland)
                            {
                                continue;
                            }

                            // Add the other body to the island.
                            other._flags |= BodyFlags.IslandFlag;

                            if (other.BodyType != BodyType.Static)
                            {
                                other.Awake = true;
                            }

                            Island.Add(other);
                        }
                    }
                }

                TimeStep subStep;
                subStep.dt      = (1.0f - minAlpha) * step.dt;
                subStep.inv_dt  = 1.0f / subStep.dt;
                subStep.dtRatio = 1.0f;
                Island.SolveTOI(ref subStep, bA0.IslandIndex, bB0.IslandIndex);

                // Reset island flags and synchronize broad-phase proxies.
                for (var i = 0; i < Island.BodyCount; ++i)
                {
                    var body = Island.Bodies[i];
                    body._flags &= ~BodyFlags.IslandFlag;

                    if (body.BodyType != BodyType.Dynamic)
                    {
                        continue;
                    }

                    body.SynchronizeFixtures();

                    // Invalidate all contact TOIs on this displaced body.
                    for (var ce = body.ContactList; ce != null; ce = ce.Next)
                    {
                        ce.Contact._flags &= ~ContactFlags.TOIFlag;
                        ce.Contact._flags &= ~ContactFlags.IslandFlag;
                    }
                }

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

                if (Settings.EnableSubStepping)
                {
                    _stepComplete = false;
                    break;
                }
            }
        }
Ejemplo n.º 4
0
        public override void Update(GameSettings settings, GameTime gameTime)
        {
            base.Update(settings, gameTime);

            Sweep sweepA = new Sweep();

            sweepA.C0          = new Vector2(24.0f, -60.0f);
            sweepA.A0          = 2.95f;
            sweepA.C           = sweepA.C0;
            sweepA.A           = sweepA.A0;
            sweepA.LocalCenter = Vector2.Zero;

            Sweep sweepB = new Sweep();

            sweepB.C0          = new Vector2(53.474274f, -50.252514f);
            sweepB.A0          = 513.36676f; // - 162.0f * b2_pi;
            sweepB.C           = new Vector2(54.595478f, -51.083473f);
            sweepB.A           = 513.62781f; //  - 162.0f * b2_pi;
            sweepB.LocalCenter = Vector2.Zero;

            //sweepB.a0 -= 300.0f * b2_pi;
            //sweepB.a -= 300.0f * b2_pi;

            TOIInput input = new TOIInput();

            input.ProxyA.Set(_shapeA, 0);
            input.ProxyB.Set(_shapeB, 0);
            input.SweepA = sweepA;
            input.SweepB = sweepB;
            input.TMax   = 1.0f;

            TOIOutput output;

            TimeOfImpact.CalculateTimeOfImpact(out output, input);

            DrawString("TOI = " + output.T);
            DrawString(string.Format("Max TOI iters = {0:n}, Max root iters = {1:n}", TimeOfImpact.TOIMaxIters, TimeOfImpact.TOIMaxRootIters));

            Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices];

            DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View);
            Transform transformA;

            sweepA.GetTransform(out transformA, 0.0f);
            for (int i = 0; i < _shapeA.Vertices.Count; ++i)
            {
                vertices[i] = MathUtils.Mul(ref transformA, _shapeA.Vertices[i]);
            }
            DebugView.DrawPolygon(vertices, _shapeA.Vertices.Count, new Color(0.9f, 0.9f, 0.9f));

            Transform transformB;

            sweepB.GetTransform(out transformB, 0.0f);

            for (int i = 0; i < _shapeB.Vertices.Count; ++i)
            {
                vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]);
            }
            DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.5f, 0.9f, 0.5f));

            sweepB.GetTransform(out transformB, output.T);
            for (int i = 0; i < _shapeB.Vertices.Count; ++i)
            {
                vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]);
            }
            DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.5f, 0.7f, 0.9f));

            sweepB.GetTransform(out transformB, 1.0f);
            for (int i = 0; i < _shapeB.Vertices.Count; ++i)
            {
                vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]);
            }
            DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.9f, 0.5f, 0.5f));
            DebugView.EndCustomDraw();
        }
Ejemplo n.º 5
0
Archivo: World.cs Proyecto: pyneer/case
        /// <summary>
        /// Find TOI contacts and solve them.
        /// </summary>
        /// <param name="step">The step.</param>
        private void SolveTOI(ref TimeStep step)
        {
            this.Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, this.ContactManager);

            if (this._stepComplete)
            {
                for (int i = 0; i < this.BodyList.Count; i++)
                {
                    this.BodyList[i].Flags       &= ~BodyFlags.Island;
                    this.BodyList[i].Sweep.Alpha0 = 0.0f;
                }

                for (int i = 0; i < this.ContactManager.ContactList.Count; i++)
                {
                    Contact c = this.ContactManager.ContactList[i];

                    // Invalidate TOI
                    c.Flags   &= ~(ContactFlags.TOI | ContactFlags.Island);
                    c.TOICount = 0;
                    c.TOI      = 1.0f;
                }
            }

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

                for (int i = 0; i < this.ContactManager.ContactList.Count; i++)
                {
                    Contact c = this.ContactManager.ContactList[i];

                    // Is this contact disabled?
                    if (c.Enabled == false)
                    {
                        continue;
                    }

                    // Prevent excessive sub-stepping.
                    if (c.TOICount > Settings.MaxSubSteps)
                    {
                        continue;
                    }

                    float alpha;
                    if ((c.Flags & ContactFlags.TOI) == ContactFlags.TOI)
                    {
                        // This contact has a valid cached TOI.
                        alpha = c.TOI;
                    }
                    else
                    {
                        Fixture fA = c.FixtureA;
                        Fixture fB = c.FixtureB;

                        // Is there a sensor?
                        if (fA.IsSensor || fB.IsSensor)
                        {
                            continue;
                        }

                        Body bA = fA.Body;
                        Body bB = fB.Body;

                        BodyType typeA = bA.BodyType;
                        BodyType typeB = bB.BodyType;
                        Debug.Assert(typeA == BodyType.Dynamic || typeB == BodyType.Dynamic);

                        bool awakeA = bA.Awake && typeA != BodyType.Static;
                        bool awakeB = bB.Awake && typeB != BodyType.Static;

                        // Is at least one body awake?
                        if (awakeA == false && awakeB == false)
                        {
                            continue;
                        }

                        bool collideA = (bA.IsBullet || typeA != BodyType.Dynamic) && !bA.IgnoreCCD;
                        bool collideB = (bB.IsBullet || typeB != BodyType.Dynamic) && !bB.IgnoreCCD;

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

                        Debug.Assert(alpha0 < 1.0f);

                        // Compute the time of impact in interval [0, minTOI]
                        this._input.ProxyA.Set(fA.Shape, c.ChildIndexA);
                        this._input.ProxyB.Set(fB.Shape, c.ChildIndexB);
                        this._input.SweepA = bA.Sweep;
                        this._input.SweepB = bB.Sweep;
                        this._input.TMax   = 1.0f;

                        TOIOutput output;
                        TimeOfImpact.CalculateTimeOfImpact(out output, this._input);

                        // Beta is the fraction of the remaining portion of the .
                        float beta = output.T;
                        if (output.State == TOIOutputState.Touching)
                        {
                            alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
                        }
                        else
                        {
                            alpha = 1.0f;
                        }

                        c.TOI    = alpha;
                        c.Flags |= ContactFlags.TOI;
                    }

                    if (alpha < minAlpha)
                    {
                        // This is the minimum TOI found so far.
                        minContact = c;
                        minAlpha   = alpha;
                    }
                }

                if (minContact == null || 1.0f - 10.0f * Settings.Epsilon < minAlpha)
                {
                    // No more TOI events. Done!
                    this._stepComplete = true;
                    break;
                }

                // Advance the bodies to the TOI.
                Fixture fA1 = minContact.FixtureA;
                Fixture fB1 = minContact.FixtureB;
                Body    bA1 = fA1.Body;
                Body    bB1 = fB1.Body;

                Sweep backup1 = bA1.Sweep;
                Sweep backup2 = bB1.Sweep;

                bA1.Advance(minAlpha);
                bB1.Advance(minAlpha);

                // The TOI contact likely has some new contact points.
                minContact.Update(this.ContactManager);
                minContact.Flags &= ~ContactFlags.TOI;
                ++minContact.TOICount;

                // Is the contact solid?
                if (minContact.Enabled == false || minContact.IsTouching() == false)
                {
                    // Restore the sweeps.
                    minContact.Enabled = false;
                    bA1.Sweep          = backup1;
                    bB1.Sweep          = backup2;
                    bA1.SynchronizeTransform();
                    bB1.SynchronizeTransform();
                    continue;
                }

                bA1.Awake = true;
                bB1.Awake = true;

                // Build the island
                this.Island.Clear();
                this.Island.Add(bA1);
                this.Island.Add(bB1);
                this.Island.Add(minContact);

                bA1.Flags        |= BodyFlags.Island;
                bB1.Flags        |= BodyFlags.Island;
                minContact.Flags |= ContactFlags.Island;

                // Get contacts on bodyA and bodyB.
                Body[] bodies = { bA1, bB1 };
                for (int i = 0; i < 2; ++i)
                {
                    Body body = bodies[i];
                    if (body.BodyType == BodyType.Dynamic)
                    {
                        // for (ContactEdge ce = body.ContactList; ce && Island.BodyCount < Settings.MaxTOIContacts; ce = ce.Next)
                        for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
                        {
                            Contact contact = ce.Contact;

                            // Has this contact already been added to the island?
                            if ((contact.Flags & ContactFlags.Island) == ContactFlags.Island)
                            {
                                continue;
                            }

                            // Only add static, kinematic, or bullet bodies.
                            Body other = ce.Other;
                            if (other.BodyType == BodyType.Dynamic &&
                                body.IsBullet == false && other.IsBullet == false)
                            {
                                continue;
                            }

                            // Skip sensors.
                            if (contact.FixtureA.IsSensor || contact.FixtureB.IsSensor)
                            {
                                continue;
                            }

                            // Tentatively advance the body to the TOI.
                            Sweep backup = other.Sweep;
                            if ((other.Flags & BodyFlags.Island) == 0)
                            {
                                other.Advance(minAlpha);
                            }

                            // Update the contact points
                            contact.Update(this.ContactManager);

                            // Was the contact disabled by the user?
                            if (contact.Enabled == 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.Flags |= ContactFlags.Island;
                            this.Island.Add(contact);

                            // Has the other body already been added to the island?
                            if ((other.Flags & BodyFlags.Island) == BodyFlags.Island)
                            {
                                continue;
                            }

                            // Add the other body to the island.
                            other.Flags |= BodyFlags.Island;

                            if (other.BodyType != BodyType.Static)
                            {
                                other.Awake = true;
                            }

                            this.Island.Add(other);
                        }
                    }
                }

                TimeStep 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;
                this.Island.SolveTOI(ref subStep);

                // Reset island flags and synchronize broad-phase proxies.
                for (int i = 0; i < this.Island.BodyCount; ++i)
                {
                    Body body = this.Island.Bodies[i];
                    body.Flags &= ~BodyFlags.Island;

                    if (body.BodyType != BodyType.Dynamic)
                    {
                        continue;
                    }

                    body.SynchronizeFixtures();

                    // Invalidate all contact TOIs on this displaced body.
                    for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
                    {
                        ce.Contact.Flags &= ~(ContactFlags.TOI | ContactFlags.Island);
                    }
                }

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

                if (this.EnableSubStepping)
                {
                    this._stepComplete = false;
                    break;
                }
            }
        }
Ejemplo n.º 6
0
        /// <summary>
        // Advance a dynamic body to its first time of contact
        // and adjust the position to ensure clearance.
        /// </summary>
        /// <param name="body">The body.</param>
        private void SolveTOI(Body body)
        {
            // Find the minimum contact.
            Contact toiContact = null;
            float   toi        = 1.0f;
            Body    toiOther   = null;
            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)
                {
                    if (ce.Contact == toiContact)
                    {
                        continue;
                    }

                    Body     other = ce.Other;
                    BodyType type  = other.BodyType;

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

                        // No repeated hits on non-static bodies
                        if (type != BodyType.Static && (ce.Contact.Flags & ContactFlags.BulletHit) != 0)
                        {
                            continue;
                        }
                    }
                    else if (type == BodyType.Dynamic)
                    {
                        continue;
                    }

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

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

                    Fixture fixtureA = contact.FixtureA;
                    Fixture fixtureB = contact.FixtureB;
                    int     indexA   = contact.ChildIndexA;
                    int     indexB   = contact.ChildIndexB;

                    // 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.Shape, indexA);
                    input.ProxyB.Set(fixtureB.Shape, indexB);
                    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;
                        toiOther   = other;
                        found      = true;
                    }

                    ++count;
                }

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

            if (toiContact == null)
            {
                body.Advance(1.0f);
                return;
            }

            Sweep backup = body.Sweep;

            body.Advance(toi);
            toiContact.Update(ContactManager);
            if (toiContact.Enabled == false)
            {
                // Contact disabled. Backup and recurse.
                body.Sweep = backup;
                SolveTOI(body);
            }

            ++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.MaxTOIContacts); ce = ce.Next)
            {
                Body     other = ce.Other;
                BodyType type  = other.BodyType;

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

                // Check for a disabled contact.
                Contact contact = ce.Contact;
                if (contact.Enabled == 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.
                if (contact != toiContact)
                {
                    contact.Update(ContactManager);
                }

                // Did the user disable the contact?
                if (contact.Enabled == false)
                {
                    // 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;
                }
            }

            if (toiOther.BodyType != BodyType.Static)
            {
                toiContact.Flags |= ContactFlags.BulletHit;
            }
        }
Ejemplo n.º 7
0
        public override void Update(GameSettings settings, GameTime gameTime)
        {
            base.Update(settings, gameTime);

            Sweep sweepA = new Sweep();

            sweepA.C0          = new Vector2(24.0f, -60.0f);
            sweepA.A0          = 2.95f;
            sweepA.C           = sweepA.C0;
            sweepA.A           = sweepA.A0;
            sweepA.LocalCenter = Vector2.Zero;

            Sweep sweepB = new Sweep();

            sweepB.C0          = new Vector2(53.474274f, -50.252514f);
            sweepB.A0          = 513.36676f; // - 162.0f * MathConstants.Pi;
            sweepB.C           = new Vector2(54.595478f, -51.083473f);
            sweepB.A           = 513.62781f; //  - 162.0f * MathConstants.Pi;
            sweepB.LocalCenter = Vector2.Zero;

            //sweepB.a0 -= 300.0f * MathConstants.Pi;
            //sweepB.a -= 300.0f * MathConstants.Pi;

            TOIInput input = new TOIInput();

            input.ProxyA = new DistanceProxy(_shapeA, 0);
            input.ProxyB = new DistanceProxy(_shapeB, 0);
            input.SweepA = sweepA;
            input.SweepB = sweepB;
            input.TMax   = 1.0f;

            TOIOutput output;

            TimeOfImpact.CalculateTimeOfImpact(ref input, out output);

            DrawString("toi = " + output.T);
            DrawString($"max toi iters = {TimeOfImpact.TOIMaxIters}, max root iters = {TimeOfImpact.TOIMaxRootIters}");

            Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices];

            DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View);

            Transform transformA;

            sweepA.GetTransform(out transformA, 0.0f);
            for (int i = 0; i < _shapeA.Vertices.Count; ++i)
            {
                vertices[i] = MathUtils.Mul(ref transformA, _shapeA.Vertices[i]);
            }
            DebugView.DrawPolygon(vertices, _shapeA.Vertices.Count, new Color(0.9f, 0.9f, 0.9f));

            Transform transformB;

            sweepB.GetTransform(out transformB, 0.0f);

            //b2Vec2 localPoint(2.0f, -0.1f);

            for (int i = 0; i < _shapeB.Vertices.Count; ++i)
            {
                vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]);
            }
            DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.5f, 0.9f, 0.5f));

            sweepB.GetTransform(out transformB, output.T);
            for (int i = 0; i < _shapeB.Vertices.Count; ++i)
            {
                vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]);
            }
            DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.5f, 0.7f, 0.9f));

            sweepB.GetTransform(out transformB, 1.0f);
            for (int i = 0; i < _shapeB.Vertices.Count; ++i)
            {
                vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]);
            }
            DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.9f, 0.5f, 0.5f));

            DebugView.EndCustomDraw();

#if false
            for (float t = 0.0f; t < 1.0f; t += 0.1f)
            {
                sweepB.GetTransform(&transformB, t);
                for (int i = 0; i < _shapeB.m_count; ++i)
                {
                    vertices[i] = MathUtils.Mul(transformB, _shapeB.m_vertices[i]);
                }
                DebugView.DrawPolygon(vertices, _shapeB.m_count, b2Color(0.9f, 0.5f, 0.5f));
            }
#endif
        }