env_map.cpp 27.9 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_),
David Cazier's avatar
David Cazier committed
19
20
21
22
23
	obstacleMarkS(mapScenary),
	buildingMarkS(mapScenary),
	obstacleMark(map),
	buildingMark(map),
	pedWayMark(map),
Pierre Kraemer's avatar
Pierre Kraemer committed
24
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
25
26
	refineMark(map),
	coarsenMark(map)
Pierre Kraemer's avatar
Pierre Kraemer committed
27
#else
David Cazier's avatar
David Cazier committed
28
29
30
	ht_agents(1024),
	ht_neighbor_agents(1024),
	ht_obstacles(1024)
Pierre Kraemer's avatar
Pierre Kraemer committed
31
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
32
{
David Cazier's avatar
David Cazier committed
33
34
	position = map.addAttribute<VEC3, VERTEX>("position") ;
	normal = map.addAttribute<VEC3, VERTEX>("normal") ;
Pierre Kraemer's avatar
Pierre Kraemer committed
35
36

#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
37
38
39
40
41
	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
42

43
44
45
46
47
48
49
50
	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
51
	VEC3 bottomLeft(-width / 2, -height / 2, 0.0f) ;
52
53
54
55
56
57
58
59
60
61
	VEC3 topRight(width / 2, height / 2, 0.0f) ;

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

David Cazier's avatar
David Cazier committed
65
66
	switch (config)
	{
David Cazier's avatar
David Cazier committed
67
68
69
70
71
72
73
74
75
76
		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
77
78
79
80
81
82
83
84
85
86
87
		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) ;
88

89
90
91
92
//			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
93
		}
Jund Thomas's avatar
Jund Thomas committed
94
		break;
95
96
97
98
99
100
101
102
103
	}

//	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
104
105
	// TODO Check registerWallInFaces();
	registerWallInFaces() ;
106
107
//	subdivideAllToMaxLevel();

108
	for (unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
109
		subdivisableFace[i].first = false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
110
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
111
112
}

113
114
void EnvMap::markPedWay()
{
David Cazier's avatar
David Cazier committed
115
	CellMarker<FACE> treat(map) ;
David Cazier's avatar
David Cazier committed
116
117
118
119
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
120
121
122
			treat.mark(d) ;

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

136
				dd = map.phi1(dd) ;
137

138
			} while (dd != d) ;
139
140
141
		}
	}

142
	treat.unmarkAll() ;
David Cazier's avatar
David Cazier committed
143
144
145
146
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
147
148
149
			treat.mark(d) ;

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
150
151
152
153
154
155
			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))))))
				{
156
157
158
					pedWayMark.mark(d) ;
					break ;
				}
159

160
				dd = map.phi1(dd) ;
161

162
			} while (dd != d) ;
163
164
165
166
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
167
168
unsigned int EnvMap::mapMemoryCost()
{
David Cazier's avatar
David Cazier committed
169
170
171
172
	return (map.getAttributeContainer<DART>()).memorySize()
	    + (map.getAttributeContainer<VERTEX>()).memorySize()
	    + (map.getAttributeContainer<EDGE>()).memorySize()
	    + (map.getAttributeContainer<FACE>()).memorySize() ;
Thomas's avatar
Thomas committed
173
174
}

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

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
261
262
Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
David Cazier's avatar
David Cazier committed
263
	CellMarkerStore<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
264
265
266
267
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
268
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
269
270
			if (!map.isBoundaryFace(d) && !buildingMark.isMarked(d)
			    && Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true)) return d ;
271
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
272
273
	}

274
275
	std::cout << "ERROR : pos not in map" << std::endl ;
	return map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
276
}
Pierre Kraemer's avatar
Pierre Kraemer committed
277
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
278

Pierre Kraemer's avatar
Pierre Kraemer committed
279
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
280
281
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
282
	Dart dd = d ;
David Cazier's avatar
David Cazier committed
283
284
	do
	{
285
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
286
287
		while (ddd != dd)
		{
288
289
			f(ddd) ;
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
290
		}
291
292
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
293
294
}

David Cazier's avatar
David Cazier committed
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
// 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
341
342
void EnvMap::registerObstaclesInFaces()
{
David Cazier's avatar
David Cazier committed
343
	CellMarker<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
344
345
346
347
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
348
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
349
350
			if (!buildingMark.isMarked(d))
			{
351
				Dart dd = d ;
352
353
354
355

				//retrieve all obstacles within its one-ring

				//first : test all edges of the face of d
David Cazier's avatar
David Cazier committed
356
357
358
359
				do
				{
					if (obstacleMark.isMarked(dd))
					{
360
361
362
363
						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
364
365
						                           position[previous], position[map.phi1(next)],
						                           NULL, 0) ;
366
						obstvect[d].push_back(o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
367
					}
368
369
					dd = map.phi1(dd) ;
				} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
370

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

void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor)
{
391
	Dart stop ;
David Cazier's avatar
David Cazier committed
392
393
394
395
	if (edgeNeighbor)
		stop = map.phi_1(map.phi_1(map.phi_1(d))) ;
	else
		stop = map.phi_1(map.phi_1(d)) ;
396
397

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

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

428
429
	popAgentInCells(agent, oldFace) ;
	pushAgentInCells(agent, newFace) ;
430

David Cazier's avatar
David Cazier committed
431
432
	if (!refineMark.isMarked(newFace) && agentvect[newFace].size() > nbAgentsToSubdivide)
	{
433
		std::pair<bool, bool>& sf = subdivisableFace[newFace] ;
David Cazier's avatar
David Cazier committed
434
435
		if (sf.first == false || (sf.first == true && sf.second))
		{
436
437
			refineMark.mark(newFace) ;
			refineCandidate.push_back(newFace) ;
438
439
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
440

David Cazier's avatar
David Cazier committed
441
442
443
	if (!coarsenMark.isMarked(oldFace) && !refineMark.isMarked(oldFace)
	    && 4 * agentvect[oldFace].size() < nbAgentsToSimplify)
	{
444
445
		coarsenMark.mark(oldFace) ;
		coarsenCandidate.push_back(map.faceOldestDart(oldFace)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
446
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
447
448
}

Pierre Kraemer's avatar
Pierre Kraemer committed
449
450
void EnvMap::obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace)
{
451
452
	popObstacleInCells(agent, oldFace) ;
	pushObstacleInCells(agent, newFace) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
453
454
}

Pierre Kraemer's avatar
Pierre Kraemer committed
455
456
457
458
void EnvMap::updateMap()
{
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

David Cazier's avatar
David Cazier committed
459
460
461
462
	for (std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end();
	    ++it)
	{
		Dart d = (*it) ;
463
		refineMark.unmark(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
464

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

471
			bool subdivisable = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
472

473
			//check if faces resulting from a subdivision are big enough
474
			std::pair<bool, bool>& sf = subdivisableFace[old] ;
David Cazier's avatar
David Cazier committed
475
476
477
478
			if (sf.first == true)
				subdivisable = sf.second ;
			else
			{
479
				float minSizeSq = minCellSize * minCellSize ; // diametre de vision de l'agent au carré
480

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

Pierre Kraemer's avatar
Pierre Kraemer committed
499
500
				sf.first = true ;
				sf.second = subdivisable ;
501
502
			}

David Cazier's avatar
David Cazier committed
503
504
			if (subdivisable)
			{
505
				if (fLevel == -1) fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
506
507

				sf.first = false ;
508

509
				PFP::AGENTS oldAgents(agentvect[old]) ;
David Cazier's avatar
David Cazier committed
510
511
				PFP::OBSTACLEVECT oldObst(obstvect[old]) ;
				PFP::OBSTACLEVECT oldNeighborObst(neighborObstvect[old]) ;
David Cazier's avatar
David Cazier committed
512
513
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
					popAgentInCells(*ait, old) ;
David Cazier's avatar
David Cazier committed
514
515
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
					register_pop(*ait, (*ait)->index) ;
Jund Thomas's avatar
Jund Thomas committed
516
517

				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin(); ait != oldNeighborObst.end(); ++ait)
David Cazier's avatar
David Cazier committed
518
					register_pop(*ait, (*ait)->index) ;
519
				neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
520
521

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

David Cazier's avatar
David Cazier committed
533
534
				if (degree == 3)
				{
535
					Dart centerFace = map.phi2(map.phi1(old)) ;
536
					newF.mark(centerFace) ;
537
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
538
				map.setCurrentLevel(map.getMaxLevel()) ;
539

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

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

554
				//same for adjacent obstacles
David Cazier's avatar
David Cazier committed
555
556
557
				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();
				    ait != oldNeighborObst.end(); ++ait)
				{
558

David Cazier's avatar
David Cazier committed
559
560
561
					update_registration(*ait) ;
				}

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

					map.setCurrentLevel(fLevel) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
584
					dd = map.phi1(dd) ;
585
					map.setCurrentLevel(map.getMaxLevel()) ;
586
				} while (dd != old) ;
587

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

pitiot's avatar
pitiot committed
613
614
//	// On recrée une liste des faces à simplifier en empêchant les doublons
//	// On en profite pour vérifier les conditions de simplifications
pitiot's avatar
pitiot committed
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
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
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
//	std::vector<Dart> checkCoarsenCandidate ;
//	checkCoarsenCandidate.reserve(coarsenCandidate.size()) ;
//
//	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) ;
//
//		if (oldIsMarked && fLevel > 0 && map.getDartLevel(old) < fLevel)
//		{
//			unsigned int cur = map.getCurrentLevel() ;
//			map.setCurrentLevel(fLevel - 1) ;
//
//			if (map.faceIsSubdividedOnce(old))
//			{
//				// 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) ;
//			}
//			map.setCurrentLevel(cur) ;
//		}
//	}
//	coarsenCandidate.clear() ;
//
//	// On réalise la simplification (les conditions ont déjà été vérifiées)
//	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 ;
//
//		fit = old ;
//		do
//		{
//			PFP::AGENTS a(agentvect[fit]) ;
//			PFP::OBSTACLEVECT ob(obstvect[fit]) ;
//			PFP::OBSTACLEVECT nob(neighborObstvect[fit]) ;
//
//			agents.insert(agents.end(), a.begin(), a.end()) ;
//			obst.insert(obst.end(), ob.begin(), ob.end()) ;
//			neighborObst.insert(neighborObst.end(), nob.begin(), nob.end()) ;
//			map.setCurrentLevel(map.getMaxLevel()) ;
//			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)
//				{
//					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)
David Cazier's avatar
David Cazier committed
740
//					{
741
//						resetPart(mo,nf) ;
David Cazier's avatar
David Cazier committed
742
//					}
pitiot's avatar
pitiot committed
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
//				}
//				map.setCurrentLevel(fLevel - 1) ;
//			}
//			fit = map.phi1(fit) ;
//		} while (fit != old) ;
//
//		if (degree == 3)
//		{
//			map.setCurrentLevel(fLevel) ;
//			Dart centerFace = map.phi2(map.phi1(old)) ;
//			PFP::AGENTS a(agentvect[centerFace]) ;
//			PFP::OBSTACLEVECT ob(obstvect[centerFace]) ;
//			PFP::OBSTACLEVECT nob(neighborObstvect[centerFace]) ;
//			agents.insert(agents.end(), a.begin(), a.end()) ;
//			obst.insert(obst.end(), ob.begin(), ob.end()) ;
//			neighborObst.insert(neighborObst.end(), nob.begin(), nob.end()) ;
//			map.setCurrentLevel(map.getMaxLevel()) ;
//			for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
//				popAgentInCells(*ait, centerFace) ;
//			map.setCurrentLevel(fLevel - 1) ;
//		}
//		neighborAgentvect[old].clear() ;
765
766
767
//
//		for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
//			removeElementFromVector<Obstacle* >(neighborObst, (*ait));
pitiot's avatar
pitiot committed
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
//		// TODO Check with optimisation
//		map.setCurrentLevel(map.getMaxLevel()) ;
//		for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
//			register_pop(*ait, (*ait)->index) ;
//		for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
//			register_pop(*ait, (*ait)->index) ;
//		map.setCurrentLevel(fLevel - 1) ;
//
//		Algo::IHM::coarsenFace<PFP>(map, old, position) ;
//
//		std::pair<bool, bool>& sf = subdivisableFace[old] ;
//		sf.first = true ;
//		sf.second = true ;
//
//		map.setCurrentLevel(map.getMaxLevel()) ;
//
//		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) ;
//		}
//
//		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)
//		{
//			coarsenMark.mark(old) ;
//			coarsenCandidate.push_back(map.faceOldestDart(old)) ;
//		}
//
//		map.setCurrentLevel(cur) ;
//	}
//	map.setCurrentLevel(map.getMaxLevel()) ;
//	if (coarsenCandidate.size() > 0) updateMap() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
821
822
823
824
}

void EnvMap::resetAgentInFace(Agent* agent)
{
pitiot's avatar
maj    
pitiot committed
825
826
827
	VEC3 pos = agent->part_.getPosition() ;
	agent->part_.move(Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position)) ;
	agent->part_.setState(FACE) ;
828
	agent->part_.move(pos) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
829
}
Pierre Kraemer's avatar
Pierre Kraemer committed
830
#endif
David Cazier's avatar
David Cazier committed
831

pitiot's avatar
maj    
pitiot committed
832
833
834
835
836
837
838
#ifdef SPATIAL_HASHING
Geom::Vec2ui EnvMap::agentPositionCell(Agent* a)
{
	VEC3 relativePos = a->pos - geometry.min() ;
	relativePos /= minCellSize ;
	return Geom::Vec2ui(relativePos[0], relativePos[1]) ;
}
839

pitiot's avatar
maj    
pitiot committed
840
841
842
843
844
const std::vector<Agent*>& EnvMap::getNeighbors(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	return ht_agents[c] ;
}
845

pitiot's avatar
maj    
pitiot committed
846
847
848
849
850
851
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
852
853
854
855
856
857
858
859
860
861
862
863
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
864
865
866
			}
		}
	}
Jund Thomas's avatar
Jund Thomas committed
867
}
Pierre Kraemer's avatar
Pierre Kraemer committed
868

Jund Thomas's avatar
Jund Thomas committed
869
870
871
872
873
void EnvMap::addAgentInGrid(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	addAgentInGrid(a,c) ;
}
Pierre Kraemer's avatar
Pierre Kraemer committed
874

Jund Thomas's avatar
Jund Thomas committed
875
876
877
void EnvMap::addAgentInGrid(Agent* a, Geom::Vec2ui c)
{
	ht_agents[c].push_back(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
878

David Cazier's avatar
David Cazier committed
879
		PFP::AGENTS agents ;
David Cazier's avatar
David Cazier committed
880
881
882
		PFP::OBSTACLEVECT obst ;
		PFP::OBSTACLEVECT neighborObst ;

David Cazier's avatar
David Cazier committed
883
884
885
886
		fit = old ;
		do
		{
			PFP::AGENTS a(agentvect[fit]) ;
David Cazier's avatar
David Cazier committed
887
888
889
			PFP::OBSTACLEVECT ob(obstvect[fit]) ;
			PFP::OBSTACLEVECT nob(neighborObstvect[fit]) ;

David Cazier's avatar
David Cazier committed
890
			agents.insert(agents.end(), a.begin(), a.end()) ;
David Cazier's avatar
David Cazier committed
891
892
			obst.insert(obst.end(), ob.begin(), ob.end()) ;
			neighborObst.insert(neighborObst.end(), nob.begin(), nob.end()) ;
David Cazier's avatar
David Cazier committed
893
894
895
896
897
898
899
900
901
			map.setCurrentLevel(map.getMaxLevel()) ;
			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] ;
David Cazier's avatar
David Cazier committed
902
				PFP::OBSTACLEVECT resetob = obstvect[nf] ;
David Cazier's avatar
David Cazier committed
903
904
905
906
				for (PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
				{
					if ((*ait)->part_.d == map.phi1(nf)) (*ait)->part_.d = nf ;
				}
David Cazier's avatar
David Cazier committed
907
908
909
910
911
912
913
914
				for (PFP::OBSTACLEVECT::iterator ait = resetob.begin(); ait != resetob.end(); ++ait)
				{
					MovingObstacle* mo = (*ait)->mo ;
					if (mo != NULL)
					{
						resetPart(mo, nf) ;
					}
				}
David Cazier's avatar
David Cazier committed
915
916
917
918
				map.setCurrentLevel(fLevel - 1) ;
			}
			fit = map.phi1(fit) ;
		} while (fit != old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
919

David Cazier's avatar
David Cazier committed
920
921
922
923
924
		if (degree == 3)
		{
			map.setCurrentLevel(fLevel) ;
			Dart centerFace = map.phi2(map.phi1(old)) ;
			PFP::AGENTS a(agentvect[centerFace]) ;
David Cazier's avatar
David Cazier committed
925
926
			PFP::OBSTACLEVECT ob(obstvect[centerFace]) ;
			PFP::OBSTACLEVECT nob(neighborObstvect[centerFace]) ;
David Cazier's avatar
David Cazier committed
927
			agents.insert(agents.end(), a.begin(), a.end()) ;
David Cazier's avatar
David Cazier committed
928
929
			obst.insert(obst.end(), ob.begin(), ob.end()) ;
			neighborObst.insert(neighborObst.end(), nob.begin(), nob.end()) ;
David Cazier's avatar
David Cazier committed
930
931
932
933
934
935
			map.setCurrentLevel(map.getMaxLevel()) ;
			for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
				popAgentInCells(*ait, centerFace) ;
			map.setCurrentLevel(fLevel - 1) ;
		}
		neighborAgentvect[old].clear() ;
David Cazier's avatar
David Cazier committed
936
937
938
939
		// TODO Check with optimisation
		map.setCurrentLevel(map.getMaxLevel()) ;
		for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
			register_pop(*ait, (*ait)->index) ;
940
		for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
David Cazier's avatar
David Cazier committed
941
942
			register_pop(*ait, (*ait)->index) ;
		map.setCurrentLevel(fLevel - 1) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
943

David Cazier's avatar
David Cazier committed
944
		Algo::IHM::coarsenFace<PFP>(map, old, position) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
945

David Cazier's avatar
David Cazier committed
946
947
948
949
		std::pair<bool, bool>& sf = subdivisableFace[old] ;
		sf.first = true ;
		sf.second = true ;

Jund Thomas's avatar
Jund Thomas committed
950
951
952
953
954
void EnvMap::removeAgentFromGrid(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	removeAgentFromGrid(a,c) ;
}
David Cazier's avatar
David Cazier committed
955
956


Jund Thomas's avatar
Jund Thomas committed
957
958
959
960
961
962
963
964
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)
David Cazier's avatar
David Cazier committed
965
		{
Jund Thomas's avatar
Jund Thomas committed
966
			if(!(ii == 0 && jj == 0))
David Cazier's avatar
David Cazier committed
967
			{
Jund Thomas's avatar
Jund Thomas committed
968
969
970
				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) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
971
972
			}
		}
David Cazier's avatar
David Cazier committed
973
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
974
}
Pierre Kraemer's avatar
Pierre Kraemer committed
975
#endif
David Cazier's avatar
David Cazier committed
976
977
978
979
980
981
982
983
984

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

David Cazier's avatar
David Cazier committed
985
986
const std::vector<Agent*>& EnvMap::getNeighbors(Agent* a)
{
David Cazier's avatar
David Cazier committed
987
988
989
990
	Geom::Vec2ui c = agentPositionCell(a) ;
	return ht_agents[c] ;
}

David Cazier's avatar
David Cazier committed
991
992
993
994
995
996
997
998
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)
{
David Cazier's avatar
David Cazier committed
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
	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
1014
1015
void EnvMap::addAgentInGrid(Agent* a)
{
David Cazier's avatar
David Cazier committed
1016
1017
1018
1019
	Geom::Vec2ui c = agentPositionCell(a) ;
	addAgentInGrid(a,c) ;
}

David Cazier's avatar
David Cazier committed
1020
1021
void EnvMap::addAgentInGrid(Agent* a, Geom::Vec2ui c)
{
David Cazier's avatar
David Cazier committed
1022
	ht_agents[c].push_back(a) ;
David Cazier's avatar
David Cazier committed
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034

	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) ;
			}
		}
	}
David Cazier's avatar
David Cazier committed
1035
1036
}

David Cazier's avatar
David Cazier committed
1037
1038
void EnvMap::removeAgentFromGrid(Agent* a)
{
David Cazier's avatar
David Cazier committed
1039
1040
1041
1042
	Geom::Vec2ui c = agentPositionCell(a) ;
	removeAgentFromGrid(a,c) ;
}

David Cazier's avatar
David Cazier committed
1043
1044
void EnvMap::removeAgentFromGrid(Agent* a, Geom::Vec2ui c)
{
David Cazier's avatar
David Cazier committed
1045
1046
	removeElementFromVector<Agent*>(ht_agents[c], a) ;
	if (ht_agents[c].empty()) ht_agents.erase(c) ;
David Cazier's avatar
David Cazier committed
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059

	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) ;
			}
		}
	}
David Cazier's avatar
David Cazier committed
1060
1061
1062
}

#endif