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