Exemple #1
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b = _body2;

            // Compute the effective mass matrix.
            Vec2 r = Common.Math.Mul(b.GetXForm().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();

            K1.Col1.X = invMass; K1.Col2.X = 0.0f;
            K1.Col1.Y = 0.0f; K1.Col2.Y = invMass;

            Mat22 K2 = new Mat22();

            K2.Col1.X = invI * r.Y * r.Y; K2.Col2.X = -invI * r.X * r.Y;
            K2.Col1.Y = -invI * r.X * r.Y; K2.Col2.Y = invI * r.X * r.X;

            Mat22 K = K1 + K2;

            K.Col1.X += _gamma;
            K.Col2.Y += _gamma;

            _mass = K.Invert();

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

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

            // Warm starting.
            Vec2 P = Settings.FORCE_SCALE(step.Dt) * _impulse;

            b._linearVelocity  += invMass * P;
            b._angularVelocity += invI * Vec2.Cross(r, P);
        }
Exemple #2
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body  body = this._body2;
            float mass = body.GetMass();
            float num  = 2f * Settings.Pi * this._frequencyHz;
            float num2 = 2f * mass * this._dampingRatio * num;
            float num3 = mass * (num * num);

            Box2DXDebug.Assert(num2 + step.Dt * num3 > Settings.FLT_EPSILON);
            this._gamma = 1f / (step.Dt * (num2 + step.Dt * num3));
            this._beta  = step.Dt * num3 * this._gamma;
            Vec2  vec     = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor - body.GetLocalCenter());
            float invMass = body._invMass;
            float invI    = body._invI;
            Mat22 a       = default(Mat22);

            a.Col1.X = invMass;
            a.Col2.X = 0f;
            a.Col1.Y = 0f;
            a.Col2.Y = invMass;
            Mat22 b = default(Mat22);

            b.Col1.X = invI * vec.Y * vec.Y;
            b.Col2.X = -invI * vec.X * vec.Y;
            b.Col1.Y = -invI * vec.X * vec.Y;
            b.Col2.Y = invI * vec.X * vec.X;
            Mat22 mat = a + b;

            mat.Col1.X             = mat.Col1.X + this._gamma;
            mat.Col2.Y             = mat.Col2.Y + this._gamma;
            this._mass             = mat.Invert();
            this._C                = body._sweep.C + vec - this._target;
            body._angularVelocity *= 0.98f;
            this._impulse         *= step.DtRatio;
            Body expr_21D = body;

            expr_21D._linearVelocity += invMass * this._impulse;
            body._angularVelocity    += invI * Vec2.Cross(vec, this._impulse);
        }
Exemple #3
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b = _bodyB;

            float mass = b.GetMass();

            // Frequency
            float omega = 2.0f * Settings.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.
            Box2DXDebug.Assert(d + step.Dt * k > Settings.FLT_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.
            Vec2 r = Math.Mul(b.GetTransform().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();

            K1.Col1.X = invMass; K1.Col2.X = 0.0f;
            K1.Col1.Y = 0.0f; K1.Col2.Y = invMass;

            Mat22 K2 = new Mat22();

            K2.Col1.X = invI * r.Y * r.Y; K2.Col2.X = -invI * r.X * r.Y;
            K2.Col1.Y = -invI * r.X * r.Y; K2.Col2.Y = invI * r.X * r.X;

            Mat22 K = K1 + K2;

            K.Col1.X += _gamma;
            K.Col2.Y += _gamma;

            _mass = K.Invert();

            _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 * Vec2.Cross(r, _impulse);
        }
        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;
        }
Exemple #5
0
 public OBBViewportTransform()
 {
     YFlip       = false;
     yFlipMatInv = yFlipMat.Invert();
     Box.R.SetIdentity();
 }