transformation3.hh

#ifndef _TRANSFORMATION3_HXX_
#define _TRANSFORMATION3_HXX_

#include <assert.h>
#include <cmath>
#include "dmatrix.hh"

template <class T>
struct Vector3 {
  T elems[3] ;

  Vector3(T x, T y, T z) {elems[0]=x; elems[1]=y; elems[2]=z;}
  Vector3()              {elems[0]=0.; elems[1]=0.; elems[2]=0.;}
  Vector3(const DVector<T>& t){}

  // translational view
  inline const T& x()     const  {return elems[0];}
  inline const T& y()     const  {return elems[1];}
  inline const T& z()     const  {return elems[2];}
  inline T& x()            {return elems[0];}
  inline T& y()            {return elems[1];}
  inline T& z()            {return elems[2];}

  // rotational view
  inline const T& roll()   const  {return elems[0];}
  inline const T& pitch() const  {return elems[1];}
  inline const T& yaw()   const  {return elems[2];}
  inline T& roll()          {return elems[0];}
  inline T& pitch()        {return elems[1];}
  inline T& yaw()          {return elems[2];}

};

template <class T>
struct Pose3 : public DVector<T>{
  Pose3();
  Pose3(const Vector3<T>& rot, const Vector3<T>& trans);
  Pose3(const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
  Pose3(const DVector<T>& v): DVector<T>(v) {assert(v.dim()==6);} 
    
  inline operator const DVector<T>&  () {return (const DVector<T>)*this;}
  inline operator DVector<T>&        () {return *this;}

  inline const T& roll()  const  {return DVector<T>::elems[0];}
  inline const T& pitch() const  {return DVector<T>::elems[1];}
  inline const T& yaw()   const  {return DVector<T>::elems[2];}
  inline const T& x()     const  {return DVector<T>::elems[3];}
  inline const T& y()     const  {return DVector<T>::elems[4];}
  inline const T& z()     const  {return DVector<T>::elems[5];}

  inline T& roll()               {return DVector<T>::elems[0];}
  inline T& pitch()              {return DVector<T>::elems[1];}
  inline T& yaw()                {return DVector<T>::elems[2];}
  inline T& x()                  {return DVector<T>::elems[3];}
  inline T& y()                  {return DVector<T>::elems[4];}
  inline T& z()                  {return DVector<T>::elems[5];}

};


template <class T>
struct Quaternion{

  Quaternion();
  
  Quaternion(const Vector3<T>& pose);
  
  Quaternion(const T _w, const T _x, const T _y, const T _z);
  
  Quaternion(const T _roll_x_phi, const T _pitch_y_theta, const T _yaw_z_psi);
  
  inline Quaternion<T> conjugated() const;
  
  inline Quaternion<T> normalized() const;
  
  inline Quaternion<T> inverse() const;

  /*construct a quaternion on the axis/angle representation*/
  inline Quaternion(const Vector3<T>& axis, const T& angle);

  inline Quaternion<T> rotateThisAlong (const Vector3<T>& axis, const T alpha) const;
  
        
  inline Quaternion<T> rotatePoint(const Quaternion& p) const;
  
  inline Vector3<T> rotatePoint(const Vector3<T>& p) const;
  
  inline Quaternion withRotation (const T alpha) const;
        
  inline Vector3<T> toAngles() const;
  
  inline Vector3<T> axis() const;
  inline T angle() const;

        
  inline T norm() const;
  
  inline T re() const;
  
  inline Vector3<T> im() const;
        
  T w,x,y,z;
};



template <class T> inline Quaternion<T> operator + (const Quaternion<T> & left, const Quaternion<T>& right);
template <class T> inline Quaternion<T> operator - (const Quaternion<T> & left, const Quaternion<T>& right);
template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const Quaternion<T>& right);
template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const T scalar);
template <class T> inline Quaternion<T> operator * (const T scalar, const Quaternion<T>& right);
template <class T> std::ostream& operator << (std::ostream& os, const Quaternion<T>& q);

template <class T> inline T innerproduct(const Quaternion<T>& left, const Quaternion<T>& right);
template <class T> inline Quaternion<T> slerp(const Quaternion<T>& from, const Quaternion<T>& to, const T lambda);



template <class T>
struct Transformation3{
  Quaternion<T> rotationQuaternion;
  Vector3<T>    translationVector;

  Transformation3(){}

  inline static Transformation3<T> identity();

  Transformation3 (const Vector3<T>& trans, const Quaternion<T>& rot);
  Transformation3 (const Pose3<T>& v);
  Transformation3 (const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);

  inline Vector3<T> translation() const;
  inline Quaternion <T> rotation() const;

  inline Pose3<T> toPoseType() const;
  
  inline void setTranslation(const Vector3<T>& t);
  inline void setTranslation(const T& x, const T& y, const T& z);

  inline void setRotation(const Vector3<T>& r);
  inline void setRotation(const T& roll, const T& pitch, const T& yaw);
  inline void setRotation(const Quaternion<T>& q);


  inline Transformation3<T> inv() const;
  inline bool validRotation(const T& epsilon=0.001) const;

};

template <class T>
inline Vector3<T> operator * (const Transformation3<T>& m, const Vector3<T>& v);

template <class T>
inline Transformation3<T> operator * (const Transformation3<T>& m1, const Transformation3<T>& m2);

template <class T>
struct Operations3D{
  typedef T                  BaseType;
  typedef Pose3<T>           PoseType;
  typedef Quaternion<T>      RotationType;
  typedef Vector3<T>         TranslationType;
  typedef Transformation3<T> TransformationType;
  typedef DMatrix<T>         CovarianceType;
  typedef DMatrix<T>         InformationType;
  typedef Transformation3<T> ParametersType;
};

/**************************** IMPLEMENTATION ****************************/

#include "transformation3.hxx"

#endif

---