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