public void AdaptiveInvert()
{
var maxDelta = 0.001m;
var deltas = new List <decimal>();
// Scalability and edge cases
foreach (var m in testCases)
{
Matrix3x3 testCase = m;
FloatMatrix3x3 floatMatrix = MathConverter.Convert(testCase);
FloatMatrix3x3 expected;
FloatMatrix3x3.AdaptiveInvert(ref floatMatrix, out expected);
Matrix3x3 actual;
Matrix3x3.AdaptiveInvert(ref testCase, out actual);
bool success = true;
foreach (decimal delta in GetDeltas(expected, actual))
{
deltas.Add(delta);
success &= delta <= maxDelta;
}
Assert.True(success, string.Format("Precision: Matrix3x3Invert({0}): Expected {1} Actual {2}", testCase, expected, actual));
}
output.WriteLine("Max error: {0} ({1} times precision)", deltas.Max(), deltas.Max() / Fix64.Precision);
output.WriteLine("Average precision: {0} ({1} times precision)", deltas.Average(), deltas.Average() / Fix64.Precision);
}
public void BenchmarkAdaptiveInvert()
{
var swf = new Stopwatch();
var swd = new Stopwatch();
var deltas = new List <decimal>();
foreach (var m in testCases)
{
Matrix3x3 testCase = m;
for (int i = 0; i < 10000; i++)
{
FloatMatrix3x3 floatMatrix = MathConverter.Convert(testCase);
FloatMatrix3x3 expected;
swf.Start();
FloatMatrix3x3.AdaptiveInvert(ref floatMatrix, out expected);
swf.Stop();
Matrix3x3 actual;
swd.Start();
Matrix3x3.AdaptiveInvert(ref testCase, out actual);
swd.Stop();
foreach (decimal delta in GetDeltas(expected, actual))
{
deltas.Add(delta);
}
}
}
output.WriteLine("Max error: {0} ({1} times precision)", deltas.Max(), deltas.Max() / Fix64.Precision);
output.WriteLine("Average precision: {0} ({1} times precision)", deltas.Average(), deltas.Average() / Fix64.Precision);
output.WriteLine("Fix64.AdaptiveInvert time = {0}ms, float.AdaptiveInvert time = {1}ms", swf.ElapsedMilliseconds, swd.ElapsedMilliseconds);
}
protected internal override void ComputeEffectiveMass()
{
//For all constraints, the effective mass matrix is 1 / (J * M^-1 * JT).
//For single bone constraints, J has 2 3x3 matrices. M^-1 (W below) is a 6x6 matrix with 2 3x3 block diagonal matrices.
//To compute the whole denominator,
Matrix3x3 linearW;
Matrix3x3.CreateScale(TargetBone.inverseMass, out linearW);
Matrix3x3 linear;
Matrix3x3.Multiply(ref linearJacobian, ref linearW, out linear); //Compute J * M^-1 for linear component
Matrix3x3.MultiplyByTransposed(ref linear, ref linearJacobian,
out linear); //Compute (J * M^-1) * JT for linear component
Matrix3x3 angular;
Matrix3x3.Multiply(ref angularJacobian, ref TargetBone.inertiaTensorInverse,
out angular); //Compute J * M^-1 for angular component
Matrix3x3.MultiplyByTransposed(ref angular, ref angularJacobian,
out angular); //Compute (J * M^-1) * JT for angular component
//A nice side effect of the block diagonal nature of M^-1 is that the above separated components are now combined into the complete denominator matrix by addition!
Matrix3x3.Add(ref linear, ref angular, out effectiveMass);
//Incorporate the constraint softness into the effective mass denominator. This pushes the matrix away from singularity.
//Softness will also be incorporated into the velocity solve iterations to complete the implementation.
if (effectiveMass.M11 != 0)
{
effectiveMass.M11 += softness;
}
if (effectiveMass.M22 != 0)
{
effectiveMass.M22 += softness;
}
if (effectiveMass.M33 != 0)
{
effectiveMass.M33 += softness;
}
//Invert! Takes us from J * M^-1 * JT to 1 / (J * M^-1 * JT).
Matrix3x3.AdaptiveInvert(ref effectiveMass, out effectiveMass);
}
protected internal override void ComputeEffectiveMass()
{
//For all constraints, the effective mass matrix is 1 / (J * M^-1 * JT).
//For two bone constraints, J has 4 3x3 matrices. M^-1 (W below) is a 12x12 matrix with 4 3x3 block diagonal matrices.
//To compute the whole denominator,
Matrix3x3 linearW;
Matrix3x3 linearA, angularA, linearB, angularB;
if (!ConnectionA.Pinned)
{
Matrix3x3.CreateScale(ConnectionA.inverseMass, out linearW);
Matrix3x3.Multiply(ref linearJacobianA, ref linearW, out linearA); //Compute J * M^-1 for linear component
Matrix3x3.MultiplyByTransposed(ref linearA, ref linearJacobianA, out linearA); //Compute (J * M^-1) * JT for linear component
Matrix3x3.Multiply(ref angularJacobianA, ref ConnectionA.inertiaTensorInverse, out angularA); //Compute J * M^-1 for angular component
Matrix3x3.MultiplyByTransposed(ref angularA, ref angularJacobianA, out angularA); //Compute (J * M^-1) * JT for angular component
}
else
{
//Treat pinned bones as if they have infinite inertia.
linearA = new Matrix3x3();
angularA = new Matrix3x3();
}
if (!ConnectionB.Pinned)
{
Matrix3x3.CreateScale(ConnectionB.inverseMass, out linearW);
Matrix3x3.Multiply(ref linearJacobianB, ref linearW, out linearB); //Compute J * M^-1 for linear component
Matrix3x3.MultiplyByTransposed(ref linearB, ref linearJacobianB, out linearB); //Compute (J * M^-1) * JT for linear component
Matrix3x3.Multiply(ref angularJacobianB, ref ConnectionB.inertiaTensorInverse, out angularB); //Compute J * M^-1 for angular component
Matrix3x3.MultiplyByTransposed(ref angularB, ref angularJacobianB, out angularB); //Compute (J * M^-1) * JT for angular component
}
else
{
//Treat pinned bones as if they have infinite inertia.
linearB = new Matrix3x3();
angularB = new Matrix3x3();
}
//A nice side effect of the block diagonal nature of M^-1 is that the above separated components are now combined into the complete denominator matrix by addition!
Matrix3x3.Add(ref linearA, ref angularA, out effectiveMass);
Matrix3x3.Add(ref effectiveMass, ref linearB, out effectiveMass);
Matrix3x3.Add(ref effectiveMass, ref angularB, out effectiveMass);
//Incorporate the constraint softness into the effective mass denominator. This pushes the matrix away from singularity.
//Softness will also be incorporated into the velocity solve iterations to complete the implementation.
if (effectiveMass.M11 != 0)
{
effectiveMass.M11 += softness;
}
if (effectiveMass.M22 != 0)
{
effectiveMass.M22 += softness;
}
if (effectiveMass.M33 != 0)
{
effectiveMass.M33 += softness;
}
//Invert! Takes us from J * M^-1 * JT to 1 / (J * M^-1 * JT).
Matrix3x3.AdaptiveInvert(ref effectiveMass, out effectiveMass);
}
