mightabilitiesGrids.hpp

#ifndef MIGHTABILITIESGRIDS_HPP
#define MIGHTABILITIESGRIDS_HPP

#include "grid.hpp"
#include "API/Grids/ThreeDGrid.hpp"
#include "API/Grids/TwoDGrid.hpp"
#include "worldState.hpp"
#include "P3d-pkg.h"

#include "API/planningAPI.hpp"

#include "move3d-headless.h"
#include "Planner-pkg.h"
#include "Collision-pkg.h"

class MightabilitiesCell;
class WorldState;

enum typeCells {
    agentC,
    reachDist,
    reachCol,
    reachDistSupp,
    reachColSupp,
    visDist,
    visCol,
    visDistSupp,
    visColSupp,
    typeCellSize
};


class agentLinkedCells
{
public:
    agentLinkedCells(){}
    Robot* agent;
    std::map<typeCells, std::vector<MightabilitiesCell*> > cells;
};


class typeCellLists {
public:
    typeCellLists(){}
    std::vector<MightabilitiesCell*> getListFromType(typeCells tc);
    void addCellTo(MightabilitiesCell* c, typeCells tc);


    std::vector<std::pair<typeCells, std::vector<MightabilitiesCell*> > > lists;
};

class MightabilitiesGrid : public API::ThreeDGrid
{
public:
    MightabilitiesGrid();
    MightabilitiesGrid( Eigen::Vector3i size,    std::vector<double> envSize);
    MightabilitiesGrid( double samplingRate,     std::vector<double> envSize);
    MightabilitiesGrid( const ThreeDGrid& grid);

    void initVariables();

    void init(Eigen::Vector3i size,    std::vector<double> envSize);
    void init(double samplingRate ,    std::vector<double> envSize);

    void setVisBall();
    void placeVisball(MightabilitiesCell* c);
    void placeVisball(p3d_point p);

    void computeMights(std::vector<Robot*> agents, std::vector<Robot *> supports, std::vector<typeCells> abilities);

    std::vector<MightabilitiesCell*> getCellsFromEntity(Robot* entity);

    std::vector<MightabilitiesCell*> mergeVectors(std::vector<MightabilitiesCell*> c1,std::vector<MightabilitiesCell*> c2);

    void computeReachDist(Robot* ag, MightabilitiesCell* cells);
    void computeReachCol(Robot* ag, MightabilitiesCell* cells);
    void computeVisDist(Robot* ag, MightabilitiesCell* cells);
    void computeVisCol(Robot* ag, MightabilitiesCell* cells);
    bool computeVisballCollision();

    bool testLineArea(MightabilitiesCell* cells, p3d_point p);



//    void init(double samplingRate, vector<Robot*> ag);

    API::ThreeDCell* createNewCell(unsigned int index, unsigned int x, unsigned int y, unsigned int z );

    void draw();

    MightabilitiesCell* getCell(int i);

    WorldState* getWorldState() {return _WS;}
    void setWorldState(WorldState* ws) {_WS = ws;}
    void show();
    void show(std::vector< std::pair<Robot*,typeCells> > toShow);

    void setSampleRate(double SR) { _samplingRate = SR; }

    std::vector<MightabilitiesCell*> getList(Robot* ag);
    std::vector<MightabilitiesCell*> getList(Robot* ag, typeCells tc);
    std::vector<MightabilitiesCell*> getList(std::vector< std::pair<Robot*,typeCells> > rpc);

    void addCellToList(MightabilitiesCell* c, Robot* ag, typeCells tc);


private:
    WorldState* _WS;

    double _samplingRate;
    Robot* _visball;
    std::vector<agentLinkedCells> _ALC;

    std::map<Robot*, typeCellLists* > listMapAgentsType;

    std::vector< std::pair<Robot*,typeCells> > _toShow;
    std::vector<MightabilitiesCell*> _allWorldCells;

};


class MightabilitiesCell : public API::ThreeDCell
{
public:
    MightabilitiesCell();
    MightabilitiesCell(int i, MightabilitiesGrid* grid);
    MightabilitiesCell(int i, Eigen::Vector3d corner, MightabilitiesGrid* grid);
    MightabilitiesCell(MightabilitiesCell* c);

    void draw();

    void addAgent(Robot* ag);
    void removeAgent(Robot* ag);
    bool belongToAgent(Robot* ag);
    bool belongToAtLeastOneAgent();

    void addTypeC(Robot* ag, typeCells tc);
    void removeTypeC(Robot *ag, typeCells tc);
    bool belongToTypeC(Robot* ag, typeCells tc);

    void setObject(Robot* r){_object = r;}
    Robot* getObject(){return _object;}

    void setIsSupport(bool v) {isSupport = v;}
    bool isItSupport() {return isSupport;}

private:
    std::vector< std::pair<Robot*, typeCells> > agentTypeBelongings;

    Robot* _object;
    bool isSupport;

};


#endif // MIGHTABILITIESGRIDS_HPP