FarseerPhysics.Dynamics.Island.solveTOI C# (CSharp) Method

		internal void solveTOI( ref TimeStep subStep, int toiIndexA, int toiIndexB )
		{
			Debug.Assert( toiIndexA < BodyCount );
			Debug.Assert( toiIndexB < BodyCount );

			// Initialize the body state.
			for( int i = 0; i < BodyCount; ++i )
			{
				Body b = Bodies[i];
				_positions[i].c = b._sweep.C;
				_positions[i].a = b._sweep.A;
				_velocities[i].v = b._linearVelocity;
				_velocities[i].w = b._angularVelocity;
			}

			_contactSolver.reset( subStep, ContactCount, _contacts, _positions, _velocities );

			// Solve position constraints.
			for( int i = 0; i < Settings.toiPositionIterations; ++i )
			{
				bool contactsOkay = _contactSolver.solveTOIPositionConstraints( toiIndexA, toiIndexB );
				if( contactsOkay )
				{
					break;
				}
			}

			// Leap of faith to new safe state.
			Bodies[toiIndexA]._sweep.C0 = _positions[toiIndexA].c;
			Bodies[toiIndexA]._sweep.A0 = _positions[toiIndexA].a;
			Bodies[toiIndexB]._sweep.C0 = _positions[toiIndexB].c;
			Bodies[toiIndexB]._sweep.A0 = _positions[toiIndexB].a;

			// No warm starting is needed for TOI events because warm
			// starting impulses were applied in the discrete solver.
			_contactSolver.initializeVelocityConstraints();

			// Solve velocity constraints.
			for( int i = 0; i < Settings.toiVelocityIterations; ++i )
			{
				_contactSolver.solveVelocityConstraints();
			}

			// Don't store the TOI contact forces for warm starting
			// because they can be quite large.

			float h = subStep.dt;

			// Integrate positions.
			for( int i = 0; i < BodyCount; ++i )
			{
				Vector2 c = _positions[i].c;
				float a = _positions[i].a;
				Vector2 v = _velocities[i].v;
				float w = _velocities[i].w;

				// Check for large velocities
				Vector2 translation = h * v;
				if( Vector2.Dot( translation, translation ) > Settings.maxTranslationSquared )
				{
					float ratio = Settings.maxTranslation / translation.Length();
					v *= ratio;
				}

				float rotation = h * w;
				if( rotation * rotation > Settings.maxRotationSquared )
				{
					float ratio = Settings.maxRotation / Math.Abs( rotation );
					w *= ratio;
				}

				// Integrate
				c += h * v;
				a += h * w;

				_positions[i].c = c;
				_positions[i].a = a;
				_velocities[i].v = v;
				_velocities[i].w = w;

				// Sync bodies
				Body body = Bodies[i];
				body._sweep.C = c;
				body._sweep.A = a;
				body._linearVelocity = v;
				body._angularVelocity = w;
				body.synchronizeTransform();
			}

			report( _contactSolver._velocityConstraints );
		}