env_map.cpp 24.5 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_),
David Cazier's avatar
David Cazier committed
19
20
21
22
23
	obstacleMarkS(mapScenary),
	buildingMarkS(mapScenary),
	obstacleMark(map),
	buildingMark(map),
	pedWayMark(map),
Pierre Kraemer's avatar
Pierre Kraemer committed
24
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
25
26
	refineMark(map),
	coarsenMark(map)
Pierre Kraemer's avatar
Pierre Kraemer committed
27
#else
David Cazier's avatar
David Cazier committed
28
29
30
	ht_agents(1024),
	ht_neighbor_agents(1024),
	ht_obstacles(1024)
Pierre Kraemer's avatar
Pierre Kraemer committed
31
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
32
{
David Cazier's avatar
David Cazier committed
33
34
	position = map.addAttribute<VEC3, VERTEX>("position") ;
	normal = map.addAttribute<VEC3, VERTEX>("normal") ;
Pierre Kraemer's avatar
Pierre Kraemer committed
35
36

#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
37
38
39
40
41
	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
42

43
44
45
46
47
48
49
	refineCandidate.reserve(100) ;
	coarsenCandidate.reserve(100) ;
#endif
}

void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, REAL maxSize)
{
David Cazier's avatar
David Cazier committed
50
51
52
53
	nbSquares = 6;
	sideSize = 400.0f;
	max_for_obstacles=(sideSize*nbSquares/2)-sideSize/2;

54
	std::cout << "Init EnvMap" << std::endl ;
David Cazier's avatar
David Cazier committed
55
	VEC3 bottomLeft(-width / 2, -height / 2, 0.0f) ;
56
57
58
59
60
61
62
63
64
65
	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
66
	std::cout << " - Table de hachage : " << agentGridSize(0) << " x " << agentGridSize(1) << std::endl ;
67
68
#endif

David Cazier's avatar
David Cazier committed
69
70
	switch (config)
	{
David Cazier's avatar
David Cazier committed
71
72
73
74
75
76
77
78
79
80
81
		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 : {
82
83
84
85
86
87
88
89
90
91
92
93
94
//			std::string filename = "/home/jund/Desktop/drawingQuads.svg" ;
//			std::string filename = "/home/jund/Desktop/drawingTest.svg" ;
//			std::string filename = "/home/jund/Desktop/drawingSimple.svg" ;
//			std::string filename = "/home/jund/Desktop/drawingLines.svg" ;
//			std::string filename = "/home/jund/Desktop/mapKrutSimple.svg" ;
//			std::string filename = "/home/jund/Desktop/mapKrut.svg" ;
//			Algo::Import::importSVG<PFP>(map, filename, position, obstacleMark, buildingMark) ;
//			scale(3.2808399f) ;
//
//			Algo::BooleanOperator::mergeVertices<PFP>(map, position) ;
//			map.closeMap() ;
//			Algo::Modelisation::CatmullClarkSubdivision<PFP>(map, position) ;
//			Algo::Modelisation::computeDual<PFP>(map) ;
David Cazier's avatar
David Cazier committed
95
96
97
98
99
100
101
		}
			CityGenerator::generateCity<PFP>(*this) ;
			break ;
		case 4 :
			CityGenerator::generatePlanet<PFP>(map, position, obstacleMark, buildingMark, 200.0f,
			                                   nbSquares) ;
			break ;
102
103
104
105
106
107
108
109
110
	}

//	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
111
112
	// TODO Check registerWallInFaces();
	registerWallInFaces() ;
113
114
//	subdivideAllToMaxLevel();

David Cazier's avatar
David Cazier committed
115
116
	for (unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end();
	    subdivisableFace.next(i))
117
		subdivisableFace[i].first = false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
118
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
119
120
}

121
122
void EnvMap::markPedWay()
{
David Cazier's avatar
David Cazier committed
123
	CellMarker<FACE> treat(map) ;
David Cazier's avatar
David Cazier committed
124
125
126
127
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
128
129
130
			treat.mark(d) ;

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
131
132
			do
			{
133
				Dart ddd = dd ;
David Cazier's avatar
David Cazier committed
134
135
136
137
				do
				{
					if (obstacleMark.isMarked(dd))
					{
138
139
						pedWayMark.mark(d) ;
						break ;
140
					}
141
142
					ddd = map.alpha1(ddd) ;
				} while (ddd != dd) ;
143

144
				dd = map.phi1(dd) ;
145

146
			} while (dd != d) ;
147
148
149
		}
	}

150
	treat.unmarkAll() ;
David Cazier's avatar
David Cazier committed
151
152
153
154
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!treat.isMarked(d))
		{
155
156
157
			treat.mark(d) ;

			Dart dd = d ;
David Cazier's avatar
David Cazier committed
158
159
160
161
162
163
			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))))))
				{
164
165
166
					pedWayMark.mark(d) ;
					break ;
				}
167

168
				dd = map.phi1(dd) ;
169

170
			} while (dd != d) ;
171
172
173
174
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
175
176
unsigned int EnvMap::mapMemoryCost()
{
David Cazier's avatar
David Cazier committed
177
178
179
180
	return (map.getAttributeContainer<DART>()).memorySize()
	    + (map.getAttributeContainer<VERTEX>()).memorySize()
	    + (map.getAttributeContainer<EDGE>()).memorySize()
	    + (map.getAttributeContainer<FACE>()).memorySize() ;
Thomas's avatar
Thomas committed
181
182
}

David Cazier's avatar
David Cazier committed
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
// TODO Check void EnvMap::scale(float scaleVal) => compatible avec champ Geometry ?
void EnvMap::scale(float scaleVal)
{
	Geom::BoundingBox<PFP::VEC3> bb(position.begin()) ;

	for (unsigned int v = position.begin(); v != position.end(); position.next(v))
	{
		bb.addPoint(position[v]) ;
	}

	for (unsigned int v = positionScenary.begin(); v != positionScenary.end();
	    positionScenary.next(v))
	{
		bb.addPoint(positionScenary[v]) ;
	}

	VEC3 center = bb.center() ;
	for (unsigned int v = position.begin(); v != position.end(); position.next(v))
	{
		position[v] -= center ;
		position[v] *= scaleVal ;
	}

	for (unsigned int v = positionScenary.begin(); v != positionScenary.end();
	    positionScenary.next(v))
	{
		positionScenary[v] -= center ;
		positionScenary[v] *= scaleVal ;
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
214
#ifndef SPATIAL_HASHING
Thomas's avatar
Thomas committed
215
216
void EnvMap::subdivideAllToMaxLevel()
{
217
	bool subdiv ;
David Cazier's avatar
David Cazier committed
218
219
	do
	{
220
		subdiv = false ;
Thomas's avatar
Thomas committed
221
		{
David Cazier's avatar
David Cazier committed
222
			CellMarker<FACE> subd(map) ;
David Cazier's avatar
David Cazier committed
223
224
225
226
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
227
					subd.mark(d) ;
David Cazier's avatar
David Cazier committed
228
229
					if (!buildingMark.isMarked(d))
					{
Thomas's avatar
Thomas committed
230
231
						//check if subdivision is authorized
						float minDistSq = Agent::neighborDistSq_ ;
232
						bool subdivisable = true ;
Thomas's avatar
Thomas committed
233
234
235
						Dart old = map.faceOldestDart(d) ;
						unsigned int fLevel = map.faceLevel(old) ;
						map.setCurrentLevel(fLevel) ;
David Cazier's avatar
David Cazier committed
236
						PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
Thomas's avatar
Thomas committed
237
						Dart fd = old ;
David Cazier's avatar
David Cazier committed
238
239
						do
						{
Thomas's avatar
Thomas committed
240
							PFP::VEC3& p = position[fd] ;
241
							PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd,
David Cazier's avatar
David Cazier committed
242
							                                                      position) ;
243
							PFP::VEC3 proj = fCenter
David Cazier's avatar
David Cazier committed
244
245
246
							    - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
							if (proj.norm2() < minDistSq)
							{
Thomas's avatar
Thomas committed
247
248
249
250
								subdivisable = false ;
								break ;
							}
							fd = map.phi1(fd) ;
251
						} while (fd != old) ;
Thomas's avatar
Thomas committed
252

David Cazier's avatar
David Cazier committed
253
254
						if (subdivisable)
						{
Thomas's avatar
Thomas committed
255
256
							map.setCurrentLevel(fLevel) ;
							Algo::IHM::subdivideFace<PFP>(map, old, position) ;
257
							subdiv = true ;
Thomas's avatar
Thomas committed
258
259
260
261
262
263
						}
						map.setCurrentLevel(map.getMaxLevel()) ;
					}
				}
			}
		}
264
	} while (subdiv) ;
Thomas's avatar
Thomas committed
265
266
267
268
}

void EnvMap::subdivideToProperLevel()
{
269
	bool subdiv ;
David Cazier's avatar
David Cazier committed
270
271
	do
	{
272
		subdiv = false ;
Thomas's avatar
Thomas committed
273
		{
David Cazier's avatar
David Cazier committed
274
			CellMarker<FACE> subd(map) ;
David Cazier's avatar
David Cazier committed
275
276
277
278
			for (Dart d = map.begin(); d != map.end(); map.next(d))
			{
				if (!subd.isMarked(d))
				{
279
					subd.mark(d) ;
David Cazier's avatar
David Cazier committed
280
281
					if (!refineMark.isMarked(d) && agentvect[d].size() > nbAgentsToSubdivide)
					{
282
						std::pair<bool, bool>& sf = subdivisableFace[d] ;
David Cazier's avatar
David Cazier committed
283
284
						if (sf.first == false || (sf.first == true && sf.second))
						{
285
286
287
							subdiv = true ;
							refineMark.mark(d) ;
							refineCandidate.push_back(d) ;
Thomas's avatar
Thomas committed
288
289
290
291
						}
					}
				}
			}
292
			subd.unmarkAll() ;
Thomas's avatar
Thomas committed
293
		}
294
295
296
297
		updateMap() ;
		refineCandidate.clear() ;
		map.setCurrentLevel(map.getMaxLevel()) ;
	} while (subdiv) ;
Thomas's avatar
Thomas committed
298
299
}

Pierre Kraemer's avatar
Pierre Kraemer committed
300
301
Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
David Cazier's avatar
David Cazier committed
302
	CellMarkerStore<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
303
304
305
306
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
307
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
308
309
			if (!map.isBoundaryFace(d) && !buildingMark.isMarked(d)
			    && Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true)) return d ;
310
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
311
312
	}

313
314
	std::cout << "ERROR : pos not in map" << std::endl ;
	return map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
315
}
Pierre Kraemer's avatar
Pierre Kraemer committed
316
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
317

Pierre Kraemer's avatar
Pierre Kraemer committed
318
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
319
320
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
{
321
	Dart dd = d ;
David Cazier's avatar
David Cazier committed
322
323
	do
	{
324
		Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
325
326
		while (ddd != dd)
		{
327
328
			f(ddd) ;
			ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
329
		}
330
331
		dd = map.phi1(dd) ;
	} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
332
333
}

David Cazier's avatar
David Cazier committed
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
// 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
380
381
void EnvMap::registerObstaclesInFaces()
{
David Cazier's avatar
David Cazier committed
382
	CellMarker<FACE> m(map) ;
David Cazier's avatar
David Cazier committed
383
384
385
386
	for (Dart d = map.begin(); d != map.end(); map.next(d))
	{
		if (!m.isMarked(d))
		{
387
			m.mark(d) ;
David Cazier's avatar
David Cazier committed
388
389
			if (!buildingMark.isMarked(d))
			{
390
				Dart dd = d ;
391
392
393
394

				//retrieve all obstacles within its one-ring

				//first : test all edges of the face of d
David Cazier's avatar
David Cazier committed
395
396
397
398
				do
				{
					if (obstacleMark.isMarked(dd))
					{
399
400
401
402
						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
403
404
						                           position[previous], position[map.phi1(next)],
						                           NULL, 0) ;
405
						obstvect[d].push_back(o) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
406
					}
407
408
					dd = map.phi1(dd) ;
				} while (dd != d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
409

410
				//second : test all edges of neighboring faces
David Cazier's avatar
David Cazier committed
411
412
				do
				{
413
					Dart ddd = map.alpha1(map.alpha1(dd)) ;
David Cazier's avatar
David Cazier committed
414
415
					while (ddd != dd)
					{
David Cazier's avatar
David Cazier committed
416
						// TODO Check effet de bord
David Cazier's avatar
David Cazier committed
417
418
						if (!buildingMark.isMarked(ddd)) addNeighborObstacles(
						    obstvect[d], ddd, ddd == map.alpha_1(dd)) ;
419
						ddd = map.alpha1(ddd) ;
420
					}
421
422
					dd = map.phi1(dd) ;
				} while (dd != d) ;
423
424
425
			}
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
426
427
428
429
}

void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbor)
{
430
	Dart stop ;
David Cazier's avatar
David Cazier committed
431
432
433
434
	if (edgeNeighbor)
		stop = map.phi_1(map.phi_1(map.phi_1(d))) ;
	else
		stop = map.phi_1(map.phi_1(d)) ;
435
436

	Dart dd = d ;
David Cazier's avatar
David Cazier committed
437
438
439
440
	do
	{
		if (obstacleMark.isMarked(dd))
		{
441
442
443
444
445
446
447
448
449
450
//			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
//			{
451
452
453
454
			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
455
			                           position[map.phi1(next)], NULL, 0) ;
456
			obst.push_back(o) ;
457
//			}
Pierre Kraemer's avatar
Pierre Kraemer committed
458
		}
459
460
		dd = map.phi1(dd) ;
	} while (dd != stop) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
461
462
463
464
}

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

467
468
	popAgentInCells(agent, oldFace) ;
	pushAgentInCells(agent, newFace) ;
469

David Cazier's avatar
David Cazier committed
470
471
	if (!refineMark.isMarked(newFace) && agentvect[newFace].size() > nbAgentsToSubdivide)
	{
472
		std::pair<bool, bool>& sf = subdivisableFace[newFace] ;
David Cazier's avatar
David Cazier committed
473
474
		if (sf.first == false || (sf.first == true && sf.second))
		{
475
476
			refineMark.mark(newFace) ;
			refineCandidate.push_back(newFace) ;
477
478
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
479

David Cazier's avatar
David Cazier committed
480
481
482
	if (!coarsenMark.isMarked(oldFace) && !refineMark.isMarked(oldFace)
	    && 4 * agentvect[oldFace].size() < nbAgentsToSimplify)
	{
483
484
		coarsenMark.mark(oldFace) ;
		coarsenCandidate.push_back(map.faceOldestDart(oldFace)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
485
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
486
487
}

Pierre Kraemer's avatar
Pierre Kraemer committed
488
489
void EnvMap::obstacleChangeFace(Obstacle* agent, Dart newFace, Dart oldFace)
{
490
491
	popObstacleInCells(agent, oldFace) ;
	pushObstacleInCells(agent, newFace) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
492
493
}

Pierre Kraemer's avatar
Pierre Kraemer committed
494
495
496
497
void EnvMap::updateMap()
{
	assert(map.getCurrentLevel() == map.getMaxLevel()) ;

David Cazier's avatar
David Cazier committed
498
499
500
501
	for (std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end();
	    ++it)
	{
		Dart d = (*it) ;
502
		refineMark.unmark(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
503

David Cazier's avatar
David Cazier committed
504
505
		if (agentvect[d].size() > nbAgentsToSubdivide)
		{
Thomas's avatar
Thomas committed
506
			int fLevel = -1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
507
508
			Dart old = map.faceOldestDart(d) ;

509
			bool subdivisable = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
510

511
			std::pair<bool, bool>& sf = subdivisableFace[old] ;
David Cazier's avatar
David Cazier committed
512
513
514
515
			if (sf.first == true)
				subdivisable = sf.second ;
			else
			{
516
				float minSizeSq = minCellSize * minCellSize ; // diametre de vision de l'agent au carré
517

Pierre Kraemer's avatar
Pierre Kraemer committed
518
				fLevel = map.faceLevel(old) ;
519
520
521
				map.setCurrentLevel(fLevel) ;
				PFP::VEC3 fCenter = Algo::Geometry::faceCentroid<PFP>(map, old, position) ;
				Dart fd = old ;
David Cazier's avatar
David Cazier committed
522
523
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
524
					PFP::VEC3& p = position[fd] ;
525
					PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
David Cazier's avatar
David Cazier committed
526
527
528
					PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
					if (proj.norm2() < minSizeSq)
					{
529
530
						subdivisable = false ;
						break ;
Pierre Kraemer's avatar
Pierre Kraemer committed
531
					}
532
					fd = map.phi1(fd) ;
533
				} while (fd != old) ;
534
535
				map.setCurrentLevel(map.getMaxLevel()) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
536
537
				sf.first = true ;
				sf.second = subdivisable ;
538
539
			}

David Cazier's avatar
David Cazier committed
540
541
			if (subdivisable)
			{
542
				if (fLevel == -1) fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
543
544

				sf.first = false ;
545

546
				PFP::AGENTS oldAgents(agentvect[old]) ;
David Cazier's avatar
David Cazier committed
547
548
				PFP::OBSTACLEVECT oldObst(obstvect[old]) ;
				PFP::OBSTACLEVECT oldNeighborObst(neighborObstvect[old]) ;
David Cazier's avatar
David Cazier committed
549
550
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
					popAgentInCells(*ait, old) ;
David Cazier's avatar
David Cazier committed
551
552
553
554
555
				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) ;
556
				neighborAgentvect[old].clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
557
558

				map.setCurrentLevel(fLevel) ;
559
				Algo::IHM::subdivideFace<PFP>(map, old, position) ;
David Cazier's avatar
David Cazier committed
560
				CellMarkerStore<FACE> newF(map) ;
561
562
				unsigned int degree = 0 ;
				Dart dd = old ;
David Cazier's avatar
David Cazier committed
563
564
				do
				{
565
					++degree ;
566
					newF.mark(dd) ;
567
					dd = map.phi1(dd) ;
568
				} while (dd != old) ;
David Cazier's avatar
David Cazier committed
569

David Cazier's avatar
David Cazier committed
570
571
				if (degree == 3)
				{
572
					Dart centerFace = map.phi2(map.phi1(old)) ;
573
					newF.mark(centerFace) ;
574
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
575
				map.setCurrentLevel(map.getMaxLevel()) ;
576

David Cazier's avatar
David Cazier committed
577
578
579
580
				for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
				{
					resetAgentInFace(*ait) ;
					pushAgentInCells(*ait, (*ait)->part_.d) ;
581
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
582

David Cazier's avatar
David Cazier committed
583
584
585
586
587
588
589
590
591
592
593
594
595
				for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
				{
					resetObstInFace(*ait) ;
					update_registration(*ait) ;
				}

				for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();
				    ait != oldNeighborObst.end(); ++ait)
				{
					resetObstInFace(*ait) ;
					update_registration(*ait) ;
				}

596
				dd = old ;
David Cazier's avatar
David Cazier committed
597
598
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
599
					Dart d3 = dd ;
David Cazier's avatar
David Cazier committed
600
601
					do
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
602
603
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
David Cazier's avatar
David Cazier committed
604
605
606
607
						while (d4 != d3)
						{
							if (!newF.isMarked(d4))
							{
Pierre Kraemer's avatar
Pierre Kraemer committed
608
609
610
611
								PFP::AGENTS& ad4 = agentvect[d4] ;
								nad3.insert(nad3.end(), ad4.begin(), ad4.end()) ;
							}
							d4 = map.alpha1(d4) ;
612
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
613
						d3 = map.phi1(d3) ;
614
					} while (d3 != dd) ;
615
616

					map.setCurrentLevel(fLevel) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
617
					dd = map.phi1(dd) ;
618
					map.setCurrentLevel(map.getMaxLevel()) ;
619
				} while (dd != old) ;
620

David Cazier's avatar
David Cazier committed
621
622
				if (degree == 3)
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
623
624
					Dart centerFace = map.phi2(map.phi1(old)) ;
					Dart d3 = centerFace ;
David Cazier's avatar
David Cazier committed
625
626
					do
					{
Pierre Kraemer's avatar
Pierre Kraemer committed
627
628
						Dart d4 = map.alpha1(map.alpha1(d3)) ;
						PFP::AGENTS& nad3 = neighborAgentvect[d3] ;
David Cazier's avatar
David Cazier committed
629
630
631
632
						while (d4 != d3)
						{
							if (!newF.isMarked(d4))
							{
Pierre Kraemer's avatar
Pierre Kraemer committed
633
								PFP::AGENTS& ad4 = agentvect[d4] ;
David Cazier's avatar
David Cazier committed
634
								nad3.insert(neighborAgentvect[d3].end(), ad4.begin(), ad4.end()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
635
636
							}
							d4 = map.alpha1(d4) ;
637
						}
Pierre Kraemer's avatar
Pierre Kraemer committed
638
						d3 = map.phi1(d3) ;
639
					} while (d3 != centerFace) ;
640
				}
Pierre Kraemer's avatar
Pierre Kraemer committed
641
642
643
			}
		}
	}
David Cazier's avatar
David Cazier committed
644
645
646
647
648
649
650
651
	refineCandidate.clear() ;

	// On recrée une liste des faces à simplifier en empêchant les doublons
	// On en profite pour vérifier les conditions de simplifications
	std::vector<Dart> checkCoarsenCandidate ;
	checkCoarsenCandidate.reserve(coarsenCandidate.size()) ;
	for (unsigned int it = 0; it < coarsenCandidate.size(); ++it)
	{
Thomas's avatar
Thomas committed
652
		Dart old = coarsenCandidate[it] ;
David Cazier's avatar
David Cazier committed
653
		bool oldIsMarked = coarsenMark.isMarked(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
654
		coarsenMark.unmark(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
655

Pierre Kraemer's avatar
Pierre Kraemer committed
656
		unsigned int fLevel = map.faceLevel(old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
657

David Cazier's avatar
David Cazier committed
658
659
		if (oldIsMarked && fLevel > 0 && map.getDartLevel(old) < fLevel)
		{
Pierre Kraemer's avatar
Pierre Kraemer committed
660
661
662
			unsigned int cur = map.getCurrentLevel() ;
			map.setCurrentLevel(fLevel - 1) ;

David Cazier's avatar
David Cazier committed
663
664
			if (map.faceIsSubdividedOnce(old))
			{
Pierre Kraemer's avatar
Pierre Kraemer committed
665
666
				// on compte le nombre d'agents dans les sous-faces
				// on en profite pour compter le degré de la face grossière
Pierre Kraemer's avatar
Pierre Kraemer committed
667
668
669
				unsigned int degree = 0 ;
				unsigned int nbAgents = 0 ;
				Dart fit = old ;
David Cazier's avatar
David Cazier committed
670
671
				do
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
672
					nbAgents += agentvect[fit].size() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
673
					++degree ;
Pierre Kraemer's avatar
Pierre Kraemer committed
674
					coarsenMark.unmark(fit) ;
David Cazier's avatar
David Cazier committed
675
676
677
678
679
680
681
682
683
684
685
686
687
688
// 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 ;
//					}

Pierre Kraemer's avatar
Pierre Kraemer committed
689
					fit = map.phi1(fit) ;
690
691
				} while (fit != old) ;

David Cazier's avatar
David Cazier committed
692
693
				if (degree == 3)
				{
Pierre Kraemer's avatar
Pierre Kraemer committed
694
695
696
					map.setCurrentLevel(fLevel) ;
					Dart centerFace = map.phi2(map.phi1(old)) ;
					nbAgents += agentvect[centerFace].size() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
697
					coarsenMark.unmark(centerFace) ;
David Cazier's avatar
David Cazier committed
698
699
700
701
702
703
704
705
706
707
708
709
710
// 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 ;
//					}					
Pierre Kraemer's avatar
Pierre Kraemer committed
711
712
					map.setCurrentLevel(fLevel - 1) ;
				}
David Cazier's avatar
David Cazier committed
713
714
715
716
717
718
				if (nbAgents < nbAgentsToSimplify) checkCoarsenCandidate.push_back(old) ;
			}
			map.setCurrentLevel(cur) ;
		}
	}
	coarsenCandidate.clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
719

David Cazier's avatar
David Cazier committed
720
721
722
723
	// On réalise la simplification (les conditions ont déjà été vérifiées)
	for (unsigned int it = 0; it < checkCoarsenCandidate.size(); ++it)
	{
		Dart old = checkCoarsenCandidate[it] ;
Pierre Kraemer's avatar
Pierre Kraemer committed
724

David Cazier's avatar
David Cazier committed
725
726
727
		unsigned int fLevel = map.faceLevel(old) ;
		unsigned int cur = map.getCurrentLevel() ;
		map.setCurrentLevel(fLevel - 1) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
728

David Cazier's avatar
David Cazier committed
729
730
731
732
733
734
735
736
		// 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) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
737

David Cazier's avatar
David Cazier committed
738
		PFP::AGENTS agents ;
David Cazier's avatar
David Cazier committed
739
740
741
		PFP::OBSTACLEVECT obst ;
		PFP::OBSTACLEVECT neighborObst ;

David Cazier's avatar
David Cazier committed
742
743
744
745
		fit = old ;
		do
		{
			PFP::AGENTS a(agentvect[fit]) ;
David Cazier's avatar
David Cazier committed
746
747
748
			PFP::OBSTACLEVECT ob(obstvect[fit]) ;
			PFP::OBSTACLEVECT nob(neighborObstvect[fit]) ;

David Cazier's avatar
David Cazier committed
749
			agents.insert(agents.end(), a.begin(), a.end()) ;
David Cazier's avatar
David Cazier committed
750
751
			obst.insert(obst.end(), ob.begin(), ob.end()) ;
			neighborObst.insert(neighborObst.end(), nob.begin(), nob.end()) ;
David Cazier's avatar
David Cazier committed
752
753
754
755
756
757
758
759
760
			map.setCurrentLevel(map.getMaxLevel()) ;
			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] ;
David Cazier's avatar
David Cazier committed
761
				PFP::OBSTACLEVECT resetob = obstvect[nf] ;
David Cazier's avatar
David Cazier committed
762
763
764
765
				for (PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
				{
					if ((*ait)->part_.d == map.phi1(nf)) (*ait)->part_.d = nf ;
				}
David Cazier's avatar
David Cazier committed
766
767
768
769
770
771
772
773
				for (PFP::OBSTACLEVECT::iterator ait = resetob.begin(); ait != resetob.end(); ++ait)
				{
					MovingObstacle* mo = (*ait)->mo ;
					if (mo != NULL)
					{
						resetPart(mo, nf) ;
					}
				}
David Cazier's avatar
David Cazier committed
774
775
776
777
				map.setCurrentLevel(fLevel - 1) ;
			}
			fit = map.phi1(fit) ;
		} while (fit != old) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
778

David Cazier's avatar
David Cazier committed
779
780
781
782
783
		if (degree == 3)
		{
			map.setCurrentLevel(fLevel) ;
			Dart centerFace = map.phi2(map.phi1(old)) ;
			PFP::AGENTS a(agentvect[centerFace]) ;
David Cazier's avatar
David Cazier committed
784
785
			PFP::OBSTACLEVECT ob(obstvect[centerFace]) ;
			PFP::OBSTACLEVECT nob(neighborObstvect[centerFace]) ;
David Cazier's avatar
David Cazier committed
786
			agents.insert(agents.end(), a.begin(), a.end()) ;
David Cazier's avatar
David Cazier committed
787
788
			obst.insert(obst.end(), ob.begin(), ob.end()) ;
			neighborObst.insert(neighborObst.end(), nob.begin(), nob.end()) ;
David Cazier's avatar
David Cazier committed
789
790
791
792
793
794
			map.setCurrentLevel(map.getMaxLevel()) ;
			for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
				popAgentInCells(*ait, centerFace) ;
			map.setCurrentLevel(fLevel - 1) ;
		}
		neighborAgentvect[old].clear() ;
David Cazier's avatar
David Cazier committed
795
796
797
798
799
800
801
802
		// TODO Check with optimisation
		map.setCurrentLevel(map.getMaxLevel()) ;
		for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
			register_pop(*ait, (*ait)->index) ;
		for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end();
		    ++ait)
			register_pop(*ait, (*ait)->index) ;
		map.setCurrentLevel(fLevel - 1) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
803

David Cazier's avatar
David Cazier committed
804
		Algo::IHM::coarsenFace<PFP>(map, old, position) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
805

David Cazier's avatar
David Cazier committed
806
807
808
809
810
811
812
813
814
815
816
		std::pair<bool, bool>& sf = subdivisableFace[old] ;
		sf.first = true ;
		sf.second = true ;

		map.setCurrentLevel(map.getMaxLevel()) ;

		for (PFP::AGENTS::iterator itA = agents.begin(); itA != agents.end(); ++itA)
		{
			(*itA)->part_.d = old ;
			pushAgentInCells(*itA, old) ;
		}
David Cazier's avatar
David Cazier committed
817
818
819
820
821
822
823
824
825
826
		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) ;
		}
David Cazier's avatar
David Cazier committed
827
828
829
830
831
832
833

		Dart dd = old ;
		do
		{
			Dart ddd = map.alpha1(map.alpha1(dd)) ;
			while (ddd != dd)
			{
David Cazier's avatar
David Cazier committed
834
835
				neighborAgentvect[old].insert(neighborAgentvect[old].end(), agentvect[ddd].begin(),
				                              agentvect[ddd].end()) ;
David Cazier's avatar
David Cazier committed
836
				ddd = map.alpha1(ddd) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
837
			}
David Cazier's avatar
David Cazier committed
838
839
840
			dd = map.phi1(dd) ;
		} while (dd != old) ;

David Cazier's avatar
David Cazier committed
841
		if (fLevel > 1 && !coarsenMark.isMarked(old) && agentvect[old].size() < nbAgentsToSimplify)
David Cazier's avatar
David Cazier committed
842
843
844
		{
			coarsenMark.mark(old) ;
			coarsenCandidate.push_back(map.faceOldestDart(old)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
845
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
846

David Cazier's avatar
David Cazier committed
847
848
		map.setCurrentLevel(cur) ;
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
849
	map.setCurrentLevel(map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
850
	if (coarsenCandidate.size() > 0) updateMap() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
851
852
853
854
}

void EnvMap::resetAgentInFace(Agent* agent)
{
David Cazier's avatar
David Cazier committed
855
856
857
	VEC3 pos = agent->part_.getPosition() ;
	agent->part_.move(Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position)) ;
	agent->part_.setState(FACE) ;
858
	agent->part_.move(pos) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
859
}
Pierre Kraemer's avatar
Pierre Kraemer committed
860
#endif
David Cazier's avatar
David Cazier committed
861
862
863
864
865
866
867
868
869

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

David Cazier's avatar
David Cazier committed
870
871
const std::vector<Agent*>& EnvMap::getNeighbors(Agent* a)
{
David Cazier's avatar
David Cazier committed
872
873
874
875
	Geom::Vec2ui c = agentPositionCell(a) ;
	return ht_agents[c] ;
}

David Cazier's avatar
David Cazier committed
876
877
878
879
880
881
882
883
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)
{
David Cazier's avatar
David Cazier committed
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
	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()) ;
			}
		}
	}
}

David Cazier's avatar
David Cazier committed
899
900
void EnvMap::addAgentInGrid(Agent* a)
{
David Cazier's avatar
David Cazier committed
901
902
903
904
	Geom::Vec2ui c = agentPositionCell(a) ;
	addAgentInGrid(a,c) ;
}

David Cazier's avatar
David Cazier committed
905
906
void EnvMap::addAgentInGrid(Agent* a, Geom::Vec2ui c)
{
David Cazier's avatar
David Cazier committed
907
	ht_agents[c].push_back(a) ;
David Cazier's avatar
David Cazier committed
908
909
910
911
912
913
914
915
916
917
918
919

	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) ;
			}
		}
	}
David Cazier's avatar
David Cazier committed
920
921
}

David Cazier's avatar
David Cazier committed
922
923
void EnvMap::removeAgentFromGrid(Agent* a)
{
David Cazier's avatar
David Cazier committed
924
925
926
927
	Geom::Vec2ui c = agentPositionCell(a) ;
	removeAgentFromGrid(a,c) ;
}

David Cazier's avatar
David Cazier committed
928
929
void EnvMap::removeAgentFromGrid(Agent* a, Geom::Vec2ui c)
{
David Cazier's avatar
David Cazier committed
930
931
	removeElementFromVector<Agent*>(ht_agents[c], a) ;
	if (ht_agents[c].empty()) ht_agents.erase(c) ;
David Cazier's avatar
David Cazier committed
932
933
934
935
936
937
938
939
940
941
942
943
944

	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) ;
			}
		}
	}
David Cazier's avatar
David Cazier committed
945
946
947
}

#endif