Coupure prévue mardi 3 Août au matin pour maintenance du serveur. Nous faisons au mieux pour que celle-ci soit la plus brève possible.

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

			std::string filename2 = "./svg/mapBuild.svg" ;
			Algo::Import::importSVG<PFP>(mapScenary, filename2, positionScenary, obstacleMarkS, buildingMarkS) ;

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

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

			std::vector<Dart> v;
Jund Thomas's avatar
Jund Thomas committed
113
114
115
			TraversorF<PFP::MAP> tF(mapScenary);
			for(Dart d = tF.begin() ; d != tF.end() ; d = tF.next())
			{
Jund Thomas's avatar
Jund Thomas committed
116
117
118
119
120
121
122
123
124
				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
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
				//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)
				{
					if(Algo::Geometry::convexFaceArea<PFP>(mapScenary, d, positionScenary)<20.0f) //if big enough create building with "stairs"
						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
			Algo::Modelisation::EarTriangulation<PFP> et(mapScenary);
			et.triangule();

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

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

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

//	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
198
199
	// TODO Check registerWallInFaces();
	registerWallInFaces() ;
200
201
//	subdivideAllToMaxLevel();

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

Pierre Kraemer's avatar
Pierre Kraemer committed
206
207
}

208
209
210
211
212
213
214
215
216
217
218
219
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;

}

220
221
void EnvMap::markPedWay()
{
David Cazier's avatar
David Cazier committed
222
	CellMarker<FACE> treat(map) ;
David Cazier's avatar
David Cazier committed
223
224
225
226
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
227
228
229
			treat.mark(d) ;

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
230
231
			do
			{
232
				Dart ddd = dd ;
David Cazier's avatar
David Cazier committed
233
234
235
236
				do
				{
					if (obstacleMark.isMarked(dd))
					{
237
238
						pedWayMark.mark(d) ;
						break ;
239
					}
240
241
					ddd = map.alpha1(ddd) ;
				} while (ddd != dd) ;
242

243
				dd = map.phi1(dd) ;
244

245
			} while (dd != d) ;
246
247
248
		}
	}

249
	treat.unmarkAll() ;
David Cazier's avatar
David Cazier committed
250
251
252
253
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
254
255
256
			treat.mark(d) ;

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
257
258
259
260
261
262
			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))))))
				{
263
264
265
					pedWayMark.mark(d) ;
					break ;
				}
266

267
				dd = map.phi1(dd) ;
268

269
			} while (dd != d) ;
270
271
272
273
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
274
275
unsigned int EnvMap::mapMemoryCost()
{
David Cazier's avatar
David Cazier committed
276
277
278
279
	return (map.getAttributeContainer<DART>()).memorySize()
	    + (map.getAttributeContainer<VERTEX>()).memorySize()
	    + (map.getAttributeContainer<EDGE>()).memorySize()
	    + (map.getAttributeContainer<FACE>()).memorySize() ;
Thomas's avatar
Thomas committed
280
281
}

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

David Cazier's avatar
David Cazier committed
321
322
						if (subdivisable)
						{
Thomas's avatar
Thomas committed
323
324
							map.setCurrentLevel(fLevel) ;
							Algo::IHM::subdivideFace<PFP>(map, old, position) ;
325
							subdiv = true ;
Thomas's avatar
Thomas committed
326
327
328
329
330
331
						}
						map.setCurrentLevel(map.getMaxLevel()) ;
					}
				}
			}
		}
332
	} while (subdiv) ;
Thomas's avatar
Thomas committed
333
334
335
336
}

void EnvMap::subdivideToProperLevel()
{
337
	bool subdiv ;
David Cazier's avatar
David Cazier committed
338
339
	do
	{
340
		subdiv = false ;
Thomas's avatar
Thomas committed
341
		{
David Cazier's avatar
David Cazier committed
342
			CellMarker<FACE> subd(map) ;
David Cazier's avatar
David Cazier committed
343
344
345
346
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
347
					subd.mark(d) ;
David Cazier's avatar
David Cazier committed
348
349
					if (!refineMark.isMarked(d) && agentvect[d].size() > nbAgentsToSubdivide)
					{
350
						std::pair<bool, bool>& sf = subdivisableFace[d] ;
David Cazier's avatar
David Cazier committed
351
352
						if (sf.first == false || (sf.first == true && sf.second))
						{
353
354
355
							subdiv = true ;
							refineMark.mark(d) ;
							refineCandidate.push_back(d) ;
Thomas's avatar
Thomas committed
356
357
358
359
						}
					}
				}
			}
360
			subd.unmarkAll() ;
Thomas's avatar
Thomas committed
361
		}
362
363
364
365
		updateMap() ;
		refineCandidate.clear() ;
		map.setCurrentLevel(map.getMaxLevel()) ;
	} while (subdiv) ;
Thomas's avatar
Thomas committed
366
367
}

Pierre Kraemer's avatar
Pierre Kraemer committed
368
369
Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
pitiot's avatar
...  
pitiot committed
370
//	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
pitiot's avatar
pitiot committed
371

David Cazier's avatar
David Cazier committed
372
	CellMarkerStore<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
373
374
375
376
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
377
			m.mark(d) ;
pitiot's avatar
pitiot committed
378
			if (!buildingMark.isMarked(d)
David Cazier's avatar
David Cazier committed
379
			    && Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true)) return d ;
380
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
381
382
	}

383
	std::cout << "ERROR : pos not in map for getBelongingCell " << pos << std::endl ;
384
	return map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
385
}
Pierre Kraemer's avatar
Pierre Kraemer committed
386
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
387

Pierre Kraemer's avatar
Pierre Kraemer committed
388
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
389
390
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
391
	Dart dd = d ;
David Cazier's avatar
David Cazier committed
392
393
	do
	{
394
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
395
396
		while (ddd != dd)
		{
397
398
			f(ddd) ;
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
399
		}
400
401
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
402
403
}

David Cazier's avatar
David Cazier committed
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
// TODO Check void EnvMap::registerWallInFaces() boundary markers ?
void EnvMap::registerWallInFaces()
{
	CellMarker<FACE> m(map) ;
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
			m.mark(d) ;

			Dart dd = d ;

			//retrieve all obstacles within its one-ring

			//first : test all edges of the face of d
			do
			{
				if (obstacleMark.isMarked(dd) && map.isBoundaryMarked(dd))
				{
					Dart dd2 = map.phi2(dd) ;
					Dart next = map.phi1(dd2) ;
					Dart previous = map.phi_1(dd2) ;
					Obstacle* o = new Obstacle(position[next], position[dd2], position[previous],
					                           position[map.phi1(next)], NULL, 0) ;
					obstvect[dd2].push_back(o) ;
				}
				dd = map.phi1(dd) ;
			} while (dd != d) ;

//				//second : test all edges of neighboring faces
//				do
//				{
//					Dart ddd = map.alpha1(map.alpha1(dd));
//					while(ddd != dd)
//					{
//						if(!buildingMark.isMarked(ddd))
//							addNeighborObstacles(obstvect[d], ddd, ddd == map.alpha_1(dd));
//						ddd = map.alpha1(ddd);
//					}
//					dd = map.phi1(dd);
//				} while(dd != d);
		}
	}

}

Pierre Kraemer's avatar
Pierre Kraemer committed
450
451
void EnvMap::registerObstaclesInFaces()
{
David Cazier's avatar
David Cazier committed
452
	CellMarker<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
453
454
455
456
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
457
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
458
459
			if (!buildingMark.isMarked(d))
			{
460
				Dart dd = d ;
461
462
463
464

				//retrieve all obstacles within its one-ring

				//first : test all edges of the face of d
David Cazier's avatar
David Cazier committed
465
466
467
468
				do
				{
					if (obstacleMark.isMarked(dd))
					{
469
470
471
472
						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
473
474
						                           position[previous], position[map.phi1(next)],
						                           NULL, 0) ;
475
						obstvect[d].push_back(o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
476
					}
477
478
					dd = map.phi1(dd) ;
				} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
479

480
				//second : test all edges of neighboring faces
David Cazier's avatar
David Cazier committed
481
482
				do
				{
483
					Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
484
485
					while (ddd != dd)
					{
David Cazier's avatar
David Cazier committed
486
						// TODO Check effet de bord
David Cazier's avatar
David Cazier committed
487
488
						if (!buildingMark.isMarked(ddd)) addNeighborObstacles(
						    obstvect[d], ddd, ddd == map.alpha_1(dd)) ;
489
						ddd = map.alpha1(ddd) ;
490
					}
491
492
					dd = map.phi1(dd) ;
				} while (dd != d) ;
493
494
495
			}
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
496
497
498
499
}

void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor)
{
500
	Dart stop ;
David Cazier's avatar
David Cazier committed
501
502
503
504
	if (edgeNeighbor)
		stop = map.phi_1(map.phi_1(map.phi_1(d))) ;
	else
		stop = map.phi_1(map.phi_1(d)) ;
505
506

	Dart dd = d ;
David Cazier's avatar
David Cazier committed
507
508
509
510
	do
	{
		if (obstacleMark.isMarked(dd))
		{
511
512
513
514
515
516
517
518
519
520
//			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
//			{
521
522
523
524
			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
525
			                           position[map.phi1(next)], NULL, 0) ;
526
			obst.push_back(o) ;
527
//			}
Pierre Kraemer's avatar
Pierre Kraemer committed
528
		}
529
530
		dd = map.phi1(dd) ;
	} while (dd != stop) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
531
532
}

533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
void EnvMap::pushObstacleInCells(Obstacle * o)// réenregistre l'obstacle en question
{
	MovingObstacle * mo = o->mo;
	if (mo != NULL)
	{
		int n = o->index;
		VEC3 p1 = mo->vertices[n];
		VEC3 p2 = mo->vertices[(n+1)%(mo->nbVertices)];

		Dart d1=NIL;
		Dart d2=NIL;
		std::vector<Dart> memo;
		memo = mo->getMemoCross(p1,p2,d1);
		d2=mo->registering_part->d;

		if(map.sameFace(d1,d2))
		{
			pushObstacleInOneRingCells(o , d1 , n);
		}
		else
		{
			pushObstacleInCells(o, n, memo);
		}
	}
}

void EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistrement
{
	MovingObstacle * mo = o->mo;
	VEC3 p1 = mo->vertices[n];
	VEC3 p2 = mo->vertices[(n+1)%mo->nbVertices];

	Dart d1=NIL;
	Dart d2=NIL;
	std::vector<Dart> memo;

//	bool modif=false;

//	if(p1->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS
//		|| p2->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//	{
		memo = mo->getMemoCross(p1,p2,d1);
		d2=mo->registering_part->d;
//		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);

		if(map.sameFace(d1,d2))
		{
			pushObstacleInOneRingCells(o , d1 , n);
		}
		else
		{
			pushObstacleInCells(o, n, memo);
		}
	}
}

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

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

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

	mo->belonging_cells[n].clear();
	mo->belonging_cells[n].push_back(d);
	mo->neighbor_cells[n].clear();

	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;

	mo->belonging_cells[n].clear();
	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
652
653
		addElementToVector<Obstacle*>(neighborObstvect[*it],o);
//		pushObstNeighborInCells(o, *it);
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
	}
}

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) ;
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
677
678
void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
679
	Dart newFace = agent->part_.d ;
680

681
682
	popAgentInCells(agent, oldFace) ;
	pushAgentInCells(agent, newFace) ;
683

David Cazier's avatar
David Cazier committed
684
685
	if (!refineMark.isMarked(newFace) && agentvect[newFace].size() > nbAgentsToSubdivide)
	{
686
		std::pair<bool, bool>& sf = subdivisableFace[newFace] ;
David Cazier's avatar
David Cazier committed
687
688
		if (sf.first == false || (sf.first == true && sf.second))
		{
689
690
			refineMark.mark(newFace) ;
			refineCandidate.push_back(newFace) ;
691
692
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
693

David Cazier's avatar
David Cazier committed
694
695
696
	if (!coarsenMark.isMarked(oldFace) && !refineMark.isMarked(oldFace)
	    && 4 * agentvect[oldFace].size() < nbAgentsToSimplify)
	{
697
698
		coarsenMark.mark(oldFace) ;
		coarsenCandidate.push_back(map.faceOldestDart(oldFace)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
699
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
700
701
}

702
void EnvMap::refine()
Pierre Kraemer's avatar
Pierre Kraemer committed
703
{
704
	for (std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end(); ++it)
David Cazier's avatar
David Cazier committed
705
706
	{
		Dart d = (*it) ;
707
		refineMark.unmark(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
708

709
		//if the number of agents is big enough
David Cazier's avatar
David Cazier committed
710
711
		if (agentvect[d].size() > nbAgentsToSubdivide)
		{
Thomas's avatar
Thomas committed
712
			int fLevel = -1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
713
714
			Dart old = map.faceOldestDart(d) ;

715
			bool subdivisable = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
716

717
			//check if faces resulting from a subdivision are big enough
718
			std::pair<bool, bool>& sf = subdivisableFace[old] ;
David Cazier's avatar
David Cazier committed
719
720
721
722
			if (sf.first == true)
				subdivisable = sf.second ;
			else
			{
723
				float minSizeSq = minCellSize * minCellSize ; // diametre de vision de l'agent au carré
724

Pierre Kraemer's avatar
Pierre Kraemer committed
725
				fLevel = map.faceLevel(old) ;
726
727
728
				map.setCurrentLevel(fLevel) ;
				PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
				Dart fd = old ;
David Cazier's avatar
David Cazier committed
729
730
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
731
					PFP::VEC3& p = position[fd] ;
732
					PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
David Cazier's avatar
David Cazier committed
733
734
735
					PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
					if (proj.norm2() < minSizeSq)
					{
736
737
						subdivisable = false ;
						break ;
Pierre Kraemer's avatar
Pierre Kraemer committed
738
					}
739
					fd = map.phi1(fd) ;
740
				} while (fd != old) ;
741
742
				map.setCurrentLevel(map.getMaxLevel()) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
743
744
				sf.first = true ;
				sf.second = subdivisable ;
745
746
			}

David Cazier's avatar
David Cazier committed
747
748
			if (subdivisable)
			{
749
750
				if (fLevel == -1)
					fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
751
752

				sf.first = false ;
753

754
				PFP::AGENTS oldAgents(agentvect[old]) ;
David Cazier's avatar
David Cazier committed
755
756
				PFP::OBSTACLEVECT oldObst(obstvect[old]) ;
				PFP::OBSTACLEVECT oldNeighborObst(neighborObstvect[old]) ;
David Cazier's avatar
David Cazier committed
757
758
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
					popAgentInCells(*ait, old) ;
David Cazier's avatar
David Cazier committed
759
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
760
761
762
					this->popObstacleInCells(*ait, (*ait)->index) ;
				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();ait != oldNeighborObst.end(); ++ait)
					this->popObstacleInCells(*ait, (*ait)->index) ;
763
				neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
764
765

				map.setCurrentLevel(fLevel) ;
766
				Algo::IHM::subdivideFace<PFP>(map, old, position) ;
David Cazier's avatar
David Cazier committed
767
				CellMarkerStore<FACE> newF(map) ;
768
769
				unsigned int degree = 0 ;
				Dart dd = old ;
David Cazier's avatar
David Cazier committed
770
771
				do
				{
772
					++degree ;
773
					newF.mark(dd) ;
774
					dd = map.phi1(dd) ;
775
				} while (dd != old) ;
David Cazier's avatar
David Cazier committed
776

David Cazier's avatar
David Cazier committed
777
778
				if (degree == 3)
				{
779
					Dart centerFace = map.phi2(map.phi1(old)) ;
780
					newF.mark(centerFace) ;
781
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
782
				map.setCurrentLevel(map.getMaxLevel()) ;
783

784
				//agents contained in the subdivided cell are pushed correctly
David Cazier's avatar
David Cazier committed
785
786
787
788
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
				{
					resetAgentInFace(*ait) ;
					pushAgentInCells(*ait, (*ait)->part_.d) ;
789
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
790

791
				//same for obstacles contained
David Cazier's avatar
David Cazier committed
792
793
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
				{
794
					resetPartSubdiv(*ait);
795
					pushObstacleInCells(*ait);
pitiot's avatar
maj  
pitiot committed
796

David Cazier's avatar
David Cazier committed
797
798
				}

799
				//same for adjacent obstacles
David Cazier's avatar
David Cazier committed
800
				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();
801
					ait != oldNeighborObst.end(); ++ait)
David Cazier's avatar
David Cazier committed
802
				{
803
					pushObstacleInCells(*ait) ;
David Cazier's avatar
David Cazier committed
804
805
				}

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

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

David Cazier's avatar
David Cazier committed
832
833
				if (degree == 3)
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
834
835
					Dart centerFace = map.phi2(map.phi1(old)) ;
					Dart d3 = centerFace ;
David Cazier's avatar
David Cazier committed
836
837
					do
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
838
839
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
David Cazier's avatar
David Cazier committed
840
841
842
843
						while (d4 != d3)
						{
							if (!newF.isMarked(d4))
							{
Pierre Kraemer's avatar
Pierre Kraemer committed
844
								PFP::AGENTS& ad4 = agentvect[d4] ;
David Cazier's avatar
David Cazier committed
845
								nad3.insert(neighborAgentvect[d3].end(), ad4.begin(), ad4.end()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
846
847
							}
							d4 = map.alpha1(d4) ;
848
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
849
						d3 = map.phi1(d3) ;
850
					} while (d3 != centerFace) ;
851
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
852
853
854
			}
		}
	}
David Cazier's avatar
David Cazier committed
855
856
	refineCandidate.clear() ;

857
}
858
859


860
861
void EnvMap::coarse()
{
David Cazier's avatar
David Cazier committed
862
863
	// On recrée une liste des faces à simplifier en empêchant les doublons
	// On en profite pour vérifier les conditions de simplifications
864
865
	std::vector<Dart> checkCoarsenCandidate ;
	checkCoarsenCandidate.reserve(coarsenCandidate.size()) ;
866

867
868
869
870
871
872
873
	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) ;
874

875
876
877
878
		if (oldIsMarked && fLevel > 0 && map.getDartLevel(old) < fLevel)
		{
			unsigned int cur = map.getCurrentLevel() ;
			map.setCurrentLevel(fLevel - 1) ;
879

880
			if (map.faceIsSubdividedOnce(old))
881
			{
882
883
884
885
886
887
888
889
890
891
892
893
894
895
				// 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
896
//					{
897
898
899
900
901
902
903
//						if(map.getEmbedding<FACE>(coarsenCandidate[start]) == fEmb)
//						{
//							coarsenCandidate[start] = coarsenCandidate.back() ;
//							coarsenCandidate.pop_back() ;
//						}
//						else
//							++start ;
David Cazier's avatar
David Cazier committed
904
//					}
905
906
907
908

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

909

910
911
				//Loop subdivision
				if (degree == 3)
912
				{
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
					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) ;
931
				}
932
				if (nbAgents < nbAgentsToSimplify) checkCoarsenCandidate.push_back(old) ;
933
			}
934
			map.setCurrentLevel(cur) ;
935
		}
936
937
	}
	coarsenCandidate.clear() ;
938

939
//		 On réalise la simplification (les conditions ont déjà été vérifiées)
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
	for (unsigned int it = 0; it < checkCoarsenCandidate.size(); ++it)
	{
		Dart old = checkCoarsenCandidate[it] ;

		unsigned int fLevel = map.faceLevel(old) ;
		unsigned int cur = map.getCurrentLevel() ;
		map.setCurrentLevel(fLevel - 1) ;

		// on compte le degré de la face grossière
		unsigned int degree = 0 ;
		Dart fit = old ;
		do
		{
			++degree ;
			fit = map.phi1(fit) ;
		} while (fit != old) ;

		PFP::AGENTS agents ;
		PFP::OBSTACLEVECT obst ;
		PFP::OBSTACLEVECT neighborObst ;
		//premier tour pour les présents
		fit = old ;
		do
963
		{
964
965
			PFP::AGENTS a(agentvect[fit]) ;
			PFP::OBSTACLEVECT ob(obstvect[fit]) ;
966

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

969
970
			map.setCurrentLevel(map.getMaxLevel()) ;
			for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
971
			{
972
973
974
				this->popObstacleInCells(*ait, (*ait)->index) ;
				obst.push_back(*ait);
			}
975
976


977

978
979
980
981
982
983
984
985
986
987
			for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
				popAgentInCells(*ait, fit) ;
			map.setCurrentLevel(fLevel - 1) ;
			Dart nf = map.phi2(fit) ;
			if (!map.faceIsSubdivided(nf))
			{
				map.setCurrentLevel(fLevel) ;
				PFP::AGENTS& an = agentvect[nf] ;
				PFP::OBSTACLEVECT resetob = obstvect[nf] ;
				for (PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
988
				{
989
					if ((*ait)->part_.d == map.phi1(nf)) (*ait)->part_.d = nf ;
990
				}
991
				for (PFP::OBSTACLEVECT::iterator ait = resetob.begin(); ait != resetob.end(); ++ait)
992
				{
993
994
					MovingObstacle* mo = (*ait)->mo ;
					if (mo != NULL)
995
					{
996
						resetPart(mo,nf) ;
997
					}
998
999
				}
				map.setCurrentLevel(fLevel - 1) ;
1000
			}
1001
1002
			fit = map.phi1(fit) ;
		} while (fit != old) ;
1003

1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
		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)
1014
			{
1015
1016
1017
1018
1019
1020
1021
				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) ;
		}
1022