interface/private/sseTkRotation.h

0001 #ifndef Geom_newTkRotation_H
0002 #define Geom_newTkRotation_H
0003
0004 #include "DataFormats/GeometryVector/interface/Basic2DVector.h"
0005 #include "DataFormats/GeometryVector/interface/Basic3DVector.h"
0006 #include "DataFormats/GeometryVector/interface/GlobalVector.h"
0007
0008 #include "DataFormats/Math/interface/SSERot.h"
0009
0010 #include <iosfwd>
0011
0012 template <class T>
0013 class TkRotation;
0014 template <class T>
0015 class TkRotation2D;
0016
0017 template <class T>
0018 std::ostream& operator<<(std::ostream& s, const TkRotation<T>& r);
0019 template <class T>
0020 std::ostream& operator<<(std::ostream& s, const TkRotation2D<T>& r);
0021
0022 namespace geometryDetails {
0023   void TkRotationErr1();
0024   void TkRotationErr2();
0025
0026 }  // namespace geometryDetails
0027
0028 /** Rotaion matrix used by Surface.
0029  */
0030
0031 template <class T>
0032 class TkRotation {
0033 public:
0034   typedef Vector3DBase<T, GlobalTag> GlobalVector;
0035   typedef Basic3DVector<T> BasicVector;
0036
0037   TkRotation() {}
0038   TkRotation(mathSSE::Rot3<T> const& irot) : rot(irot) {}
0039
0040   TkRotation(T xx, T xy, T xz, T yx, T yy, T yz, T zx, T zy, T zz) : rot(xx, xy, xz, yx, yy, yz, zx, zy, zz) {}
0041
0042   TkRotation(const T* p) : rot(p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8]) {}
0043
0044   TkRotation(const GlobalVector& aX, const GlobalVector& aY) {
0045     GlobalVector uX = aX.unit();
0046     GlobalVector uY = aY.unit();
0047     GlobalVector uZ(uX.cross(uY));
0048
0049     rot.axis[0] = uX.basicVector().v;
0050     rot.axis[1] = uY.basicVector().v;
0051     rot.axis[2] = uZ.basicVector().v;
0052   }
0053
0054   TkRotation(const BasicVector& aX, const BasicVector& aY) {
0055     BasicVector uX = aX.unit();
0056     BasicVector uY = aY.unit();
0057     BasicVector uZ(uX.cross(uY));
0058
0059     rot.axis[0] = uX.v;
0060     rot.axis[1] = uY.v;
0061     rot.axis[2] = uZ.v;
0062   }
0063
0064   /** Construct from global vectors of the x, y and z axes.
0065    *  The axes are assumed to be unit vectors forming
0066    *  a right-handed orthonormal basis. No checks are performed!
0067    */
0068   TkRotation(const GlobalVector& uX, const GlobalVector& uY, const GlobalVector& uZ) {
0069     rot.axis[0] = uX.basicVector().v;
0070     rot.axis[1] = uY.basicVector().v;
0071     rot.axis[2] = uZ.basicVector().v;
0072   }
0073
0074   TkRotation(const BasicVector& uX, const BasicVector& uY, const BasicVector& uZ) {
0075     rot.axis[0] = uX.v;
0076     rot.axis[1] = uY.v;
0077     rot.axis[2] = uZ.v;
0078   }
0079
0080   /** rotation around abritrary axis by the amount of phi:
0081    *  its constructed by  O^-1(z<->axis) rot_z(phi) O(z<->axis)
0082    *  the frame is rotated such that the z-asis corresponds to the rotation
0083    *  axis desired. THen it's rotated round the "new" z-axis, and then
0084    *  the initial transformation is "taken back" again.
0085    *  unfortuately I'm too stupid to describe such thing directly by 3 Euler
0086    *  angles.. hence I have to construckt it this way...by brute force
0087    */
0088   TkRotation(const Basic3DVector<T>& axis, T phi) : rot(cos(phi), sin(phi), 0, -sin(phi), cos(phi), 0, 0, 0, 1) {
0089     //rotation around the z-axis by  phi
0090     TkRotation tmpRotz(cos(axis.phi()), sin(axis.phi()), 0., -sin(axis.phi()), cos(axis.phi()), 0., 0., 0., 1.);
0091     //rotation around y-axis by theta
0092     TkRotation tmpRoty(cos(axis.theta()), 0., -sin(axis.theta()), 0., 1., 0., sin(axis.theta()), 0., cos(axis.theta()));
0093     (*this) *= tmpRoty;
0094     (*this) *= tmpRotz;  // =  this * tmpRoty * tmpRotz
0095
0096     // (tmpRoty * tmpRotz)^-1 * this * tmpRoty * tmpRotz
0097
0098     *this = (tmpRoty * tmpRotz).multiplyInverse(*this);
0099   }
0100   /* this is the same thing...derived from the CLHEP ... it gives the
0101      same results MODULO the sign of the rotation....  but I don't want
0102      that... had
0103      TkRotation (const Basic3DVector<T>& axis, float phi) {
0104      T cx = axis.x();
0105      T cy = axis.y();
0106      T cz = axis.z();
0107
0108      T ll = axis.mag();
0109      if (ll == 0) {
0110      geometryDetails::TkRotationErr1();
0111      }else{
0112
0113      float cosa = cos(phi), sina = sin(phi);
0114      cx /= ll; cy /= ll; cz /= ll;
0115
0116      R11 = cosa + (1-cosa)*cx*cx;
0117      R12 =        (1-cosa)*cx*cy - sina*cz;
0118      R13 =        (1-cosa)*cx*cz + sina*cy;
0119
0120      R21 =        (1-cosa)*cy*cx + sina*cz;
0121      R22 = cosa + (1-cosa)*cy*cy;
0122      R23 =        (1-cosa)*cy*cz - sina*cx;
0123
0124      R31 =        (1-cosa)*cz*cx - sina*cy;
0125      R32 =        (1-cosa)*cz*cy + sina*cx;
0126      R33 = cosa + (1-cosa)*cz*cz;
0127
0128      }
0129
0130      }
0131   */
0132
0133   template <typename U>
0134   TkRotation(const TkRotation<U>& a) : rot(a.xx(), a.xy(), a.xz(), a.yx(), a.yy(), a.yz(), a.zx(), a.zy(), a.zz()) {}
0135
0136   TkRotation transposed() const { return rot.transpose(); }
0137
0138   Basic3DVector<T> rotate(const Basic3DVector<T>& v) const { return rot.rotate(v.v); }
0139
0140   Basic3DVector<T> rotateBack(const Basic3DVector<T>& v) const { return rot.rotateBack(v.v); }
0141
0142   Basic3DVector<T> operator*(const Basic3DVector<T>& v) const { return rot.rotate(v.v); }
0143
0144   Basic3DVector<T> multiplyInverse(const Basic3DVector<T>& v) const { return rot.rotateBack(v.v); }
0145
0146   template <class Scalar>
0147   Basic3DVector<Scalar> multiplyInverse(const Basic3DVector<Scalar>& v) const {
0148     return Basic3DVector<Scalar>(xx() * v.x() + yx() * v.y() + zx() * v.z(),
0149                                  xy() * v.x() + yy() * v.y() + zy() * v.z(),
0150                                  xz() * v.x() + yz() * v.y() + zz() * v.z());
0151   }
0152
0153   Basic3DVector<T> operator*(const Basic2DVector<T>& v) const {
0154     return Basic3DVector<T>(xx() * v.x() + xy() * v.y(), yx() * v.x() + yy() * v.y(), zx() * v.x() + zy() * v.y());
0155   }
0156   Basic3DVector<T> multiplyInverse(const Basic2DVector<T>& v) const {
0157     return Basic3DVector<T>(xx() * v.x() + yx() * v.y(), xy() * v.x() + yy() * v.y(), xz() * v.x() + yz() * v.y());
0158   }
0159
0160   TkRotation operator*(const TkRotation& b) const { return rot * b.rot; }
0161   TkRotation multiplyInverse(const TkRotation& b) const { return rot.transpose() * b.rot; }
0162
0163   TkRotation& operator*=(const TkRotation& b) { return *this = operator*(b); }
0164
0165   // Note a *= b; <=> a = a * b; while a.transform(b); <=> a = b * a;
0166   TkRotation& transform(const TkRotation& b) { return *this = b.operator*(*this); }
0167
0168   TkRotation& rotateAxes(const Basic3DVector<T>& newX, const Basic3DVector<T>& newY, const Basic3DVector<T>& newZ) {
0169     T del = 0.001;
0170
0171     if (
0172
0173         // the check for right-handedness is not needed since
0174         // we want to change the handedness when it's left in cmsim
0175         //
0176         //       fabs(newZ.x()-w.x()) > del ||
0177         //       fabs(newZ.y()-w.y()) > del ||
0178         //       fabs(newZ.z()-w.z()) > del ||
0179         fabs(newX.mag2() - 1.) > del || fabs(newY.mag2() - 1.) > del || fabs(newZ.mag2() - 1.) > del ||
0180         fabs(newX.dot(newY)) > del || fabs(newY.dot(newZ)) > del || fabs(newZ.dot(newX)) > del) {
0181       geometryDetails::TkRotationErr2();
0182       return *this;
0183     } else {
0184       return transform(
0185           TkRotation(newX.x(), newY.x(), newZ.x(), newX.y(), newY.y(), newZ.y(), newX.z(), newY.z(), newZ.z()));
0186     }
0187   }
0188
0189   Basic3DVector<T> x() const { return rot.axis[0]; }
0190   Basic3DVector<T> y() const { return rot.axis[1]; }
0191   Basic3DVector<T> z() const { return rot.axis[2]; }
0192
0193   T xx() const { return rot.axis[0].arr[0]; }
0194   T xy() const { return rot.axis[0].arr[1]; }
0195   T xz() const { return rot.axis[0].arr[2]; }
0196   T yx() const { return rot.axis[1].arr[0]; }
0197   T yy() const { return rot.axis[1].arr[1]; }
0198   T yz() const { return rot.axis[1].arr[2]; }
0199   T zx() const { return rot.axis[2].arr[0]; }
0200   T zy() const { return rot.axis[2].arr[1]; }
0201   T zz() const { return rot.axis[2].arr[2]; }
0202
0203 private:
0204   mathSSE::Rot3<T> rot;
0205 };
0206
0207 template <>
0208 std::ostream& operator<< <float>(std::ostream& s, const TkRotation<float>& r);
0209
0210 template <>
0211 std::ostream& operator<< <double>(std::ostream& s, const TkRotation<double>& r);
0212
0213 template <class T, class U>
0214 inline Basic3DVector<U> operator*(const TkRotation<T>& r, const Basic3DVector<U>& v) {
0215   return Basic3DVector<U>(r.xx() * v.x() + r.xy() * v.y() + r.xz() * v.z(),
0216                           r.yx() * v.x() + r.yy() * v.y() + r.yz() * v.z(),
0217                           r.zx() * v.x() + r.zy() * v.y() + r.zz() * v.z());
0218 }
0219
0220 template <class T, class U>
0221 inline TkRotation<typename PreciseFloatType<T, U>::Type> operator*(const TkRotation<T>& a, const TkRotation<U>& b) {
0222   typedef TkRotation<typename PreciseFloatType<T, U>::Type> RT;
0223   return RT(a.xx() * b.xx() + a.xy() * b.yx() + a.xz() * b.zx(),
0224             a.xx() * b.xy() + a.xy() * b.yy() + a.xz() * b.zy(),
0225             a.xx() * b.xz() + a.xy() * b.yz() + a.xz() * b.zz(),
0226             a.yx() * b.xx() + a.yy() * b.yx() + a.yz() * b.zx(),
0227             a.yx() * b.xy() + a.yy() * b.yy() + a.yz() * b.zy(),
0228             a.yx() * b.xz() + a.yy() * b.yz() + a.yz() * b.zz(),
0229             a.zx() * b.xx() + a.zy() * b.yx() + a.zz() * b.zx(),
0230             a.zx() * b.xy() + a.zy() * b.yy() + a.zz() * b.zy(),
0231             a.zx() * b.xz() + a.zy() * b.yz() + a.zz() * b.zz());
0232 }
0233
0234 template <class T>
0235 class TkRotation2D {
0236 public:
0237   typedef Basic2DVector<T> BasicVector;
0238
0239   TkRotation2D() {}
0240   TkRotation2D(mathSSE::Rot2<T> const& irot) : rot(irot) {}
0241
0242   TkRotation2D(T xx, T xy, T yx, T yy) : rot(xx, xy, yx, yy) {}
0243
0244   TkRotation2D(const T* p) : rot(p[0], p[1], p[2], p[3]) {}
0245
0246   TkRotation2D(const BasicVector& aX) {
0247     BasicVector uX = aX.unit();
0248     BasicVector uY(-uX.y(), uX.x());
0249
0250     rot.axis[0] = uX.v;
0251     rot.axis[1] = uY.v;
0252   }
0253
0254   TkRotation2D(const BasicVector& uX, const BasicVector& uY) {
0255     rot.axis[0] = uX.v;
0256     rot.axis[1] = uY.v;
0257   }
0258
0259   BasicVector x() const { return rot.axis[0]; }
0260   BasicVector y() const { return rot.axis[1]; }
0261
0262   TkRotation2D transposed() const { return rot.transpose(); }
0263
0264   BasicVector rotate(const BasicVector& v) const { return rot.rotate(v.v); }
0265
0266   BasicVector rotateBack(const BasicVector& v) const { return rot.rotateBack(v.v); }
0267
0268 private:
0269   mathSSE::Rot2<T> rot;
0270 };
0271
0272 template <>
0273 std::ostream& operator<< <float>(std::ostream& s, const TkRotation2D<float>& r);
0274
0275 template <>
0276 std::ostream& operator<< <double>(std::ostream& s, const TkRotation2D<double>& r);
0277
0278 #endif