GCOP: lib/utils/so3.h Source File

Go to the documentation of this file.
00001 #ifndef GCOP_SO3_H
00002 #define GCOP_SO3_H
00003 
00004 #include "group.h"
00005 
00006 namespace gcop {
00007 
00008   using namespace Eigen;
00009 
00010   class SO3 : public Group<3,3> {
00011   public:
00012 
00013     SO3();
00014 
00015     static SO3& Instance();
00016   
00017     void inv(Matrix3d &mi, const Matrix3d &m) const;
00018 
00019     void hat(Matrix3d &vh, const Vector3d &v) const;
00020 
00021     void hatinv(Vector3d &v, const Matrix3d &vh) const;
00022 
00023     void Tg(Matrix3d& m, const Matrix3d &g) const;
00024     
00025     void Ad(Matrix3d& m, const Matrix3d &g) const;
00026 
00027     void ad(Matrix3d &m, const Vector3d &v) const;
00028 
00029     void adinv(Vector3d& v, const Matrix3d& m) const;
00030 
00031     void exp(Matrix3d &m, const Vector3d &v) const;
00032 
00033     void log(Vector3d& v, const Matrix3d& m) const;
00034 
00035     void cay(Matrix3d& g, const Vector3d &v) const;
00036     
00037     void cayinv(Vector3d& v, const Matrix3d& m) const;
00038 
00039     void dcay(Matrix3d& m, const Vector3d& v) const;
00040     
00041     void dcayinv(Matrix3d& m, const Vector3d& v) const;
00042 
00043     void dexp(Matrix3d& m, const Vector3d& v) const;
00044     
00045     void dexpinv(Matrix3d& m, const Vector3d& v) const;
00046 
00047     void skew(Vector3d& v, const Matrix3d& m) const;  
00048 
00049     double roll(const Matrix3d& m) const;
00050 
00051     double pitch(const Matrix3d& m) const;
00052 
00053     double yaw(const Matrix3d& m) const;
00054 
00055     void q2g(Matrix3d &m, const Vector3d &rpy);
00056 
00057     void quat2g(Matrix3d &m, const Vector4d &wxyz);
00058 
00059     void g2quat(Vector4d &wxyz, const Matrix3d &m);
00060 
00061     void g2q(Vector3d &rpy, const Matrix3d &m);
00062 
00063     double tol;               
00064     
00065   };
00066 }
00067 
00068 #endif