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

  
}

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;

  
}

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