#ifndef ENV_MAP_H #define ENV_MAP_H #include #include #include "Topology/generic/parameters.h" #include "Topology/map/embeddedMap2.h" #include "Algo/ImplicitHierarchicalMesh/ihm.h" #include "Algo/ImplicitHierarchicalMesh/subdivision.h" #include "Algo/Modelisation/polyhedron.h" #include "Algo/Modelisation/extrusion.h" #include "Algo/Modelisation/subdivision.h" #include "Algo/Geometry/centroid.h" #include "Algo/Geometry/area.h" #include "Geometry/bounding_box.h" #include "Container/fakeAttribute.h" #include "Algo/Parallel/parallel_foreach.h" #include "spatialHashing.h" using namespace CGoGN ; class Agent; class Obstacle; class MovingObstacle; class ArticulatedObstacle; #include "pfp.h" // #define EXPORTING3 //#define TWO_AND_HALF_DIM #ifdef EXPORTING3 #include "Utils/Shaders/shaderPhongTexture.h" #include "Utils/Shaders/shaderSimpleTexture.h" #include "shaderCustomTex.h" #include "Algo/Import/importObjTex.h" #endif class EnvMap { public: PFP::MAP map ; /* Geometry of the EnvMap : the bounding box of the scene * The density of cells in the EnvMap is given by two parameters : * - minCellSize : the size under which no subdivision occurs * - maxCellSize : the size of the initial cells (before subdivisions occur) * - obstacleDistance : the minimal goal distance between agents and obstacles * The number of cells in the initial EnvMap is about (width*height)/(size*size) */ Geom::BoundingBox geometry ; REAL maxCellSize ; REAL minCellSize ; REAL obstacleDistance ; unsigned int config; EnvMap() ; void init(unsigned int config, REAL width, REAL height, REAL minSize, REAL maxSize) ; void initGL(); void draw(); void scale(float val); bool is_insideConvexCell2D(VEC3 p, Dart d); bool isInsideFace3D(VEC3 p, Dart d); void markPedWay() ; unsigned int mapMemoryCost() ; #ifndef SPATIAL_HASHING Dart getBelongingCell(const VEC3& pos) ; Dart getBelongingCellOnSurface(const VEC3& pos) ; void subdivideAllToMaxLevel() ; void subdivideToProperLevel() ; void foreach_neighborFace(Dart d, FunctorType& f) ; void registerObstaclesInFaces() ; void registerWallInFaces(); void addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor) ; void setAgentNeighborsAndObstacles(Agent* agent) ; unsigned int totalNeighborSize(Dart d) ; void nbAgentsIncrease(Dart d) ; void nbAgentsDecrease(Dart d) ; void pushAgentInCells(Agent* agent, Dart d) ; void popAgentInCells(Agent* agent, Dart d) ; void agentChangeFace(Agent* agent, Dart oldFace) ; Dart popAndPushObstacleInCells(Obstacle* o, int n); void pushObstacleInCells(Obstacle * mo); void pushObstacleInOneRingCells(Obstacle * o, Dart d, int n); void pushObstacleInCells(Obstacle* o, int n, const std::vector& memo_cross); void popObstacleInCells(Obstacle* o, int n); void popObstacleInCells(Obstacle* o, Dart d) ; void refine() ; void coarse() ; void updateMap() ; int testOrientation(VEC3 p, VEC3 p1, VEC3 p2, Dart d); void resetAgentInFace(Agent* agent) ; #endif PFP::TVEC3 position ; PFP::TVEC3 normal ; Geom::BoundingBox bb; PFP::MAP mapScenary ; PFP::TVEC3 positionScenary ; PFP::TVEC3 normalScenary ; CellMarker obstacleMarkS ; CellMarker buildingMarkS ; CellMarker obstacleMark ; CellMarker buildingMark ; CellMarker pedWayMark ; // CellMarkerMemo memo_mark; // ajout moving obst void addObstAsNeighbor (Obstacle * o, const std::vector& belonging_cells, std::vector * nieghbor_cells); // void addMovingObstAsNeighbor (MovingObstacle * mo,std::vector belonging_cells, std::vector *neighbor_cells); void pushObstNeighborInCells(Obstacle* o, Dart d); void popObstNeighborInCells(Obstacle* o, Dart d); void find_next(Obstacle* o,Dart * d, CellMarkerMemo& cms); bool movingObstacleFree(Dart d); bool movingObstacleNeighborFree(Dart d); #ifdef EXPORTING3 std::vector m_map_Export; std::vector *> m_obj_Export; std::vector*> m_texture_Export; std::vector m_positionVBO_Export; std::vector m_normalVBO_Export; std::vector m_texcoordVBO_Export; std::vector m_shaderTex_Export; std::vector m_nbIndice_Export; // PFP2::MAP m_map_Export; // VertexAttribute position_Export ; // VertexAttribute normal_Export ; // Algo::Surface::Import::OBJModel * m_obj_Export; // // Utils::Texture<2,Geom::Vec3uc>* m_texture_Export; // Utils::VBO* m_positionVBO_Export; // Utils::VBO* m_normalVBO_Export; // Utils::VBO* m_texcoordVBO_Export; // ShaderCustomTex* m_shaderTex_Export; // // unsigned int m_nbIndice_Export; #endif std::vector newBuildings ; #ifndef SPATIAL_HASHING FaceAttribute subdivisableFace ; PFP::TAB_AGENTVECT agentvect ; PFP::TAB_AGENTVECT neighborAgentvect ; PFP::TAB_OBSTACLEVECT neighborObstvect; #endif PFP::TAB_OBSTACLEVECT obstvect ; #ifndef SPATIAL_HASHING static const unsigned int nbAgentsToSubdivide = 5 ; static const unsigned int nbAgentsToSimplify = 4 ; CellMarker refineMark ; CellMarker coarsenMark ; std::vector refineCandidate ; std::vector coarsenCandidate ; void clearUpdateCandidates() ; #endif #ifdef SPATIAL_HASHING unsigned int agentGridSize(unsigned int i) { return geometry.size(i) / minCellSize ; } unsigned int obstacleGridSize(unsigned int i) { return geometry.size(i) / obstacleDistance ; } Geom::Vec2ui agentPositionCell(Agent* a) ; Geom::Vec2ui obstaclePositionCell(VEC3 pos) { VEC3 relativePos = pos - geometry.min() ; relativePos /= obstacleDistance ; return Geom::Vec2ui(relativePos[0], relativePos[1]) ; } const std::vector& getNeighbors(Geom::Vec2ui c) { assert(ht_agents.hasData(c)) ; return ht_agents[c]; } const std::vector& getNeighbors(Agent* a) ; const std::vector& getNeighborAgents(Agent* a) ; void addAgentInGrid(Agent* a) ; void addAgentInGrid(Agent* a, Geom::Vec2ui c) ; void removeAgentFromGrid(Agent* a) ; void removeAgentFromGrid(Agent* a, Geom::Vec2ui c) ; HashTable2D< std::vector > ht_agents ; HashTable2D< std::vector > ht_neighbor_agents ; HashTable2D< std::vector > ht_obstacles ; #endif } ; void update_registration(Obstacle * o); void register_pop(Obstacle* o, int n); void resetPartSubdiv(Obstacle* o); //void resetObstPartInFace(Obstacle* o, Dart d, unsigned int fLevel);// empeche de viser une dart ayant disparu void resetObstPartInFace(Obstacle* o, Dart d);// empeche de viser une dart ayant disparu void resetPart(Obstacle * mo, Dart d); // empeche de viser une dart ayant disparu pour les voisins void displayMO(Obstacle * o); /************************************** * INLINE FUNCTIONS * **************************************/ template inline void addElementToVector(std::vector& a, T ag) { assert(std::find(a.begin(), a.end(), ag) == a.end()); a.push_back(ag) ; } template inline bool removeElementFromVector(std::vector& a, T ag) { assert(std::find(a.begin(), a.end(), ag) != a.end()); typename std::vector::iterator end = a.template end() ; for (typename std::vector::iterator it = a.begin(); it != end; ++it) { if (*it == ag) { *it = a.back() ; a.pop_back() ; return true ; } } return false ; } #ifndef SPATIAL_HASHING inline unsigned int EnvMap::totalNeighborSize(Dart d) { return agentvect[d].size() + neighborAgentvect[d].size() ; } inline void EnvMap::nbAgentsIncrease(Dart d) { if (refineMark.isMarked(d)) return ; if (totalNeighborSize(d) < nbAgentsToSubdivide) return ; std::pair& sf = subdivisableFace[d] ; if (sf.first == false || (sf.first == true && sf.second)) { refineMark.mark(d) ; refineCandidate.push_back(d) ; } } inline void EnvMap::nbAgentsDecrease(Dart d) { if (coarsenMark.isMarked(d) || refineMark.isMarked(d)) return ; if (totalNeighborSize(d) > nbAgentsToSimplify) return ; coarsenMark.mark(d) ; coarsenCandidate.push_back(map.faceOldestDart(d)) ; } inline void EnvMap::pushAgentInCells(Agent* agent, Dart d) { assert(map.getCurrentLevel() == map.getMaxLevel()) ; // assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) == agentvect[d].end()); // map.check(); // TraversorF tF(map); // for(Dart ddd = tF.begin() ; ddd != tF.end() ; ddd = tF.next()) // { // if(std::find(agentvect[ddd].begin(), agentvect[ddd].end(), agent) != agentvect[ddd].end()) // std::cout <(agentvect[d],agent); // agentvect[d].push_back(agent) ; // nbAgentsIncrease(d); Dart dd = d ; do { Dart ddd = map.alpha1(map.alpha1(dd)) ; while (ddd != dd) { if (!map.isBoundaryMarked2(ddd)) addElementToVector(neighborAgentvect[ddd],agent); // neighborAgentvect[ddd].push_back(agent) ; // nbAgentsIncrease(ddd); ddd = map.alpha1(ddd) ; } dd = map.phi1(dd) ; } while (dd != d) ; } inline void EnvMap::popAgentInCells(Agent* agent, Dart d) { assert(map.getCurrentLevel() == map.getMaxLevel()) ; removeElementFromVector(agentvect[d], agent) ; // nbAgentsDecrease(d) ; Dart dd = d ; do { Dart ddd = map.alpha1(map.alpha1(dd)) ; while (ddd != dd) { if (!map.isBoundaryMarked2(ddd)) removeElementFromVector(neighborAgentvect[ddd], agent) ; // nbAgentsDecrease(ddd) ; ddd = map.alpha1(ddd) ; } dd = map.phi1(dd) ; } while (dd != d) ; // TraversorF tF(map); // for(Dart ddd = tF.begin() ; ddd != tF.end() ; ddd = tF.next()) // { // if(std::find(agentvect[ddd].begin(), agentvect[ddd].end(), agent) != agentvect[ddd].end()) // std::cout <& belonging_cells, std::vector *neighbor_cells) { assert(map.getCurrentLevel() == map.getMaxLevel()); assert(!belonging_cells.empty()); neighbor_cells->clear(); // CellMarkerMemo memo_mark_speed(map);//marqueur additionnel (vitesse) CellMarkerMemo memo_mark(map); //marqueur des cellules "présentes" CellMarkerMemo OneRingMark(map); // marquer des cellules voisines for (std::vector::const_iterator it =belonging_cells.begin();it::const_iterator it=belonging_cells.begin(); Dart beg = NIL; Dart first =NIL; Dart d=NIL; Dart dd=NIL; /////////////////////////////////////////////boucle pour trouver une face du voisinage de l'obstacle ne contenant pas l'obstacle // CGoGNout<<"debut neighbors cellules : "; // for (std::vector::const_iterator it =belonging_cells.begin();it(neighborObstvect[d],o); } //inline void EnvMap::pushObstacleInCell(Obstacle* o, Dart d) //{ // assert(map.getCurrentLevel() == map.getMaxLevel()) ; // assert(!buildingMark.isMarked(d)) ; // // addElementToVector(obstvect[d],o); //} inline void EnvMap::clearUpdateCandidates() { refineCandidate.clear() ; coarsenCandidate.clear() ; } ///addMovingObstAsNeighbor est pour détecter les voisins du movingobstacle global afin de le considerer comme un super agent #endif #endif