/// <summary> /// Diagonalizes the inertia matrix. /// </summary> /// <param name="inertia">The inertia matrix.</param> /// <param name="inertiaDiagonal">The inertia of the principal axes.</param> /// <param name="rotation"> /// The rotation that rotates from principal axis space to parent/world space. /// </param> /// <remarks> /// All valid inertia matrices can be transformed into a coordinate space where all elements /// non-diagonal matrix elements are 0. The axis of this special space are the principal axes. /// </remarks> internal static void DiagonalizeInertia(Matrix33F inertia, out Vector3F inertiaDiagonal, out Matrix33F rotation) { // Alternatively we could use Jacobi transformation (iterative method, see Bullet/btMatrix3x3.diagonalize() // and Numerical Recipes book) or we could find the eigenvalues using the characteristic // polynomial which is a cubic polynomial and then solve for the eigenvectors (see Numeric // Recipes and "Mathematics for 3D Game Programming and Computer Graphics" chapter ray-tracing // for cubic equations and computation of bounding boxes. // Perform eigenvalue decomposition. var eigenValueDecomposition = new EigenvalueDecompositionF(inertia.ToMatrixF()); inertiaDiagonal = eigenValueDecomposition.RealEigenvalues.ToVector3F(); rotation = eigenValueDecomposition.V.ToMatrix33F(); if (!rotation.IsRotation) { // V is orthogonal but not necessarily a rotation. If it is no rotation // we have to swap two columns. MathHelper.Swap(ref inertiaDiagonal.Y, ref inertiaDiagonal.Z); Vector3F dummy = rotation.GetColumn(1); rotation.SetColumn(1, rotation.GetColumn(2)); rotation.SetColumn(2, dummy); Debug.Assert(rotation.IsRotation); } }
/// <summary> /// Diagonalizes the inertia matrix. /// </summary> /// <param name="inertia">The inertia matrix.</param> /// <param name="inertiaDiagonal">The inertia of the principal axes.</param> /// <param name="rotation"> /// The rotation that rotates from principal axis space to parent/world space. /// </param> /// <remarks> /// All valid inertia matrices can be transformed into a coordinate space where all elements /// non-diagonal matrix elements are 0. The axis of this special space are the principal axes. /// </remarks> internal static void DiagonalizeInertia(Matrix33F inertia, out Vector3F inertiaDiagonal, out Matrix33F rotation) { // Alternatively we could use Jacobi transformation (iterative method, see Bullet/btMatrix3x3.diagonalize() // and Numerical Recipes book) or we could find the eigenvalues using the characteristic // polynomial which is a cubic polynomial and then solve for the eigenvectors (see Numeric // Recipes and "Mathematics for 3D Game Programming and Computer Graphics" chapter ray-tracing // for cubic equations and computation of bounding boxes. // Perform eigenvalue decomposition. var eigenValueDecomposition = new EigenvalueDecompositionF(inertia.ToMatrixF()); inertiaDiagonal = eigenValueDecomposition.RealEigenvalues.ToVector3F(); rotation = eigenValueDecomposition.V.ToMatrix33F(); if (!rotation.IsRotation) { // V is orthogonal but not necessarily a rotation. If it is no rotation // we have to swap two columns. MathHelper.Swap(ref inertiaDiagonal.Y, ref inertiaDiagonal.Z); Vector3F dummy = rotation.GetColumn(1); rotation.SetColumn(1, rotation.GetColumn(2)); rotation.SetColumn(2, dummy); Debug.Assert(rotation.IsRotation); } }
public void ToMatrixF() { Matrix33F m33 = new Matrix33F(1, 2, 3, 4, 5, 6, 7, 8, 9); MatrixF m = m33.ToMatrixF(); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { Assert.AreEqual(i * 3 + j + 1, m[i, j]); } } m = m33; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { Assert.AreEqual(i * 3 + j + 1, m[i, j]); } } }
public void ToMatrixF() { Matrix33F m33 = new Matrix33F(1, 2, 3, 4, 5, 6, 7, 8, 9); MatrixF m = m33.ToMatrixF(); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) Assert.AreEqual(i * 3 + j + 1, m[i, j]); m = m33; for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) Assert.AreEqual(i * 3 + j + 1, m[i, j]); }