uuvmanifold.h

Go to the documentation of this file.

#ifndef GCOP_UUVMANIFOLD_H
#define GCOP_UUVMANIFOLD_H

#include "manifold.h"

namespace gcop {
  
  using namespace std;
  using namespace Eigen;

  typedef Matrix<double, 6, 1> Vector6d;
  typedef Matrix<double, 9, 1> Vector9d;
  typedef Matrix<double, 12, 1> Vector12d;
  typedef Matrix<double, 12, 12> Matrix12d;
  
  struct UuvState {
    UuvState() {g.setIdentity(); v.setZero(); }
    virtual ~UuvState() {};

    Matrix4d g;  
    Vector6d v;  
  };
  
  class UuvManifold : public Manifold<UuvState, 12> {
    
  public:
    static UuvManifold& Instance();

    void Lift(Vector12d &v,
              const UuvState &xa,
              const UuvState &xb);      

    void Retract(UuvState &xb, 
                 const UuvState &xa,
                 const Vector12d &v);

    void dtau(Matrix12d &M, const Vector12d &v);

    void Adtau(Matrix12d &M, const Vector12d &v);

  private:
    UuvManifold();
  };  
}


#endif