Création d'un compte pour un collaborateur extérieur au laboratoire depuis l'intranet ICube : https://intranet.icube.unistra.fr/fr/labs/member/profile

simulator.cpp 28.5 KB
Newer Older
Pierre Kraemer's avatar
Pierre Kraemer committed
1
2
#include "simulator.h"

3
Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgent, unsigned int nbObst) :
Thomas Jund's avatar
Thomas Jund committed
4
	timeStep_(0.05f),
David Cazier's avatar
David Cazier committed
5
6
7
8
9
10
11
	globalTime_(0.0f),
	nbSteps_(0),
	nbUpdates(0),
	nbSorts(0),
	nbRefineCandidate(0),
	nbCoarsenCandidate(0),
	nearNeighbors(0),
David Cazier's avatar
David Cazier committed
12
	totalNeighbors(0),
pitiot's avatar
maj    
pitiot committed
13
	avoidance(1),
pitiot's avatar
pitiot committed
14
	nb_dead(0)
Pierre Kraemer's avatar
Pierre Kraemer committed
15
{
pitiot's avatar
pitiot committed
16
	minSize=minS;
pitiot's avatar
merging    
pitiot committed
17
18
	multires=true;
	detect_agent_collision=true;
19
20
	srand(10) ;
	nbStepsPerUnit_ = 1 / timeStep_ ;
pitiot's avatar
merging    
pitiot committed
21
	this->config=config;
22
	init(2.0f, nbAgent, nbObst) ;
Thomas's avatar
Thomas committed
23
24
25
26
}

Simulator::~Simulator()
{
David Cazier's avatar
David Cazier committed
27
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
28
		delete agents_[i] ;
Thomas's avatar
Thomas committed
29
30
}

31
void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst, bool enablePathFinding)
Thomas's avatar
Thomas committed
32
{
33
	std::cout << "Setup scenario" << std::endl ;
Thomas's avatar
Thomas committed
34

David Cazier's avatar
David Cazier committed
35
36
	switch (config)
	{
David Cazier's avatar
David Cazier committed
37
		case 0 :
38
			setupCircleScenario(nbAgent,nbObst) ;
Thomas Jund's avatar
Thomas Jund committed
39
//			addPathsToAgents();
David Cazier's avatar
David Cazier committed
40
41
			break ;
		case 1 :
42
			setupCorridorScenario(nbAgent,nbObst) ;
David Cazier's avatar
David Cazier committed
43
44
			break ;
		case 2 :
45
			setupSnakeCorridorScenario(nbAgent,nbObst,10) ;
David Cazier's avatar
David Cazier committed
46
			break ;
pitiot's avatar
pitiot committed
47
48
49
50
51
52
53
54
//		case 3 :
//			setupCityScenario(20, 20) ;
////			setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10),
////					-1.0f * (12 * (70.0f / 2.0f) - 10), 20, 20);
//			break ;
//		case 4 :
//			importAgents("myAgents.pos") ;
//			break ;
Jund Thomas's avatar
Jund Thomas committed
55
		case 5:
56
			envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ; //svg import
57
			setupScenario(nbAgent) ;
58
			addPathsToAgents();
59
//			SelectorCellNotMarked<PFP::MAP> scnm(envMap_.pedWayMark);
60
			addMovingObstacles(nbObst);
Thomas Jund's avatar
Thomas Jund committed
61
//			addPathToObstacles();
Jund Thomas's avatar
Jund Thomas committed
62
			break;
David Cazier's avatar
David Cazier committed
63
64
65
		default:
			std::cout << "Unknown scenario !" << std::endl ;
			exit(1) ;
Thomas's avatar
Thomas committed
66
67
	}

David Cazier's avatar
David Cazier committed
68
69
70
71
72
#ifndef SPATIAL_HASHING
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
#endif

Pierre Kraemer's avatar
Pierre Kraemer committed
73
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
74
75
76
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
David Cazier's avatar
David Cazier committed
77
			addPathsToAgents() ;
78
79
		else if (dimension == 2.5f)
			addPathsToAgents_height() ;
Thomas's avatar
Thomas committed
80
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
81
#endif
82

David Cazier's avatar
David Cazier committed
83
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
84
	{
David Cazier's avatar
David Cazier committed
85
86
		agents_[i]->updateObstacleNeighbors() ;
		agents_[i]->updateAgentNeighbors() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
87
	}
88

89

Pierre Kraemer's avatar
Pierre Kraemer committed
90
#ifndef SPATIAL_HASHING
91
92
	if (multires)
		envMap_.subdivideToProperLevel() ;
David Cazier's avatar
David Cazier committed
93
	//	envMap_.subdivideAllToMaxLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
94
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
95
96
}

David Cazier's avatar
David Cazier committed
97
void Simulator::doStep()
Pierre Kraemer's avatar
Pierre Kraemer committed
98
{
David Cazier's avatar
David Cazier committed
99
100
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
Jund Thomas's avatar
Jund Thomas committed
101
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
102
#endif
David Cazier's avatar
David Cazier committed
103

David Cazier's avatar
David Cazier committed
104
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
pitiot's avatar
pitiot committed
105
	{
David Cazier's avatar
David Cazier committed
106
		movingObstacles_[i]->computePrefVelocity() ;
pitiot's avatar
pitiot committed
107
		movingObstacles_[i]->computeNewVelocity() ;
pitiot's avatar
pitiot committed
108
109
110
#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
David Cazier's avatar
David Cazier committed
111
		movingObstacles_[i]->update() ;
112

113
		movingObstacles_[i]->updateMesh() ;
pitiot's avatar
pitiot committed
114
115
	}

David Cazier's avatar
David Cazier committed
116
117
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
pitiot's avatar
pitiot committed
118
119
120
		if (agents_[i]->alive)
		{
			agents_[i]->updateObstacleNeighbors() ;
pitiot's avatar
maj    
pitiot committed
121
122

#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
123
			envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
124
#endif
pitiot's avatar
pitiot committed
125
			agents_[i]->updateAgentNeighbors() ;
pitiot's avatar
maj    
pitiot committed
126
#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
127
			envMap_.map.setCurrentLevel(0) ;
pitiot's avatar
maj    
pitiot committed
128
129
#endif

pitiot's avatar
pitiot committed
130
131
132
			agents_[i]->computePrefVelocity() ;
			agents_[i]->computeNewVelocity() ;
		}
David Cazier's avatar
David Cazier committed
133
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
134

Pierre Kraemer's avatar
Pierre Kraemer committed
135
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
136
137
138
139
140
141
142
143
144
145
146
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		Geom::Vec2ui cold = envMap_.agentPositionCell(agents_[i]) ;
		agents_[i]->update() ;
		Geom::Vec2ui cnew = envMap_.agentPositionCell(agents_[i]) ;
		if (cnew != cold)
		{
			envMap_.removeAgentFromGrid(agents_[i], cold) ;
			envMap_.addAgentInGrid(agents_[i], cnew) ;
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
147
#else
148
149
	if (multires)
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
150
151
152
153
154
155
156
	nb_dead = 0 ;
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		if (agents_[i]->alive)
		{
			Dart oldFace = agents_[i]->part_.d ;
			agents_[i]->update() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
157

David Cazier's avatar
David Cazier committed
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
//		if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE))
			if (agents_[i]->part_.crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//		switch(agents_[i]->part_.crossCell)
			{
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace) ;
//				break;
			}
		}
		else
			nb_dead++ ;

	}

	nbRefineCandidate += envMap_.refineCandidate.size() ;
	nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
Jund Thomas's avatar
Jund Thomas committed
177

Thomas Jund's avatar
Thomas Jund committed
178
179
	 if (multires)
		 envMap_.updateMap() ;
David Cazier's avatar
David Cazier committed
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
#endif

	globalTime_ += timeStep_ ;
	++nbSteps_ ;
}

bool Simulator::reachedGoal()
{
	/* Check if all agents have reached their goals. */
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
		if ((agents_[i]->getPosition() - agents_[i]->goals_[agents_[i]->curGoal_]).norm2()
		    > agents_[i]->radius_ * agents_[i]->radius_) return false ;

	return true ;
}

David Cazier's avatar
David Cazier committed
196
void Simulator::addAgent(const VEC3& start, const VEC3& goal)
David Cazier's avatar
David Cazier committed
197
{
David Cazier's avatar
David Cazier committed
198
	agents_.push_back(new Agent(this, start, goal)) ;
David Cazier's avatar
David Cazier committed
199
200
}

201
202
203
204
205
void Simulator::addAgent(const VEC3& start, const VEC3& goal, Dart d)
{
	agents_.push_back(new Agent(this, start, goal, d)) ;
}

David Cazier's avatar
David Cazier committed
206
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
David Cazier's avatar
David Cazier committed
207
{
pitiot's avatar
merging    
pitiot committed
208
209
210
211
212
213
214
215
216
217
	if (multires)
	{
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
	}
	else
	{

		envMap_.init(config, 1600.0f, 960.0f, minSize, 400.0f) ; //cases fines
	}

David Cazier's avatar
David Cazier committed
218
219
220
221
222
223
224
225
226
227
228
229
230
231
	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents" << std::endl ;

	// Bordure à éviter autour de la scène (10% de sa taille)
	int xBorder = envMap_.geometry.size(0) / 10 ;
	int yBorder = envMap_.geometry.size(1) / 10 ;

	// Les coordonnées sont comprises entre xMin et xMin+xDelta

	// Départ des agents sur un cercle centré
	int xSize = envMap_.geometry.size(0) - 2 * xBorder ;
	int ySize = envMap_.geometry.size(1) - 2 * yBorder ;
	int radius = min(xSize, ySize) / 2 ;
	VEC3 center = (envMap_.geometry.min() + envMap_.geometry.max()) / 2 ;

David Cazier's avatar
David Cazier committed
232
	double pi = 3.14159265358979323846f ;
David Cazier's avatar
David Cazier committed
233
234
235
236
237
238
239

	for (unsigned int i = 0 ; i < nbAgents ; ++i)
	{
		double angle = i * 2.0f * pi / float(nbAgents) ;
		VEC3 v(std::cos(angle) * radius, std::sin(angle) * radius, 0) ;
		VEC3 start = center + v ;
		VEC3 goal = center - v ;
David Cazier's avatar
David Cazier committed
240
		addAgent(start, goal) ;
241
	}
pitiot's avatar
pitiot committed
242
243
244
245
246
	VEC3 xSide (5.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,10.0f,0.0f);
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;
	std::vector<VEC3> positions;
pitiot's avatar
pitiot committed
247
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
David Cazier's avatar
David Cazier committed
248
	{
pitiot's avatar
pitiot committed
249
250
251
252
253
254
255
256
		double angle = i * 2.0f * pi / float(nbObstacles/2) ;
		VEC3 v,start;
		if(i<nbObstacles/2)
		{
			v=VEC3 (std::cos(angle) * (2*radius/3), std::sin(angle) * (2*radius/3), 0) ;
		}
		else
		{
pitiot's avatar
merging    
pitiot committed
257
			v=VEC3 (std::cos(angle) * (radius/2), std::sin(angle) * (radius/2), 0) ;
pitiot's avatar
pitiot committed
258
259
260
		}
		start = center + v ;
		positions.push_back(start);
pitiot's avatar
pitiot committed
261
262
263
264
	}
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
265
266
267
268
269
270
271
//		std::vector<VEC3> goals;
//		for(unsigned int k = 0 ; k < nbObstacles/2 ; k++)
//		{
//			goals.push_back(positions[(i+k)%(nbObstacles/2)]);
//		}

		unsigned int nbGoals=360;
pitiot's avatar
pitiot committed
272
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
273
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
274
		{
Thomas Jund's avatar
Thomas Jund committed
275
			goals.push_back(VEC3(std::cos(2.0f*M_PI*k/nbGoals) * (2*radius/3), std::sin(2.0f*M_PI*k/nbGoals) * (2*radius/3), 0)) ;
pitiot's avatar
pitiot committed
276
277
278
279
280
281
282
283
284
		}

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		vPos.push_back(start+xSide-ySide);
		vPos.push_back(start+xSide+ySide);
		vPos.push_back(start-xSide+ySide);
		vPos.push_back(start-xSide-ySide);

285
		mo4= new MovingObstacle(this, i,vPos,goals, true, true);
Thomas Jund's avatar
Thomas Jund committed
286
		mo4->curGoal_ = i*float(nbGoals)/float(nbObstacles/2.0f);
pitiot's avatar
pitiot committed
287

pitiot's avatar
pitiot committed
288
289
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
290

pitiot's avatar
pitiot committed
291
292
293
	for (unsigned int i = nbObstacles/2 ; i < nbObstacles ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
294
295
296
297
298
299
300
//		std::vector<VEC3> goals;
//		for(unsigned int k = (nbObstacles/2) ; k > 0 ; k--)
//		{
//			goals.push_back(positions[((i-(nbObstacles/2)+k)%(nbObstacles/2))+nbObstacles/2]);
//		}

		unsigned int nbGoals=360;
pitiot's avatar
pitiot committed
301
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
302
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
303
		{
Thomas Jund's avatar
Thomas Jund committed
304
			goals.push_back(VEC3(std::cos(2.0f*M_PI*k/nbGoals) * (radius/2), std::sin(2.0f*M_PI*k/nbGoals) * (radius/2), 0)) ;
pitiot's avatar
pitiot committed
305
306
307
308
309
310
311
312
313
		}

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		vPos.push_back(start+xSide-ySide);
		vPos.push_back(start+xSide+ySide);
		vPos.push_back(start-xSide+ySide);
		vPos.push_back(start-xSide-ySide);

314
		mo4= new MovingObstacle(this, i,vPos,goals, true, true);
Thomas Jund's avatar
Thomas Jund committed
315
		mo4->curGoal_ = (i-nbObstacles/2)*float(nbGoals)/float(nbObstacles/2.0f);
pitiot's avatar
pitiot committed
316
317

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
318
	}
Thomas's avatar
Thomas committed
319

320
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
321
322
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
323
#endif
pitiot's avatar
merging    
pitiot committed
324
325

	std::cout << "nb agents : " << agents_.size() << std::endl ;
Thomas's avatar
Thomas committed
326
327
}

David Cazier's avatar
David Cazier committed
328
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
329
{
pitiot's avatar
merging    
pitiot committed
330
331
332
333
334
335
336
337
338
339
340
	if (multires)
	{
		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
	}
	else
	{

		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //cases fines
	}


David Cazier's avatar
David Cazier committed
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
	std::cout << " - Setup Corridor Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles" << std::endl ;

	// Bordure à éviter autour de la scène (10% de sa taille)
	int xBorder = envMap_.geometry.size(0) / 10 ;
	int yBorder = envMap_.geometry.size(1) / 10 ;

	// Les coordonnées sont comprises entre xMin et xMin+xDelta

	// Départ des agents du quart gauche sur toute la hauteur
	int xStartMin = envMap_.geometry.min()[0] + xBorder ;
	int xStartDelta = envMap_.geometry.size(0) / 5 ;
	int yStartMin = envMap_.geometry.min()[1] + yBorder ;
	int yStartDelta = envMap_.geometry.size(1) - 2 * yBorder ;

	// Arrivée des agents à l'opposée
	int xGoalDelta = envMap_.geometry.size(0) / 5 ;
	int xGoalMin = envMap_.geometry.max()[0] - xBorder - xGoalDelta ;
	int yGoalMin = yStartMin ;
	int yGoalDelta = yStartDelta ;

	for (unsigned int i = 0 ; i < nbAgents ; ++i)
	{
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;

		// Un agent sur 2 va de droite à gauche
		if (i % 2 == 1)
			addAgent(start, goal) ;
		else
			addAgent(goal, start) ;
	}

	// Départ des obstacles du quart haut sur toute une demi-largeur
	xStartMin = envMap_.geometry.min()[0] + envMap_.geometry.size(0) / 4 ;
	xStartDelta = envMap_.geometry.size(0) / 2 ;
	yStartMin = envMap_.geometry.min()[1] + yBorder ;
	yStartDelta = envMap_.geometry.size(1) / 5 ;
pitiot's avatar
pitiot committed
378

David Cazier's avatar
David Cazier committed
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
	// Arrivée des obstacles à l'opposée
	yGoalDelta = envMap_.geometry.size(1) / 5 ;
	yGoalMin = envMap_.geometry.max()[1] - yBorder - yGoalDelta ;

	VEC3 xSide (5.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,10.0f,0.0f);
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
		float x = xStartMin + ((int)i*30) % xStartDelta;
//		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(x, yGoalMin  + rand() % yGoalDelta,  0) ;

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		VEC3 tmp ;
		if (i % 2 == 1)
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
		}
		else {
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
		}
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
417
		mo4= new MovingObstacle(this, i,vPos,goals, true, false);
David Cazier's avatar
David Cazier committed
418
419


420
421
422
423
424
425
426
427
428
429
430
431
432
433
//		//for generating a random path
//		unsigned int dartDistForPath = 50 ;
//		mo4->goals_.clear() ;
//		Dart dStart = mo4->registering_part->d;
//		Dart dStop = dStart ;
//		for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20 || envMap_.map.sameFace(dStop, dStart) ; ++j)
//		{
//			envMap_.map.next(dStop) ;
//			if (dStop == envMap_.map.end())
//				dStop = envMap_.map.begin() ;
//		}
//
//		addPathToObstacle(mo4, dStart, dStop);
//		addPathToObstacle(mo4, dStop, dStart);
David Cazier's avatar
David Cazier committed
434
435
436
437
438

		movingObstacles_.push_back(mo4);
	}
}

pitiot's avatar
merging    
pitiot committed
439
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
David Cazier's avatar
David Cazier committed
440
{
pitiot's avatar
merging    
pitiot committed
441
442
443
444
445
446
	if (multires)
		envMap_.init(config, 1600.0f, 960.0f, minSize, 160.0f) ; //grosses cases
	else
		envMap_.init(config, 1600.0f, 960.0f, minSize, 160.0f) ; //cases fines

	std::cout << " - Setup Snake Corridor Scenario : " << nbAgents << " agents et " << nbSnakes*snakeSize << " obstacles" << std::endl ;
David Cazier's avatar
David Cazier committed
447
448

	// Bordure à éviter autour de la scène (10% de sa taille)
pitiot's avatar
merging    
pitiot committed
449
450
	int xBorder = envMap_.geometry.size(0) / 5 ;
	int yBorder = envMap_.geometry.size(1) / 5 ;
David Cazier's avatar
David Cazier committed
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466

	// Les coordonnées sont comprises entre xMin et xMin+xDelta

	// Départ des agents du quart gauche sur toute la hauteur
	int xStartMin = envMap_.geometry.min()[0] + xBorder ;
	int xStartDelta = envMap_.geometry.size(0) / 5 ;
	int yStartMin = envMap_.geometry.min()[1] + yBorder ;
	int yStartDelta = envMap_.geometry.size(1) - 2 * yBorder ;

	// Arrivée des agents à l'opposée
	int xGoalDelta = envMap_.geometry.size(0) / 5 ;
	int xGoalMin = envMap_.geometry.max()[0] - xBorder - xGoalDelta ;
	int yGoalMin = yStartMin ;
	int yGoalDelta = yStartDelta ;

	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
467
	{
David Cazier's avatar
David Cazier committed
468
469
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;
470

David Cazier's avatar
David Cazier committed
471
		// Un agent sur 2 va de droite à gauche
pitiot's avatar
merging    
pitiot committed
472
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
473
		if (i % 2 == 1)
pitiot's avatar
merging    
pitiot committed
474
475
476
477
478
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
		}
pitiot's avatar
merge    
pitiot committed
479

pitiot's avatar
merging    
pitiot committed
480
		addAgent(start, goal) ;
pitiot's avatar
merge    
pitiot committed
481

Thomas's avatar
Thomas committed
482
483
	}

David Cazier's avatar
David Cazier committed
484
485
486
487
	// Départ des obstacles du quart haut sur toute une demi-largeur
	xStartMin = envMap_.geometry.min()[0] + envMap_.geometry.size(0) / 4 ;
	xStartDelta = envMap_.geometry.size(0) / 2 ;
	yStartMin = envMap_.geometry.min()[1] + yBorder ;
pitiot's avatar
merging    
pitiot committed
488
	yStartDelta = envMap_.geometry.size(1) / 20 ;
David Cazier's avatar
David Cazier committed
489
490

	// Arrivée des obstacles à l'opposée
pitiot's avatar
merge    
pitiot committed
491

pitiot's avatar
merging    
pitiot committed
492
	yGoalDelta = 3* envMap_.geometry.size(1) / 5 ;
pitiot's avatar
merge    
pitiot committed
493

David Cazier's avatar
David Cazier committed
494
495
	yGoalMin = envMap_.geometry.max()[1] - yBorder - yGoalDelta ;

496
	VEC3 xSide (5.0f,0.0f,0.0f);
497
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
498

pitiot's avatar
merging    
pitiot committed
499
500
501

	int sumObstacles=0;
	for(unsigned int j = 0; j<nbSnakes; j++)
David Cazier's avatar
David Cazier committed
502
	{
pitiot's avatar
merging    
pitiot committed
503
504
505
506
		std::vector<PFP::VEC3> positions [snakeSize] ;
		float x = xStartMin + rand() % xStartDelta;
		VEC3 goal;

David Cazier's avatar
David Cazier committed
507
508
		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;

pitiot's avatar
merging    
pitiot committed
509
510
511
512
513
514
515
516
517
518
519
520
521

		std::vector<VEC3> vPos;


		vPos.push_back(start+xSide-ySide);
		vPos.push_back(start+xSide+ySide);
		vPos.push_back(start-xSide+ySide);
		vPos.push_back(start-xSide-ySide);

		std::vector<VEC3> goals;
		goals.push_back(start);
		int r=0;
		while (r<40)
David Cazier's avatar
David Cazier committed
522
		{
pitiot's avatar
merging    
pitiot committed
523
524
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
525
526
			if ((goal-goals[r]).norm2()>1000)
			{
pitiot's avatar
merging    
pitiot committed
527
				goals.push_back(goal);
528
				r++;
pitiot's avatar
merging    
pitiot committed
529
530
			}

David Cazier's avatar
David Cazier committed
531
		}
pitiot's avatar
merging    
pitiot committed
532
533
534
535
536
537
538
539
540
541

		positions[0]=vPos;

		for (int i = 1 ; i < snakeSize ; i++)
		{

			start=start-ySide-ySide;

			vPos.clear();

David Cazier's avatar
David Cazier committed
542
543
544
545
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
pitiot's avatar
merging    
pitiot committed
546
			positions[i]=vPos;
David Cazier's avatar
David Cazier committed
547
		}
pitiot's avatar
merging    
pitiot committed
548
549
550
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
551

pitiot's avatar
merging    
pitiot committed
552
553
		for (int i = 0 ; i < snakeSize ; i++)
			movingObstacles_.push_back(art->members[i]);
David Cazier's avatar
David Cazier committed
554
	}
Thomas's avatar
Thomas committed
555
556
}

David Cazier's avatar
David Cazier committed
557
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
558
{
David Cazier's avatar
David Cazier committed
559
560
561
562
563
	std::cout << " - Setup City Scenario : " << nbLines << " x " << nbRank << std::endl ;
	int xBorder = 300 ;
	int yBorder = 200 ;
	int xCornerDown = envMap_.geometry.min()[0] + xBorder ;
	int yCornerDown = envMap_.geometry.min()[1] + yBorder ;
Thomas's avatar
Thomas committed
564

David Cazier's avatar
David Cazier committed
565
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
566
	{
David Cazier's avatar
David Cazier committed
567
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
568
		{
569
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
David Cazier's avatar
David Cazier committed
570
			addAgent(posagent, -1.0f * posagent) ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
571
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
572
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
573
574
}

575
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
576
577
{
	/*
David Cazier's avatar
David Cazier committed
578
	 * Add agents, specifying their start position, and store their goal on the
David Cazier's avatar
David Cazier committed
579
580
	 * opposite side of the environment.
	 */
581
582
	TraversorF<PFP::MAP> tF(envMap_.map);
	Dart d = tF.begin() ;
David Cazier's avatar
David Cazier committed
583

584
585
	unsigned int nbx = 2 ;
	unsigned int nby = 2 ;
David Cazier's avatar
David Cazier committed
586
587
588

	unsigned int bMax = nbx * nby > 0 ? nbMaxAgent / (nbx * nby) : nbMaxAgent ;

David Cazier's avatar
David Cazier committed
589
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
590
	{
David Cazier's avatar
David Cazier committed
591
592
593
		bool found = false ;
		VEC3 pos ;
		Dart dCell ;
594
		while (!found && d != tF.end())
David Cazier's avatar
David Cazier committed
595
		{
596
			if (!envMap_.buildingMark.isMarked(d)
597
			    && envMap_.pedWayMark.isMarked(d)
598
			    )
David Cazier's avatar
David Cazier committed
599
			{
600
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
601
602
603
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
604
			}
605
			d = tF.next() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
606
607
		}

David Cazier's avatar
David Cazier committed
608
609
		if (found)
		{
610
			float ecart = 3.0f ;
611
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
612
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
David Cazier's avatar
David Cazier committed
613
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
614
			{
David Cazier's avatar
David Cazier committed
615
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
616
				{
617
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
618
					addAgent(posagent, -1.0f * posagent, dCell) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
619
620
621
622
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
623
624
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
625
}
626

627
628
629
630
631
632
633
634
635
636
637
638
639
640
void Simulator::addMovingObstacles(unsigned int nb)
{
	TraversorF<PFP::MAP> tF(envMap_.map);
	Dart d = tF.begin() ;
	for (unsigned int i = 0 ; i < nb && d != envMap_.map.end() ; ++i)
	{
		bool found = false ;
		Dart dCell ;
		while (!found && d != tF.end())
		{
			if (!envMap_.buildingMark.isMarked(d)
				&& !envMap_.pedWayMark.isMarked(d)
				)
			{
641
642
643
644
645
646
				float area = Algo::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
				if(area>1400)
				{
					dCell = d ;
					found = true ;
				}
647
648
649
650
651
			}
			d = tF.next() ;
		}

		if (found)
652
			addMovingObstacle(dCell,1) ;
653
654
655
656
657
658
659
	}
}

void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
{
	std::vector<VEC3> vPos;
	VEC3 start;
Thomas Jund's avatar
Thomas Jund committed
660
	float maxHeight=10.0f;
661

662
663
	MovingMesh* mm = NULL;

664
665
666
667
668
669
670
671
672
673
674
675
676
677
	switch(obstType)
	{
		case 0 :
		{
			start = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
			VEC3 xSide (2.0f,0.0f,0.0f);
			VEC3 ySide (0.0f,5.0f,0.0f);

			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
		}
			break;
678
679
		case 1 :
		{
Thomas Jund's avatar
Thomas Jund committed
680
			mm = new MovingMesh(envMap_, d, "meshRessources/Limace.ply");
681
			movingMeshes_.push_back(mm);
682
			vPos = mm->computeProjectedPointSet(maxHeight);
683
684
		}
			break;
685
686
687
688
689
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

690
	bool orientAccordingToFace=false;
Thomas Jund's avatar
Thomas Jund committed
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
	if(orientAccordingToFace)
	{
		while(envMap_.pedWayMark.isMarked(envMap_.map.phi2(d)))
		{
			d = envMap_.map.phi1(d);
		}

		VEC3 mid = (envMap_.position[d]+envMap_.position[envMap_.map.phi1(d)])*0.5f;
		VEC3 front = vPos[0];

		VEC3 bary(0);
		for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
			bary += *it;

		bary /= vPos.size();

		float angle = get_angle(mid - bary,front  - bary);

		TraversorV<PFP::MAP> tv(mm->map);
		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
		{
			mm->position[d]+=rotate(mm->position[d], bary, angle);
		}
		vPos = mm->computeProjectedPointSet(maxHeight);
	}

717
718
	std::vector<VEC3> goals;
	goals.push_back(start);
719
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), false);
720
721

	movingObstacles_.push_back(mo);
722

723
	if(mm != NULL)
724
	{
725
		mo->attachMesh(mm);
726
	}
727
728
729
730
731
732
733
734
735
}

void Simulator::addPathToObstacles()
{
	unsigned int dartDistForPath = 50 ;

	for (std::vector<MovingObstacle*>::iterator it = movingObstacles_.begin() ; it != movingObstacles_.end() ; ++it)
	{
		Dart dStart = (*it)->registering_part->d;
736
737
738
739
740
741
742
743
744

		while(envMap_.pedWayMark.isMarked(dStart))
		{
			if(!envMap_.pedWayMark.isMarked(envMap_.map.phi2(dStart)))
				dStart = envMap_.map.phi2(dStart);
			else
				dStart = envMap_.map.phi1(dStart);
		}

745
746
747
748
749
		Dart dStop = dStart;

		for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop)
			|| j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStart)
750
			|| envMap_.pedWayMark.isMarked(dStop)
751
752
753
754
755
756
757
758
759
760
761
			; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
		}

		addPathToObstacle(*it, dStart, dStop);
	}
}

762
763
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
David Cazier's avatar
David Cazier committed
764
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
765
766
																   envMap_.position, dStart,
																   dGoal,
767
																   envMap_.pedWayMark) ;
768
769
770

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
771
772
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;
773
774
775

		mo->goals_.push_back(dest) ;
	}
776
777
778
779
780

	path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
												envMap_.position,
												dGoal,
												dStart,
781
												envMap_.pedWayMark) ;
782
783
784

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
785
786
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;
787
788
789

		mo->goals_.push_back(dest) ;
	}
790

Thomas Jund's avatar
Thomas Jund committed
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
//	if(((mo->front-mo->center)*(mo->goals_[mo->curGoal_]-mo->center))<0)
//	{
//		std::cout << __FILE__ << " " << __LINE__ << " rotate limace" << std::endl;
//
//		Geom::Matrix44f m;
//		m.identity();
//		rotateZ(180.0f,m);
//
//		TraversorV<PFP::MAP> tv(mo->map);
//		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
//		{
//			mo->position[d] -= mo->center;
//			mo->position[d] = Geom::transform(mo->position[d],m);
//			mo->position[d] += mo->center;
//			mo->position[d][2] += 10.0f;
//		}
//	}
808
809
}

Pierre Kraemer's avatar
Pierre Kraemer committed
810
#ifndef SPATIAL_HASHING
811
812
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
813
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
814
	{
David Cazier's avatar
David Cazier committed
815
816
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
817

David Cazier's avatar
David Cazier committed
818
819
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
820

David Cazier's avatar
David Cazier committed
821
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
822
823
824
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
825

David Cazier's avatar
David Cazier committed
826
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
827
		{
828
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
829
830
831
832
833

//			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)];
//			dest /= 2.0f;
//			dest[2]=0;

David Cazier's avatar
David Cazier committed
834
			agents_[i]->goals_.push_back(dest) ;
835
836
837
838
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
839
840
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
841
	//city
David Cazier's avatar
David Cazier committed
842
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
843
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
844
	{
David Cazier's avatar
David Cazier committed
845
846
		agents_[i]->goals_.clear() ;

847
848
		TraversorF<PFP::MAP> tF(envMap_.map);

David Cazier's avatar
David Cazier committed
849
850
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
851
852
853
854
855
856
857
		for (unsigned int j = 0 ;
			/*!envMap_.pedWayMark.isMarked(dStop) || */
			envMap_.buildingMark.isMarked(dStop)
		    || j < dartDistForPath + rand() * 20
		    || envMap_.map.sameFace(dStop, dStart)
		    || envMap_.obstacleMark.isMarked(dStop)
		    ; ++j)
David Cazier's avatar
David Cazier committed
858
		{
David Cazier's avatar
David Cazier committed
859
			envMap_.map.next(dStop) ;
860
861
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
862
		}
863

864
865
//		std::cout << "dest1" << Algo::Geometry::faceCentroid<PFP>(envMap_.map, dStop, envMap_.position) << std::endl;

David Cazier's avatar
David Cazier committed
866
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
867
868
869
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
870

David Cazier's avatar
David Cazier committed
871
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
872
		{
Thomas Jund's avatar
Thomas Jund committed
873
874
875
876
877
878
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

879
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
Thomas Jund's avatar
Thomas Jund committed
880
881

			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
David Cazier's avatar
David Cazier committed
882
883
			dest /= 2.0f ;
			dest[2] = 0 ;
884

David Cazier's avatar
David Cazier committed
885
			agents_[i]->goals_.push_back(dest) ;
886
		}
887

David Cazier's avatar
David Cazier committed
888
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
889
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
890
891
892
893
894
895
		    envMap_.buildingMark.isMarked(dStop2)
		    || j < dartDistForPath + rand() * 20
		    || envMap_.map.sameFace(dStop, dStop2)
		    || envMap_.map.sameFace(dStop2, dStart)
		    || envMap_.obstacleMark.isMarked(dStop)
		    ; ++j)
David Cazier's avatar
David Cazier committed
896
		{
David Cazier's avatar
David Cazier committed
897
			envMap_.map.next(dStop2) ;
898
899
			if (dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
900
		}
901

902
903
//		std::cout << "dest2" << Algo::Geometry::faceCentroid<PFP>(envMap_.map, dStop2, envMap_.position) << std::endl;

David Cazier's avatar
David Cazier committed
904
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_, envMap_.position, dStop, dStop2,
David Cazier's avatar
David Cazier committed
905
		                                             envMap_.buildingMark) ;
906

David Cazier's avatar
David Cazier committed
907
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
908
		{
Thomas Jund's avatar
Thomas Jund committed
909
910
911
912
913
914
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

915
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
Thomas Jund's avatar
Thomas Jund committed
916
			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
David Cazier's avatar
David Cazier committed
917
918
			dest /= 2.0f ;
			dest[2] = 0 ;
919

David Cazier's avatar
David Cazier committed
920
			agents_[i]->goals_.push_back(dest) ;
921
		}
922

David Cazier's avatar
David Cazier committed
923
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_, envMap_.position, dStop2, dStart,
David Cazier's avatar
David Cazier committed
924
		                                             envMap_.buildingMark) ;
925

926
927
//		std::cout << "destStart" << Algo::Geometry::faceCentroid<PFP>(envMap_.map, dStart, envMap_.position) << std::endl;

David Cazier's avatar
David Cazier committed
928
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
929
		{
930
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
931
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
932
			dest /= 2.0f ;
933

David Cazier's avatar
David Cazier committed
934
			dest[2] = 0 ;
935

David Cazier's avatar
David Cazier committed
936
			agents_[i]->goals_.push_back(dest) ;
937
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
938
939
	}
}
940

Thomas's avatar
Thomas committed
941
942
943
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
944
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
945
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
946
	{
David Cazier's avatar
David Cazier committed
947
948
949
950
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
951
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
952
953
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
954
		{
David Cazier's avatar
David Cazier committed
955
956
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
957
958
		}

David Cazier's avatar
David Cazier committed
959
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
960
961
962
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
963

David Cazier's avatar
David Cazier committed
964
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
965
		{
966
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
967
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
968
969
		}

David Cazier's avatar
David Cazier committed
970
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
971
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
972
973
974
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
975
		{
David Cazier's avatar
David Cazier committed
976
977
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
978
979
		}

David Cazier's avatar
David Cazier committed
980
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_, envMap_.position, dStop, dStop2,
David Cazier's avatar
David Cazier committed
981
		                                             envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
982

David Cazier's avatar
David Cazier committed
983
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
984
		{
985
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
986
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
987
988
		}

David Cazier's avatar
David Cazier committed
989
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_, envMap_.position, dStop2, dStart,
David Cazier's avatar
David Cazier committed
990
		                                             envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
991

David Cazier's avatar
David Cazier committed
992
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
993
		{