Exemple #1
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

            // Compute the effective mass matrix.
            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

            _u = b2._sweep.c + r2 - b1._sweep.c - r1;

            // Handle singularity.
            float length = _u.Length();

            if (length > Settings.b2_linearSlop)
            {
                _u *= 1.0f / length;
            }
            else
            {
                _u = new Vector2(0.0f, 0.0f);
            }

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

            Debug.Assert(invMass > Settings.b2_epsilon);
            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

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

                // Frequency
                float omega = 2.0f * Settings.b2_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 impulse to support a variable time step.
                _impulse *= step.dtRatio;

                Vector2 P = _impulse * _u;
                b1._linearVelocity  -= b1._invMass * P;
                b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
                b2._linearVelocity  += b2._invMass * P;
                b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
            }
            else
            {
                _impulse = 0.0f;
            }
        }