env_map.cpp 19.1 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() :
18 19 20
		obstacleDistance(Agent::range_), obstacleMarkS(mapScenary, EDGE), buildingMarkS(
				mapScenary, FACE), obstacleMark(map, EDGE), buildingMark(map, FACE), pedWayMark(
				map, FACE)
Pierre Kraemer's avatar
Pierre Kraemer committed
21
#ifndef SPATIAL_HASHING
22
				, refineMark(map, FACE), coarsenMark(map, FACE)
Pierre Kraemer's avatar
Pierre Kraemer committed
23
#else
24 25 26 27
				,
				ht_agents(1024),
				ht_neighbor_agents(1024),
				ht_obstacles(1024)
Pierre Kraemer's avatar
Pierre Kraemer committed
28
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
29
{
30 31
	position = map.addAttribute<PFP::VEC3>(VERTEX, "position") ;
	normal = map.addAttribute<PFP::VEC3>(VERTEX, "normal") ;
Pierre Kraemer's avatar
Pierre Kraemer committed
32 33

#ifndef SPATIAL_HASHING
34 35 36 37
	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
38

39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 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
	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 ;
	VEC3 bottomLeft( -width / 2, -height / 2, 0.0f) ;
	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
	a_cell_nb_x = geometry.size(0) / minCellSize ;
	a_cell_nb_y = geometry.size(1) / minCellSize ;
	o_cell_nb_x = geometry.size(0) / obstacleDistance ;
	o_cell_nb_y = geometry.size(1) / obstacleDistance ;
	std::cout << " - Table de hachage : " << a_cell_nb_x << " x " << a_cell_nb_y << std::endl ;
#endif

	unsigned int nbSquares = 6 ;

	switch (config) {
		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 : {
//			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) ;
//			map.closeMap() ;
//			Algo::Modelisation::CatmullClarkSubdivision<PFP>(map, position) ;
//			Algo::Modelisation::computeDual<PFP>(map) ;
		}
			break ;
		case 4 :
			CityGenerator::generatePlanet<PFP>(map, position, obstacleMark, buildingMark,
					200.0f, nbSquares) ;
			break ;
	}

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

//	subdivideAllToMaxLevel();

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

116 117
void EnvMap::markPedWay()
{
118 119 120 121 122 123 124 125 126 127 128 129
	CellMarker treat(map, FACE) ;
	for (Dart d = map.begin() ; d != map.end() ; map.next(d)) {
		if ( !treat.isMarked(d)) {
			treat.mark(d) ;

			Dart dd = d ;
			do {
				Dart ddd = dd ;
				do {
					if (obstacleMark.isMarked(dd)) {
						pedWayMark.mark(d) ;
						break ;
130
					}
131 132
					ddd = map.alpha1(ddd) ;
				} while (ddd != dd) ;
133

134
				dd = map.phi1(dd) ;
135

136
			} while (dd != d) ;
137 138 139
		}
	}

140 141 142 143 144 145 146 147 148 149 150 151 152 153
	treat.unmarkAll() ;
	for (Dart d = map.begin() ; d != map.end() ; map.next(d)) {
		if ( !treat.isMarked(d)) {
			treat.mark(d) ;

			Dart dd = d ;
			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)))))) {
					pedWayMark.mark(d) ;
					break ;
				}
154

155
				dd = map.phi1(dd) ;
156

157
			} while (dd != d) ;
158 159 160 161
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
162 163
unsigned int EnvMap::mapMemoryCost()
{
Pierre Kraemer's avatar
Pierre Kraemer committed
164 165 166
	return (map.getAttributeContainer(DART)).memorySize()
			+ (map.getAttributeContainer(VERTEX)).memorySize()
			+ (map.getAttributeContainer(EDGE)).memorySize()
167
			+ (map.getAttributeContainer(FACE)).memorySize() ;
Thomas's avatar
Thomas committed
168 169
}

Pierre Kraemer's avatar
Pierre Kraemer committed
170
#ifndef SPATIAL_HASHING
Thomas's avatar
Thomas committed
171 172
void EnvMap::subdivideAllToMaxLevel()
{
173 174 175
	bool subdiv ;
	do {
		subdiv = false ;
Thomas's avatar
Thomas committed
176
		{
177 178 179 180 181
			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)) {
Thomas's avatar
Thomas committed
182 183
						//check if subdivision is authorized
						float minDistSq = Agent::neighborDistSq_ ;
184
						bool subdivisable = true ;
Thomas's avatar
Thomas committed
185 186 187
						Dart old = map.faceOldestDart(d) ;
						unsigned int fLevel = map.faceLevel(old) ;
						map.setCurrentLevel(fLevel) ;
188 189
						PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old,
								position) ;
Thomas's avatar
Thomas committed
190
						Dart fd = old ;
191
						do {
Thomas's avatar
Thomas committed
192
							PFP::VEC3& p = position[fd] ;
193 194 195 196 197
							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) {
Thomas's avatar
Thomas committed
198 199 200 201
								subdivisable = false ;
								break ;
							}
							fd = map.phi1(fd) ;
202
						} while (fd != old) ;
Thomas's avatar
Thomas committed
203

204
						if (subdivisable) {
Thomas's avatar
Thomas committed
205 206
							map.setCurrentLevel(fLevel) ;
							Algo::IHM::subdivideFace<PFP>(map, old, position) ;
207
							subdiv = true ;
Thomas's avatar
Thomas committed
208 209 210 211 212 213
						}
						map.setCurrentLevel(map.getMaxLevel()) ;
					}
				}
			}
		}
214
	} while (subdiv) ;
Thomas's avatar
Thomas committed
215 216 217 218
}

void EnvMap::subdivideToProperLevel()
{
219 220 221
	bool subdiv ;
	do {
		subdiv = false ;
Thomas's avatar
Thomas committed
222
		{
223 224 225 226 227 228 229 230 231 232
			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) ;
Thomas's avatar
Thomas committed
233 234 235 236
						}
					}
				}
			}
237
			subd.unmarkAll() ;
Thomas's avatar
Thomas committed
238
		}
239 240 241 242
		updateMap() ;
		refineCandidate.clear() ;
		map.setCurrentLevel(map.getMaxLevel()) ;
	} while (subdiv) ;
Thomas's avatar
Thomas committed
243 244
}

Pierre Kraemer's avatar
Pierre Kraemer committed
245 246
Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
247 248 249 250 251 252 253
	CellMarkerStore m(map, FACE) ;
	for (Dart d = map.begin() ; d != map.end() ; map.next(d)) {
		if ( !m.isMarked(d)) {
			m.mark(d) ;
			if ( !map.isBoundaryFace(d) && !buildingMark.isMarked(d)
					&& Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true))
				return d ;
254
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
255 256
	}

257 258
	std::cout << "ERROR : pos not in map" << std::endl ;
	return map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
259
}
Pierre Kraemer's avatar
Pierre Kraemer committed
260
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
261

Pierre Kraemer's avatar
Pierre Kraemer committed
262
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
263 264
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
265 266 267 268 269 270
	Dart dd = d ;
	do {
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
		while (ddd != dd) {
			f(ddd) ;
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
271
		}
272 273
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
274 275
}

Pierre Kraemer's avatar
Pierre Kraemer committed
276 277
void EnvMap::registerObstaclesInFaces()
{
278 279 280 281 282 283
	CellMarker m(map, FACE) ;
	for (Dart d = map.begin() ; d != map.end() ; map.next(d)) {
		if ( !m.isMarked(d)) {
			m.mark(d) ;
			if ( !buildingMark.isMarked(d)) {
				Dart dd = d ;
284 285 286 287

				//retrieve all obstacles within its one-ring

				//first : test all edges of the face of d
288 289 290 291 292 293 294 295
				do {
					if (obstacleMark.isMarked(dd)) {
						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) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
296
					}
297 298
					dd = map.phi1(dd) ;
				} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
299

300
				//second : test all edges of neighboring faces
301 302 303 304 305 306
				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) ;
307
					}
308 309
					dd = map.phi1(dd) ;
				} while (dd != d) ;
310 311 312
			}
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
313 314 315 316
}

void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor)
{
317 318 319 320 321 322 323
	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)) {
324 325 326 327 328 329 330 331 332 333
//			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
//			{
334 335 336 337 338 339
			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) ;
340
//			}
Pierre Kraemer's avatar
Pierre Kraemer committed
341
		}
342 343
		dd = map.phi1(dd) ;
	} while (dd != stop) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
344 345
}

346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364
//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
365
//	CellMarkerNoUnmark f(map, FACE);
366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422
//	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
423

Pierre Kraemer's avatar
Pierre Kraemer committed
424 425
void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
426
	Dart newFace = agent->part_.d ;
427

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

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

439 440 441 442
	if ( !coarsenMark.isMarked(oldFace) && !refineMark.isMarked(oldFace)
			&& agentvect[oldFace].size() < nbAgentsToSimplify) {
		coarsenMark.mark(oldFace) ;
		coarsenCandidate.push_back(map.faceOldestDart(oldFace)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
443
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
444 445
}

Pierre Kraemer's avatar
Pierre Kraemer committed
446 447
void EnvMap::obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace)
{
448 449
	popObstacleInCells(agent, oldFace) ;
	pushObstacleInCells(agent, newFace) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
450 451
}

Pierre Kraemer's avatar
Pierre Kraemer committed
452 453 454 455
void EnvMap::updateMap()
{
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

456 457 458 459
	for (std::vector<Dart>::iterator it = refineCandidate.begin() ;
			it != refineCandidate.end() ; ++it) {
		Dart d = ( *it) ;
		refineMark.unmark(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
460

461
		if (agentvect[d].size() > nbAgentsToSubdivide) {
Thomas's avatar
Thomas committed
462
			int fLevel = -1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
463 464
			Dart old = map.faceOldestDart(d) ;

465
			bool subdivisable = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
466

467 468 469 470
			std::pair<bool, bool>& sf = subdivisableFace[old] ;
			if (sf.first == true) subdivisable = sf.second ;
			else {
				float minSizeSq = minCellSize * minCellSize ; // diametre de vision de l'agent au carré
471

Pierre Kraemer's avatar
Pierre Kraemer committed
472
				fLevel = map.faceLevel(old) ;
473 474 475
				map.setCurrentLevel(fLevel) ;
				PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
				Dart fd = old ;
476
				do {
Pierre Kraemer's avatar
Pierre Kraemer committed
477
					PFP::VEC3& p = position[fd] ;
478
					PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
479 480 481
					PFP::VEC3 proj = fCenter
							- (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
					if (proj.norm2() < minSizeSq) {
482 483
						subdivisable = false ;
						break ;
Pierre Kraemer's avatar
Pierre Kraemer committed
484
					}
485
					fd = map.phi1(fd) ;
486
				} while (fd != old) ;
487 488
				map.setCurrentLevel(map.getMaxLevel()) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
489 490
				sf.first = true ;
				sf.second = subdivisable ;
491 492
			}

493 494
			if (subdivisable) {
				if (fLevel == -1) fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
495 496

				sf.first = false ;
497

498 499 500 501
				PFP::AGENTS oldAgents(agentvect[old]) ;
				for (PFP::AGENTS::iterator ait = oldAgents.begin() ; ait != oldAgents.end() ;
						++ait)
					popAgentInCells( *ait, old) ;
502 503

				neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
504 505

				map.setCurrentLevel(fLevel) ;
506
				Algo::IHM::subdivideFace<PFP>(map, old, position) ;
507 508 509
				CellMarkerStore newF(map, FACE) ;
				unsigned int degree = 0 ;
				Dart dd = old ;
510
				do {
511
					++degree ;
512
					newF.mark(dd) ;
513
					dd = map.phi1(dd) ;
514 515
				} while (dd != old) ;
				if (degree == 3) {
516
					Dart centerFace = map.phi2(map.phi1(old)) ;
517
					newF.mark(centerFace) ;
518
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
519
				map.setCurrentLevel(map.getMaxLevel()) ;
520

521 522 523 524
				for (PFP::AGENTS::iterator ait = oldAgents.begin() ; ait != oldAgents.end() ;
						++ait) {
					resetAgentInFace( *ait) ;
					pushAgentInCells( *ait, ( *ait)->part_.d) ;
525
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
526

527
				dd = old ;
528
				do {
Pierre Kraemer's avatar
Pierre Kraemer committed
529
					Dart d3 = dd ;
530
					do {
Pierre Kraemer's avatar
Pierre Kraemer committed
531 532
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
533 534
						while (d4 != d3) {
							if ( !newF.isMarked(d4)) {
Pierre Kraemer's avatar
Pierre Kraemer committed
535 536 537 538
								PFP::AGENTS& ad4 = agentvect[d4] ;
								nad3.insert(nad3.end(), ad4.begin(), ad4.end()) ;
							}
							d4 = map.alpha1(d4) ;
539
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
540
						d3 = map.phi1(d3) ;
541
					} while (d3 != dd) ;
542 543

					map.setCurrentLevel(fLevel) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
544
					dd = map.phi1(dd) ;
545
					map.setCurrentLevel(map.getMaxLevel()) ;
546
				} while (dd != old) ;
547

548
				if (degree == 3) {
Pierre Kraemer's avatar
Pierre Kraemer committed
549 550
					Dart centerFace = map.phi2(map.phi1(old)) ;
					Dart d3 = centerFace ;
551
					do {
Pierre Kraemer's avatar
Pierre Kraemer committed
552 553
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
554 555
						while (d4 != d3) {
							if ( !newF.isMarked(d4)) {
Pierre Kraemer's avatar
Pierre Kraemer committed
556
								PFP::AGENTS& ad4 = agentvect[d4] ;
557 558
								nad3.insert(neighborAgentvect[d3].end(), ad4.begin(),
										ad4.end()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
559 560
							}
							d4 = map.alpha1(d4) ;
561
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
562
						d3 = map.phi1(d3) ;
563
					} while (d3 != centerFace) ;
564
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
565 566 567
			}
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
568

569
	for (unsigned int it = 0 ; it < coarsenCandidate.size() ; ++it) {
Thomas's avatar
Thomas committed
570
		Dart old = coarsenCandidate[it] ;
Pierre Kraemer's avatar
Pierre Kraemer committed
571
		coarsenMark.unmark(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
572

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

575
		if (fLevel > 0 && map.getDartLevel(old) < fLevel) {
Pierre Kraemer's avatar
Pierre Kraemer committed
576 577 578
			unsigned int cur = map.getCurrentLevel() ;
			map.setCurrentLevel(fLevel - 1) ;

579
			if (map.faceIsSubdividedOnce(old)) {
Pierre Kraemer's avatar
Pierre Kraemer committed
580 581
				// 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
582 583 584
				unsigned int degree = 0 ;
				unsigned int nbAgents = 0 ;
				Dart fit = old ;
585
				do {
Pierre Kraemer's avatar
Pierre Kraemer committed
586
					nbAgents += agentvect[fit].size() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
587
					++degree ;
Pierre Kraemer's avatar
Pierre Kraemer committed
588 589

					coarsenMark.unmark(fit) ;
590
					unsigned int start = it + 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
591
					unsigned int fEmb = map.getEmbedding(FACE, fit) ;
592 593
					while (start < coarsenCandidate.size()) {
						if (map.getEmbedding(FACE, coarsenCandidate[start]) == fEmb) {
Pierre Kraemer's avatar
Pierre Kraemer committed
594 595 596
							coarsenCandidate[start] = coarsenCandidate.back() ;
							coarsenCandidate.pop_back() ;
						}
597
						else ++start ;
Pierre Kraemer's avatar
Pierre Kraemer committed
598 599
					}

Pierre Kraemer's avatar
Pierre Kraemer committed
600
					fit = map.phi1(fit) ;
601 602 603
				} while (fit != old) ;

				if (degree == 3) {
Pierre Kraemer's avatar
Pierre Kraemer committed
604 605 606
					map.setCurrentLevel(fLevel) ;
					Dart centerFace = map.phi2(map.phi1(old)) ;
					nbAgents += agentvect[centerFace].size() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
607
					coarsenMark.unmark(centerFace) ;
608
					unsigned int start = it + 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
609
					unsigned int fEmb = map.getEmbedding(FACE, centerFace) ;
610 611
					while (start < coarsenCandidate.size()) {
						if (map.getEmbedding(FACE, coarsenCandidate[start]) == fEmb) {
Pierre Kraemer's avatar
Pierre Kraemer committed
612 613 614
							coarsenCandidate[start] = coarsenCandidate.back() ;
							coarsenCandidate.pop_back() ;
						}
615
						else ++start ;
Pierre Kraemer's avatar
Pierre Kraemer committed
616
					}
Pierre Kraemer's avatar
Pierre Kraemer committed
617 618 619
					map.setCurrentLevel(fLevel - 1) ;
				}

620
				if (nbAgents < nbAgentsToSimplify) {
Pierre Kraemer's avatar
Pierre Kraemer committed
621 622
					PFP::AGENTS agents ;
					Dart fit = old ;
623
					do {
Pierre Kraemer's avatar
Pierre Kraemer committed
624 625 626
						PFP::AGENTS a(agentvect[fit]) ;
						agents.insert(agents.end(), a.begin(), a.end()) ;
						map.setCurrentLevel(map.getMaxLevel()) ;
627 628
						for (PFP::AGENTS::iterator ait = a.begin() ; ait != a.end() ; ++ait)
							popAgentInCells( *ait, fit) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
629 630
						map.setCurrentLevel(fLevel - 1) ;
						Dart nf = map.phi2(fit) ;
631
						if ( !map.faceIsSubdivided(nf)) {
Pierre Kraemer's avatar
Pierre Kraemer committed
632 633
							map.setCurrentLevel(fLevel) ;
							PFP::AGENTS& an = agentvect[nf] ;
634 635 636
							for (PFP::AGENTS::iterator ait = an.begin() ; ait != an.end() ;
									++ait) {
								if (( *ait)->part_.d == map.phi1(nf)) ( *ait)->part_.d = nf ;
Pierre Kraemer's avatar
Pierre Kraemer committed
637 638 639 640
							}
							map.setCurrentLevel(fLevel - 1) ;
						}
						fit = map.phi1(fit) ;
641 642
					} while (fit != old) ;
					if (degree == 3) {
Pierre Kraemer's avatar
Pierre Kraemer committed
643 644
						map.setCurrentLevel(fLevel) ;
						Dart centerFace = map.phi2(map.phi1(old)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
645 646
						PFP::AGENTS a(agentvect[centerFace]) ;
						agents.insert(agents.end(), a.begin(), a.end()) ;
647 648 649
						map.setCurrentLevel(map.getMaxLevel()) ;
						for (PFP::AGENTS::iterator ait = a.begin() ; ait != a.end() ; ++ait)
							popAgentInCells( *ait, centerFace) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
650 651
						map.setCurrentLevel(fLevel - 1) ;
					}
652
					neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
653 654 655

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

656 657 658
					std::pair<bool, bool>& sf = subdivisableFace[old] ;
					sf.first = true ;
					sf.second = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
659

660
					map.setCurrentLevel(map.getMaxLevel()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
661

662 663 664 665
					for (PFP::AGENTS::iterator itA = agents.begin() ; itA != agents.end() ;
							++itA) {
						( *itA)->part_.d = old ;
						pushAgentInCells( *itA, old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
666
					}
Pierre Kraemer's avatar
Pierre Kraemer committed
667

668 669 670 671 672 673 674
					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) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
675
						}
676 677
						dd = map.phi1(dd) ;
					} while (dd != old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
678

679 680 681 682
					if (fLevel > 1 && !coarsenMark.isMarked(old)
							&& agentvect[old].size() < nbAgentsToSimplify) {
						coarsenMark.mark(old) ;
						coarsenCandidate.push_back(map.faceOldestDart(old)) ;
Thomas's avatar
Thomas committed
683
					}
Pierre Kraemer's avatar
Pierre Kraemer committed
684 685 686 687 688
				}
			}
			map.setCurrentLevel(cur) ;
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
689

Pierre Kraemer's avatar
Pierre Kraemer committed
690 691 692 693 694
	map.setCurrentLevel(map.getMaxLevel()) ;
}

void EnvMap::resetAgentInFace(Agent* agent)
{
695 696 697 698
	VEC3 pos = agent->part_.m_position ;
	agent->part_.m_position = Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position) ;
	agent->part_.state = FACE ;
	agent->part_.move(pos) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
699
}
Pierre Kraemer's avatar
Pierre Kraemer committed
700
#endif