DDCore/src/MatrixHelpers.cpp

0001 //==========================================================================
0002 //  AIDA Detector description implementation
0003 //--------------------------------------------------------------------------
0004 // Copyright (C) Organisation europeenne pour la Recherche nucleaire (CERN)
0005 // All rights reserved.
0006 //
0007 // For the licensing terms see $DD4hepINSTALL/LICENSE.
0008 // For the list of contributors see $DD4hepINSTALL/doc/CREDITS.
0009 //
0010 // Author     : M.Frank
0011 //
0012 //==========================================================================
0013
0014 // Framework include files
0015 #include <DD4hep/MatrixHelpers.h>
0016 #include <DD4hep/DD4hepUnits.h>
0017
0018 #ifdef DD4HEP_USE_GEANT4_UNITS
0019 #define MM_2_CM 1.0
0020 #else
0021 #define MM_2_CM 0.1
0022 #endif
0023
0024 // ROOT includes
0025 #include <TGeoMatrix.h>
0026
0027 TGeoIdentity* dd4hep::detail::matrix::_identity() {
0028   return gGeoIdentity;
0029 }
0030
0031 ROOT::Math::XYZVector dd4hep::detail::matrix::_scale(const TGeoMatrix* matrix) {
0032   if ( matrix->IsScale() )   {
0033     const Double_t* mat_scale = matrix->GetScale();
0034     return ROOT::Math::XYZVector(mat_scale[0],mat_scale[1],mat_scale[2]);
0035   }
0036   return ROOT::Math::XYZVector(1e0,1e0,1e0);
0037 }
0038
0039 ROOT::Math::XYZVector dd4hep::detail::matrix::_scale(const TGeoMatrix& matrix) {
0040   return _scale(&matrix);
0041 }
0042
0043 dd4hep::Position dd4hep::detail::matrix::_translation(const TGeoMatrix* matrix) {
0044   if ( matrix->IsTranslation() )   {
0045     const Double_t* trans = matrix->GetTranslation();
0046     return Position(trans[0]*MM_2_CM,trans[1]*MM_2_CM,trans[2]*MM_2_CM);
0047   }
0048   return Position(0e0,0e0,0e0);
0049 }
0050
0051 dd4hep::Position dd4hep::detail::matrix::_translation(const TGeoMatrix& matrix) {
0052   return _translation(&matrix);
0053 }
0054
0055 TGeoTranslation* dd4hep::detail::matrix::_translation(const Position& pos) {
0056   return new TGeoTranslation("", pos.X(), pos.Y(), pos.Z());
0057 }
0058
0059 TGeoRotation* dd4hep::detail::matrix::_rotationZYX(const RotationZYX& rot) {
0060   return new TGeoRotation("", rot.Phi() * RAD_2_DEGREE, rot.Theta() * RAD_2_DEGREE, rot.Psi() * RAD_2_DEGREE);
0061 }
0062
0063 TGeoRotation* dd4hep::detail::matrix::_rotation3D(const Rotation3D& rot3D) {
0064   EulerAngles rot(rot3D);
0065   return new TGeoRotation("", rot.Phi() * RAD_2_DEGREE, rot.Theta() * RAD_2_DEGREE, rot.Psi() * RAD_2_DEGREE);
0066 }
0067
0068 /// Extract the rotational part of a TGeoMatrix as a Rotation3D                           \ingroup DD4HEP \ingroup DD4HEP_CORE
0069 dd4hep::Rotation3D dd4hep::detail::matrix::_rotation3D(const TGeoMatrix* matrix)   {
0070   if ( matrix->IsRotation() )  {
0071     const Double_t* rot = matrix->GetRotationMatrix();
0072     return Rotation3D(rot[0],rot[1],rot[2],
0073                       rot[3],rot[4],rot[5],
0074                       rot[6],rot[7],rot[8]);
0075   }
0076   return Rotation3D(0e0,0e0,0e0,
0077                     0e0,0e0,0e0,
0078                     0e0,0e0,0e0);
0079 }
0080
0081 /// Extract the rotational part of a TGeoMatrix as a Rotation3D                           \ingroup DD4HEP \ingroup DD4HEP_CORE
0082 dd4hep::Rotation3D dd4hep::detail::matrix::_rotation3D(const TGeoMatrix& matrix)   {
0083   return _rotation3D(&matrix);
0084 }
0085
0086 /// Set a RotationZYX object to a TGeoHMatrix            \ingroup DD4HEP \ingroup DD4HEP_CORE
0087 TGeoHMatrix& dd4hep::detail::matrix::_transform(TGeoHMatrix& tr, const RotationZYX& rot)   {
0088   tr.RotateZ(rot.Phi()   * RAD_2_DEGREE);
0089   tr.RotateY(rot.Theta() * RAD_2_DEGREE);
0090   tr.RotateX(rot.Psi()   * RAD_2_DEGREE);
0091   return tr;
0092 }
0093
0094 /// Set a Position object (translation) to a TGeoHMatrix \ingroup DD4HEP \ingroup DD4HEP_CORE
0095 TGeoHMatrix& dd4hep::detail::matrix::_transform(TGeoHMatrix& tr, const Position& pos)   {
0096   double t[3];
0097   pos.GetCoordinates(t);
0098   tr.SetDx(t[0]);
0099   tr.SetDy(t[1]);
0100   tr.SetDz(t[2]);
0101   return tr;
0102 }
0103
0104 /// Set a Rotation3D object to a TGeoHMatrix           \ingroup DD4HEP \ingroup DD4HEP_CORE
0105 TGeoHMatrix& dd4hep::detail::matrix::_transform(TGeoHMatrix& tr, const Rotation3D& rot)   {
0106   Double_t* r = tr.GetRotationMatrix();
0107   rot.GetComponents(r);
0108   return tr;
0109 }
0110
0111 /// Set a Transform3D object to a TGeoHMatrix            \ingroup DD4HEP \ingroup DD4HEP_CORE
0112 TGeoHMatrix& dd4hep::detail::matrix::_transform(TGeoHMatrix& tr, const Transform3D& trans) {
0113   Position pos;
0114   RotationZYX rot;
0115   trans.GetDecomposition(rot, pos);
0116   return _transform(tr, pos, rot);
0117 }
0118
0119 /// Set a Position followed by a RotationZYX to a TGeoHMatrix  \ingroup DD4HEP \ingroup DD4HEP_CORE
0120 TGeoHMatrix& dd4hep::detail::matrix::_transform(TGeoHMatrix& tr, const Position& pos, const RotationZYX& rot) {
0121   return _transform(_transform(tr, rot), pos);
0122 }
0123
0124 /// Convert a Position object to a TGeoTranslation         \ingroup DD4HEP \ingroup DD4HEP_CORE
0125 TGeoHMatrix* dd4hep::detail::matrix::_transform(const Position& pos)   {
0126   return &_transform(*(new TGeoHMatrix()), pos);
0127 }
0128
0129 /// Convert a RotationZYX object to a TGeoHMatrix          \ingroup DD4HEP \ingroup DD4HEP_CORE
0130 TGeoHMatrix* dd4hep::detail::matrix::_transform(const RotationZYX& rot)   {
0131   return &_transform(*(new TGeoHMatrix()), rot);
0132 }
0133
0134 /// Convert a Rotation3D object to a TGeoHMatrix           \ingroup DD4HEP \ingroup DD4HEP_CORE
0135 TGeoHMatrix* dd4hep::detail::matrix::_transform(const Rotation3D& rot)   {
0136   return &_transform(*(new TGeoHMatrix()), rot);
0137 }
0138
0139 /// Convert a Transform3D object to a TGeoHMatrix          \ingroup DD4HEP \ingroup DD4HEP_CORE
0140 TGeoHMatrix* dd4hep::detail::matrix::_transform(const Transform3D& trans) {
0141   return &_transform(*(new TGeoHMatrix()), trans);
0142 }
0143
0144 /// Convert a Position followed by a RotationZYX to a TGeoHMatrix  \ingroup DD4HEP \ingroup DD4HEP_CORE
0145 TGeoHMatrix* dd4hep::detail::matrix::_transform(const Position& pos, const RotationZYX& rot) {
0146   return &_transform(*(new TGeoHMatrix()), pos, rot);
0147 }
0148
0149 /// Convert a TGeoMatrix object to a generic Transform3D  \ingroup DD4HEP \ingroup DD4HEP_CORE
0150 dd4hep::Transform3D dd4hep::detail::matrix::_transform(const TGeoMatrix* matrix)    {
0151   const Double_t* t = matrix->GetTranslation();
0152   if ( matrix->IsRotation() )  {
0153     const Double_t* rot = matrix->GetRotationMatrix();
0154     return Transform3D(rot[0],rot[1],rot[2],t[0]*MM_2_CM,
0155                        rot[3],rot[4],rot[5],t[1]*MM_2_CM,
0156                        rot[6],rot[7],rot[8],t[2]*MM_2_CM);
0157   }
0158   return Transform3D(0e0,0e0,0e0,t[0]*MM_2_CM,
0159                      0e0,0e0,0e0,t[1]*MM_2_CM,
0160                      0e0,0e0,0e0,t[2]*MM_2_CM);
0161 }
0162
0163 // add another implementation that takes const reference
0164 dd4hep::Transform3D dd4hep::detail::matrix::_transform(const TGeoMatrix& matrix)    {
0165   const Double_t* t = matrix.GetTranslation();
0166   if ( matrix.IsRotation() )  {
0167     const Double_t* r = matrix.GetRotationMatrix();
0168     return Transform3D(r[0],r[1],r[2],t[0]*MM_2_CM,
0169                        r[3],r[4],r[5],t[1]*MM_2_CM,
0170                        r[6],r[7],r[8],t[2]*MM_2_CM);
0171   }
0172   return Transform3D(0e0,0e0,0e0,t[0]*MM_2_CM,
0173                      0e0,0e0,0e0,t[1]*MM_2_CM,
0174                      0e0,0e0,0e0,t[2]*MM_2_CM);
0175 }
0176
0177 /// Decompose a generic ROOT Matrix into a generic transformation Transform3D            \ingroup DD4HEP \ingroup DD4HEP_CORE
0178 void dd4hep::detail::matrix::_transform(const TGeoMatrix& matrix, Transform3D& tr)   {
0179   tr = _transform(matrix);
0180 }
0181
0182 /// Decompose a generic ROOT Matrix into a generic transformation Transform3D            \ingroup DD4HEP \ingroup DD4HEP_CORE
0183 void dd4hep::detail::matrix::_transform(const TGeoMatrix* matrix, Transform3D& tr)   {
0184   if ( matrix )   {
0185     _transform(*matrix, tr);
0186     return;
0187   }
0188   tr = Transform3D();
0189 }
0190
0191 dd4hep::XYZAngles dd4hep::detail::matrix::_xyzAngles(const TGeoMatrix* matrix) {
0192   return matrix->IsRotation() ? _xyzAngles(matrix->GetRotationMatrix()) : XYZAngles(0,0,0);
0193 }
0194
0195 dd4hep::XYZAngles dd4hep::detail::matrix::_xyzAngles(const double* rot) {
0196   Double_t cosb = std::sqrt(rot[0]*rot[0] + rot[1]*rot[1]);
0197   if (cosb > 0.00001) {
0198     return XYZAngles(atan2(rot[5], rot[8]), atan2(-rot[2], cosb), atan2(rot[1], rot[0]));
0199   }
0200   return XYZAngles(atan2(-rot[7], rot[4]),atan2(-rot[2], cosb),0);
0201 }
0202
0203 void dd4hep::detail::matrix::_decompose(const TGeoMatrix& trafo, Position& pos, Rotation3D& rot)  {
0204   _decompose(_transform(trafo), pos, rot);
0205 }
0206
0207 void dd4hep::detail::matrix::_decompose(const TGeoMatrix& trafo, Position& pos, RotationZYX& rot)   {
0208   _decompose(_transform(trafo), pos, rot);
0209 }
0210
0211 void dd4hep::detail::matrix::_decompose(const TGeoMatrix& trafo, Position& pos, XYZAngles& rot)   {
0212   _decompose(_transform(trafo), pos, rot);
0213 }
0214
0215 void dd4hep::detail::matrix::_decompose(const Transform3D& trafo, Position& pos, Rotation3D& rot)  {
0216   trafo.GetDecomposition(rot, pos);
0217 }
0218
0219 void dd4hep::detail::matrix::_decompose(const Rotation3D& rot, Position& x, Position& y, Position& z)  {
0220   rot.GetComponents(x,y,z);
0221 }
0222
0223 void dd4hep::detail::matrix::_decompose(const Transform3D& trafo, Translation3D& pos, RotationZYX& rot)   {
0224   trafo.GetDecomposition(rot, pos);
0225 }
0226
0227 void dd4hep::detail::matrix::_decompose(const Transform3D& trafo, Translation3D& pos, XYZAngles& rot)   {
0228   EulerAngles r;
0229   trafo.GetDecomposition(r,pos);
0230   rot.SetXYZ(r.Psi(),r.Theta(),r.Phi());
0231 }
0232
0233 void dd4hep::detail::matrix::_decompose(const Transform3D& trafo, Position& pos, RotationZYX& rot)  {
0234   trafo.GetDecomposition(rot,pos);
0235 }
0236
0237 void dd4hep::detail::matrix::_decompose(const Transform3D& trafo, Position& pos, XYZAngles& rot)  {
0238   EulerAngles r;
0239   trafo.GetDecomposition(r,pos);
0240   rot.SetXYZ(r.Psi(),r.Theta(),r.Phi());
0241 }
0242
0243 /// Access determinant of rotation component (0 if no rotation present)
0244 double dd4hep::detail::matrix::determinant(const TGeoMatrix* matrix)   {
0245   return (matrix) ? determinant(*matrix) : 0e0;
0246 }
0247
0248 /// Access determinant of rotation component (0 if no rotation present)
0249 double dd4hep::detail::matrix::determinant(const TGeoMatrix& matrix)   {
0250   const double* r = matrix.GetRotationMatrix();
0251   if ( r )   {
0252     double det =
0253       r[0]*r[4]*r[8] + r[3]*r[7]*r[2] + r[6]*r[1]*r[5] -
0254       r[2]*r[4]*r[6] - r[5]*r[7]*r[0] - r[8]*r[1]*r[3];
0255     return det;
0256   }
0257   return 0.0;
0258 }
0259
0260 /// Access determinant of the rotation component of a Transform3D
0261 double dd4hep::detail::matrix::determinant(const Transform3D& tr)   {
0262   Position p;
0263   Rotation3D r;
0264   tr.GetDecomposition(r, p);
0265   return determinant(r);
0266 }
0267
0268 /// Access determinant of a Rotation3D
0269 double dd4hep::detail::matrix::determinant(const Rotation3D& rot)   {
0270   Position x, y, z;
0271   rot.GetComponents(x,y,z);
0272   double det = (x.Cross(y)).Dot(z);
0273   return det;
0274 }
0275
0276 /// Check matrices for equality
0277 int dd4hep::detail::matrix::_matrixEqual(const TGeoMatrix& left, const TGeoMatrix& right)   {
0278   static constexpr double epsilon = 1e-12;
0279   int result = MATRICES_EQUAL;
0280   const Double_t* t1 = left.GetTranslation();
0281   const Double_t* t2 = right.GetTranslation();
0282   for(int i=0; i<3; ++i)   {
0283     if ( std::fabs(t1[i]-t2[i]) > epsilon )  {
0284       result = MATRICES_DIFFER_TRANSLATION;
0285       break;
0286     }
0287   }
0288   const Double_t* r1 = left.GetRotationMatrix();
0289   const Double_t* r2 = right.GetRotationMatrix();
0290   for(int i=0; i<9; ++i)   {
0291     if ( std::fabs(r1[i]-r2[i]) > epsilon )  {
0292       result |= MATRICES_DIFFER_ROTATION;
0293       break;
0294     }
0295   }
0296   return result;
0297 }
0298
0299