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

		internal override bool solvePositionConstraints( ref SolverData data )
		{
			Vector2 cA = data.positions[_indexA].c;
			float aA = data.positions[_indexA].a;
			Vector2 cB = data.positions[_indexB].c;
			float aB = data.positions[_indexB].a;

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

			float angularError = 0.0f;
			float positionError;

			bool fixedRotation = ( _invIA + _invIB == 0.0f );

			// Solve angular limit constraint.
			if( _enableLimit && _limitState != LimitState.Inactive && fixedRotation == false )
			{
				float angle = aB - aA - referenceAngle;
				float limitImpulse = 0.0f;

				if( _limitState == LimitState.Equal )
				{
					// Prevent large angular corrections
					float C = MathUtils.clamp( angle - _lowerAngle, -Settings.maxAngularCorrection, Settings.maxAngularCorrection );
					limitImpulse = -_motorMass * C;
					angularError = Math.Abs( C );
				}
				else if( _limitState == LimitState.AtLower )
				{
					float C = angle - _lowerAngle;
					angularError = -C;

					// Prevent large angular corrections and allow some slop.
					C = MathUtils.clamp( C + Settings.angularSlop, -Settings.maxAngularCorrection, 0.0f );
					limitImpulse = -_motorMass * C;
				}
				else if( _limitState == LimitState.AtUpper )
				{
					float C = angle - _upperAngle;
					angularError = C;

					// Prevent large angular corrections and allow some slop.
					C = MathUtils.clamp( C - Settings.angularSlop, 0.0f, Settings.maxAngularCorrection );
					limitImpulse = -_motorMass * C;
				}

				aA -= _invIA * limitImpulse;
				aB += _invIB * limitImpulse;
			}

			// Solve point-to-point constraint.
			{
				qA.Set( aA );
				qB.Set( aB );
				Vector2 rA = MathUtils.mul( qA, localAnchorA - _localCenterA );
				Vector2 rB = MathUtils.mul( qB, localAnchorB - _localCenterB );

				Vector2 C = cB + rB - cA - rA;
				positionError = C.Length();

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

				Mat22 K = new Mat22();
				K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y;
				K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y;
				K.ey.X = K.ex.Y;
				K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X;

				Vector2 impulse = -K.Solve( C );

				cA -= mA * impulse;
				aA -= iA * MathUtils.cross( rA, impulse );

				cB += mB * impulse;
				aB += iB * MathUtils.cross( rB, impulse );
			}

			data.positions[_indexA].c = cA;
			data.positions[_indexA].a = aA;
			data.positions[_indexB].c = cB;
			data.positions[_indexB].a = aB;

			return positionError <= Settings.linearSlop && angularError <= Settings.angularSlop;
		}
	}