env_map.cpp 24.6 KB
Newer Older
Pierre Kraemer's avatar
Pierre Kraemer committed
1 2 3
#include "env_map.h"
#include "utils.h"
#include "agent.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
4
#include "obstacle.h"
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),
24
	memo_mark(map),
Pierre Kraemer's avatar
Pierre Kraemer committed
25
#ifndef SPATIAL_HASHING
pitiot's avatar
maj  
pitiot committed
26 27
	refineMark(map),
	coarsenMark(map)
Pierre Kraemer's avatar
Pierre Kraemer committed
28 29 30 31 32
#else
	ht_agents(1024),
	ht_neighbor_agents(1024),
	ht_obstacles(1024)
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
33
{
pitiot's avatar
maj  
pitiot committed
34 35
	position = map.addAttribute<VEC3, VERTEX>("position") ;
	normal = map.addAttribute<VEC3, VERTEX>("normal") ;
Pierre Kraemer's avatar
Pierre Kraemer committed
36 37

#ifndef SPATIAL_HASHING
pitiot's avatar
maj  
pitiot committed
38 39 40 41 42
	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
43

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

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

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

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

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
114 115
}

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

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

139
				dd = map.phi1(dd) ;
140

141
			} while (dd != d) ;
142 143 144
		}
	}

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

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

163
				dd = map.phi1(dd) ;
164

165
			} while (dd != d) ;
166 167 168 169
		}
	}
}

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

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

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

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

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

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

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

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

				//retrieve all obstacles within its one-ring

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

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

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

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

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

431 432
	popAgentInCells(agent, oldFace) ;
	pushAgentInCells(agent, newFace) ;
433

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

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

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

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

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

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

474
			bool subdivisable = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
475

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

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

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

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

				sf.first = false ;
511

512
				PFP::AGENTS oldAgents(agentvect[old]) ;
pitiot's avatar
maj  
pitiot committed
513 514
				PFP::OBSTACLEVECT oldObst(obstvect[old]) ;
				PFP::OBSTACLEVECT oldNeighborObst(neighborObstvect[old]) ;
David Cazier's avatar
David Cazier committed
515 516
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
					popAgentInCells(*ait, old) ;
pitiot's avatar
maj  
pitiot committed
517 518
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
					register_pop(*ait, (*ait)->index) ;
pitiot's avatar
pitiot committed
519
				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();ait != oldNeighborObst.end(); ++ait)
pitiot's avatar
maj  
pitiot committed
520
					register_pop(*ait, (*ait)->index) ;
521
				neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
522 523

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

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

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

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

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

pitiot's avatar
maj  
pitiot committed
561 562 563 564
					update_registration(*ait) ;
				}

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

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

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

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
	// 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()) ;

	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())
pitiot's avatar
maj  
pitiot committed
649
//					{
650 651 652 653 654 655 656
//						if(map.getEmbedding<FACE>(coarsenCandidate[start]) == fEmb)
//						{
//							coarsenCandidate[start] = coarsenCandidate.back() ;
//							coarsenCandidate.pop_back() ;
//						}
//						else
//							++start ;
pitiot's avatar
maj  
pitiot committed
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 741 742 743 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

					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 ;
		//premier tour pour les présents
		fit = old ;
		do
		{
			PFP::AGENTS a(agentvect[fit]) ;
			PFP::OBSTACLEVECT ob(obstvect[fit]) ;


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


			map.setCurrentLevel(map.getMaxLevel()) ;
			for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
			{
				register_pop(*ait, (*ait)->index) ;
				obst.push_back(*ait);
			}



			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)
					{
						resetPart(mo,nf) ;
					}
				}
				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]) ;

			agents.insert(agents.end(), a.begin(), a.end()) ;
			map.setCurrentLevel(map.getMaxLevel()) ;
			for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
			{
				register_pop(*ait, (*ait)->index) ;
				obst.push_back(*ait);
			}
			for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
				popAgentInCells(*ait, centerFace) ;
			map.setCurrentLevel(fLevel - 1) ;
		}

		//deuxieme tour concernant les voisins
		fit = old ;
		do
		{

			PFP::OBSTACLEVECT nob(neighborObstvect[fit]) ;

			map.setCurrentLevel(map.getMaxLevel()) ;
			for(PFP::OBSTACLEVECT::iterator ait = nob.begin(); ait != nob.end(); ++ait)
			{
				register_pop(*ait, (*ait)->index) ;
				neighborObst.push_back(*ait);
			}
			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::OBSTACLEVECT nob(neighborObstvect[centerFace]) ;
			map.setCurrentLevel(map.getMaxLevel()) ;
			for(PFP::OBSTACLEVECT::iterator ait = nob.begin(); ait != nob.end(); ++ait)
			{
				register_pop(*ait, (*ait)->index) ;
				neighborObst.push_back(*ait);
			}
			map.setCurrentLevel(fLevel - 1) ;
		}

		neighborAgentvect[old].clear() ;

		// TODO Check with optimisation

pitiot's avatar
pitiot committed
814
//
815 816
//		CGoGNout<<"Avant pop"<<CGoGNendl;
//		CGoGNout<<"Obst dans la case : ";
pitiot's avatar
pitiot committed
817
//		for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
818 819 820
//			CGoGNout<<(*ait)->index<<" ; ";
//		CGoGNout<<CGoGNendl;
//		CGoGNout<<"Obst voisins : ";
pitiot's avatar
pitiot committed
821
//		for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
822 823 824 825 826 827 828 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
//			CGoGNout<<(*ait)->index<<" ; ";
//		CGoGNout<<CGoGNendl;


		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
873 874 875 876
}

void EnvMap::resetAgentInFace(Agent* agent)
{
pitiot's avatar
maj  
pitiot committed
877 878 879
	VEC3 pos = agent->part_.getPosition() ;
	agent->part_.move(Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position)) ;
	agent->part_.setState(FACE) ;
880
	agent->part_.move(pos) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
881
}
Pierre Kraemer's avatar
Pierre Kraemer committed
882
#endif
pitiot's avatar
maj  
pitiot committed
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 916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968 969

#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