VecGeom/base/Transformation3D.h

0001 /// \file Transformation3D.h
0002 /// \author Johannes de Fine Licht (johannes.definelicht@cern.ch)
0003
0004 #ifndef VECGEOM_BASE_TRANSFORMATION3D_H_
0005 #define VECGEOM_BASE_TRANSFORMATION3D_H_
0006
0007 #include "VecGeom/base/Config.h"
0008 #include "VecGeom/base/Cuda.h"
0009 #include "VecGeom/base/Global.h"
0010
0011 #include "VecGeom/base/Vector3D.h"
0012 #include "VecGeom/backend/scalar/Backend.h"
0013 #ifdef VECGEOM_ENABLE_CUDA
0014 #include "VecGeom/backend/cuda/Interface.h"
0015 #endif
0016
0017 #include <algorithm>
0018 #include <cmath>
0019 #include <cstring>
0020 #include <iostream>
0021 #include <vector>
0022
0023 #ifdef VECGEOM_ROOT
0024 class TGeoMatrix;
0025 #endif
0026
0027 namespace vecgeom {
0028
0029 VECGEOM_DEVICE_FORWARD_DECLARE(class Transformation3D;);
0030
0031 inline namespace VECGEOM_IMPL_NAMESPACE {
0032
0033 #ifndef VECCORE_CUDA
0034 }
0035 namespace cuda {
0036 class Transformation3D;
0037 }
0038 inline namespace VECGEOM_IMPL_NAMESPACE {
0039 // class vecgeom::cuda::Transformation3D;
0040 #endif
0041
0042 class Transformation3D {
0043
0044 private:
0045   // TODO: it might be better to directly store this in terms of Vector3D<Precision> !!
0046   // and would allow for higher level abstraction
0047   Precision tx_{0.}, ty_{0.}, tz_{0.};
0048   Precision rxx_{1.}, ryx_{0.}, rzx_{0.};
0049   Precision rxy_{0.}, ryy_{1.}, rzy_{0.};
0050   Precision rxz_{0.}, ryz_{0.}, rzz_{1.};
0051   bool fIdentity;
0052   bool fHasRotation;
0053   bool fHasTranslation;
0054
0055 public:
0056   VECCORE_ATT_HOST_DEVICE
0057   constexpr Transformation3D() : fIdentity(true), fHasRotation(false), fHasTranslation(false) {};
0058
0059   /**
0060    * Constructor for translation only.
0061    * @param tx Translation in x-coordinate.
0062    * @param ty Translation in y-coordinate.
0063    * @param tz Translation in z-coordinate.
0064    */
0065   VECCORE_ATT_HOST_DEVICE
0066   Transformation3D(const Precision tx, const Precision ty, const Precision tz)
0067       : tx_{tx}, ty_{ty}, tz_{tz}, fIdentity(tx == 0 && ty == 0 && tz == 0), fHasRotation(false),
0068         fHasTranslation(tx != 0 || ty != 0 || tz != 0)
0069   {
0070   }
0071
0072   /**
0073    * @brief Rotation followed by translation.
0074    * @param tx Translation in x-coordinate.
0075    * @param ty Translation in y-coordinate.
0076    * @param tz Translation in z-coordinate.
0077    * @param phi Rotation angle about z-axis.
0078    * @param theta Rotation angle about new y-axis.
0079    * @param psi Rotation angle about new z-axis.
0080    */
0081   VECCORE_ATT_HOST_DEVICE
0082   Transformation3D(const Precision tx, const Precision ty, const Precision tz, const Precision phi,
0083                    const Precision theta, const Precision psi);
0084
0085   /**
0086    * @brief RScale followed by rotation followed by translation.
0087    * @param tx Translation in x-coordinate.
0088    * @param ty Translation in y-coordinate.
0089    * @param tz Translation in z-coordinate.
0090    * @param phi Rotation angle about z-axis.
0091    * @param theta Rotation angle about new y-axis.
0092    * @param psi Rotation angle about new z-axis.
0093    * @param sx X scaling factor
0094    * @param sy Y scaling factor
0095    * @param sz Z scaling factor
0096    */
0097   VECCORE_ATT_HOST_DEVICE
0098   Transformation3D(const Precision tx, const Precision ty, const Precision tz, const Precision phi,
0099                    const Precision theta, const Precision psi, Precision sx, Precision sy, Precision sz);
0100
0101   /**
0102    * Constructor to manually set each entry. Used when converting from different
0103    * geometry.
0104    */
0105   VECCORE_ATT_HOST_DEVICE
0106   Transformation3D(const Precision tx, const Precision ty, const Precision tz, const Precision r0, const Precision r1,
0107                    const Precision r2, const Precision r3, const Precision r4, const Precision r5, const Precision r6,
0108                    const Precision r7, const Precision r8);
0109
0110   /**
0111    * Constructor using the rotation from a different transformation
0112    */
0113   VECCORE_ATT_HOST_DEVICE
0114   Transformation3D(const Precision tx, const Precision ty, const Precision tz, Transformation3D const &trot);
0115
0116   /**
0117    * Constructor to manually set each entry. Used when converting from different
0118    * geometry, supporting scaling.
0119    */
0120   VECCORE_ATT_HOST_DEVICE
0121   Transformation3D(const Precision tx, const Precision ty, const Precision tz, const Precision r0, const Precision r1,
0122                    const Precision r2, const Precision r3, const Precision r4, const Precision r5, const Precision r6,
0123                    const Precision r7, const Precision r8, Precision sx, Precision sy, Precision sz);
0124
0125   /**
0126    * Constructor copying the translation and rotation from memory
0127    * geometry.
0128    */
0129   VECCORE_ATT_HOST_DEVICE
0130   Transformation3D(const Precision *trans, const Precision *rot, bool has_trans, bool has_rot)
0131   {
0132     this->Set(trans, rot, has_trans, has_rot);
0133   }
0134
0135   /**
0136    * Constructor for a rotation based on a given direction
0137    * @param axis direction of the new z axis
0138    * @param inverse if true the origial axis will be rotated into (0,0,u)
0139                     if false a vector (0,0,u) will be rotated into the original axis
0140    */
0141   VECCORE_ATT_HOST_DEVICE
0142   Transformation3D(const Vector3D<Precision> &axis, bool inverse = true);
0143
0144   VECCORE_ATT_HOST_DEVICE
0145   VECGEOM_FORCE_INLINE
0146   bool operator==(Transformation3D const &rhs) const;
0147
0148   VECCORE_ATT_HOST_DEVICE
0149   VECGEOM_FORCE_INLINE
0150   Transformation3D operator*(Transformation3D const &tf) const;
0151
0152   VECCORE_ATT_HOST_DEVICE
0153   VECGEOM_FORCE_INLINE
0154   Transformation3D const &operator*=(Transformation3D const &rhs);
0155
0156   VECCORE_ATT_HOST_DEVICE
0157   ~Transformation3D() {}
0158
0159   VECCORE_ATT_HOST_DEVICE
0160   void ApplyScale(const Precision sx, const Precision sy, const Precision sz)
0161   {
0162     rxx_ *= sx;
0163     ryx_ *= sy;
0164     rzx_ *= sz;
0165     rxy_ *= sx;
0166     ryy_ *= sy;
0167     rzy_ *= sz;
0168     rxz_ *= sx;
0169     ryz_ *= sy;
0170     rzz_ *= sz;
0171   }
0172
0173   VECCORE_ATT_HOST_DEVICE
0174   bool ApproxEqual(Transformation3D const &rhs, Precision tolerance = kTolerance) const
0175   {
0176     auto const data1 = &tx_;
0177     auto const data2 = &rhs.tx_;
0178     for (int i = 0; i < 12; ++i)
0179       if (vecCore::math::Abs(data1[i] - data2[i]) > tolerance) return false;
0180     return true;
0181   }
0182
0183   VECCORE_ATT_HOST_DEVICE
0184   void Clear()
0185   {
0186     tx_             = 0.;
0187     ty_             = 0.;
0188     tz_             = 0.;
0189     rxx_            = 1.;
0190     ryx_            = 0.;
0191     rzx_            = 0.;
0192     rxy_            = 0.;
0193     ryy_            = 1.;
0194     rzy_            = 0.;
0195     rxz_            = 0.;
0196     ryz_            = 0.;
0197     rzz_            = 1.;
0198     fIdentity       = true;
0199     fHasRotation    = false;
0200     fHasTranslation = false;
0201   }
0202
0203   int MemorySize() const { return sizeof(*this); }
0204
0205   VECCORE_ATT_HOST_DEVICE
0206   void FixZeroes()
0207   {
0208     if (std::abs(tx_) < vecgeom::kTolerance) tx_ = 0.;
0209     if (std::abs(ty_) < vecgeom::kTolerance) ty_ = 0.;
0210     if (std::abs(tz_) < vecgeom::kTolerance) tz_ = 0.;
0211     if (std::abs(rxx_) < vecgeom::kTolerance) rxx_ = 0.;
0212     if (std::abs(ryx_) < vecgeom::kTolerance) ryx_ = 0.;
0213     if (std::abs(rzx_) < vecgeom::kTolerance) rzx_ = 0.;
0214     if (std::abs(rxy_) < vecgeom::kTolerance) rxy_ = 0.;
0215     if (std::abs(ryy_) < vecgeom::kTolerance) ryy_ = 0.;
0216     if (std::abs(rzy_) < vecgeom::kTolerance) rzy_ = 0.;
0217     if (std::abs(rxz_) < vecgeom::kTolerance) rxz_ = 0.;
0218     if (std::abs(ryz_) < vecgeom::kTolerance) ryz_ = 0.;
0219     if (std::abs(rzz_) < vecgeom::kTolerance) rzz_ = 0.;
0220   }
0221
0222   VECCORE_ATT_HOST_DEVICE
0223   VECGEOM_FORCE_INLINE
0224   Vector3D<Precision> Translation() const { return Vector3D<Precision>(tx_, ty_, tz_); }
0225
0226   VECCORE_ATT_HOST_DEVICE
0227   VECGEOM_FORCE_INLINE
0228   Vector3D<Precision> Scale() const
0229   {
0230     Precision sx = std::sqrt(rxx_ * rxx_ + rxy_ * rxy_ + rxz_ * rxz_);
0231     Precision sy = std::sqrt(ryx_ * ryx_ + ryy_ * ryy_ + ryz_ * ryz_);
0232     Precision sz = std::sqrt(rzx_ * rzx_ + rzy_ * rzy_ + rzz_ * rzz_);
0233
0234     if (Determinant() < 0) sz = -sz;
0235
0236     return Vector3D<Precision>(sx, sy, sz);
0237   }
0238
0239   /**
0240    * No safety against faulty indexing.
0241    * @param index Index of translation entry in the range [0-2].
0242    */
0243   VECCORE_ATT_HOST_DEVICE
0244   VECGEOM_FORCE_INLINE
0245   Precision Translation(const int index) const { return *(&tx_ + index); }
0246
0247   VECCORE_ATT_HOST_DEVICE
0248   VECGEOM_FORCE_INLINE
0249   Precision const *Rotation() const { return &rxx_; }
0250
0251   /**
0252    * No safety against faulty indexing.
0253    * \param index Index of rotation entry in the range [0-8].
0254    */
0255   VECCORE_ATT_HOST_DEVICE
0256   VECGEOM_FORCE_INLINE
0257   Precision Rotation(const int index) const { return *(&rxx_ + index); }
0258
0259   VECCORE_ATT_HOST_DEVICE
0260   VECGEOM_FORCE_INLINE
0261   bool IsIdentity() const { return fIdentity; }
0262
0263   VECCORE_ATT_HOST_DEVICE
0264   VECGEOM_FORCE_INLINE
0265   bool IsXYRotation() const
0266   {
0267     return (fHasRotation && (std::abs(rzx_) < vecgeom::kTolerance) && (std::abs(rzy_) < vecgeom::kTolerance) &&
0268             (std::abs(rxz_) < vecgeom::kTolerance) && (std::abs(ryz_) < vecgeom::kTolerance) &&
0269             (std::abs(rzz_ - Precision(1.)) < vecgeom::kTolerance));
0270   }
0271
0272   VECCORE_ATT_HOST_DEVICE
0273   VECGEOM_FORCE_INLINE
0274   bool IsReflected() const { return Determinant() < 0; }
0275
0276   VECCORE_ATT_HOST_DEVICE
0277   VECGEOM_FORCE_INLINE
0278   bool HasRotation() const { return fHasRotation; }
0279
0280   VECCORE_ATT_HOST_DEVICE
0281   VECGEOM_FORCE_INLINE
0282   bool HasTranslation() const { return fHasTranslation; }
0283
0284   VECCORE_ATT_HOST_DEVICE
0285   void Print() const;
0286
0287   // print to a stream
0288   void Print(std::ostream &) const;
0289
0290   VECCORE_ATT_HOST_DEVICE
0291   void PrintG4() const;
0292
0293   // Mutators
0294
0295   VECCORE_ATT_HOST_DEVICE
0296   void SetTranslation(const Precision tx, const Precision ty, const Precision tz);
0297
0298   VECCORE_ATT_HOST_DEVICE
0299   void SetTranslation(Vector3D<Precision> const &vec);
0300
0301   VECCORE_ATT_HOST_DEVICE
0302   void SetProperties();
0303
0304   VECCORE_ATT_HOST_DEVICE
0305   void SetRotation(const Precision phi, const Precision theta, const Precision psi);
0306
0307   VECCORE_ATT_HOST_DEVICE
0308   void SetRotation(Vector3D<Precision> const &vec);
0309
0310   VECCORE_ATT_HOST_DEVICE
0311   void SetRotation(const Precision rot0, const Precision rot1, const Precision rot2, const Precision rot3,
0312                    const Precision rot4, const Precision rot5, const Precision rot6, const Precision rot7,
0313                    const Precision rot8);
0314
0315   /**
0316    * Set rotation given an arbitrary axis and an angle.
0317    * \param aaxis Rotation axis. No need to be a unit vector.
0318    * \param ddelta Rotation angle in radians.
0319    */
0320   VECCORE_ATT_HOST_DEVICE
0321   void Set(Vector3D<double> const &aaxis, double ddelta);
0322
0323   /**
0324    * Set transformation and rotation.
0325    * \param trans Pointer to at least 3 values.
0326    * \param rot Pointer to at least 9 values.
0327    */
0328   VECCORE_ATT_HOST_DEVICE
0329   VECGEOM_FORCE_INLINE
0330   void Set(const Precision *trans, const Precision *rot, bool has_trans, bool has_rot)
0331   {
0332     if (has_trans) {
0333       tx_ = trans[0];
0334       ty_ = trans[1];
0335       tz_ = trans[2];
0336     }
0337
0338     if (has_rot) {
0339       rxx_ = rot[0];
0340       ryx_ = rot[1];
0341       rzx_ = rot[2];
0342       rxy_ = rot[3];
0343       ryy_ = rot[4];
0344       rzy_ = rot[5];
0345       rxz_ = rot[6];
0346       ryz_ = rot[7];
0347       rzz_ = rot[8];
0348     }
0349
0350     fHasTranslation = has_trans;
0351     fHasRotation    = has_rot;
0352     fIdentity       = !fHasTranslation && !fHasRotation;
0353   }
0354
0355 private:
0356   // Templated rotation and translation methods which inline and compile to
0357   // optimized versions.
0358
0359   template <typename InputType>
0360   VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void DoRotation(Vector3D<InputType> const &master,
0361                                                                Vector3D<InputType> &local) const;
0362
0363 private:
0364   template <typename InputType>
0365   VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void DoTranslation(Vector3D<InputType> const &master,
0366                                                                   Vector3D<InputType> &local) const;
0367
0368   template <bool vectortransform, typename InputType>
0369   VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void InverseTransformKernel(Vector3D<InputType> const &local,
0370                                                                            Vector3D<InputType> &master) const;
0371
0372 public:
0373   // Transformation interface
0374
0375   template <typename InputType>
0376   VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void Transform(Vector3D<InputType> const &master,
0377                                                               Vector3D<InputType> &local) const;
0378
0379   template <typename InputType>
0380   VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE Vector3D<InputType> Transform(Vector3D<InputType> const &master) const;
0381
0382   template <typename InputType>
0383   VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void TransformDirection(Vector3D<InputType> const &master,
0384                                                                        Vector3D<InputType> &local) const;
0385
0386   template <typename InputType>
0387   VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE Vector3D<InputType> TransformDirection(
0388       Vector3D<InputType> const &master) const;
0389
0390   /** The inverse transformation ( aka LocalToMaster ) of an object transform like a point
0391    *  this does not need to currently template on placement since such a transformation is much less used
0392    */
0393   template <typename InputType>
0394   VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void InverseTransform(Vector3D<InputType> const &local,
0395                                                                      Vector3D<InputType> &master) const;
0396
0397   template <typename InputType>
0398   VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE Vector3D<InputType> InverseTransform(
0399       Vector3D<InputType> const &local) const;
0400
0401   /** The inverse transformation of an object transforming like a vector */
0402   template <typename InputType>
0403   VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void InverseTransformDirection(Vector3D<InputType> const &local,
0404                                                                               Vector3D<InputType> &master) const;
0405
0406   template <typename InputType>
0407   VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE Vector3D<InputType> InverseTransformDirection(
0408       Vector3D<InputType> const &local) const;
0409
0410   /** compose transformations - multiply transformations */
0411   VECCORE_ATT_HOST_DEVICE
0412   VECGEOM_FORCE_INLINE
0413   void MultiplyFromRight(Transformation3D const &rhs);
0414
0415   /** compose transformations - multiply transformations */
0416   VECCORE_ATT_HOST_DEVICE
0417   VECGEOM_FORCE_INLINE
0418   void CopyFrom(Transformation3D const &rhs)
0419   {
0420     // not sure this compiles under CUDA
0421     copy(&rhs, &rhs + 1, this);
0422   }
0423
0424   VECCORE_ATT_HOST_DEVICE
0425   Transformation3D &RotateX(double a);
0426
0427   VECCORE_ATT_HOST_DEVICE
0428   Transformation3D &RotateY(double a);
0429
0430   VECCORE_ATT_HOST_DEVICE
0431   Transformation3D &RotateZ(double a);
0432
0433   /**
0434    * @brief Returns determinant for the rotation.
0435    */
0436   VECCORE_ATT_HOST_DEVICE
0437   double Determinant() const
0438   {
0439     // Computes determinant in double precision
0440     double xx_ = rxx_;
0441     double xy_ = rxy_;
0442     double xz_ = rxz_;
0443     double yx_ = ryx_;
0444     double yy_ = ryy_;
0445     double yz_ = ryz_;
0446     double zx_ = rzx_;
0447     double zy_ = rzy_;
0448     double zz_ = rzz_;
0449
0450     double detxx = yy_ * zz_ - yz_ * zy_;
0451     double detxy = yx_ * zz_ - yz_ * zx_;
0452     double detxz = yx_ * zy_ - yy_ * zx_;
0453     double det   = xx_ * detxx - xy_ * detxy + xz_ * detxz;
0454     return det;
0455   }
0456
0457   /**
0458    * @brief Returns rotation axis as a unit vector
0459    */
0460   VECCORE_ATT_HOST_DEVICE
0461   Vector3D<double> Axis() const;
0462
0463   /**
0464    * @brief Rectifies the round-off making the rotqation true.
0465    * @details  From clhep/src/RotationC.cc
0466      Assuming the representation of this is close to a true Rotation,
0467      but may have drifted due to round-off error from many operations,
0468      this forms an "exact" orthonormal matrix for the rotation again.
0469
0470      The first step is to average with the transposed inverse.  This
0471      will correct for small errors such as those occuring when decomposing
0472      a LorentzTransformation.  Then we take the bull by the horns and
0473      formally extract the axis and delta (assuming the Rotation were true)
0474      and re-setting the rotation according to those.
0475    */
0476   VECCORE_ATT_HOST_DEVICE
0477   Transformation3D &Rectify();
0478
0479   /**
0480    * @brief Stores the inverse of this matrix into inverse. Taken from G4AffineTransformation
0481    */
0482   VECCORE_ATT_HOST_DEVICE
0483   Transformation3D &Invert()
0484   {
0485     double ttx = -tx_, tty = -ty_, ttz = -tz_;
0486     tx_ = ttx * rxx_ + tty * rxy_ + ttz * rxz_;
0487     ty_ = ttx * ryx_ + tty * ryy_ + ttz * ryz_;
0488     tz_ = ttx * rzx_ + tty * rzy_ + ttz * rzz_;
0489
0490     auto tmp1 = ryx_;
0491     ryx_      = rxy_;
0492     rxy_      = tmp1;
0493     auto tmp2 = rzx_;
0494     rzx_      = rxz_;
0495     rxz_      = tmp2;
0496     auto tmp3 = rzy_;
0497     rzy_      = ryz_;
0498     ryz_      = tmp3;
0499     return *this;
0500   }
0501
0502   /**
0503    * @brief Returns the inverse of this matrix. Taken from G4AffineTransformation
0504    */
0505   VECCORE_ATT_HOST_DEVICE
0506   Transformation3D Inverse() const
0507   {
0508     double ttx = -tx_, tty = -ty_, ttz = -tz_;
0509     return Transformation3D(ttx * rxx_ + tty * rxy_ + ttz * rxz_, ttx * ryx_ + tty * ryy_ + ttz * ryz_,
0510                             ttx * rzx_ + tty * rzy_ + ttz * rzz_, rxx_, rxy_, rxz_, ryx_, ryy_, ryz_, rzx_, rzy_, rzz_);
0511   }
0512
0513   // Utility and CUDA
0514
0515 #ifdef VECGEOM_CUDA_INTERFACE
0516   size_t DeviceSizeOf() const { return DevicePtr<cuda::Transformation3D>::SizeOf(); }
0517   DevicePtr<cuda::Transformation3D> CopyToGpu() const;
0518   DevicePtr<cuda::Transformation3D> CopyToGpu(DevicePtr<cuda::Transformation3D> const gpu_ptr) const;
0519   static void CopyManyToGpu(const std::vector<Transformation3D const *> &trafos,
0520                             const std::vector<DevicePtr<cuda::Transformation3D>> &gpu_ptrs);
0521 #endif
0522
0523 #ifdef VECGEOM_ROOT
0524   // function to convert this transformation to a TGeo transformation
0525   // mainly used for the benchmark comparisons with ROOT
0526   static TGeoMatrix *ConvertToTGeoMatrix(Transformation3D const &);
0527 #endif
0528
0529 public:
0530   static const Transformation3D kIdentity;
0531
0532 }; // End class Transformation3D
0533
0534 VECCORE_ATT_HOST_DEVICE
0535 bool Transformation3D::operator==(Transformation3D const &rhs) const { return equal(&tx_, &tx_ + 12, &rhs.tx_); }
0536
0537 /**
0538  * Rotates a vector to this transformation's frame of reference.
0539  * \param master Vector in original frame of reference.
0540  * \param local Output vector rotated to the new frame of reference.
0541  */
0542 template <typename InputType>
0543 VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void Transformation3D::DoRotation(Vector3D<InputType> const &master,
0544                                                                                Vector3D<InputType> &local) const
0545 {
0546   local[0] = master[0] * rxx_;
0547   local[1] = master[0] * ryx_;
0548   local[2] = master[0] * rzx_;
0549   local[0] += master[1] * rxy_;
0550   local[1] += master[1] * ryy_;
0551   local[2] += master[1] * rzy_;
0552   local[0] += master[2] * rxz_;
0553   local[1] += master[2] * ryz_;
0554   local[2] += master[2] * rzz_;
0555 }
0556
0557 template <typename InputType>
0558 VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void Transformation3D::DoTranslation(Vector3D<InputType> const &master,
0559                                                                                   Vector3D<InputType> &local) const
0560 {
0561
0562   local[0] = master[0] - tx_;
0563   local[1] = master[1] - ty_;
0564   local[2] = master[2] - tz_;
0565 }
0566
0567 /**
0568  * Transform a point to the local reference frame.
0569  * \param master Point to be transformed.
0570  * \param local Output destination. Should never be the same as the input
0571  *              vector!
0572  */
0573 template <typename InputType>
0574 VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void Transformation3D::Transform(Vector3D<InputType> const &master,
0575                                                                               Vector3D<InputType> &local) const
0576 {
0577   Vector3D<InputType> tmp;
0578   DoTranslation(master, tmp);
0579   DoRotation(tmp, local);
0580 }
0581
0582 /**
0583  * Since transformation cannot be done in place, allows the transformed vector
0584  * to be constructed by Transform directly.
0585  * \param master Point to be transformed.
0586  * \return Newly constructed Vector3D with the transformed coordinates.
0587  */
0588 template <typename InputType>
0589 VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE Vector3D<InputType> Transformation3D::Transform(
0590     Vector3D<InputType> const &master) const
0591 {
0592
0593   Vector3D<InputType> local;
0594   Transform(master, local);
0595   return local;
0596 }
0597
0598 template <bool transform_direction, typename InputType>
0599 VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void Transformation3D::InverseTransformKernel(
0600     Vector3D<InputType> const &local, Vector3D<InputType> &master) const
0601 {
0602
0603   // we are just doing the full stuff here ( LocalToMaster is less critical
0604   // than other way round )
0605
0606   if (transform_direction) {
0607     master[0] = local[0] * rxx_;
0608     master[0] += local[1] * ryx_;
0609     master[0] += local[2] * rzx_;
0610     master[1] = local[0] * rxy_;
0611     master[1] += local[1] * ryy_;
0612     master[1] += local[2] * rzy_;
0613     master[2] = local[0] * rxz_;
0614     master[2] += local[1] * ryz_;
0615     master[2] += local[2] * rzz_;
0616   } else {
0617     master[0] = tx_;
0618     master[0] += local[0] * rxx_;
0619     master[0] += local[1] * ryx_;
0620     master[0] += local[2] * rzx_;
0621     master[1] = ty_;
0622     master[1] += local[0] * rxy_;
0623     master[1] += local[1] * ryy_;
0624     master[1] += local[2] * rzy_;
0625     master[2] = tz_;
0626     master[2] += local[0] * rxz_;
0627     master[2] += local[1] * ryz_;
0628     master[2] += local[2] * rzz_;
0629   }
0630 }
0631
0632 template <typename InputType>
0633 VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void Transformation3D::InverseTransform(Vector3D<InputType> const &local,
0634                                                                                      Vector3D<InputType> &master) const
0635 {
0636   InverseTransformKernel<false, InputType>(local, master);
0637 }
0638
0639 template <typename InputType>
0640 VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE Vector3D<InputType> Transformation3D::InverseTransform(
0641     Vector3D<InputType> const &local) const
0642 {
0643   Vector3D<InputType> tmp;
0644   InverseTransform(local, tmp);
0645   return tmp;
0646 }
0647
0648 template <typename InputType>
0649 VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void Transformation3D::InverseTransformDirection(
0650     Vector3D<InputType> const &local, Vector3D<InputType> &master) const
0651 {
0652   InverseTransformKernel<true, InputType>(local, master);
0653 }
0654
0655 template <typename InputType>
0656 VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE Vector3D<InputType> Transformation3D::InverseTransformDirection(
0657     Vector3D<InputType> const &local) const
0658 {
0659   Vector3D<InputType> tmp;
0660   InverseTransformDirection(local, tmp);
0661   return tmp;
0662 }
0663
0664 VECCORE_ATT_HOST_DEVICE
0665 VECGEOM_FORCE_INLINE
0666 Transformation3D Transformation3D::operator*(Transformation3D const &rhs) const
0667 {
0668   if (rhs.fIdentity) return Transformation3D(*this);
0669   return Transformation3D(tx_ * rhs.rxx_ + ty_ * rhs.ryx_ + tz_ * rhs.rzx_ + rhs.tx_, // tx
0670                           tx_ * rhs.rxy_ + ty_ * rhs.ryy_ + tz_ * rhs.rzy_ + rhs.ty_, // ty
0671                           tx_ * rhs.rxz_ + ty_ * rhs.ryz_ + tz_ * rhs.rzz_ + rhs.tz_, // tz
0672                           rxx_ * rhs.rxx_ + rxy_ * rhs.ryx_ + rxz_ * rhs.rzx_,        // rxx
0673                           ryx_ * rhs.rxx_ + ryy_ * rhs.ryx_ + ryz_ * rhs.rzx_,        // ryx
0674                           rzx_ * rhs.rxx_ + rzy_ * rhs.ryx_ + rzz_ * rhs.rzx_,        // rzx
0675                           rxx_ * rhs.rxy_ + rxy_ * rhs.ryy_ + rxz_ * rhs.rzy_,        // rxy
0676                           ryx_ * rhs.rxy_ + ryy_ * rhs.ryy_ + ryz_ * rhs.rzy_,        // ryy
0677                           rzx_ * rhs.rxy_ + rzy_ * rhs.ryy_ + rzz_ * rhs.rzy_,        // rzy
0678                           rxx_ * rhs.rxz_ + rxy_ * rhs.ryz_ + rxz_ * rhs.rzz_,        // rxz
0679                           ryx_ * rhs.rxz_ + ryy_ * rhs.ryz_ + ryz_ * rhs.rzz_,        // ryz
0680                           rzx_ * rhs.rxz_ + rzy_ * rhs.ryz_ + rzz_ * rhs.rzz_);       // rzz
0681 }
0682
0683 VECCORE_ATT_HOST_DEVICE
0684 VECGEOM_FORCE_INLINE
0685 Transformation3D const &Transformation3D::operator*=(Transformation3D const &rhs)
0686 {
0687   if (rhs.fIdentity) return *this;
0688   fIdentity = false;
0689
0690   fHasTranslation |= rhs.HasTranslation();
0691   if (fHasTranslation) {
0692     auto tx = tx_ * rhs.rxx_ + ty_ * rhs.ryx_ + tz_ * rhs.rzx_ + rhs.tx_;
0693     auto ty = tx_ * rhs.rxy_ + ty_ * rhs.ryy_ + tz_ * rhs.rzy_ + rhs.ty_;
0694     auto tz = tx_ * rhs.rxz_ + ty_ * rhs.ryz_ + tz_ * rhs.rzz_ + rhs.tz_;
0695     tx_     = tx;
0696     ty_     = ty;
0697     tz_     = tz;
0698   }
0699
0700   fHasRotation |= rhs.HasRotation();
0701   if (rhs.HasRotation()) {
0702     auto rxx = rxx_ * rhs.rxx_ + rxy_ * rhs.ryx_ + rxz_ * rhs.rzx_;
0703     auto ryx = ryx_ * rhs.rxx_ + ryy_ * rhs.ryx_ + ryz_ * rhs.rzx_;
0704     auto rzx = rzx_ * rhs.rxx_ + rzy_ * rhs.ryx_ + rzz_ * rhs.rzx_;
0705     auto rxy = rxx_ * rhs.rxy_ + rxy_ * rhs.ryy_ + rxz_ * rhs.rzy_;
0706     auto ryy = ryx_ * rhs.rxy_ + ryy_ * rhs.ryy_ + ryz_ * rhs.rzy_;
0707     auto rzy = rzx_ * rhs.rxy_ + rzy_ * rhs.ryy_ + rzz_ * rhs.rzy_;
0708     auto rxz = rxx_ * rhs.rxz_ + rxy_ * rhs.ryz_ + rxz_ * rhs.rzz_;
0709     auto ryz = ryx_ * rhs.rxz_ + ryy_ * rhs.ryz_ + ryz_ * rhs.rzz_;
0710     auto rzz = rzx_ * rhs.rxz_ + rzy_ * rhs.ryz_ + rzz_ * rhs.rzz_;
0711
0712     rxx_ = rxx;
0713     rxy_ = rxy;
0714     rxz_ = rxz;
0715     ryx_ = ryx;
0716     ryy_ = ryy;
0717     ryz_ = ryz;
0718     rzx_ = rzx;
0719     rzy_ = rzy;
0720     rzz_ = rzz;
0721   }
0722
0723   return *this;
0724 }
0725
0726 VECCORE_ATT_HOST_DEVICE
0727 VECGEOM_FORCE_INLINE
0728 void Transformation3D::MultiplyFromRight(Transformation3D const &rhs)
0729 {
0730   // TODO: this code should directly operator on Vector3D and Matrix3D
0731
0732   if (rhs.fIdentity) return;
0733   fIdentity = false;
0734
0735   if (rhs.HasTranslation()) {
0736     fHasTranslation = true;
0737     // ideal for fused multiply add
0738     tx_ += rxx_ * rhs.tx_;
0739     tx_ += ryx_ * rhs.ty_;
0740     tx_ += rzx_ * rhs.tz_;
0741
0742     ty_ += rxy_ * rhs.tx_;
0743     ty_ += ryy_ * rhs.ty_;
0744     ty_ += rzy_ * rhs.tz_;
0745
0746     tz_ += rxz_ * rhs.tx_;
0747     tz_ += ryz_ * rhs.ty_;
0748     tz_ += rzz_ * rhs.tz_;
0749   }
0750
0751   if (rhs.HasRotation()) {
0752     fHasRotation   = true;
0753     Precision tmpx = rxx_;
0754     Precision tmpy = ryx_;
0755     Precision tmpz = rzx_;
0756
0757     // first row of matrix
0758     rxx_ = tmpx * rhs.rxx_;
0759     ryx_ = tmpx * rhs.ryx_;
0760     rzx_ = tmpx * rhs.rzx_;
0761     rxx_ += tmpy * rhs.rxy_;
0762     ryx_ += tmpy * rhs.ryy_;
0763     rzx_ += tmpy * rhs.rzy_;
0764     rxx_ += tmpz * rhs.rxz_;
0765     ryx_ += tmpz * rhs.ryz_;
0766     rzx_ += tmpz * rhs.rzz_;
0767
0768     tmpx = rxy_;
0769     tmpy = ryy_;
0770     tmpz = rzy_;
0771
0772     // second row of matrix
0773     rxy_ = tmpx * rhs.rxx_;
0774     ryy_ = tmpx * rhs.ryx_;
0775     rzy_ = tmpx * rhs.rzx_;
0776     rxy_ += tmpy * rhs.rxy_;
0777     ryy_ += tmpy * rhs.ryy_;
0778     rzy_ += tmpy * rhs.rzy_;
0779     rxy_ += tmpz * rhs.rxz_;
0780     ryy_ += tmpz * rhs.ryz_;
0781     rzy_ += tmpz * rhs.rzz_;
0782
0783     tmpx = rxz_;
0784     tmpy = ryz_;
0785     tmpz = rzz_;
0786
0787     // third row of matrix
0788     rxz_ = tmpx * rhs.rxx_;
0789     ryz_ = tmpx * rhs.ryx_;
0790     rzz_ = tmpx * rhs.rzx_;
0791     rxz_ += tmpy * rhs.rxy_;
0792     ryz_ += tmpy * rhs.ryy_;
0793     rzz_ += tmpy * rhs.rzy_;
0794     rxz_ += tmpz * rhs.rxz_;
0795     ryz_ += tmpz * rhs.ryz_;
0796     rzz_ += tmpz * rhs.rzz_;
0797   }
0798 }
0799
0800 /**
0801  * Only transforms by rotation, ignoring the translation part. This is useful
0802  * when transforming directions.
0803  * \param master Point to be transformed.
0804  * \param local Output destination of transformation.
0805  */
0806 template <typename InputType>
0807 VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void Transformation3D::TransformDirection(
0808     Vector3D<InputType> const &master, Vector3D<InputType> &local) const
0809 {
0810   DoRotation(master, local);
0811 }
0812
0813 /**
0814  * Since transformation cannot be done in place, allows the transformed vector
0815  * to be constructed by TransformDirection directly.
0816  * \param master Point to be transformed.
0817  * \return Newly constructed Vector3D with the transformed coordinates.
0818  */
0819 template <typename InputType>
0820 VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE Vector3D<InputType> Transformation3D::TransformDirection(
0821     Vector3D<InputType> const &master) const
0822 {
0823
0824   Vector3D<InputType> local;
0825   TransformDirection(master, local);
0826   return local;
0827 }
0828
0829 std::ostream &operator<<(std::ostream &os, Transformation3D const &trans);
0830 }
0831 } // namespace vecgeom
0832
0833 #endif // VECGEOM_BASE_TRANSFORMATION3D_H_