GeometrySurface/interface/SOARotation.h

0001 #ifndef DataFormats_GeometrySurface_SOARotation_h
0002 #define DataFormats_GeometrySurface_SOARotation_h
0003
0004 template <class T>
0005 class TkRotation;
0006
0007 // to be moved in an external common library???
0008
0009 /** Rotation matrix used by SOA (as in GPU)
0010  */
0011
0012 template <class T>
0013 class SOARotation {
0014 public:
0015   constexpr inline SOARotation() {}
0016
0017   constexpr inline explicit SOARotation(T) : R11(1), R12(0), R13(0), R21(0), R22(1), R23(0), R31(0), R32(0), R33(1) {}
0018
0019   constexpr inline SOARotation(T xx, T xy, T xz, T yx, T yy, T yz, T zx, T zy, T zz)
0020       : R11(xx), R12(xy), R13(xz), R21(yx), R22(yy), R23(yz), R31(zx), R32(zy), R33(zz) {}
0021
0022   constexpr inline SOARotation(const T *p)
0023       : R11(p[0]), R12(p[1]), R13(p[2]), R21(p[3]), R22(p[4]), R23(p[5]), R31(p[6]), R32(p[7]), R33(p[8]) {}
0024
0025   template <typename U>
0026   constexpr inline SOARotation(const TkRotation<U> &a)
0027       : R11(a.xx()),
0028         R12(a.xy()),
0029         R13(a.xz()),
0030         R21(a.yx()),
0031         R22(a.yy()),
0032         R23(a.yz()),
0033         R31(a.zx()),
0034         R32(a.zy()),
0035         R33(a.zz()) {}
0036
0037   constexpr inline SOARotation transposed() const { return SOARotation(R11, R21, R31, R12, R22, R32, R13, R23, R33); }
0038
0039   // if frame this is to local
0040   constexpr inline void multiply(T const vx, T const vy, T const vz, T &ux, T &uy, T &uz) const {
0041     ux = R11 * vx + R12 * vy + R13 * vz;
0042     uy = R21 * vx + R22 * vy + R23 * vz;
0043     uz = R31 * vx + R32 * vy + R33 * vz;
0044   }
0045
0046   // if frame this is to global
0047   constexpr inline void multiplyInverse(T const vx, T const vy, T const vz, T &ux, T &uy, T &uz) const {
0048     ux = R11 * vx + R21 * vy + R31 * vz;
0049     uy = R12 * vx + R22 * vy + R32 * vz;
0050     uz = R13 * vx + R23 * vy + R33 * vz;
0051   }
0052
0053   // if frame this is to global
0054   constexpr inline void multiplyInverse(T const vx, T const vy, T &ux, T &uy, T &uz) const {
0055     ux = R11 * vx + R21 * vy;
0056     uy = R12 * vx + R22 * vy;
0057     uz = R13 * vx + R23 * vy;
0058   }
0059
0060   constexpr inline T const &xx() const { return R11; }
0061   constexpr inline T const &xy() const { return R12; }
0062   constexpr inline T const &xz() const { return R13; }
0063   constexpr inline T const &yx() const { return R21; }
0064   constexpr inline T const &yy() const { return R22; }
0065   constexpr inline T const &yz() const { return R23; }
0066   constexpr inline T const &zx() const { return R31; }
0067   constexpr inline T const &zy() const { return R32; }
0068   constexpr inline T const &zz() const { return R33; }
0069
0070 private:
0071   T R11, R12, R13;
0072   T R21, R22, R23;
0073   T R31, R32, R33;
0074 };
0075
0076 template <class T>
0077 class SOAFrame {
0078 public:
0079   constexpr inline SOAFrame() {}
0080
0081   constexpr inline SOAFrame(T ix, T iy, T iz, SOARotation<T> const &irot) : px(ix), py(iy), pz(iz), rot(irot) {}
0082
0083   constexpr inline SOARotation<T> const &rotation() const { return rot; }
0084
0085   constexpr inline void toLocal(T const vx, T const vy, T const vz, T &ux, T &uy, T &uz) const {
0086     rot.multiply(vx - px, vy - py, vz - pz, ux, uy, uz);
0087   }
0088
0089   constexpr inline void toGlobal(T const vx, T const vy, T const vz, T &ux, T &uy, T &uz) const {
0090     rot.multiplyInverse(vx, vy, vz, ux, uy, uz);
0091     ux += px;
0092     uy += py;
0093     uz += pz;
0094   }
0095
0096   constexpr inline void toGlobal(T const vx, T const vy, T &ux, T &uy, T &uz) const {
0097     rot.multiplyInverse(vx, vy, ux, uy, uz);
0098     ux += px;
0099     uy += py;
0100     uz += pz;
0101   }
0102
0103   constexpr inline void toGlobal(T cxx, T cxy, T cyy, T *gl) const {
0104     auto const &r = rot;
0105     gl[0] = r.xx() * (r.xx() * cxx + r.yx() * cxy) + r.yx() * (r.xx() * cxy + r.yx() * cyy);
0106     gl[1] = r.xx() * (r.xy() * cxx + r.yy() * cxy) + r.yx() * (r.xy() * cxy + r.yy() * cyy);
0107     gl[2] = r.xy() * (r.xy() * cxx + r.yy() * cxy) + r.yy() * (r.xy() * cxy + r.yy() * cyy);
0108     gl[3] = r.xx() * (r.xz() * cxx + r.yz() * cxy) + r.yx() * (r.xz() * cxy + r.yz() * cyy);
0109     gl[4] = r.xy() * (r.xz() * cxx + r.yz() * cxy) + r.yy() * (r.xz() * cxy + r.yz() * cyy);
0110     gl[5] = r.xz() * (r.xz() * cxx + r.yz() * cxy) + r.yz() * (r.xz() * cxy + r.yz() * cyy);
0111   }
0112
0113   constexpr inline void toLocal(T const *ge, T &lxx, T &lxy, T &lyy) const {
0114     auto const &r = rot;
0115
0116     T cxx = ge[0];
0117     T cyx = ge[1];
0118     T cyy = ge[2];
0119     T czx = ge[3];
0120     T czy = ge[4];
0121     T czz = ge[5];
0122
0123     lxx = r.xx() * (r.xx() * cxx + r.xy() * cyx + r.xz() * czx) +
0124           r.xy() * (r.xx() * cyx + r.xy() * cyy + r.xz() * czy) + r.xz() * (r.xx() * czx + r.xy() * czy + r.xz() * czz);
0125     lxy = r.yx() * (r.xx() * cxx + r.xy() * cyx + r.xz() * czx) +
0126           r.yy() * (r.xx() * cyx + r.xy() * cyy + r.xz() * czy) + r.yz() * (r.xx() * czx + r.xy() * czy + r.xz() * czz);
0127     lyy = r.yx() * (r.yx() * cxx + r.yy() * cyx + r.yz() * czx) +
0128           r.yy() * (r.yx() * cyx + r.yy() * cyy + r.yz() * czy) + r.yz() * (r.yx() * czx + r.yy() * czy + r.yz() * czz);
0129   }
0130
0131   constexpr inline T x() const { return px; }
0132   constexpr inline T y() const { return py; }
0133   constexpr inline T z() const { return pz; }
0134
0135 private:
0136   T px, py, pz;
0137   SOARotation<T> rot;
0138 };
0139
0140 #endif  // DataFormats_GeometrySurface_SOARotation_h