From 1bed3a3df42b5ebcb96745294e0f4244c008e254 Mon Sep 17 00:00:00 2001 From: Johannes Junggeburth <johannes.josef.junggeburth@cern.ch> Date: Mon, 22 Jan 2024 18:44:59 +0100 Subject: [PATCH] GeoDefinitions - Add function to extract the euler angles. Fix bugs in the intrinsic calculation of GeoRotation. Add corresponding unit test --- .../GeoModelKernel/GeoDefinitions.h | 161 ++++++++++++++---- .../GeoModelKernel/src/GeoDefinitions.cxx | 121 ++++++++++--- .../GeoModelKernel/tests/testEulerAngles.cxx | 97 +++++++++++ 3 files changed, 330 insertions(+), 49 deletions(-) create mode 100644 GeoModelCore/GeoModelKernel/tests/testEulerAngles.cxx diff --git a/GeoModelCore/GeoModelKernel/GeoModelKernel/GeoDefinitions.h b/GeoModelCore/GeoModelKernel/GeoModelKernel/GeoDefinitions.h index 0e31cdfe3..cbc81fd9d 100644 --- a/GeoModelCore/GeoModelKernel/GeoModelKernel/GeoDefinitions.h +++ b/GeoModelCore/GeoModelKernel/GeoModelKernel/GeoDefinitions.h @@ -1,5 +1,5 @@ /* - Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration + Copyright (C) 2002-2024 CERN for the benefit of the ATLAS collaboration */ #ifndef GEOMODELKERNEL_GEODEFINITIONS_H @@ -16,27 +16,28 @@ #endif namespace GeoTrf { - typedef Eigen::Quaternion<double> Rotation3D; - typedef Eigen::Translation<double, 3> Translation3D; - typedef Eigen::AngleAxisd AngleAxis3D; - typedef Eigen::Affine3d Transform3D; - typedef Eigen::Matrix<double, 3, 1> Vector3D; - typedef Eigen::Matrix<double, 2, 1> Vector2D; - typedef Eigen::Matrix<double, 3, 3> RotationMatrix3D; - typedef Eigen::DiagonalMatrix <double, 3> Diagonal3DMatrix; + using Rotation3D = Eigen::Quaternion<double>; + using Translation3D = Eigen::Translation<double, 3>; + using AngleAxis3D = Eigen::AngleAxisd; + using Transform3D = Eigen::Affine3d; + template<size_t N> using VectorN = Eigen::Matrix<double, N, 1>; + using Vector3D = VectorN<3>; + using Vector2D = VectorN<2>; + using RotationMatrix3D = Eigen::Matrix<double, 3, 3>; + using Diagonal3DMatrix = Eigen::DiagonalMatrix <double, 3>; /// check if we can use Eigen Hyperplane for this struct Plane3D { protected: - double a_, b_, c_, d_; + double a_{0.}; + double b_{0.}; + double c_{1.}; + double d_{0.}; public: /** * Default constructor - creates plane z=0. */ - Plane3D() - : a_(0.), b_(0.), c_(1.), d_(0.) - {} - + Plane3D() = default; /** * Constructor from four numbers - creates plane a*x+b*y+c*z+d=0. */ Plane3D(double a1, double b1, double c1, double d1) @@ -72,7 +73,7 @@ namespace GeoTrf { Translate3D(double x, double y, double z) : Transform3D(Translation3D(x,y,z)) {} - virtual ~Translate3D() {} + virtual ~Translate3D() = default; }; class Scale3D : public Transform3D { @@ -80,7 +81,7 @@ namespace GeoTrf { Scale3D(double x, double y, double z) : Transform3D(Diagonal3DMatrix(x,y,z)) {} - virtual ~Scale3D() {} + virtual ~Scale3D() = default; }; class TranslateX3D : public Transform3D { @@ -88,7 +89,7 @@ namespace GeoTrf { TranslateX3D(double x) : Transform3D(Translation3D(x,0,0)) {} - virtual ~TranslateX3D() {} + virtual ~TranslateX3D() = default; }; class TranslateY3D : public Transform3D { @@ -96,7 +97,7 @@ namespace GeoTrf { TranslateY3D(double y) : Transform3D(Translation3D(0,y,0)) {} - virtual ~TranslateY3D() {} + virtual ~TranslateY3D() = default; }; class TranslateZ3D : public Transform3D { @@ -104,45 +105,147 @@ namespace GeoTrf { TranslateZ3D(double z) : Transform3D(Translation3D(0,0,z)) {} - virtual ~TranslateZ3D() {} + virtual ~TranslateZ3D() = default; }; + /** Starting from the three rotation matrices + * + * | 1 0 0 | + * R_{x}(\alpha) = | 0 cos(\alpha) -sin(\alpha) |, + * | 0 sin(\alpha) cos(\alpha) | + * + * | cos(\alpha) 0 sin(\alpha) | + * R_{y}(\alpha) = | 0 1 0 |, + * | -sin(\alpha) 0 cos(\alpha) | + * + * | cos(\alpha) -sin(\alpha) 0 | + * R_{z}(\alpha) = | sin(\alpha) cos(\alpha) 0 |, + * | 0 0 1 | + * + * a genericrotation can be either expressed in terms of the three rotation angles (\alpha, \beta, \gamma) + * + * R(\alpha, \beta, \gamma) = R_{z}(\gamma) * R_{y}(\beta) * R_{x}(\alpha) + * + * or as a function of the three rotation angles (\phi, \psi, \theta) + * + * R(\phi, \psi, \theta) = R_{z}(\phi) * R_{x}(\psi) * R_{z}(\phi) + * + * The first one is referred to as coordinate Euler angles and the second one is referred to geometric Euler angles + */ + inline RotationMatrix3D get3DRotMatX(const double angle) { + return RotationMatrix3D{AngleAxis3D{angle, Vector3D::UnitX()}}; + } + inline RotationMatrix3D get3DRotMatY(const double angle) { + return RotationMatrix3D{AngleAxis3D{angle, Vector3D::UnitY()}}; + } + inline RotationMatrix3D get3DRotMatZ(const double angle) { + return RotationMatrix3D{AngleAxis3D{angle, Vector3D::UnitZ()}}; + } class Rotate3D : public Transform3D { public: + /** @param angle: Rotation angle + * @param axis: Arbitrary rotation axis + */ Rotate3D(double angle, const Vector3D& axis) : Transform3D(AngleAxis3D(angle,axis)) {} - virtual ~Rotate3D() {} + virtual ~Rotate3D() = default; }; - + + /// @brief Rotation around the x-axis class RotateX3D : public Transform3D { public: RotateX3D(double angle) - : Transform3D(AngleAxis3D(angle,Vector3D::UnitX())) + : Transform3D(get3DRotMatX(angle)) {} - virtual ~RotateX3D() {} + virtual ~RotateX3D() = default; }; + /// @brief Rotation around the y-axis class RotateY3D : public Transform3D { public: RotateY3D(double angle) - : Transform3D(AngleAxis3D(angle,Vector3D::UnitY())) + : Transform3D(get3DRotMatY(angle)) {} - virtual ~RotateY3D() {} + virtual ~RotateY3D() = default; }; + /// @brief Rotation around the z-axis class RotateZ3D : public Transform3D { public: RotateZ3D(double angle) - : Transform3D(AngleAxis3D(angle,Vector3D::UnitZ())) + : Transform3D(get3DRotMatZ(angle)) {} - virtual ~RotateZ3D() {} + virtual ~RotateZ3D() = default; + }; + + /// @brief Representation of the Euler angles in the geometric description + struct EulerAngles{ + + EulerAngles() = default; + /// @brief Constructor taking the Euler angles values (phi, theta, psi) + EulerAngles(const double _ph, const double _th, const double _ps): + phi{_ph}, theta{_th}, psi{_ps} {} + /// @brief Rotation angle around the z-axis + double phi{0.}; + /// @brief Rotation angle around the y-axis + double theta{0.}; + /// @brief Rotation angle around the z-axis + double psi{0.}; + /// @brief Ordering operator to put the angles into a set + bool operator<(const EulerAngles& other) const; + /// @brief Simple comparison returning -1, 0, 1 + int compare(const EulerAngles& other) const; + operator bool() const; + }; + /// @brief Representation of the Euler angles in the coordinate description + struct CoordEulerAngles { + CoordEulerAngles() = default; + /// @brief Constructor taking the Euler angles values (alpha, beta, gamma) + CoordEulerAngles(const double _a, const double _b, const double _c): + alpha{_a}, beta{_b}, gamma{_c} {} + + /// @brief Rotation angles around the x-axis + double alpha{0.}; + /// @brief Rotation angle around the y-axis + double beta{0.}; + /// @brief Rotation angle around the z-axis + double gamma{0.}; + + /// @brief Ordering operator to put the angles into a set + bool operator<(const CoordEulerAngles& other) const; + /// @brief Simple comparison returning -1, 0, 1 + int compare(const CoordEulerAngles& other) const; + operator bool() const; + }; + + /// @brief Calculates the geometrical Euler angles + /// @param trans: Input transform + /// @return Euler angles in the geometrical representation + EulerAngles getGeoRotationAngles(const Transform3D& trans); + EulerAngles getGeoRotationAngles(const RotationMatrix3D& rotMat); + + /// @brief Calculates the coordinate Euler angles + /// @param trans: Input transform + /// @return Euler angles in the coordinate representation + CoordEulerAngles getCoordRotationAngles(const Transform3D& trans); + CoordEulerAngles getCoordRotationAngles(const RotationMatrix3D& rotMat); + + /// @brief Arbitrary rotation around an axis using the Euler angles class GeoRotation : public RotationMatrix3D { public: + /// @brief Constructor taking the Euler angles in the Geometrical representation + /// @param phi: Euler angle around the z-axis + /// @param theta: Euler angle around the x-axis + /// @param psi: Euler angle around the z-axis GeoRotation(double phi, double theta, double psi); - virtual ~GeoRotation() {} + + GeoRotation(const EulerAngles& angles); + GeoRotation(const CoordEulerAngles& angles); + + virtual ~GeoRotation() = default; }; class GeoTransformRT : public Transform3D { @@ -150,7 +253,7 @@ namespace GeoTrf { GeoTransformRT(const GeoRotation& rot, const Vector3D& trans) : Transform3D(Translation3D(trans)*Transform3D(AngleAxis3D(rot))) {} - virtual ~GeoTransformRT() {} + virtual ~GeoTransformRT() = default; }; } diff --git a/GeoModelCore/GeoModelKernel/src/GeoDefinitions.cxx b/GeoModelCore/GeoModelKernel/src/GeoDefinitions.cxx index c67561a14..4c55243d0 100755 --- a/GeoModelCore/GeoModelKernel/src/GeoDefinitions.cxx +++ b/GeoModelCore/GeoModelKernel/src/GeoDefinitions.cxx @@ -1,28 +1,109 @@ /* - Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration + Copyright (C) 2002-2024 CERN for the benefit of the ATLAS collaboration */ #include "GeoModelKernel/GeoDefinitions.h" +#include "GeoModelKernel/Units.h" +#include <cmath> -namespace GeoTrf -{ - GeoRotation::GeoRotation(double phi, double theta, double psi) - { - double sinPhi = std::sin( phi ), cosPhi = std::cos( phi ); - double sinTheta = std::sin( theta ), cosTheta = std::cos( theta ); - double sinPsi = std::sin( psi ), cosPsi = std::cos( psi ); - - this->operator()(0,0) = cosPsi * cosPhi - cosTheta * sinPhi * sinPsi; - this->operator()(0,1) = cosPsi * sinPhi + cosTheta * cosPhi * sinPsi; - this->operator()(0,2) = sinPsi * sinTheta; - - this->operator()(1,0) = - sinPsi * cosPhi - cosTheta * sinPhi * cosPsi; - this->operator()(1,1) = - sinPsi * sinPhi + cosTheta * cosPhi * cosPsi; - this->operator()(1,2) = cosPsi * sinTheta; - - this->operator()(2,0) = sinTheta * sinPhi; - this->operator()(2,1) = - sinTheta * cosPhi; - this->operator()(2,2) = cosTheta; +namespace { + constexpr double rotTolerance = 0.01 * GeoModelKernelUnits::deg; + + inline double cutOff(const double val, const double cutOff) { + return std::abs(val) > cutOff ? val : 0.; + } + inline double calcAngle(const double y, const double x) { + return cutOff(std::atan2(cutOff(y, std::numeric_limits<float>::epsilon()), + cutOff(x, std::numeric_limits<float>::epsilon())), rotTolerance); } } +namespace GeoTrf { + + + GeoRotation::GeoRotation(double phi, double theta, double psi): + GeoRotation{EulerAngles{phi, theta, psi}} {} + + GeoRotation::GeoRotation(const EulerAngles& angles): + RotationMatrix3D{get3DRotMatZ(angles.phi) * + get3DRotMatX(angles.theta) * + get3DRotMatZ(angles.psi)} {} + + + GeoRotation::GeoRotation(const CoordEulerAngles& angles): + RotationMatrix3D{get3DRotMatZ(angles.gamma) * + get3DRotMatY(angles.beta)* + get3DRotMatX(angles.alpha)} {} + + + + CoordEulerAngles getCoordRotationAngles(const Transform3D& trans) { + return getCoordRotationAngles(trans.linear()); + } + CoordEulerAngles getCoordRotationAngles(const RotationMatrix3D& rot) { + CoordEulerAngles toRet{}; + const double beta1 = -std::asin(std::clamp(rot(2,0), -1., 1.)); + const double beta2 = M_PI - beta1; + /// Choose the beta value where cos(\beta) > 0 + toRet.beta = cutOff(std::cos(beta1) >= 0. ? beta1 : beta2, rotTolerance); + /// Check that cos(beta) != 0. + const double cosBeta = std::cos(toRet.beta); + if (std::abs(cosBeta) > std::numeric_limits<float>::epsilon()) { + const double iCosBeta = 1./ cosBeta; + toRet.gamma = calcAngle(rot(1,0) * iCosBeta, rot(0,0) * iCosBeta); + toRet.alpha = calcAngle(rot(2,1) * iCosBeta, rot(2,2) * iCosBeta); + } else { + toRet.beta = rot(2,0) > 0 ? -M_PI_2 : M_PI_2; + toRet.alpha = rot(2,0) < 0 ? calcAngle(rot(0,1), rot(0,2)) : + calcAngle(-rot(0,1), -rot(0,2)); + } + return toRet; + } + EulerAngles getGeoRotationAngles(const Transform3D& trans) { + return getGeoRotationAngles(trans.linear()); + } + EulerAngles getGeoRotationAngles(const RotationMatrix3D& rotMat){ + EulerAngles toRet{}; + toRet.theta = cutOff(std::acos(std::clamp(rotMat(2,2),-1., 1.)), rotTolerance); + const double sinTheta = std::sin(toRet.theta); + if (std::abs(sinTheta) > std::numeric_limits<float>::epsilon()) { + toRet.phi = calcAngle(sinTheta* rotMat(0,2), -sinTheta * rotMat(1,2)); + toRet.psi = calcAngle(sinTheta *rotMat(2,0), sinTheta * rotMat(2,1)); + } else { + toRet.theta = rotMat(2,2) > 0 ? 0. : M_PI; + toRet.phi = calcAngle(rotMat(1,0), rotMat(0,0)); + } + + return toRet; + } + + + + int EulerAngles::compare(const EulerAngles& other) const { + if (std::abs(phi - other.phi) > rotTolerance) return phi < other.phi ? -1 : 1; + if (std::abs(theta - other.theta) > rotTolerance) return theta < other.theta ? -1 : 1; + if (std::abs(psi - other.psi) > rotTolerance) return psi < other.psi ? -1 : 1; + return 0; + } + bool EulerAngles::operator<(const EulerAngles& other) const { + return compare(other) < 0; + } + EulerAngles::operator bool() const { + return std::abs(phi) > rotTolerance || std::abs(theta) > rotTolerance || std::abs(psi) > rotTolerance; + } + + int CoordEulerAngles::compare(const CoordEulerAngles& other) const { + if (std::abs(alpha - other.alpha) > rotTolerance) return alpha < other.alpha ? -1 : 1; + if (std::abs(beta - other.beta) > rotTolerance) return beta < other.beta ? -1 : 1; + if (std::abs(gamma - other.gamma) > rotTolerance) return gamma < other.gamma ? -1 : 1; + return 0; + } + bool CoordEulerAngles::operator<(const CoordEulerAngles& other) const { + return compare(other) < 0; + } + CoordEulerAngles::operator bool() const { + return std::abs(alpha) > rotTolerance || std::abs(beta) > rotTolerance || std::abs(gamma) > rotTolerance; + } + +} + diff --git a/GeoModelCore/GeoModelKernel/tests/testEulerAngles.cxx b/GeoModelCore/GeoModelKernel/tests/testEulerAngles.cxx new file mode 100644 index 000000000..bc3ed0758 --- /dev/null +++ b/GeoModelCore/GeoModelKernel/tests/testEulerAngles.cxx @@ -0,0 +1,97 @@ +// Copyright (C) 2002-2023 CERN for the benefit of the ATLAS collaboration + + + +#include "GeoModelKernel/Units.h" +#include "GeoModelKernel/GeoDefinitions.h" + +#include <stdlib.h> +#include <iostream> + +bool isIdentity(const GeoTrf::RotationMatrix3D& mat) { + for (int i = 0; i < 3; ++i) { + if (std::abs(GeoTrf::Vector3D::Unit(i).dot(mat*GeoTrf::Vector3D::Unit(i)) - 1 ) > std::numeric_limits<float>::epsilon()){ + return false; + } + } + return true; +} + +constexpr double toDeg = 1. / GeoModelKernelUnits::deg; +int main() { + for (int alpha = 0; alpha < 360 ; alpha+=15) { + GeoTrf::GeoRotation rotX{alpha*GeoModelKernelUnits::deg, 0. ,0.}; + if (std::abs(rotX.determinant() - 1) > std::numeric_limits<float>::epsilon()) { + std::cerr<<"testEulerAngles() "<<__LINE__<<" The GeoTrf::GeoRotation around the z-axis with angle "<<alpha<<" is not a rotation "<<std::endl; + return EXIT_FAILURE; + } + std::cout<<"testEulerAngles() "<<__LINE__<<" The GeoTrf::GeoRotation around the z-axis with angle "<<alpha<<" "<<std::endl<<rotX<<std::endl; + + + GeoTrf::GeoRotation rotY{0., alpha*GeoModelKernelUnits::deg, 0.}; + if (std::abs(rotY.determinant() - 1) > std::numeric_limits<float>::epsilon()) { + std::cerr<<"testEulerAngles() "<<__LINE__<<" The GeoTrf::GeoRotation around the x-axis with angle "<<alpha<<" is not a rotation "<<std::endl; + return EXIT_FAILURE; + } + std::cout<<"testEulerAngles() "<<__LINE__<<" The GeoTrf::GeoRotation around the x-axis with angle "<<alpha<<" "<<std::endl<<rotY<<std::endl; + + + GeoTrf::GeoRotation rotZ{0., 0., alpha*GeoModelKernelUnits::deg}; + if (std::abs(rotZ.determinant() - 1) > std::numeric_limits<float>::epsilon()) { + std::cerr<<"testEulerAngles() "<<__LINE__<<" The GeoTrf::GeoRotation around the z-axis with angle "<<alpha<<" is not a rotation "<<std::endl; + return EXIT_FAILURE; + } + std::cout<<"testEulerAngles() "<<__LINE__<<" The GeoTrf::GeoRotation around the z-axis with angle "<<alpha<<" "<<std::endl<<rotZ<<std::endl; + } + + for (int psi = 0; psi <= 360 ; psi+= 15) { + for (int phi = 0 ; phi <= 360; phi+= 15) { + for (int theta = 0; theta<= 360; theta+= 15) { + GeoTrf::GeoRotation rot(phi * GeoModelKernelUnits::deg, + theta*GeoModelKernelUnits::deg, + psi *GeoModelKernelUnits::deg); + if (std::abs(rot.determinant() - 1.) > std::numeric_limits<float>::epsilon()) { + std::cerr<<"testEulerAngles() "<<__LINE__<<" The rotation angles psi: "<<psi<<", phi: "<<phi<<", theta: "<<theta + <<" plugged into the GeoTrf::GeoRotation do not result into a rotation matrix "<<std::endl<<rot<< + std::endl<< GeoTrf::get3DRotMatZ(psi *GeoModelKernelUnits::deg) * + GeoTrf::get3DRotMatX(theta *GeoModelKernelUnits::deg) * + GeoTrf::get3DRotMatZ(phi *GeoModelKernelUnits::deg) <<std::endl; + return EXIT_FAILURE; + } + GeoTrf::EulerAngles angles = GeoTrf::getGeoRotationAngles(rot); + GeoTrf::GeoRotation angleRot{angles}; + if (!isIdentity(angleRot.inverse()* rot)) { + std::cout<<"testEulerAngles() "<<__LINE__<<" Injected rotation angles phi: "<<phi<<", theta: "<<theta<<" psi: "<<psi + <<std::endl<<rot<<std::endl; + std::cout<<"testEulerAngles() "<<__LINE__<<" Extracted rotation angles "<<angles.phi*toDeg<<"/" + <<angles.theta*toDeg<<"/"<<angles.psi*toDeg<<std::endl<<angleRot<<std::endl; + return EXIT_FAILURE; + } + + const GeoTrf::CoordEulerAngles coordAngles{phi*GeoModelKernelUnits::deg, + theta*GeoModelKernelUnits::deg, + psi*GeoModelKernelUnits::deg}; + const GeoTrf::GeoRotation coordRot{coordAngles}; + if (std::abs(coordRot.determinant() -1.) > std::numeric_limits<float>::epsilon()) { + std::cerr<<"testEulerAngles() "<<__LINE__<<" The rotation angles alpha: "<<psi<<", beta: "<<phi<<", gamma: "<<theta + <<" plugged into the GeoTrf::GeoRotation do not result into a rotation matrix "<<std::endl<<coordRot<<std::endl<< + GeoTrf::get3DRotMatZ(coordAngles.gamma *GeoModelKernelUnits::deg) * + GeoTrf::get3DRotMatY(coordAngles.beta *GeoModelKernelUnits::deg) * + GeoTrf::get3DRotMatX(coordAngles.alpha *GeoModelKernelUnits::deg)<<std::endl; + + } + const GeoTrf::CoordEulerAngles calcCoordAngles = GeoTrf::getCoordRotationAngles(coordRot); + const GeoTrf::GeoRotation extCoordRot{calcCoordAngles}; + if (!isIdentity(extCoordRot.inverse()* coordRot)) { + std::cout<<"testEulerAngles() "<<__LINE__<<" Injected rotation angles alpha: "<<coordAngles.alpha*toDeg + <<", beta: "<<coordAngles.beta*toDeg<<" gamma: "<<coordAngles.gamma*toDeg + <<std::endl<<coordRot<<std::endl; + std::cout<<"testEulerAngles() "<<__LINE__<<" Extracted rotation angles "<<calcCoordAngles.alpha*toDeg<<"/" + <<calcCoordAngles.beta*toDeg<<"/"<<calcCoordAngles.gamma*toDeg<<std::endl<<extCoordRot<<std::endl; + return EXIT_FAILURE; + } + } + } + } + return EXIT_SUCCESS; +} \ No newline at end of file -- GitLab