A 2D column vector with 3 elements.
Esempio n. 1
0
		/// <summary>
		/// Solve A * x = b, where b is a column vector. This is more efficient
		/// than computing the inverse in one-shot cases.
		/// </summary>
		public Vec3 Solve33(Vec3 b)
		{
			float det = Vec3.Dot(Col1, Vec3.Cross(Col2, Col3));
			Box2DXDebug.Assert(det != 0.0f);
			det = 1.0f / det;
			Vec3 x = new Vec3();
			x.X = det * Vec3.Dot(b, Vec3.Cross(Col2, Col3));
			x.Y = det * Vec3.Dot(Col1, Vec3.Cross(b, Col3));
			x.Z = det * Vec3.Dot(Col1, Vec3.Cross(Col2, b));
			return x;
		}
Esempio n. 2
0
 public Vec3 Solve33(Vec3 b)
 {
     float num = Vec3.Dot(this.Col1, Vec3.Cross(this.Col2, this.Col3));
     Box2DXDebug.Assert(num != 0f);
     num = 1f / num;
     return new Vec3
     {
         X = num * Vec3.Dot(b, Vec3.Cross(this.Col2, this.Col3)),
         Y = num * Vec3.Dot(this.Col1, Vec3.Cross(b, this.Col3)),
         Z = num * Vec3.Dot(this.Col1, Vec3.Cross(this.Col2, b))
     };
 }
Esempio n. 3
0
 public RevoluteJoint(RevoluteJointDef def)
     : base(def)
 {
     this._localAnchor1 = def.LocalAnchor1;
     this._localAnchor2 = def.LocalAnchor2;
     this._referenceAngle = def.ReferenceAngle;
     this._impulse = default(Vec3);
     this._motorImpulse = 0f;
     this._lowerAngle = def.LowerAngle;
     this._upperAngle = def.UpperAngle;
     this._maxMotorTorque = def.MaxMotorTorque;
     this._motorSpeed = def.MotorSpeed;
     this._enableLimit = def.EnableLimit;
     this._enableMotor = def.EnableMotor;
 }
Esempio n. 4
0
        public RevoluteJoint(RevoluteJointDef def)
            : base(def)
        {
            _localAnchor1 = def.LocalAnchor1;
            _localAnchor2 = def.LocalAnchor2;
            _referenceAngle = def.ReferenceAngle;

            _impulse = new Vec3();
            _motorImpulse = 0.0f;

            _lowerAngle = def.LowerAngle;
            _upperAngle = def.UpperAngle;
            _maxMotorTorque = def.MaxMotorTorque;
            _motorSpeed = def.MotorSpeed;
            _enableLimit = def.EnableLimit;
            _enableMotor = def.EnableMotor;
            _limitState = LimitState.InactiveLimit;
        }
Esempio n. 5
0
 internal override void InitVelocityConstraints(TimeStep step)
 {
     Body body = this._body1;
     Body body2 = this._body2;
     this._localCenter1 = body.GetLocalCenter();
     this._localCenter2 = body2.GetLocalCenter();
     XForm xForm = body.GetXForm();
     XForm xForm2 = body2.GetXForm();
     Vec2 v = Box2DX.Common.Math.Mul(xForm.R, this._localAnchor1 - this._localCenter1);
     Vec2 vec = Box2DX.Common.Math.Mul(xForm2.R, this._localAnchor2 - this._localCenter2);
     Vec2 vec2 = body2._sweep.C + vec - body._sweep.C - v;
     this._invMass1 = body._invMass;
     this._invI1 = body._invI;
     this._invMass2 = body2._invMass;
     this._invI2 = body2._invI;
     this._axis = Box2DX.Common.Math.Mul(xForm.R, this._localXAxis1);
     this._a1 = Vec2.Cross(vec2 + v, this._axis);
     this._a2 = Vec2.Cross(vec, this._axis);
     this._motorMass = this._invMass1 + this._invMass2 + this._invI1 * this._a1 * this._a1 + this._invI2 * this._a2 * this._a2;
     Box2DXDebug.Assert(this._motorMass > Settings.FLT_EPSILON);
     this._motorMass = 1f / this._motorMass;
     this._perp = Box2DX.Common.Math.Mul(xForm.R, this._localYAxis1);
     this._s1 = Vec2.Cross(vec2 + v, this._perp);
     this._s2 = Vec2.Cross(vec, this._perp);
     float invMass = this._invMass1;
     float invMass2 = this._invMass2;
     float invI = this._invI1;
     float invI2 = this._invI2;
     float x = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2;
     float num = invI * this._s1 + invI2 * this._s2;
     float num2 = invI * this._s1 * this._a1 + invI2 * this._s2 * this._a2;
     float y = invI + invI2;
     float num3 = invI * this._a1 + invI2 * this._a2;
     float z = invMass + invMass2 + invI * this._a1 * this._a1 + invI2 * this._a2 * this._a2;
     this._K.Col1.Set(x, num, num2);
     this._K.Col2.Set(num, y, num3);
     this._K.Col3.Set(num2, num3, z);
     if (this._enableLimit)
     {
         float num4 = Vec2.Dot(this._axis, vec2);
         if (Box2DX.Common.Math.Abs(this._upperTranslation - this._lowerTranslation) < 2f * Settings.LinearSlop)
         {
             this._limitState = LimitState.EqualLimits;
         }
         else
         {
             if (num4 <= this._lowerTranslation)
             {
                 if (this._limitState != LimitState.AtLowerLimit)
                 {
                     this._limitState = LimitState.AtLowerLimit;
                     this._impulse.Z = 0f;
                 }
             }
             else
             {
                 if (num4 >= this._upperTranslation)
                 {
                     if (this._limitState != LimitState.AtUpperLimit)
                     {
                         this._limitState = LimitState.AtUpperLimit;
                         this._impulse.Z = 0f;
                     }
                 }
                 else
                 {
                     this._limitState = LimitState.InactiveLimit;
                     this._impulse.Z = 0f;
                 }
             }
         }
     }
     if (!this._enableMotor)
     {
         this._motorImpulse = 0f;
     }
     if (step.WarmStarting)
     {
         this._impulse *= step.DtRatio;
         this._motorImpulse *= step.DtRatio;
         Vec2 v2 = this._impulse.X * this._perp + (this._motorImpulse + this._impulse.Z) * this._axis;
         float num5 = this._impulse.X * this._s1 + this._impulse.Y + (this._motorImpulse + this._impulse.Z) * this._a1;
         float num6 = this._impulse.X * this._s2 + this._impulse.Y + (this._motorImpulse + this._impulse.Z) * this._a2;
         Body expr_4BB = body;
         expr_4BB._linearVelocity -= this._invMass1 * v2;
         body._angularVelocity -= this._invI1 * num5;
         Body expr_4EF = body2;
         expr_4EF._linearVelocity += this._invMass2 * v2;
         body2._angularVelocity += this._invI2 * num6;
     }
     else
     {
         this._impulse.SetZero();
         this._motorImpulse = 0f;
     }
 }
Esempio n. 6
0
 internal override void SolveVelocityConstraints(TimeStep step)
 {
     Body body = this._body1;
     Body body2 = this._body2;
     Vec2 vec = body._linearVelocity;
     float num = body._angularVelocity;
     Vec2 vec2 = body2._linearVelocity;
     float num2 = body2._angularVelocity;
     float invMass = body._invMass;
     float invMass2 = body2._invMass;
     float invI = body._invI;
     float invI2 = body2._invI;
     if (this._enableMotor && this._limitState != LimitState.EqualLimits)
     {
         float num3 = num2 - num - this._motorSpeed;
         float num4 = this._motorMass * -num3;
         float motorImpulse = this._motorImpulse;
         float num5 = step.Dt * this._maxMotorTorque;
         this._motorImpulse = Box2DX.Common.Math.Clamp(this._motorImpulse + num4, -num5, num5);
         num4 = this._motorImpulse - motorImpulse;
         num -= invI * num4;
         num2 += invI2 * num4;
     }
     if (this._enableLimit && this._limitState != LimitState.InactiveLimit)
     {
         Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
         Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
         Vec2 v = vec2 + Vec2.Cross(num2, a2) - vec - Vec2.Cross(num, a);
         float z = num2 - num;
         Vec3 v2 = new Vec3(v.X, v.Y, z);
         Vec3 v3 = this._mass.Solve33(-v2);
         if (this._limitState == LimitState.EqualLimits)
         {
             this._impulse += v3;
         }
         else
         {
             if (this._limitState == LimitState.AtLowerLimit)
             {
                 float num6 = this._impulse.Z + v3.Z;
                 if (num6 < 0f)
                 {
                     Vec2 vec3 = this._mass.Solve22(-v);
                     v3.X = vec3.X;
                     v3.Y = vec3.Y;
                     v3.Z = -this._impulse.Z;
                     this._impulse.X = this._impulse.X + vec3.X;
                     this._impulse.Y = this._impulse.Y + vec3.Y;
                     this._impulse.Z = 0f;
                 }
             }
             else
             {
                 if (this._limitState == LimitState.AtUpperLimit)
                 {
                     float num6 = this._impulse.Z + v3.Z;
                     if (num6 > 0f)
                     {
                         Vec2 vec3 = this._mass.Solve22(-v);
                         v3.X = vec3.X;
                         v3.Y = vec3.Y;
                         v3.Z = -this._impulse.Z;
                         this._impulse.X = this._impulse.X + vec3.X;
                         this._impulse.Y = this._impulse.Y + vec3.Y;
                         this._impulse.Z = 0f;
                     }
                 }
             }
         }
         Vec2 vec4 = new Vec2(v3.X, v3.Y);
         vec -= invMass * vec4;
         num -= invI * (Vec2.Cross(a, vec4) + v3.Z);
         vec2 += invMass2 * vec4;
         num2 += invI2 * (Vec2.Cross(a2, vec4) + v3.Z);
     }
     else
     {
         Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
         Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
         Vec2 v4 = vec2 + Vec2.Cross(num2, a2) - vec - Vec2.Cross(num, a);
         Vec2 vec5 = this._mass.Solve22(-v4);
         this._impulse.X = this._impulse.X + vec5.X;
         this._impulse.Y = this._impulse.Y + vec5.Y;
         vec -= invMass * vec5;
         num -= invI * Vec2.Cross(a, vec5);
         vec2 += invMass2 * vec5;
         num2 += invI2 * Vec2.Cross(a2, vec5);
     }
     body._linearVelocity = vec;
     body._angularVelocity = num;
     body2._linearVelocity = vec2;
     body2._angularVelocity = num2;
 }
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vec2 v1 = b1._linearVelocity;
            float w1 = b1._angularVelocity;
            Vec2 v2 = b2._linearVelocity;
            float w2 = b2._angularVelocity;

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

            //Solve motor constraint.
            if (_enableMotor && _limitState != LimitState.EqualLimits)
            {
                float Cdot = w2 - w1 - _motorSpeed;
                float impulse = _motorMass * (-Cdot);
                float oldImpulse = _motorImpulse;
                float maxImpulse = step.Dt * _maxMotorTorque;
                _motorImpulse = Box2DXMath.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse = _motorImpulse - oldImpulse;

                w1 -= i1 * impulse;
                w2 += i2 * impulse;
            }

            //Solve 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());

                // Solve point-to-point constraint
                Vec2 Cdot1 = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1);
                float Cdot2 = w2 - w1;
                Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2);

                Vec3 impulse = _mass.Solve33(-Cdot);

                if (_limitState == LimitState.EqualLimits)
                {
                    _impulse += impulse;
                }
                else if (_limitState == LimitState.AtLowerLimit)
                {
                    float newImpulse = _impulse.Z + impulse.Z;
                    if (newImpulse < 0.0f)
                    {
                        Vec2 reduced = _mass.Solve22(-Cdot1);
                        impulse.X = reduced.X;
                        impulse.Y = reduced.Y;
                        impulse.Z = -_impulse.Z;
                        _impulse.X += reduced.X;
                        _impulse.Y += reduced.Y;
                        _impulse.Z = 0.0f;
                    }
                }
                else if (_limitState == LimitState.AtUpperLimit)
                {
                    float newImpulse = _impulse.Z + impulse.Z;
                    if (newImpulse > 0.0f)
                    {
                        Vec2 reduced = _mass.Solve22(-Cdot1);
                        impulse.X = reduced.X;
                        impulse.Y = reduced.Y;
                        impulse.Z = -_impulse.Z;
                        _impulse.X += reduced.X;
                        _impulse.Y += reduced.Y;
                        _impulse.Z = 0.0f;
                    }
                }

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

                v1 -= m1 * P;
                w1 -= i1 * (Vec2.Cross(r1, P) + impulse.Z);

                v2 += m2 * P;
                w2 += i2 * (Vec2.Cross(r2, P) + impulse.Z);
            }
            else
            {
                Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
                Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

                // Solve point-to-point constraint
                Vec2 Cdot = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1);
                Vec2 impulse = _mass.Solve22(-Cdot);

                _impulse.X += impulse.X;
                _impulse.Y += impulse.Y;

                v1 -= m1 * impulse;
                w1 -= i1 * Vec2.Cross(r1, impulse);

                v2 += m2 * impulse;
                w2 += i2 * Vec2.Cross(r2, impulse);
            }

            b1._linearVelocity = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity = v2;
            b2._angularVelocity = w2;
        }
Esempio n. 8
0
 internal override void InitVelocityConstraints(TimeStep step)
 {
     Body body = this._body1;
     Body body2 = this._body2;
     Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
     Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
     float invMass = body._invMass;
     float invMass2 = body2._invMass;
     float invI = body._invI;
     float invI2 = body2._invI;
     this._mass.Col1.X = invMass + invMass2 + a.Y * a.Y * invI + a2.Y * a2.Y * invI2;
     this._mass.Col2.X = -a.Y * a.X * invI - a2.Y * a2.X * invI2;
     this._mass.Col3.X = -a.Y * invI - a2.Y * invI2;
     this._mass.Col1.Y = this._mass.Col2.X;
     this._mass.Col2.Y = invMass + invMass2 + a.X * a.X * invI + a2.X * a2.X * invI2;
     this._mass.Col3.Y = a.X * invI + a2.X * invI2;
     this._mass.Col1.Z = this._mass.Col3.X;
     this._mass.Col2.Z = this._mass.Col3.Y;
     this._mass.Col3.Z = invI + invI2;
     this._motorMass = 1f / (invI + invI2);
     if (!this._enableMotor)
     {
         this._motorImpulse = 0f;
     }
     if (this._enableLimit)
     {
         float num = body2._sweep.A - body._sweep.A - this._referenceAngle;
         if (Box2DX.Common.Math.Abs(this._upperAngle - this._lowerAngle) < 2f * Settings.AngularSlop)
         {
             this._limitState = LimitState.EqualLimits;
         }
         else
         {
             if (num <= this._lowerAngle)
             {
                 if (this._limitState != LimitState.AtLowerLimit)
                 {
                     this._impulse.Z = 0f;
                 }
                 this._limitState = LimitState.AtLowerLimit;
             }
             else
             {
                 if (num >= this._upperAngle)
                 {
                     if (this._limitState != LimitState.AtUpperLimit)
                     {
                         this._impulse.Z = 0f;
                     }
                     this._limitState = LimitState.AtUpperLimit;
                 }
                 else
                 {
                     this._limitState = LimitState.InactiveLimit;
                     this._impulse.Z = 0f;
                 }
             }
         }
     }
     if (step.WarmStarting)
     {
         this._impulse *= step.DtRatio;
         this._motorImpulse *= step.DtRatio;
         Vec2 vec = new Vec2(this._impulse.X, this._impulse.Y);
         Body expr_363 = body;
         expr_363._linearVelocity -= invMass * vec;
         body._angularVelocity -= invI * (Vec2.Cross(a, vec) + this._motorImpulse + this._impulse.Z);
         Body expr_3A8 = body2;
         expr_3A8._linearVelocity += invMass2 * vec;
         body2._angularVelocity += invI2 * (Vec2.Cross(a2, vec) + this._motorImpulse + this._impulse.Z);
     }
     else
     {
         this._impulse.SetZero();
         this._motorImpulse = 0f;
     }
 }
 /// <summary>
 /// Construct this matrix using columns.
 /// </summary>
 public Mat33(Vec3 c1, Vec3 c2, Vec3 c3)
 {
     Col1 = c1;
     Col2 = c2;
     Col3 = c3;
 }
        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.
                Box2DXDebug.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;
            }
        }
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vec2 c1 = b1._sweep.C;
            float a1 = b1._sweep.A;

            Vec2 c2 = b2._sweep.C;
            float a2 = b2._sweep.A;

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

            Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2);

            Vec2 r1 = Box2DX.Common.Math.Mul(R1, _localAnchor1 - _localCenter1);
            Vec2 r2 = Box2DX.Common.Math.Mul(R2, _localAnchor2 - _localCenter2);
            Vec2 d = c2 + r2 - c1 - r1;

            if (_enableLimit)
            {
                _axis = Box2DX.Common.Math.Mul(R1, _localXAxis1);

                _a1 = Vec2.Cross(d + r1, _axis);
                _a2 = Vec2.Cross(r2, _axis);

                float translation = Vec2.Dot(_axis, d);
                if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    // Prevent large angular corrections
                    C2 = Box2DX.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
                    linearError = Box2DX.Common.Math.Abs(translation);
                    active = true;
                }
                else if (translation <= _lowerTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = Box2DX.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                    linearError = _lowerTranslation - translation;
                    active = true;
                }
                else if (translation >= _upperTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = Box2DX.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
                    linearError = translation - _upperTranslation;
                    active = true;
                }
            }

            _perp = Box2DX.Common.Math.Mul(R1, _localYAxis1);

            _s1 = Vec2.Cross(d + r1, _perp);
            _s2 = Vec2.Cross(r2, _perp);

            Vec3 impulse;
            Vec2 C1 = new Vec2();
            C1.X = Vec2.Dot(_perp, d);
            C1.Y = a2 - a1 - _refAngle;

            linearError = Box2DX.Common.Math.Max(linearError, Box2DX.Common.Math.Abs(C1.X));
            angularError = Box2DX.Common.Math.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;

            c1 -= _invMass1 * P;
            a1 -= _invI1 * L1;
            c2 += _invMass2 * P;
            a2 += _invI2 * L2;

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

            return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
        }
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2  = _body2;

            Vec2 v1 = b1._linearVelocity;
            float w1 = b1._angularVelocity;
            Vec2 v2 = b2._linearVelocity;
            float w2 = b2._angularVelocity;

            // Solve linear motor constraint.
            if (_enableMotor && _limitState != LimitState.EqualLimits)
            {
                float Cdot = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
                float impulse = _motorMass * (_motorSpeed - Cdot);
                float oldImpulse = _motorImpulse;
                float maxImpulse = step.Dt * _maxMotorForce;
                _motorImpulse = Box2DX.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse = _motorImpulse - oldImpulse;

                Vec2 P = impulse * _axis;
                float L1 = impulse * _a1;
                float L2 = impulse * _a2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }

            Vec2 Cdot1;
            Cdot1.X = Vec2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;
            Cdot1.Y = w2 - w1;

            if (_enableLimit && _limitState != LimitState.InactiveLimit)
            {
                // Solve prismatic and limit constraint in block form.
                float Cdot2;
                Cdot2 = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
                Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2);

                Vec3 f1 = _impulse;
                Vec3 df =  _K.Solve33(-Cdot);
                _impulse += df;

                if (_limitState ==LimitState.AtLowerLimit)
                {
                    _impulse.Z = Box2DX.Common.Math.Max(_impulse.Z, 0.0f);
                }
                else if (_limitState == LimitState.AtUpperLimit)
                {
                    _impulse.Z = Box2DX.Common.Math.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)
                Vec2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vec2(_K.Col3.X, _K.Col3.Y);
                Vec2 f2r = _K.Solve22(b) + new Vec2(f1.X, f1.Y);
                _impulse.X = f2r.X;
                _impulse.Y = f2r.Y;

                df = _impulse - f1;

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

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }
            else
            {
                // Limit is inactive, just solve the prismatic constraint in block form.
                Vec2 df = _K.Solve22(-Cdot1);
                _impulse.X += df.X;
                _impulse.Y += df.Y;

                Vec2 P = df.X * _perp;
                float L1 = df.X * _s1 + df.Y;
                float L2 = df.X * _s2 + df.Y;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }

            b1._linearVelocity = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity = v2;
            b2._angularVelocity = w2;
        }
Esempio n. 13
0
 public Mat33(Vec3 c1, Vec3 c2, Vec3 c3)
 {
     this.Col1 = c1;
     this.Col2 = c2;
     this.Col3 = c3;
 }
        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 = Box2DX.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1);
            Vec2 r2 = Box2DX.Common.Math.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 = Box2DX.Common.Math.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.FLT_EPSILON);
                _motorMass = 1.0f / _motorMass;
            }

            // Prismatic constraint.
            {
                _perp = Box2DX.Common.Math.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 (_enableLimit)
            {
                float jointTranslation = Vec2.Dot(_axis, d);
                if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    _limitState = LimitState.EqualLimits;
                }
                else if (jointTranslation <= _lowerTranslation)
                {
                    if (_limitState != LimitState.AtLowerLimit)
                    {
                        _limitState = LimitState.AtLowerLimit;
                        _impulse.Z = 0.0f;
                    }
                }
                else if (jointTranslation >= _upperTranslation)
                {
                    if (_limitState != LimitState.AtUpperLimit)
                    {
                        _limitState = LimitState.AtUpperLimit;
                        _impulse.Z = 0.0f;
                    }
                }
                else
                {
                    _limitState = LimitState.InactiveLimit;
                    _impulse.Z = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.InactiveLimit;
            }

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

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

                Vec2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis;
                float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1;
                float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2;

                b1._linearVelocity -= _invMass1 * P;
                b1._angularVelocity -= _invI1 * L1;

                b2._linearVelocity += _invMass2 * P;
                b2._angularVelocity += _invI2 * L2;
            }
            else
            {
                _impulse.SetZero();
                _motorImpulse = 0.0f;
            }
        }
Esempio n. 15
0
 public static float Dot(Vec3 a, Vec3 b)
 {
     return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
 }
Esempio n. 16
0
 public static Vec3 Cross(Vec3 a, Vec3 b)
 {
     return new Vec3(a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X);
 }
Esempio n. 17
0
        public static Vec3 GetOGLPos(int x, int y)
        {
            int[] viewport = new int[4];
            double[] modelviewMatrix = new double[16];
            double[] projectionMatrix = new double[16];

            Gl.glGetDoublev(Gl.GL_MODELVIEW_MATRIX, modelviewMatrix);
            Gl.glGetDoublev(Gl.GL_PROJECTION_MATRIX, projectionMatrix);
            Gl.glGetIntegerv(Gl.GL_VIEWPORT, viewport);

            float winX, winY, winZ;
            float[] winz = new float[1];
            winX = x;
            winY = (float)viewport[3] - (float)y;
            Gl.glReadPixels(x, (int)winY, 1, 1, Gl.GL_DEPTH_COMPONENT, Gl.GL_FLOAT, winz);
            /*var winZ = (float)Marshal.PtrToStructure(ptr, typeof(float));
            Marshal.FreeHGlobal(ptr);*/
            winZ = winz[0];

            double nearX, nearY, nearZ;
            double farX, farY, farZ;

            double posX, posY, posZ;
            int success;
            success = Glu.gluUnProject(winX, winY, 0.0f, modelviewMatrix, projectionMatrix, viewport,
                                       out nearX, out nearY, out nearZ);
            if (success == 0)
            {
                throw new ApplicationException("Failed");
            }

            success = Glu.gluUnProject(winX, winY, 1000, modelviewMatrix, projectionMatrix, viewport,
                                           out farX, out farY, out farZ);

            if (success == 0)
            {
                throw new ApplicationException("Failed");
            }

            posX = nearX - farX;
            posY = nearY - farY;
            posZ = nearZ - farY;

            /*            Console.WriteLine(
                    "X:Y {0}:{1} posX:posY:posZ {2}:{3}:{4}", x, y, posX, posY, posZ);*/

            var ration = 0.0165f;
            var result = new Vec3((float)posX / ration, (float)posY / ration, (float)posZ / ration);
            return result;
        }
Esempio n. 18
0
        private void Events_MouseMotion(object sender, MouseMotionEventArgs e)
        {
            this.windowMousePos = new Vec2(e.X, e.Y);

            int[] viewport = new int[4];

            Gl.glGetIntegerv(Gl.GL_VIEWPORT, viewport);

            var wx = (float)e.X;
            var wy = (float)viewport[3] - (float)e.Y;

            float[] wz = new float[1];
            Gl.glReadPixels((int)wx, (int)wy, 1, 1, Gl.GL_DEPTH_COMPONENT, Gl.GL_FLOAT, wz);

            Console.WriteLine("wz {0}", wz[0]);

            this.mousePos = Helper.GetOGLPos(e.X, e.Y);
            var foo = Helper.Foo(e.X, e.Y);

            Console.WriteLine("x:y:z {0}:{1}:{2}", foo.X, foo.Y, foo.Z);

            foreach (var worldObject in this.worldObjects)
            {
                if (worldObject.HitTest(this.mousePos.X, this.mousePos.Y))
                {
                    break;
                }
            }
        }
Esempio n. 19
0
 internal override void SolveVelocityConstraints(TimeStep step)
 {
     Body body = this._body1;
     Body body2 = this._body2;
     Vec2 vec = body._linearVelocity;
     float num = body._angularVelocity;
     Vec2 vec2 = body2._linearVelocity;
     float num2 = body2._angularVelocity;
     if (this._enableMotor && this._limitState != LimitState.EqualLimits)
     {
         float num3 = Vec2.Dot(this._axis, vec2 - vec) + this._a2 * num2 - this._a1 * num;
         float num4 = this._motorMass * (this._motorSpeed - num3);
         float motorImpulse = this._motorImpulse;
         float num5 = step.Dt * this._maxMotorForce;
         this._motorImpulse = Box2DX.Common.Math.Clamp(this._motorImpulse + num4, -num5, num5);
         num4 = this._motorImpulse - motorImpulse;
         Vec2 v = num4 * this._axis;
         float num6 = num4 * this._a1;
         float num7 = num4 * this._a2;
         vec -= this._invMass1 * v;
         num -= this._invI1 * num6;
         vec2 += this._invMass2 * v;
         num2 += this._invI2 * num7;
     }
     Vec2 v2;
     v2.X = Vec2.Dot(this._perp, vec2 - vec) + this._s2 * num2 - this._s1 * num;
     v2.Y = num2 - num;
     if (this._enableLimit && this._limitState != LimitState.InactiveLimit)
     {
         float z = Vec2.Dot(this._axis, vec2 - vec) + this._a2 * num2 - this._a1 * num;
         Vec3 v3 = new Vec3(v2.X, v2.Y, z);
         Vec3 impulse = this._impulse;
         Vec3 v4 = this._K.Solve33(-v3);
         this._impulse += v4;
         if (this._limitState == LimitState.AtLowerLimit)
         {
             this._impulse.Z = Box2DX.Common.Math.Max(this._impulse.Z, 0f);
         }
         else
         {
             if (this._limitState == LimitState.AtUpperLimit)
             {
                 this._impulse.Z = Box2DX.Common.Math.Min(this._impulse.Z, 0f);
             }
         }
         Vec2 b = -v2 - (this._impulse.Z - impulse.Z) * new Vec2(this._K.Col3.X, this._K.Col3.Y);
         Vec2 vec3 = this._K.Solve22(b) + new Vec2(impulse.X, impulse.Y);
         this._impulse.X = vec3.X;
         this._impulse.Y = vec3.Y;
         v4 = this._impulse - impulse;
         Vec2 v = v4.X * this._perp + v4.Z * this._axis;
         float num6 = v4.X * this._s1 + v4.Y + v4.Z * this._a1;
         float num7 = v4.X * this._s2 + v4.Y + v4.Z * this._a2;
         vec -= this._invMass1 * v;
         num -= this._invI1 * num6;
         vec2 += this._invMass2 * v;
         num2 += this._invI2 * num7;
     }
     else
     {
         Vec2 vec4 = this._K.Solve22(-v2);
         this._impulse.X = this._impulse.X + vec4.X;
         this._impulse.Y = this._impulse.Y + vec4.Y;
         Vec2 v = vec4.X * this._perp;
         float num6 = vec4.X * this._s1 + vec4.Y;
         float num7 = vec4.X * this._s2 + vec4.Y;
         vec -= this._invMass1 * v;
         num -= this._invI1 * num6;
         vec2 += this._invMass2 * v;
         num2 += this._invI2 * num7;
     }
     body._linearVelocity = vec;
     body._angularVelocity = num;
     body2._linearVelocity = vec2;
     body2._angularVelocity = num2;
 }