Esempio n. 1
0
        /// <summary>
        ///     Describes whether this instance solve position constraints
        /// </summary>
        /// <param name="baumgarte">The baumgarte</param>
        /// <returns>The bool</returns>
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            Body body1 = Body1;
            Body body2 = Body2;

            Vec2  body1SweepC = body1.Sweep.C;
            float body1SweepA = body1.Sweep.A;

            Vec2  body2SweepC = body2.Sweep.C;
            float body2SweepA = body2.Sweep.A;

            // Solve linear limit constraint.
            var   linearError = 0.0f;
            float angularError;
            bool  active = false;
            float c2     = 0.0f;

            var mat22R1 = new Mat22(body1SweepA);
            var mat22R2 = new Mat22(body2SweepA);

            Vec2 r1       = Box2DXMath.Mul(mat22R1, LocalAnchor1 - LocalCenter1);
            Vec2 r2       = Box2DXMath.Mul(mat22R2, LocalAnchor2 - LocalCenter2);
            Vec2 distance = body2SweepC + r2 - body1SweepC - r1;

            if (IsLimitEnabled)
            {
                Axis = Box2DXMath.Mul(mat22R1, LocalXAxis1);

                a1 = Vec2.Cross(distance + r1, Axis);
                A2 = Vec2.Cross(r2, Axis);

                float translation = Vec2.Dot(Axis, distance);
                if (Box2DXMath.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.LinearSlop)
                {
                    // Prevent large angular corrections
                    c2          = Box2DXMath.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
                    linearError = Box2DXMath.Abs(translation);
                    active      = true;
                }
                else if (translation <= LowerLimit)
                {
                    // Prevent large linear corrections and allow some slop.
                    c2 = Box2DXMath.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 = Box2DXMath.Clamp(translation - UpperLimit - Settings.LinearSlop, 0.0f,
                                          Settings.MaxLinearCorrection);
                    linearError = translation - UpperLimit;
                    active      = true;
                }
            }

            Perp = Box2DXMath.Mul(mat22R1, LocalYAxis1);

            s1 = Vec2.Cross(distance + r1, Perp);
            s2 = Vec2.Cross(r2, Perp);

            Vec3 impulse;
            Vec2 c1 = new Vec2();

            c1.X = Vec2.Dot(Perp, distance);
            c1.Y = body2SweepA - body1SweepA - refAngle;

            linearError  = Box2DXMath.Max(linearError, Box2DXMath.Abs(c1.X));
            angularError = Box2DXMath.Abs(c1.Y);

            if (active)
            {
                float m1 = InvMass1, m2 = InvMass2;
                float i1 = InvI1, i2 = InvI2;

                float k11 = m1 + m2 + i1 * s1 * s1 + i2 * s2 * s2;
                float k12 = i1 * s1 + 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.Set(k11, k12, k13);
                K.Col2.Set(k12, k22, k23);
                K.Col3.Set(k13, k23, k33);

                Vec3 c = new Vec3();
                c.X = c1.X;
                c.Y = c1.Y;
                c.Z = c2;

                impulse = K.Solve33(-c);
            }
            else
            {
                float m1 = InvMass1, m2 = InvMass2;
                float i1 = InvI1, i2 = InvI2;

                float k11 = m1 + m2 + i1 * s1 * s1 + i2 * s2 * s2;
                float k12 = i1 * s1 + i2 * s2;
                float k22 = i1 + i2;

                K.Col1.Set(k11, k12, 0.0f);
                K.Col2.Set(k12, k22, 0.0f);

                Vec2 impulse1 = K.Solve22(-c1);
                impulse.X = impulse1.X;
                impulse.Y = impulse1.Y;
                impulse.Z = 0.0f;
            }

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

            body1SweepC -= InvMass1 * p;
            body1SweepA -= InvI1 * l1;
            body2SweepC += InvMass2 * p;
            body2SweepA += InvI2 * l2;

            // TODO_ERIN remove need for this.
            body1.Sweep.C = body1SweepC;
            body1.Sweep.A = body1SweepA;
            body2.Sweep.C = body2SweepC;
            body2.Sweep.A = body2SweepA;
            body1.SynchronizeTransform();
            body2.SynchronizeTransform();

            return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
        }
Esempio n. 2
0
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            // TODO_ERIN block solve with limit.

            Body b1 = _body1;
            Body b2 = _body2;

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

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

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

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

                    // Prevent large angular corrections and allow some slop.
                    C            = Box2DXMath.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.
            {
                Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
                Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

                Vec2 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.
                float k_allowedStretch = 10.0f * Settings.LinearSlop;
                if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
                {
                    // Use a particle solution (no rotation).
                    Vec2  u = C; u.Normalize();
                    float k = invMass1 + invMass2;
                    Box2DNetDebug.Assert(k > Settings.FLT_EPSILON);
                    float m       = 1.0f / k;
                    Vec2  impulse = m * (-C);
                    float k_beta  = 0.5f;
                    b1._sweep.C -= k_beta * invMass1 * impulse;
                    b2._sweep.C += k_beta * invMass2 * impulse;

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

                Mat22 K1 = new Mat22();
                K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
                K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;

                Mat22 K2 = new Mat22();
                K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
                K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;

                Mat22 K3 = new Mat22();
                K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
                K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;

                Mat22 K        = K1 + K2 + K3;
                Vec2  impulse_ = K.Solve(-C);

                b1._sweep.C -= b1._invMass * impulse_;
                b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse_);

                b2._sweep.C += b2._invMass * impulse_;
                b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse_);

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

            return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
        }
Esempio n. 3
0
        /// <summary>
        ///     Inits the velocity constraints using the specified step
        /// </summary>
        /// <param name="step">The step</param>
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = Body1;
            Body b2 = Body2;

            // You cannot create a prismatic joint between bodies that
            // both have fixed rotation.
            Box2DxDebug.Assert(b1.InvI > 0.0f || b2.InvI > 0.0f);

            LocalCenter1 = b1.GetLocalCenter();
            LocalCenter2 = b2.GetLocalCenter();

            XForm xf1 = b1.GetXForm();
            XForm xf2 = b2.GetXForm();

            // Compute the effective masses.
            Vec2 r1 = Box2DXMath.Mul(xf1.R, LocalAnchor1 - LocalCenter1);
            Vec2 r2 = Box2DXMath.Mul(xf2.R, LocalAnchor2 - LocalCenter2);
            Vec2 d  = b2.Sweep.C + r2 - b1.Sweep.C - r1;

            InvMass1 = b1.InvMass;
            InvI1    = b1.InvI;
            InvMass2 = b2.InvMass;
            InvI2    = b2.InvI;

            // Compute motor Jacobian and effective mass.
            {
                Axis = Box2DXMath.Mul(xf1.R, LocalXAxis1);
                a1   = Vec2.Cross(d + r1, Axis);
                A2   = Vec2.Cross(r2, Axis);

                MotorMass = InvMass1 + InvMass2 + InvI1 * a1 * a1 + InvI2 * A2 * A2;
                Box2DxDebug.Assert(MotorMass > Settings.FltEpsilon);
                MotorMass = 1.0f / MotorMass;
            }

            // Prismatic constraint.
            {
                Perp = Box2DXMath.Mul(xf1.R, LocalYAxis1);

                s1 = Vec2.Cross(d + r1, Perp);
                s2 = Vec2.Cross(r2, Perp);

                float m1 = InvMass1, m2 = InvMass2;
                float i1 = InvI1, i2 = InvI2;

                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.Set(k11, k12, k13);
                K.Col2.Set(k12, k22, k23);
                K.Col3.Set(k13, k23, k33);
            }

            // Compute motor and limit terms.
            if (IsLimitEnabled)
            {
                float jointTranslation = Vec2.Dot(Axis, d);
                if (Box2DXMath.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.LinearSlop)
                {
                    LimitState = LimitState.EqualLimits;
                }
                else if (jointTranslation <= LowerLimit)
                {
                    if (LimitState != LimitState.AtLowerLimit)
                    {
                        LimitState = LimitState.AtLowerLimit;
                        Impulse.Z  = 0.0f;
                    }
                }
                else if (jointTranslation >= UpperLimit)
                {
                    if (LimitState != LimitState.AtUpperLimit)
                    {
                        LimitState = LimitState.AtUpperLimit;
                        Impulse.Z  = 0.0f;
                    }
                }
                else
                {
                    LimitState = LimitState.InactiveLimit;
                    Impulse.Z  = 0.0f;
                }
            }
            else
            {
                LimitState = LimitState.InactiveLimit;
            }

            if (IsMotorEnabled == false)
            {
                MotorForce = 0.0f;
            }

            if (step.WarmStarting)
            {
                // Account for variable time step.
                Impulse    *= step.DtRatio;
                MotorForce *= step.DtRatio;

                Vec2  p  = Impulse.X * Perp + (MotorForce + Impulse.Z) * Axis;
                float l1 = Impulse.X * s1 + Impulse.Y + (MotorForce + Impulse.Z) * a1;
                float l2 = Impulse.X * s2 + Impulse.Y + (MotorForce + Impulse.Z) * A2;

                b1.LinearVelocity  -= InvMass1 * p;
                b1.AngularVelocity -= InvI1 * l1;

                b2.LinearVelocity  += InvMass2 * p;
                b2.AngularVelocity += InvI2 * l2;
            }
            else
            {
                Impulse.SetZero();
                MotorForce = 0.0f;
            }
        }
Esempio n. 4
0
        internal override bool SolvePositionConstraints()
        {
            Body b1 = _body1;
            Body b2 = _body2;

            float invMass1 = b1._invMass, invMass2 = b2._invMass;
            float invI1 = b1._invI, invI2 = b2._invI;

            Vec2 r1  = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2  = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
            Vec2 p1  = b1._sweep.C + r1;
            Vec2 p2  = b2._sweep.C + r2;
            Vec2 d   = p2 - p1;
            Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1);

            // Solve linear (point-to-line) constraint.
            float linearC = Vec2.Dot(ay1, d);

            // Prevent overly large corrections.
            linearC = Box2DXMath.Clamp(linearC, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
            float linearImpulse = -_linearMass * linearC;

            b1._sweep.C += (invMass1 * linearImpulse) * _linearJacobian.Linear1;
            b1._sweep.A += invI1 * linearImpulse * _linearJacobian.Angular1;
            //b1->SynchronizeTransform(); // updated by angular constraint
            b2._sweep.C += (invMass2 * linearImpulse) * _linearJacobian.Linear2;
            b2._sweep.A += invI2 * linearImpulse * _linearJacobian.Angular2;
            //b2->SynchronizeTransform(); // updated by angular constraint

            float positionError = Box2DXMath.Abs(linearC);

            // Solve angular constraint.
            float angularC = b2._sweep.A - b1._sweep.A - _refAngle;

            // Prevent overly large corrections.
            angularC = Box2DXMath.Clamp(angularC, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
            float angularImpulse = -_angularMass * angularC;

            b1._sweep.A -= b1._invI * angularImpulse;
            b2._sweep.A += b2._invI * angularImpulse;

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

            float angularError = Box2DXMath.Abs(angularC);

            // Solve linear limit constraint.
            if (_enableLimit && _limitState != LimitState.InactiveLimit)
            {
                Vec2 r1_ = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
                Vec2 r2_ = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
                Vec2 p1_ = b1._sweep.C + r1_;
                Vec2 p2_ = b2._sweep.C + r2_;
                Vec2 d_  = p2_ - p1_;
                Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1);

                float translation  = Vec2.Dot(ax1, d_);
                float limitImpulse = 0.0f;

                if (_limitState == LimitState.EqualLimits)
                {
                    // Prevent large angular corrections
                    float limitC = Box2DXMath.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
                    limitImpulse  = -_motorMass * limitC;
                    positionError = Box2DXMath.Max(positionError, Box2DXMath.Abs(angularC));
                }
                else if (_limitState == LimitState.AtLowerLimit)
                {
                    float limitC = translation - _lowerTranslation;
                    positionError = Box2DXMath.Max(positionError, -limitC);

                    // Prevent large linear corrections and allow some slop.
                    limitC       = Box2DXMath.Clamp(limitC + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                    limitImpulse = -_motorMass * limitC;
                    float oldLimitImpulse = _limitPositionImpulse;
                    _limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f);
                    limitImpulse          = _limitPositionImpulse - oldLimitImpulse;
                }
                else if (_limitState == LimitState.AtUpperLimit)
                {
                    float limitC = translation - _upperTranslation;
                    positionError = Box2DXMath.Max(positionError, limitC);

                    // Prevent large linear corrections and allow some slop.
                    limitC       = Box2DXMath.Clamp(limitC - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
                    limitImpulse = -_motorMass * limitC;
                    float oldLimitImpulse = _limitPositionImpulse;
                    _limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f);
                    limitImpulse          = _limitPositionImpulse - oldLimitImpulse;
                }

                b1._sweep.C += (invMass1 * limitImpulse) * _motorJacobian.Linear1;
                b1._sweep.A += invI1 * limitImpulse * _motorJacobian.Angular1;
                b2._sweep.C += (invMass2 * limitImpulse) * _motorJacobian.Linear2;
                b2._sweep.A += invI2 * limitImpulse * _motorJacobian.Angular2;

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

            return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
        }
Esempio n. 5
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            if (_enableMotor || _enableLimit)
            {
                // You cannot create a rotation limit between bodies that
                // both have fixed rotation.
                Box2DNetDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
            }

            // Compute the effective mass matrix.
            Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

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

            // Matlab
            // K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
            //     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
            //     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]

            float m1 = b1._invMass, m2 = b2._invMass;
            float i1 = b1._invI, i2 = b2._invI;

            _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
            _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
            _mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
            _mass.Col1.Y = _mass.Col2.X;
            _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
            _mass.Col3.Y = r1.X * i1 + r2.X * i2;
            _mass.Col1.Z = _mass.Col3.X;
            _mass.Col2.Z = _mass.Col3.Y;
            _mass.Col3.Z = i1 + i2;

            _motorMass = 1.0f / (i1 + i2);

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

            if (_enableLimit)
            {
                float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle;
                if (Box2DXMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
                {
                    _limitState = LimitState.EqualLimits;
                }
                else if (jointAngle <= _lowerAngle)
                {
                    if (_limitState != LimitState.AtLowerLimit)
                    {
                        _impulse.Z = 0.0f;
                    }
                    _limitState = LimitState.AtLowerLimit;
                }
                else if (jointAngle >= _upperAngle)
                {
                    if (_limitState != LimitState.AtUpperLimit)
                    {
                        _impulse.Z = 0.0f;
                    }
                    _limitState = LimitState.AtUpperLimit;
                }
                else
                {
                    _limitState = LimitState.InactiveLimit;
                    _impulse.Z  = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.InactiveLimit;
            }

            if (step.WarmStarting)
            {
                // Scale impulses to support a variable time step.
                _impulse      *= step.DtRatio;
                _motorImpulse *= step.DtRatio;

                Vec2 P = new Vec2(_impulse.X, _impulse.Y);

                b1._linearVelocity  -= m1 * P;
                b1._angularVelocity -= i1 * (Vec2.Cross(r1, P) + _motorImpulse + _impulse.Z);

                b2._linearVelocity  += m2 * P;
                b2._angularVelocity += i2 * (Vec2.Cross(r2, P) + _motorImpulse + _impulse.Z);
            }
            else
            {
                _impulse.SetZero();
                _motorImpulse = 0.0f;
            }
        }
Esempio n. 6
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            // Compute the effective masses.
            Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

            float invMass1 = b1._invMass, invMass2 = b2._invMass;
            float invI1 = b1._invI, invI2 = b2._invI;

            // Compute point to line constraint effective mass.
            // J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
            Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1);
            Vec2 e   = b2._sweep.C + r2 - b1._sweep.C;                  // e = d + r1

            _linearJacobian.Set(-ay1, -Vec2.Cross(e, ay1), ay1, Vec2.Cross(r2, ay1));
            _linearMass = invMass1 + invI1 * _linearJacobian.Angular1 * _linearJacobian.Angular1 +
                          invMass2 + invI2 * _linearJacobian.Angular2 * _linearJacobian.Angular2;
            Box2DXDebug.Assert(_linearMass > Settings.FLT_EPSILON);
            _linearMass = 1.0f / _linearMass;

            // Compute angular constraint effective mass.
            _angularMass = invI1 + invI2;
            if (_angularMass > Settings.FLT_EPSILON)
            {
                _angularMass = 1.0f / _angularMass;
            }

            // Compute motor and limit terms.
            if (_enableLimit || _enableMotor)
            {
                // The motor and limit share a Jacobian and effective mass.
                Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1);
                _motorJacobian.Set(-ax1, -Vec2.Cross(e, ax1), ax1, Vec2.Cross(r2, ax1));
                _motorMass = invMass1 + invI1 * _motorJacobian.Angular1 * _motorJacobian.Angular1 +
                             invMass2 + invI2 * _motorJacobian.Angular2 * _motorJacobian.Angular2;
                Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
                _motorMass = 1.0f / _motorMass;

                if (_enableLimit)
                {
                    Vec2  d = e - r1;                           // p2 - p1
                    float jointTranslation = Vec2.Dot(ax1, d);
                    if (Box2DXMath.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                    {
                        _limitState = LimitState.EqualLimits;
                    }
                    else if (jointTranslation <= _lowerTranslation)
                    {
                        if (_limitState != LimitState.AtLowerLimit)
                        {
                            _limitForce = 0.0f;
                        }
                        _limitState = LimitState.AtLowerLimit;
                    }
                    else if (jointTranslation >= _upperTranslation)
                    {
                        if (_limitState != LimitState.AtUpperLimit)
                        {
                            _limitForce = 0.0f;
                        }
                        _limitState = LimitState.AtUpperLimit;
                    }
                    else
                    {
                        _limitState = LimitState.InactiveLimit;
                        _limitForce = 0.0f;
                    }
                }
            }

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

            if (_enableLimit == false)
            {
                _limitForce = 0.0f;
            }

            if (step.WarmStarting)
            {
                Vec2  P1 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Linear1 + (_motorForce + _limitForce) * _motorJacobian.Linear1);
                Vec2  P2 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Linear2 + (_motorForce + _limitForce) * _motorJacobian.Linear2);
                float L1 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Angular1 - _torque + (_motorForce + _limitForce) * _motorJacobian.Angular1);
                float L2 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Angular2 + _torque + (_motorForce + _limitForce) * _motorJacobian.Angular2);

                b1._linearVelocity  += invMass1 * P1;
                b1._angularVelocity += invI1 * L1;

                b2._linearVelocity  += invMass2 * P2;
                b2._angularVelocity += invI2 * L2;
            }
            else
            {
                _force      = 0.0f;
                _torque     = 0.0f;
                _limitForce = 0.0f;
                _motorForce = 0.0f;
            }

            _limitPositionImpulse = 0.0f;
        }
Esempio n. 7
0
        internal override bool SolvePositionConstraints()
        {
            Body b1 = _body1;
            Body b2 = _body2;

            float positionError = 0.0f;

            // Solve point-to-point position error.
            Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

            Vec2 p1   = b1._sweep.C + r1;
            Vec2 p2   = b2._sweep.C + r2;
            Vec2 ptpC = p2 - p1;

            positionError = ptpC.Length();

            // Prevent overly large corrections.
            //b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection);
            //ptpC = b2Clamp(ptpC, -dpMax, dpMax);

            float invMass1 = b1._invMass, invMass2 = b2._invMass;
            float invI1 = b1._invI, invI2 = b2._invI;

            Mat22 K1 = new Mat22();

            K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
            K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;

            Mat22 K2 = new Mat22();

            K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
            K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;

            Mat22 K3 = new Mat22();

            K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
            K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;

            Mat22 K       = K1 + K2 + K3;
            Vec2  impulse = K.Solve(-ptpC);

            b1._sweep.C -= b1._invMass * impulse;
            b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse);

            b2._sweep.C += b2._invMass * impulse;
            b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse);

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

            // Handle limits.
            float angularError = 0.0f;

            if (_enableLimit && _limitState != LimitState.InactiveLimit)
            {
                float angle        = b2._sweep.A - b1._sweep.A - _referenceAngle;
                float limitImpulse = 0.0f;

                if (_limitState == LimitState.EqualLimits)
                {
                    // Prevent large angular corrections
                    float limitC = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
                    limitImpulse = -_motorMass * limitC;
                    angularError = Box2DXMath.Abs(limitC);
                }
                else if (_limitState == LimitState.AtLowerLimit)
                {
                    float limitC = angle - _lowerAngle;
                    angularError = Box2DXMath.Max(0.0f, -limitC);

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

                    // Prevent large angular corrections and allow some slop.
                    limitC       = Box2DXMath.Clamp(limitC - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
                    limitImpulse = -_motorMass * limitC;
                    float oldLimitImpulse = _limitPositionImpulse;
                    _limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f);
                    limitImpulse          = _limitPositionImpulse - oldLimitImpulse;
                }

                b1._sweep.A -= b1._invI * limitImpulse;
                b2._sweep.A += b2._invI * limitImpulse;

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

            return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
        }
Esempio n. 8
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            // Compute the effective mass matrix.
            Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.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 invMass1 = b1._invMass, invMass2 = b2._invMass;
            float invI1 = b1._invI, invI2 = b2._invI;

            Mat22 K1 = new Mat22();

            K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
            K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;

            Mat22 K2 = new Mat22();

            K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
            K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;

            Mat22 K3 = new Mat22();

            K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
            K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;

            Mat22 K = K1 + K2 + K3;

            _pivotMass = K.Invert();

            _motorMass = 1.0f / (invI1 + invI2);

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

            if (_enableLimit)
            {
                float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle;
                if (Box2DXMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
                {
                    _limitState = LimitState.EqualLimits;
                }
                else if (jointAngle <= _lowerAngle)
                {
                    if (_limitState != LimitState.AtLowerLimit)
                    {
                        _limitForce = 0.0f;
                    }
                    _limitState = LimitState.AtLowerLimit;
                }
                else if (jointAngle >= _upperAngle)
                {
                    if (_limitState != LimitState.AtUpperLimit)
                    {
                        _limitForce = 0.0f;
                    }
                    _limitState = LimitState.AtUpperLimit;
                }
                else
                {
                    _limitState = LimitState.InactiveLimit;
                    _limitForce = 0.0f;
                }
            }
            else
            {
                _limitForce = 0.0f;
            }

            if (step.WarmStarting)
            {
                b1._linearVelocity  -= Settings.FORCE_SCALE(step.Dt) * invMass1 * _pivotForce;
                b1._angularVelocity -= Settings.FORCE_SCALE(step.Dt) * invI1 * (Vec2.Cross(r1, _pivotForce) + Settings.FORCE_INV_SCALE(_motorForce + _limitForce));

                b2._linearVelocity  += Settings.FORCE_SCALE(step.Dt) * invMass2 * _pivotForce;
                b2._angularVelocity += Settings.FORCE_SCALE(step.Dt) * invI2 * (Vec2.Cross(r2, _pivotForce) + Settings.FORCE_INV_SCALE(_motorForce + _limitForce));
            }
            else
            {
                _pivotForce.SetZero();
                _motorForce = 0.0f;
                _limitForce = 0.0f;
            }

            _limitPositionImpulse = 0.0f;
        }
Esempio n. 9
0
        /// <summary>
        ///     Describes whether this instance solve position constraints
        /// </summary>
        /// <param name="baumgarte">The baumgarte</param>
        /// <returns>The bool</returns>
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            // TODO_ERIN block solve with limit.

            Body body1 = Body1;
            Body body2 = Body2;

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

            // Solve angular limit constraint.
            if (IsLimitEnabled && State != LimitState.InactiveLimit)
            {
                float angle        = body2.Sweep.A - body1.Sweep.A - ReferenceAngle;
                float limitImpulse = 0.0f;

                if (State == LimitState.EqualLimits)
                {
                    // Prevent large angular corrections
                    float c = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
                    limitImpulse = -MotorMass * c;
                    angularError = Box2DXMath.Abs(c);
                }
                else if (State == LimitState.AtLowerLimit)
                {
                    float c = angle - LowerLimit;
                    angularError = -c;

                    // Prevent large angular corrections and allow some slop.
                    c            = Box2DXMath.Clamp(c + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
                    limitImpulse = -MotorMass * c;
                }
                else if (State == LimitState.AtUpperLimit)
                {
                    float c = angle - UpperLimit;
                    angularError = c;

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

                body1.Sweep.A -= body1.InvI * limitImpulse;
                body2.Sweep.A += body2.InvI * limitImpulse;

                body1.SynchronizeTransform();
                body2.SynchronizeTransform();
            }

            // Solve point-to-point constraint.
            {
                Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
                Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());

                Vec2 body2SweepC = body2.Sweep.C + mulR2 - body1.Sweep.C - mulR1;
                positionError = body2SweepC.Length();

                float invMass1 = body1.InvMass, invMass2 = body2.InvMass;
                float invI1 = body1.InvI, invI2 = body2.InvI;

                // Handle large detachment.
                float kAllowedStretch = 10.0f * Settings.LinearSlop;
                if (body2SweepC.LengthSquared() > kAllowedStretch * kAllowedStretch)
                {
                    // Use a particle solution (no rotation).
                    Vec2 sweepC = body2SweepC;
                    sweepC.Normalize();
                    float mass12 = invMass1 + invMass2;
                    Box2DxDebug.Assert(mass12 > Settings.FltEpsilon);
                    float divideMass12 = 1.0f / mass12;
                    Vec2  impulseLocal = divideMass12 * -body2SweepC;
                    float kBeta        = 0.5f;
                    body1.Sweep.C -= kBeta * invMass1 * impulseLocal;
                    body2.Sweep.C += kBeta * invMass2 * impulseLocal;

                    body2SweepC = body2.Sweep.C + mulR2 - body1.Sweep.C - mulR1;
                }

                Mat22 k1 = new Mat22
                {
                    Col1 = new Vec2(invMass1 + invMass2, 0.0f),
                    Col2 = new Vec2(0.0f, invMass1 + invMass2)
                };

                Mat22 k2 = new Mat22();
                k2.Col1.X = invI1 * mulR1.Y * mulR1.Y;
                k2.Col2.X = -invI1 * mulR1.X * mulR1.Y;
                k2.Col1.Y = -invI1 * mulR1.X * mulR1.Y;
                k2.Col2.Y = invI1 * mulR1.X * mulR1.X;

                Mat22 k3 = new Mat22();
                k3.Col1.X = invI2 * mulR2.Y * mulR2.Y;
                k3.Col2.X = -invI2 * mulR2.X * mulR2.Y;
                k3.Col1.Y = -invI2 * mulR2.X * mulR2.Y;
                k3.Col2.Y = invI2 * mulR2.X * mulR2.X;

                Mat22 k       = k1 + k2 + k3;
                Vec2  impulse = k.Solve(-body2SweepC);

                body1.Sweep.C -= body1.InvMass * impulse;
                body1.Sweep.A -= body1.InvI * Vec2.Cross(mulR1, impulse);

                body2.Sweep.C += body2.InvMass * impulse;
                body2.Sweep.A += body2.InvI * Vec2.Cross(mulR2, impulse);

                body1.SynchronizeTransform();
                body2.SynchronizeTransform();
            }

            return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
        }
Esempio n. 10
0
        /// <summary>
        ///     Inits the velocity constraints using the specified step
        /// </summary>
        /// <param name="step">The step</param>
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body body1 = Body1;
            Body body2 = Body2;

            if (IsMotorEnabled || IsLimitEnabled)
            {
                // You cannot create a rotation limit between bodies that
                // both have fixed rotation.
                Box2DxDebug.Assert(body1.InvI > 0.0f || body2.InvI > 0.0f);
            }

            // Compute the effective mass matrix.
            Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
            Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());

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

            // Matlab
            // K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
            //     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
            //     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]

            float m1 = body1.InvMass, m2 = body2.InvMass;
            float i1 = body1.InvI, i2 = body2.InvI;


            float col1X = m1 + m2 + mulR1.Y * mulR1.Y * i1 + mulR2.Y * mulR2.Y * i2;
            float col2X = -mulR1.Y * mulR1.X * i1 - mulR2.Y * mulR2.X * i2;
            float col3X = -mulR1.Y * i1 - mulR2.Y * i2;

            float col1Y = Mass.Col2.X;
            float col2Y = m1 + m2 + mulR1.X * mulR1.X * i1 + mulR2.X * mulR2.X * i2;
            float col3Y = mulR1.X * i1 + mulR2.X * i2;

            float col1Z = Mass.Col3.X;
            float col2Z = Mass.Col3.Y;
            float col3Z = i1 + i2;

            Mass = new Mat33(new Vec3(col1X, col1Y, col1Z), new Vec3(col2X, col2Y, col2Z),
                             new Vec3(col3X, col3Y, col3Z));

            /*
             *          _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
             *          _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
             *          _mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
             *          _mass.Col1.Y = _mass.Col2.X;
             *          _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
             *          _mass.Col3.Y = r1.X * i1 + r2.X * i2;
             *          _mass.Col1.Z = _mass.Col3.X;
             *          _mass.Col2.Z = _mass.Col3.Y;
             *          _mass.Col3.Z = i1 + i2;
             */
            MotorMass = 1.0f / (i1 + i2);

            if (IsMotorEnabled == false)
            {
                MotorTorque = 0.0f;
            }

            if (IsLimitEnabled)
            {
                float jointAngle = body2.Sweep.A - body1.Sweep.A - ReferenceAngle;
                if (Box2DXMath.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.AngularSlop)
                {
                    State = LimitState.EqualLimits;
                }
                else if (jointAngle <= LowerLimit)
                {
                    if (State != LimitState.AtLowerLimit)
                    {
                        Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f);
                    }

                    State = LimitState.AtLowerLimit;
                }
                else if (jointAngle >= UpperLimit)
                {
                    if (State != LimitState.AtUpperLimit)
                    {
                        Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f);
                    }

                    State = LimitState.AtUpperLimit;
                }
                else
                {
                    State   = LimitState.InactiveLimit;
                    Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f);
                }
            }
            else
            {
                State = LimitState.InactiveLimit;
            }

            if (step.WarmStarting)
            {
                // Scale impulses to support a variable time step.
                Impulse     *= step.DtRatio;
                MotorTorque *= step.DtRatio;

                Vec2 p = new Vec2(Impulse.X, Impulse.Y);

                body1.LinearVelocity  -= m1 * p;
                body1.AngularVelocity -= i1 * (Vec2.Cross(mulR1, p) + MotorTorque + Impulse.Z);

                body2.LinearVelocity  += m2 * p;
                body2.AngularVelocity += i2 * (Vec2.Cross(mulR2, p) + MotorTorque + Impulse.Z);
            }
            else
            {
                Impulse.SetZero();
                MotorTorque = 0.0f;
            }
        }