FarseerPhysics.Dynamics.Joints.RevoluteJoint.solveVelocityConstraints C# (CSharp) Method

		internal override void solveVelocityConstraints( ref SolverData data )
		{
			Vector2 vA = data.velocities[_indexA].v;
			float wA = data.velocities[_indexA].w;
			Vector2 vB = data.velocities[_indexB].v;
			float wB = data.velocities[_indexB].w;

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

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

			// Solve motor constraint.
			if( _enableMotor && _limitState != LimitState.Equal && fixedRotation == false )
			{
				float Cdot = wB - wA - _motorSpeed;
				float impulse = _motorMass * ( -Cdot );
				float oldImpulse = _motorImpulse;
				float maxImpulse = data.step.dt * _maxMotorTorque;
				_motorImpulse = MathUtils.clamp( _motorImpulse + impulse, -maxImpulse, maxImpulse );
				impulse = _motorImpulse - oldImpulse;

				wA -= iA * impulse;
				wB += iB * impulse;
			}

			// Solve limit constraint.
			if( _enableLimit && _limitState != LimitState.Inactive && fixedRotation == false )
			{
				Vector2 Cdot1 = vB + MathUtils.cross( wB, _rB ) - vA - MathUtils.cross( wA, _rA );
				float Cdot2 = wB - wA;
				Vector3 Cdot = new Vector3( Cdot1.X, Cdot1.Y, Cdot2 );

				Vector3 impulse = -_mass.Solve33( Cdot );

				if( _limitState == LimitState.Equal )
				{
					_impulse += impulse;
				}
				else if( _limitState == LimitState.AtLower )
				{
					float newImpulse = _impulse.Z + impulse.Z;
					if( newImpulse < 0.0f )
					{
						Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2( _mass.ez.X, _mass.ez.Y );
						Vector2 reduced = _mass.Solve22( rhs );
						impulse.X = reduced.X;
						impulse.Y = reduced.Y;
						impulse.Z = -_impulse.Z;
						_impulse.X += reduced.X;
						_impulse.Y += reduced.Y;
						_impulse.Z = 0.0f;
					}
					else
					{
						_impulse += impulse;
					}
				}
				else if( _limitState == LimitState.AtUpper )
				{
					float newImpulse = _impulse.Z + impulse.Z;
					if( newImpulse > 0.0f )
					{
						Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2( _mass.ez.X, _mass.ez.Y );
						Vector2 reduced = _mass.Solve22( rhs );
						impulse.X = reduced.X;
						impulse.Y = reduced.Y;
						impulse.Z = -_impulse.Z;
						_impulse.X += reduced.X;
						_impulse.Y += reduced.Y;
						_impulse.Z = 0.0f;
					}
					else
					{
						_impulse += impulse;
					}
				}

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

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

				vB += mB * P;
				wB += iB * ( MathUtils.cross( _rB, P ) + impulse.Z );
			}
			else
			{
				// Solve point-to-point constraint
				Vector2 Cdot = vB + MathUtils.cross( wB, _rB ) - vA - MathUtils.cross( wA, _rA );
				Vector2 impulse = _mass.Solve22( -Cdot );

				_impulse.X += impulse.X;
				_impulse.Y += impulse.Y;

				vA -= mA * impulse;
				wA -= iA * MathUtils.cross( _rA, impulse );

				vB += mB * impulse;
				wB += iB * MathUtils.cross( _rB, impulse );
			}

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