env_map.cpp 28.3 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
77
		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 : {
78
79
80
81
82
83
84
//			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) ;
85

86
87
88
89
//			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
90
91
92
93
		}
			CityGenerator::generateCity<PFP>(*this) ;
			break ;
		case 4 :
David Cazier's avatar
David Cazier committed
94
			CityGenerator::generatePlanet<PFP>(*this) ;
David Cazier's avatar
David Cazier committed
95
			break ;
96
97
98
99
100
101
102
103
104
	}

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

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

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

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

137
				dd = map.phi1(dd) ;
138

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

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

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

161
				dd = map.phi1(dd) ;
162

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

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

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

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

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

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

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

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

David Cazier's avatar
David Cazier committed
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
// 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
342
343
void EnvMap::registerObstaclesInFaces()
{
David Cazier's avatar
David Cazier committed
344
	CellMarker<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
345
346
347
348
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
349
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
350
351
			if (!buildingMark.isMarked(d))
			{
352
				Dart dd = d ;
353
354
355
356

				//retrieve all obstacles within its one-ring

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

				sf.first = false ;
509

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

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

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

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

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

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

555
				//same for adjacent obstacles
David Cazier's avatar
David Cazier committed
556
557
558
559
560
561
562
				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();
				    ait != oldNeighborObst.end(); ++ait)
				{
					resetObstInFace(*ait) ;
					update_registration(*ait) ;
				}

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

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

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

	// 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
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
740
//	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
741
//					{
pitiot's avatar
pitiot committed
742
//						resetPart(mo,(*ait)->index, nf) ;
David Cazier's avatar
David Cazier committed
743
//					}
pitiot's avatar
pitiot committed
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
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
821
822
//				}
//				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) ;
//		}
//		for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
//		{
//			removeElementFromVector<Obstacle*>(neighborObst, (*ait)) ;
//		}
//		neighborAgentvect[old].clear() ;
//		// 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
823
824
825
826
}

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

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

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

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

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

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

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

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

David Cazier's avatar
David Cazier committed
892
			agents.insert(agents.end(), a.begin(), a.end()) ;
David Cazier's avatar
David Cazier committed
893
894
			obst.insert(obst.end(), ob.begin(), ob.end()) ;
			neighborObst.insert(neighborObst.end(), nob.begin(), nob.end()) ;
David Cazier's avatar
David Cazier committed
895
896
897
898
899
900
901
902
903
			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
904
				PFP::OBSTACLEVECT resetob = obstvect[nf] ;
David Cazier's avatar
David Cazier committed
905
906
907
908
				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
909
910
911
912
913
914
915
916
				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
917
918
919
920
				map.setCurrentLevel(fLevel - 1) ;
			}
			fit = map.phi1(fit) ;
		} while (fit != old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
921

David Cazier's avatar
David Cazier committed
922
923
924
925
926
		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
927
928
			PFP::OBSTACLEVECT ob(obstvect[centerFace]) ;
			PFP::OBSTACLEVECT nob(neighborObstvect[centerFace]) ;
David Cazier's avatar
David Cazier committed
929
			agents.insert(agents.end(), a.begin(), a.end()) ;
David Cazier's avatar
David Cazier committed
930
931
			obst.insert(obst.end(), ob.begin(), ob.end()) ;
			neighborObst.insert(neighborObst.end(), nob.begin(), nob.end()) ;
David Cazier's avatar
David Cazier committed
932
933
934
935
936
937
			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
938
939
940
941
		// TODO Check with optimisation
		map.setCurrentLevel(map.getMaxLevel()) ;
		for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
			register_pop(*ait, (*ait)->index) ;
942
		for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
David Cazier's avatar
David Cazier committed
943
944
			register_pop(*ait, (*ait)->index) ;
		map.setCurrentLevel(fLevel - 1) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
945

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

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

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


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

#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
987
988
const std::vector<Agent*>& EnvMap::getNeighbors(Agent* a)
{
David Cazier's avatar
David Cazier committed
989
990
991
992
	Geom::Vec2ui c = agentPositionCell(a) ;
	return ht_agents[c] ;
}

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

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

	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
1037
1038
}

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

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

	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
1062
1063
1064
}

#endif