public static bool IsRotMatrix(MatrixByArr rot) { // http://en.wikipedia.org/wiki/Rotation_matrix#Properties_of_a_rotation_matrix if (rot.ColSize != 3) { return(false); } if (rot.RowSize != 3) { return(false); } // R^t = R^-1 // det R = 1 { // (R-I)u = 0 where u is the null space of R-I MatrixByArr RI = rot - LinAlg.Eye(3); Vector[] eigvec; double[] eigval; NumericSolver.Eig(RI, out eigvec, out eigval); double min_eigval = eigval.HAbs().Min(); if (min_eigval > 0.0000001) { return(false); } } HDebug.ToDo("write selftest code"); return(true); }
public static Vector GetRotAxis(MatrixByArr rot) { HDebug.ToDo("write selftest code"); // http://en.wikipedia.org/wiki/Rotation_matrix#Determining_the_axis // // Determining the axis // Given a rotation matrix R, a vector u parallel to the rotation axis must satisfy // R u = u // since the rotation of u around the rotation axis must result in u. The equation // above may be solved for u which is unique up to a scalar factor. // Further, the equation may be rewritten // R u = I u => (R-I) u = 0 // which shows that u is the null space of R-I. Viewed another way, u is an eigenvector // of R corresponding to the eigenvalue λ=1(every rotation matrix must have this eigenvalue). HDebug.Assert(IsRotMatrix(rot)); MatrixByArr RI = rot - LinAlg.Eye(3); Vector[] eigvec; double[] eigval; NumericSolver.Eig(RI, out eigvec, out eigval); int idx = eigval.HAbs().HIdxMin(); Vector axis = eigvec[idx]; return(axis); }