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

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

13 14
#include "env_generator.h"

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

Pierre Kraemer's avatar
Pierre Kraemer committed
17
EnvMap::EnvMap() :
David Cazier's avatar
David Cazier committed
18
	obstacleDistance(Agent::range_),
pitiot's avatar
maj  
pitiot committed
19 20 21 22 23
	obstacleMarkS(mapScenary),
	buildingMarkS(mapScenary),
	obstacleMark(map),
	buildingMark(map),
	pedWayMark(map),
Pierre Kraemer's avatar
Pierre Kraemer committed
24
#ifndef SPATIAL_HASHING
pitiot's avatar
maj  
pitiot committed
25 26
	refineMark(map),
	coarsenMark(map)
Pierre Kraemer's avatar
Pierre Kraemer committed
27 28 29 30 31
#else
	ht_agents(1024),
	ht_neighbor_agents(1024),
	ht_obstacles(1024)
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
32
{
pitiot's avatar
maj  
pitiot 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
pitiot's avatar
maj  
pitiot 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)
	{
pitiot's avatar
maj  
pitiot 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) ;
pitiot's avatar
maj  
pitiot committed
85

86 87 88 89
//			Algo::BooleanOperator::mergeVertices<PFP>(map, position) ;
//			map.closeMap() ;
//			Algo::Modelisation::CatmullClarkSubdivision<PFP>(map, position) ;
//			Algo::Modelisation::computeDual<PFP>(map) ;
pitiot's avatar
maj  
pitiot committed
90 91 92 93 94 95
		}
			CityGenerator::generateCity<PFP>(*this) ;
			break ;
		case 4 :
			CityGenerator::generatePlanet<PFP>(*this) ;
			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();
pitiot's avatar
maj  
pitiot committed
105 106
	// TODO Check registerWallInFaces();
	registerWallInFaces() ;
107 108
//	subdivideAllToMaxLevel();

pitiot's avatar
maj  
pitiot committed
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()
{
pitiot's avatar
maj  
pitiot 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()
{
pitiot's avatar
maj  
pitiot 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
		{
pitiot's avatar
maj  
pitiot 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
		{
pitiot's avatar
maj  
pitiot 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)
{
pitiot's avatar
maj  
pitiot 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
}

pitiot's avatar
maj  
pitiot 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()
{
pitiot's avatar
maj  
pitiot 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],
pitiot's avatar
maj  
pitiot 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)
					{
pitiot's avatar
maj  
pitiot 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],
pitiot's avatar
maj  
pitiot 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

pitiot's avatar
maj  
pitiot committed
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

pitiot's avatar
maj  
pitiot committed
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]) ;
pitiot's avatar
maj  
pitiot 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) ;
pitiot's avatar
maj  
pitiot committed
515 516 517 518 519
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
					register_pop(*ait, (*ait)->index) ;
				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();
				    ait != oldNeighborObst.end(); ++ait)
					register_pop(*ait, (*ait)->index) ;
520
				neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
521 522

				map.setCurrentLevel(fLevel) ;
523
				Algo::IHM::subdivideFace<PFP>(map, old, position) ;
pitiot's avatar
maj  
pitiot 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) ;
pitiot's avatar
maj  
pitiot 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

pitiot's avatar
maj  
pitiot committed
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

pitiot's avatar
maj  
pitiot committed
548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563
				//same for obstacles contained
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
				{
					resetObstInFace(*ait) ;
					update_registration(*ait) ;
				}

				//same for adjacent obstacles
				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();
				    ait != oldNeighborObst.end(); ++ait)
				{
					resetObstInFace(*ait) ;
					update_registration(*ait) ;
				}

				//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 616 617
	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
	std::vector<Dart> checkCoarsenCandidate ;
	checkCoarsenCandidate.reserve(coarsenCandidate.size()) ;
pitiot's avatar
maj  
pitiot committed
618
	
David Cazier's avatar
David Cazier committed
619 620
	for (unsigned int it = 0; it < coarsenCandidate.size(); ++it)
	{
Thomas's avatar
Thomas committed
621
		Dart old = coarsenCandidate[it] ;
David Cazier's avatar
David Cazier committed
622
		bool oldIsMarked = coarsenMark.isMarked(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
623
		coarsenMark.unmark(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
624

Pierre Kraemer's avatar
Pierre Kraemer committed
625
		unsigned int fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
626

David Cazier's avatar
David Cazier committed
627 628
		if (oldIsMarked && fLevel > 0 && map.getDartLevel(old) < fLevel)
		{
Pierre Kraemer's avatar
Pierre Kraemer committed
629 630 631
			unsigned int cur = map.getCurrentLevel() ;
			map.setCurrentLevel(fLevel - 1) ;

David Cazier's avatar
David Cazier committed
632 633
			if (map.faceIsSubdividedOnce(old))
			{
Pierre Kraemer's avatar
Pierre Kraemer committed
634 635
				// on compte le nombre d'agents dans les sous-faces
				// on en profite pour compter le degré de la face grossière
Pierre Kraemer's avatar
Pierre Kraemer committed
636 637 638
				unsigned int degree = 0 ;
				unsigned int nbAgents = 0 ;
				Dart fit = old ;
David Cazier's avatar
David Cazier committed
639 640
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
641
					nbAgents += agentvect[fit].size() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
642
					++degree ;
Pierre Kraemer's avatar
Pierre Kraemer committed
643
					coarsenMark.unmark(fit) ;
pitiot's avatar
maj  
pitiot committed
644 645 646 647 648 649 650 651 652 653 654 655 656 657
// 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 ;
//					}

Pierre Kraemer's avatar
Pierre Kraemer committed
658
					fit = map.phi1(fit) ;
659 660
				} while (fit != old) ;

pitiot's avatar
maj  
pitiot committed
661 662

				//Loop subdivision
David Cazier's avatar
David Cazier committed
663 664
				if (degree == 3)
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
665 666 667
					map.setCurrentLevel(fLevel) ;
					Dart centerFace = map.phi2(map.phi1(old)) ;
					nbAgents += agentvect[centerFace].size() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
668
					coarsenMark.unmark(centerFace) ;
pitiot's avatar
maj  
pitiot committed
669 670 671 672 673 674 675 676 677 678 679 680 681
// 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 ;
//					}					
Pierre Kraemer's avatar
Pierre Kraemer committed
682 683
					map.setCurrentLevel(fLevel - 1) ;
				}
David Cazier's avatar
David Cazier committed
684 685 686 687 688 689
				if (nbAgents < nbAgentsToSimplify) checkCoarsenCandidate.push_back(old) ;
			}
			map.setCurrentLevel(cur) ;
		}
	}
	coarsenCandidate.clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
690

David Cazier's avatar
David Cazier committed
691 692 693 694
	// 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] ;
Pierre Kraemer's avatar
Pierre Kraemer committed
695

David Cazier's avatar
David Cazier committed
696 697 698
		unsigned int fLevel = map.faceLevel(old) ;
		unsigned int cur = map.getCurrentLevel() ;
		map.setCurrentLevel(fLevel - 1) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
699

David Cazier's avatar
David Cazier committed
700 701 702 703 704 705 706 707
		// 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) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
708

David Cazier's avatar
David Cazier committed
709
		PFP::AGENTS agents ;
pitiot's avatar
maj  
pitiot committed
710 711 712
		PFP::OBSTACLEVECT obst ;
		PFP::OBSTACLEVECT neighborObst ;

David Cazier's avatar
David Cazier committed
713 714 715 716
		fit = old ;
		do
		{
			PFP::AGENTS a(agentvect[fit]) ;
pitiot's avatar
maj  
pitiot committed
717 718 719
			PFP::OBSTACLEVECT ob(obstvect[fit]) ;
			PFP::OBSTACLEVECT nob(neighborObstvect[fit]) ;

David Cazier's avatar
David Cazier committed
720
			agents.insert(agents.end(), a.begin(), a.end()) ;
pitiot's avatar
maj  
pitiot committed
721 722
			obst.insert(obst.end(), ob.begin(), ob.end()) ;
			neighborObst.insert(neighborObst.end(), nob.begin(), nob.end()) ;
David Cazier's avatar
David Cazier committed
723 724 725 726 727 728 729 730 731
			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] ;
pitiot's avatar
maj  
pitiot committed
732
				PFP::OBSTACLEVECT resetob = obstvect[nf] ;
David Cazier's avatar
David Cazier committed
733 734 735 736
				for (PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
				{
					if ((*ait)->part_.d == map.phi1(nf)) (*ait)->part_.d = nf ;
				}
pitiot's avatar
maj  
pitiot committed
737 738 739 740 741 742 743 744
				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
745 746 747 748
				map.setCurrentLevel(fLevel - 1) ;
			}
			fit = map.phi1(fit) ;
		} while (fit != old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
749

David Cazier's avatar
David Cazier committed
750 751 752 753 754
		if (degree == 3)
		{
			map.setCurrentLevel(fLevel) ;
			Dart centerFace = map.phi2(map.phi1(old)) ;
			PFP::AGENTS a(agentvect[centerFace]) ;
pitiot's avatar
maj  
pitiot committed
755 756
			PFP::OBSTACLEVECT ob(obstvect[centerFace]) ;
			PFP::OBSTACLEVECT nob(neighborObstvect[centerFace]) ;
David Cazier's avatar
David Cazier committed
757
			agents.insert(agents.end(), a.begin(), a.end()) ;
pitiot's avatar
maj  
pitiot committed
758 759
			obst.insert(obst.end(), ob.begin(), ob.end()) ;
			neighborObst.insert(neighborObst.end(), nob.begin(), nob.end()) ;
David Cazier's avatar
David Cazier committed
760 761 762 763 764 765
			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() ;
pitiot's avatar
maj  
pitiot committed
766 767 768 769 770 771 772
		// 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) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
773

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

David Cazier's avatar
David Cazier committed
776 777 778 779 780 781 782 783 784 785 786
		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) ;
		}
pitiot's avatar
maj  
pitiot committed
787 788 789 790 791 792 793 794 795
		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) ;
		}
David Cazier's avatar
David Cazier committed
796 797 798 799 800 801 802

		Dart dd = old ;
		do
		{
			Dart ddd = map.alpha1(map.alpha1(dd)) ;
			while (ddd != dd)
			{
pitiot's avatar
maj  
pitiot committed
803
				neighborAgentvect[old].insert(neighborAgentvect[old].end(), agentvect[ddd].begin(), agentvect[ddd].end()) ;
David Cazier's avatar
David Cazier committed
804
				ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
805
			}
David Cazier's avatar
David Cazier committed
806 807 808
			dd = map.phi1(dd) ;
		} while (dd != old) ;

pitiot's avatar
maj  
pitiot committed
809
		if (fLevel > 1 && !coarsenMark.isMarked(old) && agentvect[old].size() < nbAgentsToSimplify)
David Cazier's avatar
David Cazier committed
810 811 812
		{
			coarsenMark.mark(old) ;
			coarsenCandidate.push_back(map.faceOldestDart(old)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
813
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
814

David Cazier's avatar
David Cazier committed
815 816
		map.setCurrentLevel(cur) ;
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
817
	map.setCurrentLevel(map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
818
	if (coarsenCandidate.size() > 0) updateMap() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
819 820 821 822
}

void EnvMap::resetAgentInFace(Agent* agent)
{
pitiot's avatar
maj  
pitiot committed
823 824 825
	VEC3 pos = agent->part_.getPosition() ;
	agent->part_.move(Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position)) ;
	agent->part_.setState(FACE) ;
826
	agent->part_.move(pos) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
827
}
Pierre Kraemer's avatar
Pierre Kraemer committed
828
#endif
pitiot's avatar
maj  
pitiot committed
829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915

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

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

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

void EnvMap::getOneRingNeighbors(Agent* a, std::vector<Agent*>& neighbors)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	for(int ii = -1 ; ii <= 1 ; ++ii)
	{
		for(int jj = -1 ; jj <= 1 ; ++jj)
		{
			if (ii != 0 || jj != 0)
			{
				Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
				const std::vector<Agent*>& v = ht_agents[cc] ;
				neighbors.insert(neighbors.end(), v.begin(), v.end()) ;
			}
		}
	}
}

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

void EnvMap::addAgentInGrid(Agent* a, Geom::Vec2ui c)
{
	ht_agents[c].push_back(a) ;

	for(int ii = -1; ii <= 1; ++ii)
	{
		for(int jj = -1; jj <= 1; ++jj)
		{
			if(!(ii == 0 && jj == 0))
			{
				Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
				ht_neighbor_agents[cc].push_back(a) ;
			}
		}
	}
}

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

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

	for(int ii = -1; ii <= 1; ++ii)
	{
		for(int jj = -1; jj <= 1; ++jj)
		{
			if(!(ii == 0 && jj == 0))
			{
				Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
				removeElementFromVector<Agent*>(ht_neighbor_agents[cc], a) ;
				if (ht_neighbor_agents[c].empty()) ht_neighbor_agents.erase(c) ;
			}
		}
	}
}

#endif