FarseerPhysics.Dynamics.Contacts.ContactSolver.solveTOIPositionConstraints C# (CSharp) Method

		public bool solveTOIPositionConstraints( int toiIndexA, int toiIndexB )
		{
			float minSeparation = 0.0f;

			for( int i = 0; i < _count; ++i )
			{
				ContactPositionConstraint pc = _positionConstraints[i];

				int indexA = pc.indexA;
				int indexB = pc.indexB;
				Vector2 localCenterA = pc.localCenterA;
				Vector2 localCenterB = pc.localCenterB;
				int pointCount = pc.pointCount;

				float mA = 0.0f;
				float iA = 0.0f;
				if( indexA == toiIndexA || indexA == toiIndexB )
				{
					mA = pc.invMassA;
					iA = pc.invIA;
				}

				float mB = 0.0f;
				float iB = 0.0f;
				if( indexB == toiIndexA || indexB == toiIndexB )
				{
					mB = pc.invMassB;
					iB = pc.invIB;
				}

				Vector2 cA = _positions[indexA].c;
				float aA = _positions[indexA].a;

				Vector2 cB = _positions[indexB].c;
				float aB = _positions[indexB].a;

				// Solve normal constraints
				for( int j = 0; j < pointCount; ++j )
				{
					Transform xfA = new Transform();
					Transform xfB = new Transform();
					xfA.q.Set( aA );
					xfB.q.Set( aB );
					xfA.p = cA - MathUtils.mul( xfA.q, localCenterA );
					xfB.p = cB - MathUtils.mul( xfB.q, localCenterB );

					Vector2 normal;
					Vector2 point;
					float separation;

					PositionSolverManifold.initialize( pc, xfA, xfB, j, out normal, out point, out separation );

					Vector2 rA = point - cA;
					Vector2 rB = point - cB;

					// Track max constraint error.
					minSeparation = Math.Min( minSeparation, separation );

					// Prevent large corrections and allow slop.
					float C = MathUtils.clamp( Settings.baumgarte * ( separation + Settings.linearSlop ), -Settings.maxLinearCorrection, 0.0f );

					// Compute the effective mass.
					float rnA = MathUtils.cross( rA, normal );
					float rnB = MathUtils.cross( rB, normal );
					float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;

					// Compute normal impulse
					float impulse = K > 0.0f ? -C / K : 0.0f;

					Vector2 P = impulse * normal;

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

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

				_positions[indexA].c = cA;
				_positions[indexA].a = aA;

				_positions[indexB].c = cB;
				_positions[indexB].a = aB;
			}

			// We can't expect minSpeparation >= -b2_linearSlop because we don't
			// push the separation above -b2_linearSlop.
			return minSeparation >= -1.5f * Settings.linearSlop;
		}