Example #1
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());

            if (_state == LimitState.AtUpperLimit)
            {
                Vector2 v1 = b1._linearVelocity + r1.CrossScalarPreMultiply(b1._angularVelocity);
                Vector2 v2 = b2._linearVelocity + r1.CrossScalarPreMultiply(b2._angularVelocity);

                float Cdot       = -Vector2.Dot(_u1, v1) - _ratio * Vector2.Dot(_u2, v2);
                float impulse    = _pulleyMass * (-Cdot);
                float oldImpulse = _impulse;
                _impulse = Box2DX.Common.Math.Max(0.0f, _impulse + impulse);
                impulse  = _impulse - oldImpulse;

                Vector2 P1 = -impulse * _u1;
                Vector2 P2 = -_ratio * impulse * _u2;
                b1._linearVelocity  += b1._invMass * P1;
                b1._angularVelocity += b1._invI * r1.Cross(P1);
                b2._linearVelocity  += b2._invMass * P2;
                b2._angularVelocity += b2._invI * r2.Cross(P2);
            }

            if (_limitState1 == LimitState.AtUpperLimit)
            {
                Vector2 v1 = b1._linearVelocity + r1.CrossScalarPreMultiply(b1._angularVelocity);

                float Cdot       = -Vector2.Dot(_u1, v1);
                float impulse    = -_limitMass1 * Cdot;
                float oldImpulse = _limitImpulse1;
                _limitImpulse1 = Box2DX.Common.Math.Max(0.0f, _limitImpulse1 + impulse);
                impulse        = _limitImpulse1 - oldImpulse;

                Vector2 P1 = -impulse * _u1;
                b1._linearVelocity  += b1._invMass * P1;
                b1._angularVelocity += b1._invI * r1.Cross(P1);
            }

            if (_limitState2 == LimitState.AtUpperLimit)
            {
                Vector2 v2 = b2._linearVelocity + r2.CrossScalarPreMultiply(b2._angularVelocity);

                float Cdot       = -Vector2.Dot(_u2, v2);
                float impulse    = -_limitMass2 * Cdot;
                float oldImpulse = _limitImpulse2;
                _limitImpulse2 = Box2DX.Common.Math.Max(0.0f, _limitImpulse2 + impulse);
                impulse        = _limitImpulse2 - oldImpulse;

                Vector2 P2 = -impulse * _u2;
                b2._linearVelocity  += b2._invMass * P2;
                b2._angularVelocity += b2._invI * r2.Cross(P2);
            }
        }
Example #2
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            //B2_NOT_USED(step);

            Body b1 = _body1;
            Body b2 = _body2;

            Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());

            // Cdot = dot(u, v + cross(w, r))
            Vector2 v1      = b1._linearVelocity + r1.CrossScalarPreMultiply(b1._angularVelocity);
            Vector2 v2      = b2._linearVelocity + r2.CrossScalarPreMultiply(b2._angularVelocity);
            float   Cdot    = Vector2.Dot(_u, v2 - v1);
            float   impulse = -_mass * (Cdot + _bias + _gamma * _impulse);

            _impulse += impulse;

            Vector2 P = impulse * _u;

            b1._linearVelocity  -= b1._invMass * P;
            b1._angularVelocity -= b1._invI * r1.Cross(P);
            b2._linearVelocity  += b2._invMass * P;
            b2._angularVelocity += b2._invI * r2.Cross(P);
        }
Example #3
0
        /// <summary>
        /// Get the current joint translation speed, usually in meters per second.
        /// </summary>
        public float GetJointSpeed()
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vector2 r1   = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
            Vector2 r2   = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
            Vector2 p1   = b1._sweep.C + r1;
            Vector2 p2   = b2._sweep.C + r2;
            Vector2 d    = p2 - p1;
            Vector2 axis = b1.GetWorldVector(_localXAxis1);

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

            float speed = Vector2.Dot(d, axis.CrossScalarPreMultiply(w1)) + Vector2.Dot(axis, v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1));

            return(speed);
        }
Example #4
0
        public LineJoint(LineJointDef def)
            : base(def)
        {
            _localAnchor1 = def.localAnchor1;
            _localAnchor2 = def.localAnchor2;
            _localXAxis1  = def.localAxis1;
            _localYAxis1  = _localXAxis1.CrossScalarPreMultiply(1.0f);

            _impulse      = Vector2.Zero;
            _motorMass    = 0.0f;
            _motorImpulse = 0.0f;

            _lowerTranslation = def.lowerTranslation;
            _upperTranslation = def.upperTranslation;
            _maxMotorForce    = Settings.FORCE_INV_SCALE(def.maxMotorForce);
            _motorSpeed       = def.motorSpeed;
            _enableLimit      = def.enableLimit;
            _enableMotor      = def.enableMotor;
            _limitState       = LimitState.InactiveLimit;

            _axis = Vector2.Zero;
            _perp = Vector2.Zero;
        }
Example #5
0
        public PrismaticJoint(PrismaticJointDef def)
            : base(def)
        {
            _localAnchor1 = def.LocalAnchor1;
            _localAnchor2 = def.LocalAnchor2;
            _localXAxis1  = def.LocalAxis1;
            _localYAxis1  = _localXAxis1.CrossScalarPreMultiply(1.0f);
            _refAngle     = def.ReferenceAngle;

            _impulse      = Vector3.Zero;
            _motorMass    = 0.0f;
            _motorImpulse = 0.0f;

            _lowerTranslation = def.LowerTranslation;
            _upperTranslation = def.UpperTranslation;
            _maxMotorForce    = Settings.FORCE_INV_SCALE(def.MaxMotorForce);
            _motorSpeed       = def.MotorSpeed;
            _enableLimit      = def.EnableLimit;
            _enableMotor      = def.EnableMotor;
            _limitState       = LimitState.InactiveLimit;

            _axis = Vector2.Zero;
            _perp = Vector2.Zero;
        }
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b = _body2;

            Vector2 r = b.GetTransform().TransformDirection(_localAnchor - b.GetLocalCenter());

            // Cdot = v + cross(w, r)
            Vector2 Cdot    = b._linearVelocity + r.CrossScalarPreMultiply(b._angularVelocity);
            Vector2 impulse = _mass.Multiply(-(Cdot + _beta * _C + _gamma * _impulse));

            Vector2 oldImpulse = _impulse;

            _impulse += impulse;
            float maxImpulse = step.Dt * _maxForce;

            if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
            {
                _impulse *= maxImpulse / _impulse.Length();
            }
            impulse = _impulse - oldImpulse;

            b._linearVelocity  += b._invMass * impulse;
            b._angularVelocity += b._invI * r.Cross(impulse);
        }
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vector2 v1 = b1._linearVelocity;
            float   w1 = b1._angularVelocity;
            Vector2 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 = Box2DX.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse       = _motorImpulse - oldImpulse;

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

            //Solve limit constraint.
            if (_enableLimit && _limitState != LimitState.InactiveLimit)
            {
                Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());

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

                Vector3 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)
                    {
                        Vector2 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)
                    {
                        Vector2 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;
                    }
                }

                Vector2 P = impulse.ToVector2();

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

                v2 += m2 * P;
                w2 += i2 * (r2.Cross(P) + impulse.Z);
            }
            else
            {
                Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());

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

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

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

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

            b1._linearVelocity  = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity  = v2;
            b2._angularVelocity = w2;
        }
        public PrismaticJoint(PrismaticJointDef def)
            : base(def)
        {
            _localAnchor1 = def.LocalAnchor1;
            _localAnchor2 = def.LocalAnchor2;
            _localXAxis1 = def.LocalAxis1;
            _localYAxis1 = _localXAxis1.CrossScalarPreMultiply(1.0f);
            _refAngle = def.ReferenceAngle;

            _impulse = Vector3.zero;
            _motorMass = 0.0f;
            _motorImpulse = 0.0f;

            _lowerTranslation = def.LowerTranslation;
            _upperTranslation = def.UpperTranslation;
            _maxMotorForce = Settings.FORCE_INV_SCALE(def.MaxMotorForce);
            _motorSpeed = def.MotorSpeed;
            _enableLimit = def.EnableLimit;
            _enableMotor = def.EnableMotor;
            _limitState = LimitState.InactiveLimit;

            _axis = Vector2.zero;
            _perp= Vector2.zero;
        }
Example #9
0
        public LineJoint(LineJointDef def)
            : base(def)
        {
            _localAnchor1 = def.localAnchor1;
            _localAnchor2 = def.localAnchor2;
            _localXAxis1 = def.localAxis1;
            _localYAxis1 = _localXAxis1.CrossScalarPreMultiply(1.0f);

            _impulse = Vector2.zero;
            _motorMass = 0.0f;
            _motorImpulse = 0.0f;

            _lowerTranslation = def.lowerTranslation;
            _upperTranslation = def.upperTranslation;
            _maxMotorForce = Settings.FORCE_INV_SCALE(def.maxMotorForce);
            _motorSpeed = def.motorSpeed;
            _enableLimit = def.enableLimit;
            _enableMotor = def.enableMotor;
            _limitState = LimitState.InactiveLimit;

            _axis = Vector2.zero;
            _perp = Vector2.zero;
        }