C# Mat22类代码示例

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