File indexing completed on 2024-09-10 02:58:37
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/ExtVec.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 }
0027
0028
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(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
0065
0066
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
0081
0082
0083
0084
0085
0086
0087
0088 TkRotation(const Basic3DVector<T>& axis, T phi) : rot(cos(phi), sin(phi), 0, -sin(phi), cos(phi), 0, 0, 0, 1) {
0089
0090 TkRotation tmpRotz(cos(axis.phi()), sin(axis.phi()), 0., -sin(axis.phi()), cos(axis.phi()), 0., 0., 0., 1.);
0091
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;
0095
0096
0097
0098 *this = (tmpRoty * tmpRotz).multiplyInverse(*this);
0099 }
0100
0101
0102
0103
0104
0105
0106
0107
0108
0109
0110
0111
0112
0113
0114
0115
0116
0117
0118
0119
0120
0121
0122
0123
0124
0125
0126
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
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
0174
0175
0176
0177
0178
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][0]; }
0194 T xy() const { return rot.axis[0][1]; }
0195 T xz() const { return rot.axis[0][2]; }
0196 T yx() const { return rot.axis[1][0]; }
0197 T yy() const { return rot.axis[1][1]; }
0198 T yz() const { return rot.axis[1][2]; }
0199 T zx() const { return rot.axis[2][0]; }
0200 T zy() const { return rot.axis[2][1]; }
0201 T zz() const { return rot.axis[2][2]; }
0202
0203 private:
0204 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(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 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