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

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

			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 );

			// J = [-I -r1_skew I r2_skew]
			//     [ 0       -1 0       1]
			// r_skew = [-ry; rx]

			// Matlab
			// K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
			//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
			//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]

			float mA = _invMassA, mB = _invMassB;
			float iA = _invIA, iB = _invIB;

			bool fixedRotation = ( iA + iB == 0.0f );

			_mass.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB;
			_mass.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB;
			_mass.ez.X = -_rA.Y * iA - _rB.Y * iB;
			_mass.ex.Y = _mass.ey.X;
			_mass.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB;
			_mass.ez.Y = _rA.X * iA + _rB.X * iB;
			_mass.ex.Z = _mass.ez.X;
			_mass.ey.Z = _mass.ez.Y;
			_mass.ez.Z = iA + iB;

			_motorMass = iA + iB;
			if( _motorMass > 0.0f )
			{
				_motorMass = 1.0f / _motorMass;
			}

			if( _enableMotor == false || fixedRotation )
			{
				_motorImpulse = 0.0f;
			}

			if( _enableLimit && fixedRotation == false )
			{
				float jointAngle = aB - aA - referenceAngle;
				if( Math.Abs( _upperAngle - _lowerAngle ) < 2.0f * Settings.angularSlop )
				{
					_limitState = LimitState.Equal;
				}
				else if( jointAngle <= _lowerAngle )
				{
					if( _limitState != LimitState.AtLower )
					{
						_impulse.Z = 0.0f;
					}
					_limitState = LimitState.AtLower;
				}
				else if( jointAngle >= _upperAngle )
				{
					if( _limitState != LimitState.AtUpper )
					{
						_impulse.Z = 0.0f;
					}
					_limitState = LimitState.AtUpper;
				}
				else
				{
					_limitState = LimitState.Inactive;
					_impulse.Z = 0.0f;
				}
			}
			else
			{
				_limitState = LimitState.Inactive;
			}

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

				Vector2 P = new Vector2( _impulse.X, _impulse.Y );

				vA -= mA * P;
				wA -= iA * ( MathUtils.cross( _rA, P ) + motorImpulse + _impulse.Z );

				vB += mB * P;
				wB += iB * ( MathUtils.cross( _rB, P ) + motorImpulse + _impulse.Z );
			}
			else
			{
				_impulse = Vector3.Zero;
				_motorImpulse = 0.0f;
			}

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