C# LimitState类代码示例

        internal RopeJoint(IWorldPool worldPool, RopeJointDef def)
            : base(worldPool, def)
        {
            m_localAnchorA.set(def.localAnchorA);
            m_localAnchorB.set(def.localAnchorB);

            m_maxLength = def.maxLength;

            m_mass = 0.0f;
            m_impulse = 0.0f;
            m_state = LimitState.INACTIVE;
            m_length = 0.0f;
        }

        public RopeJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
            : base(bodyA, bodyB)
        {
            LocalAnchorA = localAnchorA;
            LocalAnchorB = localAnchorB;

            Vector2 d = WorldAnchorB - WorldAnchorA;
            MaxLength = d.Length();

            _mass = 0.0f;
            _impulse = 0.0f;
            _state = LimitState.Inactive;
            _length = 0.0f;
        }

		/// <summary>
		/// Prepares the constraint for iterative processing in the current frame.
		/// </summary>
		public override void PreProcess()
		{
			RigidBody a = BodyA, b = BodyB;

			// get world points and normal
			Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA);
			Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB);
			Vector3.Subtract(ref _worldOffsetA, ref _worldOffsetB, out _normal);
			Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA);
			Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB);

			_distance = _normal.Length();
			if (_distance < Constants.Epsilon)
				_normal = Vector3.Zero;
			else
				Vector3.Divide(ref _normal, _distance, out _normal);

			_mass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, ref _normal);

			// determine the constraint behavior for this frame
			var prevState = _state;
			if (Math.Abs(_maxDistance - _minDistance) < 2f * this.Manager.LinearErrorTolerance)
				_state = LimitState.Equal;
			else if (_distance <= _minDistance)
				_state = LimitState.Min;
			else if (_distance >= _maxDistance)
				_state = LimitState.Max;
			else
				_state = LimitState.Between;

			if (this.Manager.IsSolverWarmStarted && prevState == _state)
			{
				Vector3 impulse;
				Vector3.Multiply(ref _impulse, this.Manager.TimeStep, out impulse);
				b.ApplyImpulse(ref impulse, ref _worldOffsetB);
				Vector3.Negate(ref impulse, out impulse);
				a.ApplyImpulse(ref impulse, ref _worldOffsetA);
			}
			else
				_impulse = Vector3.Zero;

			_pImpulse = Vector3.Zero;
		}

		/// <summary>
		/// Prepares the constraint for iterative processing in the current frame.
		/// </summary>
		public override void PreProcess()
		{
			ComputeVitals();

			LimitState prevX = _statePosX, prevY = _statePosY, prevZ = _statePosZ;

			_statePosX = ComputeLinearLimitState((_limitedPosAxes & Axes.X) > 0, _positions.X, _minPos.X, _maxPos.X);
			_statePosY = ComputeLinearLimitState((_limitedPosAxes & Axes.Y) > 0, _positions.Y, _minPos.Y, _maxPos.Y);
			_statePosZ = ComputeLinearLimitState((_limitedPosAxes & Axes.Z) > 0, _positions.Z, _minPos.Z, _maxPos.Z);
			_stateRotX = ComputeAngularLimitState((_limitedRotAxes & Axes.X) > 0, _angles.X, _minAngles.X, _maxAngles.X);
			_stateRotY = ComputeAngularLimitState((_limitedRotAxes & Axes.Y) > 0, _angles.Y, _minAngles.Y, _maxAngles.Y);
			_stateRotZ = ComputeAngularLimitState((_limitedRotAxes & Axes.Z) > 0, _angles.Z, _minAngles.Z, _maxAngles.Z);

			DoLinearWarmStart(prevX, _statePosX, ref _impulsePos.X, ref _worldBasisB.X);
			DoLinearWarmStart(prevY, _statePosY, ref _impulsePos.Y, ref _worldBasisB.Y);
			DoLinearWarmStart(prevZ, _statePosZ, ref _impulsePos.Z, ref _worldBasisB.Z);

			_impulseRot = Vector3.Zero;
			_pImpulsePosX = _pImpulsePosY = _pImpulsePosZ = Vector3.Zero;
			_pImpulseRotX = _pImpulseRotY = _pImpulseRotZ = Vector3.Zero;
		}

        internal LineJoint(LineJointDef def)
            : base(def)
        {
            _localAnchor1 = def.localAnchor1;
            _localAnchor2 = def.localAnchor2;
            _localXAxis1 = def.localAxis1;
            _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);

            _impulse = Vector2.Zero;
            _motorMass = 0.0f;
            _motorImpulse = 0.0f;

            _lowerTranslation = def.lowerTranslation;
            _upperTranslation = def.upperTranslation;
            _maxMotorForce = def.maxMotorForce;
            _motorSpeed = def.motorSpeed;
            _enableLimit = def.enableLimit;
            _enableMotor = def.enableMotor;
            _limitState = LimitState.Inactive;

            _axis = Vector2.Zero;
            _perp = Vector2.Zero;
        }

        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

                Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis;
                float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1;
                float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2;
//.........这里部分代码省略.........

        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            // You cannot create a prismatic joint between bodies that
            // both have fixed rotation.
            Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);

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

            Transform xf1 = b1.GetTransform();
            Transform xf2 = b2.GetTransform();

            // Compute the effective masses.
            Vector2 r1 = xf1.TransformDirection(_localAnchor1 - _localCenter1);
            Vector2 r2 = xf2.TransformDirection(_localAnchor2 - _localCenter2);
            Vector2 d = b2._sweep.C + r2 - b1._sweep.C - r1;

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

            // Compute motor Jacobian and effective mass.
            {
                _axis = xf1.TransformDirection(_localXAxis1);
                _a1 = (d + r1).Cross(_axis);
                _a2 = r2.Cross(_axis);

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

            // Prismatic constraint.
            {
                _perp = xf1.TransformDirection(_localYAxis1);

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

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

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
                float k12 = i1 * _s1 + 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 (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    _limitState = LimitState.EqualLimits;
                }
                else if (jointTranslation <= _lowerTranslation)
                {
                    if (_limitState != LimitState.AtLowerLimit)
                    {
                        _limitState = LimitState.AtLowerLimit;
                        _impulse.z = 0.0f;
                    }
                }
                else if (jointTranslation >= _upperTranslation)
                {
                    if (_limitState != LimitState.AtUpperLimit)
                    {
                        _limitState = LimitState.AtUpperLimit;
                        _impulse.z = 0.0f;
                    }
                }
                else
                {
                    _limitState = LimitState.InactiveLimit;
                    _impulse.z = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.InactiveLimit;
            }

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

            if (step.WarmStarting)
            {
//.........这里部分代码省略.........

        public override void initVelocityConstraints(SolverData data)
        {
            m_indexA = m_bodyA.m_islandIndex;
            m_indexB = m_bodyB.m_islandIndex;
            m_localCenterA.set_Renamed(m_bodyA.m_sweep.localCenter);
            m_localCenterB.set_Renamed(m_bodyB.m_sweep.localCenter);
            m_invMassA = m_bodyA.m_invMass;
            m_invMassB = m_bodyB.m_invMass;
            m_invIA = m_bodyA.m_invI;
            m_invIB = m_bodyB.m_invI;

            Vec2 cA = data.positions[m_indexA].c;
            float aA = data.positions[m_indexA].a;
            Vec2 vA = data.velocities[m_indexA].v;
            float wA = data.velocities[m_indexA].w;

            Vec2 cB = data.positions[m_indexB].c;
            float aB = data.positions[m_indexB].a;
            Vec2 vB = data.velocities[m_indexB].v;
            float wB = data.velocities[m_indexB].w;

            Rot qA = pool.popRot();
            Rot qB = pool.popRot();
            Vec2 d = pool.popVec2();
            Vec2 temp = pool.popVec2();
            Vec2 rA = pool.popVec2();
            Vec2 rB = pool.popVec2();

            qA.set_Renamed(aA);
            qB.set_Renamed(aB);

            // Compute the effective masses.
            Rot.mulToOutUnsafe(qA, d.set_Renamed(m_localAnchorA).subLocal(m_localCenterA), rA);
            Rot.mulToOutUnsafe(qB, d.set_Renamed(m_localAnchorB).subLocal(m_localCenterB), rB);
            d.set_Renamed(cB).subLocal(cA).addLocal(rB).subLocal(rA);

            float mA = m_invMassA, mB = m_invMassB;
            float iA = m_invIA, iB = m_invIB;

            // Compute motor Jacobian and effective mass.
            {
                Rot.mulToOutUnsafe(qA, m_localXAxisA, m_axis);
                temp.set_Renamed(d).addLocal(rA);
                m_a1 = Vec2.cross(temp, m_axis);
                m_a2 = Vec2.cross(rB, m_axis);

                m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
                if (m_motorMass > 0.0f)
                {
                    m_motorMass = 1.0f / m_motorMass;
                }
            }

            // Prismatic constraint.
            {
                Rot.mulToOutUnsafe(qA, m_localYAxisA, m_perp);

                temp.set_Renamed(d).addLocal(rA);
                m_s1 = Vec2.cross(temp, m_perp);
                m_s2 = Vec2.cross(rB, m_perp);

                float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
                float k12 = iA * m_s1 + iB * m_s2;
                float k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
                float k22 = iA + iB;
                if (k22 == 0.0f)
                {
                    // For bodies with fixed rotation.
                    k22 = 1.0f;
                }
                float k23 = iA * m_a1 + iB * m_a2;
                float k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;

                m_K.ex.set_Renamed(k11, k12, k13);
                m_K.ey.set_Renamed(k12, k22, k23);
                m_K.ez.set_Renamed(k13, k23, k33);
            }

            // Compute motor and limit terms.
            if (m_enableLimit)
            {

                float jointTranslation = Vec2.dot(m_axis, d);
                if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings.linearSlop)
                {
                    m_limitState = LimitState.EQUAL;
                }
                else if (jointTranslation <= m_lowerTranslation)
                {
                    if (m_limitState != LimitState.AT_LOWER)
                    {
                        m_limitState = LimitState.AT_LOWER;
                        m_impulse.z = 0.0f;
                    }
                }
                else if (jointTranslation >= m_upperTranslation)
                {
                    if (m_limitState != LimitState.AT_UPPER)
                    {
                        m_limitState = LimitState.AT_UPPER;
//.........这里部分代码省略.........

        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

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

            // Compute the effective mass matrix.
            Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

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

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

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

            _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
            _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
            _mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
            _mass.Col1.Y = _mass.Col2.X;
            _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
            _mass.Col3.Y = r1.X * i1 + r2.X * i2;
            _mass.Col1.Z = _mass.Col3.X;
            _mass.Col2.Z = _mass.Col3.Y;
            _mass.Col3.Z = i1 + i2;

            _motorMass = 1.0f / (i1 + i2);

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

            if (_enableLimit)
            {
                float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle;
                if (Box2DXMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
                {
                    _limitState = LimitState.EqualLimits;
                }
                else if (jointAngle <= _lowerAngle)
                {
                    if (_limitState != LimitState.AtLowerLimit)
                    {
                        _impulse.Z = 0.0f;
                    }
                    _limitState = LimitState.AtLowerLimit;
                }
                else if (jointAngle >= _upperAngle)
                {
                    if (_limitState != LimitState.AtUpperLimit)
                    {
                        _impulse.Z = 0.0f;
                    }
                    _limitState = LimitState.AtUpperLimit;
                }
                else
                {
                    _limitState = LimitState.InactiveLimit;
                    _impulse.Z = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.InactiveLimit;
            }

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

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

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

                b2._linearVelocity += m2 * P;
                b2._angularVelocity += i2 * (Vec2.Cross(r2, P) + _motorImpulse + _impulse.Z);
            }
            else
            {
                _impulse.SetZero();
                _motorImpulse = 0.0f;
            }
        }

        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body bA = BodyA;
            Body bB = BodyB;

            Transform xf1;
            bA.GetTransform(out xf1);

            Transform xf2;
            bB.GetTransform(out xf2);

            _rA = MathUtils.Multiply(ref xf1.R, LocalAnchorA - bA.LocalCenter);
            _rB = MathUtils.Multiply(ref xf2.R, LocalAnchorB - bB.LocalCenter);

            // Rope axis
            _u = bB.Sweep.C + _rB - bA.Sweep.C - _rA;

            _length = _u.Length;

            float C = _length - MaxLength;
            if (C > 0.0f)
            {
                _state = LimitState.AtUpper;
            }
            else
            {
                _state = LimitState.Inactive;
            }

            if (_length > Settings.LinearSlop)
            {
                _u *= 1.0f / _length;
            }
            else
            {
                _u = Vector2.Zero;
                _mass = 0.0f;
                _impulse = 0.0f;
                return;
            }

            // Compute effective mass.
            float crA = MathUtils.Cross(_rA, _u);
            float crB = MathUtils.Cross(_rB, _u);
            float invMass = bA.InvMass + bA.InvI * crA * crA + bB.InvMass + bB.InvI * crB * crB;

            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

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

                Vector2 P = _impulse * _u;
                bA.LinearVelocity -= bA.InvMass * P;
                bA.AngularVelocity -= bA.InvI * MathUtils.Cross(_rA, P);
                bB.LinearVelocity += bB.InvMass * P;
                bB.AngularVelocity += bB.InvI * MathUtils.Cross(_rB, P);
            }
            else
            {
                _impulse = 0.0f;
            }
        }

        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = BodyA;
            Body b2 = BodyB;

            LocalCenterA = b1.LocalCenter;
            LocalCenterB = b2.LocalCenter;

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

            // Compute the effective masses.
            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - LocalCenterA);
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - 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.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.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 (Settings.EnableWarmstarting)
            {
//.........这里部分代码省略.........

        internal RevoluteJoint(IWorldPool argWorld, RevoluteJointDef def)
            : base(argWorld, def)
        {
            m_localAnchorA.set(def.localAnchorA);
            m_localAnchorB.set(def.localAnchorB);
            m_referenceAngle = def.referenceAngle;

            m_motorImpulse = 0;

            m_lowerAngle = def.lowerAngle;
            m_upperAngle = def.upperAngle;
            m_maxMotorTorque = def.maxMotorTorque;
            m_motorSpeed = def.motorSpeed;
            m_enableLimit = def.enableLimit;
            m_enableMotor = def.enableMotor;
            m_limitState = LimitState.INACTIVE;
        }

        internal override void InitVelocityConstraints(ref SolverData data)
        {
            _indexA = BodyA.IslandIndex;
            _indexB = BodyB.IslandIndex;
            _localCenterA = BodyA.Sweep.LocalCenter;
            _localCenterB = BodyB.Sweep.LocalCenter;
            _invMassA = BodyA.InvMass;
            _invMassB = BodyB.InvMass;
            _invIA = BodyA.InvI;
            _invIB = BodyB.InvI;

            Vector2 cA = data.positions[_indexA].c;
            float aA = data.positions[_indexA].a;
            Vector2 vA = data.velocities[_indexA].v;
            float wA = data.velocities[_indexA].w;

            Vector2 cB = data.positions[_indexB].c;
            float aB = data.positions[_indexB].a;
            Vector2 vB = data.velocities[_indexB].v;
            float wB = data.velocities[_indexB].w;

            Rot qA = new Rot(aA), qB = new Rot(aB);

            _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
            _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
            _u = cB + _rB - cA - _rA;

            _length = _u.Length();

            float C = _length - MaxLength;
            if (C > 0.0f)
            {
                _state = LimitState.AtUpper;
            }
            else
            {
                _state = LimitState.Inactive;
            }

            if (_length > Settings.LinearSlop)
            {
                _u *= 1.0f / _length;
            }
            else
            {
                _u = Vector2.Zero;
                _mass = 0.0f;
                _impulse = 0.0f;
                return;
            }

            // Compute effective mass.
            float crA = MathUtils.Cross(_rA, _u);
            float crB = MathUtils.Cross(_rB, _u);
            float invMass = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB;

            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

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

                Vector2 P = _impulse * _u;
                vA -= _invMassA * P;
                wA -= _invIA * MathUtils.Cross(_rA, P);
                vB += _invMassB * P;
                wB += _invIB * MathUtils.Cross(_rB, P);
            }
            else
            {
                _impulse = 0.0f;
            }

            data.velocities[_indexA].v = vA;
            data.velocities[_indexA].w = wA;
            data.velocities[_indexB].v = vB;
            data.velocities[_indexB].w = wB;
        }

 internal override void InitVelocityConstraints(TimeStep step)
 {
     Body body = this._body1;
     Body body2 = this._body2;
     Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
     Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
     float invMass = body._invMass;
     float invMass2 = body2._invMass;
     float invI = body._invI;
     float invI2 = body2._invI;
     this._mass.Col1.X = invMass + invMass2 + a.Y * a.Y * invI + a2.Y * a2.Y * invI2;
     this._mass.Col2.X = -a.Y * a.X * invI - a2.Y * a2.X * invI2;
     this._mass.Col3.X = -a.Y * invI - a2.Y * invI2;
     this._mass.Col1.Y = this._mass.Col2.X;
     this._mass.Col2.Y = invMass + invMass2 + a.X * a.X * invI + a2.X * a2.X * invI2;
     this._mass.Col3.Y = a.X * invI + a2.X * invI2;
     this._mass.Col1.Z = this._mass.Col3.X;
     this._mass.Col2.Z = this._mass.Col3.Y;
     this._mass.Col3.Z = invI + invI2;
     this._motorMass = 1f / (invI + invI2);
     if (!this._enableMotor)
     {
         this._motorImpulse = 0f;
     }
     if (this._enableLimit)
     {
         float num = body2._sweep.A - body._sweep.A - this._referenceAngle;
         if (Box2DX.Common.Math.Abs(this._upperAngle - this._lowerAngle) < 2f * Settings.AngularSlop)
         {
             this._limitState = LimitState.EqualLimits;
         }
         else
         {
             if (num <= this._lowerAngle)
             {
                 if (this._limitState != LimitState.AtLowerLimit)
                 {
                     this._impulse.Z = 0f;
                 }
                 this._limitState = LimitState.AtLowerLimit;
             }
             else
             {
                 if (num >= this._upperAngle)
                 {
                     if (this._limitState != LimitState.AtUpperLimit)
                     {
                         this._impulse.Z = 0f;
                     }
                     this._limitState = LimitState.AtUpperLimit;
                 }
                 else
                 {
                     this._limitState = LimitState.InactiveLimit;
                     this._impulse.Z = 0f;
                 }
             }
         }
     }
     if (step.WarmStarting)
     {
         this._impulse *= step.DtRatio;
         this._motorImpulse *= step.DtRatio;
         Vec2 vec = new Vec2(this._impulse.X, this._impulse.Y);
         Body expr_363 = body;
         expr_363._linearVelocity -= invMass * vec;
         body._angularVelocity -= invI * (Vec2.Cross(a, vec) + this._motorImpulse + this._impulse.Z);
         Body expr_3A8 = body2;
         expr_3A8._linearVelocity += invMass2 * vec;
         body2._angularVelocity += invI2 * (Vec2.Cross(a2, vec) + this._motorImpulse + this._impulse.Z);
     }
     else
     {
         this._impulse.SetZero();
         this._motorImpulse = 0f;
     }
 }

        public PrismaticJoint(PrismaticJointDef def)
            : base(def)
        {
            _localAnchor1 = def.LocalAnchor1;
            _localAnchor2 = def.LocalAnchor2;
            _localXAxis1 = def.LocalAxis1;
            _localYAxis1 = _localXAxis1.CrossScalarPreMultiply(1.0f);
            _refAngle = def.ReferenceAngle;

            _impulse = Vector3.zero;
            _motorMass = 0.0f;
            _motorImpulse = 0.0f;

            _lowerTranslation = def.LowerTranslation;
            _upperTranslation = def.UpperTranslation;
            _maxMotorForce = Settings.FORCE_INV_SCALE(def.MaxMotorForce);
            _motorSpeed = def.MotorSpeed;
            _enableLimit = def.EnableLimit;
            _enableMotor = def.EnableMotor;
            _limitState = LimitState.InactiveLimit;

            _axis = Vector2.zero;
            _perp= Vector2.zero;
        }