Math/interface/SSERot.h

0001 #ifndef DataFormat_Math_SSERot_H
0002 #define DataFormat_Math_SSERot_H
0003
0004 #include "DataFormats/Math/interface/SSEVec.h"
0005
0006 namespace mathSSE {
0007
0008   template <typename T>
0009   struct OldRot {
0010     T R11, R12, R13;
0011     T R21, R22, R23;
0012     T R31, R32, R33;
0013   } __attribute__((aligned(16)));
0014
0015   template <typename T>
0016   struct Rot3 {
0017     Vec4<T> axis[3];
0018
0019     Rot3() {
0020       axis[0].arr[0] = 1;
0021       axis[1].arr[1] = 1;
0022       axis[2].arr[2] = 1;
0023     }
0024
0025     Rot3(Vec4<T> ix, Vec4<T> iy, Vec4<T> iz) {
0026       axis[0] = ix;
0027       axis[1] = iy;
0028       axis[2] = iz;
0029     }
0030
0031     Rot3(T xx, T xy, T xz, T yx, T yy, T yz, T zx, T zy, T zz) {
0032       axis[0].set(xx, xy, xz);
0033       axis[1].set(yx, yy, yz);
0034       axis[2].set(zx, zy, zz);
0035     }
0036
0037     Rot3 transpose() const {
0038       return Rot3(axis[0].arr[0],
0039                   axis[1].arr[0],
0040                   axis[2].arr[0],
0041                   axis[0].arr[1],
0042                   axis[1].arr[1],
0043                   axis[2].arr[1],
0044                   axis[0].arr[2],
0045                   axis[1].arr[2],
0046                   axis[2].arr[2]);
0047     }
0048
0049     Vec4<T> x() { return axis[0]; }
0050     Vec4<T> y() { return axis[1]; }
0051     Vec4<T> z() { return axis[2]; }
0052
0053     // toLocal...
0054     Vec4<T> rotate(Vec4<T> v) const { return transpose().rotateBack(v); }
0055
0056     // toGlobal...
0057     Vec4<T> rotateBack(Vec4<T> v) const {
0058       return v.template get1<0>() * axis[0] + v.template get1<1>() * axis[1] + v.template get1<2>() * axis[2];
0059     }
0060
0061     Rot3 rotate(Rot3 const& r) const {
0062       Rot3 tr = transpose();
0063       return Rot3(tr.rotateBack(r.axis[0]), tr.rotateBack(r.axis[1]), tr.rotateBack(r.axis[2]));
0064     }
0065
0066     Rot3 rotateBack(Rot3 const& r) const {
0067       return Rot3(rotateBack(r.axis[0]), rotateBack(r.axis[1]), rotateBack(r.axis[2]));
0068     }
0069   };
0070
0071   typedef Rot3<float> Rot3F;
0072
0073   typedef Rot3<double> Rot3D;
0074
0075 #ifdef CMS_USE_SSE4
0076   template <>
0077   inline Vec4<float> Rot3<float>::rotate(Vec4<float> v) const {
0078     return _mm_or_ps(_mm_or_ps(_mm_dp_ps(axis[0].vec, v.vec, 0x71), _mm_dp_ps(axis[1].vec, v.vec, 0x72)),
0079                      _mm_dp_ps(axis[2].vec, v.vec, 0x74));
0080   }
0081   template <>
0082   inline Rot3<float> Rot3<float>::rotate(Rot3<float> const& r) const {
0083     return Rot3<float>(rotate(r.axis[0]), rotate(r.axis[1]), rotate(r.axis[2]));
0084   }
0085
0086 #endif
0087
0088 }  // namespace mathSSE
0089
0090 template <typename T>
0091 inline mathSSE::Rot3<T> operator*(mathSSE::Rot3<T> const& rh, mathSSE::Rot3<T> const& lh) {
0092   //   return Rot3(lh.rotateBack(rh.axis[0]),lh.rotateBack(rh.axis[1]),lh.rotateBack(rh.axis[2]));
0093   return lh.rotateBack(rh);
0094 }
0095
0096 namespace mathSSE {
0097
0098   template <typename T>
0099   struct Rot2 {
0100     Vec2<T> axis[2];
0101
0102     Rot2() {
0103       axis[0].arr[0] = 1;
0104       axis[1].arr[1] = 1;
0105     }
0106
0107     Rot2(Vec2<T> ix, Vec2<T> iy) {
0108       axis[0] = ix;
0109       axis[1] = iy;
0110     }
0111
0112     Rot2(T xx, T xy, T yx, T yy) {
0113       axis[0].set(xx, xy);
0114       axis[1].set(yx, yy);
0115     }
0116
0117     Rot2 transpose() const { return Rot2(axis[0].arr[0], axis[1].arr[0], axis[0].arr[1], axis[1].arr[1]); }
0118
0119     Vec2<T> x() { return axis[0]; }
0120     Vec2<T> y() { return axis[1]; }
0121
0122     // toLocal...
0123     Vec2<T> rotate(Vec2<T> v) const { return transpose().rotateBack(v); }
0124
0125     // toGlobal...
0126     Vec2<T> rotateBack(Vec2<T> v) const { return v.template get1<0>() * axis[0] + v.template get1<1>() * axis[1]; }
0127
0128     Rot2 rotate(Rot2 const& r) const {
0129       Rot2 tr = transpose();
0130       return Rot2(tr.rotateBack(r.axis[0]), tr.rotateBack(r.axis[1]));
0131     }
0132
0133     Rot2 rotateBack(Rot2 const& r) const { return Rot2(rotateBack(r.axis[0]), rotateBack(r.axis[1])); }
0134   };
0135
0136   typedef Rot2<float> Rot2F;
0137
0138   typedef Rot2<double> Rot2D;
0139
0140 }  // namespace mathSSE
0141
0142 template <typename T>
0143 inline mathSSE::Rot2<T> operator*(mathSSE::Rot2<T> const& rh, mathSSE::Rot2<T> const& lh) {
0144   return lh.rotateBack(rh);
0145 }
0146
0147 #include <iosfwd>
0148 std::ostream& operator<<(std::ostream& out, mathSSE::Rot3F const& v);
0149 std::ostream& operator<<(std::ostream& out, mathSSE::Rot3D const& v);
0150 std::ostream& operator<<(std::ostream& out, mathSSE::Rot2F const& v);
0151 std::ostream& operator<<(std::ostream& out, mathSSE::Rot2D const& v);
0152
0153 #endif  //  DataFormat_Math_SSERot_H