SSERot.h

Go to the documentation of this file.

#ifndef DataFormat_Math_SSERot_H
#define DataFormat_Math_SSERot_H

#include "DataFormats/Math/interface/SSEVec.h"

namespace mathSSE {

  template <typename T>
  struct OldRot {
    T R11, R12, R13;
    T R21, R22, R23;
    T R31, R32, R33;
  } __attribute__((aligned(16)));

  template <typename T>
  struct Rot3 {
    Vec4<T> axis[3];

    Rot3() {
      axis[0].arr[0] = 1;
      axis[1].arr[1] = 1;
      axis[2].arr[2] = 1;
    }

    Rot3(Vec4<T> ix, Vec4<T> iy, Vec4<T> iz) {
      axis[0] = ix;
      axis[1] = iy;
      axis[2] = iz;
    }

    Rot3(T xx, T xy, T xz, T yx, T yy, T yz, T zx, T zy, T zz) {
      axis[0].set(xx, xy, xz);
      axis[1].set(yx, yy, yz);
      axis[2].set(zx, zy, zz);
    }

    Rot3 transpose() const {
      return Rot3(axis[0].arr[0],
                  axis[1].arr[0],
                  axis[2].arr[0],
                  axis[0].arr[1],
                  axis[1].arr[1],
                  axis[2].arr[1],
                  axis[0].arr[2],
                  axis[1].arr[2],
                  axis[2].arr[2]);
    }

    Vec4<T> x() { return axis[0]; }
    Vec4<T> y() { return axis[1]; }
    Vec4<T> z() { return axis[2]; }

    // toLocal...
    Vec4<T> rotate(Vec4<T> v) const { return transpose().rotateBack(v); }

    // toGlobal...
    Vec4<T> rotateBack(Vec4<T> v) const {
      return v.template get1<0>() * axis[0] + v.template get1<1>() * axis[1] + v.template get1<2>() * axis[2];
    }

    Rot3 rotate(Rot3 const& r) const {
      Rot3 tr = transpose();
      return Rot3(tr.rotateBack(r.axis[0]), tr.rotateBack(r.axis[1]), tr.rotateBack(r.axis[2]));
    }

    Rot3 rotateBack(Rot3 const& r) const {
      return Rot3(rotateBack(r.axis[0]), rotateBack(r.axis[1]), rotateBack(r.axis[2]));
    }
  };

  typedef Rot3<float> Rot3F;

  typedef Rot3<double> Rot3D;

#ifdef __SSE4_1__
  template <>
  inline Vec4<float> Rot3<float>::rotate(Vec4<float> v) const {
    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)),
                     _mm_dp_ps(axis[2].vec, v.vec, 0x74));
  }
  template <>
  inline Rot3<float> Rot3<float>::rotate(Rot3<float> const& r) const {
    return Rot3<float>(rotate(r.axis[0]), rotate(r.axis[1]), rotate(r.axis[2]));
  }

#endif

}  // namespace mathSSE

template <typename T>
inline mathSSE::Rot3<T> operator*(mathSSE::Rot3<T> const& rh, mathSSE::Rot3<T> const& lh) {
  //   return Rot3(lh.rotateBack(rh.axis[0]),lh.rotateBack(rh.axis[1]),lh.rotateBack(rh.axis[2]));
  return lh.rotateBack(rh);
}

namespace mathSSE {

  template <typename T>
  struct Rot2 {
    Vec2<T> axis[2];

    Rot2() {
      axis[0].arr[0] = 1;
      axis[1].arr[1] = 1;
    }

    Rot2(Vec2<T> ix, Vec2<T> iy) {
      axis[0] = ix;
      axis[1] = iy;
    }

    Rot2(T xx, T xy, T yx, T yy) {
      axis[0].set(xx, xy);
      axis[1].set(yx, yy);
    }

    Rot2 transpose() const { return Rot2(axis[0].arr[0], axis[1].arr[0], axis[0].arr[1], axis[1].arr[1]); }

    Vec2<T> x() { return axis[0]; }
    Vec2<T> y() { return axis[1]; }

    // toLocal...
    Vec2<T> rotate(Vec2<T> v) const { return transpose().rotateBack(v); }

    // toGlobal...
    Vec2<T> rotateBack(Vec2<T> v) const { return v.template get1<0>() * axis[0] + v.template get1<1>() * axis[1]; }

    Rot2 rotate(Rot2 const& r) const {
      Rot2 tr = transpose();
      return Rot2(tr.rotateBack(r.axis[0]), tr.rotateBack(r.axis[1]));
    }

    Rot2 rotateBack(Rot2 const& r) const { return Rot2(rotateBack(r.axis[0]), rotateBack(r.axis[1])); }
  };

  typedef Rot2<float> Rot2F;

  typedef Rot2<double> Rot2D;

}  // namespace mathSSE

template <typename T>
inline mathSSE::Rot2<T> operator*(mathSSE::Rot2<T> const& rh, mathSSE::Rot2<T> const& lh) {
  return lh.rotateBack(rh);
}

#include <iosfwd>
std::ostream& operator<<(std::ostream& out, mathSSE::Rot3F const& v);
std::ostream& operator<<(std::ostream& out, mathSSE::Rot3D const& v);
std::ostream& operator<<(std::ostream& out, mathSSE::Rot2F const& v);
std::ostream& operator<<(std::ostream& out, mathSSE::Rot2D const& v);

#endif  //  DataFormat_Math_SSERot_H