internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body bA = _bodyA;
            Body bB = _bodyB;

            Transform xfA, xfB;
            bA.GetTransform(out xfA);
            bB.GetTransform(out xfB);

            // Compute the effective mass matrix.
            Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchorA - bA.GetLocalCenter());
            Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchorB - bB.GetLocalCenter());

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

            // Matlab
            // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
            //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
            //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]

            float mA = bA._invMass, mB = bB._invMass;
            float iA = bA._invI, iB = bB._invI;

            _mass.col1.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
            _mass.col2.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
            _mass.col3.X = -rA.Y * iA - rB.Y * iB;
            _mass.col1.Y = _mass.col2.X;
            _mass.col2.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
            _mass.col3.Y = rA.X * iA + rB.X * iB;
            _mass.col1.Z = _mass.col3.X;
            _mass.col2.Z = _mass.col3.Y;
            _mass.col3.Z = iA + iB;

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

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

                bA._linearVelocity -= mA * P;
                bA._angularVelocity -= iA * (MathUtils.Cross(rA, P) + _impulse.Z);

                bB._linearVelocity += mB * P;
                bB._angularVelocity += iB * (MathUtils.Cross(rB, P) + _impulse.Z);
            }
            else
            {
                _impulse = Vector3.Zero;
            }
        }
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            _localCenterA = b1.GetLocalCenter();
            _localCenterB = b2.GetLocalCenter();

            Transform xf1, xf2;
            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

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

            _invMassA = b1._invMass;
            _invIA = b1._invI;
            _invMassB = b2._invMass;
            _invIB = 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 = _invMassA + _invMassB + _invIA * _a1 * _a1 + _invIB * _a2 * _a2;

                if (_motorMass > Settings.b2_epsilon)
                {
                    _motorMass = 1.0f / _motorMass;
                }
            }

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

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

                float m1 = _invMassA, m2 = _invMassB;
                float i1 = _invIA, i2 = _invIB;

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

                _K.col1 = new Vector3(k11, k12, k13);
                _K.col2 = new Vector3(k12, k22, k23);
                _K.col3 = new Vector3(k13, k23, k33);
            }

            // 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.Z = 0.0f;
                    }
                }
                else if (jointTranslation >= _upperTranslation)
                {
                    if (_limitState != LimitState.AtUpper)
                    {
                        _limitState = LimitState.AtUpper;
                        _impulse.Z = 0.0f;
                    }
                }
                else
                {
                    _limitState = LimitState.Inactive;
                    _impulse.Z = 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.Z) * _axis;
                float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1;
                float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2;

                b1._linearVelocity -= _invMassA * P;
                b1._angularVelocity -= _invIA * L1;

                b2._linearVelocity += _invMassB * P;
                b2._angularVelocity += _invIB * L2;
            }
            else
            {
                _impulse = Vector3.Zero;
                _motorImpulse = 0.0f;
            }
        }
Exemple #3
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 constraint.
	        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 -= _invMassA * P;
		        w1 -= _invIA * L1;

		        v2 += _invMassB * P;
		        w2 += _invIB * L2;
	        }

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

	        if (_enableLimit && _limitState != LimitState.Inactive)
	        {
		        // Solve prismatic and limit constraint 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;
                if (_K.col1.X != 0.0f)
                {
                    f2r = b / _K.col1.X + f1.X;
                }
                else
                {
                    f2r = 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 -= _invMassA * P;
		        w1 -= _invIA * L1;

		        v2 += _invMassB * P;
		        w2 += _invIB * L2;
	        }
	        else
	        {
		        // Limit is inactive, just solve the prismatic constraint in block form.
                
                float df;
                if (_K.col1.X != 0.0f)
                {
                    df = -Cdot1 / _K.col1.X;
                }
                else
                {
                    df = 0.0f;
                }

		        _impulse.X += df;

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

		        v1 -= _invMassA * P;
		        w1 -= _invIA * L1;

		        v2 += _invMassB * P;
		        w2 += _invIB * L2;
	        }

	        b1._linearVelocity = v1;
	        b1._angularVelocity = w1;
	        b2._linearVelocity = v2;
	        b2._angularVelocity = w2;
        }
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body bA = _bodyA;
            Body bB = _bodyB;

            Transform xfA, xfB;
            bA.GetTransform(out xfA);
            bB.GetTransform(out xfB);

            // Compute the effective mass matrix.
            Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
            Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());

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

            // Matlab
            // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
            //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
            //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]

            float mA = bA._invMass, mB = bB._invMass;
            float iA = bA._invI, iB = bB._invI;

            Mat22 K1 = new Mat22();
            K1.col1.X = mA + mB; K1.col2.X = 0.0f;
            K1.col1.Y = 0.0f; K1.col2.Y = mA + mB;

            Mat22 K2 = new Mat22();
            K2.col1.X = iA * rA.Y * rA.Y; K2.col2.X = -iA * rA.X * rA.Y;
            K2.col1.Y = -iA * rA.X * rA.Y; K2.col2.Y = iA * rA.X * rA.X;

            Mat22 K3 = new Mat22();
            K3.col1.X = iB * rB.Y * rB.Y; K3.col2.X = -iB * rB.X * rB.Y;
            K3.col1.Y = -iB * rB.X * rB.Y; K3.col2.Y = iB * rB.X * rB.X;

            Mat22 K12;
            Mat22.Add(ref K1, ref K2, out K12);
            Mat22 K;
            Mat22.Add(ref K12, ref K3, out K);
            _linearMass = K.GetInverse();

            _angularMass = iA + iB;
            if (_angularMass > 0.0f)
            {
                _angularMass = 1.0f / _angularMass;
            }

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

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

                bA._linearVelocity -= mA * P;
                bA._angularVelocity -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);

                bB._linearVelocity += mB * P;
                bB._angularVelocity += iB * (MathUtils.Cross(rB, P) + _angularImpulse);
            }
            else
            {
                _linearImpulse = Vector2.Zero;
                _angularImpulse = 0.0f;
            }
        }
Exemple #5
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
	        Body bA = _bodyA;
	        Body bB = _bodyB;

            Vector2 vA = bA._linearVelocity;
            float wA = bA._angularVelocity;
            Vector2 vB = bB._linearVelocity;
            float wB = bB._angularVelocity;

            float mA = bA._invMass, mB = bB._invMass;
            float iA = bA._invI, iB = bB._invI;

            Transform xfA, xfB;
            bA.GetTransform(out xfA);
            bB.GetTransform(out xfB);

            Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchorA - bA.GetLocalCenter());
            Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchorB - bB.GetLocalCenter());

            //  Solve point-to-point constraint
	        Vector2 Cdot1 = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);
	        float Cdot2 = wB - wA;
	        Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);

	        Vector3 impulse = _mass.Solve33(-Cdot);
	        _impulse += impulse;

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

	        vA -= mA * P;
	        wA -= iA * (MathUtils.Cross(rA, P) + impulse.Z);

	        vB += mB * P;
	        wB += iB * (MathUtils.Cross(rB, P) + impulse.Z);

	        bA._linearVelocity = vA;
	        bA._angularVelocity = wA;
	        bB._linearVelocity = vB;
	        bB._angularVelocity = wB;

        }
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body b = _bodyB;

            Transform xf1;
            b.GetTransform(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);
        }
Exemple #7
0
        void Solve(ref TimeStep step)
        {
            // Size the island for the worst case.
            _island.Reset(_bodyCount,
                          _contactManager._contactCount,
                          _jointCount,
                          _contactManager.ContactListener);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

                        Body other = ce.Other;

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

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

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

                        Body other = je.Other;

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

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

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

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

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

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

            // Synchronize fixtures, check for out of range bodies.
            for (Body b = _bodyList; b != null; b = b.GetNext())
            {
                // If a body was not in an island then it did not move.
                if ((b._flags & BodyFlags.Island) != BodyFlags.Island)
                {
                    continue;
                }

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

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

            // Look for new contacts.
            _contactManager.FindNewContacts();
        }
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
	        Body b1 = _bodyA;
	        Body b2 = _bodyB;

            Transform xf1, xf2;
            b1.GetTransform(out xf1);
            b2.GetTransform(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 < _length)
            {
                return ;
            }

	        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_epsilon);
            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

	        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 = step.dt * (d + step.dt * k);
                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
                _bias = C * step.dt * k * _gamma;

                _mass = invMass + _gamma;
                _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
	        }

	        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;
	        }
        }
        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 constraint.
            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 constraint.
            if (_enableLimit && _limitState != LimitState.Inactive)
            {
                Transform xf1, xf2;
                b1.GetTransform(out xf1);
                b2.GetTransform(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 constraint
                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
            {
                Transform xf1, xf2;
                b1.GetTransform(out xf1);
                b2.GetTransform(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 constraint
                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;
        }
        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.
            Transform xf1, xf2;
            b1.GetTransform(out xf1);
            b2.GetTransform(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 = i1 + i2;
            if (_motorMass > 0.0f)
            {
                _motorMass = 1.0f / _motorMass;
            }

            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;
            }
        }
Exemple #11
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.angularA = -1.0f;
                K          += b1._invI;
            }
            else
            {
                Transform xf1, xfg1;
                b1.GetTransform(out xf1);
                g1.GetTransform(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.linearA  = -ug;
                _J.angularA = -crug;
                K          += b1._invMass + b1._invI * crug * crug;
            }

            if (_revolute2 != null)
            {
                _J.angularB = -_ratio;
                K          += _ratio * _ratio * b2._invI;
            }
            else
            {
                Transform xfg1, xf2;
                g1.GetTransform(out xfg1);
                b2.GetTransform(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.linearB  = -_ratio * ug;
                _J.angularB = -_ratio * crug;
                K          += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug);
            }

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

            if (step.warmStarting)
            {
                // Warm starting.
                b1._linearVelocity  += b1._invMass * _impulse * _J.linearA;
                b1._angularVelocity += b1._invI * _impulse * _J.angularA;
                b2._linearVelocity  += b2._invMass * _impulse * _J.linearB;
                b2._angularVelocity += b2._invI * _impulse * _J.angularB;
            }
            else
            {
                _impulse = 0.0f;
            }
        }
Exemple #12
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(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);
            }
        }
Exemple #13
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(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_epsilon);
            Debug.Assert(_limitMass2 > Settings.b2_epsilon);
            Debug.Assert(_pulleyMass > Settings.b2_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;
            }
        }
Exemple #14
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_epsilon);

            _gamma = step.dt * (d + step.dt * k);
            if (_gamma != 0.0f)
            {
                _gamma = 1.0f / _gamma;
            }

            _beta = step.dt * k * _gamma;

            // Compute the effective mass matrix.
            Transform xf1;

            b.GetTransform(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);
        }
Exemple #15
0
	    internal override void InitVelocityConstraints(ref TimeStep step)
        {
	        Body b1 = _bodyA;
	        Body b2 = _bodyB;

            Transform xf1, xf2;
            b1.GetTransform(out xf1);
            b2.GetTransform(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_epsilon);
	        Debug.Assert(_limitMass2 > Settings.b2_epsilon);
	        Debug.Assert(_pulleyMass > Settings.b2_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;
	        }
        }
Exemple #16
0
 internal abstract void InitVelocityConstraints(ref TimeStep step);
Exemple #17
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body bA = _bodyA;
            Body bB = _bodyB;

            Transform xfA, xfB;

            bA.GetTransform(out xfA);
            bB.GetTransform(out xfB);

            // Compute the effective mass matrix.
            Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
            Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());

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

            // Matlab
            // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
            //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
            //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]

            float mA = bA._invMass, mB = bB._invMass;
            float iA = bA._invI, iB = bB._invI;

            Mat22 K1 = new Mat22();

            K1.col1.X = mA + mB; K1.col2.X = 0.0f;
            K1.col1.Y = 0.0f; K1.col2.Y = mA + mB;

            Mat22 K2 = new Mat22();

            K2.col1.X = iA * rA.Y * rA.Y; K2.col2.X = -iA * rA.X * rA.Y;
            K2.col1.Y = -iA * rA.X * rA.Y; K2.col2.Y = iA * rA.X * rA.X;

            Mat22 K3 = new Mat22();

            K3.col1.X = iB * rB.Y * rB.Y; K3.col2.X = -iB * rB.X * rB.Y;
            K3.col1.Y = -iB * rB.X * rB.Y; K3.col2.Y = iB * rB.X * rB.X;

            Mat22 K12;

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

            Mat22.Add(ref K12, ref K3, out K);
            _linearMass = K.GetInverse();

            _angularMass = iA + iB;
            if (_angularMass > 0.0f)
            {
                _angularMass = 1.0f / _angularMass;
            }

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

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

                bA._linearVelocity  -= mA * P;
                bA._angularVelocity -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);

                bB._linearVelocity  += mB * P;
                bB._angularVelocity += iB * (MathUtils.Cross(rB, P) + _angularImpulse);
            }
            else
            {
                _linearImpulse  = Vector2.Zero;
                _angularImpulse = 0.0f;
            }
        }
        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 constraint.
            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 constraint.
            if (_enableLimit && _limitState != LimitState.Inactive)
            {
                Transform xf1, xf2;
                b1.GetTransform(out xf1);
                b2.GetTransform(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 constraint
                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
            {
                Transform xf1, xf2;
                b1.GetTransform(out xf1);
                b2.GetTransform(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 constraint
                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;
        }
Exemple #19
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body bA = _bodyA;
            Body bB = _bodyB;

            Vector2 vA = bA._linearVelocity;
            float   wA = bA._angularVelocity;
            Vector2 vB = bB._linearVelocity;
            float   wB = bB._angularVelocity;

            float mA = bA._invMass, mB = bB._invMass;
            float iA = bA._invI, iB = bB._invI;

            Transform xfA, xfB;

            bA.GetTransform(out xfA);
            bB.GetTransform(out xfB);

            Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
            Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());

            // Solve angular friction
            {
                float Cdot    = wB - wA;
                float impulse = -_angularMass * Cdot;

                float oldImpulse = _angularImpulse;
                float maxImpulse = step.dt * _maxTorque;
                _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
                impulse         = _angularImpulse - oldImpulse;

                wA -= iA * impulse;
                wB += iB * impulse;
            }

            // Solve linear friction
            {
                Vector2 Cdot = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);

                Vector2 impulse    = -MathUtils.Multiply(ref _linearMass, Cdot);
                Vector2 oldImpulse = _linearImpulse;
                _linearImpulse += impulse;

                float maxImpulse = step.dt * _maxForce;

                if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
                {
                    _linearImpulse.Normalize();
                    _linearImpulse *= maxImpulse;
                }

                impulse = _linearImpulse - oldImpulse;

                vA -= mA * impulse;
                wA -= iA * MathUtils.Cross(rA, impulse);

                vB += mB * impulse;
                wB += iB * MathUtils.Cross(rB, impulse);
            }

            bA._linearVelocity  = vA;
            bA._angularVelocity = wA;
            bB._linearVelocity  = vB;
            bB._angularVelocity = wB;
        }
        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_epsilon);

            _gamma = step.dt * (d + step.dt * k);
            if (_gamma != 0.0f)
            {
                _gamma = 1.0f / _gamma;
            }

            _beta = step.dt * k * _gamma;

            // Compute the effective mass matrix.
            Transform xf1;
            b.GetTransform(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);
        }
        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 constraint.
            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 -= _invMassA * P;
                w1 -= _invIA * L1;

                v2 += _invMassB * P;
                w2 += _invIB * 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 constraint 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 -= _invMassA * P;
                w1 -= _invIA * L1;

                v2 += _invMassB * P;
                w2 += _invIB * L2;
            }
            else
            {
                // Limit is inactive, just solve the prismatic constraint 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 -= _invMassA * P;
                w1 -= _invIA * L1;

                v2 += _invMassB * P;
                w2 += _invIB * L2;
            }

            b1._linearVelocity  = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity  = v2;
            b2._angularVelocity = w2;
        }
Exemple #22
0
 internal abstract void InitVelocityConstraints(ref TimeStep step);
Exemple #23
0
 internal abstract void SolveVelocityConstraints(ref TimeStep step);
Exemple #24
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.GetType() != BodyType.Dynamic)
                {
                    continue;
                }

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

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

            // Partition contacts so that contacts with static bodies are solved last.
            int i1 = -1;
            for (int i2 = 0; i2 < _contactCount; ++i2)
            {
                Fixture fixtureA = _contacts[i2].GetFixtureA();
                Fixture fixtureB = _contacts[i2].GetFixtureB();
                Body bodyA = fixtureA.GetBody();
                Body bodyB = fixtureB.GetBody();
                bool nonStatic = bodyA.GetType() != BodyType.Static && bodyB.GetType() != BodyType.Static;
                if (nonStatic)
                {
                    ++i1;
                    //b2Swap(_contacts[i1], _contacts[i2]);
                    Contact temp = _contacts[i1];
                    _contacts[i1] = _contacts[i2];
                    _contacts[i2] = temp;
                }
            }

            // Initialize velocity constraints.
            _contactSolver.Reset(_contacts, _contactCount, step.dtRatio);
            _contactSolver.WarmStart();

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

            // Solve velocity constraints.
            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.StoreImpulses();

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

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

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

                float rotation = step.dt * b._angularVelocity;
                if (rotation * rotation > Settings.b2_maxRotationSquared)
                {
                    float ratio = Settings.b2_maxRotation / Math.Abs(rotation);
                    b._angularVelocity *= ratio;
                }

                // 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 constraints.
            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_maxFloat;

                const float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance;
                const float angTolSqr = Settings.b2_angularSleepTolerance * Settings.b2_angularSleepTolerance;

                for (int i = 0; i < _bodyCount; ++i)
                {
                    Body b = _bodies[i];
                    if (b.GetType() == BodyType.Static)
                    {
                        continue;
                    }

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

                    if ((b._flags & BodyFlags.AutoSleep) == 0 ||
                        b._angularVelocity * b._angularVelocity > angTolSqr ||
                        Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
                    {
                        b._sleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }
                    else
                    {
                        b._sleepTime += step.dt;
                        minSleepTime = Math.Min(minSleepTime, b._sleepTime);
                    }
                }

                if (minSleepTime >= Settings.b2_timeToSleep)
                {
                    for (int i = 0; i < _bodyCount; ++i)
                    {
                        Body b = _bodies[i];
                        b.SetAwake(false);
                    }
                }
            }
        }
Exemple #25
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.GetType() != BodyType.Dynamic)
                {
                    continue;
                }

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

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

            // Partition contacts so that contacts with static bodies are solved last.
	        int i1 = -1;
	        for (int i2 = 0; i2 < _contactCount; ++i2)
	        {
		        Fixture fixtureA = _contacts[i2].GetFixtureA();
		        Fixture fixtureB = _contacts[i2].GetFixtureB();
		        Body bodyA = fixtureA.GetBody();
		        Body bodyB = fixtureB.GetBody();
                bool nonStatic = bodyA.GetType() != BodyType.Static && bodyB.GetType() != BodyType.Static;
		        if (nonStatic)
		        {
			        ++i1;
			        //b2Swap(_contacts[i1], _contacts[i2]);
                    Contact temp = _contacts[i1];
                    _contacts[i1] = _contacts[i2];
                    _contacts[i2] = temp;
		        }
	        }

	        // Initialize velocity constraints.
            _contactSolver.Reset(_contacts, _contactCount, step.dtRatio);
	        _contactSolver.WarmStart();

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

	        // Solve velocity constraints.
	        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.StoreImpulses();

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

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

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

		        float rotation = step.dt * b._angularVelocity;
		        if (rotation * rotation > Settings.b2_maxRotationSquared)
		        {
                    float ratio = Settings.b2_maxRotation / Math.Abs(rotation);
                    b._angularVelocity *= ratio;
		        }

		        // 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 constraints.
            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_maxFloat;

		        const float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance;
		        const float angTolSqr = Settings.b2_angularSleepTolerance * Settings.b2_angularSleepTolerance;

		        for (int i = 0; i < _bodyCount; ++i)
		        {
			        Body b = _bodies[i];
			        if (b.GetType() == BodyType.Static)
			        {
				        continue;
			        }

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

			        if ((b._flags & BodyFlags.AutoSleep) == 0 ||
				        b._angularVelocity * b._angularVelocity > angTolSqr ||
				        Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
			        {
				        b._sleepTime = 0.0f;
				        minSleepTime = 0.0f;
			        }
			        else
			        {
				        b._sleepTime += step.dt;
				        minSleepTime = Math.Min(minSleepTime, b._sleepTime);
			        }
		        }

		        if (minSleepTime >= Settings.b2_timeToSleep)
		        {
			        for (int i = 0; i < _bodyCount; ++i)
			        {
				        Body b = _bodies[i];
                        b.SetAwake(false);
			        }
		        }
	        }
        }
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body bA = _bodyA;
            Body bB = _bodyB;

            Vector2 vA = bA._linearVelocity;
            float wA = bA._angularVelocity;
            Vector2 vB = bB._linearVelocity;
            float wB = bB._angularVelocity;

            float mA = bA._invMass, mB = bB._invMass;
            float iA = bA._invI, iB = bB._invI;

            Transform xfA, xfB;
            bA.GetTransform(out xfA);
            bB.GetTransform(out xfB);

            Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
            Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());

            // Solve angular friction
            {
                float Cdot = wB - wA;
                float impulse = -_angularMass * Cdot;

                float oldImpulse = _angularImpulse;
                float maxImpulse = step.dt * _maxTorque;
                _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
                impulse = _angularImpulse - oldImpulse;

                wA -= iA * impulse;
                wB += iB * impulse;
            }

            // Solve linear friction
            {
                Vector2 Cdot = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);

                Vector2 impulse = -MathUtils.Multiply(ref _linearMass, Cdot);
                Vector2 oldImpulse = _linearImpulse;
                _linearImpulse += impulse;

                float maxImpulse = step.dt * _maxForce;

                if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
                {
                    _linearImpulse.Normalize();
                    _linearImpulse *= maxImpulse;
                }

                impulse = _linearImpulse - oldImpulse;

                vA -= mA * impulse;
                wA -= iA * MathUtils.Cross(rA, impulse);

                vB += mB * impulse;
                wB += iB * MathUtils.Cross(rB, impulse);
            }

            bA._linearVelocity = vA;
            bA._angularVelocity = wA;
            bB._linearVelocity = vB;
            bB._angularVelocity = wB;
        }
        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.angularA = -1.0f;
                K += b1._invI;
            }
            else
            {
                Transform xf1, xfg1;
                b1.GetTransform(out xf1);
                g1.GetTransform(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.linearA = -ug;
                _J.angularA = -crug;
                K += b1._invMass + b1._invI * crug * crug;
            }

            if (_revolute2 != null)
            {
                _J.angularB = -_ratio;
                K += _ratio * _ratio * b2._invI;
            }
            else
            {
                Transform xfg1, xf2;
                g1.GetTransform(out xfg1);
                b2.GetTransform(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.linearB = -_ratio * ug;
                _J.angularB = -_ratio * crug;
                K += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug);
            }

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

            if (step.warmStarting)
            {
                // Warm starting.
                b1._linearVelocity += b1._invMass * _impulse * _J.linearA;
                b1._angularVelocity += b1._invI * _impulse * _J.angularA;
                b2._linearVelocity += b2._invMass * _impulse * _J.linearB;
                b2._angularVelocity += b2._invI * _impulse * _J.angularB;
            }
            else
            {
                _impulse = 0.0f;
            }
        }
        void Solve(ref TimeStep step)
        {
            // Size the island for the worst case.
            _island.Reset(_bodyCount,
                          _contactManager._contactCount,
                          _jointCount,
                          _contactManager.ContactListener);

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

            // Build and simulate all awake islands.
            int stackSize = _bodyCount;
            if (stackSize > stack.Length)
                stack = new Body[Math.Max(stack.Length * 2, stackSize)];

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

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

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

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

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

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

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

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

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

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

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

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

                        Body other = ce.Other;

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

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

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

                        Body other = je.Other;

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

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

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

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

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

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

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

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

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

            // Look for new contacts.
            _contactManager.FindNewContacts();
        }
        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.linearA;
            b1._angularVelocity += b1._invI * impulse * _J.angularA;
            b2._linearVelocity += b2._invMass * impulse * _J.linearB;
            b2._angularVelocity += b2._invI * impulse * _J.angularB;
        }
        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 constraint.
            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 -= _invMassA * P;
                w1 -= _invIA * L1;

                v2 += _invMassB * P;
                w2 += _invIB * 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 constraint 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 -= _invMassA * P;
                w1 -= _invIA * L1;

                v2 += _invMassB * P;
                w2 += _invIB * L2;
            }
            else
            {
                // Limit is inactive, just solve the prismatic constraint 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 -= _invMassA * P;
                w1 -= _invIA * L1;

                v2 += _invMassB * P;
                w2 += _invIB * L2;
            }

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

            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(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_epsilon);
            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

            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 = step.dt * (d + step.dt * k);
                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
                _bias  = C * step.dt * k * _gamma;

                _mass = invMass + _gamma;
                _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
            }

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

            Transform xf1, xf2;
            b1.GetTransform(out xf1);
            b2.GetTransform(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);
	        }
        }
Exemple #33
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            _localCenterA = b1.GetLocalCenter();
            _localCenterB = b2.GetLocalCenter();

            Transform xf1, xf2;

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

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

            _invMassA = b1._invMass;
            _invIA    = b1._invI;
            _invMassB = b2._invMass;
            _invIB    = 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 = _invMassA + _invMassB + _invIA * _a1 * _a1 + _invIB * _a2 * _a2;
                if (_motorMass > Settings.b2_epsilon)
                {
                    _motorMass = 1.0f / _motorMass;
                }
                else
                {
                    _motorMass = 0.0f;
                }
            }

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

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

                float m1 = _invMassA, m2 = _invMassB;
                float i1 = _invIA, i2 = _invIB;

                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  -= _invMassA * P;
                b1._angularVelocity -= _invIA * L1;

                b2._linearVelocity  += _invMassB * P;
                b2._angularVelocity += _invIB * L2;
            }
            else
            {
                _impulse      = Vector2.Zero;
                _motorImpulse = 0.0f;
            }
        }
Exemple #34
0
 internal abstract void SolveVelocityConstraints(ref TimeStep step);
Exemple #35
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 constraint.
            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 -= _invMassA * P;
                w1 -= _invIA * L1;

                v2 += _invMassB * P;
                w2 += _invIB * L2;
            }

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

            if (_enableLimit && _limitState != LimitState.Inactive)
            {
                // Solve prismatic and limit constraint 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;
                if (_K.col1.X != 0.0f)
                {
                    f2r = b / _K.col1.X + f1.X;
                }
                else
                {
                    f2r = 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 -= _invMassA * P;
                w1 -= _invIA * L1;

                v2 += _invMassB * P;
                w2 += _invIB * L2;
            }
            else
            {
                // Limit is inactive, just solve the prismatic constraint in block form.

                float df;
                if (_K.col1.X != 0.0f)
                {
                    df = -Cdot1 / _K.col1.X;
                }
                else
                {
                    df = 0.0f;
                }

                _impulse.X += df;

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

                v1 -= _invMassA * P;
                w1 -= _invIA * L1;

                v2 += _invMassB * P;
                w2 += _invIB * L2;
            }

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

            Transform xf1, xf2;
            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

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

            Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;

            float length = d.Length();

            if (length < _length)
            {
                return;
            }

	        // 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);
        }
        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.
            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(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 = i1 + i2;
            if (_motorMass > 0.0f)
            {
                _motorMass = 1.0f / _motorMass;
            }

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