Beispiel #1
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.
            //#warning Remove extra allocs

            int stackSize = _bodyCount;
            Body[] stack = new Body[_bodyCount];
            for (Body seed = _bodyList; seed != null; seed = seed._next)
            {
                if ((seed._flags & (BodyFlags.Island | BodyFlags.Sleep | BodyFlags.Frozen)) != BodyFlags.None)
                {
                    continue;
                }

                if (seed.IsStatic)
                {
                    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 raint graph.
                while (stackCount > 0)
                {
                    // Grab the next body off the stack and add it to the island.
                    Body b = stack[--stackCount];
                    _island.Add(b);

                    // Make sure the body is awake.
                    b._flags &= ~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 ce = b._contactList; ce != null; ce = ce.Next)
                    {
                        // Has this contact already been added to an island?
                        // Is this contact non-solid (involves a sensor).
                        if ((ce.Contact._flags & (ContactFlags.Island | ContactFlags.NonSolid)) != ContactFlags.None)
                        {
                            continue;
                        }

                        // Is this contact touching?
                        if ((ce.Contact._flags & ContactFlags.Touch) == ContactFlags.None)
                        {
                            continue;
                        }

                        _island.Add(ce.Contact);
                        ce.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;
                        }

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

                        Body other = je.Other;
                        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.IsStatic)
                    {
                        b._flags &= ~BodyFlags.Island;
                    }
                }
            }

            // Synchronize fixtures, check for out of range bodies.
            for (Body b = _bodyList; b != null; b = b.GetNext())
            {
                if ((b._flags & (BodyFlags.Sleep | BodyFlags.Frozen)) != BodyFlags.None)
                {
                    continue;
                }

                if (b.IsStatic)
                {
                    continue;
                }

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

            // Look for new contacts.
            _contactManager.FindNewContacts();
        }
Beispiel #2
0
        void SolveTOI(ref TimeStep step)
        {
            // Reserve an island and a queue for TOI island solution.
            _island.Reset(  _bodyCount,
                            Settings.b2_maxTOIContactsPerIsland,
                            Settings.b2_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;
            //#warning More Body array Allocs
            int queueCapacity = _bodyCount;
            Body[] queue = new Body[_bodyCount];

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

            for (Contact c = _contactManager._contactList; c != null; c = c._next)
            {
                // Invalidate TOI
                c._flags &= ~(ContactFlags.Toi | ContactFlags.Island);
            }

            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)
                {
                    if ((c._flags & (ContactFlags.Slow | ContactFlags.NonSolid)) != ContactFlags.None)
                    {
                        continue;
                    }

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

                    float toi = 1.0f;
                    if ((c._flags & ContactFlags.Toi) != ContactFlags.None)
                    {
                        // 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);
                        }

                        Debug.Assert(t0 < 1.0f);

                        // Compute the time of impact.
                        toi = c.ComputeTOI(ref b1._sweep, ref b2._sweep);
                        //CalculateTimeOfImpact(c._fixtureA.GetShape(), b1._sweep, c._fixtureB.GetShape(), b2._sweep);

                        Debug.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 |= ContactFlags.Toi;
                    }

                    if (Settings.b2_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.b2_FLT_EPSILON < minTOI)
                {
                    // No more TOI events. Done!
                    break;
                }

                // Advance the bodies to the TOI.
                Fixture s1_2 = minContact.GetFixtureA();
                Fixture s2_2 = minContact.GetFixtureB();
                Body b1_2 = s1_2.GetBody();
                Body b2_2 = s2_2.GetBody();
                b1_2.Advance(minTOI);
                b2_2.Advance(minTOI);

                // The TOI contact likely has some new contact points.
                minContact.Update(_contactManager.ContactListener);
                minContact._flags &= ~ContactFlags.Toi;

                if ((minContact._flags & ContactFlags.Touch) == 0)
                {
                    // This shouldn't happen. Numerical error?
                    //Debug.Assert(false);
                    continue;
                }

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

                // 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 |= 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(b);

                    // Make sure the body is awake.
                    b._flags &= ~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._contacts.Count == _island._contactCapacity)
                        {
                            continue;
                        }

                        // Has this contact already been added to an island? Skip slow or non-solid contacts.
                        if ((cEdge.Contact._flags & (ContactFlags.Island | ContactFlags.Slow | ContactFlags.NonSolid)) != ContactFlags.None)
                        {
                            continue;
                        }

                        // Is this contact touching? For performance we are not updating this contact.
                        if ((cEdge.Contact._flags & ContactFlags.Touch) == 0)
                        {
                            continue;
                        }

                        _island.Add(cEdge.Contact);
                        cEdge.Contact._flags |= ContactFlags.Island;

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

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

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

                        Debug.Assert(queueStart + queueSize < queueCapacity);
                        queue[queueStart + queueSize] = other;
                        ++queueSize;
                        other._flags |= 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 & BodyFlags.Island) != BodyFlags.None)
                        {
                            continue;
                        }

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

                        Debug.Assert(queueStart + queueSize < queueCapacity);
                        queue[queueStart + queueSize] = other;
                        ++queueSize;
                        other._flags |= 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 &= ~BodyFlags.Island;

                    if ((b._flags & (BodyFlags.Sleep | BodyFlags.Frozen)) != BodyFlags.None)
                    {
                        continue;
                    }

                    if (b.IsStatic)
                    {
                        continue;
                    }

                    // Update fixtures (for broad-phase).
                    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 &= ~ContactFlags.Toi;
                    }
                }

                int contactCount = _island._contacts.Count;
                for (int i = 0; i < contactCount; ++i)
                {
                    // Allow contacts to participate in future TOI islands.
                    Contact c = _island._contacts[i];
                    c._flags &= ~(ContactFlags.Toi | ContactFlags.Island);
                }

                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();
            }
        }
Beispiel #3
0
 internal abstract void SolveVelocityConstraints(ref TimeStep step);
Beispiel #4
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Vector2 v1 = b1._linearVelocity;
            float   w1 = b1._angularVelocity;
            Vector2 v2 = b2._linearVelocity;
            float   w2 = b2._angularVelocity;

            // Solve linear motor raint.
            if (_enableMotor && _limitState != LimitState.Equal)
            {
                float Cdot       = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
                float impulse    = _motorMass * (_motorSpeed - Cdot);
                float oldImpulse = _motorImpulse;
                float maxImpulse = step.dt * _maxMotorForce;
                _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse       = _motorImpulse - oldImpulse;

                Vector2 P  = impulse * _axis;
                float   L1 = impulse * _a1;
                float   L2 = impulse * _a2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }

            float Cdot1 = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;

            if (_enableLimit && _limitState != LimitState.Inactive)
            {
                // Solve prismatic and limit raint in block form.
                float   Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
                Vector2 Cdot  = new Vector2(Cdot1, Cdot2);

                Vector2 f1 = _impulse;
                Vector2 df = _K.Solve(-Cdot);
                _impulse += df;

                if (_limitState == LimitState.AtLower)
                {
                    _impulse.Y = Math.Max(_impulse.Y, 0.0f);
                }
                else if (_limitState == LimitState.AtUpper)
                {
                    _impulse.Y = Math.Min(_impulse.Y, 0.0f);
                }

                // f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
                float b   = -Cdot1 - (_impulse.Y - f1.Y) * _K.col2.X;
                float f2r = b / _K.col1.X + f1.X;
                _impulse.X = f2r;

                df = _impulse - f1;

                Vector2 P  = df.X * _perp + df.Y * _axis;
                float   L1 = df.X * _s1 + df.Y * _a1;
                float   L2 = df.X * _s2 + df.Y * _a2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }
            else
            {
                // Limit is inactive, just solve the prismatic raint in block form.
                float df = (-Cdot1) / _K.col1.X;
                _impulse.X += df;

                Vector2 P  = df * _perp;
                float   L1 = df * _s1;
                float   L2 = df * _s2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }

            b1._linearVelocity  = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity  = v2;
            b2._angularVelocity = w2;
        }
Beispiel #5
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            XForm xf1, xf2;
            b1.GetXForm(out xf1);
            b2.GetXForm(out xf2);

            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

            if (_state == LimitState.AtUpper)
            {
                Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
                Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);

                float Cdot = -Vector2.Dot(_u1, v1) - _ratio * Vector2.Dot(_u2, v2);
                float impulse = _pulleyMass * (-Cdot);
                float oldImpulse = _impulse;
                _impulse = Math.Max(0.0f, _impulse + impulse);
                impulse = _impulse - oldImpulse;

                Vector2 P1 = -impulse * _u1;
                Vector2 P2 = -_ratio * impulse * _u2;
                b1._linearVelocity += b1._invMass * P1;
                b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
                b2._linearVelocity += b2._invMass * P2;
                b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
            }

            if (_limitState1 == LimitState.AtUpper)
            {
                Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);

                float Cdot = -Vector2.Dot(_u1, v1);
                float impulse = -_limitMass1 * Cdot;
                float oldImpulse = _limitImpulse1;
                _limitImpulse1 = Math.Max(0.0f, _limitImpulse1 + impulse);
                impulse = _limitImpulse1 - oldImpulse;

                Vector2 P1 = -impulse * _u1;
                b1._linearVelocity += b1._invMass * P1;
                b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
            }

            if (_limitState2 == LimitState.AtUpper)
            {
                Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);

                float Cdot = -Vector2.Dot(_u2, v2);
                float impulse = -_limitMass2 * Cdot;
                float oldImpulse = _limitImpulse2;
                _limitImpulse2 = Math.Max(0.0f, _limitImpulse2 + impulse);
                impulse = _limitImpulse2 - oldImpulse;

                Vector2 P2 = -impulse * _u2;
                b2._linearVelocity += b2._invMass * P2;
                b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
            }
        }
Beispiel #6
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            _localCenter1 = b1.GetLocalCenter();
            _localCenter2 = b2.GetLocalCenter();

            XForm xf1, xf2;
            b1.GetXForm(out xf1);
            b2.GetXForm(out xf2);

            // Compute the effective masses.
            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - _localCenter1);
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - _localCenter2);
            Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;

            _invMass1 = b1._invMass;
            _invI1 = b1._invI;
            _invMass2 = b2._invMass;
            _invI2 = b2._invI;

            // Compute motor Jacobian and effective mass.
            {
                _axis = MathUtils.Multiply(ref xf1.R, _localXAxis1);
                _a1 = MathUtils.Cross(d + r1, _axis);
                _a2 = MathUtils.Cross(r2, _axis);

                _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
                Debug.Assert(_motorMass > Settings.b2_FLT_EPSILON);
                _motorMass = 1.0f / _motorMass;
            }

            // Prismatic raint.
            {
                _perp = MathUtils.Multiply(ref xf1.R, _localYAxis1);

                _s1 = MathUtils.Cross(d + r1, _perp);
                _s2 = MathUtils.Cross(r2, _perp);

                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
                float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
                float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;

                _K.col1 = new Vector2(k11, k12);
                _K.col2 = new Vector2(k12, k22);
            }

            // Compute motor and limit terms.
            if (_enableLimit)
            {
                float jointTranslation = Vector2.Dot(_axis, d);
                if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop)
                {
                    _limitState = LimitState.Equal;
                }
                else if (jointTranslation <= _lowerTranslation)
                {
                    if (_limitState != LimitState.AtLower)
                    {
                        _limitState = LimitState.AtLower;
                        _impulse.Y = 0.0f;
                    }
                }
                else if (jointTranslation >= _upperTranslation)
                {
                    if (_limitState != LimitState.AtUpper)
                    {
                        _limitState = LimitState.AtUpper;
                        _impulse.Y = 0.0f;
                    }
                }
                else
                {
                    _limitState = LimitState.Inactive;
                    _impulse.Y = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.Inactive;
            }

            if (_enableMotor == false)
            {
                _motorImpulse = 0.0f;
            }

            if (step.warmStarting)
            {
                // Account for variable time step.
                _impulse *= step.dtRatio;
                _motorImpulse *= step.dtRatio;

                Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis;
                float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1;
                float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2;

                b1._linearVelocity -= _invMass1 * P;
                b1._angularVelocity -= _invI1 * L1;

                b2._linearVelocity += _invMass2 * P;
                b2._angularVelocity += _invI2 * L2;
            }
            else
            {
                _impulse = Vector2.Zero;
                _motorImpulse = 0.0f;
            }
        }
Beispiel #7
0
 internal abstract void SolveVelocityConstraints(ref TimeStep step);
Beispiel #8
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            float Cdot = _J.Compute(	b1._linearVelocity, b1._angularVelocity,
                                        b2._linearVelocity, b2._angularVelocity);

            float impulse = _mass * (-Cdot);
            _impulse += impulse;

            b1._linearVelocity += b1._invMass * impulse * _J.linear1;
            b1._angularVelocity += b1._invI * impulse * _J.angular1;
            b2._linearVelocity += b2._invMass * impulse * _J.linear2;
            b2._angularVelocity += b2._invI * impulse * _J.angular2;
        }
Beispiel #9
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Vector2 v1 = b1._linearVelocity;
            float   w1 = b1._angularVelocity;
            Vector2 v2 = b2._linearVelocity;
            float   w2 = b2._angularVelocity;

            float m1 = b1._invMass, m2 = b2._invMass;
            float i1 = b1._invI, i2 = b2._invI;

            // Solve motor raint.
            if (_enableMotor && _limitState != LimitState.Equal)
            {
                float Cdot       = w2 - w1 - _motorSpeed;
                float impulse    = _motorMass * (-Cdot);
                float oldImpulse = _motorImpulse;
                float maxImpulse = step.dt * _maxMotorTorque;
                _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse       = _motorImpulse - oldImpulse;

                w1 -= i1 * impulse;
                w2 += i2 * impulse;
            }

            // Solve limit raint.
            if (_enableLimit && _limitState != LimitState.Inactive)
            {
                XForm xf1, xf2;
                b1.GetXForm(out xf1);
                b2.GetXForm(out xf2);

                Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

                // Solve point-to-point raint
                Vector2 Cdot1 = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
                float   Cdot2 = w2 - w1;
                Vector3 Cdot  = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);

                Vector3 impulse = _mass.Solve33(-Cdot);

                if (_limitState == LimitState.Equal)
                {
                    _impulse += impulse;
                }
                else if (_limitState == LimitState.AtLower)
                {
                    float newImpulse = _impulse.Z + impulse.Z;
                    if (newImpulse < 0.0f)
                    {
                        Vector2 reduced = _mass.Solve22(-Cdot1);
                        impulse.X   = reduced.X;
                        impulse.Y   = reduced.Y;
                        impulse.Z   = -_impulse.Z;
                        _impulse.X += reduced.X;
                        _impulse.Y += reduced.Y;
                        _impulse.Z  = 0.0f;
                    }
                }
                else if (_limitState == LimitState.AtUpper)
                {
                    float newImpulse = _impulse.Z + impulse.Z;
                    if (newImpulse > 0.0f)
                    {
                        Vector2 reduced = _mass.Solve22(-Cdot1);
                        impulse.X   = reduced.X;
                        impulse.Y   = reduced.Y;
                        impulse.Z   = -_impulse.Z;
                        _impulse.X += reduced.X;
                        _impulse.Y += reduced.Y;
                        _impulse.Z  = 0.0f;
                    }
                }

                Vector2 P = new Vector2(impulse.X, impulse.Y);

                v1 -= m1 * P;
                w1 -= i1 * (MathUtils.Cross(r1, P) + impulse.Z);

                v2 += m2 * P;
                w2 += i2 * (MathUtils.Cross(r2, P) + impulse.Z);
            }
            else
            {
                XForm xf1, xf2;
                b1.GetXForm(out xf1);
                b2.GetXForm(out xf2);

                Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

                // Solve point-to-point raint
                Vector2 Cdot    = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
                Vector2 impulse = _mass.Solve22(-Cdot);

                _impulse.X += impulse.X;
                _impulse.Y += impulse.Y;

                v1 -= m1 * impulse;
                w1 -= i1 * MathUtils.Cross(r1, impulse);

                v2 += m2 * impulse;
                w2 += i2 * MathUtils.Cross(r2, impulse);
            }

            b1._linearVelocity  = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity  = v2;
            b2._angularVelocity = w2;
        }
Beispiel #10
0
 internal abstract void InitVelocityConstraints(ref TimeStep step);
Beispiel #11
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            if (_enableMotor || _enableLimit)
            {
                // You cannot create a rotation limit between bodies that
                // both have fixed rotation.
                Debug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
            }

            // Compute the effective mass matrix.
            XForm xf1, xf2;

            b1.GetXForm(out xf1);
            b2.GetXForm(out xf2);

            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

            // J = [-I -r1_skew I r2_skew]
            //     [ 0       -1 0       1]
            // r_skew = [-ry; rx]

            // Matlab
            // K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
            //     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
            //     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]

            float m1 = b1._invMass, m2 = b2._invMass;
            float i1 = b1._invI, i2 = b2._invI;

            _mass.col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
            _mass.col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
            _mass.col3.X = -r1.Y * i1 - r2.Y * i2;
            _mass.col1.Y = _mass.col2.X;
            _mass.col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
            _mass.col3.Y = r1.X * i1 + r2.X * i2;
            _mass.col1.Z = _mass.col3.X;
            _mass.col2.Z = _mass.col3.Y;
            _mass.col3.Z = i1 + i2;

            _motorMass = 1.0f / (i1 + i2);

            if (_enableMotor == false)
            {
                _motorImpulse = 0.0f;
            }

            if (_enableLimit)
            {
                float jointAngle = b2._sweep.a - b1._sweep.a - _referenceAngle;
                if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.b2_angularSlop)
                {
                    _limitState = LimitState.Equal;
                }
                else if (jointAngle <= _lowerAngle)
                {
                    if (_limitState != LimitState.AtLower)
                    {
                        _impulse.Z = 0.0f;
                    }
                    _limitState = LimitState.AtLower;
                }
                else if (jointAngle >= _upperAngle)
                {
                    if (_limitState != LimitState.AtUpper)
                    {
                        _impulse.Z = 0.0f;
                    }
                    _limitState = LimitState.AtUpper;
                }
                else
                {
                    _limitState = LimitState.Inactive;
                    _impulse.Z  = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.Inactive;
            }

            if (step.warmStarting)
            {
                // Scale impulses to support a variable time step.
                _impulse      *= step.dtRatio;
                _motorImpulse *= step.dtRatio;

                Vector2 P = new Vector2(_impulse.X, _impulse.Y);

                b1._linearVelocity  -= m1 * P;
                b1._angularVelocity -= i1 * (MathUtils.Cross(r1, P) + _motorImpulse + _impulse.Z);

                b2._linearVelocity  += m2 * P;
                b2._angularVelocity += i2 * (MathUtils.Cross(r2, P) + _motorImpulse + _impulse.Z);
            }
            else
            {
                _impulse      = Vector3.Zero;
                _motorImpulse = 0.0f;
            }
        }
Beispiel #12
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            XForm xf1, xf2;
            b1.GetXForm(out xf1);
            b2.GetXForm(out xf2);

            // Compute the effective mass matrix.
            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
            _u = b2._sweep.c + r2 - b1._sweep.c - r1;

            // Handle singularity.
            float length = _u.Length();
            if (length > Settings.b2_linearSlop)
            {
                _u *= 1.0f / length;
            }
            else
            {
                _u = new Vector2(0.0f, 0.0f);
            }

            float cr1u = MathUtils.Cross(r1, _u);
            float cr2u = MathUtils.Cross(r2, _u);
            float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;
            Debug.Assert(invMass > Settings.b2_FLT_EPSILON);
            _mass = 1.0f / invMass;

            if (_frequencyHz > 0.0f)
            {
                float C = length - _length;

                // Frequency
                float omega = 2.0f * Settings.b2_pi * _frequencyHz;

                // Damping coefficient
                float d = 2.0f * _mass * _dampingRatio * omega;

                // Spring stiffness
                float k = _mass * omega * omega;

                // magic formulas
                _gamma = 1.0f / (step.dt * (d + step.dt * k));
                _bias = C * step.dt * k * _gamma;

                _mass = 1.0f / (invMass + _gamma);
            }

            if (step.warmStarting)
            {
                // Scale the impulse to support a variable time step.
                _impulse *= step.dtRatio;

                Vector2 P = _impulse * _u;
                b1._linearVelocity -= b1._invMass * P;
                b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
                b2._linearVelocity += b2._invMass * P;
                b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
            }
            else
            {
                _impulse = 0.0f;
            }
        }
Beispiel #13
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            XForm xf1, xf2;
            b1.GetXForm(out xf1);
            b2.GetXForm(out xf2);

            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

            // Cdot = dot(u, v + cross(w, r))
            Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
            Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);
            float Cdot = Vector2.Dot(_u, v2 - v1);

            float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
            _impulse += impulse;

            Vector2 P = impulse * _u;
            b1._linearVelocity -= b1._invMass * P;
            b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
            b2._linearVelocity += b2._invMass * P;
            b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
        }
Beispiel #14
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Vector2 v1 = b1._linearVelocity;
            float w1 = b1._angularVelocity;
            Vector2 v2 = b2._linearVelocity;
            float w2 = b2._angularVelocity;

            // Solve linear motor raint.
            if (_enableMotor && _limitState != LimitState.Equal)
            {
                float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
                float impulse = _motorMass * (_motorSpeed - Cdot);
                float oldImpulse = _motorImpulse;
                float maxImpulse = step.dt * _maxMotorForce;
                _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse = _motorImpulse - oldImpulse;

                Vector2 P = impulse * _axis;
                float L1 = impulse * _a1;
                float L2 = impulse * _a2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }

            Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1);

            if (_enableLimit && _limitState != LimitState.Inactive)
            {
                // Solve prismatic and limit raint in block form.
                float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
                Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);

                Vector3 f1 = _impulse;
                Vector3 df =  _K.Solve33(-Cdot);
                _impulse += df;

                if (_limitState == LimitState.AtLower)
                {
                    _impulse.Z = Math.Max(_impulse.Z, 0.0f);
                }
                else if (_limitState == LimitState.AtUpper)
                {
                    _impulse.Z = Math.Min(_impulse.Z, 0.0f);
                }

                // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
                Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.col3.X, _K.col3.Y);
                Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
                _impulse.X = f2r.X;
                _impulse.Y = f2r.Y;

                df = _impulse - f1;

                Vector2 P = df.X * _perp + df.Z * _axis;
                float L1 = df.X * _s1 + df.Y + df.Z * _a1;
                float L2 = df.X * _s2 + df.Y + df.Z * _a2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }
            else
            {
                // Limit is inactive, just solve the prismatic raint in block form.
                Vector2 df = _K.Solve22(-Cdot1);
                _impulse.X += df.X;
                _impulse.Y += df.Y;

                Vector2 P = df.X * _perp;
                float L1 = df.X * _s1 + df.Y;
                float L2 = df.X * _s2 + df.Y;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }

            b1._linearVelocity = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity = v2;
            b2._angularVelocity = w2;
        }
Beispiel #15
0
        public void Solve(ref TimeStep step, Vector2 gravity, bool allowSleep)
        {
            // Integrate velocities and apply damping.
            for (int i = 0; i < _bodyCount; ++i)
            {
                Body b = _bodies[i];

                if (b.IsStatic)
                {
                    continue;
                }

                // Integrate velocities.
                b._linearVelocity  += step.dt * (gravity + b._invMass * b._force);
                b._angularVelocity += step.dt * b._invI * b._torque;

                // Reset forces.
                b._force  = new Vector2(0.0f, 0.0f);
                b._torque = 0.0f;

                // Apply damping.
                // ODE: dv/dt + c * v = 0
                // Solution: v(t) = v0 * exp(-c * t)
                // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
                // v2 = exp(-c * dt) * v1
                // Taylor expansion:
                // v2 = (1.0f - c * dt) * v1
                b._linearVelocity  *= MathUtils.Clamp(1.0f - step.dt * b._linearDamping, 0.0f, 1.0f);
                b._angularVelocity *= MathUtils.Clamp(1.0f - step.dt * b._angularDamping, 0.0f, 1.0f);
            }

            _contactSolver.Reset(ref step, _contacts);

            // Initialize velocity raints.
            _contactSolver.InitVelocityConstraints(ref step);

            for (int i = 0; i < _jointCount; ++i)
            {
                _joints[i].InitVelocityConstraints(ref step);
            }

            // Solve velocity raints.
            for (int i = 0; i < step.velocityIterations; ++i)
            {
                for (int j = 0; j < _jointCount; ++j)
                {
                    _joints[j].SolveVelocityConstraints(ref step);
                }

                _contactSolver.SolveVelocityConstraints();
            }

            // Post-solve (store impulses for warm starting).
            _contactSolver.FinalizeVelocityConstraints();

            // Integrate positions.
            for (int i = 0; i < _bodyCount; ++i)
            {
                Body b = _bodies[i];

                if (b.IsStatic)
                {
                    continue;
                }

                // Check for large velocities.
                Vector2 translation = step.dt * b._linearVelocity;
                if (Vector2.Dot(translation, translation) > Settings.b2_maxTranslationSquared)
                {
                    translation.Normalize();
                    b._linearVelocity = (Settings.b2_maxTranslation * step.inv_dt) * translation;
                }

                float rotation = step.dt * b._angularVelocity;
                if (rotation * rotation > Settings.b2_maxRotationSquared)
                {
                    if (rotation < 0.0)
                    {
                        b._angularVelocity = -step.inv_dt * Settings.b2_maxRotation;
                    }
                    else
                    {
                        b._angularVelocity = step.inv_dt * Settings.b2_maxRotation;
                    }
                }

                // Store positions for continuous collision.
                b._sweep.c0 = b._sweep.c;
                b._sweep.a0 = b._sweep.a;

                // Integrate
                b._sweep.c += step.dt * b._linearVelocity;
                b._sweep.a += step.dt * b._angularVelocity;

                // Compute new transform
                b.SynchronizeTransform();

                // Note: shapes are synchronized later.
            }

            // Iterate over raints.
            for (int i = 0; i < step.positionIterations; ++i)
            {
                bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.b2_contactBaumgarte);

                bool jointsOkay = true;
                for (int j = 0; j < _jointCount; ++j)
                {
                    bool jointOkay = _joints[j].SolvePositionConstraints(Settings.b2_contactBaumgarte);
                    jointsOkay = jointsOkay && jointOkay;
                }

                if (contactsOkay && jointsOkay)
                {
                    // Exit early if the position errors are small.
                    break;
                }
            }

            Report(_contactSolver._constraints);

            if (allowSleep)
            {
                float minSleepTime = Settings.b2_FLT_MAX;

        #if !TARGET_float_IS_FIXED
                float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance;
                float angTolSqr = Settings.b2_angularSleepTolerance * Settings.b2_angularSleepTolerance;
        #endif

                for (int i = 0; i < _bodyCount; ++i)
                {
                    Body b = _bodies[i];
                    if (b._invMass == 0.0f)
                    {
                        continue;
                    }

                    if ((b._flags & BodyFlags.AllowSleep) == 0)
                    {
                        b._sleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }

                    if ((b._flags & BodyFlags.AllowSleep) == 0 ||
        #if TARGET_float_IS_FIXED
                        MathUtils.Abs(b._angularVelocity) > Settings.b2_angularSleepTolerance ||
                        MathUtils.Abs(b._linearVelocity.X) > Settings.b2_linearSleepTolerance ||
                        MathUtils.Abs(b._linearVelocity.Y) > Settings.b2_linearSleepTolerance)
        #else
                        b._angularVelocity *b._angularVelocity > angTolSqr ||
                        Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
        #endif
                    {
                        b._sleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }
                    else
                    {
                        b._sleepTime += step.dt;
                        minSleepTime  = Math.Min(minSleepTime, b._sleepTime);
                    }
                }
Beispiel #16
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body g1 = _ground1;
            Body g2 = _ground2;
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            float K = 0.0f;
            _J.SetZero();

            if (_revolute1 != null)
            {
                _J.angular1 = -1.0f;
                K += b1._invI;
            }
            else
            {
                XForm xf1, xfg1;
                b1.GetXForm(out xf1);
                g1.GetXForm(out xfg1);

                Vector2 ug = MathUtils.Multiply(ref xfg1.R, _prismatic1._localXAxis1);
                Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
                float crug = MathUtils.Cross(r, ug);
                _J.linear1 = -ug;
                _J.angular1 = -crug;
                K += b1._invMass + b1._invI * crug * crug;
            }

            if (_revolute2 != null)
            {
                _J.angular2 = -_ratio;
                K += _ratio * _ratio * b2._invI;
            }
            else
            {
                XForm xfg1, xf2;
                g1.GetXForm(out xfg1);
                b2.GetXForm(out xf2);

                Vector2 ug = MathUtils.Multiply(ref xfg1.R, _prismatic2._localXAxis1);
                Vector2 r = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
                float crug = MathUtils.Cross(r, ug);
                _J.linear2 = -_ratio * ug;
                _J.angular2 = -_ratio * crug;
                K += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug);
            }

            // Compute effective mass.
            Debug.Assert(K > 0.0f);
            _mass = 1.0f / K;

            if (step.warmStarting)
            {
                // Warm starting.
                b1._linearVelocity += b1._invMass * _impulse * _J.linear1;
                b1._angularVelocity += b1._invI * _impulse * _J.angular1;
                b2._linearVelocity += b2._invMass * _impulse * _J.linear2;
                b2._angularVelocity += b2._invI * _impulse * _J.angular2;
            }
            else
            {
                _impulse = 0.0f;
            }
        }
Beispiel #17
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b = _bodyB;

            float mass = b.GetMass();

            // Frequency
            float omega = 2.0f * Settings.b2_pi * _frequencyHz;

            // Damping coefficient
            float d = 2.0f * mass * _dampingRatio * omega;

            // Spring stiffness
            float k = mass * (omega * omega);

            // magic formulas
            // gamma has units of inverse mass.
            // beta has units of inverse time.
            Debug.Assert(d + step.dt * k > Settings.b2_FLT_EPSILON);
            _gamma = 1.0f / (step.dt * (d + step.dt * k));
            _beta = step.dt * k * _gamma;

            // Compute the effective mass matrix.
            XForm xf1;
            b.GetXForm(out xf1);
            Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor - b.GetLocalCenter());

            // K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
            //      = [1/m1+1/m2     0    ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y]
            //        [    0     1/m1+1/m2]           [-r1.X*r1.Y r1.X*r1.X]           [-r1.X*r1.Y r1.X*r1.X]
            float invMass = b._invMass;
            float invI = b._invI;

            Mat22 K1 = new Mat22(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass));
            Mat22 K2 = new Mat22(new Vector2(invI * r.Y * r.Y, -invI * r.X * r.Y), new Vector2(-invI * r.X * r.Y, invI * r.X * r.X));

            Mat22 K;
            Mat22.Add(ref K1, ref K2, out K);

            K.col1.X += _gamma;
            K.col2.Y += _gamma;

            _mass = K.GetInverse();

            _C = b._sweep.c + r - _target;

            // Cheat with some damping
            b._angularVelocity *= 0.98f;

            // Warm starting.
            _impulse *= step.dtRatio;
            b._linearVelocity += invMass * _impulse;
            b._angularVelocity += invI * MathUtils.Cross(r, _impulse);
        }
Beispiel #18
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            XForm xf1, xf2;
            b1.GetXForm(out xf1);
            b2.GetXForm(out xf2);

            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

            Vector2 p1 = b1._sweep.c + r1;
            Vector2 p2 = b2._sweep.c + r2;

            Vector2 s1 = _groundAnchor1;
            Vector2 s2 = _groundAnchor2;

            // Get the pulley axes.
            _u1 = p1 - s1;
            _u2 = p2 - s2;

            float length1 = _u1.Length();
            float length2 = _u2.Length();

            if (length1 > Settings.b2_linearSlop)
            {
                _u1 *= 1.0f / length1;
            }
            else
            {
                _u1 = Vector2.Zero;
            }

            if (length2 > Settings.b2_linearSlop)
            {
                _u2 *= 1.0f / length2;
            }
            else
            {
                _u2 = Vector2.Zero;
            }

            float C = _ant - length1 - _ratio * length2;
            if (C > 0.0f)
            {
                _state = LimitState.Inactive;
                _impulse = 0.0f;
            }
            else
            {
                _state = LimitState.AtUpper;
            }

            if (length1 < _maxLength1)
            {
                _limitState1 = LimitState.Inactive;
                _limitImpulse1 = 0.0f;
            }
            else
            {
                _limitState1 = LimitState.AtUpper;
            }

            if (length2 < _maxLength2)
            {
                _limitState2 = LimitState.Inactive;
                _limitImpulse2 = 0.0f;
            }
            else
            {
                _limitState2 = LimitState.AtUpper;
            }

            // Compute effective mass.
            float cr1u1 = MathUtils.Cross(r1, _u1);
            float cr2u2 = MathUtils.Cross(r2, _u2);

            _limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
            _limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
            _pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
            Debug.Assert(_limitMass1 > Settings.b2_FLT_EPSILON);
            Debug.Assert(_limitMass2 > Settings.b2_FLT_EPSILON);
            Debug.Assert(_pulleyMass > Settings.b2_FLT_EPSILON);
            _limitMass1 = 1.0f / _limitMass1;
            _limitMass2 = 1.0f / _limitMass2;
            _pulleyMass = 1.0f / _pulleyMass;

            if (step.warmStarting)
            {
                // Scale impulses to support variable time steps.
                _impulse *= step.dtRatio;
                _limitImpulse1 *= step.dtRatio;
                _limitImpulse2 *= step.dtRatio;

                // Warm starting.
                Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
                Vector2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2;
                b1._linearVelocity += b1._invMass * P1;
                b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
                b2._linearVelocity += b2._invMass * P2;
                b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
            }
            else
            {
                _impulse = 0.0f;
                _limitImpulse1 = 0.0f;
                _limitImpulse2 = 0.0f;
            }
        }
Beispiel #19
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body b = _bodyB;

            XForm xf1;
            b.GetXForm(out xf1);

            Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor - b.GetLocalCenter());

            // Cdot = v + cross(w, r)
            Vector2 Cdot = b._linearVelocity + MathUtils.Cross(b._angularVelocity, r);
            Vector2 impulse = MathUtils.Multiply(ref _mass, -(Cdot + _beta * _C + _gamma * _impulse));

            Vector2 oldImpulse = _impulse;
            _impulse += impulse;
            float maxImpulse = step.dt * _maxForce;
            if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
            {
            _impulse *= maxImpulse / _impulse.Length();
            }
            impulse = _impulse - oldImpulse;

            b._linearVelocity += b._invMass * impulse;
            b._angularVelocity += b._invI * MathUtils.Cross(r, impulse);
        }
Beispiel #20
0
        public void Solve(ref TimeStep step, Vector2 gravity, bool allowSleep)
        {
            // Integrate velocities and apply damping.
            for (int i = 0; i < _bodyCount; ++i)
            {
                Body b = _bodies[i];

                if (b.IsStatic)
                    continue;

                // Integrate velocities.
                b._linearVelocity += step.dt * (gravity + b._invMass * b._force);
                b._angularVelocity += step.dt * b._invI * b._torque;

                // Reset forces.
                b._force = new Vector2(0.0f, 0.0f);
                b._torque = 0.0f;

                // Apply damping.
                // ODE: dv/dt + c * v = 0
                // Solution: v(t) = v0 * exp(-c * t)
                // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
                // v2 = exp(-c * dt) * v1
                // Taylor expansion:
                // v2 = (1.0f - c * dt) * v1
                b._linearVelocity *= MathUtils.Clamp(1.0f - step.dt * b._linearDamping, 0.0f, 1.0f);
                b._angularVelocity *= MathUtils.Clamp(1.0f - step.dt * b._angularDamping, 0.0f, 1.0f);
            }

            _contactSolver.Reset(ref step, _contacts);

            // Initialize velocity raints.
            _contactSolver.InitVelocityConstraints(ref step);

            for (int i = 0; i < _jointCount; ++i)
            {
                _joints[i].InitVelocityConstraints(ref step);
            }

            // Solve velocity raints.
            for (int i = 0; i < step.velocityIterations; ++i)
            {
                for (int j = 0; j < _jointCount; ++j)
                {
                    _joints[j].SolveVelocityConstraints(ref step);
                }

                _contactSolver.SolveVelocityConstraints();
            }

            // Post-solve (store impulses for warm starting).
            _contactSolver.FinalizeVelocityConstraints();

            // Integrate positions.
            for (int i = 0; i < _bodyCount; ++i)
            {
                Body b = _bodies[i];

                if (b.IsStatic)
                    continue;

                // Check for large velocities.
                Vector2 translation = step.dt * b._linearVelocity;
                if (Vector2.Dot(translation, translation) > Settings.b2_maxTranslationSquared)
                {
                    translation.Normalize();
                    b._linearVelocity = (Settings.b2_maxTranslation * step.inv_dt) * translation;
                }

                float rotation = step.dt * b._angularVelocity;
                if (rotation * rotation > Settings.b2_maxRotationSquared)
                {
                    if (rotation < 0.0)
                    {
                        b._angularVelocity = -step.inv_dt * Settings.b2_maxRotation;
                    }
                    else
                    {
                        b._angularVelocity = step.inv_dt * Settings.b2_maxRotation;
                    }
                }

                // Store positions for continuous collision.
                b._sweep.c0 = b._sweep.c;
                b._sweep.a0 = b._sweep.a;

                // Integrate
                b._sweep.c += step.dt * b._linearVelocity;
                b._sweep.a += step.dt * b._angularVelocity;

                // Compute new transform
                b.SynchronizeTransform();

                // Note: shapes are synchronized later.
            }

            // Iterate over raints.
            for (int i = 0; i < step.positionIterations; ++i)
            {
                bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.b2_contactBaumgarte);

                bool jointsOkay = true;
                for (int j = 0; j < _jointCount; ++j)
                {
                    bool jointOkay = _joints[j].SolvePositionConstraints(Settings.b2_contactBaumgarte);
                    jointsOkay = jointsOkay && jointOkay;
                }

                if (contactsOkay && jointsOkay)
                {
                    // Exit early if the position errors are small.
                    break;
                }
            }

            Report(_contactSolver._constraints);

            if (allowSleep)
            {
                float minSleepTime = Settings.b2_FLT_MAX;

            #if !TARGET_float_IS_FIXED
                float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance;
                float angTolSqr = Settings.b2_angularSleepTolerance * Settings.b2_angularSleepTolerance;
            #endif

                for (int i = 0; i < _bodyCount; ++i)
                {
                    Body b = _bodies[i];
                    if (b._invMass == 0.0f)
                    {
                        continue;
                    }

                    if ((b._flags & BodyFlags.AllowSleep) == 0)
                    {
                        b._sleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }

                    if ((b._flags & BodyFlags.AllowSleep) == 0 ||
            #if TARGET_float_IS_FIXED
                        MathUtils.Abs(b._angularVelocity) > Settings.b2_angularSleepTolerance ||
                        MathUtils.Abs(b._linearVelocity.X) > Settings.b2_linearSleepTolerance ||
                        MathUtils.Abs(b._linearVelocity.Y) > Settings.b2_linearSleepTolerance)
            #else
                        b._angularVelocity * b._angularVelocity > angTolSqr ||
                        Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
            #endif
                    {
                        b._sleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }
                    else
                    {
                        b._sleepTime += step.dt;
                        minSleepTime = Math.Min(minSleepTime, b._sleepTime);
                    }
                }
Beispiel #21
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            _localCenter1 = b1.GetLocalCenter();
            _localCenter2 = b2.GetLocalCenter();

            XForm xf1, xf2;

            b1.GetXForm(out xf1);
            b2.GetXForm(out xf2);

            // Compute the effective masses.
            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - _localCenter1);
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - _localCenter2);
            Vector2 d  = b2._sweep.c + r2 - b1._sweep.c - r1;

            _invMass1 = b1._invMass;
            _invI1    = b1._invI;
            _invMass2 = b2._invMass;
            _invI2    = b2._invI;

            // Compute motor Jacobian and effective mass.
            {
                _axis = MathUtils.Multiply(ref xf1.R, _localXAxis1);
                _a1   = MathUtils.Cross(d + r1, _axis);
                _a2   = MathUtils.Cross(r2, _axis);

                _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
                Debug.Assert(_motorMass > Settings.b2_FLT_EPSILON);
                _motorMass = 1.0f / _motorMass;
            }

            // Prismatic raint.
            {
                _perp = MathUtils.Multiply(ref xf1.R, _localYAxis1);

                _s1 = MathUtils.Cross(d + r1, _perp);
                _s2 = MathUtils.Cross(r2, _perp);

                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
                float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
                float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;

                _K.col1 = new Vector2(k11, k12);
                _K.col2 = new Vector2(k12, k22);
            }

            // Compute motor and limit terms.
            if (_enableLimit)
            {
                float jointTranslation = Vector2.Dot(_axis, d);
                if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop)
                {
                    _limitState = LimitState.Equal;
                }
                else if (jointTranslation <= _lowerTranslation)
                {
                    if (_limitState != LimitState.AtLower)
                    {
                        _limitState = LimitState.AtLower;
                        _impulse.Y  = 0.0f;
                    }
                }
                else if (jointTranslation >= _upperTranslation)
                {
                    if (_limitState != LimitState.AtUpper)
                    {
                        _limitState = LimitState.AtUpper;
                        _impulse.Y  = 0.0f;
                    }
                }
                else
                {
                    _limitState = LimitState.Inactive;
                    _impulse.Y  = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.Inactive;
            }

            if (_enableMotor == false)
            {
                _motorImpulse = 0.0f;
            }

            if (step.warmStarting)
            {
                // Account for variable time step.
                _impulse      *= step.dtRatio;
                _motorImpulse *= step.dtRatio;

                Vector2 P  = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis;
                float   L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1;
                float   L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2;

                b1._linearVelocity  -= _invMass1 * P;
                b1._angularVelocity -= _invI1 * L1;

                b2._linearVelocity  += _invMass2 * P;
                b2._angularVelocity += _invI2 * L2;
            }
            else
            {
                _impulse      = Vector2.Zero;
                _motorImpulse = 0.0f;
            }
        }
Beispiel #22
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Vector2 v1 = b1._linearVelocity;
            float w1 = b1._angularVelocity;
            Vector2 v2 = b2._linearVelocity;
            float w2 = b2._angularVelocity;

            // Solve linear motor raint.
            if (_enableMotor && _limitState != LimitState.Equal)
            {
                float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
                float impulse = _motorMass * (_motorSpeed - Cdot);
                float oldImpulse = _motorImpulse;
                float maxImpulse = step.dt * _maxMotorForce;
                _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse = _motorImpulse - oldImpulse;

                Vector2 P = impulse * _axis;
                float L1 = impulse * _a1;
                float L2 = impulse * _a2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }

            float Cdot1 = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;

            if (_enableLimit && _limitState != LimitState.Inactive)
            {
                // Solve prismatic and limit raint in block form.
                float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
                Vector2 Cdot = new Vector2(Cdot1, Cdot2);

                Vector2 f1 = _impulse;
                Vector2 df =  _K.Solve(-Cdot);
                _impulse += df;

                if (_limitState == LimitState.AtLower)
                {
                    _impulse.Y = Math.Max(_impulse.Y, 0.0f);
                }
                else if (_limitState == LimitState.AtUpper)
                {
                    _impulse.Y = Math.Min(_impulse.Y, 0.0f);
                }

                // f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
                float b = -Cdot1 - (_impulse.Y - f1.Y) * _K.col2.X;
                float f2r = b / _K.col1.X + f1.X;
                _impulse.X = f2r;

                df = _impulse - f1;

                Vector2 P = df.X * _perp + df.Y * _axis;
                float L1 = df.X * _s1 + df.Y * _a1;
                float L2 = df.X * _s2 + df.Y * _a2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }
            else
            {
                // Limit is inactive, just solve the prismatic raint in block form.
                float df = (-Cdot1) / _K.col1.X;
                _impulse.X += df;

                Vector2 P = df * _perp;
                float L1 = df * _s1;
                float L2 = df * _s2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }

            b1._linearVelocity = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity = v2;
            b2._angularVelocity = w2;
        }
Beispiel #23
0
 internal abstract void InitVelocityConstraints(ref TimeStep step);