Stereosimu.hpp

#ifndef _JAFAR_STEREOSIMU_STEREOSIMU_HPP_
#define _JAFAR_STEREOSIMU_STEREOSIMU_HPP_

#include "dtm/Dtm.hpp"
#include "geom/t3dEuler.hpp"
#include "calife/Image3d.hpp"

#include <string>
#include <vector>

namespace jafar {
  namespace stereosimu {

    struct CosSin {
      double _angle; 
      double _cos;  
      double _sin;  
      CosSin() {};
      CosSin(double ang, double cos, double sin): _angle(ang) ,_cos(cos), _sin(sin) {};
      ~CosSin() {};
    };

    struct Point {
      double x;
      double y;
      double z;

      Point() {};
      Point(double _x, double _y, double _z): x(_x), y(_y), z(_z) {};
      ~Point() {};

      void change_frame(jafar::geom::T3D& t3d);

      friend std::ostream& operator << (std::ostream& o , const Point& p);
    };

    class Stereosimu {
      public:
        Stereosimu(jafar::geom::T3D& s2m, double delta_angle, double angle, double max);

        ~Stereosimu();

        void load_dtm(const std::string & path); 

        void dtm_scale(double scale);

        double dtm_scale() const;

        double cam_angle() const;

        void cam_angle(double angle);

        const jafar::geom::T3D&  cam_transfo() const;

        jafar::geom::T3D&  cam_transfo();

        void cam_transfo(jafar::geom::T3D& t3d);


        void get_im3d(jafar::geom::T3D & m2r, jafar::calife::Image3d & im3d); 

      private:
        geom::T3DEuler _sensorToMain; 
        double _max_vision;

        double _cam_angle_delta;
        double _cam_angle;
        std::vector <CosSin> _cos_sin_table;

        jafar::dtm::Dtm * _dtm;
        double _dtm_scale;
        double _dtm_x_orig;
        double _dtm_y_orig;
        void clear_dtm();
        void update_dtm_info();

        bool get_intersection(Point & A, Point & B, Point & res);
        bool check_point(Point & pt);
    };    
  }
}

#endif /* _JAFAR_STEREOSIMU_STEREOSIMU_HPP_ */