public TestbedCamera(Vec2 initPosition, float initScale, float zoomScaleDiff)
 {
     this.transform = new OBBViewportTransform();
     transform.setCamera(initPosition.x, initPosition.y, initScale);
     this.initPosition.set(initPosition);
     this.initScale = initScale;
     upScale = Mat22.createScaleTransform(1 + zoomScaleDiff);
     downScale = Mat22.createScaleTransform(1 - zoomScaleDiff);
 }

        public bool Clip(Vertices clipVertices, Vector2 position)
        {
            Mat22 mat = new Mat22(0);
            Transform t = new Transform(ref position, ref mat);

            //Transform shape
            Transform thistransform;
            Body.GetTransform(out thistransform);

            //Transform the shape
            Vertices transformedshape = new Vertices(clipVertices.Count);
            foreach (Vector2 v in clipVertices)
            {
                Vector2 newv = v;
                newv = MathUtils.Multiply(ref t, ref newv);
                newv = MathUtils.MultiplyT(ref thistransform, ref newv);
                transformedshape.Add(newv);
            }

            PolyClipError error;
            List<Vertices> result = YuPengClipper.Difference(Vertices, transformedshape, out error);

            //Need to check if the entire shape was cut,
            //so we can destroy/erase it
            if (result.Count == 0)
                return false;

            //The shape was split up,
            //so create a new DestructableBody for each piece
            if (result.Count > 1)
            {
                //Create a new destructable body for each extra shape
                for (int i = 1; i < result.Count; i++)
                {
                    DestructableBody db = new DestructableBody(_world, result[i]);
                    db.Body.Position = Body.Position;
                }
            }

            //Set Shape
            Vertices newshape = result[0];
            SetShape(newshape);

            return true;
        }

        internal override bool SolvePositionConstraints()
        {
            // TODO_ERIN block solve with limit. COME ON ERIN

            Body b1 = BodyA;

            float angularError = 0.0f;
            float positionError;

            // Solve angular limit constraint.
            if (_enableLimit && _limitState != LimitState.Inactive)
            {
                float angle = 0 - b1.Sweep.A - ReferenceAngle;
                float limitImpulse = 0.0f;

                if (_limitState == LimitState.Equal)
                {
                    // Prevent large angular corrections
                    float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection,
                                              Settings.MaxAngularCorrection);
                    limitImpulse = -_motorMass * C;
                    angularError = Math.Abs(C);
                }
                else if (_limitState == LimitState.AtLower)
                {
                    float C = angle - _lowerAngle;
                    angularError = -C;

                    // Prevent large angular corrections and allow some slop.
                    C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
                    limitImpulse = -_motorMass * C;
                }
                else if (_limitState == LimitState.AtUpper)
                {
                    float C = angle - _upperAngle;
                    angularError = C;

                    // Prevent large angular corrections and allow some slop.
                    C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
                    limitImpulse = -_motorMass * C;
                }

                b1.Sweep.A -= b1.InvI * limitImpulse;

                b1.SynchronizeTransform();
            }

            // Solve point-to-point constraint.
            {
                Transform xf1;
                b1.GetTransform(out xf1);

                Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
                Vector2 r2 = _worldAnchor;

                Vector2 C = Vector2.zero + r2 - b1.Sweep.C - r1;
                positionError = C.magnitude;

                float invMass1 = b1.InvMass;
                const float invMass2 = 0;
                float invI1 = b1.InvI;
                const float invI2 = 0;

                // Handle large detachment.
                const float k_allowedStretch = 10.0f * Settings.LinearSlop;
                if (C.sqrMagnitude > k_allowedStretch * k_allowedStretch)
                {
                    // Use a particle solution (no rotation).
                    Vector2 u = C;
                    u.Normalize();
                    float k = invMass1 + invMass2;
                    Debug.Assert(k > Settings.Epsilon);
                    float m = 1.0f / k;
                    Vector2 impulse2 = m * (-C);
                    const float k_beta = 0.5f;
                    b1.Sweep.C -= k_beta * invMass1 * impulse2;

                    C = Vector2.zero + r2 - b1.Sweep.C - r1;
                }

                Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
                Mat22 K2 = new Mat22(new Vector2(invI1 * r1.y * r1.y, -invI1 * r1.x * r1.y),
                                     new Vector2(-invI1 * r1.x * r1.y, invI1 * r1.x * r1.x));
                Mat22 K3 = new Mat22(new Vector2(invI2 * r2.y * r2.y, -invI2 * r2.x * r2.y),
                                     new Vector2(-invI2 * r2.x * r2.y, invI2 * r2.x * r2.x));

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

                Mat22 K;
                Mat22.Add(ref Ka, ref K3, out K);

                Vector2 impulse = K.Solve(-C);

                b1.Sweep.C -= b1.InvMass * impulse;
                b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, impulse);

                b1.SynchronizeTransform();
            }

//.........这里部分代码省略.........

        internal override bool SolvePositionConstraints(ref SolverData data)
        {
            FVector2 cA = data.positions[m_indexA].c;
            float aA = data.positions[m_indexA].a;
            FVector2 cB = data.positions[m_indexB].c;
            float aB = data.positions[m_indexB].a;

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

            float angularError = 0.0f;
            float positionError;

            bool fixedRotation = (m_invIA + m_invIB == 0.0f);

            // Solve angular limit constraint.
            if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
            {
                float angle = aB - aA - ReferenceAngle;
                float limitImpulse = 0.0f;

                if (_limitState == LimitState.Equal)
                {
                    // Prevent large angular corrections
                    float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
                    limitImpulse = -m_motorMass * C;
                    angularError = Math.Abs(C);
                }
                else if (_limitState == LimitState.AtLower)
                {
                    float C = angle - _lowerAngle;
                    angularError = -C;

                    // Prevent large angular corrections and allow some slop.
                    C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
                    limitImpulse = -m_motorMass * C;
                }
                else if (_limitState == LimitState.AtUpper)
                {
                    float C = angle - _upperAngle;
                    angularError = C;

                    // Prevent large angular corrections and allow some slop.
                    C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
                    limitImpulse = -m_motorMass * C;
                }

                aA -= m_invIA * limitImpulse;
                aB += m_invIB * limitImpulse;
            }

            // Solve point-to-point constraint.
            {
                qA.Set(aA);
                qB.Set(aB);
                FVector2 rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA);
                FVector2 rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB);

                FVector2 C = cB + rB - cA - rA;
                positionError = C.Length();

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

                Mat22 K = new Mat22();
                K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y;
                K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y;
                K.ey.X = K.ex.Y;
                K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X;

                FVector2 impulse = -K.Solve(C);

                cA -= mA * impulse;
                aA -= iA * MathUtils.Cross(rA, impulse);

                cB += mB * impulse;
                aB += iB * MathUtils.Cross(rB, impulse);
            }

            data.positions[m_indexA].c = cA;
            data.positions[m_indexA].a = aA;
            data.positions[m_indexB].c = cB;
            data.positions[m_indexB].a = aB;

            return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
        }

 /**
    * Multiplies the obb transform by the given transform
    */
 public void mulByTransform(Mat22 transform)
 {
     box.R.mulLocal(transform);
 }

        internal override bool SolvePositionConstraints()
        {
            Body bB = BodyB;

            Vector2 xA = Vector2.Zero;
            const float angleA = 0.0f;

            Vector2 xB = bB.Sweep.C;
            float angleB = bB.Sweep.A;

            Mat22 RA = new Mat22(angleA);
            Mat22 RB = new Mat22(angleB);

            Vector2 rA = MathUtils.Multiply(ref RA, LocalAnchorA - LocalCenterA);
            Vector2 rB = MathUtils.Multiply(ref RB, LocalAnchorB - LocalCenterB);
            Vector2 d = xB + rB - xA - rA;

            Vector2 ay = MathUtils.Multiply(ref RA, _localYAxisA);

            float sBy = MathUtils.Cross(rB, ay);

            float C = Vector2.Dot(d, ay);

            float k = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;

            float impulse;
            if (k != 0.0f)
            {
                impulse = -C / k;
            }
            else
            {
                impulse = 0.0f;
            }

            Vector2 P = impulse * ay;
            float LB = impulse * sBy;

            xB += InvMassB * P;
            angleB += InvIB * LB;

            // TODO_ERIN remove need for this.
            bB.Sweep.C = xB;
            bB.Sweep.A = angleB;
            bB.SynchronizeTransform();

            return Math.Abs(C) <= Settings.LinearSlop;
        }

        internal override bool SolvePositionConstraints()
        {
            Body b2 = BodyB;

            Vector2 c1 = Vector2.Zero;
            float a1 = 0.0f;

            Vector2 c2 = b2.Sweep.c;
            float a2 = b2.Sweep.a;

            // Solve linear limit constraint.
            float linearError = 0.0f;
            bool active = false;
            float C2 = 0.0f;

            Mat22 R1 = new Mat22(a1);
            Mat22 R2 = new Mat22(a2);

            Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA);
            Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB);
            Vector2 d = c2 + r2 - c1 - r1;

            if (_enableLimit)
            {
                _axis = MathUtils.Multiply(ref R1, _localXAxis1);

                _a1 = MathUtils.Cross(d + r1, _axis);
                _a2 = MathUtils.Cross(r2, _axis);

                float translation = Vector2.Dot(_axis, d);
                if (Math.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.LinearSlop)
                {
                    // Prevent large angular corrections
                    C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
                    linearError = Math.Abs(translation);
                    active = true;
                }
                else if (translation <= LowerLimit)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = MathUtils.Clamp(translation - LowerLimit + Settings.LinearSlop, -Settings.MaxLinearCorrection,
                                         0.0f);
                    linearError = LowerLimit - translation;
                    active = true;
                }
                else if (translation >= UpperLimit)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = MathUtils.Clamp(translation - UpperLimit - Settings.LinearSlop, 0.0f,
                                         Settings.MaxLinearCorrection);
                    linearError = translation - UpperLimit;
                    active = true;
                }
            }

            _perp = MathUtils.Multiply(ref R1, _localYAxis1);

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

            Vector2 impulse;
            float C1 = Vector2.Dot(_perp, d);

            linearError = Math.Max(linearError, Math.Abs(C1));
            const float angularError = 0.0f;

            if (active)
            {
                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);

                Vector2 C = new Vector2(-C1, -C2);

                impulse = _K.Solve(C); // note i inverted above
            }
            else
            {
                float m1 = InvMassA, m2 = InvMassB;
                float i1 = InvIA, i2 = InvIB;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;

                float impulse1;
                if (k11 != 0.0f)
                {
                    impulse1 = -C1 / k11;
                }
                else
                {
                    impulse1 = 0.0f;
                }

                impulse.X = impulse1;
//.........这里部分代码省略.........

        internal override bool SolvePositionConstraints()
        {
            // TODO_ERIN block solve with limit. COME ON ERIN

            Body b1 = BodyA;
            Body b2 = BodyB;

            float angularError = 0.0f;
            float positionError;

            // Solve angular limit constraint.
            if (_enableLimit && _limitState != LimitState.Inactive)
            {
                float angle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle;
                float limitImpulse = 0.0f;

                if (_limitState == LimitState.Equal)
                {
                    // Prevent large angular corrections
                    float C = MathHelper.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection,
                                              Settings.MaxAngularCorrection);
                    limitImpulse = -_motorMass * C;
                    angularError = Math.Abs(C);
                }
                else if (_limitState == LimitState.AtLower)
                {
                    float C = angle - _lowerAngle;
                    angularError = -C;

                    // Prevent large angular corrections and allow some slop.
                    C = MathHelper.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
                    limitImpulse = -_motorMass * C;
                }
                else if (_limitState == LimitState.AtUpper)
                {
                    float C = angle - _upperAngle;
                    angularError = C;

                    // Prevent large angular corrections and allow some slop.
                    C = MathHelper.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
                    limitImpulse = -_motorMass * C;
                }

                b1.Sweep.A -= b1.InvI * limitImpulse;
                b2.Sweep.A += b2.InvI * limitImpulse;

                b1.SynchronizeTransform();
                b2.SynchronizeTransform();
            }

            // Solve point-to-point constraint.
            {
                /*Transform xf1, xf2;
                b1.GetTransform(out xf1);
                b2.GetTransform(out xf2);*/

                Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
                Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);

                Vector2 C = b2.Sweep.C + r2 - b1.Sweep.C - r1;
                positionError = C.Length();

                float invMass1 = b1.InvMass, invMass2 = b2.InvMass;
                float invI1 = b1.InvI, invI2 = b2.InvI;

                // Handle large detachment.
                const float k_allowedStretch = 10.0f * Settings.LinearSlop;
                if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
                {
                    // Use a particle solution (no rotation).
                    Vector2 u = C;
                    u.Normalize();
                    float k = invMass1 + invMass2;
                    Debug.Assert(k > Settings.Epsilon);
                    float m = 1.0f / k;
                    Vector2 impulse2 = m * (-C);
                    const float k_beta = 0.5f;
                    b1.Sweep.C -= k_beta * invMass1 * impulse2;
                    b2.Sweep.C += k_beta * invMass2 * impulse2;

                    C = b2.Sweep.C + r2 - b1.Sweep.C - r1;
                }

                Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
                Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y),
                                     new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X));
                Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y),
                                     new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X));

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

                Mat22 K;
                Mat22.Add(ref Ka, ref K3, out K);


                Vector2 impulse = K.Solve(-C);

                b1.Sweep.C -= b1.InvMass * impulse;
                MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1);
//.........这里部分代码省略.........

 /// <summary>
 /// Multiplies the obb transform by the given transform
 /// </summary>
 /// <param name="argTransform"></param>
 public virtual void mulByTransform(Mat22 argTransform)
 {
     box.R.mulLocal(argTransform);
 }

 /// <summary>
 /// Compute the inverse of this matrix, such that inv(A) * A = identity.
 /// </summary>
 public Mat22 Invert()
 {
     float a = Col1.X, b = Col2.X, c = Col1.Y, d = Col2.Y;
     Mat22 B = new Mat22();
     float det = a * d - b * c;
     if (det != 0.0f)
     {
         det = 1.0f / det;
     }
     B.Col1.X = det * d; B.Col2.X = -det * b;
     B.Col1.Y = -det * c; B.Col2.Y = det * a;
     return B;
 }

        internal override void InitVelocityConstraints(ref SolverData data)
        {
            _indexB = BodyA.IslandIndex;
            _localCenterB = BodyA.Sweep.LocalCenter;
            _invMassB = BodyA.InvMass;
            _invIB = 0;

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

            Rot qB = new Rot(aB);

            float mass = BodyA.Mass;

            // Frequency
            float omega = 2.0f * FSSettings.Pi * Frequency;

            // 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.
            float h = data.step.dt;
            Debug.Assert(d + h * k > FSSettings.Epsilon);
            _gamma = h * (d + h * k);
            if (!Mathf.Approximately(_gamma, 0.0f))
                _gamma = 1.0f / _gamma;

            _beta = h * k * _gamma;

            // Compute the effective mass matrix.
            _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
            // 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]
            var K = new Mat22();
            K.ex.X = _invMassB + _invIB * _rB.Y * _rB.Y + _gamma;
            K.ex.Y = -_invIB * _rB.X * _rB.Y;
            K.ey.X = K.ex.Y;
            K.ey.Y = _invMassB + _invIB * _rB.X * _rB.X + _gamma;

            _mass = K.Inverse;

            _C = cB + _rB - _targetA;
            _C *= _beta;

            // Cheat with some damping
            wB *= 0.98f;

            // 			if (Settings.EnableWarmstarting)
            // 			{
                _impulse *= data.step.dtRatio;
                vB += _invMassB * _impulse;
                wB += _invIB * MathUtils.Cross(_rB, _impulse);
            // 			}
            // 			else
            // 				_impulse = FVector2.Zero;

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

 /// <summary>
 /// Initialize using a position vector and a rotation matrix.
 /// </summary>
 /// <param name="position"></param>
 /// <param name="rotation"></param>
 public Transform(Vec2 position, Mat22 rotation)
 {
     Position = position;
     R = rotation;
 }

        private static Transform Convert(Matrix4x4 mat)
        {
            Vector3 T;
            Matrix3x3 R;
            Vector3 S;

            mat.Decompress (out T, out R, out S);
            var posA = new XnaVector2 (T.X, T.Y);
            var rotA = new Mat22 (R[0], R[1], R[3], R[4]);

            return new Transform (ref posA, ref rotA);
        }

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

            float mass = b.GetMass();

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

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

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

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

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

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

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

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

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

            _mass = K.GetInverse();

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

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

            // Warm starting.
            _impulse *= step.dtRatio;
            b._linearVelocity += invMass * _impulse;
            b._angularVelocity += invI * MathUtils.Cross(r, _impulse);
        }

 /**
    * Sets the transform of the viewport. Transforms about the center.
    */
 public void setTransform(Mat22 transform)
 {
     box.R.set(transform);
 }

        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;

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

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

            // Compute the effective mass matrix.
            _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
            _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);

            // 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 = _invMassA, mB = _invMassB;
            float iA = _invIA, iB = _invIB;

            Mat22 K = new Mat22();
            K.ex.X = mA + mB + iA * _rA.Y * _rA.Y + iB * _rB.Y * _rB.Y;
            K.ex.Y = -iA * _rA.X * _rA.Y - iB * _rB.X * _rB.Y;
            K.ey.X = K.ex.Y;
            K.ey.Y = mA + mB + iA * _rA.X * _rA.X + iB * _rB.X * _rB.X;

            _linearMass = K.Inverse;

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

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

                Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
                vA -= mA * P;
                wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse);
                vB += mB * P;
                wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse);
            }
            else
            {
                _linearImpulse = Vector2.Zero;
                _angularImpulse = 0.0f;
            }

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

        internal override bool SolvePositionConstraints(ref SolverData data)
        {
            Vector2 cA = data.positions[_indexA].c;
            float aA = data.positions[_indexA].a;
            Vector2 cB = data.positions[_indexB].c;
            float aB = data.positions[_indexB].a;

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

            float mA = _invMassA, mB = _invMassB;
            float iA = _invIA, iB = _invIB;

            // Compute fresh Jacobians
            Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
            Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
            Vector2 d = cB + rB - cA - rA;

            Vector2 axis = MathUtils.Mul(qA, LocalXAxis);
            float a1 = MathUtils.Cross(d + rA, axis);
            float a2 = MathUtils.Cross(rB, axis);
            Vector2 perp = MathUtils.Mul(qA, _localYAxisA);

            float s1 = MathUtils.Cross(d + rA, perp);
            float s2 = MathUtils.Cross(rB, perp);

            Vector3 impulse;
            Vector2 C1 = new Vector2();
            C1.X = Vector2.Dot(perp, d);
            C1.Y = aB - aA - ReferenceAngle;

            float linearError = Math.Abs(C1.X);
            float angularError = Math.Abs(C1.Y);

            bool active = false;
            float C2 = 0.0f;
            if (_enableLimit)
            {
                float translation = Vector2.Dot(axis, d);
                if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    // Prevent large angular corrections
                    C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
                    linearError = Math.Max(linearError, Math.Abs(translation));
                    active = true;
                }
                else if (translation <= _lowerTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                    linearError = Math.Max(linearError, _lowerTranslation - translation);
                    active = true;
                }
                else if (translation >= _upperTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
                    linearError = Math.Max(linearError, translation - _upperTranslation);
                    active = true;
                }
            }

            if (active)
            {
                float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
                float k12 = iA * s1 + iB * s2;
                float k13 = iA * s1 * a1 + iB * s2 * a2;
                float k22 = iA + iB;
                if (k22 == 0.0f)
                {
                    // For fixed rotation
                    k22 = 1.0f;
                }
                float k23 = iA * a1 + iB * a2;
                float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;

                Mat33 K = new Mat33();
                K.ex = new Vector3(k11, k12, k13);
                K.ey = new Vector3(k12, k22, k23);
                K.ez = new Vector3(k13, k23, k33);

                Vector3 C = new Vector3();
                C.X = C1.X;
                C.Y = C1.Y;
                C.Z = C2;

                impulse = K.Solve33(-C);
            }
            else
            {
                float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
                float k12 = iA * s1 + iB * s2;
                float k22 = iA + iB;
                if (k22 == 0.0f)
                {
                    k22 = 1.0f;
                }

                Mat22 K = new Mat22();
                K.ex = new Vector2(k11, k12);
                K.ey = new Vector2(k12, k22);
//.........这里部分代码省略.........

        internal override bool SolvePositionConstraints(float baumgarte)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Vector2 c1 = b1._sweep.c;
            float a1 = b1._sweep.a;

            Vector2 c2 = b2._sweep.c;
            float a2 = b2._sweep.a;

            // Solve linear limit raint.
            float linearError = 0.0f, angularError = 0.0f;
            bool active = false;
            float C2 = 0.0f;

            Mat22 R1 = new Mat22(a1);
            Mat22 R2 = new Mat22(a2);

            Vector2 r1 = MathUtils.Multiply(ref R1, _localAnchor1 - _localCenter1);
            Vector2 r2 = MathUtils.Multiply(ref R2, _localAnchor2 - _localCenter2);
            Vector2 d = c2 + r2 - c1 - r1;

            if (_enableLimit)
            {
                _axis = MathUtils.Multiply(ref R1, _localXAxis1);

                _a1 = MathUtils.Cross(d + r1, _axis);
                _a2 = MathUtils.Cross(r2, _axis);

                float translation = Vector2.Dot(_axis, d);
                if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop)
                {
                    // Prevent large angular corrections
                    C2 = MathUtils.Clamp(translation, -Settings.b2_maxLinearCorrection, Settings.b2_maxLinearCorrection);
                    linearError = Math.Abs(translation);
                    active = true;
                }
                else if (translation <= _lowerTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
                    linearError = _lowerTranslation - translation;
                    active = true;
                }
                else if (translation >= _upperTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.b2_linearSlop, 0.0f, Settings.b2_maxLinearCorrection);
                    linearError = translation - _upperTranslation;
                    active = true;
                }
            }

            _perp = MathUtils.Multiply(ref R1, _localYAxis1);

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

            Vector2 impulse;
            float C1;
            C1 = Vector2.Dot(_perp, d);

            linearError = Math.Max(linearError, Math.Abs(C1));
            angularError = 0.0f;

            if (active)
            {
                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);

                Vector2 C = new Vector2(-C1, -C2);

                impulse = _K.Solve(C); //note i inverted above
            }
            else
            {
                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;

                float impulse1 = (-C1) / k11;
                impulse.X = impulse1;
                impulse.Y = 0.0f;
            }

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

            c1 -= _invMass1 * P;
            a1 -= _invI1 * L1;
//.........这里部分代码省略.........

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

            Transform xfA;
            bA.GetTransform(out xfA);

            // Compute the effective mass Matrix4x4.
            Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);

            // 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;
            float iA = bA.InvI;

            Mat22 K1 = new Mat22();
            K1.Col1.x = mA;
            K1.Col2.x = 0.0f;
            K1.Col1.y = 0.0f;
            K1.Col2.y = mA;

            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 K12;
            Mat22.Add(ref K1, ref K2, out K12);

            _linearMass = K12.Inverse;

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

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

                Vector2 P = new Vector2(_linearImpulse.x, _linearImpulse.y);

                bA.LinearVelocityInternal -= mA * P;
                bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
            }
            else
            {
                _linearImpulse = Vector2.zero;
                _angularImpulse = 0.0f;
            }
        }