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

		internal unsafe void ContactSolverSetup(Manifold manifold, WorldManifold worldManifold, ContactConstraint cc) 
		{
			// this is kind of yucky but we do know these were setup before entry to this method
			var bodyA = cc.BodyA;
			var bodyB = cc.BodyB;
				
			Vector2 vA = bodyA._linearVelocity;
			Vector2 vB = bodyB._linearVelocity;
			float wA = bodyA._angularVelocity;
			float wB = bodyB._angularVelocity;
			
			fixed (ContactConstraintPoint* ccPointsPtr = cc.Points)
			{
				for (int j = 0; j < cc.PointCount; ++j)
				{
					ManifoldPoint cp = manifold.Points[j];
					ContactConstraintPoint* ccp = &ccPointsPtr[j];

					ccp->NormalImpulse = cp.NormalImpulse;
					ccp->TangentImpulse = cp.TangentImpulse;

					ccp->LocalPoint = cp.LocalPoint;

					ccp->RA = worldManifold.Points[j] - bodyA._sweep.C;
					ccp->RB = worldManifold.Points[j] - bodyB._sweep.C;

					float rnA = ccp->RA.Cross(cc.Normal);
					float rnB = ccp->RB.Cross(cc.Normal);
					rnA *= rnA;
					rnB *= rnB;

					float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;

					Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON);
					ccp->NormalMass = 1.0f / kNormal;

					float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass;
					kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB;

					Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON);
					ccp->EqualizedMass = 1.0f / kEqualized;

					Vector2 tangent = cc.Normal.CrossScalarPostMultiply(1.0f);

					float rtA = ccp->RA.Cross(tangent);
					float rtB = ccp->RB.Cross(tangent);
					rtA *= rtA;
					rtB *= rtB;

					float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;

					Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
					ccp->TangentMass = 1.0f / kTangent;

					// Setup a velocity bias for restitution.
					ccp->VelocityBias = 0.0f;
					float vRel = Vector2.Dot(cc.Normal, vB + ccp->RB.CrossScalarPreMultiply(wB) - vA - ccp->RA.CrossScalarPreMultiply(wA));
					if (vRel < -Common.Settings.VelocityThreshold)
					{
						ccp->VelocityBias = -cc.Restitution * vRel;
					}
				}

				// If we have two points, then prepare the block solver.
				if (cc.PointCount == 2)
				{
					ContactConstraintPoint* ccp1 = &ccPointsPtr[0];
					ContactConstraintPoint* ccp2 = &ccPointsPtr[1];

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

					float rn1A = ccp1->RA.Cross(cc.Normal);
					float rn1B = ccp1->RB.Cross(cc.Normal);
					float rn2A = ccp2->RA.Cross(cc.Normal);
					float rn2B = ccp2->RB.Cross(cc.Normal);

					float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
					float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
					float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;

					// Ensure a reasonable condition number.
					const float k_maxConditionNumber = 100.0f;
					if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
					{
						// K is safe to invert.
						cc.K.Col1 = new Vector2(k11, k12);
						cc.K.Col2 = new Vector2(k12, k22);
						cc.NormalMass = cc.K.GetInverse();
					}
					else
					{
						// The constraints are redundant, just use one.
						// TODO_ERIN use deepest?
						cc.PointCount = 1;
					}
				}
			}
		}
#else