grid.hpp

#ifndef GRID_H
#define GRID_H

#include "API/Grids/ThreeDGrid.hpp"
#include "API/Grids/TwoDGrid.hpp"

#include "API/planningAPI.hpp"



using namespace Eigen;

class GTPGrid : public API::ThreeDGrid
{
public:
    GTPGrid();
};

class GTPCell : public API::ThreeDCell
{
public:
    GTPCell();
};

class GTP2DCell;

class GTP2DGrid : public API::TwoDGrid
{
    MOVE3D_STATIC_LOGGER;
public:
    GTP2DGrid();
    GTP2DGrid(double pace, std::vector<double> envSize);

    bool initCylinders();

    void addAgentAndComputeNav(Robot* robot);
    void propagateDist(Robot* robot);
    void initAllCells();
    std::vector<GTP2DCell*> getCellsWithinReach(double xCenter, double yCenter, double reach, Robot* agent);


    double getNbCellX() {return _nbCellsX;}
    double getNbCellY() {return _nbCellsY;}

    void draw();


    void createAllCells();
    GTP2DCell* createNewCell2(unsigned int index,unsigned  int x,unsigned  int y );

    std::vector<GTP2DCell*> getPath(Robot *r, GTP2DCell* targetCell);
    std::vector<GTP2DCell*> getHumanAwarePath(Robot *r, GTP2DCell* targetCell, double distWeight, double Hweight,
                                         double angleWeight, double distHWeight, double distWeight2);

    std::vector<GTP2DCell*> getCellsArroundSupport(Robot* agent, Robot *support);
    std::vector<GTP2DCell*> getCellsArroundObject(Robot* agent, Robot* manipulable);
    std::vector<GTP2DCell*> getCellsArroundAgent(Robot* agent, Robot* targetAgent);

    void setCurrentRobot(Robot* rob);

    int getNbExploredCells(){return nbExploredCells;}


private:
    std::vector<Robot*> _robotList;
    Robot* _humCyl;
    Robot* _robotCyl;
    int nbExploredCells;

};

enum cellState {
    NotTested,
    Open,
    Closed
};




class GTP2DCell : public API::TwoDCell
{
    MOVE3D_STATIC_LOGGER;
public:
    GTP2DCell();
    GTP2DCell(int i, Eigen::Vector2i coord, Eigen::Vector2d corner, GTP2DGrid* grid);
    std::vector<GTP2DCell*> getNeighboors();
    void setState(cellState s);
    cellState getState() {return _state;}
    void initialize();
    void addToNavigability(Robot* robot);

    double dist(GTP2DCell* c);
    void print();

    Eigen::Vector2i getCoord() { return _Coord; }

    bool getNavigability(Robot* r);

    bool toBeDrawn();
    void setDrawn(bool v) {_toBeDrawn = v;}


    void setPrev(Robot* r, GTP2DCell* c);
    GTP2DCell* getPrev(Robot* r);

    void setDist(Robot* r, double d);
    double getDist(Robot* r);
    bool isReachable(Robot* r);

    void setScore(double f){f_score = f;}
    double getScore() {return f_score;}

    double computeDistTo(GTP2DCell* c);

    void setIsInCol(bool val) { _collisionCheck = val;}
    bool isInCol() { return _collisionCheck;}


    void setCurrentRobot(Robot* rob) {_currentRobot = rob;}
    double getDistCurrentRobot();

    double Ecolor;

private:
    bool _toBeDrawn;
    cellState _state;
    Eigen::Vector2i _Coord;

    std::vector< std::pair<Robot*,bool> > navigability;

    std::map<Robot*,double> distances_to_init;
    std::map<Robot*,GTP2DCell*> previous_to_init;

    bool _collisionCheck;

    Robot* _currentRobot;

    double f_score;
};



#endif // GRID_H