示例#1
0
        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 constraint.
            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 - _localCenterA);
            Vector2 r2 = MathUtils.Multiply(ref R2, _localAnchor2 - _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(_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);

            Vector3 impulse;
            Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - _refAngle);

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

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

                Vector3 C = new Vector3(-C1.X, -C1.Y, -C2);
                impulse = _K.Solve33(C);         // negated above
            }
            else
            {
                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 k22 = i1 + i2;

                _K.col1 = new Vector3(k11, k12, 0.0f);
                _K.col2 = new Vector3(k12, k22, 0.0f);

                Vector2 impulse1 = _K.Solve22(-C1);
                impulse.X = impulse1.X;
                impulse.Y = impulse1.Y;
                impulse.Z = 0.0f;
            }

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

            c1 -= _invMassA * P;
            a1 -= _invIA * L1;
            c2 += _invMassB * P;
            a2 += _invIB * L2;

            // TODO_ERIN remove need for this.
            b1._sweep.c = c1;
            b1._sweep.a = a1;
            b2._sweep.c = c2;
            b2._sweep.a = a2;
            b1.SynchronizeTransform();
            b2.SynchronizeTransform();

            return(linearError <= Settings.b2_linearSlop && angularError <= Settings.b2_angularSlop);
        }
示例#2
0
 public static Vector2 Multiply(ref Mat22 A, Vector2 v)
 {
     return(new Vector2(A.col1.X * v.X + A.col2.X * v.Y, A.col1.Y * v.X + A.col2.Y * v.Y));
 }
示例#3
0
 public static Vector2 MultiplyT(ref Mat22 A, Vector2 v)
 {
     return(new Vector2(Vector2.Dot(v, A.col1), Vector2.Dot(v, A.col2)));
 }
示例#4
0
 /// Initialize using a position vector and a rotation matrix.
 public Transform(Vector2 position, ref Mat22 r)
 {
     Position = position;
     R        = r;
 }
示例#5
0
 public static void Add(ref Mat22 A, ref Mat22 B, out Mat22 R)
 {
     R = new Mat22(A.col1 + B.col1, A.col2 + B.col2);
 }
示例#6
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body bA = _bodyA;
            Body bB = _bodyB;

            Transform xfA, xfB;

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

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

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

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

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

            Mat22 K1 = new Mat22();

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

            Mat22 K2 = new Mat22();

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

            Mat22 K3 = new Mat22();

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

            Mat22 K12;

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

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

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

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

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

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

                bB._linearVelocity  += mB * P;
                bB._angularVelocity += iB * (MathUtils.Cross(rB, P) + _angularImpulse);
            }
            else
            {
                _linearImpulse  = Vector2.Zero;
                _angularImpulse = 0.0f;
            }
        }
示例#7
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b = _bodyB;

            float mass = b.GetMass();

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

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

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

            // magic formulas
            // gamma has units of inverse mass.
            // beta has units of inverse time.
            Debug.Assert(d + step.dt * k > Settings.b2_epsilon);

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

            _beta = step.dt * k * _gamma;

            // Compute the effective mass matrix.
            Transform xf1;

            b.GetTransform(out xf1);
            Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor - b.GetLocalCenter());

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

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

            Mat22 K;

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

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

            _mass = K.GetInverse();

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

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

            // Warm starting.
            _impulse           *= step.dtRatio;
            b._linearVelocity  += invMass * _impulse;
            b._angularVelocity += invI * MathUtils.Cross(r, _impulse);
        }
示例#8
0
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            // TODO_ERIN block solve with limit. COME ON ERIN

            Body b1 = _bodyA;
            Body b2 = _bodyB;

            float angularError  = 0.0f;
            float positionError = 0.0f;

            // 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 = MathUtils.Clamp(angle - _lowerAngle, -Settings.b2_maxAngularCorrection, Settings.b2_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.b2_angularSlop, -Settings.b2_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.b2_angularSlop, 0.0f, Settings.b2_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 xf1.R, _localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

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

                b2._sweep.c += b2._invMass * impulse;
                b2._sweep.a += b2._invI * MathUtils.Cross(r2, impulse);

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

            return(positionError <= Settings.b2_linearSlop && angularError <= Settings.b2_angularSlop);
        }