env_map.cpp 25.2 KB
Newer Older
Pierre Kraemer's avatar
Pierre Kraemer committed
1
2
3
#include "env_map.h"
#include "utils.h"
#include "agent.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
4
#include "obstacle.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
5

Pierre Kraemer's avatar
Pierre Kraemer committed
6
7
#include "Geometry/inclusion.h"
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
8
9
#include "Algo/Modelisation/subdivision.h"
#include "Algo/Geometry/normal.h"
Thomas's avatar
Thomas committed
10
11
#include "Algo/Import/importSvg.h"
#include "Algo/BooleanOperator/mergeVertices.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
12

13
14
#include "env_generator.h"

15
using namespace CGoGN ;
Pierre Kraemer's avatar
Pierre Kraemer committed
16

Pierre Kraemer's avatar
Pierre Kraemer committed
17
EnvMap::EnvMap() :
David Cazier's avatar
David Cazier committed
18
	obstacleDistance(Agent::range_),
pitiot's avatar
maj    
pitiot committed
19
20
21
22
23
	obstacleMarkS(mapScenary),
	buildingMarkS(mapScenary),
	obstacleMark(map),
	buildingMark(map),
	pedWayMark(map),
24

Pierre Kraemer's avatar
Pierre Kraemer committed
25
#ifndef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
26
27
	refineMark(map),
	coarsenMark(map)
Pierre Kraemer's avatar
Pierre Kraemer committed
28
29
30
31
32
#else
	ht_agents(1024),
	ht_neighbor_agents(1024),
	ht_obstacles(1024)
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
33
{
pitiot's avatar
maj    
pitiot committed
34
35
	position = map.addAttribute<VEC3, VERTEX>("position") ;
	normal = map.addAttribute<VEC3, VERTEX>("normal") ;
Pierre Kraemer's avatar
Pierre Kraemer committed
36
37

#ifndef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
38
39
40
41
42
	agentvect = map.addAttribute<PFP::AGENTVECT, FACE>("agents") ;
	neighborAgentvect = map.addAttribute<PFP::AGENTVECT, FACE>("neighborAgents") ;
	obstvect = map.addAttribute<PFP::OBSTACLEVECT, FACE>("obstacles") ;
	neighborObstvect = map.addAttribute<PFP::OBSTACLEVECT, FACE>("neighborObstacles") ;
	subdivisableFace = map.addAttribute<PFP::BOOLATTRIB, FACE>("subdivisableFace") ;
Thomas's avatar
Thomas committed
43

44
45
46
47
48
49
50
51
	refineCandidate.reserve(100) ;
	coarsenCandidate.reserve(100) ;
#endif
}

void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, REAL maxSize)
{
	std::cout << "Init EnvMap" << std::endl ;
David Cazier's avatar
David Cazier committed
52
	VEC3 bottomLeft(-width / 2, -height / 2, 0.0f) ;
53
54
55
56
57
58
59
60
	VEC3 topRight(width / 2, height / 2, 0.0f) ;

	geometry.reset() ;
	geometry.addPoint(bottomLeft) ;
	geometry.addPoint(topRight) ;
	minCellSize = minSize ;
	maxCellSize = maxSize ;
	std::cout << " - Geometry : " << geometry ;
pitiot's avatar
pitiot committed
61
	std::cout << " - Geometry size : " << geometry.size(0)<<" x "<<geometry.size(1) ;
62
63
	std::cout << " - Cell size between : " << minSize << " and " << maxSize << std::endl ;
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
64
	std::cout << " - Table de hachage : " << agentGridSize(0) << " x " << agentGridSize(1) << std::endl ;
65
66
#endif

David Cazier's avatar
David Cazier committed
67
68
	switch (config)
	{
pitiot's avatar
maj    
pitiot committed
69
70
71
72
73
74
75
76
77
78
79
		case 0 :
			CityGenerator::generateGrid<PFP>(*this) ;
			break ;
		case 1 :
			CityGenerator::generateGrid<PFP>(*this) ;
			break ;
		case 2 :
			CityGenerator::generateCity<PFP>(*this) ;
			// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
			break ;
		case 3 : {
80
81
82
83
84
85
86
//			std::string filename = "/home/jund/Desktop/drawingQuads.svg" ;
//			std::string filename = "/home/jund/Desktop/drawingTest.svg" ;
//			std::string filename = "/home/jund/Desktop/drawingSimple.svg" ;
//			std::string filename = "/home/jund/Desktop/drawingLines.svg" ;
//			std::string filename = "/home/jund/Desktop/mapKrutSimple.svg" ;
//			std::string filename = "/home/jund/Desktop/mapKrut.svg" ;
//			Algo::Import::importSVG<PFP>(map, filename, position, obstacleMark, buildingMark) ;
pitiot's avatar
maj    
pitiot committed
87

88
89
90
91
//			Algo::BooleanOperator::mergeVertices<PFP>(map, position) ;
//			map.closeMap() ;
//			Algo::Modelisation::CatmullClarkSubdivision<PFP>(map, position) ;
//			Algo::Modelisation::computeDual<PFP>(map) ;
pitiot's avatar
maj    
pitiot committed
92
93
94
95
96
97
		}
			CityGenerator::generateCity<PFP>(*this) ;
			break ;
		case 4 :
			CityGenerator::generatePlanet<PFP>(*this) ;
			break ;
98
99
100
101
102
103
104
105
106
	}

//	CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
//	CityGenerator::convexifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
//	CityGenerator::installGuardRail<PFP>(map, position, obstacleMark, buildingMark, 5.0f);

#ifndef SPATIAL_HASHING
	map.init() ;
//	registerObstaclesInFaces();
pitiot's avatar
maj    
pitiot committed
107
108
	// TODO Check registerWallInFaces();
	registerWallInFaces() ;
109
110
//	subdivideAllToMaxLevel();

pitiot's avatar
maj    
pitiot committed
111
	for (unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
112
		subdivisableFace[i].first = false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
113
#endif
114

Pierre Kraemer's avatar
Pierre Kraemer committed
115
116
}

117
118
void EnvMap::markPedWay()
{
pitiot's avatar
maj    
pitiot committed
119
	CellMarker<FACE> treat(map) ;
David Cazier's avatar
David Cazier committed
120
121
122
123
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
124
125
126
			treat.mark(d) ;

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
127
128
			do
			{
129
				Dart ddd = dd ;
David Cazier's avatar
David Cazier committed
130
131
132
133
				do
				{
					if (obstacleMark.isMarked(dd))
					{
134
135
						pedWayMark.mark(d) ;
						break ;
136
					}
137
138
					ddd = map.alpha1(ddd) ;
				} while (ddd != dd) ;
139

140
				dd = map.phi1(dd) ;
141

142
			} while (dd != d) ;
143
144
145
		}
	}

146
	treat.unmarkAll() ;
David Cazier's avatar
David Cazier committed
147
148
149
150
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
151
152
153
			treat.mark(d) ;

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
154
155
156
157
158
159
			do
			{
				if (pedWayMark.isMarked(map.phi2(dd)) && pedWayMark.isMarked(map.phi2(map.phi1(dd)))
				    && !pedWayMark.isMarked(map.phi2(map.phi1(map.phi1(dd))))
				    && !pedWayMark.isMarked(map.phi2(map.phi1(map.phi1(map.phi1(dd))))))
				{
160
161
162
					pedWayMark.mark(d) ;
					break ;
				}
163

164
				dd = map.phi1(dd) ;
165

166
			} while (dd != d) ;
167
168
169
170
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
171
172
unsigned int EnvMap::mapMemoryCost()
{
pitiot's avatar
maj    
pitiot committed
173
174
175
176
	return (map.getAttributeContainer<DART>()).memorySize()
	    + (map.getAttributeContainer<VERTEX>()).memorySize()
	    + (map.getAttributeContainer<EDGE>()).memorySize()
	    + (map.getAttributeContainer<FACE>()).memorySize() ;
Thomas's avatar
Thomas committed
177
178
}

Pierre Kraemer's avatar
Pierre Kraemer committed
179
#ifndef SPATIAL_HASHING
Thomas's avatar
Thomas committed
180
181
void EnvMap::subdivideAllToMaxLevel()
{
182
	bool subdiv ;
David Cazier's avatar
David Cazier committed
183
184
	do
	{
185
		subdiv = false ;
Thomas's avatar
Thomas committed
186
		{
pitiot's avatar
maj    
pitiot committed
187
			CellMarker<FACE> subd(map) ;
David Cazier's avatar
David Cazier committed
188
189
190
191
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
192
					subd.mark(d) ;
David Cazier's avatar
David Cazier committed
193
194
					if (!buildingMark.isMarked(d))
					{
Thomas's avatar
Thomas committed
195
196
						//check if subdivision is authorized
						float minDistSq = Agent::neighborDistSq_ ;
197
						bool subdivisable = true ;
Thomas's avatar
Thomas committed
198
199
200
						Dart old = map.faceOldestDart(d) ;
						unsigned int fLevel = map.faceLevel(old) ;
						map.setCurrentLevel(fLevel) ;
David Cazier's avatar
David Cazier committed
201
						PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
Thomas's avatar
Thomas committed
202
						Dart fd = old ;
David Cazier's avatar
David Cazier committed
203
204
						do
						{
Thomas's avatar
Thomas committed
205
							PFP::VEC3& p = position[fd] ;
206
							PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd,
David Cazier's avatar
David Cazier committed
207
							                                                      position) ;
208
							PFP::VEC3 proj = fCenter
David Cazier's avatar
David Cazier committed
209
210
211
							    - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
							if (proj.norm2() < minDistSq)
							{
Thomas's avatar
Thomas committed
212
213
214
215
								subdivisable = false ;
								break ;
							}
							fd = map.phi1(fd) ;
216
						} while (fd != old) ;
Thomas's avatar
Thomas committed
217

David Cazier's avatar
David Cazier committed
218
219
						if (subdivisable)
						{
Thomas's avatar
Thomas committed
220
221
							map.setCurrentLevel(fLevel) ;
							Algo::IHM::subdivideFace<PFP>(map, old, position) ;
222
							subdiv = true ;
Thomas's avatar
Thomas committed
223
224
225
226
227
228
						}
						map.setCurrentLevel(map.getMaxLevel()) ;
					}
				}
			}
		}
229
	} while (subdiv) ;
Thomas's avatar
Thomas committed
230
231
232
233
}

void EnvMap::subdivideToProperLevel()
{
234
	bool subdiv ;
David Cazier's avatar
David Cazier committed
235
236
	do
	{
237
		subdiv = false ;
Thomas's avatar
Thomas committed
238
		{
pitiot's avatar
maj    
pitiot committed
239
			CellMarker<FACE> subd(map) ;
David Cazier's avatar
David Cazier committed
240
241
242
243
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
244
					subd.mark(d) ;
David Cazier's avatar
David Cazier committed
245
246
					if (!refineMark.isMarked(d) && agentvect[d].size() > nbAgentsToSubdivide)
					{
247
						std::pair<bool, bool>& sf = subdivisableFace[d] ;
David Cazier's avatar
David Cazier committed
248
249
						if (sf.first == false || (sf.first == true && sf.second))
						{
250
251
252
							subdiv = true ;
							refineMark.mark(d) ;
							refineCandidate.push_back(d) ;
Thomas's avatar
Thomas committed
253
254
255
256
						}
					}
				}
			}
257
			subd.unmarkAll() ;
Thomas's avatar
Thomas committed
258
		}
259
260
261
262
		updateMap() ;
		refineCandidate.clear() ;
		map.setCurrentLevel(map.getMaxLevel()) ;
	} while (subdiv) ;
Thomas's avatar
Thomas committed
263
264
}

Pierre Kraemer's avatar
Pierre Kraemer committed
265
266
Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
pitiot's avatar
...    
pitiot committed
267
//	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
pitiot's avatar
pitiot committed
268

pitiot's avatar
maj    
pitiot committed
269
	CellMarkerStore<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
270
271
272
273
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
274
			m.mark(d) ;
pitiot's avatar
pitiot committed
275
			if (!buildingMark.isMarked(d)
David Cazier's avatar
David Cazier committed
276
			    && Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true)) return d ;
277
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
278
279
	}

pitiot's avatar
pitiot committed
280
	std::cout << "ERROR : pos not in map for getBelongingCell" << std::endl ;
281
	return map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
282
}
Pierre Kraemer's avatar
Pierre Kraemer committed
283
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
284

Pierre Kraemer's avatar
Pierre Kraemer committed
285
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
286
287
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
288
	Dart dd = d ;
David Cazier's avatar
David Cazier committed
289
290
	do
	{
291
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
292
293
		while (ddd != dd)
		{
294
295
			f(ddd) ;
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
296
		}
297
298
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
299
300
}

pitiot's avatar
maj    
pitiot committed
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
// TODO Check void EnvMap::registerWallInFaces() boundary markers ?
void EnvMap::registerWallInFaces()
{
	CellMarker<FACE> m(map) ;
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
			m.mark(d) ;

			Dart dd = d ;

			//retrieve all obstacles within its one-ring

			//first : test all edges of the face of d
			do
			{
				if (obstacleMark.isMarked(dd) && map.isBoundaryMarked(dd))
				{
					Dart dd2 = map.phi2(dd) ;
					Dart next = map.phi1(dd2) ;
					Dart previous = map.phi_1(dd2) ;
					Obstacle* o = new Obstacle(position[next], position[dd2], position[previous],
					                           position[map.phi1(next)], NULL, 0) ;
					obstvect[dd2].push_back(o) ;
				}
				dd = map.phi1(dd) ;
			} while (dd != d) ;

//				//second : test all edges of neighboring faces
//				do
//				{
//					Dart ddd = map.alpha1(map.alpha1(dd));
//					while(ddd != dd)
//					{
//						if(!buildingMark.isMarked(ddd))
//							addNeighborObstacles(obstvect[d], ddd, ddd == map.alpha_1(dd));
//						ddd = map.alpha1(ddd);
//					}
//					dd = map.phi1(dd);
//				} while(dd != d);
		}
	}

}

Pierre Kraemer's avatar
Pierre Kraemer committed
347
348
void EnvMap::registerObstaclesInFaces()
{
pitiot's avatar
maj    
pitiot committed
349
	CellMarker<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
350
351
352
353
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
354
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
355
356
			if (!buildingMark.isMarked(d))
			{
357
				Dart dd = d ;
358
359
360
361

				//retrieve all obstacles within its one-ring

				//first : test all edges of the face of d
David Cazier's avatar
David Cazier committed
362
363
364
365
				do
				{
					if (obstacleMark.isMarked(dd))
					{
366
367
368
369
						Dart dd2 = map.phi2(dd) ;
						Dart next = map.phi1(dd2) ;
						Dart previous = map.phi_1(dd2) ;
						Obstacle* o = new Obstacle(position[dd2], position[next],
pitiot's avatar
maj    
pitiot committed
370
371
						                           position[previous], position[map.phi1(next)],
						                           NULL, 0) ;
372
						obstvect[d].push_back(o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
373
					}
374
375
					dd = map.phi1(dd) ;
				} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
376

377
				//second : test all edges of neighboring faces
David Cazier's avatar
David Cazier committed
378
379
				do
				{
380
					Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
381
382
					while (ddd != dd)
					{
pitiot's avatar
maj    
pitiot committed
383
						// TODO Check effet de bord
David Cazier's avatar
David Cazier committed
384
385
						if (!buildingMark.isMarked(ddd)) addNeighborObstacles(
						    obstvect[d], ddd, ddd == map.alpha_1(dd)) ;
386
						ddd = map.alpha1(ddd) ;
387
					}
388
389
					dd = map.phi1(dd) ;
				} while (dd != d) ;
390
391
392
			}
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
393
394
395
396
}

void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor)
{
397
	Dart stop ;
David Cazier's avatar
David Cazier committed
398
399
400
401
	if (edgeNeighbor)
		stop = map.phi_1(map.phi_1(map.phi_1(d))) ;
	else
		stop = map.phi_1(map.phi_1(d)) ;
402
403

	Dart dd = d ;
David Cazier's avatar
David Cazier committed
404
405
406
407
	do
	{
		if (obstacleMark.isMarked(dd))
		{
408
409
410
411
412
413
414
415
416
417
//			if(buildingMark.isMarked(dd))
//			{
//				std::cout << "caca prout" << std::endl;
//				Dart next = map.phi1(dd);
//				Dart previous = map.phi_1(dd);
//				Obstacle* o = new Obstacle(position[dd], position[next], position[previous], position[map.phi1(next)]);
//				obst.push_back(o);
//			}
//			else
//			{
418
419
420
421
			Dart dd2 = map.phi2(dd) ;
			Dart next = map.phi1(dd2) ;
			Dart previous = map.phi_1(dd2) ;
			Obstacle* o = new Obstacle(position[dd2], position[next], position[previous],
pitiot's avatar
maj    
pitiot committed
422
			                           position[map.phi1(next)], NULL, 0) ;
423
			obst.push_back(o) ;
424
//			}
Pierre Kraemer's avatar
Pierre Kraemer committed
425
		}
426
427
		dd = map.phi1(dd) ;
	} while (dd != stop) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
428
429
430
431
}

void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
432
	Dart newFace = agent->part_.d ;
433

434
435
	popAgentInCells(agent, oldFace) ;
	pushAgentInCells(agent, newFace) ;
436

David Cazier's avatar
David Cazier committed
437
438
	if (!refineMark.isMarked(newFace) && agentvect[newFace].size() > nbAgentsToSubdivide)
	{
439
		std::pair<bool, bool>& sf = subdivisableFace[newFace] ;
David Cazier's avatar
David Cazier committed
440
441
		if (sf.first == false || (sf.first == true && sf.second))
		{
442
443
			refineMark.mark(newFace) ;
			refineCandidate.push_back(newFace) ;
444
445
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
446

David Cazier's avatar
David Cazier committed
447
448
449
	if (!coarsenMark.isMarked(oldFace) && !refineMark.isMarked(oldFace)
	    && 4 * agentvect[oldFace].size() < nbAgentsToSimplify)
	{
450
451
		coarsenMark.mark(oldFace) ;
		coarsenCandidate.push_back(map.faceOldestDart(oldFace)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
452
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
453
454
}

Pierre Kraemer's avatar
Pierre Kraemer committed
455
456
void EnvMap::obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace)
{
457
458
	popObstacleInCells(agent, oldFace) ;
	pushObstacleInCells(agent, newFace) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
459
}
460
void EnvMap::refine()
Pierre Kraemer's avatar
Pierre Kraemer committed
461
{
David Cazier's avatar
David Cazier committed
462
	for (std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end();
463
		    ++it)
David Cazier's avatar
David Cazier committed
464
		{
465
466
			Dart d = (*it) ;
			refineMark.unmark(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
467

468
469
			//if the number of agents is big enough
			if (agentvect[d].size() > nbAgentsToSubdivide)
David Cazier's avatar
David Cazier committed
470
			{
471
472
				int fLevel = -1 ;
				Dart old = map.faceOldestDart(d) ;
473

474
475
476
477
478
479
480
				bool subdivisable = true ;

				//check if faces resulting from a subdivision are big enough
				std::pair<bool, bool>& sf = subdivisableFace[old] ;
				if (sf.first == true)
					subdivisable = sf.second ;
				else
David Cazier's avatar
David Cazier committed
481
				{
482
483
484
485
486
487
488
					float minSizeSq = minCellSize * minCellSize ; // diametre de vision de l'agent au carré

					fLevel = map.faceLevel(old) ;
					map.setCurrentLevel(fLevel) ;
					PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
					Dart fd = old ;
					do
David Cazier's avatar
David Cazier committed
489
					{
490
491
492
493
494
495
496
497
498
499
500
						PFP::VEC3& p = position[fd] ;
						PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
						PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
						if (proj.norm2() < minSizeSq)
						{
							subdivisable = false ;
							break ;
						}
						fd = map.phi1(fd) ;
					} while (fd != old) ;
					map.setCurrentLevel(map.getMaxLevel()) ;
501

502
503
504
					sf.first = true ;
					sf.second = subdivisable ;
				}
505

506
507
508
				if (subdivisable)
				{
					if (fLevel == -1) fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
509

510
					sf.first = false ;
511

512
513
514
515
516
517
518
519
520
521
					PFP::AGENTS oldAgents(agentvect[old]) ;
					PFP::OBSTACLEVECT oldObst(obstvect[old]) ;
					PFP::OBSTACLEVECT oldNeighborObst(neighborObstvect[old]) ;
					for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
						popAgentInCells(*ait, old) ;
					for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
						register_pop(*ait, (*ait)->index) ;
					for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();ait != oldNeighborObst.end(); ++ait)
						register_pop(*ait, (*ait)->index) ;
					neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
522

523
524
525
526
527
528
529
530
531
532
533
					map.setCurrentLevel(fLevel) ;
					Algo::IHM::subdivideFace<PFP>(map, old, position) ;
					CellMarkerStore<FACE> newF(map) ;
					unsigned int degree = 0 ;
					Dart dd = old ;
					do
					{
						++degree ;
						newF.mark(dd) ;
						dd = map.phi1(dd) ;
					} while (dd != old) ;
pitiot's avatar
maj    
pitiot committed
534

535
536
537
538
539
540
					if (degree == 3)
					{
						Dart centerFace = map.phi2(map.phi1(old)) ;
						newF.mark(centerFace) ;
					}
					map.setCurrentLevel(map.getMaxLevel()) ;
541

542
543
544
545
546
547
					//agents contained in the subdivided cell are pushed correctly
					for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
					{
						resetAgentInFace(*ait) ;
						pushAgentInCells(*ait, (*ait)->part_.d) ;
					}
Pierre Kraemer's avatar
Pierre Kraemer committed
548

549
550
551
552
553
554
					//same for obstacles contained
					for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
					{
						resetPartSubdiv(*ait);
						update_registration(*ait) ;
					}
pitiot's avatar
maj    
pitiot committed
555

556
557
558
559
					//same for adjacent obstacles
					for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();
					    ait != oldNeighborObst.end(); ++ait)
					{
560

561
562
						update_registration(*ait) ;
					}
pitiot's avatar
maj    
pitiot committed
563

564
565
					//retrieve neighbors agents from onering cells
					dd = old ;
David Cazier's avatar
David Cazier committed
566
567
					do
					{
568
569
						Dart d3 = dd ;
						do
David Cazier's avatar
David Cazier committed
570
						{
571
572
573
							Dart d4 = map.alpha1(map.alpha1(d3)) ;
							PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
							while (d4 != d3)
David Cazier's avatar
David Cazier committed
574
							{
575
576
577
578
579
580
								if (!newF.isMarked(d4))
								{
									PFP::AGENTS& ad4 = agentvect[d4] ;
									nad3.insert(nad3.end(), ad4.begin(), ad4.end()) ;
								}
								d4 = map.alpha1(d4) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
581
							}
582
583
							d3 = map.phi1(d3) ;
						} while (d3 != dd) ;
584

585
586
587
588
						map.setCurrentLevel(fLevel) ;
						dd = map.phi1(dd) ;
						map.setCurrentLevel(map.getMaxLevel()) ;
					} while (dd != old) ;
589

590
					if (degree == 3)
David Cazier's avatar
David Cazier committed
591
					{
592
593
594
						Dart centerFace = map.phi2(map.phi1(old)) ;
						Dart d3 = centerFace ;
						do
David Cazier's avatar
David Cazier committed
595
						{
596
597
598
							Dart d4 = map.alpha1(map.alpha1(d3)) ;
							PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
							while (d4 != d3)
David Cazier's avatar
David Cazier committed
599
							{
600
601
602
603
604
605
								if (!newF.isMarked(d4))
								{
									PFP::AGENTS& ad4 = agentvect[d4] ;
									nad3.insert(neighborAgentvect[d3].end(), ad4.begin(), ad4.end()) ;
								}
								d4 = map.alpha1(d4) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
606
							}
607
608
609
							d3 = map.phi1(d3) ;
						} while (d3 != centerFace) ;
					}
610
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
611
612
			}
		}
613
		refineCandidate.clear() ;
David Cazier's avatar
David Cazier committed
614

615
616
617
}
void EnvMap::coarse()
{
618
	// On recrée une liste des faces à simplifier en empêchant les doublons
619
620
621
		// On en profite pour vérifier les conditions de simplifications
		std::vector<Dart> checkCoarsenCandidate ;
		checkCoarsenCandidate.reserve(coarsenCandidate.size()) ;
622

623
		for (unsigned int it = 0; it < coarsenCandidate.size(); ++it)
624
		{
625
626
627
			Dart old = coarsenCandidate[it] ;
			bool oldIsMarked = coarsenMark.isMarked(old) ;
			coarsenMark.unmark(old) ;
628

629
			unsigned int fLevel = map.faceLevel(old) ;
630

631
632
633
634
			if (oldIsMarked && fLevel > 0 && map.getDartLevel(old) < fLevel)
			{
				unsigned int cur = map.getCurrentLevel() ;
				map.setCurrentLevel(fLevel - 1) ;
635

636
				if (map.faceIsSubdividedOnce(old))
637
				{
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
					// on compte le nombre d'agents dans les sous-faces
					// on en profite pour compter le degré de la face grossière
					unsigned int degree = 0 ;
					unsigned int nbAgents = 0 ;
					Dart fit = old ;
					do
					{
						nbAgents += agentvect[fit].size() ;
						++degree ;
						coarsenMark.unmark(fit) ;
	// TODO Optimisation déjà faite
	//					unsigned int start = it + 1;
	//					unsigned int fEmb = map.getEmbedding<FACE>(fit) ;
	//					while(start < coarsenCandidate.size())
	//					{
	//						if(map.getEmbedding<FACE>(coarsenCandidate[start]) == fEmb)
	//						{
	//							coarsenCandidate[start] = coarsenCandidate.back() ;
	//							coarsenCandidate.pop_back() ;
	//						}
	//						else
	//							++start ;
	//					}

						fit = map.phi1(fit) ;
					} while (fit != old) ;


					//Loop subdivision
					if (degree == 3)
					{
						map.setCurrentLevel(fLevel) ;
						Dart centerFace = map.phi2(map.phi1(old)) ;
						nbAgents += agentvect[centerFace].size() ;
						coarsenMark.unmark(centerFace) ;
	// TODO Optimisation déjà faite
	//					unsigned int start = it + 1;
	//					unsigned int fEmb = map.getEmbedding<FACE>(centerFace) ;
	//					while(start < coarsenCandidate.size())
	//					{
	//						if(map.getEmbedding<FACE>(coarsenCandidate[start]) == fEmb)
	//						{
	//							coarsenCandidate[start] = coarsenCandidate.back() ;
	//							coarsenCandidate.pop_back() ;
	//						}
	//						else
	//							++start ;
	//					}
						map.setCurrentLevel(fLevel - 1) ;
					}
					if (nbAgents < nbAgentsToSimplify) checkCoarsenCandidate.push_back(old) ;
689
				}
690
				map.setCurrentLevel(cur) ;
691
692
			}
		}
693
		coarsenCandidate.clear() ;
694

695
696
//		 On réalise la simplification (les conditions ont déjà été vérifiées)
		for (unsigned int it = 0; it < checkCoarsenCandidate.size(); ++it)
697
		{
698
			Dart old = checkCoarsenCandidate[it] ;
699

700
701
702
			unsigned int fLevel = map.faceLevel(old) ;
			unsigned int cur = map.getCurrentLevel() ;
			map.setCurrentLevel(fLevel - 1) ;
703

704
705
706
707
			// on compte le degré de la face grossière
			unsigned int degree = 0 ;
			Dart fit = old ;
			do
708
			{
709
710
711
712
713
714
715
716
717
718
719
720
721
				++degree ;
				fit = map.phi1(fit) ;
			} while (fit != old) ;

			PFP::AGENTS agents ;
			PFP::OBSTACLEVECT obst ;
			PFP::OBSTACLEVECT neighborObst ;
			//premier tour pour les présents
			fit = old ;
			do
			{
				PFP::AGENTS a(agentvect[fit]) ;
				PFP::OBSTACLEVECT ob(obstvect[fit]) ;
722
723


724
				agents.insert(agents.end(), a.begin(), a.end()) ;
725

726
727
728

				map.setCurrentLevel(map.getMaxLevel()) ;
				for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
729
				{
730
731
					register_pop(*ait, (*ait)->index) ;
					obst.push_back(*ait);
732
				}
733
734
735
736
737
738
739
740



				for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
					popAgentInCells(*ait, fit) ;
				map.setCurrentLevel(fLevel - 1) ;
				Dart nf = map.phi2(fit) ;
				if (!map.faceIsSubdivided(nf))
741
				{
742
743
744
745
					map.setCurrentLevel(fLevel) ;
					PFP::AGENTS& an = agentvect[nf] ;
					PFP::OBSTACLEVECT resetob = obstvect[nf] ;
					for (PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
746
					{
747
748
749
750
751
752
753
754
755
						if ((*ait)->part_.d == map.phi1(nf)) (*ait)->part_.d = nf ;
					}
					for (PFP::OBSTACLEVECT::iterator ait = resetob.begin(); ait != resetob.end(); ++ait)
					{
						MovingObstacle* mo = (*ait)->mo ;
						if (mo != NULL)
						{
							resetPart(mo,nf) ;
						}
756
					}
757
					map.setCurrentLevel(fLevel - 1) ;
758
				}
759
760
				fit = map.phi1(fit) ;
			} while (fit != old) ;
761

762
			if (degree == 3)
763
			{
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
				map.setCurrentLevel(fLevel) ;
				Dart centerFace = map.phi2(map.phi1(old)) ;
				PFP::AGENTS a(agentvect[centerFace]) ;
				PFP::OBSTACLEVECT ob(obstvect[centerFace]) ;

				agents.insert(agents.end(), a.begin(), a.end()) ;
				map.setCurrentLevel(map.getMaxLevel()) ;
				for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
				{
					register_pop(*ait, (*ait)->index) ;
					obst.push_back(*ait);
				}
				for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
					popAgentInCells(*ait, centerFace) ;
				map.setCurrentLevel(fLevel - 1) ;
779
780
			}

781
782
783
784
			//deuxieme tour concernant les voisins
			fit = old ;
			do
			{
785

786
				PFP::OBSTACLEVECT nob(neighborObstvect[fit]) ;
787

788
789
790
791
792
793
794
				map.setCurrentLevel(map.getMaxLevel()) ;
				for(PFP::OBSTACLEVECT::iterator ait = nob.begin(); ait != nob.end(); ++ait)
				{
					register_pop(*ait, (*ait)->index) ;
					neighborObst.push_back(*ait);
				}
				map.setCurrentLevel(fLevel - 1) ;
795

796
797
				fit = map.phi1(fit) ;
			} while (fit != old) ;
798

799
			if (degree == 3)
800
			{
801
802
803
804
805
806
807
808
809
810
				map.setCurrentLevel(fLevel) ;
				Dart centerFace = map.phi2(map.phi1(old)) ;
				PFP::OBSTACLEVECT nob(neighborObstvect[centerFace]) ;
				map.setCurrentLevel(map.getMaxLevel()) ;
				for(PFP::OBSTACLEVECT::iterator ait = nob.begin(); ait != nob.end(); ++ait)
				{
					register_pop(*ait, (*ait)->index) ;
					neighborObst.push_back(*ait);
				}
				map.setCurrentLevel(fLevel - 1) ;
811
812
			}

813
			neighborAgentvect[old].clear() ;
814

815
			// TODO Check with optimisation
816

817
818
819
820
821
822
823
824
825
826
	//
	//		CGoGNout<<"Avant pop"<<CGoGNendl;
	//		CGoGNout<<"Obst dans la case : ";
	//		for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
	//			CGoGNout<<(*ait)->index<<" ; ";
	//		CGoGNout<<CGoGNendl;
	//		CGoGNout<<"Obst voisins : ";
	//		for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
	//			CGoGNout<<(*ait)->index<<" ; ";
	//		CGoGNout<<CGoGNendl;
827
828


829
			map.setCurrentLevel(fLevel - 1) ;
830

831
			Algo::IHM::coarsenFace<PFP>(map, old, position) ;
832

833
834
835
			std::pair<bool, bool>& sf = subdivisableFace[old] ;
			sf.first = true ;
			sf.second = true ;
836

837
			map.setCurrentLevel(map.getMaxLevel()) ;
838

839
840
841
842
843
844
845
846
847
848
849
850
851
852
			for (PFP::AGENTS::iterator itA = agents.begin(); itA != agents.end(); ++itA)
			{
				(*itA)->part_.d = old ;
				pushAgentInCells(*itA, old) ;
			}
			for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
			{
				resetObstPartInFace(*ait, old) ;
				update_registration(*ait) ;
			}
			for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
			{
				update_registration(*ait) ;
			}
853

854
855
856
857
858
859
860
861
862
863
864
865
866
			Dart dd = old ;
			do
			{
				Dart ddd = map.alpha1(map.alpha1(dd)) ;
				while (ddd != dd)
				{
					neighborAgentvect[old].insert(neighborAgentvect[old].end(), agentvect[ddd].begin(), agentvect[ddd].end()) ;
					ddd = map.alpha1(ddd) ;
				}
				dd = map.phi1(dd) ;
			} while (dd != old) ;

			if (fLevel > 1 && !coarsenMark.isMarked(old) && agentvect[old].size() < nbAgentsToSimplify)
867
			{
868
869
				coarsenMark.mark(old) ;
				coarsenCandidate.push_back(map.faceOldestDart(old)) ;
870
871
			}

872
			map.setCurrentLevel(cur) ;
873
		}
874
875
876
877
878
879
880
881
882
883
		map.setCurrentLevel(map.getMaxLevel()) ;
		if (coarsenCandidate.size() > 0) updateMap() ;
}
void EnvMap::updateMap()
{
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

	refine();
	coarse();

884

Pierre Kraemer's avatar
Pierre Kraemer committed
885
886
887
888
}

void EnvMap::resetAgentInFace(Agent* agent)
{
pitiot's avatar
maj    
pitiot committed
889
890
891
	VEC3 pos = agent->part_.getPosition() ;
	agent->part_.move(Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position)) ;
	agent->part_.setState(FACE) ;
892
	agent->part_.move(pos) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
893
}
Pierre Kraemer's avatar
Pierre Kraemer committed
894
#endif
pitiot's avatar
maj    
pitiot committed
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981

#ifdef SPATIAL_HASHING
Geom::Vec2ui EnvMap::agentPositionCell(Agent* a)
{
	VEC3 relativePos = a->pos - geometry.min() ;
	relativePos /= minCellSize ;
	return Geom::Vec2ui(relativePos[0], relativePos[1]) ;
}

const std::vector<Agent*>& EnvMap::getNeighbors(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	return ht_agents[c] ;
}

const std::vector<Agent*>& EnvMap::getNeighborAgents(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	return ht_neighbor_agents[c] ;
}

void EnvMap::getOneRingNeighbors(Agent* a, std::vector<Agent*>& neighbors)
{
	Geom::Vec2ui c = agentPositionCell(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) ;
				const std::vector<Agent*>& v = ht_agents[cc] ;
				neighbors.insert(neighbors.end(), v.begin(), v.end()) ;
			}
		}
	}
}

void EnvMap::addAgentInGrid(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	addAgentInGrid(a,c) ;
}

void EnvMap::addAgentInGrid(Agent* a, Geom::Vec2ui c)
{
	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) ;
				ht_neighbor_agents[cc].push_back(a) ;
			}
		}
	}
}

void EnvMap::removeAgentFromGrid(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	removeAgentFromGrid(a,c) ;
}

void EnvMap::removeAgentFromGrid(Agent* a, Geom::Vec2ui c)
{
	removeElementFromVector<Agent*>(ht_agents[c], a) ;
	if (ht_agents[c].empty()) ht_agents.erase(c) ;

	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) ;
				removeElementFromVector<Agent*>(ht_neighbor_agents[cc], a) ;
				if (ht_neighbor_agents[c].empty()) ht_neighbor_agents.erase(c) ;
			}
		}
	}
}

#endif