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,
Matrix3f linearW;
Matrix3f.CreateScale(TargetBone.inverseMass, out linearW);
Matrix3f linear;
Matrix3f.Multiply(ref linearJacobian, ref linearW, out linear); //Compute J * M^-1 for linear component
Matrix3f.MultiplyByTransposed(ref linear, ref linearJacobian, out linear); //Compute (J * M^-1) * JT for linear component
Matrix3f angular;
Matrix3f.Multiply(ref angularJacobian, ref TargetBone.inertiaTensorInverse, out angular); //Compute J * M^-1 for angular component
Matrix3f.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!
Matrix3f.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).
Matrix3f.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,
Matrix3f linearW;
Matrix3f linearA, angularA, linearB, angularB;
if (!ConnectionA.Pinned)
{
Matrix3f.CreateScale(ConnectionA.inverseMass, out linearW);
Matrix3f.Multiply(ref linearJacobianA, ref linearW, out linearA); //Compute J * M^-1 for linear component
Matrix3f.MultiplyByTransposed(ref linearA, ref linearJacobianA, out linearA); //Compute (J * M^-1) * JT for linear component
Matrix3f.Multiply(ref angularJacobianA, ref ConnectionA.inertiaTensorInverse, out angularA); //Compute J * M^-1 for angular component
Matrix3f.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 Matrix3f();
angularA = new Matrix3f();
}
if (!ConnectionB.Pinned)
{
Matrix3f.CreateScale(ConnectionB.inverseMass, out linearW);
Matrix3f.Multiply(ref linearJacobianB, ref linearW, out linearB); //Compute J * M^-1 for linear component
Matrix3f.MultiplyByTransposed(ref linearB, ref linearJacobianB, out linearB); //Compute (J * M^-1) * JT for linear component
Matrix3f.Multiply(ref angularJacobianB, ref ConnectionB.inertiaTensorInverse, out angularB); //Compute J * M^-1 for angular component
Matrix3f.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 Matrix3f();
angularB = new Matrix3f();
}
//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!
Matrix3f.Add(ref linearA, ref angularA, out effectiveMass);
Matrix3f.Add(ref effectiveMass, ref linearB, out effectiveMass);
Matrix3f.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).
Matrix3f.AdaptiveInvert(ref effectiveMass, out effectiveMass);
}