public static void SetInertia(this Link.Inertial.Inertia inertia, Rigidbody rigidbody)
{
Evd <float> Evd = inertia.Unity3DCoordTrafo().ToMatrix().Evd(Symmetricity.Symmetric);
rigidbody.inertiaTensor = Evd.EigenValues.GetReal().ToVector3().FixMinInertia(); // optionally check vector for imaginary part = 0
rigidbody.inertiaTensorRotation = Evd.EigenVectors.ToQuaternion(); // optionally check matrix for determinant = 1
}
private static Matrix ToMatrix(this Link.Inertial.Inertia inertia)
{
return(DenseMatrix.OfArray(
new float[, ] {
{ (float)inertia.ixx, (float)inertia.ixy, (float)inertia.ixz },
{ (float)inertia.ixy, (float)inertia.iyy, (float)inertia.iyz },
{ (float)inertia.ixz, (float)inertia.iyz, (float)inertia.izz }
}
));
}
public static void SetInertia(this Link.Inertial.Inertia inertia, Rigidbody rigidbody)
{
// todo: diagonalize matrix and convert transform matrix (i.e. rotation matrix) to quaternion
float ixx = (float)((inertia.ixx > imin) ? inertia.ixx : imin);
float iyy = (float)((inertia.iyy > imin) ? inertia.iyy : imin);
float izz = (float)((inertia.izz > imin) ? inertia.izz : imin);
rigidbody.inertiaTensor = new Vector3(ixx, iyy, izz);
// rigidbody.inertiaTensorRotation // todo: set inertiaRotation here
}
private static Link.Inertial.Inertia Unity3DCoordTrafo(this Link.Inertial.Inertia inertia)
{
Link.Inertial.Inertia unity3DInertia = inertia;
unity3DInertia.ixx = inertia.iyy;
unity3DInertia.iyy = inertia.izz;
unity3DInertia.izz = inertia.ixx;
unity3DInertia.ixy = inertia.iyz;
unity3DInertia.ixz = inertia.ixy;
unity3DInertia.iyz = inertia.ixz;
return(unity3DInertia);
}
