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