env_map.cpp 16.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

using namespace CGoGN;

Pierre Kraemer's avatar
Pierre Kraemer committed
15
EnvMap::EnvMap() :
Pierre Kraemer's avatar
Pierre Kraemer committed
16 17 18 19
	obstacleMark(map, EDGE),
	buildingMark(map, FACE),
	refineMark(map, FACE),
	coarsenMark(map, FACE)
Pierre Kraemer's avatar
Pierre Kraemer committed
20
{
Pierre Kraemer's avatar
Pierre Kraemer committed
21 22 23 24 25 26
	position = map.addAttribute<PFP::VEC3>(VERTEX, "position");
	normal = map.addAttribute<PFP::VEC3>(VERTEX, "normal");
	agentvect = map.addAttribute<PFP::AGENTVECT>(FACE, "agents");
	neighborAgentvect = map.addAttribute<PFP::AGENTVECT>(FACE, "neighborAgents");
	obstvect = map.addAttribute<PFP::OBSTACLEVECT>(FACE, "obstacles");
	subdivisableFace = map.addAttribute<PFP::BOOLATTRIB>(FACE, "subdivisableFace");
Thomas's avatar
Thomas committed
27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44

	refineCandidate.reserve(100);
	coarsenCandidate.reserve(100);

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

//	Algo::BooleanOperator::mergeVertices<PFP>(map,position);
//	DartMarker amelk(map);
//	map.closeMap(amelk);
//	Algo::Modelisation::CatmullClarkSubdivision<PFP>(map,position);
//	Algo::Modelisation::computeDual<PFP>(map);
Pierre Kraemer's avatar
Pierre Kraemer committed
45 46 47 48
}

unsigned int EnvMap::mapMemoryCost()
{
Pierre Kraemer's avatar
Pierre Kraemer committed
49 50 51 52
	return (map.getAttributeContainer(DART)).memorySize()
			+ (map.getAttributeContainer(VERTEX)).memorySize()
			+ (map.getAttributeContainer(EDGE)).memorySize()
			+ (map.getAttributeContainer(FACE)).memorySize();
Pierre Kraemer's avatar
Pierre Kraemer committed
53 54
}

Thomas's avatar
Thomas committed
55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154
void EnvMap::scale(float scaleVal)
{
	Geom::BoundingBox<PFP::VEC3> bb(position.begin());

	for(unsigned int v = position.begin() ; v != position.end() ; position.next(v))
	{
		bb.addPoint(position[v]);
	}

	VEC3 center = bb.center();
	for(unsigned int v = position.begin() ; v != position.end() ; position.next(v))
	{
		position[v] -= center;
		position[v] *= scaleVal;
	}
}

void EnvMap::subdivideAllToMaxLevel()
{
	bool subdiv;
	do
	{
		subdiv=false;
		{
			CellMarker subd(map,FACE);
			for(Dart d = map.begin(); d!= map.end(); map.next(d))
			{
				if(!subd.isMarked(d))
				{
					subd.mark(d);
					if(!buildingMark.isMarked(d))
					{
						//check if subdivision is authorized
						float minDistSq = Agent::neighborDistSq_ ;
						bool subdivisable = true;
						Dart old = map.faceOldestDart(d) ;
						unsigned int fLevel = map.faceLevel(old) ;
						map.setCurrentLevel(fLevel) ;
						PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
						Dart fd = old ;
						do
						{
							PFP::VEC3& p = position[fd] ;
							PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
							PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
							if(proj.norm2() < minDistSq)
							{
								subdivisable = false ;
								break ;
							}
							fd = map.phi1(fd) ;
						} while(fd != old) ;

						if(subdivisable)
						{
							map.setCurrentLevel(fLevel) ;
							Algo::IHM::subdivideFace<PFP>(map, old, position) ;
							subdiv = true;
						}
						map.setCurrentLevel(map.getMaxLevel()) ;
					}
				}
			}
		}
	} while(subdiv);
}

void EnvMap::subdivideToProperLevel()
{
	bool subdiv;
	do
	{
		subdiv=false;
		{
			CellMarker subd(map,FACE);
			for(Dart d = map.begin(); d!= map.end(); map.next(d))
			{
				if(!subd.isMarked(d))
				{
					subd.mark(d);
					if(!refineMark.isMarked(d) && agentvect[d].size() > nbAgentsToSubdivide)
					{
						std::pair<bool,bool>& sf = subdivisableFace[d];
						if(sf.first == false || (sf.first == true && sf.second))
						{
							subdiv=true;
							refineMark.mark(d);
							refineCandidate.push_back(d);
						}
					}
				}
			}
			subd.unmarkAll();
		}
		updateMap();
		refineCandidate.clear();
		map.setCurrentLevel(map.getMaxLevel());
	} while(subdiv);
}

Pierre Kraemer's avatar
Pierre Kraemer committed
155 156
Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
Pierre Kraemer's avatar
Pierre Kraemer committed
157
	CellMarkerStore m(map, FACE);
Pierre Kraemer's avatar
Pierre Kraemer committed
158 159
	for(Dart d = map.begin(); d != map.end(); map.next(d))
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
160 161
		if(!m.isMarked(d))
		{
162 163 164 165
			m.mark(d);
			if(!buildingMark.isMarked(d) && Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true))
				return d;
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
166 167 168 169 170 171 172 173 174
	}

	std::cout << "ERROR : pos not in map" << std::endl;
	return map.begin();
}

void EnvMap::init()
{
	float sideSize = 70.0f;
Pierre Kraemer's avatar
Pierre Kraemer committed
175
	unsigned int nbSquares = 12;
Thomas's avatar
Thomas committed
176

Pierre Kraemer's avatar
yeah..  
Pierre Kraemer committed
177
//	CityGenerator::generateGrid<PFP>(map, position, nbSquares, nbSquares, sideSize, obstacleMark, buildingMark);
Thomas's avatar
Thomas committed
178
//	CityGenerator::generateCorridor<PFP>(map, position, obstacleMark, buildingMark, 50, nbSquares);
Pierre Kraemer's avatar
yeah..  
Pierre Kraemer committed
179
	CityGenerator::generateCity<PFP>(map, position, obstacleMark, buildingMark, sideSize, nbSquares);
180
//	CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
Pierre Kraemer's avatar
Pierre Kraemer committed
181
//	CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
Thomas's avatar
Thomas committed
182 183
//	CityGenerator::convexifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
//	CityGenerator::installGuardRail<PFP>(map, position, obstacleMark, buildingMark, 5.0f);
Pierre Kraemer's avatar
Pierre Kraemer committed
184 185
	map.init();
	registerObstaclesInFaces();
Thomas's avatar
Thomas committed
186

Pierre Kraemer's avatar
yeah..  
Pierre Kraemer committed
187
//	subdivideAllToMaxLevel();
Thomas's avatar
Thomas committed
188

189 190
	for(unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
		subdivisableFace[i].first = false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
191 192
}

Pierre Kraemer's avatar
Pierre Kraemer committed
193 194 195 196 197 198 199 200 201 202 203 204 205 206 207
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
	Dart dd = d;
	do
	{
		Dart ddd = map.alpha1(map.alpha1(dd));
		while(ddd != dd)
		{
			f(ddd);
			ddd = map.alpha1(ddd);
		}
		dd = map.phi1(dd);
	} while(dd != d);
}

Pierre Kraemer's avatar
Pierre Kraemer committed
208 209
void EnvMap::registerObstaclesInFaces()
{
Pierre Kraemer's avatar
Pierre Kraemer committed
210
	CellMarker m(map, FACE);
Pierre Kraemer's avatar
Pierre Kraemer committed
211 212 213 214 215
	for(Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if(!m.isMarked(d))
		{
			m.mark(d);
216
			if(!buildingMark.isMarked(d))
Pierre Kraemer's avatar
Pierre Kraemer committed
217
			{
218 219 220 221
				Dart dd = d;
				do
				{
					if(obstacleMark.isMarked(dd))
Pierre Kraemer's avatar
Pierre Kraemer committed
222 223 224 225 226 227 228
					{
						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], position[map.phi1(next)]);
						obstvect[d].push_back(o);
					}
229 230
					dd = map.phi1(dd);
				} while(dd != d);
Pierre Kraemer's avatar
Pierre Kraemer committed
231

232
				do
Pierre Kraemer's avatar
Pierre Kraemer committed
233
				{
234 235 236 237 238 239 240 241 242 243 244 245
					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
246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261
}

void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor)
{
	Dart stop;
	if(edgeNeighbor)
		stop = map.phi_1(map.phi_1(map.phi_1(d)));
	else
		stop = map.phi_1(map.phi_1(d));
	
	Dart dd = d;
	do
	{
		if(obstacleMark.isMarked(dd))
		{
			if(buildingMark.isMarked(dd))
Pierre Kraemer's avatar
Pierre Kraemer committed
262 263 264 265 266 267
			{
				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);
			}
Pierre Kraemer's avatar
Pierre Kraemer committed
268
			else
Pierre Kraemer's avatar
Pierre Kraemer committed
269 270 271 272 273 274 275
			{
				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], position[map.phi1(next)]);
				obst.push_back(o);
			}
Pierre Kraemer's avatar
Pierre Kraemer committed
276 277
		}
		dd = map.phi1(dd);
Pierre Kraemer's avatar
Pierre Kraemer committed
278
	} while(dd != stop);
Pierre Kraemer's avatar
Pierre Kraemer committed
279 280
}

281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299
//void EnvMap::agentChangeFaceThroughEdge(Agent* agent)
//{
//	Dart oldFace = agent->part_.lastCrossed;
//	Dart newFace = agent->part_.d;
//
//	agentvect[newFace].push_back(agent);
//
//	PFP::AGENTS::iterator end = agentvect[oldFace].end();
//	for(PFP::AGENTS::iterator it = agentvect[oldFace].begin(); it != end; ++it)
//	{
//		if(*it == agent)
//		{
//			*it = agentvect[oldFace].back();
//			agentvect[oldFace].pop_back();
//			break;
//		}
//	}
//
//	// mark adjacent cells shared by newFace and oldFace
Pierre Kraemer's avatar
Pierre Kraemer committed
300
//	CellMarkerNoUnmark f(map, FACE);
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 344 345 346 347 348 349 350 351 352 353 354 355 356 357
//	Dart d = oldFace;
//	do
//	{
//		f.mark(d);
//		d = map.alpha1(d);
//	} while(d != oldFace);
//
//	d = map.phi1(oldFace);
//	do
//	{
//		f.mark(d);
//		d = map.alpha1(d);
//	} while(d != map.phi1(oldFace));
//
//	// remove agent from cells adjacent to oldFace but not to newFace
//	Dart dd = oldFace;
//	do
//	{
//		Dart ddd = map.alpha1(map.alpha1(dd));
//		while(ddd != dd)
//		{
//			if(!f.isMarked(ddd))
//			{
//				bool found = false;
//				PFP::AGENTS::iterator end = neighborAgentvect[ddd].end();
//				for(PFP::AGENTS::iterator it = neighborAgentvect[ddd].begin(); it != end && !found; ++it)
//				{
//					if(*it == agent)
//					{
//						*it = neighborAgentvect[ddd].back();
//						neighborAgentvect[ddd].pop_back();
//						found = true;
//					}
//				}
//			}
//			ddd = map.alpha1(ddd);
//		}
//		dd = map.phi1(dd);
//	} while(dd != oldFace);
//
//	// add agent from cells adjacent to oldFace but not to newFace
//	// and unmark shared cells
//	dd = newFace;
//	do
//	{
//		Dart ddd = map.alpha1(map.alpha1(dd));
//		while(ddd != dd)
//		{
//			if(!f.isMarked(ddd))
//				neighborAgentvect[ddd].push_back(agent);
//			else
//				f.unmark(ddd);
//			ddd = map.alpha1(ddd);
//		}
//		dd = map.phi1(dd);
//	} while(dd != newFace);
//}
Pierre Kraemer's avatar
Pierre Kraemer committed
358

Pierre Kraemer's avatar
Pierre Kraemer committed
359 360
void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
361 362
	Dart newFace = agent->part_.d;

Pierre Kraemer's avatar
Pierre Kraemer committed
363
	popAgentInCells(agent, oldFace);
364 365
	pushAgentInCells(agent, newFace);

Pierre Kraemer's avatar
Pierre Kraemer committed
366
	if(!refineMark.isMarked(newFace) && agentvect[newFace].size() > nbAgentsToSubdivide)
367 368 369 370 371 372 373 374
	{
		std::pair<bool,bool>& sf = subdivisableFace[newFace];
		if(sf.first == false || (sf.first == true && sf.second))
		{
			refineMark.mark(newFace);
			refineCandidate.push_back(newFace);
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
375

Pierre Kraemer's avatar
Pierre Kraemer committed
376
	if(!coarsenMark.isMarked(oldFace) && !refineMark.isMarked(oldFace) && agentvect[oldFace].size() < nbAgentsToSimplify / 4)
Pierre Kraemer's avatar
Pierre Kraemer committed
377 378 379 380
	{
		coarsenMark.mark(oldFace);
		coarsenCandidate.push_back(map.faceOldestDart(oldFace));
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
381 382 383 384 385 386
}

void EnvMap::updateMap()
{
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

387
	for(std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end(); ++it)
Pierre Kraemer's avatar
Pierre Kraemer committed
388
	{
389 390
		Dart d = (*it) ;
		refineMark.unmark(d);
Pierre Kraemer's avatar
Pierre Kraemer committed
391

Pierre Kraemer's avatar
Pierre Kraemer committed
392
		if(agentvect[d].size() > nbAgentsToSubdivide)
Pierre Kraemer's avatar
Pierre Kraemer committed
393
		{
Thomas's avatar
Thomas committed
394
			int fLevel = -1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
395 396
			Dart old = map.faceOldestDart(d) ;

397
			bool subdivisable = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
398

Pierre Kraemer's avatar
Pierre Kraemer committed
399
			std::pair<bool,bool>& sf = subdivisableFace[old] ;
Pierre Kraemer's avatar
Pierre Kraemer committed
400 401
			if(sf.first == true)
				subdivisable = sf.second ;
402
			else
Pierre Kraemer's avatar
Pierre Kraemer committed
403
			{
Pierre Kraemer's avatar
Pierre Kraemer committed
404
				float minDistSq = Agent::neighborDistSq_ ; // diametre de vision de l'agent au carré
405

Pierre Kraemer's avatar
Pierre Kraemer committed
406
				fLevel = map.faceLevel(old) ;
407 408 409
				map.setCurrentLevel(fLevel) ;
				PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
				Dart fd = old ;
Pierre Kraemer's avatar
Pierre Kraemer committed
410 411
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
412
					PFP::VEC3& p = position[fd] ;
413
					PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
414
					PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
415
					if(proj.norm2() < minDistSq)
Pierre Kraemer's avatar
Pierre Kraemer committed
416
					{
417 418
						subdivisable = false ;
						break ;
Pierre Kraemer's avatar
Pierre Kraemer committed
419
					}
420 421 422 423
					fd = map.phi1(fd) ;
				} while(fd != old) ;
				map.setCurrentLevel(map.getMaxLevel()) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
424 425
				sf.first = true ;
				sf.second = subdivisable ;
426 427 428 429
			}

			if(subdivisable)
			{
Pierre Kraemer's avatar
Pierre Kraemer committed
430
				if(fLevel == -1)
Pierre Kraemer's avatar
Pierre Kraemer committed
431
					fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
432 433

				sf.first = false ;
434 435 436 437 438 439

				PFP::AGENTS oldAgents(agentvect[old]);
				for(PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
					popAgentInCells(*ait, old);

				neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
440 441

				map.setCurrentLevel(fLevel) ;
442
				Algo::IHM::subdivideFace<PFP>(map, old, position) ;
443 444 445 446 447 448 449 450 451 452 453 454 455 456
				CellMarkerStore newF(map, FACE) ;
				unsigned int degree = 0 ;
				Dart dd = old ;
				do
				{
					++degree ;
					newF.mark(dd);
					dd = map.phi1(dd) ;
				} while(dd != old) ;
				if(degree == 3)
				{
					Dart centerFace = map.phi2(map.phi1(old)) ;
					newF.mark(centerFace);
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
457
				map.setCurrentLevel(map.getMaxLevel()) ;
458

459 460 461 462 463
				for(PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
				{
					resetAgentInFace(*ait) ;
					pushAgentInCells(*ait, (*ait)->part_.d) ;
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
464

465
				dd = old ;
Pierre Kraemer's avatar
Pierre Kraemer committed
466 467
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
468
					Dart d3 = dd ;
469
					do
Pierre Kraemer's avatar
Pierre Kraemer committed
470
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
471 472
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
473 474 475
						while(d4 != d3)
						{
							if(!newF.isMarked(d4))
Pierre Kraemer's avatar
Pierre Kraemer committed
476 477 478 479 480
							{
								PFP::AGENTS& ad4 = agentvect[d4] ;
								nad3.insert(nad3.end(), ad4.begin(), ad4.end()) ;
							}
							d4 = map.alpha1(d4) ;
481
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
482 483
						d3 = map.phi1(d3) ;
					} while(d3 != dd) ;
484 485

					map.setCurrentLevel(fLevel) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
486
					dd = map.phi1(dd) ;
487 488 489 490 491
					map.setCurrentLevel(map.getMaxLevel()) ;
				} while(dd != old);

				if(degree == 3)
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
492 493
					Dart centerFace = map.phi2(map.phi1(old)) ;
					Dart d3 = centerFace ;
494 495
					do
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
496 497
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
498 499 500
						while(d4 != d3)
						{
							if(!newF.isMarked(d4))
Pierre Kraemer's avatar
Pierre Kraemer committed
501 502 503 504 505
							{
								PFP::AGENTS& ad4 = agentvect[d4] ;
								nad3.insert(neighborAgentvect[d3].end(), ad4.begin(), ad4.end()) ;
							}
							d4 = map.alpha1(d4) ;
506
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
507 508
						d3 = map.phi1(d3) ;
					} while(d3 != centerFace) ;
509
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
510 511 512
			}
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
513

Thomas's avatar
Thomas committed
514
	for(unsigned int it = 0; it < coarsenCandidate.size(); ++it)
Pierre Kraemer's avatar
Pierre Kraemer committed
515
	{
Thomas's avatar
Thomas committed
516
		Dart old = coarsenCandidate[it] ;
Pierre Kraemer's avatar
Pierre Kraemer committed
517
		coarsenMark.unmark(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
518

Pierre Kraemer's avatar
Pierre Kraemer committed
519
		unsigned int fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
520 521 522 523 524 525 526 527

		if(fLevel > 0 && map.getDartLevel(old) < fLevel)
		{
			unsigned int cur = map.getCurrentLevel() ;
			map.setCurrentLevel(fLevel - 1) ;

			if(map.faceIsSubdividedOnce(old))
			{
Pierre Kraemer's avatar
Pierre Kraemer committed
528 529
				// 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
530 531 532 533 534
				unsigned int degree = 0 ;
				unsigned int nbAgents = 0 ;
				Dart fit = old ;
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
535
					nbAgents += agentvect[fit].size() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
536
					++degree ;
Pierre Kraemer's avatar
Pierre Kraemer committed
537 538 539 540 541 542 543 544 545 546 547 548 549 550 551

					coarsenMark.unmark(fit) ;
					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
552 553 554 555 556 557 558
					fit = map.phi1(fit) ;
				} while(fit != old) ;
				if(degree == 3)
				{
					map.setCurrentLevel(fLevel) ;
					Dart centerFace = map.phi2(map.phi1(old)) ;
					nbAgents += agentvect[centerFace].size() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
559 560 561 562 563 564 565 566 567 568 569 570 571
					coarsenMark.unmark(centerFace) ;
					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
572 573 574 575 576 577 578 579 580
					map.setCurrentLevel(fLevel - 1) ;
				}

				if(nbAgents < nbAgentsToSimplify)
				{
					PFP::AGENTS agents ;
					Dart fit = old ;
					do
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
581 582 583
						PFP::AGENTS a(agentvect[fit]) ;
						agents.insert(agents.end(), a.begin(), a.end()) ;
						map.setCurrentLevel(map.getMaxLevel()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
584
						for(PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
Pierre Kraemer's avatar
Pierre Kraemer committed
585
							popAgentInCells(*ait, fit) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
586 587 588 589 590 591 592 593
						map.setCurrentLevel(fLevel - 1) ;
						Dart nf = map.phi2(fit) ;
						if(!map.faceIsSubdivided(nf))
						{
							map.setCurrentLevel(fLevel) ;
							PFP::AGENTS& an = agentvect[nf] ;
							for(PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
							{
Pierre Kraemer's avatar
Pierre Kraemer committed
594 595
								if((*ait)->part_.d == map.phi1(nf))
									(*ait)->part_.d = nf ;
Pierre Kraemer's avatar
Pierre Kraemer committed
596 597 598 599 600 601 602 603 604
							}
							map.setCurrentLevel(fLevel - 1) ;
						}
						fit = map.phi1(fit) ;
					} while(fit != old) ;
					if(degree == 3)
					{
						map.setCurrentLevel(fLevel) ;
						Dart centerFace = map.phi2(map.phi1(old)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
605 606 607
						PFP::AGENTS a(agentvect[centerFace]) ;
						agents.insert(agents.end(), a.begin(), a.end()) ;
						map.setCurrentLevel(map.getMaxLevel());
Pierre Kraemer's avatar
Pierre Kraemer committed
608 609
						for(PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
							popAgentInCells(*ait, centerFace);
Pierre Kraemer's avatar
Pierre Kraemer committed
610 611
						map.setCurrentLevel(fLevel - 1) ;
					}
Pierre Kraemer's avatar
Pierre Kraemer committed
612
					neighborAgentvect[old].clear();
Pierre Kraemer's avatar
Pierre Kraemer committed
613 614 615

					Algo::IHM::coarsenFace<PFP>(map, old, position) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
616 617 618
					std::pair<bool,bool>& sf = subdivisableFace[old] ;
					sf.first = true;
					sf.second = true;
Pierre Kraemer's avatar
Pierre Kraemer committed
619 620 621

					map.setCurrentLevel(map.getMaxLevel());

Thomas's avatar
Thomas committed
622
					for(PFP::AGENTS::iterator itA = agents.begin(); itA != agents.end(); ++itA)
Pierre Kraemer's avatar
Pierre Kraemer committed
623
					{
Thomas's avatar
Thomas committed
624 625
						(*itA)->part_.d = old ;
						pushAgentInCells(*itA, old);
Pierre Kraemer's avatar
Pierre Kraemer committed
626
					}
Pierre Kraemer's avatar
Pierre Kraemer committed
627

Pierre Kraemer's avatar
Pierre Kraemer committed
628 629 630 631 632 633 634 635 636 637 638
					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);
Pierre Kraemer's avatar
Pierre Kraemer committed
639

Pierre Kraemer's avatar
Pierre Kraemer committed
640
					if(fLevel > 1 && !coarsenMark.isMarked(old) && agentvect[old].size() < nbAgentsToSimplify / 4)
Thomas's avatar
Thomas committed
641 642 643 644
					{
						coarsenMark.mark(old);
						coarsenCandidate.push_back(map.faceOldestDart(old));
					}
Pierre Kraemer's avatar
Pierre Kraemer committed
645 646 647 648 649
				}
			}
			map.setCurrentLevel(cur) ;
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
650

Pierre Kraemer's avatar
Pierre Kraemer committed
651 652 653 654 655 656 657
	map.setCurrentLevel(map.getMaxLevel()) ;
}

void EnvMap::resetAgentInFace(Agent* agent)
{
	VEC3 pos = agent->part_.m_position;
	agent->part_.m_position = Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position);
Pierre Kraemer's avatar
Pierre Kraemer committed
658
	agent->part_.state = FACE;
Pierre Kraemer's avatar
Pierre Kraemer committed
659 660
	agent->part_.move(pos);
}