Length() public méthode

Get the length of this vector (the norm).
public Length ( ) : float
Résultat float
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            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 s1 = _ground.GetXForm().Position + _groundAnchor1;
            Vec2 s2 = _ground.GetXForm().Position + _groundAnchor2;

            // Get the pulley axes.
            _u1 = p1 - s1;
            _u2 = p2 - s2;

            float length1 = _u1.Length();
            float length2 = _u2.Length();

            if (length1 > Settings.LinearSlop)
            {
                _u1 *= 1.0f / length1;
            }
            else
            {
                _u1.SetZero();
            }

            if (length2 > Settings.LinearSlop)
            {
                _u2 *= 1.0f / length2;
            }
            else
            {
                _u2.SetZero();
            }

            float C = _constant - length1 - _ratio * length2;
            if (C > 0.0f)
            {
                _state = LimitState.InactiveLimit;
                _impulse = 0.0f;
            }
            else
            {
                _state = LimitState.AtUpperLimit;
            }

            if (length1 < _maxLength1)
            {
                _limitState1 = LimitState.InactiveLimit;
                _limitImpulse1 = 0.0f;
            }
            else
            {
                _limitState1 = LimitState.AtUpperLimit;
            }

            if (length2 < _maxLength2)
            {
                _limitState2 = LimitState.InactiveLimit;
                _limitImpulse2 = 0.0f;
            }
            else
            {
                _limitState2 = LimitState.AtUpperLimit;
            }

            // Compute effective mass.
            float cr1u1 = Vec2.Cross(r1, _u1);
            float cr2u2 = Vec2.Cross(r2, _u2);

            _limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
            _limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
            _pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
            Box2DXDebug.Assert(_limitMass1 > Settings.FLT_EPSILON);
            Box2DXDebug.Assert(_limitMass2 > Settings.FLT_EPSILON);
            Box2DXDebug.Assert(_pulleyMass > Settings.FLT_EPSILON);
            _limitMass1 = 1.0f / _limitMass1;
            _limitMass2 = 1.0f / _limitMass2;
            _pulleyMass = 1.0f / _pulleyMass;

            if (step.WarmStarting)
            {
                // Scale impulses to support variable time steps.
                _impulse *= step.DtRatio;
                _limitImpulse1 *= step.DtRatio;
                _limitImpulse2 *= step.DtRatio;

                // Warm starting.
                Vec2 P1 = -(_impulse + _limitImpulse1) * _u1;
                Vec2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2;
                b1._linearVelocity += b1._invMass * P1;
                b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1);
                b2._linearVelocity += b2._invMass * P2;
                b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
            }
            else
            {
                _impulse = 0.0f;
                _limitImpulse1 = 0.0f;
                _limitImpulse2 = 0.0f;
            }
        }
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vec2 s1 = _ground.GetXForm().Position + _groundAnchor1;
            Vec2 s2 = _ground.GetXForm().Position + _groundAnchor2;

            float linearError = 0.0f;

            if (_state == LimitState.AtUpperLimit)
            {
                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;

                // Get the pulley axes.
                _u1 = p1 - s1;
                _u2 = p2 - s2;

                float length1 = _u1.Length();
                float length2 = _u2.Length();

                if (length1 > Settings.LinearSlop)
                {
                    _u1 *= 1.0f / length1;
                }
                else
                {
                    _u1.SetZero();
                }

                if (length2 > Settings.LinearSlop)
                {
                    _u2 *= 1.0f / length2;
                }
                else
                {
                    _u2.SetZero();
                }

                float C = _constant - length1 - _ratio * length2;
                linearError = Box2DXMath.Max(linearError, -C);

                C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                float impulse = -_pulleyMass * C;

                Vec2 P1 = -impulse * _u1;
                Vec2 P2 = -_ratio * impulse * _u2;

                b1._sweep.C += b1._invMass * P1;
                b1._sweep.A += b1._invI * Vec2.Cross(r1, P1);
                b2._sweep.C += b2._invMass * P2;
                b2._sweep.A += b2._invI * Vec2.Cross(r2, P2);

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

            if (_limitState1 == LimitState.AtUpperLimit)
            {
                Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
                Vec2 p1 = b1._sweep.C + r1;

                _u1 = p1 - s1;
                float length1 = _u1.Length();

                if (length1 > Settings.LinearSlop)
                {
                    _u1 *= 1.0f / length1;
                }
                else
                {
                    _u1.SetZero();
                }

                float C = _maxLength1 - length1;
                linearError = Box2DXMath.Max(linearError, -C);
                C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                float impulse = -_limitMass1 * C;

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

                b1.SynchronizeTransform();
            }

            if (_limitState2 == LimitState.AtUpperLimit)
            {
                Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
                Vec2 p2 = b2._sweep.C + r2;

                _u2 = p2 - s2;
                float length2 = _u2.Length();

                if (length2 > Settings.LinearSlop)
                {
                    _u2 *= 1.0f / length2;
                }
                else
                {
                    _u2.SetZero();
                }

                float C = _maxLength2 - length2;
                linearError = Box2DXMath.Max(linearError, -C);
                C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                float impulse = -_limitMass2 * C;

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

                b2.SynchronizeTransform();
            }

            return linearError < Settings.LinearSlop;
        }
Exemple #3
0
        public static float Distance(Vec2 a, Vec2 b)
        {
            Vec2 c = a - b;

            return(c.Length());
        }
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            // Compute the effective mass matrix.
            Vec2 r1 = Math.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Math.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());
            _u = b2._sweep.C + r2 - b1._sweep.C - r1;

            // Handle singularity.
            float length = _u.Length();
            if (length > Settings.LinearSlop)
            {
                _u *= 1.0f / length;
            }
            else
            {
                _u.Set(0.0f, 0.0f);
            }

            float cr1u = Vec2.Cross(r1, _u);
            float cr2u = Vec2.Cross(r2, _u);
            float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;

            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

            if (_frequencyHz > 0.0f)
            {
                float C = length - _length;

                // 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 = step.Dt * (d + step.Dt * k);
                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
                _bias = C * step.Dt * k * _gamma;

                _mass = invMass + _gamma;
                _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
            }

            if (step.WarmStarting)
            {
                //Scale the inpulse to support a variable timestep.
                _impulse *= step.DtRatio;

                Vec2 P = _impulse * _u;
                b1._linearVelocity -= b1._invMass * P;
                b1._angularVelocity -= b1._invI * Vec2.Cross(r1, P);
                b2._linearVelocity += b2._invMass * P;
                b2._angularVelocity += b2._invI * Vec2.Cross(r2, P);
            }
            else
            {
                _impulse = 0.0f;
            }
        }
Exemple #5
0
		internal override void InitVelocityConstraints(TimeStep step)
		{
			_inv_dt = step.Inv_Dt;

			Body b1 = _body1;
			Body b2 = _body2;

			// Compute the effective mass matrix.
			Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
			Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
			_u = b2._sweep.C + r2 - b1._sweep.C - r1;

			// Handle singularity.
			float length = _u.Length();
			if (length > Settings.LinearSlop)
			{
				_u *= 1.0f / length;
			}
			else
			{
				_u.Set(0.0f, 0.0f);
			}

			float cr1u = Vec2.Cross(r1, _u);
			float cr2u = Vec2.Cross(r2, _u);
			float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;
			Box2DXDebug.Assert(invMass > Settings.FLT_EPSILON);
			_mass = 1.0f / invMass;

			if (_frequencyHz > 0.0f)
			{
				float C = length - _length;

				// 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 = 1.0f / (step.Dt * (d + step.Dt * k));
				_bias = C * step.Dt * k * _gamma;

				_mass = 1.0f / (invMass + _gamma);
			}

			if (step.WarmStarting)
			{
				_impulse *= step.DtRatio;
				Vec2 P = _impulse * _u;
				b1._linearVelocity -= b1._invMass * P;
				b1._angularVelocity -= b1._invI * Vec2.Cross(r1, P);
				b2._linearVelocity += b2._invMass * P;
				b2._angularVelocity += b2._invI * Vec2.Cross(r2, P);
			}
			else
			{
				_impulse = 0.0f;
			}
		}
Exemple #6
0
		internal override void SolveVelocityConstraints(TimeStep step)
		{
			Body b = _body2;

			Vec2 r = Common.Math.Mul(b.GetXForm().R, _localAnchor - b.GetLocalCenter());

			// Cdot = v + cross(w, r)
			Vec2 Cdot = b._linearVelocity + Vec2.Cross(b._angularVelocity, r);
			Vec2 impulse = Box2DX.Common.Math.Mul(_mass, -(Cdot + _beta * _C + _gamma * _impulse));

			Vec2 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 * Vec2.Cross(r, impulse);
		}
Exemple #7
0
		internal override void SolveVelocityConstraints(TimeStep step)
		{
			Body b = _body2;

			Vec2 r = Common.Math.Mul(b.GetXForm().R, _localAnchor - b.GetLocalCenter());

			// Cdot = v + cross(w, r)
			Vec2 Cdot = b._linearVelocity + Vec2.Cross(b._angularVelocity, r);
			Vec2 force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * Common.Math.Mul(_mass, Cdot + 
				(_beta * step.Inv_Dt) * _C + Settings.FORCE_SCALE(step.Dt) * (_gamma * _impulse));

			Vec2 oldForce = _impulse;
			_impulse += force;
			float forceMagnitude = _impulse.Length();
			if (forceMagnitude > _maxForce)
			{
				_impulse *= _maxForce / forceMagnitude;
			}
			force = _impulse - oldForce;

			Vec2 P = Settings.FORCE_SCALE(step.Dt) * force;
			b._linearVelocity += b._invMass * P;
			b._angularVelocity += b._invI * Vec2.Cross(r, P);
		}