P3d.hpp

#ifndef _P3D_P3D_H_
#define _P3D_P3D_H_

#include <libp3d.h>

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

#include <string>
#include <vector>

#include "p3d/P3D_MAP.hpp"

namespace jafar {
  namespace p3d {
    class P3DPlanner {
      private:
        ::P3DPlanner * _pdata; 
      public:
        P3DPlanner();

        ~P3DPlanner();

        ::P3DPlanner* get_data() const;

        void configureGeom(double len[3], double rad[2], double arm[2],
            double bodyLength, double bodyWidth,
            double zBody, double groundClear,
            double rollLimits[3], double pichLimits[3], 
            double alphaLimits[3], double betaLimits[3],
            double unkMax);

        void configureRobot(double scale, double rollStep, double vMin, double vMax,
                  double wMax, double accel, double decel);
        void configureArcs(int nArcs, int nNodes, int depth, double rMin, 
            double goalTol, double dMin);
    };


    class P3DRobotConfig {
      private:
        ::P3DRobotConfig * _pdata; 
      public:
        P3DRobotConfig();

        ~P3DRobotConfig();

        ::P3DRobotConfig * get_data() const;

        void display() const;

        double quality() const;

        /*
         * The method to_T3D returns a geom::T3D representing the 3D
         * position of the robot.
         *
         * @param t3d represents the t3d which will contain the position
         * of the robot
         * @return Nothing.
         * @return t3d is modified.
         */
        void to_T3D(jafar::geom::T3DEuler & t3d) const;
    };

    class P3DPlanSolution {
      private:
        ::P3DPlanSolution * _pdata; 
        void clear_data();

      public:
        P3DPlanSolution();

        P3DPlanSolution(int depth, int nNodes);

        ~P3DPlanSolution();

        double curv() const;

        double d() const;

        double cost() const;

        /*
         */
        int depth() const;

        /*
         * @return the speed of the solution
         */
        double v() const;

        /*
         * set the speed of the robot for the next call
         * @return Nothing.
         */
        void set_v(double v);

        /*
         * set the speed of the robot for the next call
         * @return Nothing.
         */
        void set_w(double w);

        /*
         * set the average time call of the robot for the next call
         * @return Nothing.
         */
        void set_time(double time);

        double alpha() const;

        double time() const;

        void display_arc_cost() const;


        ::P3DPlanSolution* get_data();

        void set_data(int depth, int nNodes);

        std::string gdhe_display(jafar::geom::T3DEuler & m2o) const;
    };


    enum P3D_algo {
      ASTAR,
      FULL_BALANCED
    };

    class P3d {
      private:
        jafar::p3d::P3DPlanner _planner;   
        jafar::p3d::P3D_MAP _map; 
        jafar::p3d::P3DPlanSolution _sol; 
        jafar::p3d::P3D_algo _algo; 
      public:
        P3d(jafar::p3d::P3D_algo p3d_algo);

        ~P3d();

        jafar::p3d::P3DRobotConfig* placeRobot(double x, double y, double theta);

        int plan(jafar::geom::T3D& m2o, jafar::geom::T3D& g2o);

        bool check_trajectory(jafar::geom::T3D& m2o, double alpha);
         
        int aco(jafar::geom::T3D& mainToOrigin, double alpha, double v,
            double left_alpha, double right_alpha);

        int evo(jafar::geom::T3D& mainToOrigin, double alpha, double v);
            
        jafar::p3d::P3D_MAP& map() 
        {
          return _map;
        };

        const jafar::p3d::P3D_MAP& map() const
        {
          return _map;
        };

        /*
         * @return a reference to the internal planner
         */
        jafar::p3d::P3DPlanner& planner() 
        {
          return _planner;
        };

        /*
         * @return a constant reference to the internal planner
         */
        const jafar::p3d::P3DPlanner& planner() const
        {
          return _planner;
        };


        /*
         * @return a reference to the internal solution
         */
        jafar::p3d::P3DPlanSolution& solution() 
        {
          return _sol;
        };

        /*
         * @return a constant reference to the internal solution
         */
        const jafar::p3d::P3DPlanSolution& solution() const
        {
          return _sol;
        };

        static std::string conffile; 
    };

    class P3dArcDumper {
      private:
        ::P3DArcDump * _pdata; 
        jafar::geom::T3DEuler _m2o; 
        int _n_arcs; 
        int _max_depth; 
        bool has_valid_cost(int, int) const;
        void _dump(bool, std::ostream &) const;

        int evaluate_nb_arcs(int nArcs, int depth);

      public:
        P3dArcDumper(const jafar::p3d::P3d & p3d, jafar::geom::T3D& m2o);

        ~P3dArcDumper();

        void update(const jafar::p3d::P3d & p3d, jafar::geom::T3D& m2o);

        std::string gdhe_display() const;

        void display(bool only_valid) const;

        void display_to_file(bool only_valid, const std::string & path) const;

        int set_next_node_coords(jafar::geom::T3DEuler & pos) const;
    };
  }
}


#endif /* _P3D_P3D_H_ */