Matrix3d rotateXYZ(const double &alpha, const double &beta, const double &gamma)
{
Matrix3d mat;
mat.setOnes(3,3);
const double cosa = cos(alpha);
const double sina = sin(alpha);
const double cosb = cos(beta);
const double sinb = sin(beta);
const double cosg = cos(gamma);
const double sing = sin(gamma);
mat(0, 0) = cosb * cosg;
mat(0, 1) = -sing * cosb;
mat(0, 2) = sinb;
mat(1, 0) = sina * sinb * cosg + cosa * sing;
mat(1, 1) = - sina * sinb *sing + cosa * cosg;
mat(1, 2) = - sina * cosb;
mat(2, 0) = - cosa * sinb * cosg + sina * sing;
mat(2, 1) = sina * sinb * sing + sina *cosg;
mat(2, 2) = cosa * cosb;
// std::cout.precision(5);
// std::cout.setf(std::ios::fixed, std::ios::floatfield);
// std::cout << "matrix is:\n\n" << mat << "\n" << std::endl;
return mat;
}
