Box2D.Dynamics.Island.SolveToi C# (CSharp) Method

        public void SolveToi(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.Set(b.Sweep.C);
                Positions[i].A = b.Sweep.A;
                Velocities[i].V.Set(b.LinearVelocity);
                Velocities[i].W = b.AngularVelocity;
            }

            toiSolverDef.Contacts = Contacts;
            toiSolverDef.Count = ContactCount;
            toiSolverDef.Step = subStep;
            toiSolverDef.Positions = Positions;
            toiSolverDef.Velocities = Velocities;
            toiContactSolver.Init(toiSolverDef);

            // Solve position constraints.
            for (int i = 0; i < subStep.PositionIterations; ++i)
            {
                bool contactsOkay = toiContactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
                if (contactsOkay)
                {
                    break;
                }
            }

            // #if 0
            // // Is the new position really safe?
            // for (int i = 0; i < m_contactCount; ++i)
            // {
            // Contact* c = m_contacts[i];
            // Fixture* fA = c.GetFixtureA();
            // Fixture* fB = c.GetFixtureB();
            //
            // Body bA = fA.GetBody();
            // Body bB = fB.GetBody();
            //
            // int indexA = c.GetChildIndexA();
            // int indexB = c.GetChildIndexB();
            //
            // DistanceInput input;
            // input.proxyA.Set(fA.GetShape(), indexA);
            // input.proxyB.Set(fB.GetShape(), indexB);
            // input.transformA = bA.GetTransform();
            // input.transformB = bB.GetTransform();
            // input.useRadii = false;
            //
            // DistanceOutput output;
            // SimplexCache cache;
            // cache.count = 0;
            // Distance(&output, &cache, &input);
            //
            // if (output.distance == 0 || cache.count == 3)
            // {
            // cache.count += 0;
            // }
            // }
            // #endif

            // Leap of faith to new safe state.
            Bodies[toiIndexA].Sweep.C0.Set(Positions[toiIndexA].C);
            Bodies[toiIndexA].Sweep.A0 = Positions[toiIndexA].A;
            Bodies[toiIndexB].Sweep.C0.Set(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.
            toiContactSolver.InitializeVelocityConstraints();

            // Solve velocity constraints.
            for (int i = 0; i < subStep.VelocityIterations; ++i)
            {
                toiContactSolver.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)
            {
                Vec2 c = Positions[i].C;
                float a = Positions[i].A;
                Vec2 v = Velocities[i].V;
                float w = Velocities[i].W;

                // Check for large velocities
                translation.Set(v).MulLocal(h);
                if (Vec2.Dot(translation, translation) > Settings.MAX_TRANSLATION_SQUARED)
                {
                    float ratio = Settings.MAX_TRANSLATION / translation.Length();
                    v.MulLocal(ratio);
                }

                float rotation = h * w;
                if (rotation * rotation > Settings.MaxRotationSquared)
                {
                    float ratio = Settings.MAX_ROTATION / MathUtils.Abs(rotation);
                    w *= ratio;
                }

                // Integrate
                c.X += v.X * h;
                c.Y += v.Y * h;
                a += h * w;

                Positions[i].C.Set(c);
                Positions[i].A = a;
                Velocities[i].V.Set(v);
                Velocities[i].W = w;

                // Sync bodies
                Body body = Bodies[i];
                body.Sweep.C.Set(c);
                body.Sweep.A = a;
                body.LinearVelocity.Set(v);
                body.AngularVelocity = w;
                body.SynchronizeTransform();
            }

            Report(toiContactSolver.VelocityConstraints);
        }