env_map.h 11.4 KB
Newer Older
1 2
#ifndef ENV_MAP_H
#define ENV_MAP_H
Pierre Kraemer's avatar
Pierre Kraemer committed
3 4 5 6 7

#include <iostream>
#include <algorithm>

#include "Topology/generic/parameters.h"
Pierre Kraemer's avatar
update  
Pierre Kraemer committed
8
#include "Topology/map/embeddedMap2.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
9 10 11 12 13 14 15 16 17

#include "Algo/ImplicitHierarchicalMesh/ihm.h"
#include "Algo/ImplicitHierarchicalMesh/subdivision.h"

#include "Algo/Modelisation/polyhedron.h"
#include "Algo/Modelisation/extrusion.h"
#include "Algo/Modelisation/subdivision.h"
#include "Algo/Geometry/centroid.h"
#include "Algo/Geometry/area.h"
Thomas's avatar
Thomas committed
18
#include "Geometry/bounding_box.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
19 20 21 22 23

#include "Container/fakeAttribute.h"

#include "Algo/Parallel/parallel_foreach.h"

Pierre Kraemer's avatar
Pierre Kraemer committed
24 25
#include "spatialHashing.h"

26
using namespace CGoGN ;
Pierre Kraemer's avatar
Pierre Kraemer committed
27

pitiot's avatar
maj  
pitiot committed
28 29 30
class Agent;
class Obstacle;
class MovingObstacle;
Pierre Kraemer's avatar
Pierre Kraemer committed
31

David Cazier's avatar
David Cazier committed
32
struct PFP : public PFP_STANDARD
Pierre Kraemer's avatar
Pierre Kraemer committed
33 34
{
	// definition de la carte
David Cazier's avatar
David Cazier committed
35
	typedef Algo::IHM::ImplicitHierarchicalMap MAP ;
Pierre Kraemer's avatar
Pierre Kraemer committed
36 37

	// definition des listes d'agent
David Cazier's avatar
David Cazier committed
38 39
	typedef std::vector<Agent*> AGENTS ;
	typedef std::vector<Obstacle*> OBSTACLES ;
pitiot's avatar
pitiot committed
40

pitiot's avatar
maj  
pitiot committed
41 42
	typedef std::vector<MovingObstacle*> MOVINGOBSTACLES;

David Cazier's avatar
David Cazier committed
43 44
	typedef NoMathIONameAttribute<AGENTS> AGENTVECT ;
	typedef NoMathIONameAttribute<OBSTACLES> OBSTACLEVECT ;
pitiot's avatar
pitiot committed
45

pitiot's avatar
maj  
pitiot committed
46 47 48 49 50
	typedef NoMathIONameAttribute<MOVINGOBSTACLES> MOVINGOBSTACLEVECT;

	typedef VertexAttribute<PFP::VEC3> TVEC3;
	typedef FaceAttribute<AGENTVECT> TAB_AGENTVECT ;
	typedef FaceAttribute<OBSTACLEVECT> TAB_OBSTACLEVECT ;
51

David Cazier's avatar
David Cazier committed
52 53
	typedef NoMathIONameAttribute<std::pair<bool, bool> > BOOLATTRIB ;
} ;
Pierre Kraemer's avatar
Pierre Kraemer committed
54

David Cazier's avatar
David Cazier committed
55
typedef PFP::VEC3 VEC3 ;
56
typedef PFP::REAL REAL ;
Pierre Kraemer's avatar
Pierre Kraemer committed
57 58 59

class EnvMap
{
60 61
public:
	PFP::MAP map ;
62

63 64 65 66 67 68 69 70 71 72 73 74
	/* Geometry of the EnvMap : the bounding box of the scene
	 * The density of cells in the EnvMap is given by two parameters :
	 *  - minCellSize : the size under which no subdivision occurs
	 *  - maxCellSize : the size of the initial cells (before subdivisions occur)
	 *  - obstacleDistance : the minimal goal distance between agents and obstacles
	 * The number of cells in the initial EnvMap is about (width*height)/(size*size)
	 */
	Geom::BoundingBox<VEC3> geometry ;
	REAL maxCellSize ;
	REAL minCellSize ;
	REAL obstacleDistance ;

David Cazier's avatar
David Cazier committed
75
	EnvMap() ;
76
	void init(unsigned int config, REAL width, REAL height, REAL minSize, REAL maxSize) ;
77

David Cazier's avatar
David Cazier committed
78
	void markPedWay() ;
Thomas's avatar
Thomas committed
79

David Cazier's avatar
David Cazier committed
80
	unsigned int mapMemoryCost() ;
Thomas's avatar
Thomas committed
81

Pierre Kraemer's avatar
Pierre Kraemer committed
82
#ifndef SPATIAL_HASHING
83
	Dart getBelongingCell(const VEC3& pos) ;
Thomas's avatar
Thomas committed
84

85 86
	void subdivideAllToMaxLevel() ;
	void subdivideToProperLevel() ;
Thomas's avatar
Thomas committed
87

88
	void foreach_neighborFace(Dart d, FunctorType& f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
89

90
	void registerObstaclesInFaces() ;
pitiot's avatar
maj  
pitiot committed
91
	void registerWallInFaces();
92
	void addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor) ;
93

94
	void setAgentNeighborsAndObstacles(Agent* agent) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
95

96 97 98 99 100 101
	unsigned int totalNeighborSize(Dart d) ;
	void nbAgentsIncrease(Dart d) ;
	void nbAgentsDecrease(Dart d) ;
	void pushAgentInCells(Agent* agent, Dart d) ;
	void popAgentInCells(Agent* agent, Dart d) ;
	void agentChangeFace(Agent* agent, Dart oldFace) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
102

103 104 105 106
	void pushObstacleInCells(Obstacle* o, Dart d) ;
	void popObstacleInCells(Obstacle* o, Dart d) ;
	void obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace) ;
	void updateMap() ;
107

108
	void resetAgentInFace(Agent* agent) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
109 110
#endif

David Cazier's avatar
David Cazier committed
111 112
	PFP::TVEC3 position ;
	PFP::TVEC3 normal ;
Pierre Kraemer's avatar
Pierre Kraemer committed
113

David Cazier's avatar
David Cazier committed
114 115 116
	PFP::MAP mapScenary ;
	PFP::TVEC3 positionScenary ;
	PFP::TVEC3 normalScenary ;
pitiot's avatar
maj  
pitiot committed
117 118 119 120 121 122 123 124 125 126 127 128 129
	CellMarker<EDGE> obstacleMarkS ;
	CellMarker<FACE> buildingMarkS ;
	CellMarker<EDGE> obstacleMark ;
	CellMarker<FACE> buildingMark ;
	CellMarker<FACE> pedWayMark ;

	// ajout moving obst
	void addObstAsNeighbor (Obstacle *  o, const std::list<Dart>& belonging_cells, std::list<Dart> * nieghbor_cells);
	// void addMovingObstAsNeighbor (MovingObstacle *  mo,std::list<Dart> belonging_cells, std::list<Dart> *neighbor_cells);
	void pushObstNeighborInCells(Obstacle* o, Dart d);
	void popObstNeighborInCells(Obstacle* o, Dart d);
	void find_next(Obstacle* o,Dart * d, CellMarkerStore<FACE>& cms);

130

David Cazier's avatar
David Cazier committed
131
	std::vector<Dart> newBuildings ;
132

Pierre Kraemer's avatar
Pierre Kraemer committed
133
#ifndef SPATIAL_HASHING
pitiot's avatar
maj  
pitiot committed
134 135 136 137 138
	FaceAttribute<PFP::BOOLATTRIB> subdivisableFace ;

	PFP::TAB_AGENTVECT agentvect ;
	PFP::TAB_AGENTVECT neighborAgentvect ;
	PFP::TAB_OBSTACLEVECT neighborObstvect;
Pierre Kraemer's avatar
Pierre Kraemer committed
139
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
140

David Cazier's avatar
David Cazier committed
141
	PFP::TAB_OBSTACLEVECT obstvect ;
Pierre Kraemer's avatar
Pierre Kraemer committed
142 143


Pierre Kraemer's avatar
Pierre Kraemer committed
144
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
145 146
	static const unsigned int nbAgentsToSubdivide = 10 ;
	static const unsigned int nbAgentsToSimplify = 4 ;
David Cazier's avatar
David Cazier committed
147

pitiot's avatar
maj  
pitiot committed
148 149
	CellMarker<FACE> refineMark ;
	CellMarker<FACE> coarsenMark ;
150 151
	std::vector<Dart> refineCandidate ;
	std::vector<Dart> coarsenCandidate ;
Pierre Kraemer's avatar
Pierre Kraemer committed
152

153
	void clearUpdateCandidates() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
154 155 156
#endif

#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
157 158 159 160 161 162 163 164 165
	unsigned int agentGridSize(unsigned int i)
	{
		return geometry.size(i) / minCellSize ;
	}

	unsigned int obstacleGridSize(unsigned int i)
	{
		return geometry.size(i) / obstacleDistance ;
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
166

pitiot's avatar
maj  
pitiot committed
167
	Geom::Vec2ui agentPositionCell(Agent* a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
168

pitiot's avatar
maj  
pitiot committed
169
	Geom::Vec2ui obstaclePositionCell(VEC3 pos)
Pierre Kraemer's avatar
Pierre Kraemer committed
170
	{
David Cazier's avatar
David Cazier committed
171 172
		VEC3 relativePos = pos - geometry.min() ;
		relativePos /= obstacleDistance ;
173
		return Geom::Vec2ui(relativePos[0], relativePos[1]) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
174 175
	}

pitiot's avatar
maj  
pitiot committed
176 177 178 179 180 181 182 183 184 185 186 187 188 189
	const std::vector<Agent*>& getNeighbors(Geom::Vec2ui c) {
		assert(ht_agents.hasData(c)) ;
		return ht_agents[c];
	}
	const std::vector<Agent*>& getNeighbors(Agent* a) ;

	void getOneRingNeighbors(Agent* a, std::vector<Agent*>& neighbors) ;

	void addAgentInGrid(Agent* a) ;
	void addAgentInGrid(Agent* a, Geom::Vec2ui c) ;

	void removeAgentFromGrid(Agent* a) ;
	void removeAgentFromGrid(Agent* a, Geom::Vec2ui c) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
190 191 192
	HashTable2D< std::vector<Agent*> > ht_agents ;
	HashTable2D< std::vector<Obstacle*> > ht_obstacles ;
#endif
193
} ;
Pierre Kraemer's avatar
Pierre Kraemer committed
194

pitiot's avatar
maj  
pitiot committed
195 196 197 198
void update_registration(Obstacle * o);
void register_pop(Obstacle* o, int n);
void resetObstInFace(Obstacle* o);
void resetObstPartInFace(Obstacle* o, Dart d);
pitiot's avatar
pitiot committed
199
void resetPart(MovingObstacle * mo,int index, Dart d);
pitiot's avatar
maj  
pitiot committed
200 201
void displayMO(Obstacle * o);

Pierre Kraemer's avatar
Pierre Kraemer committed
202
/**************************************
David Cazier's avatar
David Cazier committed
203 204
 *           INLINE FUNCTIONS          *
 **************************************/
Pierre Kraemer's avatar
Pierre Kraemer committed
205

David Cazier's avatar
David Cazier committed
206
template <typename T>
pitiot's avatar
maj  
pitiot committed
207
inline bool removeElementFromVector(std::vector<T>& a, T ag)
Pierre Kraemer's avatar
Pierre Kraemer committed
208
{
209
	typename std::vector<T>::iterator end = a.template end() ;
David Cazier's avatar
David Cazier committed
210 211 212 213
	for (typename std::vector<T>::iterator it = a.begin(); it != end; ++it)
	{
		if (*it == ag)
		{
214 215
			*it = a.back() ;
			a.pop_back() ;
pitiot's avatar
maj  
pitiot committed
216
			return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
217 218
		}
	}
pitiot's avatar
maj  
pitiot committed
219
	return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
220 221 222
}

#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
223 224
inline unsigned int EnvMap::totalNeighborSize(Dart d)
{
225 226 227 228
	return agentvect[d].size() + neighborAgentvect[d].size() ;
}

inline void EnvMap::nbAgentsIncrease(Dart d)
Pierre Kraemer's avatar
Pierre Kraemer committed
229
{
230 231
	if (refineMark.isMarked(d)) return ;
	if (totalNeighborSize(d) < nbAgentsToSubdivide) return ;
Pierre Kraemer's avatar
Pierre Kraemer committed
232

233
	std::pair<bool, bool>& sf = subdivisableFace[d] ;
David Cazier's avatar
David Cazier committed
234 235
	if (sf.first == false || (sf.first == true && sf.second))
	{
236 237 238
		refineMark.mark(d) ;
		refineCandidate.push_back(d) ;
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
239 240
}

241
inline void EnvMap::nbAgentsDecrease(Dart d)
Thomas's avatar
Thomas committed
242
{
243 244
	if (coarsenMark.isMarked(d) || refineMark.isMarked(d)) return ;
	if (totalNeighborSize(d) > nbAgentsToSimplify) return ;
Thomas's avatar
Thomas committed
245

246 247 248
	coarsenMark.mark(d) ;
	coarsenCandidate.push_back(map.faceOldestDart(d)) ;
}
Pierre Kraemer's avatar
Pierre Kraemer committed
249 250

inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
Pierre Kraemer's avatar
Pierre Kraemer committed
251
{
David Cazier's avatar
David Cazier committed
252
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
pitiot's avatar
maj  
pitiot committed
253
//	assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) == agentvect[d].end());
David Cazier's avatar
David Cazier committed
254 255

	agentvect[d].push_back(agent) ;
256
//	nbAgentsIncrease(d);
David Cazier's avatar
David Cazier committed
257 258

	Dart dd = d ;
David Cazier's avatar
David Cazier committed
259 260
	do
	{
David Cazier's avatar
David Cazier committed
261
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
262 263
		while (ddd != dd)
		{
David Cazier's avatar
David Cazier committed
264
			neighborAgentvect[ddd].push_back(agent) ;
265
//			nbAgentsIncrease(ddd);
David Cazier's avatar
David Cazier committed
266
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
267
		}
David Cazier's avatar
David Cazier committed
268 269
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
270 271
}

Thomas's avatar
Thomas committed
272 273
inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
{
David Cazier's avatar
David Cazier committed
274 275 276
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

	removeElementFromVector<Agent*>(agentvect[d], agent) ;
277
//	nbAgentsDecrease(d) ;
David Cazier's avatar
David Cazier committed
278 279

	Dart dd = d ;
David Cazier's avatar
David Cazier committed
280 281
	do
	{
David Cazier's avatar
David Cazier committed
282
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
283 284
		while (ddd != dd)
		{
David Cazier's avatar
David Cazier committed
285
			removeElementFromVector<Agent*>(neighborAgentvect[ddd], agent) ;
286
//			nbAgentsDecrease(ddd) ;
David Cazier's avatar
David Cazier committed
287
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
288
		}
David Cazier's avatar
David Cazier committed
289 290
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
291 292
}

pitiot's avatar
maj  
pitiot committed
293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398

//ajout moving obst///////////////////////////////////////////////////////////////////////////////////////////////////////

inline void EnvMap::addObstAsNeighbor (Obstacle *  o, const std::list<Dart>& belonging_cells, std::list<Dart> *neighbor_cells)
{
	assert(map.getCurrentLevel() == map.getMaxLevel());

	neighbor_cells->clear();

	CellMarkerStore<FACE> MovingObstMark(map);
	CellMarkerStore<FACE> OneRingMark(map);

	for(std::list<Dart>::const_iterator it= belonging_cells.begin();it!=belonging_cells.end();++it)
	{
		assert(!buildingMark.isMarked(*it)) ;

		MovingObstMark.mark(*it);
	}

	std::list<Dart>::const_iterator it=belonging_cells.begin();
	Dart beg = NIL;
	Dart first =NIL;
	Dart d=NIL;
	Dart dd=NIL;

	//boucle pour trouver une face du voisinage de l'obstacle ne contenant pas l'obstacle
	do
	{
		beg = *it;
		first = NIL;
		d=beg;
		do {
			dd=map.alpha1(map.alpha1(d));
			do {
				if (!MovingObstMark.isMarked(dd))
				{
					first = dd;
				}
				dd=map.alpha1(dd);
			}while (first==NIL && dd!=d);

			d=map.phi1(d);

		} while(first==NIL && d!=beg);
		++it;
	}while(first==NIL && it!=belonging_cells.end());
	assert(!buildingMark.isMarked(d)) ;

	d=first;
	do
	{
		if (!OneRingMark.isMarked(d))
		{
			OneRingMark.mark(d);
			(*neighbor_cells).push_back(d);
		}

		find_next(o,&d, MovingObstMark);

	}while(!map.sameFace(d,first));
}

// find_next cherche la prochaine case "voisine" d'un obstacle faisant parti d'un movingobstacle (algo de parcours du one-ring )

inline void EnvMap::find_next(Obstacle* o,Dart * ddd, CellMarkerStore<FACE>& cms)
{
	Dart d=*ddd;
	Dart first = NIL;
	Dart dd;
	int rd =0;
	do{
		rd++;
		dd=map.alpha1((d));

		if (!(cms.isMarked(dd)))
		{
			first = dd;
		}

		d=map.phi_1(d);
		if (rd>10000) {
			displayMO(o);
			CGoGNout <<(first==NIL)<< " infini find next : "<< *ddd<<CGoGNendl;
			break;
		}
	}while(first==NIL);
	*ddd=first;
}

inline void EnvMap::pushObstNeighborInCells(Obstacle* o, Dart d)
{
	assert(map.getCurrentLevel() == map.getMaxLevel());
	assert(std::find(neighborObstvect[d].begin(), neighborObstvect[d].end(), o) == neighborObstvect[d].end());

	neighborObstvect[d].push_back(o);
}

inline void EnvMap::popObstNeighborInCells(Obstacle* o, Dart d)
{
	assert(map.getCurrentLevel() == map.getMaxLevel());
	assert(std::find(neighborObstvect[d].begin(), neighborObstvect[d].end(), o) != neighborObstvect[d].end());

	removeElementFromVector<Obstacle* >(neighborObstvect[d], o);
}


Pierre Kraemer's avatar
Pierre Kraemer committed
399
inline void EnvMap::pushObstacleInCells(Obstacle* o, Dart d)
400
{
401
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
pitiot's avatar
maj  
pitiot committed
402
	assert(std::find(obstvect[d].begin(), obstvect[d].end(), o) == obstvect[d].end());
David Cazier's avatar
David Cazier committed
403
	assert(!buildingMark.isMarked(d)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
404

405
	obstvect[d].push_back(o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425

//	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)
{
426
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
427
	assert(!buildingMark.isMarked(d)) ;
pitiot's avatar
maj  
pitiot committed
428
	assert(std::find(obstvect[d].begin(), obstvect[d].end(), o) != obstvect[d].end());
Pierre Kraemer's avatar
Pierre Kraemer committed
429

430
	removeElementFromVector<Obstacle*>(obstvect[d], o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
431 432 433 434 435 436 437 438 439 440 441 442 443 444 445

//	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);
446 447
}

Pierre Kraemer's avatar
Pierre Kraemer committed
448 449
inline void EnvMap::clearUpdateCandidates()
{
David Cazier's avatar
David Cazier committed
450 451
	refineCandidate.clear() ;
	coarsenCandidate.clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
452
}
pitiot's avatar
maj  
pitiot committed
453 454
///addMovingObstAsNeighbor est pour détecter les voisins du movingobstacle global afin de le considerer comme un super agent

Pierre Kraemer's avatar
Pierre Kraemer committed
455
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
456 457

#endif