FarseerPhysics.Dynamics.Joints.DistanceJoint.initVelocityConstraints C# (CSharp) Method

		internal override void initVelocityConstraints( ref SolverData data )
		{
			_indexA = bodyA.islandIndex;
			_indexB = bodyB.islandIndex;
			_localCenterA = bodyA._sweep.LocalCenter;
			_localCenterB = bodyB._sweep.LocalCenter;
			_invMassA = bodyA._invMass;
			_invMassB = bodyB._invMass;
			_invIA = bodyA._invI;
			_invIB = bodyB._invI;

			Vector2 cA = data.positions[_indexA].c;
			float aA = data.positions[_indexA].a;
			Vector2 vA = data.velocities[_indexA].v;
			float wA = data.velocities[_indexA].w;

			Vector2 cB = data.positions[_indexB].c;
			float aB = data.positions[_indexB].a;
			Vector2 vB = data.velocities[_indexB].v;
			float wB = data.velocities[_indexB].w;

			Rot qA = new Rot( aA ), qB = new Rot( aB );

			_rA = MathUtils.mul( qA, localAnchorA - _localCenterA );
			_rB = MathUtils.mul( qB, localAnchorB - _localCenterB );
			_u = cB + _rB - cA - _rA;

			// Handle singularity.
			float length = _u.Length();
			if( length > Settings.linearSlop )
			{
				_u *= 1.0f / length;
			}
			else
			{
				_u = Vector2.Zero;
			}

			float crAu = MathUtils.cross( _rA, _u );
			float crBu = MathUtils.cross( _rB, _u );
			float invMass = _invMassA + _invIA * crAu * crAu + _invMassB + _invIB * crBu * crBu;

			// Compute the effective mass matrix.
			_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

			if( frequency > 0.0f )
			{
				float C = length - this.length;

				// Frequency
				float omega = 2.0f * Settings.pi * frequency;

				// Damping coefficient
				float d = 2.0f * _mass * dampingRatio * omega;

				// Spring stiffness
				float k = _mass * omega * omega;

				// magic formulas
				float h = data.step.dt;
				_gamma = h * ( d + h * k );
				_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
				_bias = C * h * k * _gamma;

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

			if( Settings.enableWarmstarting )
			{
				// Scale the impulse to support a variable time step.
				_impulse *= data.step.dtRatio;

				Vector2 P = _impulse * _u;
				vA -= _invMassA * P;
				wA -= _invIA * MathUtils.cross( _rA, P );
				vB += _invMassB * P;
				wB += _invIB * MathUtils.cross( _rB, P );
			}
			else
			{
				_impulse = 0.0f;
			}

			data.velocities[_indexA].v = vA;
			data.velocities[_indexA].w = wA;
			data.velocities[_indexB].v = vB;
			data.velocities[_indexB].w = wB;
		}