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);
}
