Box2DX.Dynamics.ContactSolver.InitVelocityConstraints C# (CSharp) Метод

		public void InitVelocityConstraints(TimeStep step)
		{
			// Warm start.
			for (int i = 0; i < _constraintCount; ++i)
			{
				ContactConstraint c = _constraints[i];

				Body b1 = c.Body1;
				Body b2 = c.Body2;
				float invMass1 = b1._invMass;
				float invI1 = b1._invI;
				float invMass2 = b2._invMass;
				float invI2 = b2._invI;
				Vec2 normal = c.Normal;
				Vec2 tangent = Vec2.Cross(normal, 1.0f);

				if (step.WarmStarting)
				{
					for (int j = 0; j < c.PointCount; ++j)
					{
						ContactConstraintPoint ccp = c.Points[j];
						ccp.NormalImpulse *= step.DtRatio;
						ccp.TangentImpulse *= step.DtRatio;
						Vec2 P = ccp.NormalImpulse * normal + ccp.TangentImpulse * tangent;
						b1._angularVelocity -= invI1 * Vec2.Cross(ccp.R1, P);
						b1._linearVelocity -= invMass1 * P;
						b2._angularVelocity += invI2 * Vec2.Cross(ccp.R2, P);
						b2._linearVelocity += invMass2 * P;
					}
				}
				else
				{
					for (int j = 0; j < c.PointCount; ++j)
					{
						ContactConstraintPoint ccp = c.Points[j];
						ccp.NormalImpulse = 0.0f;
						ccp.TangentImpulse = 0.0f;
					}
				}
			}
		}

		public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
		{
			// Integrate velocities and apply damping.
			for (int i = 0; i < _bodyCount; ++i)
			{
				Body b = _bodies[i];

				if (b.IsStatic())
					continue;

				// Integrate velocities.
				b._linearVelocity += step.Dt * (gravity + b._invMass * b._force);
				b._angularVelocity += step.Dt * b._invI * b._torque;

				// Reset forces.
				b._force.Set(0.0f, 0.0f);
				b._torque = 0.0f;

				// Apply damping.
				// ODE: dv/dt + c * v = 0
				// Solution: v(t) = v0 * exp(-c * t)
				// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
				// v2 = exp(-c * dt) * v1
				// Taylor expansion:
				// v2 = (1.0f - c * dt) * v1
				b._linearVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
				b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);

				// Check for large velocities.
#if TARGET_FLOAT32_IS_FIXED
				// Fixed point code written this way to prevent
				// overflows, float code is optimized for speed

				float vMagnitude = b._linearVelocity.Length();
				if(vMagnitude > Settings.MaxLinearVelocity)
				{
					b._linearVelocity *= Settings.MaxLinearVelocity/vMagnitude;
				}
				b._angularVelocity = Vector2.Clamp(b._angularVelocity, 
					-Settings.MaxAngularVelocity, Settings.MaxAngularVelocity);

#else
				if (Vec2.Dot(b._linearVelocity, b._linearVelocity) > Settings.MaxLinearVelocitySquared)
				{
					b._linearVelocity.Normalize();
					b._linearVelocity *= Settings.MaxLinearVelocity;
				}

				if (b._angularVelocity * b._angularVelocity > Settings.MaxAngularVelocitySquared)
				{
					if (b._angularVelocity < 0.0f)
					{
						b._angularVelocity = -Settings.MaxAngularVelocity;
					}
					else
					{
						b._angularVelocity = Settings.MaxAngularVelocity;
					}
				}
#endif
			}

			ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount);

			// Initialize velocity constraints.
			contactSolver.InitVelocityConstraints(step);

			for (int i = 0; i < _jointCount; ++i)
			{
				_joints[i].InitVelocityConstraints(step);
			}

			// Solve velocity constraints.
			for (int i = 0; i < step.VelocityIterations; ++i)
			{
				for (int j = 0; j < _jointCount; ++j)
				{
					_joints[j].SolveVelocityConstraints(step);
				}
				contactSolver.SolveVelocityConstraints();
			}

			// Post-solve (store impulses for warm starting).
			contactSolver.FinalizeVelocityConstraints();

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

				if (b.IsStatic())
					continue;

				// Store positions for continuous collision.
				b._sweep.C0 = b._sweep.C;
				b._sweep.A0 = b._sweep.A;

				// Integrate
				b._sweep.C += step.Dt * b._linearVelocity;
				b._sweep.A += step.Dt * b._angularVelocity;

				// Compute new transform
				b.SynchronizeTransform();

				// Note: shapes are synchronized later.
			}

			// Iterate over constraints.
			for (int ii = 0; ii < step.PositionIterations; ++ii)
			{
				bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
				bool jointsOkay = true;
				for (int i = 0; i < _jointCount; ++i)
				{
					bool jointOkay = _joints[i].SolvePositionConstraints(/*Settings.ContactBaumgarte*/);
					jointsOkay = jointsOkay && jointOkay;
				}

				if (contactsOkay && jointsOkay)
				{
					// Exit early if the position errors are small.
					break;
				}
			}

			Report(contactSolver._constraints);

			if (allowSleep)
			{
				float minSleepTime = Common.Settings.FLT_MAX;
#if !TARGET_FLOAT32_IS_FIXED
				float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
				float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif

				for (int i = 0; i < _bodyCount; ++i)
				{
					Body b = _bodies[i];
					if (b._invMass == 0.0f)
					{
						continue;
					}

					if ((b._flags & Body.BodyFlags.AllowSleep) == 0)
					{
						b._sleepTime = 0.0f;
						minSleepTime = 0.0f;
					}

					if ((b._flags & Body.BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
						Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
						Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
						Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
						b._angularVelocity * b._angularVelocity > angTolSqr ||
						Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
#endif
					{
						b._sleepTime = 0.0f;
						minSleepTime = 0.0f;
					}
					else
					{
						b._sleepTime += step.Dt;
						minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime);
					}
				}