env_map.cpp 31.6 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"
5
6
#include "moving_obstacle.h"
#include "simulator.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
7

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

15
16
#include "env_generator.h"

17
using namespace CGoGN ;
Pierre Kraemer's avatar
Pierre Kraemer committed
18

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

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

#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
40
41
42
43
44
	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
45

46
47
48
49
50
51
52
53
	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
54
	VEC3 bottomLeft(-width / 2, -height / 2, 0.0f) ;
55
56
57
58
59
60
61
62
	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
63
	std::cout << " - Geometry size : " << geometry.size(0)<<" x "<<geometry.size(1) ;
64
65
	std::cout << " - Cell size between : " << minSize << " and " << maxSize << std::endl ;
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
66
	std::cout << " - Table de hachage : " << agentGridSize(0) << " x " << agentGridSize(1) << std::endl ;
67
68
#endif

David Cazier's avatar
David Cazier committed
69
70
	switch (config)
	{
David Cazier's avatar
David Cazier committed
71
72
73
74
75
76
77
78
79
80
		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 ;
Jund Thomas's avatar
Jund Thomas committed
81
82
83
84
85
86
87
88
89
90
91
		case 3 :
			CityGenerator::generateCity<PFP>(*this) ;
			break ;
		case 4 :
			CityGenerator::generatePlanet<PFP>(*this) ;
			break ;
		case 5 :
		{
//			std::string filename = "./svg/mapRoads.svg" ;
			std::string filename = "./svg/simpleCross.svg" ;
			Algo::Import::importSVG<PFP>(map, filename, position, obstacleMark, buildingMark) ;
92

93
94
95
96
//			Algo::BooleanOperator::mergeVertices<PFP>(map, position) ;
//			map.closeMap() ;
//			Algo::Modelisation::CatmullClarkSubdivision<PFP>(map, position) ;
//			Algo::Modelisation::computeDual<PFP>(map) ;
David Cazier's avatar
David Cazier committed
97
		}
Jund Thomas's avatar
Jund Thomas committed
98
		break;
99
100
101
102
103
104
105
106
107
	}

//	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();
David Cazier's avatar
David Cazier committed
108
109
	// TODO Check registerWallInFaces();
	registerWallInFaces() ;
110
111
//	subdivideAllToMaxLevel();

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

Pierre Kraemer's avatar
Pierre Kraemer committed
116
117
}

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

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

141
				dd = map.phi1(dd) ;
142

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

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

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
155
156
157
158
159
160
			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))))))
				{
161
162
163
					pedWayMark.mark(d) ;
					break ;
				}
164

165
				dd = map.phi1(dd) ;
166

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

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

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

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

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

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

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

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

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

David Cazier's avatar
David Cazier committed
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
// 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
348
349
void EnvMap::registerObstaclesInFaces()
{
David Cazier's avatar
David Cazier committed
350
	CellMarker<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
351
352
353
354
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
355
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
356
357
			if (!buildingMark.isMarked(d))
			{
358
				Dart dd = d ;
359
360
361
362

				//retrieve all obstacles within its one-ring

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

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

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

	Dart dd = d ;
David Cazier's avatar
David Cazier committed
405
406
407
408
	do
	{
		if (obstacleMark.isMarked(dd))
		{
409
410
411
412
413
414
415
416
417
418
//			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
//			{
419
420
421
422
			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],
David Cazier's avatar
David Cazier committed
423
			                           position[map.phi1(next)], NULL, 0) ;
424
			obst.push_back(o) ;
425
//			}
Pierre Kraemer's avatar
Pierre Kraemer committed
426
		}
427
428
		dd = map.phi1(dd) ;
	} while (dd != stop) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
429
430
}

431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
void EnvMap::pushObstacleInCells(Obstacle * o)// réenregistre l'obstacle en question
{
	MovingObstacle * mo = o->mo;
	if (mo != NULL)
	{
		int n = o->index;
		VEC3 p1 = mo->vertices[n];
		VEC3 p2 = mo->vertices[(n+1)%(mo->nbVertices)];

		Dart d1=NIL;
		Dart d2=NIL;
		std::vector<Dart> memo;
		memo = mo->getMemoCross(p1,p2,d1);
		d2=mo->registering_part->d;

		if(map.sameFace(d1,d2))
		{
			pushObstacleInOneRingCells(o , d1 , n);
		}
		else
		{
			pushObstacleInCells(o, n, memo);
		}
	}
}

void EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistrement
{
	MovingObstacle * mo = o->mo;
	VEC3 p1 = mo->vertices[n];
	VEC3 p2 = mo->vertices[(n+1)%mo->nbVertices];

	Dart d1=NIL;
	Dart d2=NIL;
	std::vector<Dart> memo;

//	bool modif=false;

//	if(p1->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS
//		|| p2->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//	{
		memo = mo->getMemoCross(p1,p2,d1);
		d2=mo->registering_part->d;
//		memo.sort();
//		modif=true;
//	}
//	else if(!map.sameFace(p1->d,p2->d))
//	{
//		memo = getMemoCross(p1,p2->getPosition());
//		memo.sort();
//	 	if(belonging_cells[n] != memo)
//			modif=true;
//	}
//
//	if(modif)
	{
		popObstacleInCells(o, n);

		if(map.sameFace(d1,d2))
		{
			pushObstacleInOneRingCells(o , d1 , n);
		}
		else
		{
			pushObstacleInCells(o, n, memo);
		}
	}
}

void EnvMap::pushObstacleInOneRingCells(Obstacle * o, Dart d, int n)
{
	MovingObstacle * mo = o->mo;

	assert(map.getCurrentLevel() == map.getMaxLevel());

	addElementToVector<Obstacle*>(obstvect[d],o);

	mo->belonging_cells[n].clear();
	mo->belonging_cells[n].push_back(d);
	mo->neighbor_cells[n].clear();

	Dart dd = d;
	do
	{
		Dart ddd = map.alpha1(map.alpha1(dd));
		while(ddd != dd)
		{
			pushObstNeighborInCells(o, ddd);

			mo->neighbor_cells[n].push_back(ddd);
			ddd = map.alpha1(ddd);
		}
		dd = map.phi1(dd);
	} while(dd != d);
}

void EnvMap::pushObstacleInCells(Obstacle* o, int n, const std::vector<Dart>& memo_cross)
{
	if(memo_cross.empty())
	{
		displayMO(o);
	}
	assert(!memo_cross.empty());
	MovingObstacle * mo = o->mo;

	mo->belonging_cells[n].clear();
	mo->belonging_cells[n] = memo_cross;

	for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin();	it != mo->belonging_cells[n].end(); ++it)
	{
		//		 CGoGNout <<"obstacle"<< o->p1 << "new cell : "<< *it << CGoGNendl;
		addElementToVector<Obstacle*>(obstvect[*it],o);
	}

	addObstAsNeighbor(o, mo->belonging_cells[n], &(mo->neighbor_cells[n]));

	for (std::vector<Dart>::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it)
	{
		//		 CGoGNout <<"obstacle"<< o->p1 << "new cell : "<< *it << CGoGNendl;
		pushObstNeighborInCells(o, *it);
	}
}

void EnvMap::popObstacleInCells(Obstacle* o, int n)
{
	MovingObstacle * mo = o->mo;

	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

	if (mo != NULL)
	{
		for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin();	it != mo->belonging_cells[n].end(); ++it)
		{
			removeElementFromVector<Obstacle*>(obstvect[*it], o) ;
		}

		for (std::vector<Dart>::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it)
		{
			removeElementFromVector<Obstacle*>(neighborObstvect[*it], o) ;
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
574
575
void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
576
	Dart newFace = agent->part_.d ;
577

578
579
	popAgentInCells(agent, oldFace) ;
	pushAgentInCells(agent, newFace) ;
580

David Cazier's avatar
David Cazier committed
581
582
	if (!refineMark.isMarked(newFace) && agentvect[newFace].size() > nbAgentsToSubdivide)
	{
583
		std::pair<bool, bool>& sf = subdivisableFace[newFace] ;
David Cazier's avatar
David Cazier committed
584
585
		if (sf.first == false || (sf.first == true && sf.second))
		{
586
587
			refineMark.mark(newFace) ;
			refineCandidate.push_back(newFace) ;
588
589
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
590

David Cazier's avatar
David Cazier committed
591
592
593
	if (!coarsenMark.isMarked(oldFace) && !refineMark.isMarked(oldFace)
	    && 4 * agentvect[oldFace].size() < nbAgentsToSimplify)
	{
594
595
		coarsenMark.mark(oldFace) ;
		coarsenCandidate.push_back(map.faceOldestDart(oldFace)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
596
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
597
598
}

599
void EnvMap::refine()
Pierre Kraemer's avatar
Pierre Kraemer committed
600
{
601
	for (std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end(); ++it)
David Cazier's avatar
David Cazier committed
602
603
	{
		Dart d = (*it) ;
604
		refineMark.unmark(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
605

606
		//if the number of agents is big enough
David Cazier's avatar
David Cazier committed
607
608
		if (agentvect[d].size() > nbAgentsToSubdivide)
		{
Thomas's avatar
Thomas committed
609
			int fLevel = -1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
610
611
			Dart old = map.faceOldestDart(d) ;

612
			bool subdivisable = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
613

614
			//check if faces resulting from a subdivision are big enough
615
			std::pair<bool, bool>& sf = subdivisableFace[old] ;
David Cazier's avatar
David Cazier committed
616
617
618
619
			if (sf.first == true)
				subdivisable = sf.second ;
			else
			{
620
				float minSizeSq = minCellSize * minCellSize ; // diametre de vision de l'agent au carré
621

Pierre Kraemer's avatar
Pierre Kraemer committed
622
				fLevel = map.faceLevel(old) ;
623
624
625
				map.setCurrentLevel(fLevel) ;
				PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
				Dart fd = old ;
David Cazier's avatar
David Cazier committed
626
627
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
628
					PFP::VEC3& p = position[fd] ;
629
					PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
David Cazier's avatar
David Cazier committed
630
631
632
					PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
					if (proj.norm2() < minSizeSq)
					{
633
634
						subdivisable = false ;
						break ;
Pierre Kraemer's avatar
Pierre Kraemer committed
635
					}
636
					fd = map.phi1(fd) ;
637
				} while (fd != old) ;
638
639
				map.setCurrentLevel(map.getMaxLevel()) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
640
641
				sf.first = true ;
				sf.second = subdivisable ;
642
643
			}

David Cazier's avatar
David Cazier committed
644
645
			if (subdivisable)
			{
646
647
				if (fLevel == -1)
					fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
648
649

				sf.first = false ;
650

651
				PFP::AGENTS oldAgents(agentvect[old]) ;
David Cazier's avatar
David Cazier committed
652
653
				PFP::OBSTACLEVECT oldObst(obstvect[old]) ;
				PFP::OBSTACLEVECT oldNeighborObst(neighborObstvect[old]) ;
David Cazier's avatar
David Cazier committed
654
655
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
					popAgentInCells(*ait, old) ;
David Cazier's avatar
David Cazier committed
656
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
657
658
659
					this->popObstacleInCells(*ait, (*ait)->index) ;
				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();ait != oldNeighborObst.end(); ++ait)
					this->popObstacleInCells(*ait, (*ait)->index) ;
660
				neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
661
662

				map.setCurrentLevel(fLevel) ;
663
				Algo::IHM::subdivideFace<PFP>(map, old, position) ;
David Cazier's avatar
David Cazier committed
664
				CellMarkerStore<FACE> newF(map) ;
665
666
				unsigned int degree = 0 ;
				Dart dd = old ;
David Cazier's avatar
David Cazier committed
667
668
				do
				{
669
					++degree ;
670
					newF.mark(dd) ;
671
					dd = map.phi1(dd) ;
672
				} while (dd != old) ;
David Cazier's avatar
David Cazier committed
673

David Cazier's avatar
David Cazier committed
674
675
				if (degree == 3)
				{
676
					Dart centerFace = map.phi2(map.phi1(old)) ;
677
					newF.mark(centerFace) ;
678
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
679
				map.setCurrentLevel(map.getMaxLevel()) ;
680

681
				//agents contained in the subdivided cell are pushed correctly
David Cazier's avatar
David Cazier committed
682
683
684
685
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
				{
					resetAgentInFace(*ait) ;
					pushAgentInCells(*ait, (*ait)->part_.d) ;
686
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
687

688
				//same for obstacles contained
David Cazier's avatar
David Cazier committed
689
690
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
				{
691
					resetPartSubdiv(*ait);
692
					pushObstacleInCells(*ait);
pitiot's avatar
maj    
pitiot committed
693

David Cazier's avatar
David Cazier committed
694
695
				}

696
				//same for adjacent obstacles
David Cazier's avatar
David Cazier committed
697
				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();
698
					ait != oldNeighborObst.end(); ++ait)
David Cazier's avatar
David Cazier committed
699
				{
700
					pushObstacleInCells(*ait) ;
David Cazier's avatar
David Cazier committed
701
702
				}

703
				//retrieve neighbors agents from onering cells
704
				dd = old ;
David Cazier's avatar
David Cazier committed
705
706
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
707
					Dart d3 = dd ;
David Cazier's avatar
David Cazier committed
708
709
					do
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
710
711
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
David Cazier's avatar
David Cazier committed
712
713
714
715
						while (d4 != d3)
						{
							if (!newF.isMarked(d4))
							{
Pierre Kraemer's avatar
Pierre Kraemer committed
716
717
718
719
								PFP::AGENTS& ad4 = agentvect[d4] ;
								nad3.insert(nad3.end(), ad4.begin(), ad4.end()) ;
							}
							d4 = map.alpha1(d4) ;
720
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
721
						d3 = map.phi1(d3) ;
722
					} while (d3 != dd) ;
723
724

					map.setCurrentLevel(fLevel) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
725
					dd = map.phi1(dd) ;
726
					map.setCurrentLevel(map.getMaxLevel()) ;
727
				} while (dd != old) ;
728

David Cazier's avatar
David Cazier committed
729
730
				if (degree == 3)
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
731
732
					Dart centerFace = map.phi2(map.phi1(old)) ;
					Dart d3 = centerFace ;
David Cazier's avatar
David Cazier committed
733
734
					do
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
735
736
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
David Cazier's avatar
David Cazier committed
737
738
739
740
						while (d4 != d3)
						{
							if (!newF.isMarked(d4))
							{
Pierre Kraemer's avatar
Pierre Kraemer committed
741
								PFP::AGENTS& ad4 = agentvect[d4] ;
David Cazier's avatar
David Cazier committed
742
								nad3.insert(neighborAgentvect[d3].end(), ad4.begin(), ad4.end()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
743
744
							}
							d4 = map.alpha1(d4) ;
745
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
746
						d3 = map.phi1(d3) ;
747
					} while (d3 != centerFace) ;
748
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
749
750
751
			}
		}
	}
David Cazier's avatar
David Cazier committed
752
753
	refineCandidate.clear() ;

754
}
755
756


757
758
void EnvMap::coarse()
{
759
	// On recrée une liste des faces à simplifier en empêchant les doublons
760
761
762
	// On en profite pour vérifier les conditions de simplifications
	std::vector<Dart> checkCoarsenCandidate ;
	checkCoarsenCandidate.reserve(coarsenCandidate.size()) ;
763

764
765
766
767
768
769
770
	for (unsigned int it = 0; it < coarsenCandidate.size(); ++it)
	{
		Dart old = coarsenCandidate[it] ;
		bool oldIsMarked = coarsenMark.isMarked(old) ;
		coarsenMark.unmark(old) ;

		unsigned int fLevel = map.faceLevel(old) ;
771

772
773
774
775
		if (oldIsMarked && fLevel > 0 && map.getDartLevel(old) < fLevel)
		{
			unsigned int cur = map.getCurrentLevel() ;
			map.setCurrentLevel(fLevel - 1) ;
776

777
			if (map.faceIsSubdividedOnce(old))
778
			{
779
780
781
782
783
784
785
786
787
788
789
790
791
792
				// 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())
David Cazier's avatar
David Cazier committed
793
//					{
794
795
796
797
798
799
800
//						if(map.getEmbedding<FACE>(coarsenCandidate[start]) == fEmb)
//						{
//							coarsenCandidate[start] = coarsenCandidate.back() ;
//							coarsenCandidate.pop_back() ;
//						}
//						else
//							++start ;
David Cazier's avatar
David Cazier committed
801
//					}
802
803
804
805

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

806

807
808
				//Loop subdivision
				if (degree == 3)
809
				{
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
					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) ;
828
				}
829
				if (nbAgents < nbAgentsToSimplify) checkCoarsenCandidate.push_back(old) ;
830
			}
831
			map.setCurrentLevel(cur) ;
832
		}
833
834
	}
	coarsenCandidate.clear() ;
835

836
//		 On réalise la simplification (les conditions ont déjà été vérifiées)
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
	for (unsigned int it = 0; it < checkCoarsenCandidate.size(); ++it)
	{
		Dart old = checkCoarsenCandidate[it] ;

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

		// on compte le degré de la face grossière
		unsigned int degree = 0 ;
		Dart fit = old ;
		do
		{
			++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
860
		{
861
862
			PFP::AGENTS a(agentvect[fit]) ;
			PFP::OBSTACLEVECT ob(obstvect[fit]) ;
863

864
			agents.insert(agents.end(), a.begin(), a.end()) ;
865

866
867
			map.setCurrentLevel(map.getMaxLevel()) ;
			for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
868
			{
869
870
871
				this->popObstacleInCells(*ait, (*ait)->index) ;
				obst.push_back(*ait);
			}
872
873


874

875
876
877
878
879
880
881
882
883
884
			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))
			{
				map.setCurrentLevel(fLevel) ;
				PFP::AGENTS& an = agentvect[nf] ;
				PFP::OBSTACLEVECT resetob = obstvect[nf] ;
				for (PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
885
				{
886
					if ((*ait)->part_.d == map.phi1(nf)) (*ait)->part_.d = nf ;
887
				}
888
				for (PFP::OBSTACLEVECT::iterator ait = resetob.begin(); ait != resetob.end(); ++ait)
889
				{
890
891
					MovingObstacle* mo = (*ait)->mo ;
					if (mo != NULL)
892
					{
893
						resetPart(mo,nf) ;
894
					}
895
896
				}
				map.setCurrentLevel(fLevel - 1) ;
897
			}
898
899
			fit = map.phi1(fit) ;
		} while (fit != old) ;
900

901
902
903
904
905
906
907
908
909
910
		if (degree == 3)
		{
			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)
911
			{
912
913
914
915
916
917
918
				this->popObstacleInCells(*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) ;
		}
919

920
921
922
923
924
		//deuxieme tour concernant les voisins
		fit = old ;
		do
		{
			PFP::OBSTACLEVECT nob(neighborObstvect[fit]) ;
925

926
927
928
929
930
931
932
			map.setCurrentLevel(map.getMaxLevel()) ;
			for(PFP::OBSTACLEVECT::iterator ait = nob.begin(); ait != nob.end(); ++ait)
			{
				this->popObstacleInCells(*ait, (*ait)->index) ;
				neighborObst.push_back(*ait);
			}
			map.setCurrentLevel(fLevel - 1) ;
933

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

937
938
939
940
941
942
943
		if (degree == 3)
		{
			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)
944
			{
945
946
				this->popObstacleInCells(*ait, (*ait)->index) ;
				neighborObst.push_back(*ait);
947
			}
948
949
			map.setCurrentLevel(fLevel - 1) ;
		}
950

951
		neighborAgentvect[old].clear() ;
952

953
		// TODO Check with optimisation
954

pitiot's avatar
pitiot committed
955
//
956
957
//		CGoGNout<<"Avant pop"<<CGoGNendl;
//		CGoGNout<<"Obst dans la case : ";
pitiot's avatar
pitiot committed
958
//		for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
959
960
961
//			CGoGNout<<(*ait)->index<<" ; ";
//		CGoGNout<<CGoGNendl;
//		CGoGNout<<"Obst voisins : ";
pitiot's avatar
pitiot committed
962
//		for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
963
964
//			CGoGNout<<(*ait)->index<<" ; ";
//		CGoGNout<<CGoGNendl;
965
966


967
		map.setCurrentLevel(fLevel - 1) ;
968

969
		Algo::IHM::coarsenFace<PFP>(map, old, position) ;
970

971
972
973
		std::pair<bool, bool>& sf = subdivisableFace[old] ;
		sf.first = true ;
		sf.second = true ;
974

975
		map.setCurrentLevel(map.getMaxLevel()) ;
976

977
978
979
980
981
982
983
984
985
986
987
988
989
990
		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) ;
			pushObstacleInCells(*ait) ;
		}
		for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
		{
			pushObstacleInCells(*ait) ;
		}
991

992
993
994
995
996
		Dart dd = old ;
		do
		{
			Dart ddd = map.alpha1(map.alpha1(dd)) ;
			while (ddd != dd)
997
			{
998
999
				neighborAgentvect[old].insert(neighborAgentvect[old].end(), agentvect[ddd].begin(), agentvect[ddd].end()) ;
				ddd = map.alpha1(ddd) ;
1000
			}
1001
1002
			dd = map.phi1(dd) ;
		} while (dd != old) ;
1003

1004
1005
1006
1007
		if (fLevel > 1 && !coarsenMark.isMarked(old) && agentvect[old].size() < nbAgentsToSimplify)
		{
			coarsenMark.mark(old) ;
			coarsenCandidate.push_back(map.faceOldestDart(old)) ;
1008
		}
1009
1010
1011
1012
1013

		map.setCurrentLevel(cur) ;
	}
	map.setCurrentLevel(map.getMaxLevel()) ;
	if (coarsenCandidate.size() > 0) updateMap() ;
1014
1015
1016
1017
1018
1019
1020
}
void EnvMap::updateMap()
{
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

	refine();
	coarse();
Pierre Kraemer's avatar
Pierre Kraemer committed
1021
1022
1023
1024
}

void EnvMap::resetAgentInFace(Agent* agent)
{
pitiot's avatar
maj    
pitiot committed
1025
1026
1027
	VEC3 pos = agent->part_.getPosition() ;
	agent->part_.move(Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position)) ;
	agent->part_.setState(FACE) ;
1028
	agent->part_.move(pos) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1029
}
Pierre Kraemer's avatar
Pierre Kraemer committed
1030
#endif
David Cazier's avatar
David Cazier committed
1031

pitiot's avatar
maj    
pitiot committed
1032
1033
1034
1035
1036
1037
1038
#ifdef SPATIAL_HASHING
Geom::Vec2ui EnvMap::agentPositionCell(Agent* a)
{
	VEC3 relativePos = a->pos - geometry.min() ;
	relativePos /= minCellSize ;
	return Geom::Vec2ui(relativePos[0], relativePos[1]) ;
}
1039

pitiot's avatar
maj    
pitiot committed
1040
1041
1042
1043
1044
const std::vector<Agent*>& EnvMap::getNeighbors(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	return ht_agents[c] ;
}
1045

pitiot's avatar
maj    
pitiot committed
1046
1047
1048
1049
1050
1051
const std::vector<Agent*>& EnvMap::getNeighborAgents(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	return ht_neighbor_agents[c] ;
}

Jund Thomas's avatar
Jund Thomas committed
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
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()) ;
David Cazier's avatar
David Cazier committed
1064
1065
1066
			}
		}
	}
Jund Thomas's avatar
Jund Thomas committed
1067
}
Pierre Kraemer's avatar
Pierre Kraemer committed
1068

Jund Thomas's avatar
Jund Thomas committed
1069
1070
1071
1072
1073
void EnvMap::addAgentInGrid(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	addAgentInGrid(a,c) ;
}
Pierre Kraemer's avatar
Pierre Kraemer committed
1074

Jund Thomas's avatar
Jund Thomas committed
1075
1076
1077
void EnvMap::addAgentInGrid(Agent* a, Geom::Vec2ui c)
{
	ht_agents[c].push_back(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1078

David Cazier's avatar
David Cazier committed
1079
		PFP::AGENTS agents ;
David Cazier's avatar