Box2DX.Dynamics.ContactSolver.SolvePositionConstraints C# (CSharp) Method

		public bool SolvePositionConstraints(float baumgarte)
		{
			float minSeparation = 0.0f;

			for (int i = 0; i < _constraintCount; ++i)
			{
				ContactConstraint c = _constraints[i];
				Body b1 = c.Body1;
				Body b2 = c.Body2;
				float invMass1 = b1._mass * b1._invMass;
				float invI1 = b1._mass * b1._invI;
				float invMass2 = b2._mass * b2._invMass;
				float invI2 = b2._mass * b2._invI;

				Vec2 normal = c.Normal;

				// Solver normal constraints
				for (int j = 0; j < c.PointCount; ++j)
				{
					ContactConstraintPoint ccp = c.Points[j];

					Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, ccp.LocalAnchor1 - b1.GetLocalCenter());
					Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, ccp.LocalAnchor2 - b2.GetLocalCenter());

					Vec2 p1 = b1._sweep.C + r1;
					Vec2 p2 = b2._sweep.C + r2;
					Vec2 dp = p2 - p1;

					// Approximate the current separation.
					float separation = Vec2.Dot(dp, normal) + ccp.Separation;

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

					// Prevent large corrections and allow slop.
					float C = baumgarte * Common.Math.Clamp(separation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);

					// Compute normal impulse
					float impulse = -ccp.EqualizedMass * C;

					Vec2 P = impulse * normal;

					b1._sweep.C -= invMass1 * P;
					b1._sweep.A -= invI1 * Vec2.Cross(r1, P);
					b1.SynchronizeTransform();

					b2._sweep.C += invMass2 * P;
					b2._sweep.A += invI2 * Vec2.Cross(r2, P);
					b2.SynchronizeTransform();
				}
			}

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

        public void SolveTOI(ref TimeStep subStep)
        {
            ContactSolver contactSolver = new ContactSolver(subStep, this._contacts, this._contactCount);

            for (int i = 0; i < subStep.VelocityIterations; i++)
            {
                contactSolver.SolveVelocityConstraints();
            }
            for (int i = 0; i < this._bodyCount; i++)
            {
                Body body = this._bodies[i];
                if (!body.IsStatic())
                {
                    body._sweep.C0 = body._sweep.C;
                    body._sweep.A0 = body._sweep.A;
                    Body expr_8D_cp_0 = body;
                    expr_8D_cp_0._sweep.C = expr_8D_cp_0._sweep.C + subStep.Dt * body._linearVelocity;
                    Body expr_B4_cp_0 = body;
                    expr_B4_cp_0._sweep.A = expr_B4_cp_0._sweep.A + subStep.Dt * body._angularVelocity;
                    body.SynchronizeTransform();
                }
            }
            float baumgarte = 0.75f;

            for (int i = 0; i < subStep.PositionIterations; i++)
            {
                bool flag = contactSolver.SolvePositionConstraints(baumgarte);
                if (flag)
                {
                    break;
                }
            }
            this.Report(contactSolver._constraints);
        }