diff --git a/Code/Mantid/Framework/Kernel/inc/MantidKernel/Quat.h b/Code/Mantid/Framework/Kernel/inc/MantidKernel/Quat.h index 7fa9744e5dd3..d56b21441d5c 100644 --- a/Code/Mantid/Framework/Kernel/inc/MantidKernel/Quat.h +++ b/Code/Mantid/Framework/Kernel/inc/MantidKernel/Quat.h @@ -76,6 +76,7 @@ namespace Mantid void set(const double ww, const double aa, const double bb, const double cc); void setAngleAxis(const double _deg, const V3D& _axis); void getAngleAxis(double& _deg,double& _axis1,double& _axis2,double& axis3) const; + std::vector getEulerAngles(const std::string) const; /// Set the rotation (both don't change rotation axis) void setRotation(const double deg); //! Norm of a quaternion diff --git a/Code/Mantid/Framework/Kernel/src/Quat.cpp b/Code/Mantid/Framework/Kernel/src/Quat.cpp index f82656e4f342..ec5f7b46fb1e 100644 --- a/Code/Mantid/Framework/Kernel/src/Quat.cpp +++ b/Code/Mantid/Framework/Kernel/src/Quat.cpp @@ -8,6 +8,8 @@ #include #include +#include + namespace Mantid { @@ -776,6 +778,76 @@ void Quat::rotateBB(double& xmin, double& ymin, double& zmin, double& xmax, doub return; } +/** Calculate the Euler angles that are equivalent to this Quaternion. +* @param convention :: The convention used to apply the Euler angles. Defaults to "XYZ". +* @returns A vector of the Euler angles in degrees. The order of the angles is the same as in the convention parameter. +*/ +std::vector Quat::getEulerAngles(const std::string convention = "XYZ") const +{ + std::string conv(convention); + + if(conv.length() != 3) + throw std::invalid_argument("Wrong convention name (string length not 3)"); + + boost::to_upper(conv); + + //Check it's only XYZ in the string + if(conv.find_first_not_of("XYZ") != std::string::npos) + throw std::invalid_argument("Wrong convention name (characters other than XYZ)"); + + //Cannot be XXY, XYY, or similar. Only first and last may be the same: YXY + if((conv[0]==conv[1]) || (conv[2]==conv[1])) + throw std::invalid_argument("Wrong convention name (repeated indices)"); + + boost::replace_all(conv,"X","0"); + boost::replace_all(conv,"Y","1"); + boost::replace_all(conv,"Z","2"); + + std::stringstream s; + s << conv[0] << " " << conv[1] << " " << conv[2]; + + int first, second, last; + s >> first >> second >> last; + + // Do we want Tait-Bryan angles, as opposed to 'classic' Euler angles? + const int TB = (first * second * last == 0 && first + second + last == 3) ? 1 : 0; + + const int par01 = ( (second - first + 9) % 3 == 1) ? 1 : -1; + const int par12 = ( (last - second + 9) % 3 == 1) ? 1 : -1; + + std::vector angles(3); + + const DblMatrix R = DblMatrix(this->getRotation()); + + const int i = (last + TB * par12 + 9) % 3; + const int j1 = (last - par12 + 9) % 3; + const int j2 = (last + par12 + 9) % 3; + + const double s3 = (1.0 - TB - TB * par12) * R[i][j1]; + const double c3 = (TB - (1.0 - TB) * par12) * R[i][j2]; + + V3D axis3(0,0,0); + axis3[last] = 1.0; + + const double rad2deg = 180.0 / M_PI; + + angles[2] = atan2(s3,c3) * rad2deg; + + DblMatrix Rm3(Quat(-angles[2], axis3).getRotation()); + DblMatrix Rp = R * Rm3; + + const double s1 = par01 * Rp[(first - par01 + 9) % 3][(first + par01 + 9) % 3]; + const double c1 = Rp[second][second]; + const double s2 = par01*Rp[first][3-first-second]; + const double c2 = Rp[first][first]; + + angles[0] = atan2(s1,c1) * rad2deg; + angles[1] = atan2(s2,c2) * rad2deg; + + return angles; +} + + } // Namespace Kernel } // Namespce Mantid