env_map.cpp 31 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"
5
6
#include "moving_obstacle.h"
#include "simulator.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
7

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

16
17
#include "env_generator.h"

18
using namespace CGoGN ;
Pierre Kraemer's avatar
Pierre Kraemer committed
19

Pierre Kraemer's avatar
Pierre Kraemer committed
20
EnvMap::EnvMap() :
David Cazier's avatar
David Cazier committed
21
	obstacleDistance(Agent::range_),
David Cazier's avatar
David Cazier committed
22
23
24
25
26
	obstacleMarkS(mapScenary),
	buildingMarkS(mapScenary),
	obstacleMark(map),
	buildingMark(map),
	pedWayMark(map),
27

Pierre Kraemer's avatar
Pierre Kraemer committed
28
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
29
30
	refineMark(map),
	coarsenMark(map)
Pierre Kraemer's avatar
Pierre Kraemer committed
31
#else
David Cazier's avatar
David Cazier committed
32
33
34
	ht_agents(1024),
	ht_neighbor_agents(1024),
	ht_obstacles(1024)
Pierre Kraemer's avatar
Pierre Kraemer committed
35
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
36
{
David Cazier's avatar
David Cazier committed
37
38
	position = map.addAttribute<VEC3, VERTEX>("position") ;
	normal = map.addAttribute<VEC3, VERTEX>("normal") ;
Pierre Kraemer's avatar
Pierre Kraemer committed
39

Jund Thomas's avatar
Jund Thomas committed
40
	positionScenary = mapScenary.addAttribute<VEC3, VERTEX>("position") ;
Jund Thomas's avatar
Jund Thomas committed
41
	normalScenary = mapScenary.addAttribute<VEC3, VERTEX>("normal") ;
Jund Thomas's avatar
Jund Thomas committed
42

Pierre Kraemer's avatar
Pierre Kraemer committed
43
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
44
45
46
47
48
	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
49

50
51
52
53
54
55
56
57
	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
58
	VEC3 bottomLeft(-width / 2, -height / 2, 0.0f) ;
59
60
61
62
63
64
65
66
	VEC3 topRight(width / 2, height / 2, 0.0f) ;

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

David Cazier's avatar
David Cazier committed
73
74
	switch (config)
	{
David Cazier's avatar
David Cazier committed
75
		case 0 :
pitiot's avatar
merging    
pitiot committed
76
//			CityGenerator::generateGrid<PFP>(*this) ;
pitiot's avatar
pitiot committed
77
			CityGenerator::generateCity<PFP>(*this,0) ;
David Cazier's avatar
David Cazier committed
78
79
			break ;
		case 1 :
pitiot's avatar
pitiot committed
80
81
//			CityGenerator::generateGrid<PFP>(*this) ;
			CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);
David Cazier's avatar
David Cazier committed
82
83
			break ;
		case 2 :
pitiot's avatar
merging    
pitiot committed
84
			CityGenerator::generateGrid<PFP>(*this) ;
David Cazier's avatar
David Cazier committed
85
86
			// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
			break ;
Jund Thomas's avatar
Jund Thomas committed
87
		case 3 :
pitiot's avatar
merging    
pitiot committed
88
			CityGenerator::generateCity<PFP>(*this,10) ;
Jund Thomas's avatar
Jund Thomas committed
89
90
91
92
93
94
			break ;
		case 4 :
			CityGenerator::generatePlanet<PFP>(*this) ;
			break ;
		case 5 :
		{
Jund Thomas's avatar
Jund Thomas committed
95
			std::cout << "?" << std::endl;
Jund Thomas's avatar
Jund Thomas committed
96
//			std::string filename = "./svg/planet.svg" ;
97
			std::string filename = "./svg/mapRoads.svg" ;
Jund Thomas's avatar
Jund Thomas committed
98
//			std::string filename = "./svg/simpleCross.svg" ;
pitiot's avatar
pitiot committed
99
			Algo::Surface::Import::importSVG<PFP>(map, filename, position, obstacleMark, buildingMark) ;
Jund Thomas's avatar
Jund Thomas committed
100
101

			std::string filename2 = "./svg/mapBuild.svg" ;
pitiot's avatar
pitiot committed
102
			Algo::Surface::Import::importSVG<PFP>(mapScenary, filename2, positionScenary, obstacleMarkS, buildingMarkS) ;
Jund Thomas's avatar
Jund Thomas committed
103

pitiot's avatar
pitiot committed
104
			Geom::BoundingBox<PFP::VEC3> bb1, bb2 ;
Jund Thomas's avatar
Jund Thomas committed
105
			bb1 = Algo::Geometry::computeBoundingBox<PFP>(map, position) ;
pitiot's avatar
pitiot committed
106
107
108
			bb2 = Algo::Geometry::computeBoundingBox<PFP>(mapScenary, positionScenary) ;
			bb1.addPoint(bb2.min());
			bb1.addPoint(bb2.max());
Jund Thomas's avatar
Jund Thomas committed
109

110
111
//			CityGenerator::planetify<PFP>(map, position, 100.0f, bb1);
//			CityGenerator::planetify<PFP>(mapScenary, positionScenary, 100.0f, bb1);
Jund Thomas's avatar
Jund Thomas committed
112
113

			std::vector<Dart> v;
Jund Thomas's avatar
Jund Thomas committed
114
115
116
			TraversorF<PFP::MAP> tF(mapScenary);
			for(Dart d = tF.begin() ; d != tF.end() ; d = tF.next())
			{
Jund Thomas's avatar
Jund Thomas committed
117
118
119
120
121
122
123
124
125
				v.push_back(d);
			}

			std::cout << "builds " << v.size() << std::endl;

			for(std::vector<Dart>::iterator it = v.begin() ; it != v.end() ; it++)
			{
				Dart d = *it;

Jund Thomas's avatar
Jund Thomas committed
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
155
156
157
158
159
160
161
162
163
				//test if convex
				bool convex=true;

				Dart dd = mapScenary.phi1(d);
				Dart dN = mapScenary.phi1(dd);
				Dart dNN = mapScenary.phi1(dN);

				VEC3 p1, p2, p3, v1, v2;
				p1 = positionScenary[dd];
				p2 = positionScenary[dN];
				p3 = positionScenary[dNN];

				v1 = VEC3(p2-p1);
				v2 = VEC3(p3-p2);

				float sign = (v1^v2)[2];

				while(convex && dd!=d)
				{
					dd=dN;
					dN = dNN;
					dNN = mapScenary.phi1(dNN);

					p1 = positionScenary[dd];
					p2 = positionScenary[dN];
					p3 = positionScenary[dNN];

					v1 = VEC3(p2-p1);
					v2 = VEC3(p3-p2);

					float sign2 = (v1^v2)[2];
					if((sign<0 && sign2>0) || (sign>0 && sign2<0))
							convex=false;
				}

				//make extrusion/roof/..
				if(convex)
				{
pitiot's avatar
pitiot committed
164
					if(Algo::Surface::Geometry::convexFaceArea<PFP>(mapScenary, d, positionScenary)<20.0f) //if big enough create building with "stairs"
Jund Thomas's avatar
Jund Thomas committed
165
166
167
168
169
170
171
172
173
174
175
						CityGenerator::extrudeBuilding<PFP>(mapScenary,positionScenary, d,1+rand()%2,10.0f*(1.0f+rand()/RAND_MAX));
					else //create building with simple roof
						CityGenerator::extrudeBuilding<PFP>(mapScenary,positionScenary, d,3,10.0f*(1.0f+rand()/RAND_MAX));
				}
				else //building with no roof
				{
					CityGenerator::extrudeBuilding<PFP>(mapScenary,positionScenary, d,0,5.0f+5.0f*(1.0f+2.0f*rand()/RAND_MAX));
				}
			}

			//triangulation for rendering
pitiot's avatar
pitiot committed
176
			Algo::Surface::Modelisation::EarTriangulation<PFP> et(mapScenary);
Jund Thomas's avatar
Jund Thomas committed
177
178
			et.triangule();

179
180
//			Algo::Modelisation::EarTriangulation<PFP> et2(map);
//			et2.triangule();
Jund Thomas's avatar
Jund Thomas committed
181

Jund Thomas's avatar
Jund Thomas committed
182
			//subdivision to create footpath
183
			SelectorDartNoBoundary<PFP::MAP> nb(map);
pitiot's avatar
pitiot committed
184
			Algo::Surface::Modelisation::TwoNPlusOneSubdivision<PFP,PFP::TVEC3, VEC3>(map,position,nb);
185
186
			markPedWay();

pitiot's avatar
pitiot committed
187
			scale(20.0f);
David Cazier's avatar
David Cazier committed
188
		}
Jund Thomas's avatar
Jund Thomas committed
189
		break;
190
191
192
193
194
195
196
197
198
	}

//	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();
David Cazier's avatar
David Cazier committed
199
200
	// TODO Check registerWallInFaces();
	registerWallInFaces() ;
201
202
//	subdivideAllToMaxLevel();

203
	for (unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
204
		subdivisableFace[i].first = false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
205
#endif
206

pitiot's avatar
pitiot committed
207
208
209
210
211
212
213
214
215
216
217
	if(!normal.isValid())
		normal = map.addAttribute<PFP::VEC3, VERTEX>("normal") ;

	Algo::Surface::Geometry::computeNormalVertices<PFP>(map, position, normal) ;

	if(config==5 && !normalScenary.isValid())
	{
		normalScenary = mapScenary.addAttribute<PFP::VEC3, VERTEX>("normal") ;

		Algo::Surface::Geometry::computeNormalVertices<PFP>(mapScenary, positionScenary, normalScenary) ;
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
218
219
}

220
221
222
223
224
225
226
227
228
229
230
231
void EnvMap::scale(float val)
{
	TraversorV<PFP::MAP> tV(map);
	for(Dart d = tV.begin() ; d != tV.end() ; d = tV.next())
		position[d] *= val;

	TraversorV<PFP::MAP> tV2(mapScenary);
	for(Dart d = tV2.begin() ; d != tV2.end() ; d = tV2.next())
		positionScenary[d] *= val;

}

232
233
void EnvMap::markPedWay()
{
David Cazier's avatar
David Cazier committed
234
	CellMarker<FACE> treat(map) ;
David Cazier's avatar
David Cazier committed
235
236
237
238
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
239
240
241
			treat.mark(d) ;

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
242
243
			do
			{
244
				Dart ddd = dd ;
David Cazier's avatar
David Cazier committed
245
246
247
248
				do
				{
					if (obstacleMark.isMarked(dd))
					{
249
250
						pedWayMark.mark(d) ;
						break ;
251
					}
252
253
					ddd = map.alpha1(ddd) ;
				} while (ddd != dd) ;
254

255
				dd = map.phi1(dd) ;
256

257
			} while (dd != d) ;
258
259
260
		}
	}

261
	treat.unmarkAll() ;
David Cazier's avatar
David Cazier committed
262
263
264
265
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
266
267
268
			treat.mark(d) ;

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
269
270
271
272
273
274
			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))))))
				{
275
276
277
					pedWayMark.mark(d) ;
					break ;
				}
278

279
				dd = map.phi1(dd) ;
280

281
			} while (dd != d) ;
282
283
284
285
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
286
287
unsigned int EnvMap::mapMemoryCost()
{
David Cazier's avatar
David Cazier committed
288
289
290
291
	return (map.getAttributeContainer<DART>()).memorySize()
	    + (map.getAttributeContainer<VERTEX>()).memorySize()
	    + (map.getAttributeContainer<EDGE>()).memorySize()
	    + (map.getAttributeContainer<FACE>()).memorySize() ;
Thomas's avatar
Thomas committed
292
293
}

Pierre Kraemer's avatar
Pierre Kraemer committed
294
#ifndef SPATIAL_HASHING
Thomas's avatar
Thomas committed
295
296
void EnvMap::subdivideAllToMaxLevel()
{
297
	bool subdiv ;
David Cazier's avatar
David Cazier committed
298
299
	do
	{
300
		subdiv = false ;
Thomas's avatar
Thomas committed
301
		{
David Cazier's avatar
David Cazier committed
302
			CellMarker<FACE> subd(map) ;
David Cazier's avatar
David Cazier committed
303
304
305
306
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
307
					subd.mark(d) ;
David Cazier's avatar
David Cazier committed
308
309
					if (!buildingMark.isMarked(d))
					{
Thomas's avatar
Thomas committed
310
311
						//check if subdivision is authorized
						float minDistSq = Agent::neighborDistSq_ ;
312
						bool subdivisable = true ;
Thomas's avatar
Thomas committed
313
314
315
						Dart old = map.faceOldestDart(d) ;
						unsigned int fLevel = map.faceLevel(old) ;
						map.setCurrentLevel(fLevel) ;
pitiot's avatar
pitiot committed
316
						PFP::VEC3 fCenter = Algo::Surface::Geometry::faceCentroid<PFP>(map, old, position) ;
Thomas's avatar
Thomas committed
317
						Dart fd = old ;
David Cazier's avatar
David Cazier committed
318
319
						do
						{
Thomas's avatar
Thomas committed
320
							PFP::VEC3& p = position[fd] ;
pitiot's avatar
pitiot committed
321
							PFP::VEC3 edge = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, fd,
David Cazier's avatar
David Cazier committed
322
							                                                      position) ;
323
							PFP::VEC3 proj = fCenter
David Cazier's avatar
David Cazier committed
324
325
326
							    - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
							if (proj.norm2() < minDistSq)
							{
Thomas's avatar
Thomas committed
327
328
329
330
								subdivisable = false ;
								break ;
							}
							fd = map.phi1(fd) ;
331
						} while (fd != old) ;
Thomas's avatar
Thomas committed
332

David Cazier's avatar
David Cazier committed
333
334
						if (subdivisable)
						{
Thomas's avatar
Thomas committed
335
							map.setCurrentLevel(fLevel) ;
pitiot's avatar
pitiot committed
336
							Algo::Surface::IHM::subdivideFace<PFP>(map, old, position) ;
337
							subdiv = true ;
Thomas's avatar
Thomas committed
338
339
340
341
342
343
						}
						map.setCurrentLevel(map.getMaxLevel()) ;
					}
				}
			}
		}
344
	} while (subdiv) ;
Thomas's avatar
Thomas committed
345
346
347
348
}

void EnvMap::subdivideToProperLevel()
{
349
	bool subdiv ;
David Cazier's avatar
David Cazier committed
350
351
	do
	{
352
		subdiv = false ;
Thomas's avatar
Thomas committed
353
		{
David Cazier's avatar
David Cazier committed
354
			CellMarker<FACE> subd(map) ;
David Cazier's avatar
David Cazier committed
355
356
357
358
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
359
					subd.mark(d) ;
David Cazier's avatar
David Cazier committed
360
361
					if (!refineMark.isMarked(d) && agentvect[d].size() > nbAgentsToSubdivide)
					{
362
						std::pair<bool, bool>& sf = subdivisableFace[d] ;
David Cazier's avatar
David Cazier committed
363
364
						if (sf.first == false || (sf.first == true && sf.second))
						{
365
366
367
							subdiv = true ;
							refineMark.mark(d) ;
							refineCandidate.push_back(d) ;
Thomas's avatar
Thomas committed
368
369
370
371
						}
					}
				}
			}
372
			subd.unmarkAll() ;
Thomas's avatar
Thomas committed
373
		}
374
375
376
377
		updateMap() ;
		refineCandidate.clear() ;
		map.setCurrentLevel(map.getMaxLevel()) ;
	} while (subdiv) ;
Thomas's avatar
Thomas committed
378
379
}

Pierre Kraemer's avatar
Pierre Kraemer committed
380
381
Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
pitiot's avatar
...    
pitiot committed
382
//	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
pitiot's avatar
pitiot committed
383

David Cazier's avatar
David Cazier committed
384
	CellMarkerStore<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
385
386
387
388
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
389
			m.mark(d) ;
pitiot's avatar
pitiot committed
390
			if (!buildingMark.isMarked(d)
pitiot's avatar
pitiot committed
391
			    && Algo::Surface::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true)) return d ;
392
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
393
394
	}

395
	std::cout << "ERROR : pos not in map for getBelongingCell " << pos << std::endl ;
396
	return map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
397
}
Pierre Kraemer's avatar
Pierre Kraemer committed
398
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
399

Pierre Kraemer's avatar
Pierre Kraemer committed
400
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
401
402
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
403
	Dart dd = d ;
David Cazier's avatar
David Cazier committed
404
405
	do
	{
406
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
407
408
		while (ddd != dd)
		{
409
410
			f(ddd) ;
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
411
		}
412
413
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
414
415
}

David Cazier's avatar
David Cazier committed
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
// 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
			{
pitiot's avatar
pitiot committed
433
				if (obstacleMark.isMarked(dd) && map.isBoundaryMarked2(dd))
David Cazier's avatar
David Cazier committed
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
				{
					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
462
463
void EnvMap::registerObstaclesInFaces()
{
David Cazier's avatar
David Cazier committed
464
	CellMarker<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
465
466
467
468
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
469
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
470
471
			if (!buildingMark.isMarked(d))
			{
472
				Dart dd = d ;
473
474
475
476

				//retrieve all obstacles within its one-ring

				//first : test all edges of the face of d
David Cazier's avatar
David Cazier committed
477
478
479
480
				do
				{
					if (obstacleMark.isMarked(dd))
					{
481
482
483
484
						Dart dd2 = map.phi2(dd) ;
						Dart next = map.phi1(dd2) ;
						Dart previous = map.phi_1(dd2) ;
						Obstacle* o = new Obstacle(position[dd2], position[next],
David Cazier's avatar
David Cazier committed
485
486
						                           position[previous], position[map.phi1(next)],
						                           NULL, 0) ;
487
						obstvect[d].push_back(o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
488
					}
489
490
					dd = map.phi1(dd) ;
				} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
491

492
				//second : test all edges of neighboring faces
David Cazier's avatar
David Cazier committed
493
494
				do
				{
495
					Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
496
497
					while (ddd != dd)
					{
David Cazier's avatar
David Cazier committed
498
						// TODO Check effet de bord
David Cazier's avatar
David Cazier committed
499
500
						if (!buildingMark.isMarked(ddd)) addNeighborObstacles(
						    obstvect[d], ddd, ddd == map.alpha_1(dd)) ;
501
						ddd = map.alpha1(ddd) ;
502
					}
503
504
					dd = map.phi1(dd) ;
				} while (dd != d) ;
505
506
507
			}
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
508
509
510
511
}

void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor)
{
512
	Dart stop ;
David Cazier's avatar
David Cazier committed
513
514
515
516
	if (edgeNeighbor)
		stop = map.phi_1(map.phi_1(map.phi_1(d))) ;
	else
		stop = map.phi_1(map.phi_1(d)) ;
517
518

	Dart dd = d ;
David Cazier's avatar
David Cazier committed
519
520
521
522
	do
	{
		if (obstacleMark.isMarked(dd))
		{
523
524
525
526
527
528
529
530
531
532
//			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
//			{
533
534
535
536
			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],
David Cazier's avatar
David Cazier committed
537
			                           position[map.phi1(next)], NULL, 0) ;
538
			obst.push_back(o) ;
539
//			}
Pierre Kraemer's avatar
Pierre Kraemer committed
540
		}
541
542
		dd = map.phi1(dd) ;
	} while (dd != stop) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
543
544
}

545
546
547
548
549
550
void EnvMap::pushObstacleInCells(Obstacle * o)// réenregistre l'obstacle en question
{
	MovingObstacle * mo = o->mo;
	if (mo != NULL)
	{
		int n = o->index;
pitiot's avatar
pitiot committed
551
552
		VEC3 p1 = o->p1;
		VEC3 p2 =  o->p2;
553
554
555
556

		Dart d1=NIL;
		Dart d2=NIL;
		std::vector<Dart> memo;
pitiot's avatar
pitiot committed
557
		d1=mo->parts_[n]->d;
pitiot's avatar
pitiot committed
558

pitiot's avatar
pitiot committed
559
		memo = mo->getMemoCross(p1,p2,d1,d2);
560

pitiot's avatar
pitiot committed
561

pitiot's avatar
pitiot committed
562
563
564
565
566
		if(map.sameFace(d1,d2))
		{
			pushObstacleInOneRingCells(o , d1 , n);
		}
		else
567
		{
pitiot's avatar
pitiot committed
568

569
570
571
572
573
			pushObstacleInCells(o, n, memo);
		}
	}
}

pitiot's avatar
pitiot committed
574
Dart EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistrement
575
576
{
	MovingObstacle * mo = o->mo;
pitiot's avatar
pitiot committed
577
578
	VEC3 p1 = o->p1;
	VEC3 p2 = o->p2;
579
580
581
582

	Dart d1=NIL;
	Dart d2=NIL;
	std::vector<Dart> memo;
pitiot's avatar
pitiot committed
583
	d1=mo->parts_[n]->d;
584
585
586
587
588
//	bool modif=false;

//	if(p1->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS
//		|| p2->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//	{
pitiot's avatar
pitiot committed
589
590
		memo = mo->getMemoCross(p1,p2,d1,d2);

591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
//		memo.sort();
//		modif=true;
//	}
//	else if(!map.sameFace(p1->d,p2->d))
//	{
//		memo = getMemoCross(p1,p2->getPosition());
//		memo.sort();
//	 	if(belonging_cells[n] != memo)
//			modif=true;
//	}
//
//	if(modif)
	{
		popObstacleInCells(o, n);

pitiot's avatar
pitiot committed
606
607
608
609
610
		if(map.sameFace(d1,d2))
		{
			pushObstacleInOneRingCells(o , d1 , n);
		}
		else
611
612
613
614
		{
			pushObstacleInCells(o, n, memo);
		}
	}
pitiot's avatar
pitiot committed
615
616

	return d1;
617
618
619
620
621
622
623
624
625
626
}

void EnvMap::pushObstacleInOneRingCells(Obstacle * o, Dart d, int n)
{
	MovingObstacle * mo = o->mo;

	assert(map.getCurrentLevel() == map.getMaxLevel());

	addElementToVector<Obstacle*>(obstvect[d],o);

pitiot's avatar
pitiot committed
627
//	mo->belonging_cells[n].clear();
628
	mo->belonging_cells[n].push_back(d);
pitiot's avatar
pitiot committed
629
//	mo->neighbor_cells[n].clear();
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654

	Dart dd = d;
	do
	{
		Dart ddd = map.alpha1(map.alpha1(dd));
		while(ddd != dd)
		{
			pushObstNeighborInCells(o, ddd);

			mo->neighbor_cells[n].push_back(ddd);
			ddd = map.alpha1(ddd);
		}
		dd = map.phi1(dd);
	} while(dd != d);
}

void EnvMap::pushObstacleInCells(Obstacle* o, int n, const std::vector<Dart>& memo_cross)
{
	if(memo_cross.empty())
	{
		displayMO(o);
	}
	assert(!memo_cross.empty());
	MovingObstacle * mo = o->mo;

pitiot's avatar
pitiot committed
655
656
//	mo->belonging_cells[n].clear();
//	mo->neighbor_cells[n].clear();
657
658
659
660
661
662
663
664
665
666
667
668
669
	mo->belonging_cells[n] = memo_cross;

	for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin();	it != mo->belonging_cells[n].end(); ++it)
	{
		//		 CGoGNout <<"obstacle"<< o->p1 << "new cell : "<< *it << CGoGNendl;
		addElementToVector<Obstacle*>(obstvect[*it],o);
	}

	addObstAsNeighbor(o, mo->belonging_cells[n], &(mo->neighbor_cells[n]));

	for (std::vector<Dart>::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it)
	{
		//		 CGoGNout <<"obstacle"<< o->p1 << "new cell : "<< *it << CGoGNendl;
Jund Thomas's avatar
Jund Thomas committed
670
671
		addElementToVector<Obstacle*>(neighborObstvect[*it],o);
//		pushObstNeighborInCells(o, *it);
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
	}
}

void EnvMap::popObstacleInCells(Obstacle* o, int n)
{
	MovingObstacle * mo = o->mo;

	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

	if (mo != NULL)
	{
		for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin();	it != mo->belonging_cells[n].end(); ++it)
		{
			removeElementFromVector<Obstacle*>(obstvect[*it], o) ;
		}

		for (std::vector<Dart>::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it)
		{
			removeElementFromVector<Obstacle*>(neighborObstvect[*it], o) ;
		}
pitiot's avatar
pitiot committed
692
693
		mo->belonging_cells[n].clear();
		mo->neighbor_cells[n].clear();
694
695
696
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
697
698
void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
699
	Dart newFace = agent->part_.d ;
700

701
702
	popAgentInCells(agent, oldFace) ;
	pushAgentInCells(agent, newFace) ;
703

David Cazier's avatar
David Cazier committed
704
705
	if (!refineMark.isMarked(newFace) && agentvect[newFace].size() > nbAgentsToSubdivide)
	{
706
		std::pair<bool, bool>& sf = subdivisableFace[newFace] ;
David Cazier's avatar
David Cazier committed
707
708
		if (sf.first == false || (sf.first == true && sf.second))
		{
709
710
			refineMark.mark(newFace) ;
			refineCandidate.push_back(newFace) ;
711
712
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
713

David Cazier's avatar
David Cazier committed
714
715
716
	if (!coarsenMark.isMarked(oldFace) && !refineMark.isMarked(oldFace)
	    && 4 * agentvect[oldFace].size() < nbAgentsToSimplify)
	{
717
718
		coarsenMark.mark(oldFace) ;
		coarsenCandidate.push_back(map.faceOldestDart(oldFace)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
719
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
720
721
}

722
void EnvMap::refine()
Pierre Kraemer's avatar
Pierre Kraemer committed
723
{
724
	for (std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end(); ++it)
David Cazier's avatar
David Cazier committed
725
726
	{
		Dart d = (*it) ;
727
		refineMark.unmark(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
728

729
		//if the number of agents is big enough
David Cazier's avatar
David Cazier committed
730
731
		if (agentvect[d].size() > nbAgentsToSubdivide)
		{
Thomas's avatar
Thomas committed
732
			int fLevel = -1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
733
734
			Dart old = map.faceOldestDart(d) ;

735
			bool subdivisable = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
736

737
			//check if faces resulting from a subdivision are big enough
738
			std::pair<bool, bool>& sf = subdivisableFace[old] ;
David Cazier's avatar
David Cazier committed
739
740
741
742
			if (sf.first == true)
				subdivisable = sf.second ;
			else
			{
pitiot's avatar
pitiot committed
743
744
//				float minSizeSq = minCellSize * minCellSize ; // diametre de vision de l'agent au carré
				float minSizeSq = Agent::neighborDistSq_;
745

Pierre Kraemer's avatar
Pierre Kraemer committed
746
				fLevel = map.faceLevel(old) ;
747
				map.setCurrentLevel(fLevel) ;
pitiot's avatar
pitiot committed
748
				PFP::VEC3 fCenter = Algo::Surface::Geometry::faceCentroid<PFP>(map, old, position) ;
749
				Dart fd = old ;
David Cazier's avatar
David Cazier committed
750
751
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
752
					PFP::VEC3& p = position[fd] ;
pitiot's avatar
pitiot committed
753
					PFP::VEC3 edge = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
David Cazier's avatar
David Cazier committed
754
755
756
					PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
					if (proj.norm2() < minSizeSq)
					{
757
758
						subdivisable = false ;
						break ;
Pierre Kraemer's avatar
Pierre Kraemer committed
759
					}
760
					fd = map.phi1(fd) ;
761
				} while (fd != old) ;
762
763
				map.setCurrentLevel(map.getMaxLevel()) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
764
765
				sf.first = true ;
				sf.second = subdivisable ;
766
767
			}

David Cazier's avatar
David Cazier committed
768
769
			if (subdivisable)
			{
770
771
				if (fLevel == -1)
					fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
772
773

				sf.first = false ;
774

775
				PFP::AGENTS oldAgents(agentvect[old]) ;
David Cazier's avatar
David Cazier committed
776
777
				PFP::OBSTACLEVECT oldObst(obstvect[old]) ;
				PFP::OBSTACLEVECT oldNeighborObst(neighborObstvect[old]) ;
David Cazier's avatar
David Cazier committed
778
779
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
					popAgentInCells(*ait, old) ;
David Cazier's avatar
David Cazier committed
780
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
781
782
783
					this->popObstacleInCells(*ait, (*ait)->index) ;
				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();ait != oldNeighborObst.end(); ++ait)
					this->popObstacleInCells(*ait, (*ait)->index) ;
784
				neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
785
786

				map.setCurrentLevel(fLevel) ;
pitiot's avatar
pitiot committed
787
				Algo::Surface::IHM::subdivideFace<PFP>(map, old, position) ;
David Cazier's avatar
David Cazier committed
788
				CellMarkerStore<FACE> newF(map) ;
789
790
				unsigned int degree = 0 ;
				Dart dd = old ;
David Cazier's avatar
David Cazier committed
791
792
				do
				{
793
					++degree ;
794
					newF.mark(dd) ;
795
					dd = map.phi1(dd) ;
796
				} while (dd != old) ;
David Cazier's avatar
David Cazier committed
797

David Cazier's avatar
David Cazier committed
798
799
				if (degree == 3)
				{
800
					Dart centerFace = map.phi2(map.phi1(old)) ;
801
					newF.mark(centerFace) ;
802
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
803
				map.setCurrentLevel(map.getMaxLevel()) ;
804

805
				//retrieve neighbors agents from onering cells
806
				dd = old ;
David Cazier's avatar
David Cazier committed
807
808
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
809
					Dart d3 = dd ;
David Cazier's avatar
David Cazier committed
810
811
					do
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
812
813
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
David Cazier's avatar
David Cazier committed
814
815
816
817
						while (d4 != d3)
						{
							if (!newF.isMarked(d4))
							{
Pierre Kraemer's avatar
Pierre Kraemer committed
818
819
820
821
								PFP::AGENTS& ad4 = agentvect[d4] ;
								nad3.insert(nad3.end(), ad4.begin(), ad4.end()) ;
							}
							d4 = map.alpha1(d4) ;
822
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
823
						d3 = map.phi1(d3) ;
824
					} while (d3 != dd) ;
825
826

					map.setCurrentLevel(fLevel) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
827
					dd = map.phi1(dd) ;
828
					map.setCurrentLevel(map.getMaxLevel()) ;
829
				} while (dd != old) ;
830

David Cazier's avatar
David Cazier committed
831
832
				if (degree == 3)
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
833
834
					Dart centerFace = map.phi2(map.phi1(old)) ;
					Dart d3 = centerFace ;
David Cazier's avatar
David Cazier committed
835
836
					do
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
837
838
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
David Cazier's avatar
David Cazier committed
839
840
841
842
						while (d4 != d3)
						{
							if (!newF.isMarked(d4))
							{
Pierre Kraemer's avatar
Pierre Kraemer committed
843
								PFP::AGENTS& ad4 = agentvect[d4] ;
David Cazier's avatar
David Cazier committed
844
								nad3.insert(neighborAgentvect[d3].end(), ad4.begin(), ad4.end()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
845
846
							}
							d4 = map.alpha1(d4) ;
847
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
848
						d3 = map.phi1(d3) ;
849
					} while (d3 != centerFace) ;
850
				}
pitiot's avatar
pitiot committed
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876

				//agents contained in the subdivided cell are pushed correctly
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
				{

					resetAgentInFace(*ait) ;
					pushAgentInCells(*ait, (*ait)->part_.d) ;
				}

				//same for obstacles contained
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
				{
					resetPartSubdiv(*ait);
					pushObstacleInCells(*ait);

				}

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


//				CGoGNout<<"fin refine"<<CGoGNendl;
Pierre Kraemer's avatar
Pierre Kraemer committed
877
878
879
			}
		}
	}
David Cazier's avatar
David Cazier committed
880
881
	refineCandidate.clear() ;

882
}
883
884


885
886
void EnvMap::coarse()
{
David Cazier's avatar
David Cazier committed
887
888
	// On recrée une liste des faces à simplifier en empêchant les doublons
	// On en profite pour vérifier les conditions de simplifications
889
890
	std::vector<Dart> checkCoarsenCandidate ;
	checkCoarsenCandidate.reserve(coarsenCandidate.size()) ;
891

892
893
894
895
896
897
898
	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) ;
899

900
901
902
903
		if (oldIsMarked && fLevel > 0 && map.getDartLevel(old) < fLevel)
		{
			unsigned int cur = map.getCurrentLevel() ;
			map.setCurrentLevel(fLevel - 1) ;
904

905
			if (map.faceIsSubdividedOnce(old))
906
			{
907
908
909
910
911
912
913
914
915
916
917
918
919
920
				// 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())
David Cazier's avatar
David Cazier committed
921
//					{
922
923
924
925
926
927
928
//						if(map.getEmbedding<FACE>(coarsenCandidate[start]) == fEmb)
//						{
//							coarsenCandidate[start] = coarsenCandidate.back() ;
//							coarsenCandidate.pop_back() ;
//						}
//						else
//							++start ;
David Cazier's avatar
David Cazier committed
929
//					}
930
931
932
933

					fit = map.phi1(fit) ;
				} while (fit != old) ;

934

935
936
				//Loop subdivision
				if (degree == 3)
937
				{
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
					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) ;
956
				}
957
				if (nbAgents < nbAgentsToSimplify) checkCoarsenCandidate.push_back(old) ;
958
			}
959
			map.setCurrentLevel(cur) ;
960
		}
961
962
	}
	coarsenCandidate.clear() ;
963

964
//		 On réalise la simplification (les conditions ont déjà été vérifiées)
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
	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 ;
pitiot's avatar
pitiot committed
985
		//premier tour pour les darts qui vont disparaitre
986
987
		fit = old ;
		do
988
		{
989
990
991
992
993
994
995
			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)
996
				{
997
					if ((*ait)->part_.d == map.phi1(nf)) (*ait)->part_.d = nf ;
998
				}
999
				for (PFP::OBSTACLEVECT::iterator ait = resetob.begin(); ait != resetob.end(); ++ait)
1000
				{
pitiot's avatar
pitiot committed
1001
1002
1003

						resetPart(*ait,nf) ;

1004
1005
				}
				map.setCurrentLevel(fLevel - 1) ;
1006
			}
pitiot's avatar
pitiot committed
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031

			fit = map.phi1(fit) ;
		} while (fit != old) ;
		//deuxieme 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)
			{
				this->popObstacleInCells(*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) ;

1032
1033
			fit = map.phi1(fit) ;
		} while (fit != old) ;
1034

1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
		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)
1045
			{
1046
1047
1048
1049
1050
1051
1052
				this->popObstacleInCells(*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) ;
		}
1053

pitiot's avatar
pitiot committed
1054
		//dernier tour concernant les voisins
1055
1056
1057
1058
		fit = old ;
		do
		{
			PFP::OBSTACLEVECT nob(neighborObstvect[fit]) ;
1059

1060
1061
1062
1063
1064
1065
1066
			map.setCurrentLevel(map.getMaxLevel()) ;
			for(PFP::OBSTACLEVECT::iterator ait = nob.begin(); ait != nob.end(); ++ait)
			{
				this->popObstacleInCells(*ait, (*ait)->index) ;
				neighborObst.push_back(*ait);
			}
			map.setCurrentLevel(fLevel - 1) ;
1067

1068
1069
			fit = map.phi1(fit) ;
		} while (fit != old) ;
1070

1071
1072
1073