Box2DX.Dynamics.ContactConstraintPoint.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 bodyA = c.BodyA;
				Body bodyB = c.BodyB;

				float invMassA = bodyA._mass * bodyA._invMass;
				float invIA = bodyA._mass * bodyA._invI;
				float invMassB = bodyB._mass * bodyB._invMass;
				float invIB = bodyB._mass * bodyB._invI;

				s_PositionSolverManifold.Initialize(c);
				Vector2 normal = s_PositionSolverManifold.Normal;

				// Solver normal constraints
				for (int j = 0; j < c.PointCount; ++j)
				{
					Vector2 point = s_PositionSolverManifold.Points[j];
					float separation = s_PositionSolverManifold.Separations[j];

					Vector2 rA = point - bodyA._sweep.C;
					Vector2 rB = point - bodyB._sweep.C;

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

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

					// Compute normal impulse
					float impulse = -c.Points[j].EqualizedMass * C;

					Vector2 P = impulse * normal;

					bodyA._sweep.C -= invMassA * P;
					bodyA._sweep.A -= invIA * rA.Cross(P);
					bodyA.SynchronizeTransform();

					bodyB._sweep.C += invMassB * P;
					bodyB._sweep.A += invIB * rB.Cross(P);
					bodyB.SynchronizeTransform();
				}
			}

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