env_map.cpp 25.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() :
David Cazier's avatar
David Cazier committed
18
	obstacleDistance(Agent::range_),
pitiot's avatar
maj    
pitiot committed
19
20
21
22
23
	obstacleMarkS(mapScenary),
	buildingMarkS(mapScenary),
	obstacleMark(map),
	buildingMark(map),
	pedWayMark(map),
24

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

#ifndef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
38
39
40
41
42
	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
43

44
45
46
47
48
49
50
51
	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
52
	VEC3 bottomLeft(-width / 2, -height / 2, 0.0f) ;
53
54
55
56
57
58
59
60
61
62
	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
David Cazier's avatar
David Cazier committed
63
	std::cout << " - Table de hachage : " << agentGridSize(0) << " x " << agentGridSize(1) << std::endl ;
64
65
#endif

David Cazier's avatar
David Cazier committed
66
67
	switch (config)
	{
pitiot's avatar
maj    
pitiot committed
68
69
70
71
72
73
74
75
76
77
78
		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 : {
79
80
81
82
83
84
85
//			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) ;
pitiot's avatar
maj    
pitiot committed
86

87
88
89
90
//			Algo::BooleanOperator::mergeVertices<PFP>(map, position) ;
//			map.closeMap() ;
//			Algo::Modelisation::CatmullClarkSubdivision<PFP>(map, position) ;
//			Algo::Modelisation::computeDual<PFP>(map) ;
pitiot's avatar
maj    
pitiot committed
91
92
93
94
95
96
		}
			CityGenerator::generateCity<PFP>(*this) ;
			break ;
		case 4 :
			CityGenerator::generatePlanet<PFP>(*this) ;
			break ;
97
98
99
100
101
102
103
104
105
	}

//	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();
pitiot's avatar
maj    
pitiot committed
106
107
	// TODO Check registerWallInFaces();
	registerWallInFaces() ;
108
109
//	subdivideAllToMaxLevel();

pitiot's avatar
maj    
pitiot committed
110
	for (unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
111
		subdivisableFace[i].first = false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
112
#endif
113

Pierre Kraemer's avatar
Pierre Kraemer committed
114
115
}

116
117
void EnvMap::markPedWay()
{
pitiot's avatar
maj    
pitiot committed
118
	CellMarker<FACE> treat(map) ;
David Cazier's avatar
David Cazier committed
119
120
121
122
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
123
124
125
			treat.mark(d) ;

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
126
127
			do
			{
128
				Dart ddd = dd ;
David Cazier's avatar
David Cazier committed
129
130
131
132
				do
				{
					if (obstacleMark.isMarked(dd))
					{
133
134
						pedWayMark.mark(d) ;
						break ;
135
					}
136
137
					ddd = map.alpha1(ddd) ;
				} while (ddd != dd) ;
138

139
				dd = map.phi1(dd) ;
140

141
			} while (dd != d) ;
142
143
144
		}
	}

145
	treat.unmarkAll() ;
David Cazier's avatar
David Cazier committed
146
147
148
149
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
150
151
152
			treat.mark(d) ;

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
153
154
155
156
157
158
			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))))))
				{
159
160
161
					pedWayMark.mark(d) ;
					break ;
				}
162

163
				dd = map.phi1(dd) ;
164

165
			} while (dd != d) ;
166
167
168
169
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
170
171
unsigned int EnvMap::mapMemoryCost()
{
pitiot's avatar
maj    
pitiot committed
172
173
174
175
	return (map.getAttributeContainer<DART>()).memorySize()
	    + (map.getAttributeContainer<VERTEX>()).memorySize()
	    + (map.getAttributeContainer<EDGE>()).memorySize()
	    + (map.getAttributeContainer<FACE>()).memorySize() ;
Thomas's avatar
Thomas committed
176
177
}

Pierre Kraemer's avatar
Pierre Kraemer committed
178
#ifndef SPATIAL_HASHING
Thomas's avatar
Thomas committed
179
180
void EnvMap::subdivideAllToMaxLevel()
{
181
	bool subdiv ;
David Cazier's avatar
David Cazier committed
182
183
	do
	{
184
		subdiv = false ;
Thomas's avatar
Thomas committed
185
		{
pitiot's avatar
maj    
pitiot committed
186
			CellMarker<FACE> subd(map) ;
David Cazier's avatar
David Cazier committed
187
188
189
190
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
191
					subd.mark(d) ;
David Cazier's avatar
David Cazier committed
192
193
					if (!buildingMark.isMarked(d))
					{
Thomas's avatar
Thomas committed
194
195
						//check if subdivision is authorized
						float minDistSq = Agent::neighborDistSq_ ;
196
						bool subdivisable = true ;
Thomas's avatar
Thomas committed
197
198
199
						Dart old = map.faceOldestDart(d) ;
						unsigned int fLevel = map.faceLevel(old) ;
						map.setCurrentLevel(fLevel) ;
David Cazier's avatar
David Cazier committed
200
						PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
Thomas's avatar
Thomas committed
201
						Dart fd = old ;
David Cazier's avatar
David Cazier committed
202
203
						do
						{
Thomas's avatar
Thomas committed
204
							PFP::VEC3& p = position[fd] ;
205
							PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd,
David Cazier's avatar
David Cazier committed
206
							                                                      position) ;
207
							PFP::VEC3 proj = fCenter
David Cazier's avatar
David Cazier committed
208
209
210
							    - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
							if (proj.norm2() < minDistSq)
							{
Thomas's avatar
Thomas committed
211
212
213
214
								subdivisable = false ;
								break ;
							}
							fd = map.phi1(fd) ;
215
						} while (fd != old) ;
Thomas's avatar
Thomas committed
216

David Cazier's avatar
David Cazier committed
217
218
						if (subdivisable)
						{
Thomas's avatar
Thomas committed
219
220
							map.setCurrentLevel(fLevel) ;
							Algo::IHM::subdivideFace<PFP>(map, old, position) ;
221
							subdiv = true ;
Thomas's avatar
Thomas committed
222
223
224
225
226
227
						}
						map.setCurrentLevel(map.getMaxLevel()) ;
					}
				}
			}
		}
228
	} while (subdiv) ;
Thomas's avatar
Thomas committed
229
230
231
232
}

void EnvMap::subdivideToProperLevel()
{
233
	bool subdiv ;
David Cazier's avatar
David Cazier committed
234
235
	do
	{
236
		subdiv = false ;
Thomas's avatar
Thomas committed
237
		{
pitiot's avatar
maj    
pitiot committed
238
			CellMarker<FACE> subd(map) ;
David Cazier's avatar
David Cazier committed
239
240
241
242
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
243
					subd.mark(d) ;
David Cazier's avatar
David Cazier committed
244
245
					if (!refineMark.isMarked(d) && agentvect[d].size() > nbAgentsToSubdivide)
					{
246
						std::pair<bool, bool>& sf = subdivisableFace[d] ;
David Cazier's avatar
David Cazier committed
247
248
						if (sf.first == false || (sf.first == true && sf.second))
						{
249
250
251
							subdiv = true ;
							refineMark.mark(d) ;
							refineCandidate.push_back(d) ;
Thomas's avatar
Thomas committed
252
253
254
255
						}
					}
				}
			}
256
			subd.unmarkAll() ;
Thomas's avatar
Thomas committed
257
		}
258
259
260
261
		updateMap() ;
		refineCandidate.clear() ;
		map.setCurrentLevel(map.getMaxLevel()) ;
	} while (subdiv) ;
Thomas's avatar
Thomas committed
262
263
}

Pierre Kraemer's avatar
Pierre Kraemer committed
264
265
Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
pitiot's avatar
maj    
pitiot committed
266
	CellMarkerStore<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
267
268
269
270
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
271
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
272
273
			if (!map.isBoundaryFace(d) && !buildingMark.isMarked(d)
			    && Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true)) return d ;
274
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
275
276
	}

pitiot's avatar
pitiot committed
277
//	std::cout << "ERROR : pos not in map for getBelongingCell" << std::endl ;
278
	return map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
279
}
Pierre Kraemer's avatar
Pierre Kraemer committed
280
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
281

Pierre Kraemer's avatar
Pierre Kraemer committed
282
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
283
284
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
285
	Dart dd = d ;
David Cazier's avatar
David Cazier committed
286
287
	do
	{
288
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
289
290
		while (ddd != dd)
		{
291
292
			f(ddd) ;
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
293
		}
294
295
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
296
297
}

pitiot's avatar
maj    
pitiot committed
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
// 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
344
345
void EnvMap::registerObstaclesInFaces()
{
pitiot's avatar
maj    
pitiot committed
346
	CellMarker<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
347
348
349
350
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
351
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
352
353
			if (!buildingMark.isMarked(d))
			{
354
				Dart dd = d ;
355
356
357
358

				//retrieve all obstacles within its one-ring

				//first : test all edges of the face of d
David Cazier's avatar
David Cazier committed
359
360
361
362
				do
				{
					if (obstacleMark.isMarked(dd))
					{
363
364
365
366
						Dart dd2 = map.phi2(dd) ;
						Dart next = map.phi1(dd2) ;
						Dart previous = map.phi_1(dd2) ;
						Obstacle* o = new Obstacle(position[dd2], position[next],
pitiot's avatar
maj    
pitiot committed
367
368
						                           position[previous], position[map.phi1(next)],
						                           NULL, 0) ;
369
						obstvect[d].push_back(o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
370
					}
371
372
					dd = map.phi1(dd) ;
				} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
373

374
				//second : test all edges of neighboring faces
David Cazier's avatar
David Cazier committed
375
376
				do
				{
377
					Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
378
379
					while (ddd != dd)
					{
pitiot's avatar
maj    
pitiot committed
380
						// TODO Check effet de bord
David Cazier's avatar
David Cazier committed
381
382
						if (!buildingMark.isMarked(ddd)) addNeighborObstacles(
						    obstvect[d], ddd, ddd == map.alpha_1(dd)) ;
383
						ddd = map.alpha1(ddd) ;
384
					}
385
386
					dd = map.phi1(dd) ;
				} while (dd != d) ;
387
388
389
			}
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
390
391
392
393
}

void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor)
{
394
	Dart stop ;
David Cazier's avatar
David Cazier committed
395
396
397
398
	if (edgeNeighbor)
		stop = map.phi_1(map.phi_1(map.phi_1(d))) ;
	else
		stop = map.phi_1(map.phi_1(d)) ;
399
400

	Dart dd = d ;
David Cazier's avatar
David Cazier committed
401
402
403
404
	do
	{
		if (obstacleMark.isMarked(dd))
		{
405
406
407
408
409
410
411
412
413
414
//			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
//			{
415
416
417
418
			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],
pitiot's avatar
maj    
pitiot committed
419
			                           position[map.phi1(next)], NULL, 0) ;
420
			obst.push_back(o) ;
421
//			}
Pierre Kraemer's avatar
Pierre Kraemer committed
422
		}
423
424
		dd = map.phi1(dd) ;
	} while (dd != stop) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
425
426
427
428
}

void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
429
	Dart newFace = agent->part_.d ;
430

431
432
	popAgentInCells(agent, oldFace) ;
	pushAgentInCells(agent, newFace) ;
433

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

David Cazier's avatar
David Cazier committed
444
445
446
	if (!coarsenMark.isMarked(oldFace) && !refineMark.isMarked(oldFace)
	    && 4 * agentvect[oldFace].size() < nbAgentsToSimplify)
	{
447
448
		coarsenMark.mark(oldFace) ;
		coarsenCandidate.push_back(map.faceOldestDart(oldFace)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
449
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
450
451
}

Pierre Kraemer's avatar
Pierre Kraemer committed
452
453
void EnvMap::obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace)
{
454
455
	popObstacleInCells(agent, oldFace) ;
	pushObstacleInCells(agent, newFace) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
456
}
457
void EnvMap::refine()
Pierre Kraemer's avatar
Pierre Kraemer committed
458
{
David Cazier's avatar
David Cazier committed
459
	for (std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end();
460
		    ++it)
David Cazier's avatar
David Cazier committed
461
		{
462
463
			Dart d = (*it) ;
			refineMark.unmark(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
464

465
466
			//if the number of agents is big enough
			if (agentvect[d].size() > nbAgentsToSubdivide)
David Cazier's avatar
David Cazier committed
467
			{
468
469
				int fLevel = -1 ;
				Dart old = map.faceOldestDart(d) ;
470

471
472
473
474
475
476
477
				bool subdivisable = true ;

				//check if faces resulting from a subdivision are big enough
				std::pair<bool, bool>& sf = subdivisableFace[old] ;
				if (sf.first == true)
					subdivisable = sf.second ;
				else
David Cazier's avatar
David Cazier committed
478
				{
479
480
481
482
483
484
485
					float minSizeSq = minCellSize * minCellSize ; // diametre de vision de l'agent au carré

					fLevel = map.faceLevel(old) ;
					map.setCurrentLevel(fLevel) ;
					PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
					Dart fd = old ;
					do
David Cazier's avatar
David Cazier committed
486
					{
487
488
489
490
491
492
493
494
495
496
497
						PFP::VEC3& p = position[fd] ;
						PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
						PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
						if (proj.norm2() < minSizeSq)
						{
							subdivisable = false ;
							break ;
						}
						fd = map.phi1(fd) ;
					} while (fd != old) ;
					map.setCurrentLevel(map.getMaxLevel()) ;
498

499
500
501
					sf.first = true ;
					sf.second = subdivisable ;
				}
502

503
504
505
				if (subdivisable)
				{
					if (fLevel == -1) fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
506

507
					sf.first = false ;
508

509
510
511
512
513
514
515
516
517
518
					PFP::AGENTS oldAgents(agentvect[old]) ;
					PFP::OBSTACLEVECT oldObst(obstvect[old]) ;
					PFP::OBSTACLEVECT oldNeighborObst(neighborObstvect[old]) ;
					for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
						popAgentInCells(*ait, old) ;
					for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
						register_pop(*ait, (*ait)->index) ;
					for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();ait != oldNeighborObst.end(); ++ait)
						register_pop(*ait, (*ait)->index) ;
					neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
519

520
521
522
523
524
525
526
527
528
529
530
					map.setCurrentLevel(fLevel) ;
					Algo::IHM::subdivideFace<PFP>(map, old, position) ;
					CellMarkerStore<FACE> newF(map) ;
					unsigned int degree = 0 ;
					Dart dd = old ;
					do
					{
						++degree ;
						newF.mark(dd) ;
						dd = map.phi1(dd) ;
					} while (dd != old) ;
pitiot's avatar
maj    
pitiot committed
531

532
533
534
535
536
537
					if (degree == 3)
					{
						Dart centerFace = map.phi2(map.phi1(old)) ;
						newF.mark(centerFace) ;
					}
					map.setCurrentLevel(map.getMaxLevel()) ;
538

539
540
541
542
543
544
					//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) ;
					}
Pierre Kraemer's avatar
Pierre Kraemer committed
545

546
547
548
549
550
551
					//same for obstacles contained
					for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
					{
						resetPartSubdiv(*ait);
						update_registration(*ait) ;
					}
pitiot's avatar
maj    
pitiot committed
552

553
554
555
556
					//same for adjacent obstacles
					for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();
					    ait != oldNeighborObst.end(); ++ait)
					{
557

558
559
						update_registration(*ait) ;
					}
pitiot's avatar
maj    
pitiot committed
560

561
562
					//retrieve neighbors agents from onering cells
					dd = old ;
David Cazier's avatar
David Cazier committed
563
564
					do
					{
565
566
						Dart d3 = dd ;
						do
David Cazier's avatar
David Cazier committed
567
						{
568
569
570
							Dart d4 = map.alpha1(map.alpha1(d3)) ;
							PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
							while (d4 != d3)
David Cazier's avatar
David Cazier committed
571
							{
572
573
574
575
576
577
								if (!newF.isMarked(d4))
								{
									PFP::AGENTS& ad4 = agentvect[d4] ;
									nad3.insert(nad3.end(), ad4.begin(), ad4.end()) ;
								}
								d4 = map.alpha1(d4) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
578
							}
579
580
							d3 = map.phi1(d3) ;
						} while (d3 != dd) ;
581

582
583
584
585
						map.setCurrentLevel(fLevel) ;
						dd = map.phi1(dd) ;
						map.setCurrentLevel(map.getMaxLevel()) ;
					} while (dd != old) ;
586

587
					if (degree == 3)
David Cazier's avatar
David Cazier committed
588
					{
589
590
591
						Dart centerFace = map.phi2(map.phi1(old)) ;
						Dart d3 = centerFace ;
						do
David Cazier's avatar
David Cazier committed
592
						{
593
594
595
							Dart d4 = map.alpha1(map.alpha1(d3)) ;
							PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
							while (d4 != d3)
David Cazier's avatar
David Cazier committed
596
							{
597
598
599
600
601
602
								if (!newF.isMarked(d4))
								{
									PFP::AGENTS& ad4 = agentvect[d4] ;
									nad3.insert(neighborAgentvect[d3].end(), ad4.begin(), ad4.end()) ;
								}
								d4 = map.alpha1(d4) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
603
							}
604
605
606
							d3 = map.phi1(d3) ;
						} while (d3 != centerFace) ;
					}
607
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
608
609
			}
		}
610
		refineCandidate.clear() ;
David Cazier's avatar
David Cazier committed
611

612
613
614
}
void EnvMap::coarse()
{
615
	// On recrée une liste des faces à simplifier en empêchant les doublons
616
617
618
		// On en profite pour vérifier les conditions de simplifications
		std::vector<Dart> checkCoarsenCandidate ;
		checkCoarsenCandidate.reserve(coarsenCandidate.size()) ;
619

620
		for (unsigned int it = 0; it < coarsenCandidate.size(); ++it)
621
		{
622
623
624
			Dart old = coarsenCandidate[it] ;
			bool oldIsMarked = coarsenMark.isMarked(old) ;
			coarsenMark.unmark(old) ;
625

626
			unsigned int fLevel = map.faceLevel(old) ;
627

628
629
630
631
			if (oldIsMarked && fLevel > 0 && map.getDartLevel(old) < fLevel)
			{
				unsigned int cur = map.getCurrentLevel() ;
				map.setCurrentLevel(fLevel - 1) ;
632

633
				if (map.faceIsSubdividedOnce(old))
634
				{
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
					// 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())
	//					{
	//						if(map.getEmbedding<FACE>(coarsenCandidate[start]) == fEmb)
	//						{
	//							coarsenCandidate[start] = coarsenCandidate.back() ;
	//							coarsenCandidate.pop_back() ;
	//						}
	//						else
	//							++start ;
	//					}

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


					//Loop subdivision
					if (degree == 3)
					{
						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) ;
					}
					if (nbAgents < nbAgentsToSimplify) checkCoarsenCandidate.push_back(old) ;
686
				}
687
				map.setCurrentLevel(cur) ;
688
689
			}
		}
690
		coarsenCandidate.clear() ;
691

692
693
//		 On réalise la simplification (les conditions ont déjà été vérifiées)
		for (unsigned int it = 0; it < checkCoarsenCandidate.size(); ++it)
694
		{
695
			Dart old = checkCoarsenCandidate[it] ;
696

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

701
702
703
704
			// on compte le degré de la face grossière
			unsigned int degree = 0 ;
			Dart fit = old ;
			do
705
			{
706
707
708
709
710
711
712
713
714
715
716
717
718
				++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
			{
				PFP::AGENTS a(agentvect[fit]) ;
				PFP::OBSTACLEVECT ob(obstvect[fit]) ;
719
720


721
				agents.insert(agents.end(), a.begin(), a.end()) ;
722

723
724
725

				map.setCurrentLevel(map.getMaxLevel()) ;
				for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
726
				{
727
728
					register_pop(*ait, (*ait)->index) ;
					obst.push_back(*ait);
729
				}
730
731
732
733
734
735
736
737



				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))
738
				{
739
740
741
742
					map.setCurrentLevel(fLevel) ;
					PFP::AGENTS& an = agentvect[nf] ;
					PFP::OBSTACLEVECT resetob = obstvect[nf] ;
					for (PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
743
					{
744
745
746
747
748
749
750
751
752
						if ((*ait)->part_.d == map.phi1(nf)) (*ait)->part_.d = nf ;
					}
					for (PFP::OBSTACLEVECT::iterator ait = resetob.begin(); ait != resetob.end(); ++ait)
					{
						MovingObstacle* mo = (*ait)->mo ;
						if (mo != NULL)
						{
							resetPart(mo,nf) ;
						}
753
					}
754
					map.setCurrentLevel(fLevel - 1) ;
755
				}
756
757
				fit = map.phi1(fit) ;
			} while (fit != old) ;
758

759
			if (degree == 3)
760
			{
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
				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)
				{
					register_pop(*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) ;
776
777
			}

778
779
780
781
			//deuxieme tour concernant les voisins
			fit = old ;
			do
			{
782

783
				PFP::OBSTACLEVECT nob(neighborObstvect[fit]) ;
784

785
786
787
788
789
790
791
				map.setCurrentLevel(map.getMaxLevel()) ;
				for(PFP::OBSTACLEVECT::iterator ait = nob.begin(); ait != nob.end(); ++ait)
				{
					register_pop(*ait, (*ait)->index) ;
					neighborObst.push_back(*ait);
				}
				map.setCurrentLevel(fLevel - 1) ;
792

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

796
			if (degree == 3)
797
			{
798
799
800
801
802
803
804
805
806
807
				map.setCurrentLevel(fLevel) ;
				Dart centerFace = map.phi2(map.phi1(old)) ;
				PFP::OBSTACLEVECT nob(neighborObstvect[centerFace]) ;
				map.setCurrentLevel(map.getMaxLevel()) ;
				for(PFP::OBSTACLEVECT::iterator ait = nob.begin(); ait != nob.end(); ++ait)
				{
					register_pop(*ait, (*ait)->index) ;
					neighborObst.push_back(*ait);
				}
				map.setCurrentLevel(fLevel - 1) ;
808
809
			}

810
			neighborAgentvect[old].clear() ;
811

812
			// TODO Check with optimisation
813

814
815
816
817
818
819
820
821
822
823
	//
	//		CGoGNout<<"Avant pop"<<CGoGNendl;
	//		CGoGNout<<"Obst dans la case : ";
	//		for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
	//			CGoGNout<<(*ait)->index<<" ; ";
	//		CGoGNout<<CGoGNendl;
	//		CGoGNout<<"Obst voisins : ";
	//		for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
	//			CGoGNout<<(*ait)->index<<" ; ";
	//		CGoGNout<<CGoGNendl;
824
825


826
			map.setCurrentLevel(fLevel - 1) ;
827

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

830
831
832
			std::pair<bool, bool>& sf = subdivisableFace[old] ;
			sf.first = true ;
			sf.second = true ;
833

834
			map.setCurrentLevel(map.getMaxLevel()) ;
835

836
837
838
839
840
841
842
843
844
845
846
847
848
849
			for (PFP::AGENTS::iterator itA = agents.begin(); itA != agents.end(); ++itA)
			{
				(*itA)->part_.d = old ;
				pushAgentInCells(*itA, old) ;
			}
			for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
			{
				resetObstPartInFace(*ait, old) ;
				update_registration(*ait) ;
			}
			for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
			{
				update_registration(*ait) ;
			}
850

851
852
853
854
855
856
857
858
859
860
861
862
863
			Dart dd = old ;
			do
			{
				Dart ddd = map.alpha1(map.alpha1(dd)) ;
				while (ddd != dd)
				{
					neighborAgentvect[old].insert(neighborAgentvect[old].end(), agentvect[ddd].begin(), agentvect[ddd].end()) ;
					ddd = map.alpha1(ddd) ;
				}
				dd = map.phi1(dd) ;
			} while (dd != old) ;

			if (fLevel > 1 && !coarsenMark.isMarked(old) && agentvect[old].size() < nbAgentsToSimplify)
864
			{
865
866
				coarsenMark.mark(old) ;
				coarsenCandidate.push_back(map.faceOldestDart(old)) ;
867
868
			}

869
			map.setCurrentLevel(cur) ;
870
		}
871
872
873
874
875
876
877
878
879
880
		map.setCurrentLevel(map.getMaxLevel()) ;
		if (coarsenCandidate.size() > 0) updateMap() ;
}
void EnvMap::updateMap()
{
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

	refine();
	coarse();

881

Pierre Kraemer's avatar
Pierre Kraemer committed
882
883
884
885
}

void EnvMap::resetAgentInFace(Agent* agent)
{
pitiot's avatar
maj    
pitiot committed
886
887
888
	VEC3 pos = agent->part_.getPosition() ;
	agent->part_.move(Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position)) ;
	agent->part_.setState(FACE) ;
889
	agent->part_.move(pos) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
890
}
Pierre Kraemer's avatar
Pierre Kraemer committed
891
#endif
pitiot's avatar
maj    
pitiot committed
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978

#ifdef SPATIAL_HASHING
Geom::Vec2ui EnvMap::agentPositionCell(Agent* a)
{
	VEC3 relativePos = a->pos - geometry.min() ;
	relativePos /= minCellSize ;
	return Geom::Vec2ui(relativePos[0], relativePos[1]) ;
}

const std::vector<Agent*>& EnvMap::getNeighbors(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	return ht_agents[c] ;
}

const std::vector<Agent*>& EnvMap::getNeighborAgents(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	return ht_neighbor_agents[c] ;
}

void EnvMap::getOneRingNeighbors(Agent* a, std::vector<Agent*>& neighbors)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	for(int ii = -1 ; ii <= 1 ; ++ii)
	{
		for(int jj = -1 ; jj <= 1 ; ++jj)
		{
			if (ii != 0 || jj != 0)
			{
				Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
				const std::vector<Agent*>& v = ht_agents[cc] ;
				neighbors.insert(neighbors.end(), v.begin(), v.end()) ;
			}
		}
	}
}

void EnvMap::addAgentInGrid(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	addAgentInGrid(a,c) ;
}

void EnvMap::addAgentInGrid(Agent* a, Geom::Vec2ui c)
{
	ht_agents[c].push_back(a) ;

	for(int ii = -1; ii <= 1; ++ii)
	{
		for(int jj = -1; jj <= 1; ++jj)
		{
			if(!(ii == 0 && jj == 0))
			{
				Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
				ht_neighbor_agents[cc].push_back(a) ;
			}
		}
	}
}

void EnvMap::removeAgentFromGrid(Agent* a)
{
	Geom::Vec2ui c = agentPositionCell(a) ;
	removeAgentFromGrid(a,c) ;
}

void EnvMap::removeAgentFromGrid(Agent* a, Geom::Vec2ui c)
{
	removeElementFromVector<Agent*>(ht_agents[c], a) ;
	if (ht_agents[c].empty()) ht_agents.erase(c) ;

	for(int ii = -1; ii <= 1; ++ii)
	{
		for(int jj = -1; jj <= 1; ++jj)
		{
			if(!(ii == 0 && jj == 0))
			{
				Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
				removeElementFromVector<Agent*>(ht_neighbor_agents[cc], a) ;
				if (ht_neighbor_agents[c].empty()) ht_neighbor_agents.erase(c) ;
			}
		}
	}
}

#endif