public static void Cross(FP s, ref FPVector2 a, out FPVector2 b)
 {
     b = new FPVector2(-s * a.y, s * a.x);
 }
        internal override bool SolvePositionConstraints(ref SolverData data)
        {
            FPVector2 cA = data.positions[_indexA].c;
            FP        aA = data.positions[_indexA].a;
            FPVector2 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
            FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
            FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
            FPVector2 d  = cB + rB - cA - rA;

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

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

            FPVector  impulse;
            FPVector2 C1 = new FPVector2();

            C1.x = FPVector2.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 = FPVector2.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 = KBEngine.FPMath.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 = KBEngine.FPMath.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 = KBEngine.FPMath.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 FPVector(k11, k12, k13);
                K.ey = new FPVector(k12, k22, k23);
                K.ez = new FPVector(k13, k23, k33);

                FPVector C = new FPVector();
                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 FPVector2(k11, k12);
                K.ey = new FPVector2(k12, k22);

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

            FPVector2 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);
        }
        internal override void InitVelocityConstraints(ref SolverData data)
        {
            _indexA       = BodyA.IslandIndex;
            _localCenterA = BodyA._sweep.LocalCenter;
            _invMassA     = BodyA._invMass;
            _invIA        = BodyA._invI;

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

            Rot qA = new Rot(aA);

            FP mass = BodyA.Mass;

            // Frequency
            FP omega = 2.0f * Settings.Pi * Frequency;

            // Damping coefficient
            FP d = 2.0f * mass * DampingRatio * omega;

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

            // magic formulas
            // gamma has units of inverse mass.
            // beta has units of inverse time.
            FP h = data.step.dt;

            //Debug.Assert(d + h * k > Settings.Epsilon);
            _gamma = h * (d + h * k);
            if (_gamma != 0.0f)
            {
                _gamma = 1.0f / _gamma;
            }

            _beta = h * k * _gamma;

            // Compute the effective mass matrix.
            _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
            // 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]
            Mat22 K = new Mat22();

            K.ex.x = _invMassA + _invIA * _rA.y * _rA.y + _gamma;
            K.ex.y = -_invIA * _rA.x * _rA.y;
            K.ey.x = K.ex.y;
            K.ey.y = _invMassA + _invIA * _rA.x * _rA.x + _gamma;

            _mass = K.Inverse;

            _C  = cA + _rA - _worldAnchor;
            _C *= _beta;

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

            if (Settings.EnableWarmstarting)
            {
                _impulse *= data.step.dtRatio;
                vA       += _invMassA * _impulse;
                wA       += _invIA * MathUtils.Cross(_rA, _impulse);
            }
            else
            {
                _impulse = FPVector2.zero;
            }

            data.velocities[_indexA].v = vA;
            data.velocities[_indexA].w = wA;
        }
Beispiel #4
0
        internal override bool SolvePositionConstraints(ref SolverData data)
        {
            FPVector2 cA = data.positions[_indexA].c;
            FP        aA = data.positions[_indexA].a;
            FPVector2 cB = data.positions[_indexB].c;
            FP        aB = data.positions[_indexB].a;
            FPVector2 cC = data.positions[_indexC].c;
            FP        aC = data.positions[_indexC].a;
            FPVector2 cD = data.positions[_indexD].c;
            FP        aD = data.positions[_indexD].a;

            Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD);

            FP linearError = 0.0f;

            FP coordinateA, coordinateB;

            FPVector2 JvAC, JvBD;
            FP        JwA, JwB, JwC, JwD;
            FP        mass = 0.0f;

            if (_typeA == JointType.Revolute)
            {
                JvAC  = FPVector2.zero;
                JwA   = 1.0f;
                JwC   = 1.0f;
                mass += _iA + _iC;

                coordinateA = aA - aC - _referenceAngleA;
            }
            else
            {
                FPVector2 u  = MathUtils.Mul(qC, _localAxisC);
                FPVector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC);
                FPVector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA);
                JvAC  = u;
                JwC   = MathUtils.Cross(rC, u);
                JwA   = MathUtils.Cross(rA, u);
                mass += _mC + _mA + _iC * JwC * JwC + _iA * JwA * JwA;

                FPVector2 pC = _localAnchorC - _lcC;
                FPVector2 pA = MathUtils.MulT(qC, rA + (cA - cC));
                coordinateA = FPVector2.Dot(pA - pC, _localAxisC);
            }

            if (_typeB == JointType.Revolute)
            {
                JvBD  = FPVector2.zero;
                JwB   = _ratio;
                JwD   = _ratio;
                mass += _ratio * _ratio * (_iB + _iD);

                coordinateB = aB - aD - _referenceAngleB;
            }
            else
            {
                FPVector2 u  = MathUtils.Mul(qD, _localAxisD);
                FPVector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD);
                FPVector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB);
                JvBD  = _ratio * u;
                JwD   = _ratio * MathUtils.Cross(rD, u);
                JwB   = _ratio * MathUtils.Cross(rB, u);
                mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB;

                FPVector2 pD = _localAnchorD - _lcD;
                FPVector2 pB = MathUtils.MulT(qD, rB + (cB - cD));
                coordinateB = FPVector2.Dot(pB - pD, _localAxisD);
            }

            FP C = (coordinateA + _ratio * coordinateB) - _constant;

            FP impulse = 0.0f;

            if (mass > 0.0f)
            {
                impulse = -C / mass;
            }

            cA += _mA * impulse * JvAC;
            aA += _iA * impulse * JwA;
            cB += _mB * impulse * JvBD;
            aB += _iB * impulse * JwB;
            cC -= _mC * impulse * JvAC;
            aC -= _iC * impulse * JwC;
            cD -= _mD * impulse * JvBD;
            aD -= _iD * impulse * JwD;

            data.positions[_indexA].c = cA;
            data.positions[_indexA].a = aA;
            data.positions[_indexB].c = cB;
            data.positions[_indexB].a = aB;
            data.positions[_indexC].c = cC;
            data.positions[_indexC].a = aC;
            data.positions[_indexD].c = cD;
            data.positions[_indexD].a = aD;

            // TODO_ERIN not implemented
            return(linearError < Settings.LinearSlop);
        }
        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;

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

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

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

            // Compute the effective masses.
            FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
            FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
            FPVector2 d  = (cB - cA) + rB - rA;

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

            // Compute motor Jacobian and effective mass.
            {
                _axis = MathUtils.Mul(qA, LocalXAxis);
                _a1   = MathUtils.Cross(d + rA, _axis);
                _a2   = MathUtils.Cross(rB, _axis);

                _motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
                if (_motorMass > 0.0f)
                {
                    _motorMass = 1.0f / _motorMass;
                }
            }

            // Prismatic constraint.
            {
                _perp = MathUtils.Mul(qA, _localYAxisA);

                _s1 = MathUtils.Cross(d + rA, _perp);
                _s2 = MathUtils.Cross(rB, _perp);

                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 bodies with fixed rotation.
                    k22 = 1.0f;
                }
                FP k23 = iA * _a1 + iB * _a2;
                FP k33 = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;

                _K.ex = new FPVector(k11, k12, k13);
                _K.ey = new FPVector(k12, k22, k23);
                _K.ez = new FPVector(k13, k23, k33);
            }

            // Compute motor and limit terms.
            if (_enableLimit)
            {
                FP jointTranslation = FPVector2.Dot(_axis, d);
                if (FP.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;
                _impulse.z  = 0.0f;
            }

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

            if (Settings.EnableWarmstarting)
            {
                // Account for variable time step.
                _impulse     *= data.step.dtRatio;
                MotorImpulse *= data.step.dtRatio;

                FPVector2 P  = _impulse.x * _perp + (MotorImpulse + _impulse.z) * _axis;
                FP        LA = _impulse.x * _s1 + _impulse.y + (MotorImpulse + _impulse.z) * _a1;
                FP        LB = _impulse.x * _s2 + _impulse.y + (MotorImpulse + _impulse.z) * _a2;

                vA -= mA * P;
                wA -= iA * LA;

                vB += mB * P;
                wB += iB * LB;
            }
            else
            {
                _impulse     = FPVector.zero;
                MotorImpulse = 0.0f;
            }

            data.velocities[_indexA].v = vA;
            data.velocities[_indexA].w = wA;
            data.velocities[_indexB].v = vB;
            data.velocities[_indexB].w = wB;
        }
Beispiel #6
0
 public static Vector3 ToVector(this FPVector2 jVector)
 {
     return(new Vector3((float)jVector.x, (float)jVector.y, 0));
 }
Beispiel #7
0
        public override FPVector2 GetReactionForce(FP invDt)
        {
            FPVector2 P = _impulse * _JvAC;

            return(invDt * P);
        }
 public static FPVector2 Cross(FP s, FPVector2 a)
 {
     return(new FPVector2(-s * a.y, s * a.x));
 }
 public static FPVector2 Abs(FPVector2 v)
 {
     return(new FPVector2(FP.Abs(v.x), FP.Abs(v.y)));
 }
 public static FPVector2 Cross(FPVector2 a, FP s)
 {
     return(new FPVector2(s * a.y, -s * a.x));
 }
 /// <summary>
 /// Initialize this matrix using columns.
 /// </summary>
 /// <param name="c1">The c1.</param>
 /// <param name="c2">The c2.</param>
 public void Set(FPVector2 c1, FPVector2 c2)
 {
     ex = c1;
     ey = c2;
 }
 /// <summary>
 /// Construct this matrix using scalars.
 /// </summary>
 /// <param name="a11">The a11.</param>
 /// <param name="a12">The a12.</param>
 /// <param name="a21">The a21.</param>
 /// <param name="a22">The a22.</param>
 public Mat22(FP a11, FP a12, FP a21, FP a22)
 {
     ex = new FPVector2(a11, a21);
     ey = new FPVector2(a12, a22);
 }
 /// <summary>
 /// Construct this matrix using columns.
 /// </summary>
 /// <param name="c1">The c1.</param>
 /// <param name="c2">The c2.</param>
 public Mat22(FPVector2 c1, FPVector2 c2)
 {
     ex = c1;
     ey = c2;
 }
 public static FPVector2 MulT(ref Rot rot, FPVector2 axis)
 {
     return(MulT(rot, axis));
 }
        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;

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

            FP        aB = data.positions[_indexB].a;
            FPVector2 vB = data.velocities[_indexB].v;
            FP        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);

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

            Mat33 K = new Mat33();

            K.ex.x = mA + mB + _rA.y * _rA.y * iA + _rB.y * _rB.y * iB;
            K.ey.x = -_rA.y * _rA.x * iA - _rB.y * _rB.x * iB;
            K.ez.x = -_rA.y * iA - _rB.y * iB;
            K.ex.y = K.ey.x;
            K.ey.y = mA + mB + _rA.x * _rA.x * iA + _rB.x * _rB.x * iB;
            K.ez.y = _rA.x * iA + _rB.x * iB;
            K.ex.z = K.ez.x;
            K.ey.z = K.ez.y;
            K.ez.z = iA + iB;

            if (FrequencyHz > 0.0f)
            {
                K.GetInverse22(ref _mass);

                FP invM = iA + iB;
                FP m    = invM > 0.0f ? 1.0f / invM : 0.0f;

                FP C = aB - aA - ReferenceAngle;

                // Frequency
                FP omega = 2.0f * Settings.Pi * FrequencyHz;

                // Damping coefficient
                FP d = 2.0f * m * DampingRatio * omega;

                // Spring stiffness
                FP k = m * omega * omega;

                // magic formulas
                FP h = data.step.dt;
                _gamma = h * (d + h * k);
                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
                _bias  = C * h * k * _gamma;

                invM      += _gamma;
                _mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;
            }
            else
            {
                K.GetSymInverse33(ref _mass);
                _gamma = 0.0f;
                _bias  = 0.0f;
            }

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

                FPVector2 P = new FPVector2(_impulse.x, _impulse.y);

                vA -= mA * P;
                wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.z);

                vB += mB * P;
                wB += iB * (MathUtils.Cross(_rB, P) + _impulse.z);
            }
            else
            {
                _impulse = FPVector.zero;
            }

            data.velocities[_indexA].v = vA;
            data.velocities[_indexA].w = wA;
            data.velocities[_indexB].v = vB;
            data.velocities[_indexB].w = wB;
        }
 public static FPVector2 Mul(ref Mat22 A, ref FPVector2 v)
 {
     return(new FPVector2(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)
        {
            FPVector2 cA = data.positions[_indexA].c;
            FP        aA = data.positions[_indexA].a;
            FPVector2 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;

            FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
            FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);

            FP positionError, angularError;

            Mat33 K = new Mat33();

            K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
            K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
            K.ez.x = -rA.y * iA - rB.y * iB;
            K.ex.y = K.ey.x;
            K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
            K.ez.y = rA.x * iA + rB.x * iB;
            K.ex.z = K.ez.x;
            K.ey.z = K.ez.y;
            K.ez.z = iA + iB;

            if (FrequencyHz > 0.0f)
            {
                FPVector2 C1 = cB + rB - cA - rA;

                positionError = C1.magnitude;
                angularError  = 0.0f;

                FPVector2 P = -K.Solve22(C1);

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

                cB += mB * P;
                aB += iB * MathUtils.Cross(rB, P);
            }
            else
            {
                FPVector2 C1 = cB + rB - cA - rA;
                FP        C2 = aB - aA - ReferenceAngle;

                positionError = C1.magnitude;
                angularError  = FP.Abs(C2);

                FPVector C = new FPVector(C1.x, C1.y, C2);

                FPVector  impulse = K.Solve33(C) * -1;
                FPVector2 P       = new FPVector2(impulse.x, impulse.y);

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

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

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

            return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
        }
 /// <summary>
 /// Initialize using a position vector and a rotation matrix.
 /// </summary>
 /// <param name="position">The position.</param>
 /// <param name="rotation">The r.</param>
 public Transform(ref FPVector2 position, ref Rot rotation)
 {
     p = position;
     q = rotation;
 }
Beispiel #19
0
        /// <summary>
        /// Requires two existing revolute or prismatic joints (any combination will work).
        /// The provided joints must attach a dynamic body to a static body.
        /// </summary>
        /// <param name="jointA">The first joint.</param>
        /// <param name="jointB">The second joint.</param>
        /// <param name="ratio">The ratio.</param>
        /// <param name="bodyA">The first body</param>
        /// <param name="bodyB">The second body</param>
        // TS - public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, FP ratio = 1f)
        public GearJoint(Body bodyA, Body bodyB, Joint2D jointA, Joint2D jointB, FP ratio)
        {
            JointType = JointType.Gear;
            BodyA     = bodyA;
            BodyB     = bodyB;
            JointA    = jointA;
            JointB    = jointB;
            Ratio     = ratio;

            _typeA = jointA.JointType;
            _typeB = jointB.JointType;

            Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic || _typeA == JointType.FixedRevolute || _typeA == JointType.FixedPrismatic);
            Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic || _typeB == JointType.FixedRevolute || _typeB == JointType.FixedPrismatic);

            FP coordinateA, coordinateB;

            // TODO_ERIN there might be some problem with the joint edges in b2Joint.

            _bodyC = JointA.BodyA;
            _bodyA = JointA.BodyB;

            // Get geometry of joint1
            Transform xfA = _bodyA._xf;
            FP        aA  = _bodyA._sweep.A;
            Transform xfC = _bodyC._xf;
            FP        aC  = _bodyC._sweep.A;

            if (_typeA == JointType.Revolute)
            {
                RevoluteJoint revolute = (RevoluteJoint)jointA;
                _localAnchorC    = revolute.LocalAnchorA;
                _localAnchorA    = revolute.LocalAnchorB;
                _referenceAngleA = revolute.ReferenceAngle;
                _localAxisC      = FPVector2.zero;

                coordinateA = aA - aC - _referenceAngleA;
            }
            else
            {
                PrismaticJoint prismatic = (PrismaticJoint)jointA;
                _localAnchorC    = prismatic.LocalAnchorA;
                _localAnchorA    = prismatic.LocalAnchorB;
                _referenceAngleA = prismatic.ReferenceAngle;
                _localAxisC      = prismatic.LocalXAxis;

                FPVector2 pC = _localAnchorC;
                FPVector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p));
                coordinateA = FPVector2.Dot(pA - pC, _localAxisC);
            }

            _bodyD = JointB.BodyA;
            _bodyB = JointB.BodyB;

            // Get geometry of joint2
            Transform xfB = _bodyB._xf;
            FP        aB  = _bodyB._sweep.A;
            Transform xfD = _bodyD._xf;
            FP        aD  = _bodyD._sweep.A;

            if (_typeB == JointType.Revolute)
            {
                RevoluteJoint revolute = (RevoluteJoint)jointB;
                _localAnchorD    = revolute.LocalAnchorA;
                _localAnchorB    = revolute.LocalAnchorB;
                _referenceAngleB = revolute.ReferenceAngle;
                _localAxisD      = FPVector2.zero;

                coordinateB = aB - aD - _referenceAngleB;
            }
            else
            {
                PrismaticJoint prismatic = (PrismaticJoint)jointB;
                _localAnchorD    = prismatic.LocalAnchorA;
                _localAnchorB    = prismatic.LocalAnchorB;
                _referenceAngleB = prismatic.ReferenceAngle;
                _localAxisD      = prismatic.LocalXAxis;

                FPVector2 pD = _localAnchorD;
                FPVector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p));
                coordinateB = FPVector2.Dot(pB - pD, _localAxisD);
            }

            _ratio    = ratio;
            _constant = coordinateA + _ratio * coordinateB;
            _impulse  = 0.0f;
        }
 /// <summary>
 /// Set this to the identity transform.
 /// </summary>
 public void SetIdentity()
 {
     p = FPVector2.zero;
     q.SetIdentity();
 }
Beispiel #21
0
        internal override void InitVelocityConstraints(ref SolverData data)
        {
            _indexA = _bodyA.IslandIndex;
            _indexB = _bodyB.IslandIndex;
            _indexC = _bodyC.IslandIndex;
            _indexD = _bodyD.IslandIndex;
            _lcA    = _bodyA._sweep.LocalCenter;
            _lcB    = _bodyB._sweep.LocalCenter;
            _lcC    = _bodyC._sweep.LocalCenter;
            _lcD    = _bodyD._sweep.LocalCenter;
            _mA     = _bodyA._invMass;
            _mB     = _bodyB._invMass;
            _mC     = _bodyC._invMass;
            _mD     = _bodyD._invMass;
            _iA     = _bodyA._invI;
            _iB     = _bodyB._invI;
            _iC     = _bodyC._invI;
            _iD     = _bodyD._invI;

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

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

            FP        aC = data.positions[_indexC].a;
            FPVector2 vC = data.velocities[_indexC].v;
            FP        wC = data.velocities[_indexC].w;

            FP        aD = data.positions[_indexD].a;
            FPVector2 vD = data.velocities[_indexD].v;
            FP        wD = data.velocities[_indexD].w;

            Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD);

            _mass = 0.0f;

            if (_typeA == JointType.Revolute)
            {
                _JvAC  = FPVector2.zero;
                _JwA   = 1.0f;
                _JwC   = 1.0f;
                _mass += _iA + _iC;
            }
            else
            {
                FPVector2 u  = MathUtils.Mul(qC, _localAxisC);
                FPVector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC);
                FPVector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA);
                _JvAC  = u;
                _JwC   = MathUtils.Cross(rC, u);
                _JwA   = MathUtils.Cross(rA, u);
                _mass += _mC + _mA + _iC * _JwC * _JwC + _iA * _JwA * _JwA;
            }

            if (_typeB == JointType.Revolute)
            {
                _JvBD  = FPVector2.zero;
                _JwB   = _ratio;
                _JwD   = _ratio;
                _mass += _ratio * _ratio * (_iB + _iD);
            }
            else
            {
                FPVector2 u  = MathUtils.Mul(qD, _localAxisD);
                FPVector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD);
                FPVector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB);
                _JvBD  = _ratio * u;
                _JwD   = _ratio * MathUtils.Cross(rD, u);
                _JwB   = _ratio * MathUtils.Cross(rB, u);
                _mass += _ratio * _ratio * (_mD + _mB) + _iD * _JwD * _JwD + _iB * _JwB * _JwB;
            }

            // Compute effective mass.
            _mass = _mass > 0.0f ? 1.0f / _mass : 0.0f;

            if (Settings.EnableWarmstarting)
            {
                vA += (_mA * _impulse) * _JvAC;
                wA += _iA * _impulse * _JwA;
                vB += (_mB * _impulse) * _JvBD;
                wB += _iB * _impulse * _JwB;
                vC -= (_mC * _impulse) * _JvAC;
                wC -= _iC * _impulse * _JwC;
                vD -= (_mD * _impulse) * _JvBD;
                wD -= _iD * _impulse * _JwD;
            }
            else
            {
                _impulse = 0.0f;
            }

            data.velocities[_indexA].v = vA;
            data.velocities[_indexA].w = wA;
            data.velocities[_indexB].v = vB;
            data.velocities[_indexB].w = wB;
            data.velocities[_indexC].v = vC;
            data.velocities[_indexC].w = wC;
            data.velocities[_indexD].v = vD;
            data.velocities[_indexD].w = wD;
        }
 /// <summary>
 /// Set this based on the position and angle.
 /// </summary>
 /// <param name="position">The position.</param>
 /// <param name="angle">The angle.</param>
 public void Set(FPVector2 position, FP angle)
 {
     p = position;
     q.Set(angle);
 }
 public PrismaticJoint(Body bodyA, Body bodyB, FPVector2 anchor, FPVector2 axis, bool useWorldCoordinates = false)
     : base(bodyA, bodyB)
 {
     Initialize(anchor, anchor, axis, useWorldCoordinates);
 }
 public static FPVector2 MulT(ref Mat22 A, FPVector2 v)
 {
     return(MulT(ref A, ref v));
 }
        internal override void SolveVelocityConstraints(ref SolverData data)
        {
            FPVector2 vA = data.velocities[_indexA].v;
            FP        wA = data.velocities[_indexA].w;
            FPVector2 vB = data.velocities[_indexB].v;
            FP        wB = data.velocities[_indexB].w;

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

            // Solve linear motor constraint.
            if (_enableMotor && _limitState != LimitState.Equal)
            {
                FP Cdot       = FPVector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA;
                FP impulse    = _motorMass * (_motorSpeed - Cdot);
                FP oldImpulse = MotorImpulse;
                FP maxImpulse = data.step.dt * _maxMotorForce;
                MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse      = MotorImpulse - oldImpulse;

                FPVector2 P  = impulse * _axis;
                FP        LA = impulse * _a1;
                FP        LB = impulse * _a2;

                vA -= mA * P;
                wA -= iA * LA;

                vB += mB * P;
                wB += iB * LB;
            }

            FPVector2 Cdot1 = new FPVector2();

            Cdot1.x = FPVector2.Dot(_perp, vB - vA) + _s2 * wB - _s1 * wA;
            Cdot1.y = wB - wA;

            if (_enableLimit && _limitState != LimitState.Inactive)
            {
                // Solve prismatic and limit constraint in block form.
                FP Cdot2;
                Cdot2 = FPVector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA;
                FPVector Cdot = new FPVector(Cdot1.x, Cdot1.y, Cdot2);

                FPVector f1 = _impulse;
                FPVector df = _K.Solve33(Cdot * -1);
                _impulse += df;

                if (_limitState == LimitState.AtLower)
                {
                    _impulse.z = KBEngine.FPMath.Max(_impulse.z, 0.0f);
                }
                else if (_limitState == LimitState.AtUpper)
                {
                    _impulse.z = KBEngine.FPMath.Min(_impulse.z, 0.0f);
                }

                // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
                FPVector2 b   = -Cdot1 - (_impulse.z - f1.z) * new FPVector2(_K.ez.x, _K.ez.y);
                FPVector2 f2r = _K.Solve22(b) + new FPVector2(f1.x, f1.y);
                _impulse.x = f2r.x;
                _impulse.y = f2r.y;

                df = _impulse - f1;

                FPVector2 P  = df.x * _perp + df.z * _axis;
                FP        LA = df.x * _s1 + df.y + df.z * _a1;
                FP        LB = df.x * _s2 + df.y + df.z * _a2;

                vA -= mA * P;
                wA -= iA * LA;

                vB += mB * P;
                wB += iB * LB;
            }
            else
            {
                // Limit is inactive, just solve the prismatic constraint in block form.
                FPVector2 df = _K.Solve22(-Cdot1);
                _impulse.x += df.x;
                _impulse.y += df.y;

                FPVector2 P  = df.x * _perp;
                FP        LA = df.x * _s1 + df.y;
                FP        LB = df.x * _s2 + df.y;

                vA -= mA * P;
                wA -= iA * LA;

                vB += mB * P;
                wB += iB * LB;
            }

            data.velocities[_indexA].v = vA;
            data.velocities[_indexA].w = wA;
            data.velocities[_indexB].v = vB;
            data.velocities[_indexB].w = wB;
        }
 public static FPVector2 MulT(ref Mat22 A, ref FPVector2 v)
 {
     return(new FPVector2(v.x * A.ex.x + v.y * A.ex.y, v.x * A.ey.x + v.y * A.ey.y));
 }
Beispiel #27
0
 /**
  * @brief Instantiates a new prefab in a deterministic way.
  *
  * @param prefab GameObject's prefab to instantiate.
  * @param position Position to place the new GameObject.
  * @param rotation Rotation to set in the new GameObject.
  **/
 public static GameObject SyncedInstantiate(GameObject prefab, FPVector2 position, FPQuaternion rotation)
 {
     return(SyncedInstantiate(prefab, new FPVector(position.x, position.y, 0), rotation));
 }
 public static FPVector2 MulT(ref Transform T, FPVector2 v)
 {
     return(MulT(ref T, ref v));
 }
Beispiel #29
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;

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

            FP        aB = data.positions[_indexB].a;
            FPVector2 vB = data.velocities[_indexB].v;
            FP        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]

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

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

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

            data.velocities[_indexA].v = vA;
            data.velocities[_indexA].w = wA;
            data.velocities[_indexB].v = vB;
            data.velocities[_indexB].w = wB;
        }
 /// <summary>
 /// Determines if three vertices are collinear (ie. on a straight line)
 /// </summary>
 /// <param name="a">First vertex</param>
 /// <param name="b">Second vertex</param>
 /// <param name="c">Third vertex</param>
 /// <param name="tolerance">The tolerance</param>
 /// <returns></returns>
 // FP - public static bool IsCollinear(ref Vector2 a, ref Vector2 b, ref Vector2 c, FP tolerance = 0)
 public static bool IsCollinear(ref FPVector2 a, ref FPVector2 b, ref FPVector2 c, FP tolerance)
 {
     return(FPInRange(Area(ref a, ref b, ref c), -tolerance, tolerance));
 }