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 26.3 KB
Newer Older
Pierre Kraemer's avatar
Pierre Kraemer committed
1
2
#include "simulator.h"

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

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

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

pitiot's avatar
pitiot committed
36
37
38
39
40
#ifndef SPATIAL_HASHING
//	envMap_.subdivideToProperLevel() ;
//	envMap_.subdivideAllToMaxLevel();
#endif

David Cazier's avatar
David Cazier committed
41
42
	switch (config)
	{
pitiot's avatar
maj    
pitiot committed
43
		case 0 :
pitiot's avatar
maj    
pitiot committed
44
			setupCircleScenario(5,40) ;
pitiot's avatar
maj    
pitiot committed
45
46
			break ;
		case 1 :
pitiot's avatar
pitiot committed
47
			setupCorridorScenario(1000,40) ;
pitiot's avatar
maj    
pitiot committed
48
			break ;
49
50
51
		case 2 :
			setupSnakeCorridorScenario(1000,5,10) ;
			break ;
pitiot's avatar
pitiot committed
52
53
54
55
56
57
58
59
//		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 ;
Thomas's avatar
Thomas committed
60
61
	}

pitiot's avatar
maj    
pitiot committed
62
63
64
65
66
#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
67
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
68
69
70
71
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
			addPathsToAgents() ;
72
		else if (dimension == 2.5f) addPathsToAgents_height() ;
Thomas's avatar
Thomas committed
73
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
74
#endif
75

pitiot's avatar
maj    
pitiot committed
76
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
77
	{
David Cazier's avatar
David Cazier committed
78
79
		agents_[i]->updateObstacleNeighbors() ;
		agents_[i]->updateAgentNeighbors() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
80
	}
81

pitiot's avatar
pitiot committed
82

Pierre Kraemer's avatar
Pierre Kraemer committed
83
#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
84
85
	if (multires)
		envMap_.subdivideToProperLevel() ;
pitiot's avatar
maj    
pitiot committed
86
//	envMap_.subdivideAllToMaxLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
87
#endif
pitiot's avatar
maj    
pitiot committed
88
89

//	setupMovingObstacle(40);
pitiot's avatar
pitiot committed
90

Pierre Kraemer's avatar
Pierre Kraemer committed
91
92
}

pitiot's avatar
maj    
pitiot committed
93
void Simulator::doStep()
Pierre Kraemer's avatar
Pierre Kraemer committed
94
{
pitiot's avatar
maj    
pitiot committed
95
96
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
pitiot's avatar
pitiot committed
97
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
98
99
100
101
102
#endif

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

pitiot's avatar
maj    
pitiot committed
110
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
111
	{
pitiot's avatar
pitiot committed
112
113
114
		if (agents_[i]->alive)
		{
			agents_[i]->updateObstacleNeighbors() ;
pitiot's avatar
maj    
pitiot committed
115
116

#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
117
			envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
118
#endif
pitiot's avatar
pitiot committed
119
			agents_[i]->updateAgentNeighbors() ;
pitiot's avatar
maj    
pitiot committed
120
#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
121
			envMap_.map.setCurrentLevel(0) ;
pitiot's avatar
maj    
pitiot committed
122
123
#endif

pitiot's avatar
pitiot committed
124
125
126
			agents_[i]->computePrefVelocity() ;
			agents_[i]->computeNewVelocity() ;
		}
pitiot's avatar
maj    
pitiot committed
127
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
128

Pierre Kraemer's avatar
Pierre Kraemer committed
129
#ifdef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
130
131
132
133
134
135
136
137
138
139
140
141
	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) ;
		}
	}
#else
142
	if (multires) envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
143
144
145
146
	nb_dead = 0 ;
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		if (agents_[i]->alive)
Pierre Kraemer's avatar
Pierre Kraemer committed
147
		{
pitiot's avatar
maj    
pitiot committed
148
149
150
151
152
153
			Dart oldFace = agents_[i]->part_.d ;
			agents_[i]->update() ;

//		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)
Pierre Kraemer's avatar
Pierre Kraemer committed
154
			{
pitiot's avatar
maj    
pitiot committed
155
156
157
158
159
160
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace) ;
//				break;
Pierre Kraemer's avatar
Pierre Kraemer committed
161
162
			}
		}
pitiot's avatar
maj    
pitiot committed
163
164
165
166
167
168
169
		else
			nb_dead++ ;

	}

	nbRefineCandidate += envMap_.refineCandidate.size() ;
	nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
pitiot's avatar
pitiot committed
170
171
172

	if (multires)
		envMap_.updateMap() ;
pitiot's avatar
maj    
pitiot committed
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
#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 ;
}

189
void Simulator::addAgent(const VEC3& start, const std::vector<VEC3>& goal)
pitiot's avatar
maj    
pitiot committed
190
191
192
193
194
{
#ifdef SPATIAL_HASHING
	Agent* a = new Agent(this, start) ;
	agents_.push_back(a) ;
	envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
195
#else
pitiot's avatar
pitiot committed
196

pitiot's avatar
maj    
pitiot committed
197
	Dart dCell = envMap_.getBelongingCell(start) ;
pitiot's avatar
pitiot committed
198
//	if (dCell==envMap_.map.begin()) CGoGNout<<"pbagents"<<CGoGNendl;
pitiot's avatar
maj    
pitiot committed
199
	agents_.push_back(new Agent(this, start, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
200
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
201

202
	agents_.back()->goals_=goal ;
pitiot's avatar
maj    
pitiot committed
203
204
205
	agents_.back()->curGoal_ = 0 ;
}

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

pitiot's avatar
maj    
pitiot committed
215
		envMap_.init(config, 1600.0f, 960.0f, minSize, 400.0f) ; //cases fines
pitiot's avatar
pitiot committed
216
217
	}

pitiot's avatar
maj    
pitiot committed
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
	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 ;

	double pi = 3.14159265358979323846f ;

	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 ;
240
241
242
243
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
244
	}
pitiot's avatar
pitiot committed
245
246
247
248
249
	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
250
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
pitiot's avatar
maj    
pitiot committed
251
	{
pitiot's avatar
pitiot committed
252
253
254
255
256
257
258
259
260
261
262
263
		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
		{
			v=VEC3 (std::cos(angle) * (radius/3), std::sin(angle) * (radius/3), 0) ;
		}
		start = center + v ;
		positions.push_back(start);
pitiot's avatar
pitiot committed
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
	}
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
		std::vector<VEC3> goals;
		for(unsigned int k = 0 ; k < nbObstacles/2 ; k++)
		{
			goals.push_back(positions[(i+k)%(nbObstacles/2)]);
		}

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

pitiot's avatar
pitiot committed
281
		mo4= new MovingObstacle(this, i,vPos,goals,true);
pitiot's avatar
pitiot committed
282

pitiot's avatar
pitiot committed
283
284
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
285

pitiot's avatar
pitiot committed
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
	for (unsigned int i = nbObstacles/2 ; i < nbObstacles ; i++)
	{
		VEC3 start = positions[i];
		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]);
		}

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

		mo4= new MovingObstacle(this, i,vPos,goals,true);
pitiot's avatar
pitiot committed
303
304

		movingObstacles_.push_back(mo4);
pitiot's avatar
maj    
pitiot committed
305
	}
pitiot's avatar
pitiot committed
306

307
#ifndef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
308
309
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
310
#endif
pitiot's avatar
maj    
pitiot committed
311
312

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

pitiot's avatar
maj    
pitiot committed
315
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
316
{
pitiot's avatar
pitiot committed
317
318
319
320
321
322
323
	if (multires)
	{
		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
	}
	else
	{

pitiot's avatar
maj    
pitiot committed
324
		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //cases fines
pitiot's avatar
pitiot committed
325
326
327
	}


pitiot's avatar
maj    
pitiot committed
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
	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)
David Cazier's avatar
David Cazier committed
349
	{
pitiot's avatar
maj    
pitiot committed
350
351
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;
352

pitiot's avatar
maj    
pitiot committed
353
354
		// Un agent sur 2 va de droite à gauche
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
355
356
		if (i % 2 == 1)
		{
pitiot's avatar
maj    
pitiot committed
357
358
359
			tmp = goal ;
			goal = start ;
			start = tmp ;
Pierre Kraemer's avatar
Pierre Kraemer committed
360
		}
361
362
363
364
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
Thomas's avatar
Thomas committed
365
366
	}

pitiot's avatar
maj    
pitiot committed
367
368
369
370
371
	// 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 ;
Thomas's avatar
Thomas committed
372

pitiot's avatar
maj    
pitiot committed
373
374
375
376
377
378
379
380
381
382
383
384
	// 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;
385
//		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
pitiot's avatar
maj    
pitiot committed
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
		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);
		}
pitiot's avatar
pitiot committed
408
409
410
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
pitiot's avatar
pitiot committed
411
		mo4= new MovingObstacle(this, i,vPos,goals,false);
pitiot's avatar
pitiot committed
412
413
414
415
416


		//for generating a random path
//		unsigned int dartDistForPath = 50 ;
//		mo4->goals_.clear() ;
pitiot's avatar
pitiot committed
417
//		Dart dStart = mo4->registering_part->d;
pitiot's avatar
pitiot committed
418
419
420
421
422
423
424
425
426
427
428
//		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);

pitiot's avatar
maj    
pitiot committed
429
430
		movingObstacles_.push_back(mo4);
	}
Thomas's avatar
Thomas committed
431
432
}

433
434
435
436
437
438
439
440
441
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
{
	if (multires)
	{
		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
	}
	else
	{

pitiot's avatar
maj    
pitiot committed
442
		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //cases fines
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
	}


	std::cout << " - Setup Snake Corridor Scenario : " << nbAgents << " agents et " << nbSnakes*snakeSize << " obstacles" << std::endl ;

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

	// 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
		VEC3 tmp ;
		if (i % 2 == 1)
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
		}
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
	}

	// 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) / 20 ;

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


	int sumObstacles=0;
	for(unsigned int j = 0; j<nbSnakes; j++)
	{
		std::vector<PFP::VEC3> positions [snakeSize] ;
		float x = xStartMin + rand() % xStartDelta;
		VEC3 goal;

		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;


		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)
		{
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
			if ((goal-goals[r]).norm2()>1000){
				goals.push_back(goal);
							r++;
			}

		}

		positions[0]=vPos;

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

		{

			start=start-ySide-ySide;

			vPos.clear();


			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
			positions[i]=vPos;
		}
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
		for (int i = 0 ; i < snakeSize ; i++)
		{
			movingObstacles_.push_back(art->members[i]);
		}
	}
}

pitiot's avatar
maj    
pitiot committed
558
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
559
{
pitiot's avatar
maj    
pitiot committed
560
561
562
563
564
	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
565

pitiot's avatar
maj    
pitiot committed
566
	VEC3 posagent(xCornerDown - xBorder / 2, yCornerDown - yBorder / 2, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
567
568
569
#ifdef SPATIAL_HASHING
	Agent* a = new Agent(this, posagent) ;
	agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
570
	envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
571
#else
572
573
	Dart dCell = envMap_.getBelongingCell(posagent) ;
	agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
574
#endif
David Cazier's avatar
David Cazier committed
575
	agents_.back()->goals_.push_back(-1.0f * posagent) ;
576
	agents_.back()->curGoal_ = 0 ;
577

pitiot's avatar
maj    
pitiot committed
578
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
579
	{
pitiot's avatar
maj    
pitiot committed
580
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
581
		{
582
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
583
584
585
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, posagent) ;
			agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
586
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
587
#else
588
589
			Dart dCell = envMap_.getBelongingCell(posagent) ;
			agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
590
#endif
David Cazier's avatar
David Cazier committed
591
			agents_.back()->goals_.push_back(-1.0f * posagent) ;
592
			agents_.back()->curGoal_ = 0 ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
593
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
594
	}
Thomas's avatar
Thomas committed
595

David Cazier's avatar
David Cazier committed
596
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
597
598
}

599
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
600
{
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
//	Geom::BoundingBox<PFP::VEC3> bb(envMap_.position.begin());
//	for(unsigned int v = envMap_.position.begin() ; v != envMap_.position.end() ; envMap_.position.next(v))
//		bb.addPoint(envMap_.position[v]);
//	VEC3 posinit = bb.center();
//	posinit[1] = bb.min()[1] + 35.0f;

//	float ecart = bb.size(0)/nbMaxAgent;
//	posinit[0] -= ecart * (nbMaxAgent/2 - 0.5f);
//	for(unsigned int i = 0; i < nbMaxAgent; ++i)
//	{
//		VEC3 posagent = posinit + VEC3(ecart * i, 0.0f, 0.0f);
//		agents_.push_back(new Agent(this, posagent, envMap_.getBelongingCell(posagent)));
//		agents_.back()->goals_.push_back(posagent);
//		VEC3 goal = posagent;
//		goal *= -1.0f;
//		agents_.back()->goals_.push_back(goal);
//		agents_.back()->curGoal_ = 1;
//		agents_.back()->finalGoal = goal;
//		agents_.back()->finalDart = envMap_.getBelongingCell(goal);
//	}

Pierre Kraemer's avatar
Pierre Kraemer committed
622
	/*
David Cazier's avatar
David Cazier committed
623
624
625
626
	 * Add agents, specifying their start position, and store their goals on the
	 * opposite side of the environment.
	 */
	Dart d = envMap_.map.begin() ;
pitiot's avatar
maj    
pitiot committed
627
	CellMarker<FACE> filled(envMap_.map) ;
David Cazier's avatar
David Cazier committed
628
629
630
631
632
633

	unsigned int nbx = 5 ;
	unsigned int nby = 5 ;

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

pitiot's avatar
maj    
pitiot committed
634
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
635
	{
David Cazier's avatar
David Cazier committed
636
637
638
		bool found = false ;
		VEC3 pos ;
		Dart dCell ;
David Cazier's avatar
David Cazier committed
639
640
641
642
643
		while (!found && d != envMap_.map.end())
		{
			if (!filled.isMarked(d)
			    && !envMap_.buildingMark.isMarked(d) /*&& envMap_.pedWayMark.isMarked(d)*/)
			{
David Cazier's avatar
David Cazier committed
644
				filled.mark(d) ;
645
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
646
647
648
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
649
			}
David Cazier's avatar
David Cazier committed
650
			envMap_.map.next(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
651
652
		}

David Cazier's avatar
David Cazier committed
653
654
		if (found)
		{
655
656
			float ecart = 3.0f ;
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
657
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
pitiot's avatar
maj    
pitiot committed
658
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
659
			{
pitiot's avatar
maj    
pitiot committed
660
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
661
				{
662
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
663
664
665
#ifdef SPATIAL_HASHING
					Agent* a = new Agent(this, posagent) ;
					agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
666
					envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
667
#else
668
					agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
669
#endif
670
					agents_.back()->goals_.push_back(posagent) ;
David Cazier's avatar
David Cazier committed
671
					agents_.back()->goals_.push_back(-1.0f * posagent) ;
pitiot's avatar
maj    
pitiot committed
672

673
					agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
674
675
676
677
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
678
679
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
680
}
681

pitiot's avatar
pitiot committed
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
																   envMap_.position, dStart,
																   dGoal,
																   envMap_.buildingMark) ;

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
		VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;

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

Pierre Kraemer's avatar
Pierre Kraemer committed
697
#ifndef SPATIAL_HASHING
698
699
void Simulator::addPathToCorner()
{
pitiot's avatar
maj    
pitiot committed
700
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
701
	{
David Cazier's avatar
David Cazier committed
702
703
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
704

David Cazier's avatar
David Cazier committed
705
706
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
707

708
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
709
710
711
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
712

pitiot's avatar
maj    
pitiot committed
713
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
714
		{
715
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
716
717
718
719
720

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

David Cazier's avatar
David Cazier committed
721
			agents_[i]->goals_.push_back(dest) ;
722
723
724
725
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
726
727
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
728
	//city
David Cazier's avatar
David Cazier committed
729
	unsigned int dartDistForPath = 50 ;
pitiot's avatar
maj    
pitiot committed
730
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
731
	{
David Cazier's avatar
David Cazier committed
732
733
734
735
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
pitiot's avatar
maj    
pitiot committed
736
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
737
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
738
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
739
		{
David Cazier's avatar
David Cazier committed
740
741
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
742
		}
743

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

746
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
747
748
749
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
750

pitiot's avatar
maj    
pitiot committed
751
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
752
		{
753
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
754
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
755
756
			dest /= 2.0f ;
			dest[2] = 0 ;
757

David Cazier's avatar
David Cazier committed
758
			agents_[i]->goals_.push_back(dest) ;
759
		}
760

David Cazier's avatar
David Cazier committed
761
		Dart dStop2 = dStop ;
pitiot's avatar
maj    
pitiot committed
762
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
763
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
764
765
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
766
		{
David Cazier's avatar
David Cazier committed
767
768
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
769
		}
770

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

David Cazier's avatar
David Cazier committed
773
774
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop, dStop2,
		                                             envMap_.buildingMark) ;
775

pitiot's avatar
maj    
pitiot committed
776
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
777
		{
778
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
779
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
780
781
			dest /= 2.0f ;
			dest[2] = 0 ;
782

David Cazier's avatar
David Cazier committed
783
			agents_[i]->goals_.push_back(dest) ;
784
		}
785

David Cazier's avatar
David Cazier committed
786
787
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop2, dStart,
		                                             envMap_.buildingMark) ;
788

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

pitiot's avatar
maj    
pitiot committed
791
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
792
		{
793
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
794
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
795
796
			dest /= 2.0f ;
			dest[2] = 0 ;
797

David Cazier's avatar
David Cazier committed
798
			agents_[i]->goals_.push_back(dest) ;
799
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
800
801
	}
}
802

Thomas's avatar
Thomas committed
803
804
805
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
806
	unsigned int dartDistForPath = 50 ;
pitiot's avatar
maj    
pitiot committed
807
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
808
	{
David Cazier's avatar
David Cazier committed
809
810
811
812
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
pitiot's avatar
maj    
pitiot committed
813
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
814
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
815
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
816
		{
David Cazier's avatar
David Cazier committed
817
818
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
819
820
		}

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

pitiot's avatar
maj    
pitiot 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) ;
David Cazier's avatar
David Cazier committed
829
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
830
831
		}

David Cazier's avatar
David Cazier committed
832
		Dart dStop2 = dStop ;
pitiot's avatar
maj    
pitiot committed
833
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
834
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
835
836
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
837
		{
David Cazier's avatar
David Cazier committed
838
839
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
840
841
		}

David Cazier's avatar
David Cazier committed
842
843
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop, dStop2,
		                                             envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
844

pitiot's avatar
maj    
pitiot committed
845
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
846
		{
847
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
848
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
849
850
		}

David Cazier's avatar
David Cazier committed
851
852
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop2, dStart,
		                                             envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
853

pitiot's avatar
maj    
pitiot committed
854
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
855
		{
856
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
857
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
858
859
860
		}
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
861
#endif
Thomas's avatar
Thomas committed
862

Pierre Kraemer's avatar
Pierre Kraemer committed
863
864
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
865
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
866

David Cazier's avatar
David Cazier committed
867
868
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
869
870
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
871
872
	}

David Cazier's avatar
David Cazier committed
873
874
875
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
876
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
877

David Cazier's avatar
David Cazier committed
878
879
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
880
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
881
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
882

David Cazier's avatar
David Cazier committed
883
884
885
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
886

887
			VEC3 pos(x, y, z) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
888
889
890
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, pos) ;
			agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
891
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
892
#else
893
894
			Dart dCell = envMap_.getBelongingCell(pos) ;
			agents_.push_back(new Agent(this, pos, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
895
#endif
896
			agents_.back()->goals_.push_back(pos) ;
David Cazier's avatar
David Cazier committed
897
			agents_.back()->goals_.push_back(-1.0f * pos) ;
898
			agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
899
900
901
		}
	}

David Cazier's avatar
David Cazier committed
902
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
903

David Cazier's avatar
David Cazier committed
904
905
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
906
907
908
909
}

bool Simulator::exportAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
910
	std::ofstream out(filename.c_str(), std::ios::out) ;
David Cazier's avatar
David Cazier committed
911
912
	if (!out.good())
	{
David Cazier's avatar
David Cazier committed
913
914
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
915
916
	}

pitiot's avatar
maj    
pitiot committed
917
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
918
#ifdef SPATIAL_HASHING
919
		out << agents_[i]->pos << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
920
#else
pitiot's avatar
maj    
pitiot committed
921
		out << agents_[i]->part_.getPosition() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
922
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
923

David Cazier's avatar
David Cazier committed
924
925
	out.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
926
927
928
929
}

void Simulator::swapAgentsGoals()
{
David Cazier's avatar
David Cazier committed
930
	unsigned int nbAgents = agents_.size() ;
pitiot's avatar
maj    
pitiot committed
931
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
932
	{
David Cazier's avatar
David Cazier committed
933
		unsigned int r = rand() % nbAgents ;
Thomas's avatar
Thomas committed
934
935
936
937
938
939
940
//		for(unsigned int nbg = 0; nbg < agents_[i]->goals_.size(); ++nbg)
//		{
//			VEC3 g = agents_[i]->goals_[nbg];
//			agents_[i]->goals_[nbg] = agents_[r]->goals_[nbg];
//			agents_[r]->goals_[nbg] = g;
//		}

941
		std::swap(agents_[i]->goals_, agents_[r]->goals_) ;
Thomas's avatar
Thomas committed
942
943
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
944

Thomas's avatar
Thomas committed
945
946
Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
{
David Cazier's avatar
David Cazier committed
947
948
	Geom::BoundingBox<VEC3> bb ;
	unsigned int nbAgents = agents_.size() ;
pitiot's avatar
maj    
pitiot committed
949
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
950
	{
David Cazier's avatar
David Cazier committed
951
		bb.addPoint(agents_[i]->getPosition()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
952
	}
Thomas's avatar
Thomas committed
953

David Cazier's avatar
David Cazier committed
954
	return bb ;
Pierre Kraemer's avatar
Pierre Kraemer committed
955
}