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
...    
pitiot committed
266
//	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
267
	CellMarkerStore<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
268
269
270
271
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
272
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
273
274
			if (!map.isBoundaryFace(d) && !buildingMark.isMarked(d)
			    && Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true)) return d ;
275
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
276
277
	}

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

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

pitiot's avatar
maj    
pitiot committed
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
344
// 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
345
346
void EnvMap::registerObstaclesInFaces()
{
pitiot's avatar
maj    
pitiot committed
347
	CellMarker<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
348
349
350
351
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
352
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
353
354
			if (!buildingMark.isMarked(d))
			{
355
				Dart dd = d ;
356
357
358
359

				//retrieve all obstacles within its one-ring

				//first : test all edges of the face of d
David Cazier's avatar
David Cazier committed
360
361
362
363
				do
				{
					if (obstacleMark.isMarked(dd))
					{
364
365
366
367
						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
368
369
						                           position[previous], position[map.phi1(next)],
						                           NULL, 0) ;
370
						obstvect[d].push_back(o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
371
					}
372
373
					dd = map.phi1(dd) ;
				} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
374

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

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

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

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

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

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

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

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

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

472
473
474
475
476
477
478
				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
479
				{
480
481
482
483
484
485
486
					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
487
					{
488
489
490
491
492
493
494
495
496
497
498
						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()) ;
499

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

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

508
					sf.first = false ;
509

510
511
512
513
514
515
516
517
518
519
					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
520

521
522
523
524
525
526
527
528
529
530
531
					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
532

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

540
541
542
543
544
545
					//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
546

547
548
549
550
551
552
					//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
553

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

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

562
563
					//retrieve neighbors agents from onering cells
					dd = old ;
David Cazier's avatar
David Cazier committed
564
565
					do
					{
566
567
						Dart d3 = dd ;
						do
David Cazier's avatar
David Cazier committed
568
						{
569
570
571
							Dart d4 = map.alpha1(map.alpha1(d3)) ;
							PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
							while (d4 != d3)
David Cazier's avatar
David Cazier committed
572
							{
573
574
575
576
577
578
								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
579
							}
580
581
							d3 = map.phi1(d3) ;
						} while (d3 != dd) ;
582

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

588
					if (degree == 3)
David Cazier's avatar
David Cazier committed
589
					{
590
591
592
						Dart centerFace = map.phi2(map.phi1(old)) ;
						Dart d3 = centerFace ;
						do
David Cazier's avatar
David Cazier committed
593
						{
594
595
596
							Dart d4 = map.alpha1(map.alpha1(d3)) ;
							PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
							while (d4 != d3)
David Cazier's avatar
David Cazier committed
597
							{
598
599
600
601
602
603
								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
604
							}
605
606
607
							d3 = map.phi1(d3) ;
						} while (d3 != centerFace) ;
					}
608
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
609
610
			}
		}
611
		refineCandidate.clear() ;
David Cazier's avatar
David Cazier committed
612

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

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

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

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

634
				if (map.faceIsSubdividedOnce(old))
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
686
					// 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) ;
687
				}
688
				map.setCurrentLevel(cur) ;
689
690
			}
		}
691
		coarsenCandidate.clear() ;
692

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

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

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


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

724
725
726

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



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

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

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

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

786
787
788
789
790
791
792
				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) ;
793

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

797
			if (degree == 3)
798
			{
799
800
801
802
803
804
805
806
807
808
				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) ;
809
810
			}

811
			neighborAgentvect[old].clear() ;
812

813
			// TODO Check with optimisation
814

815
816
817
818
819
820
821
822
823
824
	//
	//		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;
825
826


827
			map.setCurrentLevel(fLevel - 1) ;
828

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

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

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

837
838
839
840
841
842
843
844
845
846
847
848
849
850
			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) ;
			}
851

852
853
854
855
856
857
858
859
860
861
862
863
864
			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)
865
			{
866
867
				coarsenMark.mark(old) ;
				coarsenCandidate.push_back(map.faceOldestDart(old)) ;
868
869
			}

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

	refine();
	coarse();

882

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

void EnvMap::resetAgentInFace(Agent* agent)
{
pitiot's avatar
maj    
pitiot committed
887
888
889
	VEC3 pos = agent->part_.getPosition() ;
	agent->part_.move(Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position)) ;
	agent->part_.setState(FACE) ;
890
	agent->part_.move(pos) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
891
}
Pierre Kraemer's avatar
Pierre Kraemer committed
892
#endif
pitiot's avatar
maj    
pitiot committed
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
979

#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