From ae1ce28503be10c53324067c40aab3140e2f4d50 Mon Sep 17 00:00:00 2001 From: Pierre Kraemer Date: Fri, 16 Dec 2011 15:55:19 +0100 Subject: [PATCH] ajout SPATIAL_HASHING --- CMakeLists.txt | 2 + include/agent.h | 13 ++- include/env_generator.h | 2 + include/env_generator.hpp | 29 +++++ include/env_map.h | 131 ++++++++++++++++----- include/env_render.h | 6 +- include/moving_obstacle.h | 91 ++++++++------- include/simulator.h | 6 +- include/spatialHashing.h | 139 ++++++++++++++++++++++ include/viewer.h | 2 + src/agent.cpp | 124 +++++++++++++++++--- src/env_map.cpp | 91 ++++++--------- src/moving_obstacle.cpp | 236 +++++++++++++++++++------------------- src/simulator.cpp | 223 +++++++++++++++++++++++++++++------ src/viewer.cpp | 101 +++++++++++++++- 15 files changed, 890 insertions(+), 306 deletions(-) create mode 100644 include/spatialHashing.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 07eea79..4cb74c8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 2.8) project(SocialAgents) +add_definitions(-DSPATIAL_HASHING) + SET(CGoGN_ROOT_DIR ${CMAKE_SOURCE_DIR}/../../CGoGN CACHE STRING "CGoGN root dir") include(${CGoGN_ROOT_DIR}/apps_cmake.txt) diff --git a/include/agent.h b/include/agent.h index 980ffec..306da5f 100644 --- a/include/agent.h +++ b/include/agent.h @@ -5,6 +5,7 @@ #include "utils.h" #include "env_map.h" +#include "spatialHashing.h" #include "Algo/MovingObjects/particle_cell_2D.h" class Simulator; @@ -12,7 +13,11 @@ class Simulator; class Agent { public: +#ifdef SPATIAL_HASHING + Agent(Simulator* sim, const VEC3& position) ; +#else Agent(Simulator* sim, const VEC3& position, Dart d); +#endif VEC3 getPosition(); @@ -46,8 +51,12 @@ public: std::vector > agentNeighbors_; std::vector > obstacleNeighbors_; - + +#ifdef SPATIAL_HASHING + VEC3 pos ; +#else CGoGN::Algo::MovingObjects::ParticleCell2D part_; +#endif std::vector goals_; @@ -58,10 +67,12 @@ public: static unsigned int maxNeighbors_; static float maxSpeed_; + static float neighborDist_; static float neighborDistSq_; static float radius_; static float timeHorizon_; static float timeHorizonObst_; + static float range_; static float rangeSq_; static float timeStep ; diff --git a/include/env_generator.h b/include/env_generator.h index 2f6b4e7..7493359 100644 --- a/include/env_generator.h +++ b/include/env_generator.h @@ -22,8 +22,10 @@ Algo::Modelisation::Polyhedron generateTrianGrid(typename PFP::MAP& map, ty template Dart extrudeFaceAndMark(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart d, CellMarker& buildingMark, float height); +#ifndef SPATIAL_HASHING template bool animateCity(EnvMap* envMap); +#endif //template //Dart generateBuilding(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart d, float height, unsigned int buildingType, CellMarker& obstacleMark, CellMarker& buildingMark); diff --git a/include/env_generator.hpp b/include/env_generator.hpp index 352bc73..51d6b3f 100644 --- a/include/env_generator.hpp +++ b/include/env_generator.hpp @@ -1,6 +1,7 @@ #include "Algo/Modelisation/subdivision.h" #include "Geometry/inclusion.h" #include "Topology/generic/traversorCell.h" +#include namespace CGoGN { @@ -170,6 +171,7 @@ Dart extrudeFaceAndMark(typename PFP::MAP& map, typename PFP::TVEC3& position, D return dRoof; } +#ifndef SPATIAL_HASHING template bool animateCity(EnvMap* envMap) { @@ -252,6 +254,7 @@ bool animateCity(EnvMap* envMap) return false; } +#endif //template //Dart generateBuilding(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart d, float height, unsigned int buildingType, CellMarker& obstacleMark, CellMarker& buildingMark) @@ -279,7 +282,33 @@ Dart generateBuilding(EnvMap* envMap, Dart d, float height, unsigned int buildin Dart previous = map.phi_1(dd); Obstacle* o = new Obstacle(position[dd], position[next], position[previous], position[map.phi1(next)]); +#ifdef SPATIAL_HASHING + VEC3 ov = o->p2 - o->p1 ; + unsigned int nbsteps = ov.norm() / envMap->o_cell_size_x + 1 ; + ov.normalize() ; + VEC3 step = ov * envMap->o_cell_size_x ; + VEC3 pos = o->p1 ; + for(unsigned int i = 0; i < nbsteps; ++i) + { + Geom::Vec2ui c = envMap->obstaclePositionCell(pos) ; + for(int ii = -1; ii <= 1; ++ii) + { + for(int jj = -1; jj <= 1; ++jj) + { + if(!(ii == 0 && jj == 0)) + { + Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ; + std::vector& ovect = envMap->ht_obstacles[cc] ; + if(std::find(ovect.begin(), ovect.end(), o) == ovect.end()) + ovect.push_back(o) ; + } + } + } + pos += step ; + } +#else envMap->pushObstacleInCells(o, map.phi2(dd)); +#endif dd = map.phi1(dd); } while(dd != d); diff --git a/include/env_map.h b/include/env_map.h index cd3e905..8d94e05 100644 --- a/include/env_map.h +++ b/include/env_map.h @@ -21,6 +21,8 @@ #include "Algo/Parallel/parallel_foreach.h" +#include "spatialHashing.h" + using namespace CGoGN; class Agent; @@ -55,38 +57,38 @@ public : unsigned int mapMemoryCost(); void scale(float scaleVal); +#ifndef SPATIAL_HASHING Dart getBelongingCell(const PFP::VEC3& pos); void subdivideAllToMaxLevel(); void subdivideToProperLevel(); +#endif void init(unsigned int config); - + +#ifndef SPATIAL_HASHING void foreach_neighborFace(Dart d, FunctorType& f); void registerObstaclesInFaces(); void addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor); - - void pushObstacleInCells(Obstacle* o, Dart d); - void popObstacleInCells(Obstacle* o, Dart d); - void obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace); void setAgentNeighborsAndObstacles(Agent* agent); - -// void agentChangeFaceThroughEdge(Agent* agent); - void agentChangeFace(Agent* agent, Dart oldFace); void pushAgentInCells(Agent* agent, Dart d); void popAgentInCells(Agent* agent, Dart d); - - template - inline void removeElementFromVector(std::vector& a, T ag); +// void agentChangeFaceThroughEdge(Agent* agent); + void agentChangeFace(Agent* agent, Dart oldFace); + + void pushObstacleInCells(Obstacle* o, Dart d); + void popObstacleInCells(Obstacle* o, Dart d); + void obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace); void clearUpdateCandidates(); void updateMap(); void resetAgentInFace(Agent* agent); - +#endif + PFP::MAP map; PFP::TVEC3 position; @@ -98,19 +100,22 @@ public : CellMarker obstacleMarkS; CellMarker buildingMarkS; - AttributeHandler subdivisableFace; - std::vector newBuildings; +#ifndef SPATIAL_HASHING + AttributeHandler subdivisableFace; + PFP::TAB_AGENTVECT agentvect; PFP::TAB_AGENTVECT neighborAgentvect; PFP::TAB_OBSTACLEVECT obstvect; +#endif CellMarker obstacleMark; CellMarker buildingMark; CellMarker pedWayMark; +#ifndef SPATIAL_HASHING static const unsigned int nbAgentsToSubdivide = 5; static const unsigned int nbAgentsToSimplify = 4; @@ -118,12 +123,53 @@ public : CellMarker coarsenMark; std::vector refineCandidate; std::vector coarsenCandidate; +#endif + +#ifdef SPATIAL_HASHING + PFP::VEC3 origin ; + + unsigned int a_cell_size_x, a_cell_size_y ; + float a_cell_nb_x, a_cell_nb_y ; + + unsigned int o_cell_size_x, o_cell_size_y ; + float o_cell_nb_x, o_cell_nb_y ; + + Geom::Vec2ui agentPositionCell(VEC3& pos) + { + return Geom::Vec2ui((origin[0] + pos[0]) / a_cell_size_x, (origin[1] + pos[1]) / a_cell_size_y) ; + } + + Geom::Vec2ui obstaclePositionCell(VEC3& pos) + { + return Geom::Vec2ui((origin[0] + pos[0]) / o_cell_size_x, (origin[1] + pos[1]) / o_cell_size_y) ; + } + + HashTable2D< std::vector > ht_agents ; + HashTable2D< std::vector > ht_neighbor_agents ; + HashTable2D< std::vector > ht_obstacles ; +#endif }; /************************************** * INLINE FUNCTIONS * **************************************/ +template +inline void removeElementFromVector(std::vector& a, T ag) +{ + typename std::vector::iterator end = a.template end(); + for(typename std::vector::iterator it = a.begin(); it != end; ++it) + { + if(*it == ag) + { + *it = a.back(); + a.pop_back(); + return; + } + } +} + +#ifndef SPATIAL_HASHING inline void EnvMap::pushAgentInCells(Agent* agent, Dart d) { assert(map.getCurrentLevel() == map.getMaxLevel()); @@ -164,19 +210,51 @@ inline void EnvMap::popAgentInCells(Agent* agent, Dart d) } while(dd != d); } -template -inline void EnvMap::removeElementFromVector(std::vector& a, T ag) +inline void EnvMap::pushObstacleInCells(Obstacle* o, Dart d) { - typename std::vector::iterator end = a.template end(); - for(typename std::vector::iterator it = a.begin(); it != end; ++it) - { - if(*it == ag) - { - *it = a.back(); - a.pop_back(); - return; - } - } + assert(map.getCurrentLevel() == map.getMaxLevel()); + assert(!buildingMark.isMarked(d)); + + obstvect[d].push_back(o); + +// obstvect[map.phi<12>(d)].push_back(o); +// obstvect[map.phi2(map.phi_1(d))].push_back(o); + +// Dart dd = d; +// do +// { +// Dart ddd = map.alpha1(map.alpha1(dd)); +// while(ddd != dd) +// { +// obstvect[ddd].push_back(o); +// ddd = map.alpha1(ddd); +// } +// +// dd = map.phi1(dd); +// } while(dd != d); +} + +inline void EnvMap::popObstacleInCells(Obstacle* o, Dart d) +{ + assert(map.getCurrentLevel() == map.getMaxLevel()); + assert(!buildingMark.isMarked(d)); + + removeElementFromVector(obstvect[d], o); + +// removeElementFromVector(obstvect[map.phi<12>(d)],o); +// removeElementFromVector(obstvect[map.phi2(map.phi_1(d))],o); + +// Dart dd = d; +// do +// { +// Dart ddd = map.alpha1(map.alpha1(dd)); +// while(ddd != dd) +// { +// removeElementFromVector(obstvect[ddd], o); +// ddd = map.alpha1(ddd); +// } +// dd = map.phi1(dd); +// } while(dd != d); } inline void EnvMap::clearUpdateCandidates() @@ -184,5 +262,6 @@ inline void EnvMap::clearUpdateCandidates() refineCandidate.clear(); coarsenCandidate.clear(); } +#endif #endif diff --git a/include/env_render.h b/include/env_render.h index 04d7b95..bb585db 100644 --- a/include/env_render.h +++ b/include/env_render.h @@ -46,7 +46,11 @@ static const float sinT[5] = { 0, 0.951057, 0.587785, -0.587785, -0.951057 }; inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false, bool showObstacleDist = false) { +#ifdef SPATIAL_HASHING + const VEC3& pos = agent->pos; +#else VEC3 pos = agent->part_.m_position; +#endif float radius = agent->radius_; // Geom::Plane3D pl = Algo::Geometry::facePlane(m.map,agent->part_.d,m.position); @@ -99,7 +103,7 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false, if(showNeighborDist) { - radius = sqrt(agent->neighborDistSq_); + radius = agent->neighborDist_; glColor3f(0.0f,1.0f,0.0f); glBegin(GL_LINE_LOOP); for(unsigned int i = 0; i < 5; ++i) diff --git a/include/moving_obstacle.h b/include/moving_obstacle.h index 6d7c371..bf4b852 100644 --- a/include/moving_obstacle.h +++ b/include/moving_obstacle.h @@ -1,46 +1,45 @@ -#ifndef M_MOVING_OBSTACLE_H -#define M_MOVING_OBSTACLE_H - -#include - -#include "utils.h" -#include "env_map.h" -#include "Algo/MovingObjects/particle_cell_2D.h" - - -class Simulator; - -class MovingObstacle -{ -public: - MovingObstacle(EnvMap* envmap, std::vector pos); - - void updateAgentNeighbors(); - - void computePrefVelocity(); - void computeNewVelocity(); - - void update(); - - unsigned int nbVertices; - CGoGN::Algo::MovingObjects::ParticleCell2D* * parts_; - Obstacle* * obstacles_; - - std::vector > agentNeighbors_; - - VEC3 finalGoal; - Dart finalDart; - - unsigned int curGoal_; - - static float neighborDistSq_; - static float timeStep ; - - VEC3 velocity_; - VEC3 newVelocity_; - VEC3 prefVelocity_; - - EnvMap* envMap_; -}; - -#endif +//#ifndef M_MOVING_OBSTACLE_H +//#define M_MOVING_OBSTACLE_H +// +//#include +// +//#include "utils.h" +//#include "env_map.h" +//#include "Algo/MovingObjects/particle_cell_2D.h" +// +//class Simulator; +// +//class MovingObstacle +//{ +//public: +// MovingObstacle(EnvMap* envmap, std::vector pos); +// +// void updateAgentNeighbors(); +// +// void computePrefVelocity(); +// void computeNewVelocity(); +// +// void update(); +// +// unsigned int nbVertices; +// CGoGN::Algo::MovingObjects::ParticleCell2D* * parts_; +// Obstacle* * obstacles_; +// +// std::vector > agentNeighbors_; +// +// VEC3 finalGoal; +// Dart finalDart; +// +// unsigned int curGoal_; +// +// static float neighborDistSq_; +// static float timeStep ; +// +// VEC3 velocity_; +// VEC3 newVelocity_; +// VEC3 prefVelocity_; +// +// EnvMap* envMap_; +//}; +// +//#endif diff --git a/include/simulator.h b/include/simulator.h index 093c8e5..cd95e86 100644 --- a/include/simulator.h +++ b/include/simulator.h @@ -6,7 +6,7 @@ #include "env_map.h" #include "agent.h" #include "obstacle.h" -#include "moving_obstacle.h" +//#include "moving_obstacle.h" #include "path_finder.h" #include @@ -97,9 +97,11 @@ public: void setupMovingObstacle(unsigned int nbObstacles); +#ifndef SPATIAL_HASHING void addPathToCorner(); void addPathsToAgents(); void addPathsToAgents_height(); +#endif bool importAgents(std::string filename); bool exportAgents(std::string filename); @@ -110,7 +112,7 @@ public: EnvMap envMap_; std::vector agents_; - std::vector movingObstacles_; +// std::vector movingObstacles_; float timeStep_; float globalTime_; diff --git a/include/spatialHashing.h b/include/spatialHashing.h new file mode 100644 index 0000000..91952c2 --- /dev/null +++ b/include/spatialHashing.h @@ -0,0 +1,139 @@ +#ifndef SPATIAL_HASHING_H +#define SPATIAL_HASHING_H + +#include "Geometry/vector_gen.h" +//#include + +namespace CGoGN +{ + +//class Vec2uiHash : public boost::hash +//{ +//private: +// unsigned int size ; +// +//public: +// Vec2uiHash(unsigned int s) : size(s) {} +// unsigned int operator() (const Geom::Vec2ui& k) const +// { +// return ( (k[0] * 73856093) ^ (k[1] * 19349663) ) % size ; +// } +//}; + +//class Vec2uiEqual +//{ +//public: +// bool operator() (const Geom::Vec2ui& v1, const Geom::Vec2ui& v2) const +// { +// return v1[0] == v2[0] && v1[1] == v2[1] ; +// } +//}; + +//class Vec2uiComp +//{ +//public: +// bool operator() (const Geom::Vec2ui& v1, const Geom::Vec2ui& v2) const +// { +// return v1[0] < v2[0] ; +// } +//}; + +//template +//class HashTable2D : public boost::unordered_map +//{ +//public: +// HashTable2D(unsigned int size, const Vec2uiHash& key_hasher, const Vec2uiEqual& key_equal) : +// boost::unordered_map(size, key_hasher, key_equal) +// {} +// +// bool hasData(const Geom::Vec2ui& k) const +// { +// if(this->find(k) != this->end()) +// return true ; +// return false ; +// } +//}; + +template +class HashTable2D +{ +public: + typedef std::list< std::pair > cell_type ; + + HashTable2D(unsigned int s) : m_size(s) + { + m_table = new cell_type[s] ; + } + + ~HashTable2D() + { + delete[] m_table ; + } + + unsigned int size() { return m_size ; } + + cell_type& cell(unsigned int i) { return m_table[i] ; } + + DATA& operator[] (const Geom::Vec2ui& k) + { + unsigned int c = hash(k) ; + + for(typename cell_type::iterator it = m_table[c].begin(); it != m_table[c].end(); ++it) + { + if(it->first == k) + return it->second ; + } + m_table[c].push_back(std::make_pair(k, DATA())) ; + return m_table[c].back().second ; + } + + const DATA& operator[] (const Geom::Vec2ui& k) const + { + unsigned int c = hash(k) ; + + for(typename cell_type::iterator it = m_table[c].begin(); it != m_table[c].end(); ++it) + { + if(it->first == k) + return it->second ; + } + } + + bool hasData(const Geom::Vec2ui& k) + { + unsigned int c = hash(k) ; + + for(typename cell_type::iterator it = m_table[c].begin(); it != m_table[c].end(); ++it) + { + if(it->first == k) + return true ; + } + return false ; + } + + void erase(const Geom::Vec2ui& k) + { + unsigned int c = hash(k) ; + + for(typename cell_type::iterator it = m_table[c].begin(); it != m_table[c].end(); ++it) + { + if(it->first == k) + { + m_table[c].erase(it) ; + return ; + } + } + } + +private: + unsigned int hash(const Geom::Vec2ui& k) + { + return ( (k[0] * 73856093) ^ (k[1] * 19349663) ) % m_size ; + } + + unsigned int m_size ; + cell_type* m_table ; +} ; + +} // namespace CGoGN + +#endif diff --git a/include/viewer.h b/include/viewer.h index be9b637..dd36bc4 100644 --- a/include/viewer.h +++ b/include/viewer.h @@ -57,8 +57,10 @@ public: void cb_initGL() ; void cb_redraw() ; +#ifndef SPATIAL_HASHING void exportInfoFace(std::ofstream& out, Dart d); 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()); +#endif void cb_keyPress(int keycode) ; diff --git a/src/agent.cpp b/src/agent.cpp index 317adf3..dbd9062 100644 --- a/src/agent.cpp +++ b/src/agent.cpp @@ -3,16 +3,34 @@ unsigned int Agent::maxNeighbors_ = 10; float Agent::maxSpeed_ = 2.0f; -float Agent::neighborDistSq_ = 15.0f * 15.0f; +float Agent::neighborDist_ = 15.0f; +float Agent::neighborDistSq_ = neighborDist_ * neighborDist_; float Agent::radius_ = 1.5f; float Agent::timeHorizon_ = 10.0f; float Agent::timeHorizonObst_ = 10.0f; -float Agent::rangeSq_ = (timeHorizonObst_ * maxSpeed_ + radius_) * (timeHorizonObst_ * maxSpeed_ + radius_); +float Agent::range_ = timeHorizonObst_ * maxSpeed_ + radius_; +float Agent::rangeSq_ = range_ * range_; float Agent::timeStep = 0.25f; unsigned int Agent::cptAgent = 0 ; +#ifdef SPATIAL_HASHING +Agent::Agent(Simulator* sim, const VEC3& position) : + pos(position), + curGoal_(-1), + velocity_(0), + newVelocity_(0), + prefVelocity_(0), + meanSpeed_(0), + sim_(sim) +{ + agentNeighbors_.reserve(maxNeighbors_* 2); + obstacleNeighbors_.reserve(maxNeighbors_* 2); + timeStep = sim->timeStep_; + agentNo = cptAgent++; +} +#else Agent::Agent(Simulator* sim, const VEC3& position, Dart d) : part_(sim->envMap_.map, d, position, sim->envMap_.position), curGoal_(-1), @@ -22,15 +40,20 @@ Agent::Agent(Simulator* sim, const VEC3& position, Dart d) : meanSpeed_(0), sim_(sim) { - agentNeighbors_.reserve(maxNeighbors_*2); - obstacleNeighbors_.reserve(maxNeighbors_*2); + agentNeighbors_.reserve(maxNeighbors_* 2); + obstacleNeighbors_.reserve(maxNeighbors_* 2); timeStep = sim->timeStep_; agentNo = cptAgent++; } +#endif VEC3 Agent::getPosition() { +#ifdef SPATIAL_HASHING + return pos ; +#else return part_.m_position; +#endif } bool agentSort(const std::pair& a1, const std::pair& a2) @@ -45,26 +68,40 @@ bool obstacleSort(const std::pair& o1, const std::pair void Agent::updateAgentNeighbors() { + agentNeighbors_.clear(); + +#ifdef SPATIAL_HASHING + Geom::Vec2ui c = sim_->envMap_.agentPositionCell(pos) ; + const std::vector& agents = sim_->envMap_.ht_agents[c] ; + const std::vector& neighborAgents = sim_->envMap_.ht_neighbor_agents[c] ; +#else std::vector& agents = sim_->envMap_.agentvect[part_.d]; std::vector& neighborAgents = sim_->envMap_.neighborAgentvect[part_.d]; +#endif - agentNeighbors_.clear(); float maxDist = 0.0f; - - for(std::vector::iterator it = agents.begin(); it != agents.end(); ++it) + for(std::vector::const_iterator it = agents.begin(); it != agents.end(); ++it) { +#ifdef SPATIAL_HASHING + float distSq = (pos - (*it)->pos).norm2() ; +#else float distSq = (part_.m_position - (*it)->part_.m_position).norm2(); +#endif if(*it != this && (agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist) && distSq < neighborDistSq_) { if(distSq > maxDist) - maxDist = distSq; - agentNeighbors_.push_back(std::make_pair(distSq, *it)); + maxDist = distSq ; + agentNeighbors_.push_back(std::make_pair(distSq, *it)) ; } } - for(std::vector::iterator it = neighborAgents.begin(); it != neighborAgents.end(); ++it) + for(std::vector::const_iterator it = neighborAgents.begin(); it != neighborAgents.end(); ++it) { +#ifdef SPATIAL_HASHING + float distSq = (pos - (*it)->pos).norm2(); +#else float distSq = (part_.m_position - (*it)->part_.m_position).norm2(); +#endif if((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist) && distSq < neighborDistSq_) { if(distSq > maxDist) @@ -79,21 +116,35 @@ void Agent::updateAgentNeighbors() void Agent::updateObstacleNeighbors() { - std::vector& obst = sim_->envMap_.obstvect[part_.d]; - - obstacleNeighbors_.clear(); + obstacleNeighbors_.clear() ; - for(std::vector::iterator it = obst.begin(); it != obst.end(); ++it) +#ifdef SPATIAL_HASHING + Geom::Vec2ui c = sim_->envMap_.obstaclePositionCell(pos) ; + if(sim_->envMap_.ht_obstacles.hasData(c)) + { + const std::vector& obst = sim_->envMap_.ht_obstacles[c] ; + for(std::vector::const_iterator it = obst.begin(); it != obst.end(); ++it) + { + float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, pos) ; + if(distSq < rangeSq_) + { + if(Geom::testOrientation2D(pos, (*it)->p1, (*it)->p2) == Geom::RIGHT) + obstacleNeighbors_.push_back(std::make_pair(distSq, *it)); + } + } + } +#else + std::vector& obst = sim_->envMap_.obstvect[part_.d]; + for(std::vector::const_iterator it = obst.begin(); it != obst.end(); ++it) { float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.m_position); if(distSq < rangeSq_) { - if(Geom::testOrientation2D(part_.m_position,(*it)->p1,(*it)->p2)==Geom::RIGHT) - { + if(Geom::testOrientation2D(part_.m_position, (*it)->p1, (*it)->p2) == Geom::RIGHT) obstacleNeighbors_.push_back(std::make_pair(distSq, *it)); - } } } +#endif // std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort); } @@ -102,29 +153,43 @@ void Agent::update() { velocity_[0] = newVelocity_[0]; velocity_[1] = newVelocity_[1]; +#ifdef SPATIAL_HASHING + pos = pos + (velocity_ * timeStep); +#else VEC3 target = part_.m_position + (velocity_ * timeStep); +#endif meanSpeed_ *= 3.0f; meanSpeed_ += velocity_; meanSpeed_ /= 4.0f; -// + // meanPos_ *= 9.0f; // meanPos_ += part_.m_position; // meanPos_ /= 10.0f; +#ifndef SPATIAL_HASHING part_.move(target); +#endif } void Agent::computePrefVelocity() { +#ifdef SPATIAL_HASHING + VEC3 goalVector = goals_[curGoal_] - pos; +#else VEC3 goalVector = goals_[curGoal_] - part_.m_position; +#endif float goalDist2 = goalVector.norm2(); if(goalDist2 < 35.0f) // if(goalDist2 < 5.0f) { curGoal_ = (curGoal_ + 1) % goals_.size(); +#ifdef SPATIAL_HASHING + goalVector = goals_[curGoal_] - pos; +#else goalVector = goals_[curGoal_] - part_.m_position; +#endif goalDist2 = goalVector.norm2(); } @@ -163,8 +228,13 @@ void Agent::computeNewVelocity2() it != agentNeighbors_.end() && nbA < maxNeighbors_; ++it, ++nbA) { +#ifdef SPATIAL_HASHING + VEC3 mPos = pos; + VEC3 oPos = (*it).second->pos; +#else VEC3 mPos = part_.m_position; VEC3 oPos = (*it).second->part_.m_position; +#endif centroid += oPos; VEC3 pOp(oPos - mPos); @@ -180,7 +250,11 @@ void Agent::computeNewVelocity2() if(nbA > 0) { centroid /= nbA; +#ifdef SPATIAL_HASHING + centroid -= pos; +#else centroid -= part_.m_position; +#endif centroid /= 10; vel /= nbA; @@ -206,8 +280,13 @@ void Agent::computeNewVelocity() { const Obstacle* obst = it->second; +#ifdef SPATIAL_HASHING + const VEC3 relativePosition1(obst->p1 - pos); + const VEC3 relativePosition2(obst->p2 - pos); +#else const VEC3 relativePosition1(obst->p1 - part_.m_position); const VEC3 relativePosition2(obst->p2 - part_.m_position); +#endif /* * Check if velocity obstacle of obstacle is already taken care of by @@ -384,8 +463,13 @@ void Agent::computeNewVelocity() } /* Compute cut-off centers. */ +#ifdef SPATIAL_HASHING + const VEC3 leftCutoff = invTimeHorizonObst * ((obst1EQobst2 ? obst->p2 : obst->p1) - pos); + const VEC3 rightCutoff = invTimeHorizonObst * ((obst2EQobst1 ? obst->p1 : obst->p2) - pos); +#else const VEC3 leftCutoff = invTimeHorizonObst * ((obst1EQobst2 ? obst->p2 : obst->p1) - part_.m_position); const VEC3 rightCutoff = invTimeHorizonObst * ((obst2EQobst1 ? obst->p1 : obst->p2) - part_.m_position); +#endif const VEC3 cutoffVec = rightCutoff - leftCutoff; /* Project current velocity on velocity obstacle. */ @@ -475,7 +559,11 @@ void Agent::computeNewVelocity() { Agent* other = it->second; +#ifdef SPATIAL_HASHING + const VEC3 relativePosition(other->pos - pos); +#else const VEC3 relativePosition(other->part_.m_position - part_.m_position); +#endif const VEC3 relativeVelocity(velocity_ - other->velocity_); const float distSq = relativePosition.norm2(); const float combinedRadius = radius_ + other->radius_; diff --git a/src/env_map.cpp b/src/env_map.cpp index 33684eb..c0d13e1 100644 --- a/src/env_map.cpp +++ b/src/env_map.cpp @@ -19,12 +19,31 @@ EnvMap::EnvMap() : buildingMarkS(mapScenary, FACE), obstacleMark(map, EDGE), buildingMark(map, FACE), - pedWayMark(map, FACE), + pedWayMark(map, FACE) +#ifndef SPATIAL_HASHING + , refineMark(map, FACE), coarsenMark(map, FACE) +#else + , + origin(70 * 24 / 2.0, 70 * 24 / 2.0, 0.0), + a_cell_size_x(Agent::neighborDist_), + a_cell_size_y(Agent::neighborDist_), + a_cell_nb_x(70 * 24 / a_cell_size_x), + a_cell_nb_y(70 * 24 / a_cell_size_y), + o_cell_size_x(Agent::range_), + o_cell_size_y(Agent::range_), + o_cell_nb_x(70 * 24 / o_cell_size_x), + o_cell_nb_y(70 * 24 / o_cell_size_y), + ht_agents(1024), + ht_neighbor_agents(1024), + ht_obstacles(1024) +#endif { position = map.addAttribute(VERTEX, "position"); normal = map.addAttribute(VERTEX, "normal"); + +#ifndef SPATIAL_HASHING agentvect = map.addAttribute(FACE, "agents"); neighborAgentvect = map.addAttribute(FACE, "neighborAgents"); obstvect = map.addAttribute(FACE, "obstacles"); @@ -32,6 +51,7 @@ EnvMap::EnvMap() : refineCandidate.reserve(100); coarsenCandidate.reserve(100); +#endif } void EnvMap::markPedWay() @@ -123,6 +143,7 @@ void EnvMap::scale(float scaleVal) } } +#ifndef SPATIAL_HASHING void EnvMap::subdivideAllToMaxLevel() { bool subdiv; @@ -222,6 +243,7 @@ Dart EnvMap::getBelongingCell(const PFP::VEC3& pos) std::cout << "ERROR : pos not in map" << std::endl; return map.begin(); } +#endif void EnvMap::init(unsigned int config) { @@ -234,7 +256,8 @@ void EnvMap::init(unsigned int config) break; case 1 : CityGenerator::generateCity(this, sideSize, nbSquares); break; - case 2 : CityGenerator::generateMall(map, position, obstacleMark, buildingMark, sideSize); + case 2 : CityGenerator::generateCity(this, sideSize, nbSquares); + // CityGenerator::generateMall(map, position, obstacleMark, buildingMark, sideSize); break; case 3 : { @@ -261,16 +284,18 @@ void EnvMap::init(unsigned int config) // CityGenerator::convexifyFreeSpace(map, position, obstacleMark, buildingMark); // CityGenerator::installGuardRail(map, position, obstacleMark, buildingMark, 5.0f); +#ifndef SPATIAL_HASHING map.init(); // registerObstaclesInFaces(); - // subdivideAllToMaxLevel(); for(unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i)) subdivisableFace[i].first = false ; +#endif } +#ifndef SPATIAL_HASHING void EnvMap::foreach_neighborFace(Dart d, FunctorType& f) { Dart dd = d; @@ -365,53 +390,6 @@ void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbo } while(dd != stop); } -void EnvMap::pushObstacleInCells(Obstacle* o, Dart d) -{ - assert(map.getCurrentLevel() == map.getMaxLevel()); - assert(!buildingMark.isMarked(d)); - - obstvect[d].push_back(o); - -// obstvect[map.phi<12>(d)].push_back(o); -// obstvect[map.phi2(map.phi_1(d))].push_back(o); - -// Dart dd = d; -// do -// { -// Dart ddd = map.alpha1(map.alpha1(dd)); -// while(ddd != dd) -// { -// obstvect[ddd].push_back(o); -// ddd = map.alpha1(ddd); -// } -// -// dd = map.phi1(dd); -// } while(dd != d); -} - -void EnvMap::popObstacleInCells(Obstacle* o, Dart d) -{ - assert(map.getCurrentLevel() == map.getMaxLevel()); - assert(!buildingMark.isMarked(d)); - - removeElementFromVector(obstvect[d], o); - -// removeElementFromVector(obstvect[map.phi<12>(d)],o); -// removeElementFromVector(obstvect[map.phi2(map.phi_1(d))],o); - -// Dart dd = d; -// do -// { -// Dart ddd = map.alpha1(map.alpha1(dd)); -// while(ddd != dd) -// { -// removeElementFromVector(obstvect[ddd], o); -// ddd = map.alpha1(ddd); -// } -// dd = map.phi1(dd); -// } while(dd != d); -} - //void EnvMap::agentChangeFaceThroughEdge(Agent* agent) //{ // Dart oldFace = agent->part_.lastCrossed; @@ -490,12 +468,6 @@ void EnvMap::popObstacleInCells(Obstacle* o, Dart d) // } while(dd != newFace); //} -void EnvMap::obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace) -{ - popObstacleInCells(agent, oldFace); - pushObstacleInCells(agent, newFace); -} - void EnvMap::agentChangeFace(Agent* agent, Dart oldFace) { Dart newFace = agent->part_.d; @@ -520,6 +492,12 @@ void EnvMap::agentChangeFace(Agent* agent, Dart oldFace) } } +void EnvMap::obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace) +{ + popObstacleInCells(agent, oldFace); + pushObstacleInCells(agent, newFace); +} + void EnvMap::updateMap() { assert(map.getCurrentLevel() == map.getMaxLevel()) ; @@ -798,3 +776,4 @@ void EnvMap::resetAgentInFace(Agent* agent) agent->part_.state = FACE; agent->part_.move(pos); } +#endif diff --git a/src/moving_obstacle.cpp b/src/moving_obstacle.cpp index 3b35339..6368f59 100644 --- a/src/moving_obstacle.cpp +++ b/src/moving_obstacle.cpp @@ -1,118 +1,118 @@ -#include "moving_obstacle.h" -#include "obstacle.h" -#include "agent.h" - -float MovingObstacle::neighborDistSq_ = 5.0f * 5.0f; - -float MovingObstacle::timeStep = 0.25f; - -MovingObstacle::MovingObstacle(EnvMap* envmap, std::vector pos) : -envMap_(envmap) -{ - assert(pos.size()>2); - - nbVertices = pos.size(); - - parts_ = new CGoGN::Algo::MovingObjects::ParticleCell2D*[nbVertices]; - obstacles_ = new Obstacle*[nbVertices]; - - PFP::VEC3 prev = pos[pos.size()-1]; - for(unsigned int i = 0; i < nbVertices ; ++i) - { - Obstacle* o = new Obstacle(pos[i], pos[(i+1)%nbVertices], pos[(i-1+nbVertices)%nbVertices], pos[(i+2)%nbVertices]); - obstacles_[i] = o; - - Dart d = envMap_->getBelongingCell(pos[i]); - CGoGN::Algo::MovingObjects::ParticleCell2D* part = new CGoGN::Algo::MovingObjects::ParticleCell2D(envMap_->map, d, pos[i], envMap_->position); - parts_[i] = part; - - envMap_->pushObstacleInCells(o,d); - } -} - -void MovingObstacle::updateAgentNeighbors() -{ - agentNeighbors_.clear(); - - for(unsigned int i = 0 ; i< nbVertices ; ++i) - { - std::vector& agents = envMap_->agentvect[parts_[i]->d]; - std::vector& neighborAgents = envMap_->neighborAgentvect[parts_[i]->d]; - - for(std::vector::iterator it = agents.begin(); it != agents.end(); ++it) - { - float distSq = (parts_[i]->m_position - (*it)->part_.m_position).norm2(); - if(distSq < neighborDistSq_) - { - agentNeighbors_.push_back(std::make_pair(distSq, *it)); - } - } - - for(std::vector::iterator it = neighborAgents.begin(); it != neighborAgents.end(); ++it) - { - float distSq = (parts_[i]->m_position - (*it)->part_.m_position).norm2(); - if(distSq < neighborDistSq_) - { - agentNeighbors_.push_back(std::make_pair(distSq, *it)); - } - } - } -} - -void MovingObstacle::computePrefVelocity() -{ - -} - -void MovingObstacle::computeNewVelocity() -{ - if(parts_[0]->m_position[1]<400 && newVelocity_[1]>0.0f) - newVelocity_[1] = 0.2f; - else if(parts_[0]->m_position[1]>-400) - newVelocity_[1] = -0.2f; - else - newVelocity_[1] = 0.2f; - - if(agentNeighbors_.size()>0) - newVelocity_ /= agentNeighbors_.size(); - - for(std::vector >::iterator it = agentNeighbors_.begin(); - it != agentNeighbors_.end(); - ++it) - { - Agent* other = it->second; - float relativeVelocity = newVelocity_ * other->velocity_; - if(it->first<2.0f*2.0f || relativeVelocity<-0.0001f) - { - newVelocity_[1] *= 0.00001f; - } - } -} - -void MovingObstacle::update() -{ - velocity_[0] = newVelocity_[0]; - velocity_[1] = newVelocity_[1]; - - for(unsigned int i=0; i < nbVertices ; ++i) - { - Dart oldFace = parts_[i]->d; - - VEC3 target = parts_[i]->m_position + (velocity_ * timeStep); - parts_[i]->move(target); - - if(parts_[i]->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS) - { - envMap_->obstacleChangeFace(obstacles_[i], parts_[i]->d, oldFace); - } - } - - for(unsigned int i=0; i < nbVertices ; ++i) - { - Obstacle* o = obstacles_[i]; - o->p1 = parts_[i]->m_position; - o->p2 = parts_[(i+1)%nbVertices]->m_position; - o->prevP = parts_[(i-1+nbVertices)%nbVertices]->m_position; - o->nextP = parts_[(i+2)%nbVertices]->m_position; - } -} +//#include "moving_obstacle.h" +//#include "obstacle.h" +//#include "agent.h" +// +//float MovingObstacle::neighborDistSq_ = 5.0f * 5.0f; +// +//float MovingObstacle::timeStep = 0.25f; +// +//MovingObstacle::MovingObstacle(EnvMap* envmap, std::vector pos) : +//envMap_(envmap) +//{ +// assert(pos.size()>2); +// +// nbVertices = pos.size(); +// +// parts_ = new CGoGN::Algo::MovingObjects::ParticleCell2D*[nbVertices]; +// obstacles_ = new Obstacle*[nbVertices]; +// +// PFP::VEC3 prev = pos[pos.size()-1]; +// for(unsigned int i = 0; i < nbVertices ; ++i) +// { +// Obstacle* o = new Obstacle(pos[i], pos[(i+1)%nbVertices], pos[(i-1+nbVertices)%nbVertices], pos[(i+2)%nbVertices]); +// obstacles_[i] = o; +// +// Dart d = envMap_->getBelongingCell(pos[i]); +// CGoGN::Algo::MovingObjects::ParticleCell2D* part = new CGoGN::Algo::MovingObjects::ParticleCell2D(envMap_->map, d, pos[i], envMap_->position); +// parts_[i] = part; +// +// envMap_->pushObstacleInCells(o,d); +// } +//} +// +//void MovingObstacle::updateAgentNeighbors() +//{ +// agentNeighbors_.clear(); +// +// for(unsigned int i = 0 ; i< nbVertices ; ++i) +// { +// std::vector& agents = envMap_->agentvect[parts_[i]->d]; +// std::vector& neighborAgents = envMap_->neighborAgentvect[parts_[i]->d]; +// +// for(std::vector::iterator it = agents.begin(); it != agents.end(); ++it) +// { +// float distSq = (parts_[i]->m_position - (*it)->part_.m_position).norm2(); +// if(distSq < neighborDistSq_) +// { +// agentNeighbors_.push_back(std::make_pair(distSq, *it)); +// } +// } +// +// for(std::vector::iterator it = neighborAgents.begin(); it != neighborAgents.end(); ++it) +// { +// float distSq = (parts_[i]->m_position - (*it)->part_.m_position).norm2(); +// if(distSq < neighborDistSq_) +// { +// agentNeighbors_.push_back(std::make_pair(distSq, *it)); +// } +// } +// } +//} +// +//void MovingObstacle::computePrefVelocity() +//{ +// +//} +// +//void MovingObstacle::computeNewVelocity() +//{ +// if(parts_[0]->m_position[1]<400 && newVelocity_[1]>0.0f) +// newVelocity_[1] = 0.2f; +// else if(parts_[0]->m_position[1]>-400) +// newVelocity_[1] = -0.2f; +// else +// newVelocity_[1] = 0.2f; +// +// if(agentNeighbors_.size()>0) +// newVelocity_ /= agentNeighbors_.size(); +// +// for(std::vector >::iterator it = agentNeighbors_.begin(); +// it != agentNeighbors_.end(); +// ++it) +// { +// Agent* other = it->second; +// float relativeVelocity = newVelocity_ * other->velocity_; +// if(it->first<2.0f*2.0f || relativeVelocity<-0.0001f) +// { +// newVelocity_[1] *= 0.00001f; +// } +// } +//} +// +//void MovingObstacle::update() +//{ +// velocity_[0] = newVelocity_[0]; +// velocity_[1] = newVelocity_[1]; +// +// for(unsigned int i=0; i < nbVertices ; ++i) +// { +// Dart oldFace = parts_[i]->d; +// +// VEC3 target = parts_[i]->m_position + (velocity_ * timeStep); +// parts_[i]->move(target); +// +// if(parts_[i]->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS) +// { +// envMap_->obstacleChangeFace(obstacles_[i], parts_[i]->d, oldFace); +// } +// } +// +// for(unsigned int i=0; i < nbVertices ; ++i) +// { +// Obstacle* o = obstacles_[i]; +// o->p1 = parts_[i]->m_position; +// o->p2 = parts_[(i+1)%nbVertices]->m_position; +// o->prevP = parts_[(i-1+nbVertices)%nbVertices]->m_position; +// o->nextP = parts_[(i+2)%nbVertices]->m_position; +// } +//} diff --git a/src/simulator.cpp b/src/simulator.cpp index ec96fc6..ff2eba3 100644 --- a/src/simulator.cpp +++ b/src/simulator.cpp @@ -1,11 +1,13 @@ #include "simulator.h" -Simulator::Simulator() : timeStep_(0.2f), globalTime_(0.0f), nbSteps_(0) +Simulator::Simulator() : + timeStep_(0.2f), + globalTime_(0.0f), + nbSteps_(0) { srand(10); nbStepsPerUnit_ = 1 / timeStep_; - - init(0, 2.0f); + init(2, 2.0f); } Simulator::~Simulator() @@ -26,7 +28,7 @@ void Simulator::init(unsigned int config, float dimension, bool enablePathFindin break; case 1 : importAgents("myAgents.pos"); break; - case 2 : setupScenario(700); + case 2 : setupScenario(1000); break; case 3 : setupCityScenario( -1.0f * (12 * (70.0f / 2.0f) - 10), @@ -37,6 +39,7 @@ void Simulator::init(unsigned int config, float dimension, bool enablePathFindin break; } +#ifndef SPATIAL_HASHING if(enablePathFinding) { if(dimension == 2.0f) @@ -44,6 +47,7 @@ void Simulator::init(unsigned int config, float dimension, bool enablePathFindin else if(dimension == 2.5f) addPathsToAgents_height(); } +#endif // setupMovingObstacle(1); @@ -61,15 +65,17 @@ void Simulator::init(unsigned int config, float dimension, bool enablePathFindin // tc2 = new ThreadUpdateInfo(agents_, nbAgents / 4, nbAgents / 2); // tc3 = new ThreadUpdateInfo(agents_, nbAgents / 2, nbAgents * 3 / 4); // tc4 = new ThreadUpdateInfo(agents_, nbAgents * 3 / 4, nbAgents); - +#ifndef SPATIAL_HASHING envMap_.subdivideToProperLevel(); +#endif } void Simulator::doStep() { +#ifndef SPATIAL_HASHING envMap_.clearUpdateCandidates(); - envMap_.map.setCurrentLevel(0); +#endif // for (unsigned int i = 0; i < movingObstacles_.size(); ++i) // { @@ -84,9 +90,13 @@ void Simulator::doStep() { agents_[i]->updateObstacleNeighbors(); +#ifndef SPATIAL_HASHING envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()); +#endif agents_[i]->updateAgentNeighbors(); +#ifndef SPATIAL_HASHING envMap_.map.setCurrentLevel(0); +#endif agents_[i]->computePrefVelocity(); agents_[i]->computeNewVelocity(); @@ -102,12 +112,55 @@ void Simulator::doStep() // thread3.join(); // thread4.join(); +#ifdef SPATIAL_HASHING + for (unsigned int i = 0; i < agents_.size(); ++i) + { + Geom::Vec2ui cold = envMap_.agentPositionCell(agents_[i]->pos) ; + agents_[i]->update(); + Geom::Vec2ui cnew = envMap_.agentPositionCell(agents_[i]->pos) ; + if(cnew != cold) + { + std::vector& old_agents = envMap_.ht_agents[cold] ; + removeElementFromVector(old_agents, agents_[i]) ; + if(old_agents.empty()) + envMap_.ht_agents.erase(cold) ; + + for(int ii = -1; ii <= 1; ++ii) + { + for(int jj = -1; jj <= 1; ++jj) + { + if(!(ii == 0 && jj == 0)) + { + Geom::Vec2ui cc = cold + Geom::Vec2ui(ii, jj) ; + std::vector& v = envMap_.ht_neighbor_agents[cc] ; + removeElementFromVector(v, agents_[i]) ; + if(v.empty()) + envMap_.ht_neighbor_agents.erase(cc) ; + } + } + } + + envMap_.ht_agents[cnew].push_back(agents_[i]) ; + + for(int ii = -1; ii <= 1; ++ii) + { + for(int jj = -1; jj <= 1; ++jj) + { + if(!(ii == 0 && jj == 0)) + { + Geom::Vec2ui cc = cnew + Geom::Vec2ui(ii, jj) ; + envMap_.ht_neighbor_agents[cc].push_back(agents_[i]) ; + } + } + } + } + } +#else envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()); for (unsigned int i = 0; i < agents_.size(); ++i) { Dart oldFace = agents_[i]->part_.d; - agents_[i]->update(); // if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE)) @@ -125,6 +178,7 @@ void Simulator::doStep() } envMap_.updateMap(); +#endif globalTime_ += timeStep_; ++nbSteps_; @@ -153,27 +207,67 @@ void Simulator::setupCircleScenario(unsigned int nbMaxAgent) float r = 800.0f; for (unsigned int i = 0; i < nbMaxAgent; ++i) { - VEC3 posagent(std::cos(i * 2.0f * pi / float(nbMaxAgent))*r,std::sin(i * 2.0f * pi / float(nbMaxAgent))*r,0); + VEC3 posagent(std::cos(i * 2.0f * pi / float(nbMaxAgent)) * r, std::sin(i * 2.0f * pi / float(nbMaxAgent)) * r, 0); + +#ifdef SPATIAL_HASHING + Geom::Vec2ui c = envMap_.agentPositionCell(posagent) ; + Agent* a = new Agent(this, posagent) ; + agents_.push_back(a) ; + envMap_.ht_agents[c].push_back(a) ; + for(int ii = -1; ii <= 1; ++ii) + { + for(int jj = -1; jj <= 1; ++jj) + { + if(!(ii == 0 && jj == 0)) + { + Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ; + envMap_.ht_neighbor_agents[cc].push_back(a) ; + } + } + } +#else Dart dCell = envMap_.getBelongingCell(posagent); agents_.push_back(new Agent(this, posagent, dCell)); +#endif + agents_.back()->goals_.push_back(-1.0f * posagent); agents_.back()->curGoal_ = 0; } std::cout << "nb agents : " << agents_.size() << std::endl; +#ifndef SPATIAL_HASHING for(unsigned int i = 0; i < agents_.size(); ++i) envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d); +#endif } -void Simulator::setupCityScenario(float startX, float startY, int nbLines , int nbRank) +void Simulator::setupCityScenario(float startX, float startY, int nbLines, int nbRank) { float xCornerDown = startX; float yCornerDown = startY; - VEC3 posagent(xCornerDown-140, yCornerDown-140, 0.0f); + VEC3 posagent(xCornerDown - 140, yCornerDown - 140, 0.0f); +#ifdef SPATIAL_HASHING + Geom::Vec2ui c = envMap_.agentPositionCell(posagent) ; + Agent* a = new Agent(this, posagent) ; + agents_.push_back(a) ; + envMap_.ht_agents[c].push_back(a) ; + for(int ii = -1; ii <= 1; ++ii) + { + for(int jj = -1; jj <= 1; ++jj) + { + if(!(ii == 0 && jj == 0)) + { + Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ; + envMap_.ht_neighbor_agents[cc].push_back(a) ; + } + } + } +#else Dart dCell = envMap_.getBelongingCell(posagent); agents_.push_back(new Agent(this, posagent, dCell)); +#endif agents_.back()->goals_.push_back(-1.0f * posagent); agents_.back()->curGoal_ = 0; @@ -182,8 +276,26 @@ void Simulator::setupCityScenario(float startX, float startY, int nbLines , int for (int j = 0; j < nbRank; ++j) { VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f); +#ifdef SPATIAL_HASHING + Geom::Vec2ui c = envMap_.agentPositionCell(posagent) ; + Agent* a = new Agent(this, posagent) ; + agents_.push_back(a) ; + envMap_.ht_agents[c].push_back(a) ; + for(int ii = -1; ii <= 1; ++ii) + { + for(int jj = -1; jj <= 1; ++jj) + { + if(!(ii == 0 && jj == 0)) + { + Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ; + envMap_.ht_neighbor_agents[cc].push_back(a) ; + } + } + } +#else Dart dCell = envMap_.getBelongingCell(posagent); agents_.push_back(new Agent(this, posagent, dCell)); +#endif agents_.back()->goals_.push_back(-1.0f * posagent); agents_.back()->curGoal_ = 0; } @@ -191,8 +303,10 @@ void Simulator::setupCityScenario(float startX, float startY, int nbLines , int std::cout << "nb agents : " << agents_.size() << std::endl; +#ifndef SPATIAL_HASHING for(unsigned int i = 0; i < agents_.size(); ++i) envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d); +#endif } void Simulator::setupScenario(unsigned int nbMaxAgent) @@ -257,9 +371,27 @@ void Simulator::setupScenario(unsigned int nbMaxAgent) for(unsigned int cury = 0; cury < nby; ++cury) { VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f); +#ifdef SPATIAL_HASHING + Geom::Vec2ui c = envMap_.agentPositionCell(posagent) ; + Agent* a = new Agent(this, posagent) ; + agents_.push_back(a) ; + envMap_.ht_agents[c].push_back(a) ; + for(int ii = -1; ii <= 1; ++ii) + { + for(int jj = -1; jj <= 1; ++jj) + { + if(!(ii == 0 && jj == 0)) + { + Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ; + envMap_.ht_neighbor_agents[cc].push_back(a) ; + } + } + } +#else agents_.push_back(new Agent(this, posagent, dCell)); +#endif agents_.back()->goals_.push_back(posagent); -// agents_.back()->goals_.push_back(VEC3(0,0,0)); +// agents_.back()->goals_.push_back(VEC3(0,0,0)); agents_.back()->goals_.push_back(-1.0f * posagent); agents_.back()->curGoal_ = 1; } @@ -270,10 +402,13 @@ void Simulator::setupScenario(unsigned int nbMaxAgent) swapAgentsGoals(); +#ifndef SPATIAL_HASHING for(unsigned int i = 0; i < agents_.size(); ++i) envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d); +#endif } +#ifndef SPATIAL_HASHING void Simulator::addPathToCorner() { for(unsigned int i = 0; i < agents_.size(); ++i) @@ -418,6 +553,7 @@ void Simulator::addPathsToAgents_height() } } } +#endif bool Simulator::importAgents(std::string filename) { @@ -444,9 +580,26 @@ bool Simulator::importAgents(std::string filename) iss >> z; VEC3 pos(x, y, z); +#ifdef SPATIAL_HASHING + Geom::Vec2ui c = envMap_.agentPositionCell(pos) ; + Agent* a = new Agent(this, pos) ; + agents_.push_back(a) ; + envMap_.ht_agents[c].push_back(a) ; + for(int ii = -1; ii <= 1; ++ii) + { + for(int jj = -1; jj <= 1; ++jj) + { + if(!(ii == 0 && jj == 0)) + { + Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ; + envMap_.ht_neighbor_agents[cc].push_back(a) ; + } + } + } +#else Dart dCell = envMap_.getBelongingCell(pos); - agents_.push_back(new Agent(this, pos, dCell)); +#endif agents_.back()->goals_.push_back(pos); agents_.back()->goals_.push_back(-1.0f * pos); agents_.back()->curGoal_ = 1; @@ -455,8 +608,10 @@ bool Simulator::importAgents(std::string filename) swapAgentsGoals(); +#ifndef SPATIAL_HASHING for (unsigned int i = 0; i < agents_.size(); ++i) envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d); +#endif myfile.close(); return true; @@ -472,7 +627,11 @@ bool Simulator::exportAgents(std::string filename) } for (unsigned int i = 0; i < agents_.size(); ++i) +#ifdef SPATIAL_HASHING + out << agents_[i]->pos << std::endl; +#else out << agents_[i]->part_.m_position << std::endl; +#endif out.close(); return true; @@ -491,7 +650,7 @@ void Simulator::swapAgentsGoals() // agents_[r]->goals_[nbg] = g; // } - std::swap(agents_[i]->goals_,agents_[r]->goals_); + std::swap(agents_[i]->goals_, agents_[r]->goals_); } } @@ -507,22 +666,22 @@ Geom::BoundingBox Simulator::getAgentsBB() return bb; } -void Simulator::setupMovingObstacle( unsigned int nbMovingObstacles) -{ - float xSize = 10.0f; - float ySize = 20.0f; - - VEC3 posInit(30,-420,0); - - std::vector vPos; - vPos.push_back(posInit); - posInit[0] += xSize; - vPos.push_back(posInit); - posInit[1] += ySize; - vPos.push_back(posInit); - posInit[0] -= xSize; - vPos.push_back(posInit); - - MovingObstacle* mo = new MovingObstacle(&envMap_, vPos); - movingObstacles_.push_back(mo); -} +//void Simulator::setupMovingObstacle( unsigned int nbMovingObstacles) +//{ +// float xSize = 10.0f; +// float ySize = 20.0f; +// +// VEC3 posInit(30,-420,0); +// +// std::vector vPos; +// vPos.push_back(posInit); +// posInit[0] += xSize; +// vPos.push_back(posInit); +// posInit[1] += ySize; +// vPos.push_back(posInit); +// posInit[0] -= xSize; +// vPos.push_back(posInit); +// +// MovingObstacle* mo = new MovingObstacle(&envMap_, vPos); +// movingObstacles_.push_back(mo); +//} diff --git a/src/viewer.cpp b/src/viewer.cpp index 9ac9ba7..1eb2c25 100644 --- a/src/viewer.cpp +++ b/src/viewer.cpp @@ -100,6 +100,27 @@ void SocialAgents::cb_redraw() glVertex3f(0.0f, 0.0f, 1.0f); glEnd(); +//#ifdef SPATIAL_HASHING +// glPointSize(1.0f) ; +// glBegin(GL_POINTS) ; +// VEC3 p = sim.envMap_.origin ; +// p *= -1.0f ; +// p[1] -= 15.0f ; +// for(unsigned int i = 0; i < sim.envMap_.ht_agents.size(); ++i) +// { +// HashTable2D >::cell_type& c = sim.envMap_.ht_agents.cell(i) ; +// unsigned int nbelem = c.size() ; +// for(unsigned int k = 0; k < nbelem; ++k) +// { +// VEC3 pp = p ; +// pp[0] += ( float(i) / float(sim.envMap_.ht_agents.size()) ) * 70 * 24 ; +// pp[1] += k ; +// glVertex3fv(pp.data()) ; +// } +// } +// glEnd() ; +//#endif + if(drawAgents) { for(std::vector::iterator it = sim.agents_.begin(); it != sim.agents_.end(); ++ it) @@ -172,6 +193,7 @@ void SocialAgents::cb_redraw() // } } +#ifndef SPATIAL_HASHING if(drawAgentsPredictionTri) { for(std::vector::iterator it = sim.agents_.begin(); it != sim.agents_.end(); ++ it) @@ -181,6 +203,7 @@ void SocialAgents::cb_redraw() renderPredictionTriangle(sim.envMap_, (*it)->part_.d, (*it)->getPosition()); } } +#endif //draw the environment if(drawEnvLines) @@ -188,9 +211,39 @@ void SocialAgents::cb_redraw() glDisable(GL_LIGHTING); glColor3f(1.0f, 1.0f, 1.0f); glLineWidth(1.0f); + +#ifdef SPATIAL_HASHING + glBegin(GL_LINES) ; + for(unsigned int i = 0; i < sim.envMap_.a_cell_nb_x; ++i) + { + for(unsigned int j = 0; j < sim.envMap_.a_cell_nb_y; ++j) + { + Geom::Vec2ui c(i, j) ; + if(sim.envMap_.ht_agents.hasData(c)) + { + VEC3 v(i * sim.envMap_.a_cell_size_x, j * sim.envMap_.a_cell_size_y, 0.0f) ; + v -= sim.envMap_.origin ; + + glVertex3f(v[0], v[1], 0.0f) ; + glVertex3f(v[0] + sim.envMap_.a_cell_size_x, v[1], 0.0f) ; + + glVertex3f(v[0] + sim.envMap_.a_cell_size_x, v[1], 0.0f) ; + glVertex3f(v[0] + sim.envMap_.a_cell_size_x, v[1] + sim.envMap_.a_cell_size_y, 0.0f) ; + + glVertex3f(v[0] + sim.envMap_.a_cell_size_x, v[1] + sim.envMap_.a_cell_size_y, 0.0f) ; + glVertex3f(v[0], v[1] + sim.envMap_.a_cell_size_y, 0.0f) ; + + glVertex3f(v[0], v[1] + sim.envMap_.a_cell_size_y, 0.0f) ; + glVertex3f(v[0], v[1], 0.0f) ; + } + } + } + glEnd() ; +#else glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); Algo::Render::GL1::renderTriQuadPoly(sim.envMap_.map, Algo::Render::GL1::LINE, 1.0, sim.envMap_.position, sim.envMap_.normal); // Algo::Render::GL1::renderTriQuadPoly(sim.envMap_.mapScenary, Algo::Render::GL1::LINE, 1.0, sim.envMap_.positionScenary, sim.envMap_.normalScenary); +#endif } if(drawEnvFaces) @@ -226,16 +279,46 @@ void SocialAgents::cb_redraw() } } - glColor3f(1.0f, 0.0f, 1.0f); - for(std::vector::iterator it = sim.movingObstacles_.begin() ; it != sim.movingObstacles_.end() ; ++it) +#ifdef SPATIAL_HASHING + glLineWidth(1.0f); + glBegin(GL_LINES) ; + for(unsigned int i = 0; i < sim.envMap_.o_cell_nb_x; ++i) { - glBegin(GL_LINE_LOOP); - for(unsigned int i = 0; i < ((*it)->nbVertices); ++i) + for(unsigned int j = 0; j < sim.envMap_.o_cell_nb_y; ++j) { - glVertex3fv((*it)->obstacles_[i]->p1.data()); + Geom::Vec2ui c(i, j) ; + if(sim.envMap_.ht_obstacles.hasData(c)) + { + VEC3 v(i * sim.envMap_.o_cell_size_x, j * sim.envMap_.o_cell_size_y, 0.0f) ; + v -= sim.envMap_.origin ; + + glVertex3f(v[0], v[1], 0.0f) ; + glVertex3f(v[0] + sim.envMap_.o_cell_size_x, v[1], 0.0f) ; + + glVertex3f(v[0] + sim.envMap_.o_cell_size_x, v[1], 0.0f) ; + glVertex3f(v[0] + sim.envMap_.o_cell_size_x, v[1] + sim.envMap_.o_cell_size_y, 0.0f) ; + + glVertex3f(v[0] + sim.envMap_.o_cell_size_x, v[1] + sim.envMap_.o_cell_size_y, 0.0f) ; + glVertex3f(v[0], v[1] + sim.envMap_.o_cell_size_y, 0.0f) ; + + glVertex3f(v[0], v[1] + sim.envMap_.o_cell_size_y, 0.0f) ; + glVertex3f(v[0], v[1], 0.0f) ; + } } - glEnd(); } + glEnd() ; +#endif + +// glColor3f(1.0f, 0.0f, 1.0f); +// for(std::vector::iterator it = sim.movingObstacles_.begin() ; it != sim.movingObstacles_.end() ; ++it) +// { +// glBegin(GL_LINE_LOOP); +// for(unsigned int i = 0; i < ((*it)->nbVertices); ++i) +// { +// glVertex3fv((*it)->obstacles_[i]->p1.data()); +// } +// glEnd(); +// } } // renderDart(sim.envMap_,264); @@ -383,6 +466,7 @@ void SocialAgents::animate() // timer->stop(); // } +#ifndef SPATIAL_HASHING if(render_anim) { std::ostringstream oss; @@ -448,10 +532,12 @@ void SocialAgents::animate() exit(0); } } +#endif updateGL(); } +#ifndef SPATIAL_HASHING void SocialAgents::exportInfoFace(std::ofstream& out, Dart d) { //get corner @@ -886,6 +972,7 @@ bool SocialAgents::exportScenePov(PFP::MAP& map, PFP::TVEC3& position, const std return true; } +#endif void SocialAgents::cb_keyPress(int keycode) { @@ -924,6 +1011,7 @@ void SocialAgents::cb_keyPress(int keycode) std::cout << sim.globalTime_ << std::endl; break; } +#ifndef SPATIAL_HASHING case 'r' : { exportScenePov(sim.envMap_.map,sim.envMap_.position,"planSerre/exportSceneOle0000.pov",VEC3(10,3,10),VEC3(0,0,0),VEC3(1.0f,0,0),0,0,0); @@ -931,6 +1019,7 @@ void SocialAgents::cb_keyPress(int keycode) // 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; } +#endif case 's': { animate(); -- GitLab