Commit ae1ce285 authored by Pierre Kraemer's avatar Pierre Kraemer

ajout SPATIAL_HASHING

parent af4b0a5f
...@@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 2.8) ...@@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 2.8)
project(SocialAgents) project(SocialAgents)
add_definitions(-DSPATIAL_HASHING)
SET(CGoGN_ROOT_DIR ${CMAKE_SOURCE_DIR}/../../CGoGN CACHE STRING "CGoGN root dir") SET(CGoGN_ROOT_DIR ${CMAKE_SOURCE_DIR}/../../CGoGN CACHE STRING "CGoGN root dir")
include(${CGoGN_ROOT_DIR}/apps_cmake.txt) include(${CGoGN_ROOT_DIR}/apps_cmake.txt)
......
...@@ -5,6 +5,7 @@ ...@@ -5,6 +5,7 @@
#include "utils.h" #include "utils.h"
#include "env_map.h" #include "env_map.h"
#include "spatialHashing.h"
#include "Algo/MovingObjects/particle_cell_2D.h" #include "Algo/MovingObjects/particle_cell_2D.h"
class Simulator; class Simulator;
...@@ -12,7 +13,11 @@ class Simulator; ...@@ -12,7 +13,11 @@ class Simulator;
class Agent class Agent
{ {
public: public:
#ifdef SPATIAL_HASHING
Agent(Simulator* sim, const VEC3& position) ;
#else
Agent(Simulator* sim, const VEC3& position, Dart d); Agent(Simulator* sim, const VEC3& position, Dart d);
#endif
VEC3 getPosition(); VEC3 getPosition();
...@@ -46,8 +51,12 @@ public: ...@@ -46,8 +51,12 @@ public:
std::vector<std::pair<float, Agent*> > agentNeighbors_; std::vector<std::pair<float, Agent*> > agentNeighbors_;
std::vector<std::pair<float, Obstacle*> > obstacleNeighbors_; std::vector<std::pair<float, Obstacle*> > obstacleNeighbors_;
#ifdef SPATIAL_HASHING
VEC3 pos ;
#else
CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> part_; CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> part_;
#endif
std::vector<VEC3> goals_; std::vector<VEC3> goals_;
...@@ -58,10 +67,12 @@ public: ...@@ -58,10 +67,12 @@ public:
static unsigned int maxNeighbors_; static unsigned int maxNeighbors_;
static float maxSpeed_; static float maxSpeed_;
static float neighborDist_;
static float neighborDistSq_; static float neighborDistSq_;
static float radius_; static float radius_;
static float timeHorizon_; static float timeHorizon_;
static float timeHorizonObst_; static float timeHorizonObst_;
static float range_;
static float rangeSq_; static float rangeSq_;
static float timeStep ; static float timeStep ;
......
...@@ -22,8 +22,10 @@ Algo::Modelisation::Polyhedron<PFP> generateTrianGrid(typename PFP::MAP& map, ty ...@@ -22,8 +22,10 @@ Algo::Modelisation::Polyhedron<PFP> generateTrianGrid(typename PFP::MAP& map, ty
template <typename PFP> template <typename PFP>
Dart extrudeFaceAndMark(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart d, CellMarker& buildingMark, float height); Dart extrudeFaceAndMark(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart d, CellMarker& buildingMark, float height);
#ifndef SPATIAL_HASHING
template <typename PFP> template <typename PFP>
bool animateCity(EnvMap* envMap); bool animateCity(EnvMap* envMap);
#endif
//template <typename PFP> //template <typename PFP>
//Dart generateBuilding(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart d, float height, unsigned int buildingType, CellMarker& obstacleMark, CellMarker& buildingMark); //Dart generateBuilding(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart d, float height, unsigned int buildingType, CellMarker& obstacleMark, CellMarker& buildingMark);
......
#include "Algo/Modelisation/subdivision.h" #include "Algo/Modelisation/subdivision.h"
#include "Geometry/inclusion.h" #include "Geometry/inclusion.h"
#include "Topology/generic/traversorCell.h" #include "Topology/generic/traversorCell.h"
#include <algorithm>
namespace CGoGN namespace CGoGN
{ {
...@@ -170,6 +171,7 @@ Dart extrudeFaceAndMark(typename PFP::MAP& map, typename PFP::TVEC3& position, D ...@@ -170,6 +171,7 @@ Dart extrudeFaceAndMark(typename PFP::MAP& map, typename PFP::TVEC3& position, D
return dRoof; return dRoof;
} }
#ifndef SPATIAL_HASHING
template <typename PFP> template <typename PFP>
bool animateCity(EnvMap* envMap) bool animateCity(EnvMap* envMap)
{ {
...@@ -252,6 +254,7 @@ bool animateCity(EnvMap* envMap) ...@@ -252,6 +254,7 @@ bool animateCity(EnvMap* envMap)
return false; return false;
} }
#endif
//template <typename PFP> //template <typename PFP>
//Dart generateBuilding(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart d, float height, unsigned int buildingType, CellMarker& obstacleMark, CellMarker& buildingMark) //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 ...@@ -279,7 +282,33 @@ Dart generateBuilding(EnvMap* envMap, Dart d, float height, unsigned int buildin
Dart previous = map.phi_1(dd); Dart previous = map.phi_1(dd);
Obstacle* o = new Obstacle(position[dd], position[next], position[previous], position[map.phi1(next)]); 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<Obstacle*>& 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)); envMap->pushObstacleInCells(o, map.phi2(dd));
#endif
dd = map.phi1(dd); dd = map.phi1(dd);
} while(dd != d); } while(dd != d);
......
...@@ -21,6 +21,8 @@ ...@@ -21,6 +21,8 @@
#include "Algo/Parallel/parallel_foreach.h" #include "Algo/Parallel/parallel_foreach.h"
#include "spatialHashing.h"
using namespace CGoGN; using namespace CGoGN;
class Agent; class Agent;
...@@ -55,38 +57,38 @@ public : ...@@ -55,38 +57,38 @@ public :
unsigned int mapMemoryCost(); unsigned int mapMemoryCost();
void scale(float scaleVal); void scale(float scaleVal);
#ifndef SPATIAL_HASHING
Dart getBelongingCell(const PFP::VEC3& pos); Dart getBelongingCell(const PFP::VEC3& pos);
void subdivideAllToMaxLevel(); void subdivideAllToMaxLevel();
void subdivideToProperLevel(); void subdivideToProperLevel();
#endif
void init(unsigned int config); void init(unsigned int config);
#ifndef SPATIAL_HASHING
void foreach_neighborFace(Dart d, FunctorType& f); void foreach_neighborFace(Dart d, FunctorType& f);
void registerObstaclesInFaces(); void registerObstaclesInFaces();
void addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor); 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 setAgentNeighborsAndObstacles(Agent* agent);
// void agentChangeFaceThroughEdge(Agent* agent);
void agentChangeFace(Agent* agent, Dart oldFace);
void pushAgentInCells(Agent* agent, Dart d); void pushAgentInCells(Agent* agent, Dart d);
void popAgentInCells(Agent* agent, Dart d); void popAgentInCells(Agent* agent, Dart d);
// void agentChangeFaceThroughEdge(Agent* agent);
template <typename T> void agentChangeFace(Agent* agent, Dart oldFace);
inline void removeElementFromVector(std::vector<T>& a, T ag);
void pushObstacleInCells(Obstacle* o, Dart d);
void popObstacleInCells(Obstacle* o, Dart d);
void obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace);
void clearUpdateCandidates(); void clearUpdateCandidates();
void updateMap(); void updateMap();
void resetAgentInFace(Agent* agent); void resetAgentInFace(Agent* agent);
#endif
PFP::MAP map; PFP::MAP map;
PFP::TVEC3 position; PFP::TVEC3 position;
...@@ -98,19 +100,22 @@ public : ...@@ -98,19 +100,22 @@ public :
CellMarker obstacleMarkS; CellMarker obstacleMarkS;
CellMarker buildingMarkS; CellMarker buildingMarkS;
AttributeHandler<PFP::BOOLATTRIB> subdivisableFace;
std::vector<Dart> newBuildings; std::vector<Dart> newBuildings;
#ifndef SPATIAL_HASHING
AttributeHandler<PFP::BOOLATTRIB> subdivisableFace;
PFP::TAB_AGENTVECT agentvect; PFP::TAB_AGENTVECT agentvect;
PFP::TAB_AGENTVECT neighborAgentvect; PFP::TAB_AGENTVECT neighborAgentvect;
PFP::TAB_OBSTACLEVECT obstvect; PFP::TAB_OBSTACLEVECT obstvect;
#endif
CellMarker obstacleMark; CellMarker obstacleMark;
CellMarker buildingMark; CellMarker buildingMark;
CellMarker pedWayMark; CellMarker pedWayMark;
#ifndef SPATIAL_HASHING
static const unsigned int nbAgentsToSubdivide = 5; static const unsigned int nbAgentsToSubdivide = 5;
static const unsigned int nbAgentsToSimplify = 4; static const unsigned int nbAgentsToSimplify = 4;
...@@ -118,12 +123,53 @@ public : ...@@ -118,12 +123,53 @@ public :
CellMarker coarsenMark; CellMarker coarsenMark;
std::vector<Dart> refineCandidate; std::vector<Dart> refineCandidate;
std::vector<Dart> coarsenCandidate; std::vector<Dart> 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<Agent*> > ht_agents ;
HashTable2D< std::vector<Agent*> > ht_neighbor_agents ;
HashTable2D< std::vector<Obstacle*> > ht_obstacles ;
#endif
}; };
/************************************** /**************************************
* INLINE FUNCTIONS * * INLINE FUNCTIONS *
**************************************/ **************************************/
template <typename T>
inline void removeElementFromVector(std::vector<T>& a, T ag)
{
typename std::vector<T>::iterator end = a.template end();
for(typename std::vector<T>::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) inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
{ {
assert(map.getCurrentLevel() == map.getMaxLevel()); assert(map.getCurrentLevel() == map.getMaxLevel());
...@@ -164,19 +210,51 @@ inline void EnvMap::popAgentInCells(Agent* agent, Dart d) ...@@ -164,19 +210,51 @@ inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
} while(dd != d); } while(dd != d);
} }
template <typename T> inline void EnvMap::pushObstacleInCells(Obstacle* o, Dart d)
inline void EnvMap::removeElementFromVector(std::vector<T>& a, T ag)
{ {
typename std::vector<T>::iterator end = a.template end(); assert(map.getCurrentLevel() == map.getMaxLevel());
for(typename std::vector<T>::iterator it = a.begin(); it != end; ++it) assert(!buildingMark.isMarked(d));
{
if(*it == ag) obstvect[d].push_back(o);
{
*it = a.back(); // obstvect[map.phi<12>(d)].push_back(o);
a.pop_back(); // obstvect[map.phi2(map.phi_1(d))].push_back(o);
return;
} // 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<Obstacle* >(obstvect[d], o);
// removeElementFromVector<Obstacle* >(obstvect[map.phi<12>(d)],o);
// removeElementFromVector<Obstacle* >(obstvect[map.phi2(map.phi_1(d))],o);
// Dart dd = d;
// do
// {
// Dart ddd = map.alpha1(map.alpha1(dd));
// while(ddd != dd)
// {
// removeElementFromVector<Obstacle* >(obstvect[ddd], o);
// ddd = map.alpha1(ddd);
// }
// dd = map.phi1(dd);
// } while(dd != d);
} }
inline void EnvMap::clearUpdateCandidates() inline void EnvMap::clearUpdateCandidates()
...@@ -184,5 +262,6 @@ inline void EnvMap::clearUpdateCandidates() ...@@ -184,5 +262,6 @@ inline void EnvMap::clearUpdateCandidates()
refineCandidate.clear(); refineCandidate.clear();
coarsenCandidate.clear(); coarsenCandidate.clear();
} }
#endif
#endif #endif
...@@ -46,7 +46,11 @@ static const float sinT[5] = { 0, 0.951057, 0.587785, -0.587785, -0.951057 }; ...@@ -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) 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; VEC3 pos = agent->part_.m_position;
#endif
float radius = agent->radius_; float radius = agent->radius_;
// Geom::Plane3D<float> pl = Algo::Geometry::facePlane<PFP>(m.map,agent->part_.d,m.position); // Geom::Plane3D<float> pl = Algo::Geometry::facePlane<PFP>(m.map,agent->part_.d,m.position);
...@@ -99,7 +103,7 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false, ...@@ -99,7 +103,7 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
if(showNeighborDist) if(showNeighborDist)
{ {
radius = sqrt(agent->neighborDistSq_); radius = agent->neighborDist_;
glColor3f(0.0f,1.0f,0.0f); glColor3f(0.0f,1.0f,0.0f);
glBegin(GL_LINE_LOOP); glBegin(GL_LINE_LOOP);
for(unsigned int i = 0; i < 5; ++i) for(unsigned int i = 0; i < 5; ++i)
......
#ifndef M_MOVING_OBSTACLE_H //#ifndef M_MOVING_OBSTACLE_H
#define M_MOVING_OBSTACLE_H //#define M_MOVING_OBSTACLE_H
//
#include <iostream> //#include <iostream>
//
#include "utils.h" //#include "utils.h"
#include "env_map.h" //#include "env_map.h"
#include "Algo/MovingObjects/particle_cell_2D.h" //#include "Algo/MovingObjects/particle_cell_2D.h"
//
//class Simulator;
class Simulator; //
//class MovingObstacle
class MovingObstacle //{
{ //public:
public: // MovingObstacle(EnvMap* envmap, std::vector<PFP::VEC3> pos);
MovingObstacle(EnvMap* envmap, std::vector<PFP::VEC3> pos); //
// void updateAgentNeighbors();
void updateAgentNeighbors(); //
// void computePrefVelocity();
void computePrefVelocity(); // void computeNewVelocity();
void computeNewVelocity(); //
// void update();
void update(); //
// unsigned int nbVertices;
unsigned int nbVertices; // CGoGN::Algo::MovingObjects::ParticleCell2D<PFP>* * parts_;
CGoGN::Algo::MovingObjects::ParticleCell2D<PFP>* * parts_; // Obstacle* * obstacles_;
Obstacle* * obstacles_; //
// std::vector<std::pair<float, Agent*> > agentNeighbors_;
std::vector<std::pair<float, Agent*> > agentNeighbors_; //
// VEC3 finalGoal;
VEC3 finalGoal; // Dart finalDart;
Dart finalDart; //
// unsigned int curGoal_;
unsigned int curGoal_; //
// static float neighborDistSq_;
static float neighborDistSq_; // static float timeStep ;
static float timeStep ; //
// VEC3 velocity_;
VEC3 velocity_; // VEC3 newVelocity_;
VEC3 newVelocity_; // VEC3 prefVelocity_;
VEC3 prefVelocity_; //
// EnvMap* envMap_;
EnvMap* envMap_; //};
}; //
//#endif
#endif
...@@ -6,7 +6,7 @@ ...@@ -6,7 +6,7 @@
#include "env_map.h" #include "env_map.h"
#include "agent.h" #include "agent.h"
#include "obstacle.h" #include "obstacle.h"
#include "moving_obstacle.h" //#include "moving_obstacle.h"
#include "path_finder.h" #include "path_finder.h"
#include <iostream> #include <iostream>
...@@ -97,9 +97,11 @@ public: ...@@ -97,9 +97,11 @@ public:
void setupMovingObstacle(unsigned int nbObstacles); void setupMovingObstacle(unsigned int nbObstacles);
#ifndef SPATIAL_HASHING
void addPathToCorner(); void addPathToCorner();
void addPathsToAgents(); void addPathsToAgents();
void addPathsToAgents_height(); void addPathsToAgents_height();
#endif
bool importAgents(std::string filename); bool importAgents(std::string filename);
bool exportAgents(std::string filename); bool exportAgents(std::string filename);
...@@ -110,7 +112,7 @@ public: ...@@ -110,7 +112,7 @@ public:
EnvMap envMap_; EnvMap envMap_;
std::vector<Agent*> agents_; std::vector<Agent*> agents_;
std::vector<MovingObstacle*> movingObstacles_; // std::vector<MovingObstacle*> movingObstacles_;
float timeStep_; float timeStep_;
float globalTime_; float globalTime_;
......
#ifndef SPATIAL_HASHING_H
#define SPATIAL_HASHING_H
#include "Geometry/vector_gen.h"
//#include <boost/unordered_map.hpp>
namespace CGoGN
{
//class Vec2uiHash : public boost::hash<Geom::Vec2ui>
//{
//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: