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

		public void SolveVelocityConstraints()
		{
			for (int i = 0; i < _constraintCount; ++i)
			{
				ContactConstraint c = _constraints[i];
				Body b1 = c.Body1;
				Body b2 = c.Body2;
				float w1 = b1._angularVelocity;
				float w2 = b2._angularVelocity;
				Vec2 v1 = b1._linearVelocity;
				Vec2 v2 = b2._linearVelocity;
				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);
				float friction = c.Friction;

				Box2DXDebug.Assert(c.PointCount == 1 || c.PointCount == 2);

				// Solve normal constraints
				if (c.PointCount == 1)
				{
					ContactConstraintPoint ccp = c.Points[0];

					// Relative velocity at contact
					Vec2 dv = v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1);

					// Compute normal impulse
					float vn = Vec2.Dot(dv, normal);
					float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias);

					// Clamp the accumulated impulse
					float newImpulse = Common.Math.Max(ccp.NormalImpulse + lambda, 0.0f);
					lambda = newImpulse - ccp.NormalImpulse;

					// Apply contact impulse
					Vec2 P = lambda * normal;

					v1 -= invMass1 * P;
					w1 -= invI1 * Vec2.Cross(ccp.R1, P);

					v2 += invMass2 * P;
					w2 += invI2 * Vec2.Cross(ccp.R2, P);

					ccp.NormalImpulse = newImpulse;
				}
				else
				{
					// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
					// Build the mini LCP for this contact patch
					//
					// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
					//
					// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
					// b = vn_0 - velocityBias
					//
					// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
					// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
					// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
					// solution that satisfies the problem is chosen.
					// 
					// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
					// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
					//
					// Substitute:
					// 
					// x = x' - a
					// 
					// Plug into above equation:
					//
					// vn = A * x + b
					//    = A * (x' - a) + b
					//    = A * x' + b - A * a
					//    = A * x' + b'
					// b' = b - A * a;

					ContactConstraintPoint cp1 = c.Points[0];
					ContactConstraintPoint cp2 = c.Points[1];

					Vec2 a = new Vec2(cp1.NormalImpulse, cp2.NormalImpulse);
					Box2DXDebug.Assert(a.X >= 0.0f && a.Y >= 0.0f);

					// Relative velocity at contact
					Vec2 dv1 = v2 + Vec2.Cross(w2, cp1.R2) - v1 - Vec2.Cross(w1, cp1.R1);
					Vec2 dv2 = v2 + Vec2.Cross(w2, cp2.R2) - v1 - Vec2.Cross(w1, cp2.R1);

					// Compute normal velocity
					float vn1 = Vec2.Dot(dv1, normal);
					float vn2 = Vec2.Dot(dv2, normal);

					Vec2 b;
					b.X = vn1 - cp1.VelocityBias;
					b.Y = vn2 - cp2.VelocityBias;
					b -= Common.Math.Mul(c.K, a);

					const float k_errorTol = 1e-3f;
					for (; ; )
					{
						//
						// Case 1: vn = 0
						//
						// 0 = A * x' + b'
						//
						// Solve for x':
						//
						// x' = - inv(A) * b'
						//
						Vec2 x = -Common.Math.Mul(c.NormalMass, b);

						if (x.X >= 0.0f && x.Y >= 0.0f)
						{
							// Resubstitute for the incremental impulse
							Vec2 d = x - a;

							// Apply incremental impulse
							Vec2 P1 = d.X * normal;
							Vec2 P2 = d.Y * normal;
							v1 -= invMass1 * (P1 + P2);
							w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));

							v2 += invMass2 * (P1 + P2);
							w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));

							// Accumulate
							cp1.NormalImpulse = x.X;
							cp2.NormalImpulse = x.Y;

#if B2_DEBUG_SOLVER
							// Postconditions
							dv1 = v2 + Vector2.Cross(w2, cp1.R2) - v1 - Vector2.Cross(w1, cp1.R1);
							dv2 = v2 + Vector2.Cross(w2, cp2.R2) - v1 - Vector2.Cross(w1, cp2.R1);

							// Compute normal velocity
							vn1 = Vector2.Dot(dv1, normal);
							vn2 = Vector2.Dot(dv2, normal);

							Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
							Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol);
#endif
							break;
						}

						//
						// Case 2: vn1 = 0 and x2 = 0
						//
						//   0 = a11 * x1' + a12 * 0 + b1' 
						// vn2 = a21 * x1' + a22 * 0 + b2'
						//
						x.X = -cp1.NormalMass * b.X;
						x.Y = 0.0f;
						vn1 = 0.0f;
						vn2 = c.K.Col1.Y * x.X + b.Y;

						if (x.X >= 0.0f && vn2 >= 0.0f)
						{
							// Resubstitute for the incremental impulse
							Vec2 d = x - a;

							// Apply incremental impulse
							Vec2 P1 = d.X * normal;
							Vec2 P2 = d.Y * normal;
							v1 -= invMass1 * (P1 + P2);
							w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));

							v2 += invMass2 * (P1 + P2);
							w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));

							// Accumulate
							cp1.NormalImpulse = x.X;
							cp2.NormalImpulse = x.Y;

#if B2_DEBUG_SOLVER
							// Postconditions
							dv1 = v2 + Vector2.Cross(w2, cp1.R2) - v1 - Vector2.Cross(w1, cp1.R1);

							// Compute normal velocity
							vn1 = Vector2.Dot(dv1, normal);

							Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
#endif
							break;
						}


						//
						// Case 3: w2 = 0 and x1 = 0
						//
						// vn1 = a11 * 0 + a12 * x2' + b1' 
						//   0 = a21 * 0 + a22 * x2' + b2'
						//
						x.X = 0.0f;
						x.Y = -cp2.NormalMass * b.Y;
						vn1 = c.K.Col2.X * x.Y + b.X;
						vn2 = 0.0f;

						if (x.Y >= 0.0f && vn1 >= 0.0f)
						{
							// Resubstitute for the incremental impulse
							Vec2 d = x - a;

							// Apply incremental impulse
							Vec2 P1 = d.X * normal;
							Vec2 P2 = d.Y * normal;
							v1 -= invMass1 * (P1 + P2);
							w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));

							v2 += invMass2 * (P1 + P2);
							w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));

							// Accumulate
							cp1.NormalImpulse = x.X;
							cp2.NormalImpulse = x.Y;

#if B2_DEBUG_SOLVER
							// Postconditions
							dv2 = v2 + Vector2.Cross(w2, cp2.R2) - v1 - Vector2.Cross(w1, cp2.R1);

							// Compute normal velocity
							vn2 = Vector2.Dot(dv2, normal);

							Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol);
#endif
							break;
						}

						//
						// Case 4: x1 = 0 and x2 = 0
						// 
						// vn1 = b1
						// vn2 = b2;
						x.X = 0.0f;
						x.Y = 0.0f;
						vn1 = b.X;
						vn2 = b.Y;

						if (vn1 >= 0.0f && vn2 >= 0.0f)
						{
							// Resubstitute for the incremental impulse
							Vec2 d = x - a;

							// Apply incremental impulse
							Vec2 P1 = d.X * normal;
							Vec2 P2 = d.Y * normal;
							v1 -= invMass1 * (P1 + P2);
							w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));

							v2 += invMass2 * (P1 + P2);
							w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));

							// Accumulate
							cp1.NormalImpulse = x.X;
							cp2.NormalImpulse = x.Y;

							break;
						}

						// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
						break;
					}
				}

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

					// Relative velocity at contact
					Vec2 dv = v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1);

					// Compute tangent force
					float vt = Vec2.Dot(dv, tangent);
					float lambda = ccp.TangentMass * (-vt);

					// Clamp the accumulated force
					float maxFriction = friction * ccp.NormalImpulse;
					float newImpulse = Common.Math.Clamp(ccp.TangentImpulse + lambda, -maxFriction, maxFriction);
					lambda = newImpulse - ccp.TangentImpulse;

					// Apply contact impulse
					Vec2 P = lambda * tangent;

					v1 -= invMass1 * P;
					w1 -= invI1 * Vec2.Cross(ccp.R1, P);

					v2 += invMass2 * P;
					w2 += invI2 * Vec2.Cross(ccp.R2, P);

					ccp.TangentImpulse = newImpulse;
				}

				b1._linearVelocity = v1;
				b1._angularVelocity = w1;
				b2._linearVelocity = v2;
				b2._angularVelocity = w2;
			}
		}

		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);
					}
				}