From 658baf8ffb4ba7ac1772cd3ec63bff44bc94bfdc Mon Sep 17 00:00:00 2001 From: Pierre Kraemer Date: Thu, 3 Feb 2011 17:27:30 +0100 Subject: [PATCH] suppression SocialAgents correction unsewFaces + edgeCollapse dans embeddedMap2 ajout simplif faces IHM --- Apps/CMakeLists.txt | 2 - .../SocialAgents/Debug/CMakeLists.txt | 26 - .../SocialAgents/Release/CMakeLists.txt | 25 - Apps/Examples/SocialAgents/include/agent.h | 82 -- .../SocialAgents/include/env_generator.h | 51 -- .../SocialAgents/include/env_generator.hpp | 685 ---------------- Apps/Examples/SocialAgents/include/env_map.h | 130 ---- .../SocialAgents/include/env_render.h | 65 -- .../SocialAgents/include/mainwindow.h | 8 - .../SocialAgents/include/path_finder.h | 21 - .../SocialAgents/include/path_finder.hpp | 78 -- .../Examples/SocialAgents/include/simulator.h | 80 -- Apps/Examples/SocialAgents/include/utils.h | 95 --- Apps/Examples/SocialAgents/include/viewer.h | 90 --- Apps/Examples/SocialAgents/src/agent.cpp | 511 ------------ Apps/Examples/SocialAgents/src/env_map.cpp | 733 ------------------ Apps/Examples/SocialAgents/src/simulator.cpp | 411 ---------- Apps/Examples/SocialAgents/src/viewer.cpp | 702 ----------------- include/Topology/generic/embeddedMap2.hpp | 38 +- src/Algo/ImplicitHierarchicalMesh/ihm.cpp | 44 +- 20 files changed, 68 insertions(+), 3809 deletions(-) delete mode 100644 Apps/Examples/SocialAgents/Debug/CMakeLists.txt delete mode 100644 Apps/Examples/SocialAgents/Release/CMakeLists.txt delete mode 100644 Apps/Examples/SocialAgents/include/agent.h delete mode 100644 Apps/Examples/SocialAgents/include/env_generator.h delete mode 100644 Apps/Examples/SocialAgents/include/env_generator.hpp delete mode 100644 Apps/Examples/SocialAgents/include/env_map.h delete mode 100644 Apps/Examples/SocialAgents/include/env_render.h delete mode 100644 Apps/Examples/SocialAgents/include/mainwindow.h delete mode 100644 Apps/Examples/SocialAgents/include/path_finder.h delete mode 100644 Apps/Examples/SocialAgents/include/path_finder.hpp delete mode 100644 Apps/Examples/SocialAgents/include/simulator.h delete mode 100644 Apps/Examples/SocialAgents/include/utils.h delete mode 100644 Apps/Examples/SocialAgents/include/viewer.h delete mode 100644 Apps/Examples/SocialAgents/src/agent.cpp delete mode 100644 Apps/Examples/SocialAgents/src/env_map.cpp delete mode 100644 Apps/Examples/SocialAgents/src/simulator.cpp delete mode 100644 Apps/Examples/SocialAgents/src/viewer.cpp diff --git a/Apps/CMakeLists.txt b/Apps/CMakeLists.txt index 01454332..49363caa 100644 --- a/Apps/CMakeLists.txt +++ b/Apps/CMakeLists.txt @@ -9,8 +9,6 @@ ELSE(WIN32) add_subdirectory(Examples/Release) add_subdirectory(Examples/Debug) add_subdirectory(Examples/Tests) - add_subdirectory(Examples/SocialAgents/Release) - add_subdirectory(Examples/SocialAgents/Debug) add_subdirectory(Tuto) ENDIF(WIN32) diff --git a/Apps/Examples/SocialAgents/Debug/CMakeLists.txt b/Apps/Examples/SocialAgents/Debug/CMakeLists.txt deleted file mode 100644 index 1b964f51..00000000 --- a/Apps/Examples/SocialAgents/Debug/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 2.6) - -project(SocialAgents) - -SET(CMAKE_BUILD_TYPE Debug) - -link_directories( - ${CGoGN_ROOT_DIR}/lib/Debug/ - ${CGoGN_ROOT_DIR}/lib/Release/ -) - -# define includes path -include_directories( - /usr/include/libxml2/ - ${CGoGN_ROOT_DIR}/Apps/Examples/SocialAgents/include - ${CGoGN_ROOT_DIR}/include - ${CGoGN_ROOT_DIR}/ThirdParty/AntTweakBar/include - ${CGoGN_ROOT_DIR}/ThirdParty/Numerical - ${CGoGN_ROOT_DIR}/ThirdParty/Numerical/UFconfig - ${CGoGN_ROOT_DIR}/ThirdParty/gzstream - ${CGoGN_ROOT_DIR}/ThirdParty/Zinri -) - -#define exec to compile -add_executable(socialAgentsD ../src/viewer.cpp ../src/env_map.cpp ../src/agent.cpp ../src/simulator.cpp ) -target_link_libraries( socialAgentsD containerD topologyD utilsD algoD ${COMMON_LIBS} ) diff --git a/Apps/Examples/SocialAgents/Release/CMakeLists.txt b/Apps/Examples/SocialAgents/Release/CMakeLists.txt deleted file mode 100644 index 19a98ad5..00000000 --- a/Apps/Examples/SocialAgents/Release/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -cmake_minimum_required(VERSION 2.6) - -project(SocialAgents) - -SET(CMAKE_BUILD_TYPE Release) - -link_directories( - ${CGoGN_ROOT_DIR}/lib/Release/ -) - -# define includes path -include_directories( - /usr/include/libxml2/ - ${CGoGN_ROOT_DIR}/Apps/Examples/SocialAgents/include - ${CGoGN_ROOT_DIR}/include - ${CGoGN_ROOT_DIR}/ThirdParty/AntTweakBar/include - ${CGoGN_ROOT_DIR}/ThirdParty/Numerical - ${CGoGN_ROOT_DIR}/ThirdParty/Numerical/UFconfig - ${CGoGN_ROOT_DIR}/ThirdParty/gzstream - ${CGoGN_ROOT_DIR}/ThirdParty/Zinri -) - -#define exec to compile -add_executable(socialAgents ../src/viewer.cpp ../src/env_map.cpp ../src/agent.cpp ../src/simulator.cpp ) -target_link_libraries( socialAgents container topology utils algo ${COMMON_LIBS} ) diff --git a/Apps/Examples/SocialAgents/include/agent.h b/Apps/Examples/SocialAgents/include/agent.h deleted file mode 100644 index 8c9bd50a..00000000 --- a/Apps/Examples/SocialAgents/include/agent.h +++ /dev/null @@ -1,82 +0,0 @@ -#ifndef M_AGENT_H -#define M_AGENT_H - -// #include "Definitions.h" -#include - -#include "utils.h" -#include "env_map.h" -#include "Algo/MovingObjects/particle_cell_2D.h" - -class Simulator; - - class Agent - { - public: -// typedef RVO::Vector2 VEC_A; - typedef VEC3 VEC_A; - - explicit Agent(Simulator* sim, size_t agentNo); - - Agent(Simulator* sim, const VEC_A& position, size_t agentNo); - - Agent(Simulator* sim, const VEC_A& position, size_t agentNo, Dart d); - - Agent(Simulator* sim, const VEC_A& position, float neighborDist, - size_t maxNeighbors, float timeHorizon, float timeHorizonObst, - float radius, const VEC_A& velocity, float maxSpeed, size_t agentNo); - - ~Agent(); - - void setGoalNo(size_t goal); - - void computeNewVelocity(); - - void insertAgentNeighbor(size_t agentNo, float& rangeSq); - -// void insertObstacleNeighbor(size_t obstacleNo, float rangeSq); - void insertObstacleNeighbor(Dart d); - - void update(); - - std::vector > agentNeighbors_; - size_t maxNeighbors_; - float maxSpeed_; - float neighborDist_; -// RVO::Vector2 newVelocity_; - VEC_A newVelocity_; -// std::vector > obstacleNeighbors_; - std::vector > obstacleNeighbors_; - std::vector orcaLines_; -// RVO::Vector2 position_; -// RVO::Vector2 prefVelocity_; - VEC_A position_; - VEC_A prefVelocity_; - float radius_; - Simulator* sim_; - float timeHorizon_; - float timeHorizonObst_; -// RVO::Vector2 velocity_; - VEC_A velocity_; - - size_t agentNo_; - size_t goalNo_; -// Dart nearestDart; -// std::vector includingFaces; - CGoGN::Algo::MovingObjects::ParticleCell2D * part; - float rangeSq; - bool treated; - }; - - bool linearProgram1(const std::vector& lines, size_t lineNo, - float radius, const Agent::VEC_A& optVelocity, - bool directionOpt, Agent::VEC_A& result); - - bool linearProgram2(const std::vector& lines, size_t num, float radius, - const Agent::VEC_A& optVelocity, bool directionOpt, - Agent::VEC_A& result); - - void linearProgram3(const std::vector& lines, size_t numObstLines, - float radius, Agent::VEC_A& result); - -#endif diff --git a/Apps/Examples/SocialAgents/include/env_generator.h b/Apps/Examples/SocialAgents/include/env_generator.h deleted file mode 100644 index fceb1ea1..00000000 --- a/Apps/Examples/SocialAgents/include/env_generator.h +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef ENV_GENERATOR -#define ENV_GENERATOR - -namespace CGoGN -{ - -namespace CityGenerator -{ - -template -void generateAbsolutSpiralOfDeath(typename PFP::MAP& map,typename PFP::TVEC3& position, DartMarker& closeMark, unsigned int side, float rMin, float rMax,float nbTurns); - -template -void generateToboggan(typename PFP::MAP& map,typename PFP::TVEC3& position, DartMarker& closeMark, unsigned int side, float rMin, float rMax,float nbTurns); - -template -void generateSmallCity(typename PFP::MAP& map, EMBV& position, DartMarker& closeMark, float sideSize); - -template -void generateCity(typename PFP::MAP& map, EMBV& position, DartMarker& closeMark, float sideSize); - -template -void animateCity(typename PFP::MAP& map, EMBV& position, EMBAGENT& agents, DartMarker& closeMark, std::vector& newBuilding); - -template -bool notEightAdjacentToAnObstacle(typename PFP::MAP& map, Dart d, DartMarker& closeMark); - -template -void generateGrid(typename PFP::MAP& map, EMBV& position, unsigned int cX,unsigned int cY,float sideLength, DartMarker& closeMark); - -template -void markObstacleSteepFaces(typename PFP::MAP& map, EMBV& position, float steepness, DartMarker& closeMark); - -template -void generatePathToUpperStair(typename PFP::MAP& map, EMBV& position, Dart dLower, Dart dUpper, DartMarker& closeMark); - -template -void generateBuilding(typename PFP::MAP& map, EMBV& position, Dart d,DartMarkerStore& build, float height, float side, unsigned int buildingType, DartMarker& closeMark); - -template -static void generateBridge(typename PFP::MAP& map, EMBV& position, Dart dStart, Dart dStop,float bridgeHeight, DartMarker& closeMark); - -template -void generateBridge(typename PFP::MAP& map, EMBV& position, unsigned int cX, unsigned int cY,float sideLength, DartMarker& closeMark); - -} - -} - -#include "env_generator.hpp" -#endif \ No newline at end of file diff --git a/Apps/Examples/SocialAgents/include/env_generator.hpp b/Apps/Examples/SocialAgents/include/env_generator.hpp deleted file mode 100644 index 5b487c64..00000000 --- a/Apps/Examples/SocialAgents/include/env_generator.hpp +++ /dev/null @@ -1,685 +0,0 @@ -namespace CGoGN -{ - -namespace CityGenerator -{ - -template -void generateAbsolutSpiralOfDeath(typename PFP::MAP& map,typename PFP::TVEC3& position, DartMarker& closeMark, unsigned int side, float rMin, float rMax,float nbTurns) -{ - Algo::Modelisation::Polyhedron prim(map,position); - prim.grid_topo(side,1); - -// prim.embedCircle(rMin,rMax); - prim.embedHelicoid(rMin,rMax,nbTurns*20.0f,nbTurns); -// prim.embedGrid(100,100); - - CellMarker traite(map,FACE_CELL); - for(Dart d = map.begin(); d!=map.end() ; map.next(d)) { - if(!traite.isMarked(d)) { - traite.mark(d); - map.splitFace(d,map.phi1(map.phi1(d))); - } - } - - map.closeMap(closeMark) ; - - map.initOrbitEmbedding(FACE_ORBIT); -} - -template -void generateToboggan(typename PFP::MAP& map,typename PFP::TVEC3& position, DartMarker& closeMark, unsigned int side, float rMin, float rMax,float nbTurns) -{ - //the stairs - Algo::Modelisation::Polyhedron prim(map,position); - prim.grid_topo(side,3); - //the slide - Algo::Modelisation::Polyhedron prim2(map,position); - prim2.grid_topo(side/4,3); - - float height=150; - - prim2.embedHelicoid(rMin*4,rMax,-1*height,0.5f,-1); - -// typename PFP::VEC3 transl(0,2*rMax,height); - typename PFP::VEC3 transl(-rMax/2,0,height); - CellMarker m(map, VERTEX_CELL) ; - for(Dart d=map.begin();d!=map.end();map.next(d)) { - if(!m.isMarked(d)) { - m.mark(d); - position[d] += transl; - position[d][0] *= 2.0f; - } - } - - prim.embedHelicoid(rMin,rMax,height,nbTurns-0.5f); - - Dart dStairsDown = map.phi_1(prim.getDart()); - Dart dStairsUp = dStairsDown; - do { - closeMark.markOrbit(FACE_ORBIT,dStairsUp); - closeMark.markOrbit(FACE_ORBIT,map.phi2(map.phi1(map.phi1(map.phi2(map.phi_1(dStairsUp)))))); - dStairsUp= map.phi2(map.phi1(map.phi1(dStairsUp))); - } while(map.phi2(dStairsUp)!=dStairsUp); - - Dart dSlideUp = map.phi_1(prim2.getDart()); - Dart dSlideDown = dSlideUp; - do { - closeMark.markOrbit(FACE_ORBIT,dSlideDown); - closeMark.markOrbit(FACE_ORBIT,map.phi2(map.phi1(map.phi1(map.phi2(map.phi_1(dSlideDown)))))); - dSlideDown= map.phi2(map.phi1(map.phi1(dSlideDown))); - } while(map.phi2(dSlideDown)!=dSlideDown); - - - Algo::Modelisation::Polyhedron primPathUp(map,position); - primPathUp.grid_topo(1,3); - Dart dPathUp = map.phi_1(primPathUp.getDart()); - - Algo::Modelisation::Polyhedron primPathDown(map,position); - primPathDown.grid_topo(1,3); - Dart dPathDown = map.phi_1(primPathDown.getDart()); - - closeMark.markOrbit(FACE_ORBIT,dPathUp); - closeMark.markOrbit(FACE_ORBIT,dPathDown); - - map.sewFaces(dStairsUp,dPathUp); - map.sewFaces(dSlideUp,map.phi1(map.phi1(dPathUp))); - map.sewFaces(dSlideDown,dPathDown); - map.sewFaces(dStairsDown,map.phi1(map.phi1(dPathDown))); - - dPathUp = map.phi_1(map.alpha1(dPathUp)); - dPathDown = map.phi_1(map.alpha1(dPathDown)); - - dSlideUp = map.phi_1(map.alpha1(dSlideUp)); - dSlideDown =map.alpha_1(map.phi1(dSlideDown)); - dStairsUp = map.alpha_1(map.phi1(dStairsUp)); - dStairsDown = map.phi_1(map.alpha1(dStairsDown)); - -// Dart dPathUp = map.newOrientedFace(4); -// Dart dPathDown = map.newOrientedFace(4); - - map.sewFaces(dStairsUp,dPathUp); - map.sewFaces(dSlideUp,map.phi1(map.phi1(dPathUp))); - map.sewFaces(dSlideDown,dPathDown); - map.sewFaces(dStairsDown,map.phi1(map.phi1(dPathDown))); - - dPathUp = map.phi_1(map.alpha1(dPathUp)); - dPathDown = map.phi_1(map.alpha1(dPathDown)); - dSlideUp = map.phi_1(map.alpha1(dSlideUp)); - dStairsUp = map.alpha_1(map.phi1(dStairsUp)); - dSlideDown =map.alpha_1(map.phi1(dSlideDown)); - dStairsDown = map.phi_1(map.alpha1(dStairsDown)); - - closeMark.markOrbit(FACE_ORBIT,dPathUp); - closeMark.markOrbit(FACE_ORBIT,dPathDown); - - map.sewFaces(dStairsUp,dPathUp); - map.sewFaces(dSlideUp,map.phi1(map.phi1(dPathUp))); - map.sewFaces(dSlideDown,dPathDown); - map.sewFaces(dStairsDown,map.phi1(map.phi1(dPathDown))); - -// map.sewFaces(dStairsDown,dPathDown); -// map.sewFaces(dSlideDown,map.phi1(map.phi1(dPathDown))); - -// map.sewFaces(dStairs,dSlide); - - map.closeMap(closeMark) ; - CellMarker traite(map,FACE_CELL); - for(Dart d = map.begin(); d!=map.end() ; map.next(d)) { - if(!traite.isMarked(d)) { - traite.mark(d); - map.splitFace(d,map.phi1(map.phi1(d))); - } - } - - for(Dart d = map.begin(); d!=map.end() ; map.next(d)) { - if(closeMark.isMarked(d)) { - closeMark.markOrbit(FACE_ORBIT,d); - } - } - - map.initOrbitEmbedding(FACE_ORBIT); -} - -template -void generateSmallCity(typename PFP::MAP& map, EMBV& position, DartMarker& closeMark, float sideSize) -{ - DartMarkerStore obj(map); -// sideSize *= 0.2f; - unsigned int nbBuilding=1000; - float height = sideSize/2.0f; - unsigned int side = 5; - generateGrid(map,position,side,side,sideSize,closeMark); - - Dart dEnd = map.end(); - for(Dart d = map.begin(); d!=dEnd && nbBuilding>0; map.next(d)) { - if(!obj.isMarked(d) && (rand()%20==0) && notEightAdjacentToAnObstacle(map,d,closeMark)) { - typename PFP::VEC3 n1 = Algo::Geometry::faceNormal(map,d,position); - typename PFP::VEC3 n2 = Algo::Geometry::faceNormal(map,map.phi2(d),position); - if(position[d][2]==0.0f && position[map.phi1(d)][2]==0.0f && position[map.phi_1(d)][2]==0.0f && n1[0]==0.0f && n1[1]==0.0f && n1[2]==1.0f && n1[0]==n2[0] && n1[1]==n2[1] && n1[2]==n2[2] && map.faceDegree(d)==4) { - obj.markOrbit(FACE_ORBIT,d); - generateBuilding(map,position,d,obj,(1+(rand()%3))*height/2.0f,side,rand()%2,closeMark); - --nbBuilding; - } - } - } - - for(Dart d = map.begin(); d!=map.end(); map.next(d)) { - if(closeMark.isMarked(d) && map.faceDegree(d)>4) { - Dart dd=d; - do { - closeMark.markOrbit(DART_ORBIT,dd); - dd = map.phi1(dd); - } while(position[dd][2]==0.0f && position[map.phi1(dd)][2]==0.0f && dd!=d); - - dd=d; - do { - closeMark.markOrbit(DART_ORBIT,dd); - dd = map.phi_1(dd); - } while(position[dd][2]==0.0f && position[map.phi1(dd)][2]==0.0f && dd!=d); - } - } - - map.closeMap(closeMark); -} - -template -void generateCity(typename PFP::MAP& map, EMBV& position, DartMarker& closeMark, float sideSize) -{ - DartMarkerStore obj(map); - unsigned int nbBuilding=100; -// unsigned int nbBuilding=20; - float height = sideSize; -// unsigned int side = 50; - unsigned int side = 40; -// unsigned int side = 20; -// unsigned int side = 10; - generateGrid(map,position,side,side,height,closeMark); - Dart dEnd = map.end(); - for(Dart d = map.begin(); d!=dEnd && nbBuilding>0; map.next(d)) { - if(!obj.isMarked(d) && (rand()%20==0) && notEightAdjacentToAnObstacle(map,d,closeMark)) { - typename PFP::VEC3 n1 = Algo::Geometry::faceNormal(map,d,position); - typename PFP::VEC3 n2 = Algo::Geometry::faceNormal(map,map.phi2(d),position); - if(position[d][2]==0.0f && position[map.phi1(d)][2]==0.0f && position[map.phi_1(d)][2]==0.0f && n1[0]==0.0f && n1[1]==0.0f && n1[2]==1.0f && n1[0]==n2[0] && n1[1]==n2[1] && n1[2]==n2[2] && map.faceDegree(d)==4) { - obj.markOrbit(FACE_ORBIT,d); - generateBuilding(map,position,d,obj,(1+(rand()%3))*height/2.0f,side,rand()%4,closeMark); - --nbBuilding; - } - } - } - - for(Dart d = map.begin(); d!=map.end(); map.next(d)) { - if(closeMark.isMarked(d) && map.faceDegree(d)>4) { - Dart dd=d; - do { - closeMark.markOrbit(DART_ORBIT,dd); - dd = map.phi1(dd); - } while(position[dd][2]==0.0f && position[map.phi1(dd)][2]==0.0f && dd!=d); - - dd=d; - do { - closeMark.markOrbit(DART_ORBIT,dd); - dd = map.phi_1(dd); - } while(position[dd][2]==0.0f && position[map.phi1(dd)][2]==0.0f && dd!=d); - } - } - - map.closeMap(closeMark); -} - -template -void animateCity(typename PFP::MAP& map, EMBV& position, EMBAGENT& agents, DartMarker& closeMark, std::vector& newBuildings) -{ - unsigned int state=rand()%10; - if(state<3) { - //generate new building - bool found=false; - Dart d; - for(d = map.begin() ;!found && d!=map.end() ; map.next(d)) { - if(!closeMark.isMarked(d) && agents[d].size()==0 && notEightAdjacentToAnObstacle(map,d,closeMark)) { - typename PFP::VEC3 n1 = Algo::Geometry::faceNormal(map,d,position); - typename PFP::VEC3 n2 = Algo::Geometry::faceNormal(map,map.phi2(d),position); - - bool groundZero=true; - Dart dd = d; - do { - if(position[dd][2]!=0.0f) - groundZero=false; - dd = map.phi1(dd); - } while(groundZero && dd!=d); - - if(groundZero && n1[0]==0.0f && n1[1]==0.0f && n1[2]==1.0f && n1[0]==n2[0] && n1[1]==n2[1] && n1[2]==n2[2]) { - found=true; - break; - } - } - } - - if(found) { - if(map.faceDegree(d)>16) { - - } - closeMark.markOrbit(FACE_ORBIT,d); - d = Algo::Modelisation::extrudeFace(map,position,d,2.0f); - newBuildings.push_back(d); - } - } - else if(newBuildings.size()>0) { - //take a newBuilding and make it higher - state = rand()%newBuildings.size(); - Dart d = newBuildings[state]; - //decide if we create a new floor, or just make it higher - unsigned int typeOfUpdate = rand()%10; - if(typeOfUpdate<9) { - Dart dd=d; - do { - position[dd][2] += 2.0f; - dd = map.phi1(dd); - } while(dd!=d); - - if(position[dd][2]>((10+rand()%10)*10.0f)) - newBuildings.erase(newBuildings.begin()+state); - } - else { - typename PFP::VEC3 c = Algo::Geometry::faceCentroid(map,d,position); - Dart dRoofSmall = Algo::Modelisation::extrudeFace(map,position,d,0.0f); - Dart dd = dRoofSmall; - do { - position[dd] = position[dd] + (c-position[dd])/3.0f; - dd = map.phi1(dd); - } while (dd!=dRoofSmall); - Dart dRoof = Algo::Modelisation::extrudeFace(map,position,dRoofSmall,2.0f); - - newBuildings.erase(newBuildings.begin()+state); - newBuildings.push_back(dRoof); - } - - } -} - -template -bool notEightAdjacentToAnObstacle(typename PFP::MAP& map, Dart d, DartMarker& closeMark) -{ - Dart dd = d; - do { - if(closeMark.isMarked(map.alpha1(map.alpha1(dd)))) - return false; - dd = map.phi1(dd); - }while(dd!=d); - - return true; -} - -template -void generateGrid(typename PFP::MAP& map, EMBV& position, unsigned int cX,unsigned int cY,float sideLength, DartMarker& closeMark) -{ - Algo::Modelisation::Polyhedron prim(map,position); - prim.grid_topo(cX,cY); - map.closeMap(closeMark) ; - - prim.embedGrid(sideLength*cX,sideLength*cY); - - map.initOrbitEmbedding(FACE_ORBIT); -} - -template -void markObstacleSteepFaces(typename PFP::MAP& map, EMBV& position, float steepness, DartMarker& closeMark) -{ - DartMarkerStore treated(map); - for(Dart d=map.begin();d!=map.end(); map.next(d)) { - if(!closeMark.isMarked(d) && !treated.isMarked(d)) { - treated.markOrbit(FACE_ORBIT,d); - typename PFP::VEC3 n = Algo::Geometry::faceNormal(map,d,position); - if(fabs(n*typename PFP::VEC3(1,1,0))>steepness && !(n[2]<-0.5f)) { - closeMark.markOrbit(FACE_ORBIT,d); - } - } - if(closeMark.isMarked(d) && closeMark.isMarked(map.phi2(d))) { - closeMark.unmarkOrbit(EDGE_ORBIT,d); - } - } -} - -//void EnvMap::generateStairs(Dart d, float stepSize, float maxHeight) -//{ -// for(float currentHeight=0.0f; currentHeight(map,position,d,stepSize); -// typename PFP::VEC3 vecStep = position[map.phi1(map.phi1(d))]-position[map.phi1(d)]; -// vecStep.normalize(); -// vecStep *= stepSize; -// map.cutEdge(map.phi1(dStep)); -// map.cutEdge(map.phi_1(dStep)); -// position[map.phi1(map.phi1(dStep))] = position[map.phi1(dStep)] + vecStep; -// position[map.phi_1(dStep)] = position[dStep] + vecStep; -// map.splitFace(map.phi_1(dStep),map.phi1(map.phi1(dStep))); -// d = map.phi2(map.phi1(map.phi1(dStep))); -// } -//} - -template -void generatePathToUpperStair(typename PFP::MAP& map, EMBV& position, Dart dLower,Dart dUpper) -{ - if(dLower!= map.phi2(dLower) || dUpper!=map.phi2(dUpper)) { - std::cout << "generatePathToUpperStair : lower and upper stair darts must be fixpoint in phi2" << std::endl; - return; - } - - //create the path - Dart dPathUp = map.newOrientedFace(4); - Dart dInBetween = map.newOrientedFace(4); - map.cutEdge(dInBetween); - map.cutEdge(dInBetween); - Dart dPathDown = map.newOrientedFace(4); - - //sew the path - map.sewFaces(dLower,dPathUp); - map.sewFaces(map.phi1(map.phi1(dPathUp)),map.phi1(map.phi1(dInBetween))); - map.sewFaces(map.phi1(map.phi1(dPathDown)),dInBetween); - map.sewFaces(dUpper,dPathDown); - - //embed the inBetween floor - float z = (position[dLower][2]+position[dUpper][2])/2.0f; - - //from lower path - position[dInBetween] = position[dLower] + (position[dLower]-position[map.phi_1(dLower)]); - position[map.phi1(dInBetween)] = position[map.phi1(dLower)] + (position[map.phi1(dLower)]-position[map.phi1(map.phi1(dLower))]); - - //from upper path - position[map.phi1(map.phi1(dInBetween))] = position[dUpper] + (position[dUpper]-position[map.phi_1(dUpper)]); - position[map.phi1(map.phi1(map.phi1(dInBetween)))] = position[map.phi1(dUpper)] + (position[map.phi1(dUpper)]-position[map.phi1(map.phi1(dUpper))]); - - //position[map.phi_1(dInBetween)] = - std::cout << "un brin a plonger !!" << std::endl; - - Dart dd = dInBetween; - do { - position[dd][2] = z; - dd = map.phi1(dd); - } while(dd!=dInBetween); -} - -template -void generateBuilding(typename PFP::MAP& map, EMBV& position, Dart d, DartMarkerStore& build, float height, float side, unsigned int buildingType, DartMarker& closeMark) -{ - Dart dRoof; - if(buildingType<3) { - closeMark.markOrbit(FACE_ORBIT,d); - - dRoof = Algo::Modelisation::extrudeFace(map,position,d,height); - } - -// switch(buildingType) { -// case 0 : { -// dRoof = Algo::Modelisation::extrudeFace(map,position,dRoof,height/3); -// Dart dNext = map.phi1(dRoof); -// Dart dPrev = map.phi2(map.phi_1(dRoof)); -// map.collapseEdge(dNext); -// map.collapseEdge(dPrev); -// } -// break; -// case 1 : { -// dRoof = Algo::Modelisation::extrudeFace(map,position,dRoof,height/3); -// Dart dNext = map.phi1(dRoof); -// Dart dPrev = map.phi2(map.phi_1(dRoof)); -// typename PFP::VEC3 mid1 = (position[dNext]+position[map.phi1(dNext)])/2.0f; -// typename PFP::VEC3 mid2 = (position[dPrev]+position[map.phi1(dPrev)])/2.0f; -// map.collapseEdge(dNext); -// map.collapseEdge(dPrev); -// position[dRoof] = mid2; -// position[map.phi1(dRoof)] = mid1; -// } -// break; -// case 2 : { -// unsigned int nbStairs=rand()%5; -// for(unsigned int i = 0;i(map,dRoof,position); -// Dart dRoofSmall = Algo::Modelisation::extrudeFace(map,position,dRoof,0.0f); -// Dart dd = dRoofSmall; -// do { -// position[dd] = position[dd] + (c-position[dd])/3.0f; -// dd = map.phi1(dd); -// } while (dd!=dRoofSmall); -// dRoof = Algo::Modelisation::extrudeFace(map,position,dRoofSmall,height/2.0f); -// } -// bool spike=(rand() % 2); -// if(spike) { -// typename PFP::VEC3 c = Algo::Geometry::faceCentroid(map,dRoof,position); -// c[2] += height/1.5f; -// // Dart dRoofSmall = Algo::Modelisation::extrudeFace(map,position,dRoof,height/1.5f); -// Dart dRoofSmall = Algo::Modelisation::trianguleFace(map,dRoof); -// -// position[dRoofSmall] = c; -// } -// } -// break; -// case 3 : { -// typename PFP::VEC3 c = Algo::Geometry::faceCentroid(map,d,position); -// Dart dd=d; -// do { -// Dart f = map.phi1(dd); -// map.cutEdge(dd); -// Dart e = map.phi1(dd); -// position[e] = position[dd]; -// position[e] += position[f]; -// position[e] *= 0.5; -// dd=map.phi1(map.phi1(dd)); -// } while(dd!=d); -// Dart dMid = Algo::Modelisation::quadranguleFace(map,d); -// position[dMid] = c; -// -// dd = dMid; -// do { -// Dart ddd=map.phi1(map.phi1(dd)); -// -// Dart f = map.phi1(ddd); -// map.cutEdge(ddd); -// Dart e = map.phi1(ddd); -// position[e] = position[ddd]; -// position[e] += position[f]; -// position[e] *= 0.5; -// -// ddd=map.phi1(dd); -// f = map.phi1(ddd); -// map.cutEdge(ddd); -// e = map.phi1(ddd); -// position[e] = position[ddd]; -// position[e] += position[f]; -// position[e] *= 0.5; -// -// ddd=dd; -// f = map.phi1(ddd); -// map.cutEdge(ddd); -// e = map.phi1(ddd); -// position[e] = position[ddd]; -// position[e] += position[f]; -// position[e] *= 0.5; -// -// build.markOrbit(FACE_ORBIT,dd); -// dd=map.alpha1(dd); -// } while(dd!=dMid); -// -// dd=dMid; -// do { -// c = Algo::Geometry::faceCentroid(map,dd,position); -// Dart ddMid = Algo::Modelisation::quadranguleFace(map,dd); -// Dart ddd=dMid; -// do { -// build.markOrbit(FACE_ORBIT,ddMid); -// ddd = map.alpha1(ddd); -// }while(ddd!=dMid); -// position[ddMid] = c; -// dd=map.alpha1(dd); -// } while(dd!=dMid); -// -// float bridgeHeight=20.0f; -// Dart dStart = d; -// Dart dStop=dStart; -// for(unsigned int i=0;i<3;++i) -// dStop = map.phi2(map.phi1(map.phi1(dStop))); -// -// dStop = map.phi1(map.phi1(dStop)); -// -// Dart dUnderStart = map.phi2(map.phi1(map.phi1(dStart))); -// Dart dUnderStop = map.phi2(map.phi1(map.phi1(dStop))); -// -// Dart f = map.phi1(map.phi1(dUnderStart)); -// map.cutEdge(map.phi1(dUnderStart)); -// Dart e = map.phi1(map.phi1(dUnderStart)); -// position[e] = (position[f]+position[map.phi1(dUnderStart)])*0.5f; -// -// f = map.phi_1(dUnderStart); -// map.cutEdge(f); -// e = map.phi_1(dUnderStart); -// position[e] = (position[f]+position[dUnderStart])*0.5f; -// -// map.splitFace(map.phi1(map.phi1(dUnderStart)),map.phi_1(dUnderStart)); -// closeMark.markOrbit(FACE_ORBIT, dUnderStart); -// -// f = map.phi1(map.phi1(dUnderStop)); -// map.cutEdge(map.phi1(dUnderStop)); -// e = map.phi1(map.phi1(dUnderStop)); -// position[e] = (position[f]+position[map.phi1(dUnderStop)])*0.5f; -// -// f = map.phi_1(dUnderStop); -// map.cutEdge(f); -// e = map.phi_1(dUnderStop); -// position[e] = (position[f]+position[dUnderStop])*0.5f; -// -// map.splitFace(map.phi1(map.phi1(dUnderStop)),map.phi_1(dUnderStop)); -// closeMark.markOrbit(FACE_ORBIT, dUnderStop); -// -// generateBridge(map,position,dStart,dStop,bridgeHeight,closeMark); -// } -// break; -// default : -// break; -// } -} - -template -void generateBridge(typename PFP::MAP& map, EMBV& position, Dart dStart, Dart dStop, float bridgeHeight, DartMarker& closeMark) -{ - typename PFP::VEC3 cStart = Algo::Geometry::faceCentroid(map,dStart,position); - typename PFP::VEC3 cStop = Algo::Geometry::faceCentroid(map,dStop,position); - - Dart ddStart = Algo::Modelisation::extrudeFace(map,position,dStart,bridgeHeight); - Dart ddStop = Algo::Modelisation::extrudeFace(map,position,dStop,bridgeHeight/2.0f); - position[ddStop][2] += bridgeHeight/2.0f; - typename PFP::VEC3 transl = position[map.phi_1(ddStop)]-position[ddStop]; - position[ddStop] += transl *0.5f; - position[map.phi1(ddStop)][2] += bridgeHeight/2.0f; - transl = position[map.phi1(map.phi1(ddStop))]-position[map.phi1(ddStop)]; - position[map.phi1(ddStop)] += transl *0.5f; - transl = Algo::Geometry::faceCentroid(map,ddStop,position); - Dart dLoop = ddStop; - do { - position[dLoop] += (transl-position[dLoop])*0.1f; - if(position[dLoop][2]>(bridgeHeight/2.0f+bridgeHeight/10.0f)) { - position[dLoop] += (transl-position[dLoop])*0.4f; - } - position[dLoop] += (cStart-cStop)/4.0f; - dLoop = map.phi1(dLoop); - } while(dLoop !=ddStop); - - Dart dddStop = Algo::Modelisation::extrudeFace(map,position,ddStop,0.0f); - dLoop = dddStop; - - transl = (cStart-cStop)/3.0f; - do { - position[dLoop] += (transl)*position[dLoop][2]/bridgeHeight; - dLoop = map.phi1(dLoop); - } while(dLoop!=dddStop); - - Dart dBef = map.phi2(dddStop); - Dart dBef2 = map.phi2(ddStop); - map.mergeVolumes(dddStop,ddStart); - - closeMark.markOrbit(FACE_ORBIT,map.alpha1(map.alpha1(map.phi2(dBef)))); - closeMark.markOrbit(FACE_ORBIT,map.phi2(map.alpha1(map.alpha1(map.phi2(dBef))))); - closeMark.markOrbit(FACE_ORBIT,map.alpha1(map.alpha1(dBef))); - closeMark.markOrbit(FACE_ORBIT,map.alpha1(map.alpha1(map.phi2(dBef2)))); - closeMark.markOrbit(FACE_ORBIT,map.alpha1(map.alpha1(dBef2))); - closeMark.markOrbit(FACE_ORBIT,map.phi2(map.alpha1(map.alpha1(dBef2)))); - -// map.mergeFaces(map.phi1(ddStart)); -// map.mergeFaces(map.phi_1(ddStart)); - -// map.mergeFaces(map.alpha1(map.alpha1(map.phi2(dBef)))); -// map.mergeFaces(map.alpha1(map.alpha1(dBef))); -// map.mergeFaces(map.alpha1(map.alpha1(map.phi2(dBef2)))); -// map.mergeFaces(map.alpha1(map.alpha1(dBef2))); - -// map.mergeFaces(map.alpha_1(dBef)); -} - -template -void generateBridge(typename PFP::MAP& map, EMBV& position, unsigned int cX, unsigned int cY,float sideLength, DartMarker& closeMark) -{ - std::cout << "Warning : current version allows only one bridge.." << std::endl; - - //define a ground grid - Algo::Modelisation::Polyhedron ground(map,position); - ground.grid_topo(5,3); - - ground.embedGrid(sideLength*5,sideLength*3); - - std::cout << "embedded" << std::endl; - - //get the insertion darts of the bridge - Dart d = ground.getDart(); - Dart dStart = map.phi1(map.phi2(map.phi1(map.phi1(d)))); - Dart dStop=dStart; - for(unsigned int i=0;i<3;++i) { - dStop = map.phi1(map.phi1(map.phi2(dStop))); - } - dStop = map.phi2(dStop); - - - std::cout << "ground S : " << dStart << " stop " << dStop << std::endl; - std::cout << "ground S : " << map.phi2(dStart) << " stop " << map.phi2(dStop) << std::endl; - - dStart = map.phi2(dStart); - Dart ddStart = Algo::Modelisation::extrudeFace(map,position,dStart,100.0f); - - dStop = map.phi2(dStop); - Dart ddStop = Algo::Modelisation::extrudeFace(map,position,dStop,50.0f); - position[ddStop][2] += 50.0f; - typename PFP::VEC3 transl = position[map.phi_1(ddStop)]-position[ddStop]; - position[ddStop] += transl *0.5f; - position[map.phi1(ddStop)][2] += 50.0f; - transl = position[map.phi1(map.phi1(ddStop))]-position[map.phi1(ddStop)]; - position[map.phi1(ddStop)] += transl *0.5f; - transl = Algo::Geometry::faceCentroid(map,ddStop,position); - Dart dLoop = ddStop; - do { - position[dLoop] += (transl-position[dLoop])*0.1f; - if(position[dLoop][2]>70.0f) { - std::cout << "changement de plan supérieur" << std::endl; - position[dLoop] += (transl-position[dLoop])*0.1f; - } - dLoop = map.phi1(dLoop); - } while(dLoop !=ddStop); - - Dart dddStop = Algo::Modelisation::extrudeFace(map,position,ddStop,0.0f); - dLoop = dddStop; - transl = typename PFP::VEC3(-sideLength*2.5f,0,0); - do { - position[dLoop] += (transl)*position[dLoop][2]/100.0f; - if(position[dLoop][2]==100.0f) { - - } - dLoop = map.phi1(dLoop); - } while(dLoop!=dddStop); - - map.mergeVolumes(dddStop,ddStart); - - map.closeMap(closeMark); - - map.check(); - std::cout << "bridge built" << std::endl; -} - -} - -} diff --git a/Apps/Examples/SocialAgents/include/env_map.h b/Apps/Examples/SocialAgents/include/env_map.h deleted file mode 100644 index d010a37a..00000000 --- a/Apps/Examples/SocialAgents/include/env_map.h +++ /dev/null @@ -1,130 +0,0 @@ -#ifndef ENVMap_H -#define ENVMap_H - -#include -// #include "Definitions.h" - -#include "Topology/generic/parameters.h" -#include "Topology/map/map2.h" -#include "Topology/generic/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 "Container/fakeAttribute.h" - -#include "env_generator.h" - -using namespace CGoGN; - -class Agent; - -struct PFP: public PFP_STANDARD -{ - // definition de la carte -// typedef EmbeddedMap2 MAP; - typedef Algo::IHM::ImplicitHierarchicalMap MAP; - // definition des listes d'agent -// typedef std::vector AGENTS; - typedef std::vector AGENTS; - typedef NoMathIONameAttribute AGENTVECT; - - - typedef AttributeHandler TAB_AGENTVECT; - -}; - -typedef PFP::VEC3 VEC3 ; - -class EnvMap -{ - public : - PFP::MAP map; - //SelectorTrue allDarts; - - //identifier for the position attribute -// unsigned int idPos; - PFP::TVEC3 position; - //identifier for the normal attribute -// unsigned int idNorm; - PFP::TVEC3 normal; - -// unsigned int idAgent; - PFP::TAB_AGENTVECT agentvect; - - float sideSize; - -// PFP::POSITION tablePosition; -// PFP::TAB_AGENTVECT tableAgent; - - DartMarker closeMark; -// DartMarker subdivMarker; - - std::vector newBuildings; - -// std::vector subdividedDarts; - Dart dSimplify; - Dart dSubdivide; - - EnvMap(); - - bool simplifiable(Dart d); - bool simplifiableDart(Dart d); - bool collapsableEdge(Dart& d,Dart& dStop); - bool simplifyVertex(Dart& d); - bool simplifyEdge(Dart& d); - void simplify(); - - VEC3 faceCenter(Dart d); - -// const PFP::VEC3& getPosition(const Dart& d); -// RVO::Vector2 getPosition2(Dart d); - - unsigned int getNbFaces(); - - Dart getDart(unsigned int i); - - Dart getBelongingCell(const PFP::VEC3& pos); - - bool noAgentSurroundingVertex(Dart d); - - void findAgentContactInFace(Dart d, PFP::VEC3 pos, float radius, std::vector& l_contact_Dart,DartMarker& m); - -// void insertObstacleOfFace(Agent * agent, const Dart d); - void insertObstacleOfFace(PFP::AGENTS agents,const Dart d); - -// void getAllFacesOfAgent(Agent * agent, Dart d, PFP::VEC3 pos, float radius); - void getAllFacesOfAgents(Dart d); - - void linkAgentWithCells(Agent* agent); - - void addObstacle(std::vector points); - - bool subdivideSquare(Dart d); - - bool subdivideFace(Dart d); - - void resetAgentInFace(Agent * agent); - - bool checkForSubdivision(Dart d); - - void updateMap(); - - void subdivideFaces(); - - void simplifyFaces(); - - void pushAgentInCell(Agent * agent, Dart d); - - void popAgentInCell(Agent * agent, Dart d); - -// void addNeighborAgents(size_t agentno, Agent * agent); - void addNeighborAgents(PFP::AGENTS agentsFrom,PFP::AGENTS agentsTo); -}; - -#endif diff --git a/Apps/Examples/SocialAgents/include/env_render.h b/Apps/Examples/SocialAgents/include/env_render.h deleted file mode 100644 index 6ef40c31..00000000 --- a/Apps/Examples/SocialAgents/include/env_render.h +++ /dev/null @@ -1,65 +0,0 @@ -#ifndef ENVMAP_RENDER -#define ENVMAP_RENDER - -#include "env_map.h" -#include - - -//static void renderPoint(EnvMap& m, Dart& d) -//{ -// PFP::VEC3 p = m.position[d]; -// glBegin(GL_POINTS); -// glVertex3f(p[0],p[1],p[2]); -// glEnd(); -//} - -static void renderDart(EnvMap& m, Dart d) -{ - PFP::VEC3 p1 = m.position[d]; - PFP::VEC3 p2 = m.position[m.map.phi1(d)]; - - glBegin(GL_LINES); - glVertex3f(p1[0],p1[1],p1[2]); - glVertex3f(p2[0],p2[1],p2[2]); - glEnd(); -} - -static void renderFace(EnvMap& m, Dart& d) -{ - Dart dd=d; - do { - renderDart(m,dd); - dd = m.map.phi1(dd); - }while(dd!=d); -} - -//static void renderCellOfDim(EnvMap& m,Dart d, unsigned int dim) -//{ -// switch(dim) { -// case 0 : renderPoint(m,d); break; -// case 1 : renderDart(m,d); break; -// case 2 : renderFace(m,d); break; -// } -//} -// -//static void renderAllPoints(EnvMap& m) -//{ -// glBegin(GL_POINTS); -// for(Dart d=m.map.begin();d!=m.map.end();m.map.next(d)) { -// PFP::VEC3 p = m.position[d]; -// glVertex3f(p[0],p[1],p[2]); -// } -// glEnd(); -//} - -static void renderPredictionTriangle(EnvMap& m, Dart d, PFP::VEC3 p) -{ - glBegin(GL_LINE_LOOP); - glVertex3fv(&p[0]); - glVertex3fv(& m.position[d][0]); - glVertex3fv(& m.position[m.map.phi1(d)][0]); - glEnd(); -} - - -#endif diff --git a/Apps/Examples/SocialAgents/include/mainwindow.h b/Apps/Examples/SocialAgents/include/mainwindow.h deleted file mode 100644 index c3ec2dd7..00000000 --- a/Apps/Examples/SocialAgents/include/mainwindow.h +++ /dev/null @@ -1,8 +0,0 @@ -/**************************************************************************** -** Form interface generated from reading ui file 'mainwindow.ui' -** -** Created: Thu Sep 30 11:00:52 2010 -** -** WARNING! All changes made in this file will be lost! -****************************************************************************/ - diff --git a/Apps/Examples/SocialAgents/include/path_finder.h b/Apps/Examples/SocialAgents/include/path_finder.h deleted file mode 100644 index e015940f..00000000 --- a/Apps/Examples/SocialAgents/include/path_finder.h +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef PATH_FINDER -#define PATH_FINDER - -namespace CGoGN -{ - -namespace PathFinder -{ - - template - float pathCostSqr(typename PFP::MAP& map,const typename PFP::TVEC3& position, Dart startPos, Dart stopPos); - - template - std::vector pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3& position, Dart startPos, Dart stopPos, FunctorSelect& bad = FunctorType()); - -} - -} - -#include "path_finder.hpp" -#endif \ No newline at end of file diff --git a/Apps/Examples/SocialAgents/include/path_finder.hpp b/Apps/Examples/SocialAgents/include/path_finder.hpp deleted file mode 100644 index 586db54f..00000000 --- a/Apps/Examples/SocialAgents/include/path_finder.hpp +++ /dev/null @@ -1,78 +0,0 @@ -#include "Algo/Geometry/centroid.h" -#include -#include - -namespace CGoGN -{ - -namespace PathFinder -{ - - -template -float pathCostSqr(typename PFP::MAP& map,const typename PFP::TVEC3& position, Dart startPos, Dart stopPos) -{ - return VEC3(Algo::Geometry::faceCentroid(map,stopPos,position)-Algo::Geometry::faceCentroid(map,startPos,position)).norm2(); -} - -template -std::vector pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3& position, Dart startPos, Dart stopPos, FunctorType& bad) -{ - std::vector path; - - AutoAttributeHandler< Dart > tablePred(map,FACE_ORBIT); - std::vector > vToDev; - - Dart toDev = stopPos; - vToDev.push_back(std::make_pair(0, toDev)); - tablePred[toDev] = toDev; - do { - //dev cell - //get all vertex-adjacent cells - Dart dd = toDev; - do { - Dart ddd = map.alpha1(dd); - do { - ddd = map.alpha1(ddd); - if(ddd!=dd) { - if(!map.foreach_dart_of_face(ddd,bad) && tablePred[ddd]==EMBNULL) { - //evaluate their cost and push them in the vector to dev - if(tablePred[ddd]==EMBNULL) - tablePred[ddd]=toDev; - std::vector >::iterator it=vToDev.begin(); - float costDDD=pathCostSqr(map,position,startPos,ddd)+(vToDev.begin())->first; - while(it!=vToDev.end() && (*it).first0) { - toDev = (vToDev.begin())->second; - vToDev.erase(vToDev.begin()); - } - } while(vToDev.size()>0 && !map.sameOrientedFace(startPos,toDev)); - - //if path found : from start to stop -> push all predecessors - if(map.sameOrientedFace(startPos,toDev)) - { - std::cout << "found" << std::endl; - Dart toPush=startPos; - std::cout << tablePred[startPos] << std::endl; - do { - path.push_back(toPush); - toPush = tablePred[toPush]; - } while(!map.sameOrientedFace(toPush,stopPos)); - } - - return path; -} - -//namespace -} - -} diff --git a/Apps/Examples/SocialAgents/include/simulator.h b/Apps/Examples/SocialAgents/include/simulator.h deleted file mode 100644 index 6702e170..00000000 --- a/Apps/Examples/SocialAgents/include/simulator.h +++ /dev/null @@ -1,80 +0,0 @@ -#ifndef SIMULATOR_H -#define SIMULATOR_H - -#include -#include - -#include "utils.h" - -#include "env_map.h" -#include "env_generator.h" -#include "agent.h" - -#include "path_finder.h" - -class Simulator -{ - public: - Simulator(); - - Simulator(float timeStep, float neighborDist, size_t maxNeighbors, - float timeHorizon, float timeHorizonObst, float radius, - float maxSpeed, const VEC3& velocity = VEC3(0,0,0)); - ~Simulator(); - - size_t addAgent(VEC3 position); - - size_t addAgent(VEC3 position, Dart d); - - size_t addAgent(VEC3 position, float neighborDist, - size_t maxNeighbors, float timeHorizon, - float timeHorizonObst, float radius, float maxSpeed, - const VEC3& velocity = VEC3()); - - void doStep(); - VEC3 getAgentPrefVelocity(size_t agentNo) const; - - float getGlobalTime() const; - - void setAgentDefaults(float neighborDist, size_t maxNeighbors, - float timeHorizon, float timeHorizonObst, - float radius, float maxSpeed, - const VEC3& velocity = VEC3()); - - void setAgentPrefVelocity(size_t agentNo, const VEC3& prefVelocity); - - void setTimeStep(float timeStep); - - std::vector agents_; - Agent* defaultAgent_; - float globalTime_; - EnvMap envMap; - float timeStep_; - - std::vector socialAgents; - - void setPreferredNextCellVelocities(); - -// std::vector path; - - /** Added function */ - std::vector getAgentsPosition(); - - //just one goal - std::vector goals; - - void setPreferredVelocities(PFP::VEC3 posToReach); - - //for a path of goals - std::vector > pathPointer; - std::vector * > pathToFollow; - - void setPreferredPathVelocities(); - - //scenarii - void setupScenario(); - void setupTobogganScenario(float rMax,float rMin); - void setupHelicoidScenario(float rMax,float rMin); -}; - -#endif diff --git a/Apps/Examples/SocialAgents/include/utils.h b/Apps/Examples/SocialAgents/include/utils.h deleted file mode 100644 index 765ca6f5..00000000 --- a/Apps/Examples/SocialAgents/include/utils.h +++ /dev/null @@ -1,95 +0,0 @@ -#ifndef UTILS_H -#define UTILS_H - -#include -#include -#include - -// #include "Vector2.h" -#include "Geometry/vector_gen.h" - -typedef CGoGN::Geom::Vec3f VEC3; - -static const float RVO_EPSILON = 0.00001f; -static const size_t RVO_ERROR = std::numeric_limits::max(); - - typedef VEC3 VEC_A; - - struct Line { - VEC_A point; - VEC_A direction; - }; - -inline float abs(const VEC3& vector) -{ -return std::sqrt(vector * vector); -} - -inline float absSq(const VEC3& vector) -{ -return vector[0] * vector[0] + vector[1] * vector[1]; -// return vector * vector; -} - -inline float det2D(const VEC_A& vector1, const VEC_A& vector2) -{ -return vector1[0] * vector2[1] - vector1[1] * vector2[0]; -} - -inline VEC3 normalize(const VEC3& vector) -{ -return vector / abs(vector); -} - -inline float sqr(float a) -{ -return a * a; -} - -inline float distSqPointLineSegment(const VEC3& a, const VEC3& b, - const VEC3& c) -{ -VEC3 a1(a[0],a[1],0); -VEC3 b1(b[0],b[1],0); -VEC3 c1(c[0],c[1],0); - -const float r = ((c1 - a1) * (b1 - a1)) / absSq(b1 - a1); - -if (r < 0.0f) { - return absSq(c1 - a1); -} else if (r > 1.0f) { - return absSq(c1 - b1); -} else { - return absSq(c1 - (a1 + r * (b1 - a1))); -} -} - - -inline void projectPointOnLine(const VEC3& p, const VEC3& p1, const VEC3& p2, VEC3& proj) -{ - VEC3 v(p-p1); - VEC3 seg(p2-p1); - - seg.normalize(); - - VEC3::DATA_TYPE dot = v*seg; - - proj = p1+seg*dot; -} - -inline bool isInInterval(const VEC3& p, const VEC3& p1, const VEC3& p2) -{ - if( (VEC3(p2-p1) * VEC3(p-p1)) > 0.0f) - if( VEC3(p1-p2) * VEC3(p-p2) > 0.0f) - return true; - - return false; -} - - -inline float leftOf(const VEC_A& a, const VEC_A& b, const VEC_A& c) -{ -return det2D(a - c, b - a); -} - -#endif diff --git a/Apps/Examples/SocialAgents/include/viewer.h b/Apps/Examples/SocialAgents/include/viewer.h deleted file mode 100644 index dfd514a8..00000000 --- a/Apps/Examples/SocialAgents/include/viewer.h +++ /dev/null @@ -1,90 +0,0 @@ -/******************************************************************************* -* CGoGN: Combinatorial and Geometric modeling with Generic N-dimensional Maps * -* version 0.1 * -* Copyright (C) 2009, IGG Team, LSIIT, University of Strasbourg * -* * -* This library is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by the * -* Free Software Foundation; either version 2.1 of the License, or (at your * -* option) any later version. * -* * -* This library is distributed in the hope that it will be useful, but WITHOUT * -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this library; if not, write to the Free Software Foundation, * -* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * -* * -* Web site: https://iggservis.u-strasbg.fr/CGoGN/ * -* Contact information: cgogn@unistra.fr * -* * -*******************************************************************************/ - -#include - -#include "Utils/glutwin_atb.h" - -#include "Utils/os_spec.h" - -#include "Utils/GLSLShader.h" -// #include "Utils/glutwin.h" - -#include "simulator.h" -#include "env_render.h" - -#include "Algo/Export/exportPov.h" -#include "Algo/Render/map_glRender.h" -#include "Algo/Render/vbo_MapRender.h" -// #include "Algo/Render/test_map_RenderVBO.h" -// #include "Algo/Render/test_vbo_pointsColCB.hpp" -// #include "Algo/Render/topo_vboRender.h" -#include "Algo/Render/topo_render.h" - -using namespace CGoGN ; - -typedef PFP::MAP MAP; - -class MyGlutWin : public Utils::GlutWin_ATB -{ -public: - Simulator * sim; - SelectorTrue allDarts; - - bool b_animate; - bool render_anim; - int visu; - int nbGenerated; - - PFP::VEC3 posToReach; - - MyGlutWin(int* argc, char **argv, int winX, int winY) ; - - bool reachedGoal(Simulator* sim); - - float computeSelectRadius(int x, int y, int pixelRadius); - - bool exportScenePov(PFP::MAP& map, PFP::TVEC3& position, const std::string& filename, PFP::VEC3 cameraPos, PFP::VEC3 cameraLook, PFP::VEC3 translate, float angle_X, float angle_Y, float angle_Z,const FunctorSelect& good = SelectorTrue()); - - void animate(void); - void myRedraw(); - void myKeyboard(unsigned char keycode, int x, int y); - void myMouse(int button, int state, int x, int y); - - Algo::Render::VBO::MapRender_VBO* m_render; - Algo::Render::VBO::MapRender_VBO* m_render_line ; -// Algo::Render::VBO::topo_MD2_VBORender* topo_render ; -// Algo::Render::VBO::topo_VBORenderMapD* topo_render ; - - //to count fps - int frames; - clock_t nextUpdate; - bool displayFps; - - TwBar* viewer_bar ; - - void updateUI(); - void init() ; - void initGUI() ; -} ; diff --git a/Apps/Examples/SocialAgents/src/agent.cpp b/Apps/Examples/SocialAgents/src/agent.cpp deleted file mode 100644 index dac969c6..00000000 --- a/Apps/Examples/SocialAgents/src/agent.cpp +++ /dev/null @@ -1,511 +0,0 @@ -#include "agent.h" -#include "simulator.h" - -Agent::Agent(Simulator* sim, size_t agentNo) : agentNeighbors_(), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), newVelocity_(), obstacleNeighbors_(), orcaLines_(), position_(), prefVelocity_(), radius_(0.0f), sim_(sim), timeHorizon_(0.0f), timeHorizonObst_(0.0f), velocity_(), agentNo_(agentNo), goalNo_(agentNo), treated(false) -{ - rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); -} - -Agent::Agent(Simulator* sim, const VEC_A& position, size_t agentNo) : agentNeighbors_(), maxNeighbors_(sim->defaultAgent_->maxNeighbors_), maxSpeed_(sim->defaultAgent_->maxSpeed_), neighborDist_(sim->defaultAgent_->neighborDist_), newVelocity_(sim->defaultAgent_->velocity_), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(sim->defaultAgent_->radius_), sim_(sim), timeHorizon_(sim->defaultAgent_->timeHorizon_), timeHorizonObst_(sim->defaultAgent_->timeHorizonObst_), velocity_(sim->defaultAgent_->velocity_), agentNo_(agentNo), goalNo_(agentNo), treated(false) -{ - Dart d = sim->envMap.getBelongingCell(VEC3(position[0],position[1],0)); - part = new Algo::MovingObjects::ParticleCell2D(sim->envMap.map,d,position,sim->envMap.position,sim->envMap.closeMark); - rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); -} - -Agent::Agent(Simulator* sim, const VEC_A& position, size_t agentNo, Dart d) : agentNeighbors_(), maxNeighbors_(sim->defaultAgent_->maxNeighbors_), maxSpeed_(sim->defaultAgent_->maxSpeed_), neighborDist_(sim->defaultAgent_->neighborDist_), newVelocity_(sim->defaultAgent_->velocity_), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(sim->defaultAgent_->radius_), sim_(sim), timeHorizon_(sim->defaultAgent_->timeHorizon_), timeHorizonObst_(sim->defaultAgent_->timeHorizonObst_), velocity_(sim->defaultAgent_->velocity_), agentNo_(agentNo), goalNo_(agentNo), treated(false) -{ - part = new Algo::MovingObjects::ParticleCell2D(sim->envMap.map,d,position,sim->envMap.position,sim->envMap.closeMark); - rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); -} - -Agent::Agent(Simulator* sim, const VEC_A& position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, const VEC_A& velocity, float maxSpeed, size_t agentNo) : agentNeighbors_(), maxNeighbors_(maxNeighbors), maxSpeed_(maxSpeed), neighborDist_(neighborDist), newVelocity_(velocity), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(radius), sim_(sim), timeHorizon_(timeHorizon), timeHorizonObst_(timeHorizonObst), velocity_(velocity), agentNo_(agentNo), goalNo_(agentNo), treated(false) -{ - Dart d = sim->envMap.getBelongingCell(VEC3(position[0],position[1],0)); - part = new Algo::MovingObjects::ParticleCell2D(sim->envMap.map,d,position,sim->envMap.position,sim->envMap.closeMark); - rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); -} - -Agent::~Agent() -{ -} - -void Agent::setGoalNo(size_t goal) -{ - goalNo_ = goal; -} - - /* Search for the best new velocity. */ -void Agent::computeNewVelocity() -{ - orcaLines_.clear(); - - /* Create obstacle ORCA lines. */ - for (size_t i = 0; i < obstacleNeighbors_.size(); ++i) { - float invTimeHorizonObst = 1.0f / timeHorizonObst_; - -// // const Obstacle* obstacle1 = sim_->obstacles_[obstacleNeighbors_[i].second]; -// // const Obstacle* obstacle2 = sim_->obstacles_[obstacle1->nextObstacleNo_]; - Dart obst1 = obstacleNeighbors_[i].second; - Dart obst2 = sim_->envMap.map.phi1(obst1); - - VEC3 obst1Pos = sim_->envMap.position[obst1]; - VEC3 obst2Pos = sim_->envMap.position[obst2]; - - const VEC3 relativePosition1(obst1Pos - position_); - const VEC3 relativePosition2(obst2Pos - position_); - const VEC3 obstacleVector(obst2Pos - obst1Pos); - - /* - * Check if velocity obstacle of obstacle is already taken care of by - * previously constructed obstacle ORCA lines. - */ - bool alreadyCovered = false; - - for (size_t j = 0; j < orcaLines_.size(); ++j) { - if (det2D(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON && det2D(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON) { -// std::cout << "already covered (?) " << obst1 << std::endl; - alreadyCovered = true; - break; - } - } - - if (alreadyCovered) { - continue; - } - -// std::cout << "MUST AVOIIID OBSTAAACLE " << obst1 << std::endl; - - /* - * Compute legs. When obliquely viewed, both legs can come from a single - * vertex. Legs can never point into neighboring edge when convex vertex, - * take cutoff-line of neighboring edge instead. If velocity projected on - * "foreign" leg, no constraint is added. Legs extend cut-off line when - * nonconvex vertex. - */ - const float distSq1 = absSq(relativePosition1); - const float distSq2 = absSq(relativePosition2); - - const float radiusSq = sqr(radius_); - - VEC3 leftLegDirection, rightLegDirection; - - const float s = (-1.0f*relativePosition1 * obstacleVector) / absSq(obstacleVector); - const float distSqLine = absSq(-1.0f*relativePosition1 - s * obstacleVector); - - if (s < 0 && distSq1 <= radiusSq) { - /* Collision with left vertex. */ - - invTimeHorizonObst = 1.0f / sim_->timeStep_; - obst2 = obst1; - leftLegDirection = normalize(VEC3(-1.0f*relativePosition1[1], relativePosition1[0],0)); - rightLegDirection = -1.0f*leftLegDirection; - } else if (s > 1 && distSq2 <= radiusSq) { - /* Collision with right vertex. */ - - invTimeHorizonObst = 1.0f / sim_->timeStep_; - obst1 = obst2; - leftLegDirection = normalize(VEC3(-1.0*relativePosition2[1], relativePosition2[0],0)); - rightLegDirection = -1.0f*leftLegDirection; - } else if (s >= 0 && s < 1 && distSqLine <= radiusSq) { - /* Collision with obstacle segment. */ - invTimeHorizonObst = 1.0f / sim_->timeStep_; - leftLegDirection = -1.0f*normalize(obstacleVector); - rightLegDirection = -1.0f*leftLegDirection; - } else if (s < 0 && distSqLine <= radiusSq) { - /* - * No collision, but obstacle viewed obliquely so that left vertex - * defines velocity obstacle. - */ - - obst2 = obst1; - - const float leg1 = std::sqrt(distSq1 - radiusSq); - - leftLegDirection = VEC3(relativePosition1[0] * leg1 - relativePosition1[1] * radius_, relativePosition1[0] * radius_ + relativePosition1[1] * leg1,0) / distSq1; - rightLegDirection = VEC3(relativePosition1[0] * leg1 + relativePosition1[1] * radius_, -relativePosition1[0] * radius_ + relativePosition1[1] * leg1,0) / distSq1; - } else if (s > 1 && distSqLine <= radiusSq) { - /* - * No collision, but obstacle viewed obliquely so that - * right vertex defines velocity obstacle. - */ - - obst1 = obst2; - - - const float leg2 = std::sqrt(distSq2 - radiusSq); - - leftLegDirection = VEC3(relativePosition2[0] * leg2 - relativePosition2[1] * radius_, relativePosition2[0] * radius_ + relativePosition2[1] * leg2,0) / distSq2; - rightLegDirection = VEC3(relativePosition2[0] * leg2 + relativePosition2[1] * radius_, -relativePosition2[0] * radius_ + relativePosition2[1] * leg2,0) / distSq2; - } else { - /* Usual situation. */ -// std::cout << "usual (?)" << std::endl; - - const float leg1 = std::sqrt(distSq1 - radiusSq); - leftLegDirection = VEC3(relativePosition1[0] * leg1 - relativePosition1[1] * radius_, relativePosition1[0] * radius_ + relativePosition1[1] * leg1,0) / distSq1; - - const float leg2 = std::sqrt(distSq2 - radiusSq); - rightLegDirection = VEC3(relativePosition2[0] * leg2 + relativePosition2[1] * radius_, -relativePosition2[0] * radius_ + relativePosition2[1] * leg2,0) / distSq2; - } - -// const Obstacle* const leftNeighbor = sim_->obstacles_[obstacle1->prevObstacleNo_]; - Dart leftObst1 = sim_->envMap.map.phi_1(obst1); - VEC3 leftObstPos = sim_->envMap.position[leftObst1]; - -// const Obstacle* const rightNeighbor = sim_->obstacles_[obstacle2->nextObstacleNo_]; - Dart rightObst2 = sim_->envMap.map.phi1(obst2); - VEC3 rightObstPos = sim_->envMap.position[rightObst2]; - - bool isLeftLegForeign = false; - bool isRightLegForeign = false; - - if (/*obstacle1->isConvex_ && */det2D(leftLegDirection, leftObstPos - obst1Pos) > 0.0f) { - /* Leg points into obstacle. */ - leftLegDirection = normalize(leftObstPos - obst1Pos); - isLeftLegForeign = true; - } - - if (/*obstacle2->isConvex_ &&*/ det2D(rightLegDirection, rightObstPos - obst2Pos) < 0.0f) { - /* Leg points into obstacle. */ - rightLegDirection = normalize(rightObstPos - obst2Pos); - isRightLegForeign = true; - } - - /* Compute cut-off centers. */ - const VEC3 leftCutoff = invTimeHorizonObst * (obst1Pos - position_); - const VEC3 rightCutoff = invTimeHorizonObst * (obst2Pos - position_); - const VEC3 cutoffVec = rightCutoff - leftCutoff; - - /* Project current velocity on velocity obstacle. */ - Line line; - - const float t = (obst1 == obst2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / absSq(cutoffVec)); - const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection); - const float tRight = ((velocity_ - rightCutoff) * rightLegDirection); - - if ((t < 0.0f && tLeft < 0.0f) || (obst1 == obst2 && tLeft < 0.0f && tRight < 0.0f)) { - /* Project on left cut-off circle. */ - const VEC3 unitW = normalize(velocity_ - leftCutoff); - - line.direction = VEC3(unitW[1], -unitW[0],0); - line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW; - } else if (t > 1.0f && tRight < 0.0f) { - /* Project on right cut-off circle. */ - const VEC3 unitW = normalize(velocity_ - rightCutoff); - - line.direction = VEC3(unitW[1], -unitW[0],0); - line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW; - } else { - /* - * Project on left leg, right leg, or cut-off line, whichever is closest - * to velocity. - */ - - const float distSqCutoff = ((t < 0.0f || t > 1.0f || obst1 == obst2) ? std::numeric_limits::infinity() : absSq(velocity_ - (leftCutoff + t * cutoffVec))); - const float distSqLeft = ((tLeft < 0.0f) ? std::numeric_limits::infinity() : absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection))); - const float distSqRight = ((tRight < 0.0f) ? std::numeric_limits::infinity() : absSq(velocity_ - (rightCutoff + tRight * rightLegDirection))); - - if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) { - /* Project on cut-off line. */ - line.direction = -1.0f*normalize(cutoffVec); - line.point = leftCutoff + radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0],0); - } else if (distSqLeft <= distSqRight) { - /* Project on left leg. */ - if (isLeftLegForeign) { - continue; - } else { - line.direction = leftLegDirection; - line.point = leftCutoff + radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0],0); - } - } else { - /* Project on right leg. */ - if (isRightLegForeign) { - continue; - } else { - line.direction = -1.0f*rightLegDirection; - line.point = rightCutoff + radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0],0); - } - } - } - - orcaLines_.push_back(line); - } - - const size_t numObstLines = orcaLines_.size(); - - - const float invTimeHorizon = 1.0f / timeHorizon_; - - /* Create agent ORCA lines. */ - for (size_t i = 0; i < agentNeighbors_.size(); ++i) { - const Agent* const other = sim_->agents_[agentNeighbors_[i].second]; - - const VEC_A relativePosition(other->position_ - position_); - const VEC_A relativeVelocity(velocity_ - other->velocity_); - const float distSq = absSq(relativePosition); - const float combinedRadius = radius_ + other->radius_; - const float combinedRadiusSq = sqr(combinedRadius); - - Line line; - VEC_A u; - - if (distSq > combinedRadiusSq) { -// std::cout << "no collision" << std::endl; - /* No collision. */ - const VEC_A w = relativeVelocity - invTimeHorizon * relativePosition; - const float wLengthSq = absSq(w); - - const float dotProduct1 = w * relativePosition; - - if (dotProduct1 < 0.0f && sqr(dotProduct1) > combinedRadiusSq * wLengthSq) { - /* Project on cut-off circle. */ - const float wLength = std::sqrt(wLengthSq); - const VEC_A unitW = w / wLength; - - line.direction = VEC_A(unitW[1], -unitW[0], 0); - u = (combinedRadius * invTimeHorizon - wLength) * unitW; - } else { - /* Project on legs. */ - const float leg = std::sqrt(distSq - combinedRadiusSq); - - if (det2D(relativePosition, w) > 0.0f) { - /* Project on left leg. */ - line.direction = VEC_A(relativePosition[0] * leg - relativePosition[1] * combinedRadius, relativePosition[0] * combinedRadius + relativePosition[1] * leg,0) / distSq; - } else { - /* Project on right leg. */ - line.direction = -1.0f*VEC_A(relativePosition[0] * leg + relativePosition[1] * combinedRadius, -relativePosition[0] * combinedRadius + relativePosition[1] * leg,0) / distSq; - } - - const float dotProduct2 = relativeVelocity * line.direction; - - u = dotProduct2 * line.direction - relativeVelocity; - } - } else { - /* Collision. */ - const float dist = std::sqrt(distSq); - const VEC_A unitRelativePosition = relativePosition / dist; - - line.direction = VEC_A(-unitRelativePosition[1], unitRelativePosition[0],0); - - const VEC_A point = ((dist - combinedRadius) / sim_->timeStep_) * unitRelativePosition; - const float dotProduct = (relativeVelocity - point) * line.direction; - - u = point + dotProduct * line.direction - relativeVelocity; - } - - line.point = velocity_ + 0.5f * u; - - orcaLines_.push_back(line); - } - - if (!linearProgram2(orcaLines_, orcaLines_.size(), maxSpeed_, prefVelocity_, false, newVelocity_)) { - linearProgram3(orcaLines_, numObstLines, maxSpeed_, newVelocity_); - } -} - -void Agent::insertAgentNeighbor(size_t agentNo, float& rangeSq) -{ - const Agent* const other = sim_->agents_[agentNo]; - - if (this != other) { - const float distSq = absSq(position_ - other->position_); - - if (distSq < rangeSq) { -// std::cout << "ajout agent voisin" << std::endl; - if (agentNeighbors_.size() < maxNeighbors_) { - agentNeighbors_.push_back(std::make_pair(0.0f,0)); - } - size_t i = agentNeighbors_.size() - 1; - while (i != 0 && (distSq < agentNeighbors_[i-1].first -// || (distSq==agentNeighbors_[i-1].first && ((other->position_[1] < position_[1]) || (other->position_[1]==position_[1] && (other->position_[0] < position_[0])))) - )) { - agentNeighbors_[i] = agentNeighbors_[i-1]; - --i; - } - agentNeighbors_[i] = std::make_pair(distSq, agentNo); - - if (agentNeighbors_.size() == maxNeighbors_) { - rangeSq = agentNeighbors_.back().first; - } - } - } -} - -void Agent::insertObstacleNeighbor(Dart d) -{ - const float distSq = distSqPointLineSegment(sim_->envMap.position[d],sim_->envMap.position[sim_->envMap.map.phi1(d)],position_); - - if (distSq < rangeSq) { - obstacleNeighbors_.push_back(std::make_pair(0.0f,0)); - - size_t i = obstacleNeighbors_.size() - 1; - while (i != 0 && distSq < obstacleNeighbors_[i-1].first) { - obstacleNeighbors_[i] = obstacleNeighbors_[i-1]; - --i; - } - obstacleNeighbors_[i] = std::make_pair(distSq, d); - } -} - -void Agent::update() -{ - velocity_[0] = newVelocity_[0]; - velocity_[1] = newVelocity_[1]; - position_ += velocity_ * sim_->timeStep_; - - part->move(VEC3(position_[0],position_[1],0)); -} - -bool linearProgram1(const std::vector& lines, size_t lineNo, float radius, const Agent::Agent::VEC_A& optVelocity, bool directionOpt, Agent::VEC_A& result) -{ - const float discriminant = sqr(radius) - sqr(det2D(lines[lineNo].direction, lines[lineNo].point)); - - if (discriminant < 0.0f) { - /* Max speed circle fully invalidates line lineNo. */ - return false; - } - - const float sqrtDiscriminant = std::sqrt(discriminant); - - float tLeft = -(lines[lineNo].direction * lines[lineNo].point) - sqrtDiscriminant; - - float tRight = -(lines[lineNo].direction * lines[lineNo].point) + sqrtDiscriminant; - - for (size_t i = 0; i < lineNo; ++i) { - const float determinant = det2D(lines[lineNo].direction, lines[i].direction); - - if (std::fabs(determinant) <= RVO_EPSILON) { - /* Lines lineNo and i are (almost) parallel. */ - if (det2D(lines[i].direction, lines[lineNo].point - lines[i].point) < 0.0f) { - /* Line i fully invalidates line lineNo. */ - return false; - } else { - /* Line i does not impose constraint on line lineNo. */ - continue; - } - } - - const float t = det2D(lines[i].direction, lines[lineNo].point - lines[i].point) / determinant; - - if (determinant > 0.0f) { - /* Line i bounds line lineNo on the right. */ - tRight = std::min(tRight, t); - } else { - /* Line i bounds line lineNo on the left. */ - tLeft = std::max(tLeft, t); - } - - if (tLeft > tRight) { - return false; - } - } - - if (directionOpt) { - /* Optimize direction. */ - if (optVelocity * lines[lineNo].direction > 0.0f) { - /* Take right extreme. */ - result = lines[lineNo].point + tRight * lines[lineNo].direction; - } else { - /* Take left extreme. */ - result = lines[lineNo].point + tLeft * lines[lineNo].direction; - } - } else { - /* Optimize closest point. */ - const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point); - - if (t < tLeft) { - result = lines[lineNo].point + tLeft * lines[lineNo].direction; - } else if (t > tRight) { - result = lines[lineNo].point + tRight * lines[lineNo].direction; - } else { - result = lines[lineNo].point + t * lines[lineNo].direction; - } - } - - return true; -} - -bool linearProgram2(const std::vector& lines, size_t num, float radius, const Agent::VEC_A& optVelocity, bool directionOpt, Agent::VEC_A& result) -{ - if (directionOpt) { - /* - * Optimize direction. Note that the optimization velocity is of unit - * length in this case. - */ - result = optVelocity * radius; - } else if (absSq(optVelocity) > sqr(radius)) { - /* Optimize closest point and outside circle. */ - result = normalize(optVelocity) * radius; - } else { - /* Optimize closest point and inside circle. */ - result = optVelocity; - } - - for (size_t i = 0; i < num; ++i) { - if (det2D(lines[i].direction, result - lines[i].point) < 0.0f) { - /* Result does not satisfy constraint i. Compute new optimal result. */ - if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) { - return false; - } - } - } - - return true; -} - -void linearProgram3(const std::vector& lines, size_t numObstLines, float radius, Agent::VEC_A& result) -{ - if (!linearProgram2(lines, numObstLines, radius, Agent::VEC_A(), false, result)) { - /* - * This should not happen unless agents are initialized within obstacle - * and time step is small. - */ - linearProgram2(lines, numObstLines, std::numeric_limits::infinity(), Agent::VEC_A(), false, result); - - return; - } - - float distance = 0.0f; - - for (size_t i = numObstLines; i < lines.size(); ++i) { - if (det2D(lines[i].direction, lines[i].point - result) > distance) { - /* Result does not satisfy constraint of line i. */ - std::vector projLines(lines.begin(), lines.begin() + numObstLines); - - for (size_t j = numObstLines; j < i; ++j) { - Line line; - - float determinant = det2D(lines[i].direction, lines[j].direction); - - if (std::fabs(determinant) <= RVO_EPSILON) { - /* Line i and line j are (almost) parallel. */ - if (lines[i].direction * lines[j].direction > 0.0f) { - /* Line i and line j point in the same direction. */ - continue; - } else { - /* Line i and line j point in opposite direction. */ - line.point = 0.5f * (lines[i].point + lines[j].point); - } - } else { - line.point = lines[i].point + (det2D(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction; - } - - line.direction = normalize(lines[j].direction - lines[i].direction); - projLines.push_back(line); - } - - const Agent::VEC_A tempResult = result; - - if (!linearProgram2(projLines, projLines.size(), radius, Agent::VEC_A(-lines[i].direction[1], lines[i].direction[0],0), true, result)) { - /* This should in principle not happen. The result is by definition - * already in the feasible region of this linear program. If it fails, - * it is due to small floating point error, and the current result is - * kept. - */ - result = tempResult; - } - - distance = det2D(lines[i].direction, lines[i].point - result); - } - } -} diff --git a/Apps/Examples/SocialAgents/src/env_map.cpp b/Apps/Examples/SocialAgents/src/env_map.cpp deleted file mode 100644 index b62c6ef3..00000000 --- a/Apps/Examples/SocialAgents/src/env_map.cpp +++ /dev/null @@ -1,733 +0,0 @@ -#include "env_map.h" -#include "Geometry/inclusion.h" -#include "utils.h" -#include "agent.h" -#include "Algo/MovingObjects/particle_cell_2D_memo.h" - -#include "Algo/Modelisation/subdivision.h" -#include "Algo/Geometry/normal.h" - -using namespace CGoGN; - -EnvMap::EnvMap() : closeMark(map) -{ - position = map.addAttribute(VERTEX_ORBIT, "position"); - normal = map.addAttribute(VERTEX_ORBIT, "normals"); - agentvect = map.addAttribute(FACE_ORBIT, "agent"); - - dSimplify = Dart(0) ; // map.begin(); - dSubdivide = Dart(0) ; // map.begin(); -} - -bool EnvMap::simplifiableDart(Dart d) -{ - VEC3 p1 = position[map.phi_1(map.phi2(d))]; - VEC3 p2 = position[map.phi1(d)]; - VEC3 p3 = position[map.phi1(map.phi1(d))]; - - if(Geom::testOrientation2D(p2,p1,p3)!=Geom::LEFT) { - return true; - } - - return false; -} - -bool EnvMap::simplifiable(Dart d) -{ - if(!closeMark.isMarked(map.phi2(d)) && position[d][2]==0.0f) { - VEC3 n1 = Algo::Geometry::triangleNormal(map,d,position); - VEC3 n2 = Algo::Geometry::triangleNormal(map,map.phi2(d),position); - if(n1[0]==0.0f && n1[1]==0.0f && n1[2]==1.0f && n1[0]==n2[0] && n1[0]==n2[0] && n1[0]==n2[0]) { - Dart d2 = map.phi2(d); - VEC3 p1 = position[map.phi_1(d)]; - VEC3 p2 = position[d]; - - VEC3 p3 = position[d2]; - VEC3 p4 = position[map.phi_1(d2)]; - - VEC3 p5 = position[map.phi1(map.phi1(d))]; - VEC3 p6 = position[map.phi1(map.phi1(d2))]; - if( - (p1[0]!=p6[0] || p1[1]!=p6[1] || p1[2]!=p6[2]) && (p5[0]!=p4[0] || p5[1]!=p4[1] || p5[2]!=p4[2]) && - Geom::testOrientation2D(p6,p1,p2)!=Geom::RIGHT - && Geom::testOrientation2D(p5,p4,p3)!=Geom::RIGHT - ) - return true; - } - } - - return false; -} - -unsigned int EnvMap::getNbFaces() -{ - unsigned int count=0; - for(unsigned int it = agentvect.begin(); it != agentvect.end() ; agentvect.next(it)) - ++count; - - return count; -} - -bool EnvMap::noAgentSurroundingVertex(Dart d) -{ - Dart dd = d; - do { - if(agentvect[dd].size()>0) - return false; - dd = map.alpha1(dd); - } while(dd!=d); - - return true; -} - -bool EnvMap::simplifyVertex(Dart& d) -{ - - bool simplifOk=true; - Dart dd = d; - do { - simplifOk = simplifiable(dd); - dd = map.alpha1(dd); - } while(simplifOk && dd!=d); - - if(simplifOk) { - Dart d2 = map.phi2(map.phi1(dd)); - if(d2!=map.phi2(d2)) { - bool finished=false; - do { - Dart d1 = map.alpha1(dd); - if (d1==dd) // last edge is pending edge inside of face - finished=true; - map.deleteFace(dd); - dd = d1; - } while (!finished); - - map.closeHole(d2); - - return true; - } - } - - return false; -} - -bool EnvMap::collapsableEdge(Dart& d,Dart& dStop) -{ - bool simplification; - bool simplifiable=false; - - if(map.alpha1(map.alpha1(d))!=d) { - Dart dd=d; - do { - simplification=false; - dd = map.phi1(dd); - if(map.alpha1(map.alpha1(dd))==dd) { - bool obstacle=closeMark.isMarked(dd) || closeMark.isMarked(map.phi2(dd)); - - if(!obstacle) { - simplification=true; - simplifiable=true; - } - else - simplifiable=false; - } - } while(simplification); - - dStop = dd; - } - - return simplifiable; -} - -bool EnvMap::simplifyEdge(Dart& d) -{ - Dart dNext; - if(collapsableEdge(d,dNext) && simplifiableDart(map.phi2(d)) && simplifiableDart(map.phi_1(dNext))) { - Dart dd=map.phi1(d); - VEC3 nextPos; - while(map.phi1(dd)!=dNext) { - map.collapseEdge(dd); - dd=map.phi1(d); - } - nextPos = position[map.phi1(dd)]; - map.collapseEdge(dd); - position[map.phi1(d)] = nextPos; - - map.mergeFaces(d); - return true; - } - return false; -} - -void EnvMap::simplify() -{ - -// for(unsigned int i=0;i<10;++i) { - bool nomodif; - do { -// map.check(); - - nomodif=true; - for(Dart d= map.begin() ; d!= map.end(); map.next(d)) { -// std::cout << "dart " << d << std::endl; -// simplifyVertex(d); - nomodif = !simplifyVertex(d) && nomodif; - } - - for(Dart d= map.begin() ; d!= map.end(); map.next(d)) { - simplifyEdge(d); - } - } while(!nomodif); - - map.check(); - -// for(Dart d= map.begin() ; d!= map.end(); map.next(d)) { -// if(simplifiable(d)) { -// map.mergeFaces(d); -// } -// } -// std::cout << "i" << i << std::endl; -// } -} - -Dart EnvMap::getDart(unsigned int i) -{ - Dart d; - for(d=map.begin(); d !=map.end() ; map.next(d)) { - if(d==i) - return d; - } - - std::cout << "Dart not found" << std::endl; - return d; -} - -Dart EnvMap::getBelongingCell(const PFP::VEC3& pos) -{ - Dart d; - for(d=map.begin(); d!=map.end() ; map.next(d)) { - if(!closeMark.isMarked(d) && Algo::Geometry::isPointInConvexFace2D(map,d,position,pos,true)) { - return d; - } - } - - std::cout << "pos not in map" << std::endl; - - return map.begin(); -} - -void EnvMap::insertObstacleOfFace(PFP::AGENTS agents,const Dart d) -{ - if(map.getDartLevel(map.faceOldestDart(d))==0) { - - Dart dd =d; - do { - if(closeMark.isMarked(dd) /*&& (position[map.phi2(dd)][2]==0.0 || position[map.phi1(map.phi2(dd))][2] ==0.0f)*/) { - for(PFP::AGENTS::iterator it=agents.begin(); it!= agents.end(); ++it) { - // if(leftOf((*it)->part->m_position,position[dd],position[map.phi1(dd)])<0.0f) { - (*it)->insertObstacleNeighbor(dd); - // } - } - // return; - } - dd = map.phi1(dd); - } while (dd!=d); - } -} - -void EnvMap::getAllFacesOfAgents(Dart d) -{ - for(PFP::AGENTS::iterator it=agentvect[d].begin(); it!= agentvect[d].end(); ++it) { - (*it)->treated=true; - (*it)->obstacleNeighbors_.clear(); - (*it)->agentNeighbors_.clear(); - } - - insertObstacleOfFace(agentvect[d],d); - addNeighborAgents(agentvect[d],agentvect[d]); - - Dart dd = d; - do { - Dart ddd = map.alpha1(dd); - do { - ddd = map.alpha1(ddd); - if(ddd!=dd) { -// if(!closeMark.isMarked(dd)) - insertObstacleOfFace(agentvect[d],ddd); - addNeighborAgents(agentvect[d],agentvect[ddd]); - } - } while(ddd!=dd); - - dd = map.phi1(dd); - }while(dd!=d); - - - -// pushAgentInCell(agent,d); -// l_contact_Dart.push_back(d); -// cm.mark(d); - -// insertObstacleOfFace(agent,d); -// -// Dart dd = d; -// do { -// Dart ddd = map.alpha1(dd); -// do { -// ddd = map.alpha1(ddd); -// if(ddd!=dd) { -// // pushAgentInCell(agent,ddd); -// // l_contact_Dart.push_back(d); -// // cm.mark(ddd); -// insertObstacleOfFace(agent,ddd); -// } -// } while(ddd!=dd); -// -// dd = map.phi1(dd); -// }while(dd!=d); - -// pushAgentInCell(agent,d,l_contact_Dart); -// -// insertObstacleOfFace(agent,d); -// -// Dart dd = d; -// do { -// Dart ddd = map.alpha1(dd); -// do { -// ddd = map.alpha1(ddd); -// if(ddd!=dd) { -// pushAgentInCell(agent,ddd,l_contact_Dart); -// insertObstacleOfFace(agent,ddd); -// } -// } while(ddd!=dd); -// -// dd = map.phi1(dd); -// }while(dd!=d); -} - -void EnvMap::pushAgentInCell(Agent * agent, Dart d) -{ - agentvect[d].push_back(agent); -} - -void EnvMap::popAgentInCell(Agent * agent, Dart d) -{ - for(PFP::AGENTVECT::iterator it = agentvect[d].begin(); it != agentvect[d].end() ; ++it) - if(*it==agent) { - agentvect[d].erase(it); - return; - } - - std::cout << "ERROR : cannot popAgentInCell" << std::endl; -} - - -void EnvMap::linkAgentWithCells(Agent* agent) -{ - if(!agent->treated) { - getAllFacesOfAgents(agent->part->d); - } -} - -void EnvMap::addNeighborAgents(PFP::AGENTS agentsFrom,PFP::AGENTS agentsTo) -{ - for(PFP::AGENTS::iterator itA = agentsFrom.begin() ; itA != agentsFrom.end() ; ++itA) { - float range= sqr((*itA)->neighborDist_); - for(PFP::AGENTS::iterator itB = agentsTo.begin() ; itB != agentsTo.end() ; ++itB) { - size_t neighbour = (*itB)->agentNo_; - (*itA)->insertAgentNeighbor(neighbour,range); - } - } -} - -void EnvMap::updateMap() -{ - simplifyFaces() ; - subdivideFaces() ; - map.setCurrentLevel(map.getMaxLevel()) ; -} - -void EnvMap::subdivideFaces() -{ - CellMarker m(map, FACE_CELL) ; - for(Dart d = map.begin(); d != map.end(); map.next(d)) - { - if(!m.isMarked(d)) - { - m.mark(d) ; - if(!closeMark.isMarked(d) && agentvect[d].size() > 6) - { - if(!map.faceIsSubdivided(d)) - { - d = map.faceOldestDart(d) ; - - std::vector agents ; - agents.swap(agentvect[d]) ; - - unsigned int cur = map.getCurrentLevel() ; - unsigned int fLevel = map.faceLevel(d) ; - - map.setCurrentLevel(fLevel) ; - std::vector marked ; - Dart fit = d ; - do - { - if(closeMark.isMarked(map.phi2(fit))) - marked.push_back(fit) ; - fit = map.phi1(fit) ; - } while(fit != d) ; - - Algo::IHM::subdivideFace(map, d, position) ; - - map.setCurrentLevel(fLevel + 1) ; - for(std::vector::iterator it = marked.begin(); it != marked.end(); ++it) - closeMark.mark(map.phi2(*it)) ; - - map.setCurrentLevel(map.getMaxLevel()) ; - for(PFP::AGENTS::iterator it = agents.begin(); it != agents.end(); ++it) - { - resetAgentInFace(*it) ; - agentvect[(*it)->part->d].push_back(*it) ; - } - - map.setCurrentLevel(cur) ; - } - } - } - } - -// for(unsigned int i=0;i<8;++i) { -// if(/*!fm.isMarked(d) && */!closeMark.isMarked(dSubdivide) && !closeMark.isMarked(map.phi2(dSubdivide))) { -// VEC3 n = Algo::Geometry::triangleNormal(map,dSubdivide,position); -// if(n[2]==1.0f) { -// if(checkForSubdivision(dSubdivide)) { -// map.next(dSubdivide); -// if(dSubdivide==map.end()) { -// dSubdivide = map.begin(); -// } -// return; -// } -// } -// } -// map.next(dSubdivide); -// -// if(dSubdivide==map.end()) { -// dSubdivide = map.begin(); -// } -// } -} - -void EnvMap::simplifyFaces() -{ - assert(map.getCurrentLevel() == map.getMaxLevel()) ; - unsigned int cur = map.getCurrentLevel() ; - - if(cur == 0) - return ; - - CellMarker m(map, FACE_CELL) ; - for(Dart d = map.begin(); d != map.end(); map.next(d)) - { - if(!m.isMarked(d)) - { - m.mark(d) ; - unsigned int fLevel = map.faceLevel(d) ; - if(!closeMark.isMarked(d) && fLevel > 0) - { - Dart old = map.faceOldestDart(d) ; - map.setCurrentLevel(fLevel - 1) ; - if(map.faceIsSubdividedOnce(old)) - { - unsigned int nbAgents = 0 ; - Dart fit = old ; - do - { - nbAgents += agentvect[fit].size() ; - fit = map.phi1(fit) ; - } while(fit != old) ; - - if(nbAgents < 6) - { - std::vector agents ; - fit = old ; - do - { - agents.insert(agents.end(), agentvect[fit].begin(), agentvect[fit].end()) ; - fit = map.phi1(fit) ; - } while(fit != old) ; - - Algo::IHM::coarsenFace(map, old, position) ; - - agentvect[old].clear() ; - for(PFP::AGENTS::iterator it = agents.begin(); it != agents.end(); ++it) - { - resetAgentInFace(*it) ; - agentvect[(*it)->part->d].push_back(*it) ; - } - } - } - map.setCurrentLevel(cur) ; - } - } - } - -//// for(Dart d = map.begin(); d!= map.end() ; map.next(d)) { -// for(unsigned int i=0;i<2;++i) { -//// std::cout << "subdividedDarts size " << subdividedDarts.size() << std::endl; -//// for(std::vector::iterator it = subdividedDarts.begin(); it != subdividedDarts.end() ; ++it) { -//// Dart d=*it; -//// if(map.phi1(d)!=map.phi_1(d)) -//// do { -//// Dart dNext = dSimplify; -//// map.next(dNext); -// -// if(!closeMark.isMarked(dSimplify) && !closeMark.isMarked(map.phi2(dSimplify)) && agentvect[dSimplify].size()==0) { -//// if(noAgentSurroundingVertex(dSimplify) && simplifyVertex(dSimplify)) { -//// std::cout << "erase some darts 1" << std::endl; -//// // subdividedDarts.erase(it); -//// map.next(dSimplify); -//// return; -//// } -//// else { -// if(agentvect[map.phi2(dSimplify)].size()==0) { -// if(simplifyEdge(dSimplify)) { -// std::cout << "erase some darts 2" << std::endl; -//// subdividedDarts.erase(it); -//// dSimplify = dNext; -// map.next(dSimplify); -// return; -// } -// else if(simplifiable(dSimplify)) { -// std::cout << "erase some darts 3" << std::endl; -// map.mergeFaces(dSimplify); -//// subdividedDarts.erase(it); -//// dSimplify = dNext; -// map.next(dSimplify); -// return; -// } -// } -//// } -// } -//// d=map.phi1(d); -//// }while(d!=*it); -// map.next(dSimplify); -//// dSimplify = dNext; -// -// if(dSimplify==map.end()) { -// dSimplify = map.begin(); -// } -// } -} - -bool EnvMap::checkForSubdivision(Dart d) -{ - if(agentvect[d].size() > 10) { -// if(map.phi<1111>(d)==d) { -// if(!closeMark.isMarked(d) && subdivideSquare(d)) { -// // map.check(); -// // std::cout << "subdivided square" << std::endl; -// for(PFP::AGENTS::iterator it = agentvect[d].begin() ;it != agentvect[d].end() ; ++it ) { -// resetAgentInFace(*it,d); -// Dart dd = map.phi2(map.phi1(d)); -// Dart ddd=dd; -// do { -// // std::cout << "push 4 times" << std::endl; -// pushAgentInCell(*it,ddd,(*it)->includingFaces); -// ddd = map.alpha1(ddd); -// } while(dd!=map.alpha1(ddd)); -// } -// // std::cout << "ok" << std::endl; -// return true; -// } -// } -// else -// { - Dart dd=d; - bool found=false; - do { - if(Geom::testOrientation2D(position[map.phi_1(dd)],position[dd],position[map.phi1(dd)])==Geom::ALIGNED && (position[map.phi1(dd)][0]!= position[dd][0] || position[map.phi1(dd)][1]!= position[dd][1])) - found = true; - else - dd = map.phi1(dd); - } while(!found && dd!=d); - - if(found && subdivideFace(dd)) { - // map.check(); - // if(found && !map.faceIsSubdivided(d)) { - // Algo::IHM::subdivideFace(map,d,position); - // std::cout << "replace agents : " << agentvect[dd].size() << std::endl; - for(PFP::AGENTS::iterator it = agentvect[dd].begin() ;it != agentvect[dd].end() ; ++it ) { - // std::cout << "agent " << (*it)->agentNo_ << std::endl; - resetAgentInFace(*it); - pushAgentInCell(*it,map.alpha1(dd)); - } - - return true; - } -// } - } - - return false; -} - -bool EnvMap::subdivideSquare(Dart d) -{ - std::cout << "subdivide square" << std::endl; - VEC3 c; - Dart dd=d; - do { - VEC3 mid(position[dd]+position[map.phi1(dd)]); - mid *= 0.5f; - map.cutEdge(dd); - if(closeMark.isMarked(map.phi2(dd))) - closeMark.mark(map.phi2(map.phi1(dd))); - position[map.phi1(dd)] = mid; - c +=mid; - dd = map.phi1(map.phi1(dd)); - } while(dd!=d); - c *= 0.25f; - - position[Algo::Modelisation::quadranguleFace(map,dd)] = c; - - return true; -} - -bool EnvMap::subdivideFace(Dart d) -{ - int coordChg = 0; - int phiCount=0; - -// std::cout << "start " << std::endl; - VEC3 posD = position[d]; - - if(posD[1]!=position[map.phi1(d)][1]) - coordChg = 1; - - if(fabs(position[map.phi1(d)][coordChg]-posD[coordChg])(position[map.phi_1(dd)],position[dd],position[map.phi1(dd)])==Geom::ALIGNED) { -// std::cout << " pos d " << position[d] << " dd " << position[dd] << std::endl; -// std::cout << " d " << d << " dd " << dd << std::endl; -// std::cout << " coord " << coordChg << std::endl; - map.splitFace(d,dd); - - int nbCut = (position[d][coordChg]-position[dd][coordChg])/sideSize; - if(nbCut>0) - nbCut--; - else - nbCut++; - - while(abs(nbCut)>0) { - map.cutEdge(map.phi_1(d)); - position[map.phi_1(d)] = position[d]; - position[map.phi_1(d)][coordChg] -= nbCut*this->sideSize; - -// std::cout << " cutEdge : " << position[map.phi_1(d)] << std::endl; - - if(nbCut>0) - nbCut--; - else - nbCut++; - } - - return true; - } - - return false; -} - -VEC3 EnvMap::faceCenter(Dart d) -{ - VEC3 center(0,0,0); - unsigned int count = 0 ; - Dart it = d ; - do - { - center += position[it]; - ++count ; -// while(map.phi1(it)!=d && Geom::testOrientation2D(position[map.phi1(d)],position[d],position[map.phi1(map.phi1(d))])==Geom::ALIGNED) -// it=map.phi1(it); - - it = map.phi1(it) ; - } while(it != d) ; - -// if(count<4) { -// std::cout << "??????" << count << std::endl; -// } - - center /= double(count); - return center ; -} - -void EnvMap::resetAgentInFace(Agent * agent) -{ -// agent->part->state = VERTEX_ORBIT; -// agent->part->m_position = position[agent->part->d]; - -// agent->part->m_position = agent->part->pointInFace(agent->part->d); - agent->part->m_position = Algo::Geometry::faceCentroid(map,agent->part->d,position); - agent->part->state = FACE_ORBIT; - agent->part->move(agent->position_) ; -// agent->part->d = d; -} - -void EnvMap::addObstacle(std::vector points) -{ - std::cout << "test" << std::endl; - std::vector::iterator it = points.begin(); - VEC3 p1 = *it; -// std::cout << "p1 " << p1 << std::endl; - it++; - VEC3 p2; - Dart d = getBelongingCell(p1); - closeMark.markOrbit(FACE_ORBIT,d); - Algo::MovingObjects::ParticleCell2DMemo part(map,d,p1,position,closeMark); -// Algo::MovingObjects::ParticleCell2D part(map,d,p1,position,closeMark); - do { - p2 = *it; - - part.move(p2); - d = part.d; - for(std::list::iterator toMark = part.memo_cross.begin();toMark != part.memo_cross.end(); ++toMark) - closeMark.markOrbit(FACE_ORBIT,*toMark); - part.memo_cross.clear(); -// map.markOrbit(FACE_ORBIT,d,closeMark); - - std::cout << "p1 " << p1 << "p2 " << p2 << std::endl; - it++; - p1 = p2; - - } while(it!=points.end()); - - p2 = *(points.begin()); - std::cout << "p1" << p1 << "p2 " << p2 << std::endl; - part.move(p2); -// d = part.d; -// map.markOrbit(FACE_ORBIT,d,closeMark); - for(std::list::iterator toMark = part.memo_cross.begin();toMark != part.memo_cross.end(); ++toMark) - closeMark.markOrbit(FACE_ORBIT,*toMark); -} - diff --git a/Apps/Examples/SocialAgents/src/simulator.cpp b/Apps/Examples/SocialAgents/src/simulator.cpp deleted file mode 100644 index 3e32d7f2..00000000 --- a/Apps/Examples/SocialAgents/src/simulator.cpp +++ /dev/null @@ -1,411 +0,0 @@ -#include "simulator.h" -#include - -// #ifdef HAVE_CONFIG_H -// #include "config.h" -// #endif -// -// #if HAVE_OPENMP || _OPENMP -// #include -// #endif - -Simulator::Simulator() : agents_(), defaultAgent_(0), globalTime_(0.0f), timeStep_(1.0f) -{ -// CGoGN::CityGenerator::generateGrid(envMap.map,envMap.position,100,100,50.0f,envMap.closeMark); - - envMap.sideSize = 70.0f; - CGoGN::CityGenerator::generateSmallCity(envMap.map,envMap.position,envMap.closeMark,envMap.sideSize); - -// CGoGN::CityGenerator::generateAbsolutSpiralOfDeath(envMap.map,envMap.position,envMap.closeMark,2000,0.25,110,100); -// CGoGN::CityGenerator::generateToboggan(envMap.map,envMap.position,envMap.closeMark,100,0.25,200,10); - -// std::cout << "simplify" << std::endl; -// envMap.simplify(); - envMap.map.init(); - std::cout << "setup scenario" << std::endl; - setupScenario(); -// setupHelicoidScenario(1,50); - -// Dart dStop=envMap.map.begin(); -// for(unsigned int i=0;i<5000;++i) envMap.map.next(dStop); -// FunctorIsMarked fum(envMap.closeMark); -// path = CGoGN::PathFinder::pathFindAStar(envMap.map,envMap.position,envMap.map.begin(),dStop,fum); -} - -Simulator::Simulator(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const VEC3& velocity) : agents_(), defaultAgent_(0), globalTime_(0.0f), timeStep_(timeStep) -{ - defaultAgent_ = new Agent(this,0); - - defaultAgent_->maxNeighbors_ = maxNeighbors; - defaultAgent_->maxSpeed_ = maxSpeed; - defaultAgent_->neighborDist_ = neighborDist; - defaultAgent_->newVelocity_ = Agent::VEC_A(velocity[0],velocity[1],0); - defaultAgent_->radius_ = radius; - defaultAgent_->timeHorizon_ = timeHorizon; - defaultAgent_->timeHorizonObst_ = timeHorizonObst; - defaultAgent_->velocity_ = Agent::VEC_A(velocity[0],velocity[1],0); -} - -Simulator::~Simulator() -{ - if (defaultAgent_ != 0) { - delete defaultAgent_; - } - - for (size_t i = 0; i < agents_.size(); ++i) { - delete agents_[i]; - } -} - -size_t Simulator::addAgent(VEC3 position) -{ - if (defaultAgent_ == 0) { - return RVO_ERROR; - } - - Agent* agent = new Agent(this, position,agents_.size()); - agents_.push_back(agent); - - return agents_.size() - 1; -} - -size_t Simulator::addAgent(VEC3 position, Dart d) -{ - if (defaultAgent_ == 0) { - return RVO_ERROR; - } - - Agent* agent = new Agent(this, position,agents_.size(),d); - agents_.push_back(agent); - - return agents_.size() - 1; -} - -size_t Simulator::addAgent(VEC3 position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const VEC3& velocity) -{ - Agent* agent = new Agent(this, Agent::VEC_A(position[0],position[1],0), neighborDist, maxNeighbors, timeHorizon, timeHorizonObst, radius, Agent::VEC_A(velocity[0],velocity[1],0), maxSpeed,agents_.size()); - agents_.push_back(agent); - - return agents_.size() - 1; -} - -void Simulator::doStep() -{ -// envMap.map.check(); - if(globalTime_==0) { - - } - - for (unsigned int i = 0; i < agents_.size(); ++i) { - envMap.linkAgentWithCells(agents_[i]); - } - -// #pragma omp parallel for - for (unsigned int i = 0; i < agents_.size(); ++i) { - agents_[i]->computeNewVelocity(); - } - - -// #pragma omp parallel for - for (unsigned int i = 0; i < agents_.size(); ++i) { - envMap.popAgentInCell(agents_[i],agents_[i]->part->d); - agents_[i]->update(); - envMap.pushAgentInCell(agents_[i],agents_[i]->part->d); - agents_[i]->treated=false; - } - - envMap.updateMap(); - - globalTime_ += timeStep_; -} - -VEC3 Simulator::getAgentPrefVelocity(size_t agentNo) const -{ - return VEC3(agents_[agentNo]->prefVelocity_[0],agents_[agentNo]->prefVelocity_[1],0); -} - -void Simulator::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const VEC3& velocity) -{ - if (defaultAgent_ == 0) { - defaultAgent_ = new Agent(this,0); - } - - defaultAgent_->maxNeighbors_ = maxNeighbors; - defaultAgent_->maxSpeed_ = maxSpeed; - defaultAgent_->neighborDist_ = neighborDist; - defaultAgent_->newVelocity_ = Agent::VEC_A(velocity[0],velocity[1],0); - defaultAgent_->radius_ = radius; - defaultAgent_->timeHorizon_ = timeHorizon; - defaultAgent_->timeHorizonObst_ = timeHorizonObst; - defaultAgent_->velocity_ = Agent::VEC_A(velocity[0],velocity[1],0); -} - -void Simulator::setAgentPrefVelocity(size_t agentNo, const VEC3& prefVelocity) -{ - agents_[agentNo]->prefVelocity_ = Agent::VEC_A(prefVelocity[0],prefVelocity[1],0); -} - -void Simulator::setTimeStep(float timeStep) -{ - timeStep_ = timeStep; -} - -/** Added functions */ - -std::vector Simulator::getAgentsPosition() -{ - std::vector v; - for (size_t i = 0; i < agents_.size() ; ++i) { - VEC3 rvo_v = agents_[i]->position_; - v.push_back(PFP::VEC3(rvo_v[0],rvo_v[1],0)); - } - - return v; -} - -void Simulator::setPreferredVelocities(PFP::VEC3 posToReach) -{ - /* - * Set the preferred velocity to be a vector of unit magnitude (speed) in the - * direction of the goal. - */ -// #pragma omp parallel for - for (int i = 0; i < static_cast(agents_.size()); ++i) { - VEC3 goalVector = goals[i] - agents_[i]->position_; - - if (absSq(goalVector) > 1.0f) { - goalVector = normalize(goalVector); - } - - this->setAgentPrefVelocity(i, goalVector); - - /* - * Perturb a little to avoid deadlocks due to perfect symmetry. - */ - float angle = std::rand() * 2.0f * M_PI / RAND_MAX; - float dist = std::rand() * 0.0001f / RAND_MAX; - - this->setAgentPrefVelocity(i, this->getAgentPrefVelocity(i) + - VEC3(dist*std::cos(angle), dist*std::sin(angle),0.0f)); - } -} - -void Simulator::setPreferredNextCellVelocities() -{ - for (int i = 0; i < static_cast(agents_.size()); ++i) { - Dart dNext = agents_[i]->part->d; - while(envMap.closeMark.isMarked(envMap.map.phi2(dNext)) && envMap.map.phi1(dNext) != agents_[i]->part->d) - dNext = envMap.map.phi1(dNext); - - VEC3 goalVector; - if(envMap.closeMark.isMarked(envMap.map.phi2(dNext))) { - goalVector = VEC3(0,0,0); - } - else { -// goalVector = Algo::Geometry::faceCentroid(envMap.map,envMap.map.phi2(dNext),envMap.position); - goalVector = (envMap.position[dNext]+envMap.position[envMap.map.phi2(dNext)])*0.5f; - goalVector -= agents_[i]->position_; - goalVector = normalize(goalVector); - } - -// goalVector *= 0.5f; -// goalVector += agents_[i]->prefVelocity_; -// goalVector *= 0.5f; - -// if (absSq(goalVector) > 1.0f) { -// goalVector = normalize(goalVector); -// } - - this->setAgentPrefVelocity(i, goalVector); - - /* - * Perturb a little to avoid deadlocks due to perfect symmetry. - */ - float angle = std::rand() * 2.0f * M_PI / RAND_MAX; - float dist = std::rand() * 0.0001f / RAND_MAX; - - this->setAgentPrefVelocity(i, this->getAgentPrefVelocity(i) + - VEC3(dist*std::cos(angle), dist*std::sin(angle),0.0f)); - } -} - -void Simulator::setPreferredPathVelocities() -{ - for (int i = 0; i < static_cast(agents_.size()); ++i) { -// std::cout << "i " << i << " pathPointer : " << pathPointer[i].first << " " << pathPointer[i].second << std::endl; -// std::cout << " point to reach : " << (*pathToFollow[pathPointer[i].first])[pathPointer[i].second] << std::endl; - VEC3 goalVector((*pathToFollow[pathPointer[i].first])[pathPointer[i].second]); - goalVector -= agents_[i]->position_; - - if (absSq(goalVector) > 100.0f) { -// goalVector = normalize(goalVector); - } - else { - pathPointer[i].second = (pathPointer[i].second+1)%pathToFollow[pathPointer[i].first]->size(); - goalVector = (*pathToFollow[pathPointer[i].first])[pathPointer[i].second] - agents_[i]->position_; -// goalVector = normalize(goalVector); - } - - this->setAgentPrefVelocity(i, goalVector); - - /* - * Perturb a little to avoid deadlocks due to perfect symmetry. - */ - float angle = std::rand() * 2.0f * M_PI / RAND_MAX; - float dist = std::rand() * 0.0001f / RAND_MAX; - - this->setAgentPrefVelocity(i, this->getAgentPrefVelocity(i) + - VEC3(dist*std::cos(angle), dist*std::sin(angle),0.0f)); - - } -} - -void Simulator::setupScenario() -{ - srand(0); - /* Specify the global time step of the simulation. */ - setTimeStep(0.25f); -// setTimeStep(1.0f); - - /* Specify the default parameters for agents that are subsequently added. */ -// setAgentDefaults(15.0f, 20, 10.0f, 10.0f, 1.5f, 2.0f); - setAgentDefaults(15.0f, 10, 10.0f, 10.0f, 1.5f, 2.0f); - - /* - * Add agents, specifying their start position, and store their goals on the - * opposite side of the environment. - */ -// int i = 0; - Dart d = envMap.map.begin(); - DartMarkerStore filled(envMap.map); - - bool found=false; - - for (int i = 0; i < 500 && d != envMap.map.end(); ++i) { - found = false; - VEC3 pos; - Dart dCell; - while(!found && d != envMap.map.end()) { - VEC3 n = Algo::Geometry::triangleNormal(envMap.map,d,envMap.position); - if(!envMap.closeMark.isMarked(d) && !filled.isMarked(d) && envMap.position[d][2]==0.0f && n[2]==1.0f) { - pos = envMap.faceCenter(d); - dCell=d; - found = true; - filled.markOrbit(FACE_ORBIT,d); - std::cout << "pos " << pos << std::endl; - } - envMap.map.next(d); - } - - if(found) { - float ecart=3.0f; - addAgent(pos,dCell); - addAgent(pos+VEC3(ecart,0,0),dCell); - addAgent(pos+VEC3(-ecart,0,0),dCell); - addAgent(pos+VEC3(ecart,ecart,0),dCell); - addAgent(pos+VEC3(0,ecart,0),dCell); - addAgent(pos+VEC3(0,-ecart,0),dCell); - addAgent(pos+VEC3(-ecart,-ecart,0),dCell); - addAgent(pos+VEC3(ecart,-ecart,0),dCell); - addAgent(pos+VEC3(-ecart,ecart,0),dCell); - goals.push_back(-1.0f*pos); - goals.push_back(-1.0f*(pos+VEC3(ecart,0,0))); - goals.push_back(-1.0f*(pos+VEC3(-ecart,0,0))); - goals.push_back(-1.0f*(pos+VEC3(ecart,ecart,0))); - goals.push_back(-1.0f*(pos+VEC3(0,ecart,0))); - goals.push_back(-1.0f*(pos+VEC3(0,-ecart,0))); - goals.push_back(-1.0f*(pos+VEC3(-ecart,-ecart,0))); - goals.push_back(-1.0f*(pos+VEC3(ecart,-ecart,0))); - goals.push_back(-1.0f*(pos+VEC3(-ecart,ecart,0))); - } - } - - std::cout << "nb of agents : " << goals.size() << std::endl; - - std::random_shuffle( goals.begin(), goals.end() ); - - for (int i = 0; i < static_cast(agents_.size()); ++i) - envMap.pushAgentInCell(agents_[i],agents_[i]->part->d); - -} - -void Simulator::setupHelicoidScenario(float rMax,float rMin) -{ - srand(0); - setTimeStep(0.25f); - setAgentDefaults(15.0f, 10, 10.0f, 10.0f, 1.5f, 2.0f); - - /* - * Add agents, specifying their start position, and store their goals on the - * opposite side of the environment. - */ -// int i = 0; - Dart d = envMap.map.begin(); - for(unsigned int i=0;i<50;++i) - envMap.map.next(d); - DartMarkerStore filled(envMap.map); - - bool found=false; - int nbPack=2000; - - for (int i = 0; i < nbPack && d != envMap.map.end(); ++i) { - found = false; - VEC3 pos; - Dart dCell; - while(!found && d != envMap.map.end()) { - if(!envMap.closeMark.isMarked(d) && !filled.isMarked(d)) { - pos = envMap.faceCenter(d); - if(i==nbPack/2) - std::cout << "pos " << pos << std::endl; - pos[2]=0.0f; - dCell=d; - found = true; - filled.markOrbit(FACE_ORBIT,d); - } - envMap.map.next(d); - } - - if(found) { - float ecart=3.0f; -// addAgent(pos,dCell); -// addAgent(pos+VEC3(ecart,0,0),dCell); -// addAgent(pos+VEC3(-ecart,0,0),dCell); -// addAgent(pos+VEC3(ecart,ecart,0),dCell); -// addAgent(pos+VEC3(0,ecart,0),dCell); -// addAgent(pos+VEC3(0,-ecart,0),dCell); -// addAgent(pos+VEC3(-ecart,-ecart,0),dCell); -// addAgent(pos+VEC3(ecart,-ecart,0),dCell); -// addAgent(pos+VEC3(-ecart,ecart,0),dCell); - - addAgent(pos+VEC3(ecart,ecart,0),dCell); - addAgent(pos+VEC3(-ecart,-ecart,0),dCell); - addAgent(pos+VEC3(ecart,-ecart,0),dCell); - addAgent(pos+VEC3(-ecart,ecart,0),dCell); - - unsigned int nbNewAgent =4; - - for(unsigned int j=0;j(i*nbNewAgent+j,0)); - pathToFollow.push_back(new std::vector()); - float coeff = VEC3(pos[0],pos[1],0).norm(); - float pas=16.0f; - for(float p = 1.0f; p<=2.0f*pas ; p=p+1.0f) { - if(ipush_back(coeff*VEC3(sin(M_PI*p/pas),cos(M_PI*p/pas),0)); - else - pathToFollow[i*nbNewAgent+j]->push_back(coeff*VEC3(cos(M_PI*p/pas),sin(M_PI*p/pas),0)); - } - } - } - } - - std::cout << "nb of agents : " << agents_.size() << std::endl; - -// std::random_shuffle( goals.begin(), goals.end() ); - - for (int i = 0; i < static_cast(agents_.size()); ++i) - envMap.pushAgentInCell(agents_[i],agents_[i]->part->d); - -} - - diff --git a/Apps/Examples/SocialAgents/src/viewer.cpp b/Apps/Examples/SocialAgents/src/viewer.cpp deleted file mode 100644 index e9f1e5a9..00000000 --- a/Apps/Examples/SocialAgents/src/viewer.cpp +++ /dev/null @@ -1,702 +0,0 @@ -/******************************************************************************* -* CGoGN: Combinatorial and Geometric modeling with Generic N-dimensional Maps * -* version 0.1 * -* Copyright (C) 2009, IGG Team, LSIIT, University of Strasbourg * -* * -* This library is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by the * -* Free Software Foundation; either version 2.1 of the License, or (at your * -* option) any later version. * -* * -* This library is distributed in the hope that it will be useful, but WITHOUT * -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this library; if not, write to the Free Software Foundation, * -* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * -* * -* Web site: https://iggservis.u-strasbg.fr/CGoGN/ * -* Contact information: cgogn@unistra.fr * -* * -*******************************************************************************/ - -#include "viewer.h" -#include - -/********************************************************************************************** - * AntTweakBar CALLBACKS * - **********************************************************************************************/ - -/********************************************************************************************** - * MyGlutWin IMPLEMENTATION * - **********************************************************************************************/ - -MyGlutWin::MyGlutWin(int* argc, char **argv, int winX, int winY) : - GlutWin_ATB(argc, argv, winX, winY) -{ - if(this->shaderOk) - { -// shaders[0].loadShaders("phong_vs.txt", "phong_ps.txt") ; -// shaders[1].loadShaders("flat_vs.txt", "flat_ps.txt", "flat_gs.txt") ; - } -} - - -void MyGlutWin::updateUI() -{ -} - -void MyGlutWin::init() -{ - posToReach = PFP::VEC3(0,0,0); - - - b_animate = false; - render_anim = false; - nbGenerated=0; - sim = new Simulator(); - - displayFps=false; - nextUpdate = clock() + 1 * CLOCKS_PER_SEC; - -// topo_render = new Algo::Render::VBO::topo_VBORenderMapD() ; - -// m_render = new Algo::Render::VBO::MapRender_VBO(sim->envMap.map, allDarts) ; -// m_render->initPrimitives(myMap, good, Algo::Render::VBO::TRIANGLES) ; -// m_render->updateData(Algo::Render::VBO::POSITIONS, sim->envMap.position) ; - -// Algo::Geometry::computeNormalFaces(sim->envMap.map, sim->envMap.position, sim->envMap.normal) ; -// m_render->updateData(Algo::Render::VBO::NORMALS, sim->envMap.normal) ;; - -// m_render->initPrimitives(myMap, good, Algo::Render::VBO::TRIANGLES) ; -// m_render->initPrimitives(myMap, good, Algo::Render::VBO::LINES) ; -} - -void MyGlutWin::initGUI() -{ -// viewer_bar = TwNewBar("SocialAgents") ; -// TwDefine("SocialAgents position='16 16' size='350 50' valueswidth='80'") ; -// TwAddSeparator(viewer_bar, "", "") ; - - glEnable( GL_POINT_SMOOTH ); - - visu=0; -} - -bool MyGlutWin::reachedGoal(Simulator* sim) -{ - if((int(sim->globalTime_)%500)==0) - std::random_shuffle( sim->goals.begin(), sim->goals.end() ); -// /* Check if all agents have reached their goals. */ -// for (size_t i = 0; i < sim->getNumAgents(); ++i) { -// // if (absSq(sim->getAgentPosition(i) - posToReach) > sqr(40.0f)) { -// if (absSq(sim->getAgentPosition(i) - sim->goals[sim->agents_[i]->goalNo_]) > 20.0f * 20.0f) -// return false; -// } -// -// return true; - - /* Check if all agents have reached their goals. */ -// for (size_t i = 0; i < sim->getNumAgents(); ++i) { -// if (absSq(sim->getAgentPosition(i) - posToReach) > sqr(40.0f)) { -// if (absSq(sim->getAgentPosition(i) - sim->goals[sim->agents_[i]->goalNo_]) < 10000.0f) { -// sim->agents_[i]->setGoalNo(rand()%sim->goals.size()); -// sim->agents_[i]->goalNo_ = (sim->agents_[i]->goalNo_+1)%sim->goals.size(); -// std::cout << "changing" << std::endl; -// for(std::vector::iterator it = sim->goals.begin() ; it!= sim->goals.end() ; ++it ) -// std::cout << *it << std::endl; -// std::random_shuffle(sim->goals.begin(),sim->goals.end()); -// return false; -// } -// } - - - return false; -} - -void glCircle3i(GLint x, GLint y, GLint radius) { - float angle; -// glPushMatrix(); -// glDisable(GL_TEXTURE_2D); - glBegin(GL_LINE_LOOP); - for(int i = 0; i < 5; i++) { - angle = i*2*M_PI/5; - glVertex2f(x + (cos(angle) * radius), y + (sin(angle) * radius)); - } - glEnd(); -// glEnable(GL_TEXTURE_2D); -// glPopMatrix(); -} - -void MyGlutWin::myRedraw() -{ -// GLfloat black[4]= {0.0f,0.0f,0.0f,1.0f}; -// GLfloat amb[4]= {0.2f,0.1f,0.1f,1.0f}; - - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - glPushMatrix(); - - if(b_animate) { - animate(); - if(reachedGoal(sim)) { - b_animate=false; - } - } - - glColor3f(0.5f,0.5f,0.5f); - glBegin(GL_LINES); - glVertex3f(0.,0.,0.); - glVertex3f(1.,0.,0.); - - glVertex3f(0.,0.,0.); - glVertex3f(0.,1.,0.); - - glVertex3f(0.,0.,0.); - glVertex3f(0.,0.,1.); - glEnd(); - -// SelectorMarked markDarts(sim->envMap.map,sim->envMap.closeMark); - SelectorTrue markDarts; - -// glEnable(GL_LIGHTING); - glDisable(GL_LIGHTING); - - glColor3f(1.0f,0.0f,0.0f); - -// glBegin(GL_POINTS); -// glVertex3fv(&posToReach[0]); -// for(Dart d = sim->envMap.map.begin() ; d != sim->envMap.map.end() ; sim->envMap.map.next(d)) -// glVertex3fv(&sim->envMap.getPosition(d)[0]); -// glEnd(); - -// glBegin(GL_LINES); -// for(std::vector::iterator it = sim->path.begin() ; it != sim->path.end() ; ++ it) { -// VEC3 c = Algo::Geometry::faceCentroid(sim->envMap.map,*it,sim->envMap.position); -// glVertex3fv(&c[0]); -// } -// glEnd(); - -// glBegin(GL_POINTS); -// for(unsigned int i = 0; i<4;++i) { -// for(std::vector::iterator it=(*sim->pathToFollow[0]).begin() ; it!=(*sim->pathToFollow[0]).end() ; ++it) { -// glVertex3fv(&(*it)[0]); -// } -// } -// glEnd(); - - unsigned int i=0; - for(std::vector::iterator it = sim->agents_.begin() ; it != sim->agents_.end() ; ++ it, ++i) { -// glColor3f(i%int(2.0f/3.0f*v.size())/(v.size()/3.0f),i%int(1.0f/3.0f*v.size())/(v.size()/3.0f),i/(v.size()/3.0f)); -// glCircle3i(v[i][0],v[i][1],1.5f); - glPushMatrix(); -// Geom::Plane3D pl = Algo::Geometry::trianglePlane(sim->envMap.map,sim->agents_[i]->part->d,sim->envMap.position); - VEC3 pBase(sim->agents_[i]->position_); - VEC3 posR = pBase; -// pl.normal() = -1.0f*pl.normal(); - -// VEC3 v(sim->envMap.position[sim->agents_[i]->nearestDart]-pBase); -// float scal = v*pl.normal(); -// posR = pBase + scal*pl.normal(); - -// VEC3 dir(0.0f,0.0f,-1.0f); -// pBase[2]=100000.0f; -// Geom::intersectPlaneRay >(pl,pBase,dir,posR); - -// VEC3 posR(sim->agents_[i]->position_[0],sim->agents_[i]->position_[1],0); - glTranslatef(posR[0],posR[1],posR[2]); -// glutSolidSphere(1.5f,5,5); -// if(i==0) -// glColor3f(1.0f,1.0f,0.0f); -// else - glColor3f(1.0f,0.0f,0.0f); -// glVertex3f(posR[0],posR[1],posR[2]); - glCircle3i(0,0,sim->agents_[i]->radius_); - glPopMatrix(); -// glEnd(); - - glColor3f(1.0f,1.0f,1.0f); - -// //draw all containing faces -// glColor3f(1,1,0); -// for(std::vector::iterator incF = (*it)->includingFaces.begin(); incF != (*it)->includingFaces.end(); ++incF) { -// renderFace(sim->envMap,*incF); -// } - - - if(visu==1) { - //draw containing center face - glColor3f(0,0,1); - glLineWidth(4.0f); - renderFace(sim->envMap,(*it)->part->d); - glLineWidth(1.0f); - - // //draw displacement - // glBegin(GL_LINES); - // glVertex3fv(&(*it)->position_[0]); - // glVertex3fv(&(*it)->part->prevPos[0]); - // glEnd(); - - // //and prediction triangle - glColor3f(((*it)->part->state)/3.0f,((*it)->part->state%2),0); - glLineWidth(5.0f); - renderPredictionTriangle(sim->envMap,(*it)->part->d,/*(*it)->part->m_position*/posR); - glLineWidth(1.0f); - - //draw next goal -// glBegin(GL_LINES); -// glVertex3f((*it)->part->m_position[0],(*it)->part->m_position[1],(*it)->part->m_position[2]); -// glVertex3f((*sim->pathToFollow[sim->pathPointer[i].first])[sim->pathPointer[i].second][0], -// (*sim->pathToFollow[sim->pathPointer[i].first])[sim->pathPointer[i].second][1], -// (*sim->pathToFollow[sim->pathPointer[i].first])[sim->pathPointer[i].second][2]); -// glEnd(); - } - -// glCircle3i((*it)->position_[0],(*it)->position_[1],(*it)->neighborDist_); - -// glCircle3i((*it)->position_[0],(*it)->position_[1],(*it)->timeHorizonObst_ * (*it)->maxSpeed_ + (*it)->radius_); - - for(std::vector >::iterator obst = (*it)->obstacleNeighbors_.begin(); - obst !=(*it)->obstacleNeighbors_.end(); ++obst) { - glColor3f(0,1,1); - glLineWidth(10.0f); - renderDart(sim->envMap,(*obst).second); - glLineWidth(1.0f); - } - } - -// glBegin(GL_POINTS); -// glVertex3f(11.8162,-2.77888,0); -// glEnd(); -// glBegin(GL_LINES); -// glVertex3f(4.6252,4.66903,0); -// glVertex3f(4,5,0); -// glEnd(); -// glLineWidth(10.0f); -// Dart d = sim->envMap.getDart(608); -// renderDart(sim->envMap,d); -// glLineWidth(1.0f); - -// glBegin(GL_LINES); -// glVertex3f(-7.00001,269,0); -// glVertex3f(-7.28877,268.963,0); -// glVertex3f(0,168,0); -// glVertex3f(-14,168,0); -// glEnd(); -// glBegin(GL_POINTS); -// glVertex3f(-798.772,168,0); -// glEnd(); - - - //draw the environment - glColor3f(1,1,1); -// glDisable(GL_LIGHTING); - glEnable(GL_LIGHTING); - - if(visu==0) { - SelectorUnmarked sum(sim->envMap.closeMark); - -// glPolygonMode(GL_FRONT_AND_BACK,GL_LINE); - Algo::Render::Direct::renderTriQuadPoly(sim->envMap.map,Algo::Render::Direct::FLAT,1.0,sim->envMap.position,sim->envMap.normal,sum); -// glPolygonMode(GL_FRONT_AND_BACK,GL_FILL); -// m_render->draw(Algo::Render::VBO::TRIANGLES) ; - -// glColor3f(0,0,0); -// glPolygonMode(GL_FRONT_AND_BACK,GL_LINE); -// Algo::Render::Direct::renderTriQuadPoly(sim->envMap.map,Algo::Render::Direct::LINE,1.0,sim->envMap.position,sim->envMap.normal); -// glPolygonMode(GL_FRONT_AND_BACK,GL_FILL); - } - else if(visu==1) { - glPolygonMode(GL_FRONT_AND_BACK,GL_LINE); - SelectorMarked sm(sim->envMap.closeMark); - Algo::Render::Direct::renderTriQuadPoly(sim->envMap.map,Algo::Render::Direct::LINE,1.0,sim->envMap.position,sim->envMap.normal,sm); - glPolygonMode(GL_FRONT_AND_BACK,GL_FILL); -// for(Dart d = sim->envMap.map.begin() ; d != sim->envMap.map.end() ; sim->envMap.map.next(d)) { -// if( sim->envMap.closeMark.isMarked(d)) { -// renderDart(sim->envMap,d); -// } -// } - } - else if(visu==2) { - -// renderDart(sim->envMap,sim->envMap.getDart(15332)); -// renderDart(sim->envMap,sim->envMap.getDart(15336)); -// renderDart(sim->envMap,sim->envMap.getDart(15325)); -// renderDart(sim->envMap,sim->envMap.getDart(15323)); -// renderDart(sim->envMap,sim->envMap.getDart(15329)); -// renderDart(sim->envMap,sim->envMap.getDart(15314)); -// renderDart(sim->envMap,sim->envMap.getDart(15334)); -// renderDart(sim->envMap,sim->envMap.getDart(53)); -// renderDart(sim->envMap,sim->envMap.getDart(50)); -// renderDart(sim->envMap,sim->envMap.getDart(49)); -// renderDart(sim->envMap,sim->envMap.getDart(46)); - glDisable(GL_LIGHTING); - SelectorTrue alldarts; -// topo_render = new Algo::Render::VBO::topo_MD2_VBORender(sim->envMap.map,alldarts) ; -// topo_render->updateData(sim->envMap.map,alldarts,sim->envMap.position, 0.9f, 0.9f); -// topo_render->drawTopo(); - Algo::Render::Direct::renderTopoMD2(sim->envMap.map,sim->envMap.position,true,true,0.9f,0.9f); - } - else if(visu==3) { -// renderDart(sim->envMap,sim->envMap.getDart(33701)); -// renderDart(sim->envMap,sim->envMap.getDart(2)); -// renderCellOfDim(sim->envMap,sim->envMap.getDart(1257),2); -// glBegin(GL_POINTS); -// glVertex3f(-494.518,-672,0); -// glEnd(); - } - - //count fps - if(displayFps) { - ++frames; - clock_t overtime = clock() - nextUpdate; - if (overtime > 0) { - float fps = frames / (float)(1+ (float)overtime/(float)CLOCKS_PER_SEC); - frames = 0; - nextUpdate = clock() + 1 * CLOCKS_PER_SEC; - std::cout << "fps : " << fps << std::endl; - } - } - - - glPopMatrix(); -} - -void updateVisualization(Simulator* sim) -{ - /* Output the current global time. */ - std::cout << sim->globalTime_ << std::endl; - - /* Output the current position of all the agents. */ -// for (size_t i = 0; i < sim->getNumAgents(); ++i) { -// // std::cout << sim->getAgentPosition(i) << std::endl; -// VEC3 v = sim->getAgentPosition(i); -// std::cout << "(" << v[0] << "," << v[1] << ")" << std::endl; -// } - -// std::cout << std::endl; -} - -void MyGlutWin::animate(void) -{ -// if(int(sim->globalTime_)%2) -// CGoGN::CityGenerator::animateCity(sim->envMap.map,sim->envMap.position,sim->envMap.agentvect,sim->envMap.closeMark,sim->envMap.newBuildings); -// sim->envMap.map.check(); -// posToReach[0] += sin(rand()); -// posToReach[1] += cos(rand()); -// std::cout << "pos to reach : " << posToReach << std::endl; -// std::cout << sim->getGlobalTime() << " " << std::endl; - updateVisualization(sim); - sim->setPreferredVelocities(posToReach); -// sim->setPreferredPathVelocities(); -// sim->setPreferredNextCellVelocities(); - sim->doStep(); -// sim->envMap.simplify(); - glutPostRedisplay(); - -// for(Dart d = sim->envMap.map.begin() ; d != sim->envMap.map.end() ; sim->envMap.map.next(d)) { -// if(sim->envMap.closeMark.isMarked(d) && !sim->envMap.closeMark.isMarked(sim->envMap.map.phi1(d))) { -// std::cout << "probleme" << std::endl; -// } -// } - - if(render_anim) { - std::ostringstream oss; - std::ostringstream tmpNb; - tmpNb << std::setfill('0') << std::setw(4) << nbGenerated; - nbGenerated++; - oss << "./helicoid3/exportTransZoomHelicoid" << tmpNb.str() << ".pov"; - std::string chaine = oss.str(); -// exportScenePov(sim->envMap.map,sim->envMap.position,chaine,VEC3(43,762,65),VEC3(0,762,0),VEC3(1.0f,0,0),0,0,0); - exportScenePov(sim->envMap.map,sim->envMap.position,chaine,VEC3(43,762,65+(1500.0f*float(nbGenerated)/400.0f)),VEC3(0,762,0),VEC3(1.0f,0,0),0,0,0); - - if(nbGenerated==400) { - std::cout << "enough .pov generated" << std::endl; - exit(0); - } - } -} - -bool MyGlutWin::exportScenePov(PFP::MAP& map, PFP::TVEC3& position, const std::string& filename, PFP::VEC3 cameraPos, PFP::VEC3 cameraLook, PFP::VEC3 translate, float angle_X, float angle_Y, float angle_Z,const FunctorSelect& good) -{ - static const bool highlightAgent=false; - static const bool wireDisplay=false; - - uint highlightAgentNo=0; - - std::ofstream out(filename.c_str(), std::ios::out); - if (!out.good()) { - std::cerr << "(export) Unable to open file " << filename << std::endl; - return false; - } - - float angleX = angle_X; - float angleY = angle_Y; - float angleZ = angle_Z; - - if(highlightAgent) { - cameraLook = sim->agents_[highlightAgentNo]->position_; - VEC3 goalV = (sim->goals[highlightAgentNo] - sim->agents_[highlightAgentNo]->position_); - goalV.normalize(); - cameraPos = cameraLook - goalV*50.0f; -// cameraPos[2]+= 100.0f; - float tmp = cameraPos[1]; - cameraPos[1] = cameraPos[2]+400.0f; - cameraPos[2] = tmp; - tmp = cameraLook[1]; - cameraLook[2] = cameraLook[1]; - cameraLook[1] = tmp; - } - - //define the camera position - out << "camera { location <" << cameraPos[0] << "," << cameraPos[1] << "," << cameraPos[2]+100.0f << "> look_at <" << cameraLook[0] << "," << cameraLook[1] << "," << cameraLook[2] <<"> "; -// if(highlightAgent) { -// out << "translate <" << translate[0] << "," << translate[1] << "," << translate[2] << "> " << std::endl; -// out << "rotate <" << angleX << "," << angleY << "," << angleZ << "> }" << std::endl; -// } - out << "}" << std::endl; - - //set a "infinite" plane -// out << "plane { y, -1 pigment { color rgb 1 } }" << std::endl; - //set a sky sphere - out << "sphere { <0, 0, 0>, 5000"; - out << "texture{ pigment { color rgb <1, 1, 1>} finish { ambient 1 diffuse 0 } } }" << std::endl; - //put some lights - out << "light_source { <" << cameraPos[0] << "," << cameraPos[1]+200 << "," << cameraPos[2] << "> color rgb 0.45}" << std::endl; -// out << "light_source { <-800, 800, -800> color rgb 0.25 }"<< std::endl; - //set a high quality rendering - out << "global_settings {" << std::endl; - out << "radiosity {" << std::endl; - out << "pretrace_start 0.08 pretrace_end 0.04" << std::endl; - out << "count 500 nearest_count 10 error_bound 0.15 recursion_limit 1 low_error_factor 0.2 gray_threshold 0.0 minimum_reuse 0.015 brightness 1 adc_bailout 0.01/2 normal off media off} max_trace_level 255}" << std::endl; - - std::vector v = sim->getAgentsPosition(); - for(unsigned int i = 0; i< sim->agents_.size() ; ++i) { - out << "cylinder {" << std::endl; - Geom::Plane3D pl = Algo::Geometry::trianglePlane(sim->envMap.map,sim->agents_[i]->part->d,sim->envMap.position); - VEC3 pBase(sim->agents_[i]->position_); - VEC3 posR; - pl.normal() = -1.0f*pl.normal(); - VEC3 dir(0.0f,0.0f,-1.0f); - pBase[2]=100000.0f; - Geom::intersectPlaneRay >(pl,pBase,dir,posR); -// VEC3 posR(sim->agents_[i]->position_[0],sim->agents_[i]->position_[1],0); - out << "<" << posR[0] << "," << posR[2]+0.05 << "," << posR[1] << ">, " << std::endl; - out << "<" << posR[0] << "," << posR[2]+(sim->agents_[i]->radius_*5.0f) << "," << posR[1] << ">, " << std::endl; - out << sim->agents_[i]->radius_ << std::endl; - - if(highlightAgent) { - if(i==highlightAgentNo) { - out << "texture{ pigment{ color rgb<0.5,1.0,0.5>} finish { ambient rgb 0.1 brilliance 0.5 } }" << std::endl; - } - else { - Agent * ag = sim->agents_[highlightAgentNo]; - bool found = false; - for(std::vector >::iterator it = ag->agentNeighbors_.begin();!found && it != ag->agentNeighbors_.end();++it) { - if((*it).second==i) - found=true; - } - if(found) { -// std::cout << "trouve voisin" << std::endl; - out << "texture{ pigment{ color rgb<0.5.0,0.5,0.0>} finish { ambient rgb 0.1 brilliance 0.5 } }" << std::endl; - } - else { - out << "texture{ pigment{ color rgb<0.5,0.5.0,0.5>} finish { ambient rgb 0.1 brilliance 0.5 } }" << std::endl; - } - } - } - else { - if(i<(sim->agents_.size()/2)) - out << "texture{ pigment{ color rgb<0.5,1.0,0.5>} finish { ambient rgb 0.1 brilliance 0.5 } }" << std::endl; - else - out << "texture{ pigment{ color rgb<0.5,0.5,1.0>} finish { ambient rgb 0.1 brilliance 0.5 } }" << std::endl; - - } - - out << "translate <" << translate[0] << "," << translate[1] << "," << translate[2] << ">" << std::endl; - out << "rotate <" << angleX << "," << angleY << "," << angleZ << "> " << std::endl; - out << "}" << std::endl; - } - - if(highlightAgent) { - DartMarkerStore highlight(map); - Agent * ag = sim->agents_[highlightAgentNo]; - for(Dart d=map.begin();d!=map.end();map.next(d)) { - if(!highlight.isMarked(d)) { - PFP::AGENTS listAgentInCell = sim->envMap.agentvect[d]; - if(std::find(listAgentInCell.begin(), listAgentInCell.end(), ag)!=listAgentInCell.end()) { - highlight.markOrbit(FACE_ORBIT,d); - } - } - } - - SelectorMarked sm(highlight); - SelectorUnmarked sum(highlight); - Algo::ExportPov::exportMeshPlain(out,map,position,"facesHighlighted",sm); - Algo::ExportPov::exportMeshPlain(out,map,position,"myMesh",sum); - - out << "object {facesHighlighted" << std::endl; - out << "translate <" << translate[0] << "," << translate[1] << "," << translate[2] << ">" << std::endl; - out << "rotate <" << angleX << "," << angleY << "," << angleZ << "> " << std::endl; - out << "texture{ pigment{ color rgb<0.4667,0.709,0.996>} finish { ambient rgb 0.35 brilliance 0.5 } } }" << std::endl; - } - else { - Algo::ExportPov::exportMeshPlain(out,map,position,"myMesh",good); - } - - if(wireDisplay) { - Algo::ExportPov::exportMeshWire(out,map,position,"myMeshWire",good); - out << "object {myMeshWire" << std::endl; - out << "translate <" << translate[0] << "," << translate[1] << "," << translate[2] << ">" << std::endl; - out << "rotate <" << angleX << "," << angleY << "," << angleZ << "> " << std::endl; - out << "texture{ pigment{ color rgb<0,0,0>} } }" << std::endl; - } - - out << "object {myMesh" << std::endl; - out << "translate <" << translate[0] << "," << translate[1] << "," << translate[2] << ">" << std::endl; - out << "rotate <" << angleX << "," << angleY << "," << angleZ << "> " << std::endl; - out << "texture{ pigment{ color rgbf<1.0,1.0,1,"<< 1.0f*(float(nbGenerated)/400.0f) <<">} finish { ambient rgb 0.3 brilliance 0.5 } } }" << std::endl; -// out << "texture{ pigment{ color rgb<1.0,1.0,1>} finish { ambient rgb 0.05 brilliance 0.5 } } }" << std::endl; - - out.close(); -// for(Dart d = sim->envMap.map.begin() ; d != sim->envMap.map.end() ; sim->envMap.map.next(d)) { -// if(sim->envMap.closeMark.isMarked(d) && !sim->envMap.closeMark.isMarked(sim->envMap.map.phi1(d))) { -// std::cout << "probleme" << std::endl; -// } -// } - return true; -} - -void MyGlutWin::myKeyboard(unsigned char keycode, int x, int y) -{ - switch (keycode) { - - case 'a': { - std::cout << ".." << std::endl; - sim->envMap.map.check(); - b_animate = !b_animate; - } - break; - case 'f': { - displayFps = ! displayFps; - } - break; - case 'p' : { - std::cout << "display agents positions : " << std::endl; - for(std::vector::iterator it = sim->agents_.begin() ; it != sim->agents_.end() ; ++ it) { - std::cout << (*it)->position_ << std::endl; - } - } - break; - case 'r' : { -// exportScenePov(sim->envMap.map,sim->envMap.position,"exportScene0002.pov",VEC3(1500,500,1500),VEC3(0,0,0),VEC3(1.0f,0,0),0,0,0); - exportScenePov(sim->envMap.map,sim->envMap.position,"exportScene0002.pov",VEC3(43,762,1565),VEC3(0,762,0),VEC3(1.0f,0,0),0,0,0); -// exportScenePov(sim->envMap.map,sim->envMap.position,"exportScene0002.pov",VEC3(43,762,65),VEC3(0,762,0),VEC3(1.0f,0,0),0,0,0); - } - break; - case 's': { - animate(); - } - break; - case 't' : { - for(std::vector::iterator it = sim->agents_.begin() ; it != sim->agents_.end() ; ++ it) { - std::cout << "(" << (*it)->position_[0] << "," << (*it)->position_[1] << ")" << std::endl; - } - } - break; - case 'v': { - ++visu; - visu = visu % 4; - } - break; - case '9': { - render_anim=!render_anim; - } - break; - case 'c': { - sim->envMap.map.check(); - } - break; - } - - updateUI(); - glutPostRedisplay(); -} - -float MyGlutWin::computeSelectRadius(int x, int y, int pixelRadius) -{ - GLint viewport[4]; - GLdouble modelview[16]; - GLdouble projection[16]; - GLfloat winX, winY, winZ; - GLdouble posX, posY, posZ; - - glGetDoublev( GL_MODELVIEW_MATRIX, modelview ); - glGetDoublev( GL_PROJECTION_MATRIX, projection ); - glGetIntegerv( GL_VIEWPORT, viewport ); - - // get depth - glReadPixels( x, viewport[3]-y , 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &winZ ); - - // if we have click on background, consider that we are in the middle of space - // to avoid too big radius - if (winZ == 1.0f) // depth vary in [0-1] - winZ = 0.5f; - - winX = (float)x; - winY = (float)viewport[3] - (float)y; - -// get first point in object space - gluUnProject( winX, winY, winZ, modelview, projection, viewport, &posX, &posY, &posZ); - PFP::VEC3 p(posX,posY,posZ); - -// get second point in object space - gluUnProject( winX+pixelRadius, winY, winZ, modelview, projection, viewport, &posX, &posY, &posZ); - PFP::VEC3 q(posX,posY,posZ); - - // compute & return distance - q -= p; - return float(q.norm()); -} - -void MyGlutWin::myMouse(int button, int state, int x, int y) -{ - glutPostRedisplay(); -} - -/********************************************************************************************** - * MAIN FUNCTION * - **********************************************************************************************/ - -int main(int argc, char** argv) -{ - MyGlutWin* mgw = new MyGlutWin(&argc, argv, 1200, 800) ; - - mgw->init() ; - - if(argc==2) { -// std::cout << "do some step" << std::endl; -// char c; -// do { -// mgw->animate(); -// c = std::getchar(); -// } while(c!='x'); - -// GLint t1 = glutGet(GLUT_ELAPSED_TIME); - do { - mgw->animate(); - } while (!mgw->reachedGoal(mgw->sim)); -// for(unsigned int i =0;i<300;++i) { -// mgw->animate(); -// } -// GLint t2 = glutGet(GLUT_ELAPSED_TIME); -// GLfloat seconds = (t2 - t1) / 1000.0f; -// std::cout << "animation : "<< seconds << "sec" << std::endl; - } - else { - mgw->initGUI() ; - mgw->mainLoop() ; - } - return 0 ; -} diff --git a/include/Topology/generic/embeddedMap2.hpp b/include/Topology/generic/embeddedMap2.hpp index 53b67c41..6e73edbe 100644 --- a/include/Topology/generic/embeddedMap2.hpp +++ b/include/Topology/generic/embeddedMap2.hpp @@ -186,7 +186,7 @@ bool EmbeddedMap2::edgeCanCollapse(Dart d) template void EmbeddedMap2::collapseEdge(Dart d, bool delDegenerateFaces) { - Dart dPrev2 = MAP2::phi2(MAP2::phi_1(d)) ; + Dart dPrev = MAP2::phi_1(d) ; unsigned int vEmb = EMBNULL ; if (MAP2::isOrbitEmbedded(VERTEX_ORBIT)) @@ -198,7 +198,9 @@ void EmbeddedMap2::collapseEdge(Dart d, bool delDegenerateFaces) if (MAP2::isOrbitEmbedded(VERTEX_ORBIT)) { - MAP2::embedOrbit(VERTEX_ORBIT, dPrev2, vEmb) ; + Dart dPrev2 = MAP2::phi2(dPrev) ; + if(dPrev2 != dPrev) + MAP2::embedOrbit(VERTEX_ORBIT, dPrev2, vEmb) ; } } @@ -308,8 +310,11 @@ void EmbeddedMap2::unsewFaces(Dart d) unsigned int vEmb2 = EMBNULL ; if (MAP2::isOrbitEmbedded(VERTEX_ORBIT)) { - vEmb1 = MAP2::getEmbedding(d, VERTEX_ORBIT) ; - vEmb2 = MAP2::getEmbedding(MAP2::phi1(d), VERTEX_ORBIT) ; + if(MAP2::isBoundaryVertex(d)) + vEmb1 = MAP2::getEmbedding(d, VERTEX_ORBIT) ; + Dart dd = MAP2::phi1(d) ; + if(MAP2::isBoundaryVertex(dd)) + vEmb2 = MAP2::getEmbedding(dd, VERTEX_ORBIT) ; } unsigned int eEmb = EMBNULL ; if (MAP2::isOrbitEmbedded(EDGE_ORBIT)) @@ -322,17 +327,26 @@ void EmbeddedMap2::unsewFaces(Dart d) if (MAP2::isOrbitEmbedded(VERTEX_ORBIT)) { - MAP2::setDartEmbedding(VERTEX_ORBIT, d, vEmb1) ; - MAP2::setDartEmbedding(VERTEX_ORBIT, MAP2::phi1(d), vEmb2) ; - if(e != d) + if(vEmb1 != EMBNULL) { - MAP2::embedNewCell(VERTEX_ORBIT, e) ; - MAP2::copyCell(VERTEX_ORBIT, e, d) ; - MAP2::embedNewCell(VERTEX_ORBIT, MAP2::phi1(e)) ; - MAP2::copyCell(VERTEX_ORBIT, MAP2::phi1(e), MAP2::phi1(d)) ; + MAP2::setDartEmbedding(VERTEX_ORBIT, d, vEmb1) ; + if(e != d) + { + MAP2::embedNewCell(VERTEX_ORBIT, MAP2::phi1(e)) ; + MAP2::copyCell(VERTEX_ORBIT, MAP2::phi1(e), d) ; + } } - } + if(vEmb2 != EMBNULL) + { + MAP2::setDartEmbedding(VERTEX_ORBIT, MAP2::phi1(d), vEmb2) ; + if(e != d) + { + MAP2::embedNewCell(VERTEX_ORBIT, e) ; + MAP2::copyCell(VERTEX_ORBIT, e, MAP2::phi1(d)) ; + } + } + } if (MAP2::isOrbitEmbedded(EDGE_ORBIT)) { MAP2::setDartEmbedding(EDGE_ORBIT, d, eEmb) ; diff --git a/src/Algo/ImplicitHierarchicalMesh/ihm.cpp b/src/Algo/ImplicitHierarchicalMesh/ihm.cpp index 1444ec5f..e52c0bb1 100644 --- a/src/Algo/ImplicitHierarchicalMesh/ihm.cpp +++ b/src/Algo/ImplicitHierarchicalMesh/ihm.cpp @@ -84,6 +84,46 @@ unsigned int ImplicitHierarchicalMap::faceLevel(Dart d) if(m_curLevel == 0) return 0 ; +// unsigned int cur = m_curLevel ; +// Dart it = d ; +// Dart end = d ; +// bool resetEnd = true ; +// bool firstEdge = true ; +// do +// { +// if(!resetEnd) +// firstEdge = false ; +// +// unsigned int eId = m_edgeId[it] ; +// Dart next = it ; +// do +// { +// unsigned int l = edgeLevel(next) ; +// if(l < m_curLevel) +// m_curLevel = l ; +// else // l == curLevel +// { +// if(!firstEdge) +// { +// --m_curLevel ; +// next = it ; +// } +// } +// next = phi1(next) ; +// } while(m_edgeId[next] == eId) ; +// it = next ; +// +// if(resetEnd) +// { +// end = it ; +// resetEnd = false ; +// } +// +// } while(!firstEdge && it != end) ; +// +// unsigned int fLevel = m_curLevel ; +// m_curLevel = cur ; + Dart it = d ; Dart old = it ; unsigned int fLevel = edgeLevel(it) ; @@ -210,11 +250,11 @@ bool ImplicitHierarchicalMap::faceIsSubdividedOnce(Dart d) do { ++m_curLevel ; - if(m_dartLevel[phi1(d)] == m_curLevel && m_edgeId[phi1(d)] != m_edgeId[d]) + if(m_dartLevel[phi1(fit)] == m_curLevel && m_edgeId[phi1(fit)] != m_edgeId[fit]) { subd = true ; ++m_curLevel ; - if(m_dartLevel[phi1(d)] == m_curLevel && m_edgeId[phi1(d)] != m_edgeId[d]) + if(m_dartLevel[phi1(fit)] == m_curLevel && m_edgeId[phi1(fit)] != m_edgeId[fit]) subdOnce = false ; --m_curLevel ; } -- GitLab