public static void RotateInertia(ref Triangular3x3 localInertiaTensor, ref Quaternion orientation, out Triangular3x3 rotatedInertiaTensor)
{
Matrix3x3.CreateFromQuaternion(ref orientation, out var orientationMatrix);
//I^-1 = RT * Ilocal^-1 * R
//NOTE: If you were willing to confuse users a little bit, the local inertia could be required to be diagonal.
//This would be totally fine for all the primitive types which happen to have diagonal inertias, but for more complex shapes (convex hulls, meshes),
//there would need to be a reorientation step. That could be confusing, and it's probably not worth it.
Triangular3x3.RotationSandwich(ref orientationMatrix, ref localInertiaTensor, out rotatedInertiaTensor);
}
public static void Scale(ref Triangular3x3 m, float scale, out Triangular3x3 scaled)
{
scaled.M11 = m.M11 * scale;
scaled.M21 = m.M21 * scale;
scaled.M22 = m.M22 * scale;
scaled.M31 = m.M31 * scale;
scaled.M32 = m.M32 * scale;
scaled.M33 = m.M33 * scale;
}
public unsafe static void SymmetricInvert(ref Triangular3x3 m, out Triangular3x3 inverse)
{
var m11 = m.M22 * m.M33 - m.M32 * m.M32;
var m21 = m.M32 * m.M31 - m.M33 * m.M21;
var m31 = m.M21 * m.M32 - m.M31 * m.M22;
var determinantInverse = 1f / (m11 * m.M11 + m21 * m.M21 + m31 * m.M31);
var m22 = m.M33 * m.M11 - m.M31 * m.M31;
var m32 = m.M31 * m.M21 - m.M11 * m.M32;
var m33 = m.M11 * m.M22 - m.M21 * m.M21;
inverse.M11 = m11 * determinantInverse;
inverse.M21 = m21 * determinantInverse;
inverse.M31 = m31 * determinantInverse;
inverse.M22 = m22 * determinantInverse;
inverse.M32 = m32 * determinantInverse;
inverse.M33 = m33 * determinantInverse;
}
public static void RotationSandwich(ref Matrix3x3 r, ref Triangular3x3 m, out Triangular3x3 sandwich)
{
//TODO: We just copied this from the wide implementation. There are a lot of ways to improve this, should it be necessary.
//(There's virtually no chance that optimizing this to a serious degree would be worth it- at the time of writing, it's only called by the pose integrator, which is
//horribly memory bound anyway.)
var i11 = r.X.X * m.M11 + r.Y.X * m.M21 + r.Z.X * m.M31;
var i12 = r.X.X * m.M21 + r.Y.X * m.M22 + r.Z.X * m.M32;
var i13 = r.X.X * m.M31 + r.Y.X * m.M32 + r.Z.X * m.M33;
var i21 = r.X.Y * m.M11 + r.Y.Y * m.M21 + r.Z.Y * m.M31;
var i22 = r.X.Y * m.M21 + r.Y.Y * m.M22 + r.Z.Y * m.M32;
var i23 = r.X.Y * m.M31 + r.Y.Y * m.M32 + r.Z.Y * m.M33;
var i31 = r.X.Z * m.M11 + r.Y.Z * m.M21 + r.Z.Z * m.M31;
var i32 = r.X.Z * m.M21 + r.Y.Z * m.M22 + r.Z.Z * m.M32;
var i33 = r.X.Z * m.M31 + r.Y.Z * m.M32 + r.Z.Z * m.M33;
sandwich.M11 = i11 * r.X.X + i12 * r.Y.X + i13 * r.Z.X;
sandwich.M21 = i21 * r.X.X + i22 * r.Y.X + i23 * r.Z.X;
sandwich.M22 = i21 * r.X.Y + i22 * r.Y.Y + i23 * r.Z.Y;
sandwich.M31 = i31 * r.X.X + i32 * r.Y.X + i33 * r.Z.X;
sandwich.M32 = i31 * r.X.Y + i32 * r.Y.Y + i33 * r.Z.Y;
sandwich.M33 = i31 * r.X.Z + i32 * r.Y.Z + i33 * r.Z.Z;
}