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