FarseerPhysics.Dynamics.Contacts.ContactSolver.initializeVelocityConstraints C# (CSharp) Method

		public void initializeVelocityConstraints()
		{
			for( int i = 0; i < _count; ++i )
			{
				ContactVelocityConstraint vc = _velocityConstraints[i];
				ContactPositionConstraint pc = _positionConstraints[i];

				float radiusA = pc.radiusA;
				float radiusB = pc.radiusB;
				Manifold manifold = _contacts[vc.contactIndex].manifold;

				int indexA = vc.indexA;
				int indexB = vc.indexB;

				float mA = vc.invMassA;
				float mB = vc.invMassB;
				float iA = vc.invIA;
				float iB = vc.invIB;
				Vector2 localCenterA = pc.localCenterA;
				Vector2 localCenterB = pc.localCenterB;

				Vector2 cA = _positions[indexA].c;
				float aA = _positions[indexA].a;
				Vector2 vA = _velocities[indexA].v;
				float wA = _velocities[indexA].w;

				Vector2 cB = _positions[indexB].c;
				float aB = _positions[indexB].a;
				Vector2 vB = _velocities[indexB].v;
				float wB = _velocities[indexB].w;

				Debug.Assert( manifold.pointCount > 0 );

				Transform xfA = new Transform();
				Transform xfB = new Transform();
				xfA.q.Set( aA );
				xfB.q.Set( aB );
				xfA.p = cA - MathUtils.mul( xfA.q, localCenterA );
				xfB.p = cB - MathUtils.mul( xfB.q, localCenterB );

				Vector2 normal;
				FixedArray2<Vector2> points;
				WorldManifold.initialize( ref manifold, ref xfA, radiusA, ref xfB, radiusB, out normal, out points );

				vc.normal = normal;

				int pointCount = vc.pointCount;
				for( int j = 0; j < pointCount; ++j )
				{
					VelocityConstraintPoint vcp = vc.points[j];

					vcp.rA = points[j] - cA;
					vcp.rB = points[j] - cB;

					float rnA = MathUtils.cross( vcp.rA, vc.normal );
					float rnB = MathUtils.cross( vcp.rB, vc.normal );

					float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;

					vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;

					Vector2 tangent = MathUtils.cross( vc.normal, 1.0f );

					float rtA = MathUtils.cross( vcp.rA, tangent );
					float rtB = MathUtils.cross( vcp.rB, tangent );

					float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;

					vcp.tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;

					// Setup a velocity bias for restitution.
					vcp.velocityBias = 0.0f;
					float vRel = Vector2.Dot( vc.normal, vB + MathUtils.cross( wB, vcp.rB ) - vA - MathUtils.cross( wA, vcp.rA ) );
					if( vRel < -Settings.velocityThreshold )
					{
						vcp.velocityBias = -vc.restitution * vRel;
					}
				}

				// If we have two points, then prepare the block solver.
				if( vc.pointCount == 2 )
				{
					VelocityConstraintPoint vcp1 = vc.points[0];
					VelocityConstraintPoint vcp2 = vc.points[1];

					float rn1A = MathUtils.cross( vcp1.rA, vc.normal );
					float rn1B = MathUtils.cross( vcp1.rB, vc.normal );
					float rn2A = MathUtils.cross( vcp2.rA, vc.normal );
					float rn2B = MathUtils.cross( vcp2.rB, vc.normal );

					float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
					float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
					float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;

					// Ensure a reasonable condition number.
					const float k_maxConditionNumber = 1000.0f;
					if( k11 * k11 < k_maxConditionNumber * ( k11 * k22 - k12 * k12 ) )
					{
						// K is safe to invert.
						vc.K.ex = new Vector2( k11, k12 );
						vc.K.ey = new Vector2( k12, k22 );
						vc.normalMass = vc.K.Inverse;
					}
					else
					{
						// The constraints are redundant, just use one.
						// TODO_ERIN use deepest?
						vc.pointCount = 1;
					}
				}
			}
		}