public override void ExclusiveUpdate()
{
//Warm starting
#if !WINDOWS
Vector3 linear = new Vector3();
Vector3 angular = new Vector3();
#else
Vector3 linear, angular;
#endif
//Matrix2x3.Transform(ref lambda, ref linearA, out linear);
linear.X = accumulatedImpulse.X * linearA.M11 + accumulatedImpulse.Y * linearA.M21;
linear.Y = accumulatedImpulse.X * linearA.M12 + accumulatedImpulse.Y * linearA.M22;
linear.Z = accumulatedImpulse.X * linearA.M13 + accumulatedImpulse.Y * linearA.M23;
if (entityADynamic)
{
//Matrix2x3.Transform(ref lambda, ref angularA, out angular);
angular.X = accumulatedImpulse.X * angularA.M11 + accumulatedImpulse.Y * angularA.M21;
angular.Y = accumulatedImpulse.X * angularA.M12 + accumulatedImpulse.Y * angularA.M22;
angular.Z = accumulatedImpulse.X * angularA.M13 + accumulatedImpulse.Y * angularA.M23;
entityA.ApplyLinearImpulse(ref linear);
entityA.ApplyAngularImpulse(ref angular);
}
if (entityBDynamic)
{
linear.X = -linear.X;
linear.Y = -linear.Y;
linear.Z = -linear.Z;
//Matrix2x3.Transform(ref lambda, ref angularB, out angular);
angular.X = accumulatedImpulse.X * angularB.M11 + accumulatedImpulse.Y * angularB.M21;
angular.Y = accumulatedImpulse.X * angularB.M12 + accumulatedImpulse.Y * angularB.M22;
angular.Z = accumulatedImpulse.X * angularB.M13 + accumulatedImpulse.Y * angularB.M23;
entityB.ApplyLinearImpulse(ref linear);
entityB.ApplyAngularImpulse(ref angular);
}
}