ajout SPATIAL_HASHING

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)
 

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<std::pair<float, Agent*> > agentNeighbors_;
 	std::vector<std::pair<float, Obstacle*> > obstacleNeighbors_;
-	
+
+#ifdef SPATIAL_HASHING
+	VEC3 pos ;
+#else
 	CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> part_;
+#endif
 	
 	std::vector<VEC3> 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 ;

include/env_generator.h

@@ -22,8 +22,10 @@ Algo::Modelisation::Polyhedron<PFP> generateTrianGrid(typename PFP::MAP& map, ty
 template <typename PFP>
 Dart extrudeFaceAndMark(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart d, CellMarker& buildingMark, float height);
 
+#ifndef SPATIAL_HASHING
 template <typename PFP>
 bool animateCity(EnvMap* envMap);
+#endif
 
 //template <typename PFP>
 //Dart generateBuilding(typename PFP::MAP& map, typename PFP::TVEC3& position, Dart d, float height, unsigned int buildingType, CellMarker& obstacleMark, CellMarker& buildingMark);

include/env_generator.hpp

 #include "Algo/Modelisation/subdivision.h"
 #include "Geometry/inclusion.h"
 #include "Topology/generic/traversorCell.h"
+#include <algorithm>
 
 namespace CGoGN
 {
@@ -170,6 +171,7 @@ Dart extrudeFaceAndMark(typename PFP::MAP& map, typename PFP::TVEC3& position, D
 	return dRoof;
 }
 
+#ifndef SPATIAL_HASHING
 template <typename PFP>
 bool animateCity(EnvMap* envMap)
 {
@@ -252,6 +254,7 @@ bool animateCity(EnvMap* envMap)
 
 	return false;
 }
+#endif
 
 //template <typename PFP>
 //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<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));
+#endif
 
 		dd = map.phi1(dd);
 	} while(dd != d);

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 <typename T>
-	inline void removeElementFromVector(std::vector<T>& 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<PFP::BOOLATTRIB> subdivisableFace;
-
 	std::vector<Dart> newBuildings;
 
+#ifndef SPATIAL_HASHING
+	AttributeHandler<PFP::BOOLATTRIB> 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<Dart> refineCandidate;
 	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          *
 **************************************/
 
+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)
 {
 	assert(map.getCurrentLevel() == map.getMaxLevel());
@@ -164,19 +210,51 @@ inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
 	} while(dd != d);
 }
 
-template <typename T>
-inline void EnvMap::removeElementFromVector(std::vector<T>& a, T ag)
+inline void EnvMap::pushObstacleInCells(Obstacle* o, Dart d)
 {
-	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;
-		}
-	}
+	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<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()
@@ -184,5 +262,6 @@ inline void EnvMap::clearUpdateCandidates()
 	refineCandidate.clear();
 	coarsenCandidate.clear();
 }
+#endif
 
 #endif

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<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,
 
 	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)

include/moving_obstacle.h

-#ifndef M_MOVING_OBSTACLE_H
-#define M_MOVING_OBSTACLE_H
-
-#include <iostream>
-
-#include "utils.h"
-#include "env_map.h"
-#include "Algo/MovingObjects/particle_cell_2D.h"
-
-
-class Simulator;
-
-class MovingObstacle
-{
-public:
-	MovingObstacle(EnvMap* envmap, std::vector<PFP::VEC3> pos);
-
-	void updateAgentNeighbors();
-
-	void computePrefVelocity();
-	void computeNewVelocity();
-
-	void update();
-
-	unsigned int nbVertices;
-	CGoGN::Algo::MovingObjects::ParticleCell2D<PFP>* * parts_;
-	Obstacle* * obstacles_;
-
-	std::vector<std::pair<float, Agent*> > 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 <iostream>
+//
+//#include "utils.h"
+//#include "env_map.h"
+//#include "Algo/MovingObjects/particle_cell_2D.h"
+//
+//class Simulator;
+//
+//class MovingObstacle
+//{
+//public:
+//	MovingObstacle(EnvMap* envmap, std::vector<PFP::VEC3> pos);
+//
+//	void updateAgentNeighbors();
+//
+//	void computePrefVelocity();
+//	void computeNewVelocity();
+//
+//	void update();
+//
+//	unsigned int nbVertices;
+//	CGoGN::Algo::MovingObjects::ParticleCell2D<PFP>* * parts_;
+//	Obstacle* * obstacles_;
+//
+//	std::vector<std::pair<float, Agent*> > agentNeighbors_;
+//
+//	VEC3 finalGoal;
+//	Dart finalDart;
+//
+//	unsigned int curGoal_;
+//
+//	static float neighborDistSq_;
+//	static float timeStep ;
+//
+//	VEC3 velocity_;
+//	VEC3 newVelocity_;
+//	VEC3 prefVelocity_;
+//
+//	EnvMap* envMap_;
+//};
+//
+//#endif

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 <iostream>
@@ -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<Agent*> agents_;
-	std::vector<MovingObstacle*> movingObstacles_;
+//	std::vector<MovingObstacle*> movingObstacles_;
 
 	float timeStep_;
 	float globalTime_;

include/spatialHashing.h

+#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:
+//	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 <typename DATA>
+//class HashTable2D : public boost::unordered_map<Geom::Vec2ui, DATA, Vec2uiHash, Vec2uiEqual>
+//{
+//public:
+//	HashTable2D(unsigned int size, const Vec2uiHash& key_hasher, const Vec2uiEqual& key_equal) :
+//		boost::unordered_map<Geom::Vec2ui, DATA, Vec2uiHash, Vec2uiEqual>(size, key_hasher, key_equal)
+//	{}
+//
+//	bool hasData(const Geom::Vec2ui& k) const
+//	{
+//		if(this->find(k) != this->end())
+//			return true ;
+//		return false ;
+//	}
+//};
+
+template <typename DATA>
+class HashTable2D
+{
+public:
+	typedef std::list< std::pair<Geom::Vec2ui, DATA> > 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

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) ;
 

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;