trv_rotations.hpp

#ifndef TRV_ROTATIONS_HPP
#define TRV_ROTATIONS_HPP

#include "kernel/jafarException.hpp"
#include "jmath/jblas.hpp"
#include "jmath/ublasExtra.hpp"

// #include <boost/numeric/ublas/matrix.hpp>
// #include <boost/numeric/ublas/io.hpp>
// #include "boost/numeric/ublas/matrix_proxy.hpp"
// #include "boost/numeric/ublas/operation.hpp"
// #include <boost/numeric/bindings/lapack/gesdd.hpp>

const double TRV_TOLERANCE = 0.001;
const double TRV_ZERO_TOLERANCE = 1e-10;
const std::string TRV_EULER_CONFIG = "zyx";

namespace jafar {
  namespace traversability {
    
    // conversions between the rotation representations
    jblas::vec4  mat2quat   ( const jblas::mat33 &R );  //imp
    jblas::mat33 quat2mat   ( const jblas::vec4 &q );   //imp
    jblas::vec4  euler2quat ( double psi, double theta, double phi, std::string config = TRV_EULER_CONFIG );  // imp
    jblas::vec4  euler2quat ( const jblas::vec3 &eulers, std::string config = TRV_EULER_CONFIG ); // imp
    jblas::mat33 euler2mat  ( double psi, double theta, double phi, std::string config = TRV_EULER_CONFIG ); // imp
    jblas::mat33 euler2mat  ( const jblas::vec3 &eulers, std::string config = TRV_EULER_CONFIG ); // imp
    jblas::vec4  axis2quat  ( const jblas::vec3 &axis, double phi );  // imp
    jblas::mat33 axis2mat   ( const jblas::vec3 &axis, double phi ); // imp
    // two output arguments therefore void
    void         quat2axis  ( const jblas::vec4 &q, jblas::vec3 &axis, double &phi ); 
    
    // internal utility functions (for euler related stuff)
    jblas::vec4 basicRotQuat( double angle, char rotAx );
    jblas::mat33 basicRotMat( double angle, char rotAx );
    inline double signof(double a) { return (a == 0) ? 0 : (a<0 ? -1 : 1); }
    //inline int signof(int a) { return (a == 0) ? 0 : (a<0 ? -1 : 1); }
    
    
    // quaternion related functions
    jblas::vec4 quatRotScale ( const jblas::vec4 &q, double b );
    jblas::vec4 quatMul ( const jblas::vec4 &q1, const jblas::vec4 &q2 );
    jblas::vec4 quatCon ( const jblas::vec4 &q );
    jblas::vec4 quatInv ( const jblas::vec4 &q );
    
    // check utilities
    // This utilities can be excluded from the final code since, if everything works
    // well, a set of euler angles always can be transformed to a rotation matrix, and 
    // a rotation matrix always can be translated into quaternions.
    // Thus, the functions above help to find out if the above really is the case...
    void isrotquat ( const jblas::vec4 &q );
    void isrotmat ( const jblas::mat33 &R );
    void issp3 ( const jblas::mat33 &R );   // is special orthogonal 3d?

  } // end namespace traversability
} // end namespace traversability
#endif