public override float SolveIteration()
{
#region Theory
//lambda = -mc * (Jv + b)
// PraT = [ bx by bz ] * [ 0 raz -ray ] = [ (-by * raz + bz * ray) (bx * raz - bz * rax) (-bx * ray + by * rax) ]
// [ cx cy cz ] [ -raz 0 rax ] [ (-cy * raz + cz * ray) (cx * raz - cz * rax) (-cx * ray + cy * rax) ]
// [ ray -rax 0 ]
//
// PrbT = [ bx by bz ] * [ 0 rbz -rby ] = [ (-by * rbz + bz * rby) (bx * rbz - bz * rbx) (-bx * rby + by * rbx) ]
// [ cx cy cz ] [ -rbz 0 rbx ] [ (-cy * rbz + cz * rby) (cx * rbz - cz * rbx) (-cx * rby + cy * rbx) ]
// [ rby -rbx 0 ]
// Jv = [ bx by bz PraT -bx -by -bz -Prbt ] * [ vax ]
// [ cx cy cz -cx -cy -cz ] [ vay ]
// [ vaz ]
// [ wax ]
// [ way ]
// [ waz ]
// [ vbx ]
// [ vby ]
// [ vbz ]
// [ wbx ]
// [ wby ]
// [ wbz ]
// va' = [ bx * vax + by * vay + bz * vaz ] = [ b * va ]
// [ cx * vax + cy * vay + cz * vaz ] [ c * va ]
// wa' = [ (PraT row 1) * wa ]
// [ (PraT row 2) * wa ]
// vb' = [ -bx * vbx - by * vby - bz * vbz ] = [ -b * vb ]
// [ -cx * vbx - cy * vby - cz * vbz ] [ -c * vb ]
// wb' = [ -(PrbT row 1) * wb ]
// [ -(PrbT row 2) * wb ]
// Jv = [ b * va + (PraT row 1) * wa - b * vb - (PrbT row 1) * wb ]
// [ c * va + (PraT row 2) * wa - c * vb - (PrbT row 2) * wb ]
// Jv = [ b * (va + wa x ra - vb - wb x rb) ]
// [ c * (va + wa x ra - vb - wb x rb) ]
//P = JT * lambda
#endregion
#if !WINDOWS
Vector2 lambda = new Vector2();
#else
Vector2 lambda;
#endif
//float va1, va2, wa1, wa2, vb1, vb2, wb1, wb2;
//Vector3.Dot(ref worldAxis1, ref myParentA.myInternalLinearVelocity, out va1);
//Vector3.Dot(ref worldAxis2, ref myParentA.myInternalLinearVelocity, out va2);
//wa1 = prAT.M11 * myParentA.myInternalAngularVelocity.X + prAT.M12 * myParentA.myInternalAngularVelocity.Y + prAT.M13 * myParentA.myInternalAngularVelocity.Z;
//wa2 = prAT.M21 * myParentA.myInternalAngularVelocity.X + prAT.M22 * myParentA.myInternalAngularVelocity.Y + prAT.M23 * myParentA.myInternalAngularVelocity.Z;
//Vector3.Dot(ref worldAxis1, ref myParentB.myInternalLinearVelocity, out vb1);
//Vector3.Dot(ref worldAxis2, ref myParentB.myInternalLinearVelocity, out vb2);
//wb1 = prBT.M11 * myParentB.myInternalAngularVelocity.X + prBT.M12 * myParentB.myInternalAngularVelocity.Y + prBT.M13 * myParentB.myInternalAngularVelocity.Z;
//wb2 = prBT.M21 * myParentB.myInternalAngularVelocity.X + prBT.M22 * myParentB.myInternalAngularVelocity.Y + prBT.M23 * myParentB.myInternalAngularVelocity.Z;
//lambda.X = va1 + wa1 - vb1 - wb1 + biasVelocity.X + mySoftness * accumulatedImpulse.X;
//lambda.Y = va2 + wa2 - vb2 - wb2 + biasVelocity.Y + mySoftness * accumulatedImpulse.Y;
Vector3 dv;
Vector3 aVel, bVel;
Vector3.Cross(ref connectionA.angularVelocity, ref rA, out aVel);
Vector3.Add(ref aVel, ref connectionA.linearVelocity, out aVel);
Vector3.Cross(ref connectionB.angularVelocity, ref rB, out bVel);
Vector3.Add(ref bVel, ref connectionB.linearVelocity, out bVel);
Vector3.Subtract(ref aVel, ref bVel, out dv);
Vector3.Dot(ref dv, ref worldRestrictedAxis1, out lambda.X);
Vector3.Dot(ref dv, ref worldRestrictedAxis2, out lambda.Y);
lambda.X += biasVelocity.X + softness * accumulatedImpulse.X;
lambda.Y += biasVelocity.Y + softness * accumulatedImpulse.Y;
//Convert to impulse
Matrix2x2.Transform(ref lambda, ref negativeEffectiveMassMatrix, out lambda);
Vector2.Add(ref lambda, ref accumulatedImpulse, out accumulatedImpulse);
float x = lambda.X;
float y = lambda.Y;
//Apply impulse
#if !WINDOWS
Vector3 impulse = new Vector3();
Vector3 torque= new Vector3();
#else
Vector3 impulse;
Vector3 torque;
#endif
impulse.X = worldRestrictedAxis1.X * x + worldRestrictedAxis2.X * y;
impulse.Y = worldRestrictedAxis1.Y * x + worldRestrictedAxis2.Y * y;
impulse.Z = worldRestrictedAxis1.Z * x + worldRestrictedAxis2.Z * y;
if (connectionA.isDynamic)
{
torque.X = x * angularA1.X + y * angularA2.X;
torque.Y = x * angularA1.Y + y * angularA2.Y;
torque.Z = x * angularA1.Z + y * angularA2.Z;
connectionA.ApplyLinearImpulse(ref impulse);
connectionA.ApplyAngularImpulse(ref torque);
}
if (connectionB.isDynamic)
{
impulse.X = -impulse.X;
impulse.Y = -impulse.Y;
impulse.Z = -impulse.Z;
torque.X = x * angularB1.X + y * angularB2.X;
torque.Y = x * angularB1.Y + y * angularB2.Y;
torque.Z = x * angularB1.Z + y * angularB2.Z;
connectionB.ApplyLinearImpulse(ref impulse);
connectionB.ApplyAngularImpulse(ref torque);
}
return (Math.Abs(lambda.X) + Math.Abs(lambda.Y));
}
