Beispiel #1
0
 public static void Add(ref Mat22 A, ref Mat22 B, out Mat22 R)
 {
     R.ex = A.ex + B.ex;
     R.ey = A.ey + B.ey;
 }
Beispiel #2
0
 public static TSVector2 MulT(ref Mat22 A, TSVector2 v)
 {
     return(MathUtils.MulT(ref A, ref v));
 }
Beispiel #3
0
 public static TSVector2 MulT(ref Mat22 A, ref TSVector2 v)
 {
     return(new TSVector2(v.x * A.ex.x + v.y * A.ex.y, v.x * A.ey.x + v.y * A.ey.y));
 }
Beispiel #4
0
 public static TSVector2 Mul(ref Mat22 A, ref TSVector2 v)
 {
     return(new TSVector2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y));
 }
        internal override bool SolvePositionConstraints(ref SolverData data)
        {
            TSVector2 cA = data.positions[_indexA].c;
            FP        aA = data.positions[_indexA].a;
            TSVector2 cB = data.positions[_indexB].c;
            FP        aB = data.positions[_indexB].a;

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

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

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

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

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

            TSVector  impulse;
            TSVector2 C1 = new TSVector2();

            C1.x = TSVector2.Dot(perp, d);
            C1.y = aB - aA - ReferenceAngle;

            FP linearError  = FP.Abs(C1.x);
            FP angularError = FP.Abs(C1.y);

            bool active = false;
            FP   C2     = 0.0f;

            if (_enableLimit)
            {
                FP translation = TSVector2.Dot(axis, d);
                if (FP.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    // Prevent large angular corrections
                    C2          = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
                    linearError = TrueSync.TSMath.Max(linearError, FP.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 = TrueSync.TSMath.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 = TrueSync.TSMath.Max(linearError, translation - _upperTranslation);
                    active      = true;
                }
            }

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

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

                TSVector C = new TSVector();
                C.x = C1.x;
                C.y = C1.y;
                C.z = C2;

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

                Mat22 K = new Mat22();
                K.ex = new TSVector2(k11, k12);
                K.ey = new TSVector2(k12, k22);

                TSVector2 impulse1 = K.Solve(-C1);
                impulse   = new TSVector();
                impulse.x = impulse1.x;
                impulse.y = impulse1.y;
                impulse.z = 0.0f;
            }

            TSVector2 P  = impulse.x * perp + impulse.z * axis;
            FP        LA = impulse.x * s1 + impulse.y + impulse.z * a1;
            FP        LB = impulse.x * s2 + impulse.y + impulse.z * a2;

            cA -= mA * P;
            aA -= iA * LA;
            cB += mB * P;
            aB += iB * LB;

            data.positions[_indexA].c = cA;
            data.positions[_indexA].a = aA;
            data.positions[_indexB].c = cB;
            data.positions[_indexB].a = aB;

            return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
        }
Beispiel #6
0
        internal override bool SolvePositionConstraints(ref SolverData data)
        {
            TSVector2 tSVector  = data.positions[this._indexA].c;
            FP        fP        = data.positions[this._indexA].a;
            TSVector2 tSVector2 = data.positions[this._indexB].c;
            FP        fP2       = data.positions[this._indexB].a;
            Rot       q         = new Rot(fP);
            Rot       q2        = new Rot(fP2);
            FP        invMassA  = this._invMassA;
            FP        invMassB  = this._invMassB;
            FP        invIA     = this._invIA;
            FP        invIB     = this._invIB;
            TSVector2 value     = MathUtils.Mul(q, this.LocalAnchorA - this._localCenterA);
            TSVector2 tSVector3 = MathUtils.Mul(q2, this.LocalAnchorB - this._localCenterB);
            TSVector2 tSVector4 = tSVector2 + tSVector3 - tSVector - value;
            TSVector2 tSVector5 = MathUtils.Mul(q, this.LocalXAxis);
            FP        y         = MathUtils.Cross(tSVector4 + value, tSVector5);
            FP        y2        = MathUtils.Cross(tSVector3, tSVector5);
            TSVector2 tSVector6 = MathUtils.Mul(q, this._localYAxisA);
            FP        y3        = MathUtils.Cross(tSVector4 + value, tSVector6);
            FP        y4        = MathUtils.Cross(tSVector3, tSVector6);
            TSVector2 tSVector7 = default(TSVector2);

            tSVector7.x = TSVector2.Dot(tSVector6, tSVector4);
            tSVector7.y = fP2 - fP - this.ReferenceAngle;
            FP   fP3         = FP.Abs(tSVector7.x);
            FP   x           = FP.Abs(tSVector7.y);
            bool flag        = false;
            FP   z           = 0f;
            bool enableLimit = this._enableLimit;

            if (enableLimit)
            {
                FP   fP4   = TSVector2.Dot(tSVector5, tSVector4);
                bool flag2 = FP.Abs(this._upperTranslation - this._lowerTranslation) < 2f * Settings.LinearSlop;
                if (flag2)
                {
                    z    = MathUtils.Clamp(fP4, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
                    fP3  = TSMath.Max(fP3, FP.Abs(fP4));
                    flag = true;
                }
                else
                {
                    bool flag3 = fP4 <= this._lowerTranslation;
                    if (flag3)
                    {
                        z    = MathUtils.Clamp(fP4 - this._lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
                        fP3  = TSMath.Max(fP3, this._lowerTranslation - fP4);
                        flag = true;
                    }
                    else
                    {
                        bool flag4 = fP4 >= this._upperTranslation;
                        if (flag4)
                        {
                            z    = MathUtils.Clamp(fP4 - this._upperTranslation - Settings.LinearSlop, 0f, Settings.MaxLinearCorrection);
                            fP3  = TSMath.Max(fP3, fP4 - this._upperTranslation);
                            flag = true;
                        }
                    }
                }
            }
            bool    flag5 = flag;
            Vector3 vector;

            if (flag5)
            {
                FP   x2    = invMassA + invMassB + invIA * y3 * y3 + invIB * y4 * y4;
                FP   fP5   = invIA * y3 + invIB * y4;
                FP   fP6   = invIA * y3 * y + invIB * y4 * y2;
                FP   fP7   = invIA + invIB;
                bool flag6 = fP7 == 0f;
                if (flag6)
                {
                    fP7 = 1f;
                }
                FP    fP8 = invIA * y + invIB * y2;
                FP    z2  = invMassA + invMassB + invIA * y * y + invIB * y2 * y2;
                Mat33 mat = default(Mat33);
                mat.ex = new Vector3(x2, fP5, fP6);
                mat.ey = new Vector3(fP5, fP7, fP8);
                mat.ez = new Vector3(fP6, fP8, z2);
                vector = mat.Solve33(-new Vector3
                {
                    X = tSVector7.x,
                    Y = tSVector7.y,
                    Z = z
                });
            }
            else
            {
                FP   x3    = invMassA + invMassB + invIA * y3 * y3 + invIB * y4 * y4;
                FP   fP9   = invIA * y3 + invIB * y4;
                FP   fP10  = invIA + invIB;
                bool flag7 = fP10 == 0f;
                if (flag7)
                {
                    fP10 = 1f;
                }
                Mat22 mat2 = default(Mat22);
                mat2.ex = new TSVector2(x3, fP9);
                mat2.ey = new TSVector2(fP9, fP10);
                TSVector2 tSVector8 = mat2.Solve(-tSVector7);
                vector   = default(Vector3);
                vector.X = tSVector8.x;
                vector.Y = tSVector8.y;
                vector.Z = 0f;
            }
            TSVector2 value2 = vector.X * tSVector6 + vector.Z * tSVector5;
            FP        y5     = vector.X * y3 + vector.Y + vector.Z * y;
            FP        y6     = vector.X * y4 + vector.Y + vector.Z * y2;

            tSVector  -= invMassA * value2;
            fP        -= invIA * y5;
            tSVector2 += invMassB * value2;
            fP2       += invIB * y6;
            data.positions[this._indexA].c = tSVector;
            data.positions[this._indexA].a = fP;
            data.positions[this._indexB].c = tSVector2;
            data.positions[this._indexB].a = fP2;
            return(fP3 <= Settings.LinearSlop && x <= Settings.AngularSlop);
        }
 public static TSVector2 Mul(ref Mat22 A, TSVector2 v)
 {
     return(Mul(ref A, ref v));
 }
Beispiel #8
0
        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;

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

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

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

            // Compute the effective mass matrix.
            _rA = MathUtils.Mul(qA, -_localCenterA);
            _rB = MathUtils.Mul(qB, -_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]

            FP mA = _invMassA, mB = _invMassB;
            FP 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;
            }

            _linearError  = cB + _rB - cA - _rA - MathUtils.Mul(qA, _linearOffset);
            _angularError = aB - aA - _angularOffset;

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

                TSVector2 P = new TSVector2(_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  = TSVector2.zero;
                _angularImpulse = 0.0f;
            }

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