Project290.Physics.Dynamics.Body.SynchronizeTransform C# (CSharp) Method

        internal void SynchronizeTransform()
        {
            Xf.R.Set(Sweep.A);

            float vx = Xf.R.Col1.X*Sweep.LocalCenter.X + Xf.R.Col2.X*Sweep.LocalCenter.Y;
            float vy = Xf.R.Col1.Y*Sweep.LocalCenter.X + Xf.R.Col2.Y*Sweep.LocalCenter.Y;

            Xf.Position.X = Sweep.C.X - vx;
            Xf.Position.Y = Sweep.C.Y - vy;
        }

        internal void SolveTOI(ref TimeStep subStep)
        {
            _contactSolver.Reset(_contacts, ContactCount, subStep.dtRatio, false);

            // Solve position constraints.
            const float kTOIBaumgarte = 0.75f;

            for (int i = 0; i < Settings.TOIPositionIterations; ++i)
            {
                bool contactsOkay = _contactSolver.SolvePositionConstraintsTOI(kTOIBaumgarte);
                if (contactsOkay)
                {
                    break;
                }

                if (i == Settings.TOIPositionIterations - 1)
                {
                    i += 0;
                }
            }

            // Leap of faith to new safe state.
            for (int i = 0; i < BodyCount; ++i)
            {
                Body body = Bodies[i];
                body.Sweep.A0 = body.Sweep.A;
                body.Sweep.C0 = body.Sweep.C;
            }

            // 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.

            // Integrate positions.
            for (int i = 0; i < BodyCount; ++i)
            {
                Body b = Bodies[i];

                if (b.BodyType == BodyType.Static)
                {
                    continue;
                }

                // Check for large velocities.
                float translationx = subStep.dt * b.LinearVelocityInternal.X;
                float translationy = subStep.dt * b.LinearVelocityInternal.Y;
                float dot          = translationx * translationx + translationy * translationy;
                if (dot > Settings.MaxTranslationSquared)
                {
                    float norm  = 1f / (float)Math.Sqrt(dot);
                    float value = Settings.MaxTranslation * subStep.inv_dt;
                    b.LinearVelocityInternal.X = value * (translationx * norm);
                    b.LinearVelocityInternal.Y = value * (translationy * norm);
                }

                float rotation = subStep.dt * b.AngularVelocity;
                if (rotation * rotation > Settings.MaxRotationSquared)
                {
                    if (rotation < 0.0)
                    {
                        b.AngularVelocityInternal = -subStep.inv_dt * Settings.MaxRotation;
                    }
                    else
                    {
                        b.AngularVelocityInternal = subStep.inv_dt * Settings.MaxRotation;
                    }
                }

                // Integrate
                b.Sweep.C.X += subStep.dt * b.LinearVelocityInternal.X;
                b.Sweep.C.Y += subStep.dt * b.LinearVelocityInternal.Y;
                b.Sweep.A   += subStep.dt * b.AngularVelocityInternal;

                // Compute new transform
                b.SynchronizeTransform();

                // Note: shapes are synchronized later.
            }

            Report(_contactSolver.Constraints);
        }