FarseerPhysics.Dynamics.Joints.RopeJoint.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;

			_length = _u.Length();

			float C = _length - maxLength;
			if( C > 0.0f )
			{
				state = LimitState.AtUpper;
			}
			else
			{
				state = LimitState.Inactive;
			}

			if( _length > Settings.linearSlop )
			{
				_u *= 1.0f / _length;
			}
			else
			{
				_u = Vector2.Zero;
				_mass = 0.0f;
				_impulse = 0.0f;
				return;
			}

			// Compute effective mass.
			float crA = MathUtils.cross( _rA, _u );
			float crB = MathUtils.cross( _rB, _u );
			float invMass = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB;

			_mass = invMass != 0.0f ? 1.0f / invMass : 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;
		}