simulator.cpp 28 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

		//PIERRE
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
265
266
267
268
269
270
271
272
273
274
275
276
277
	}
	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);

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

pitiot's avatar
pitiot committed
280
281
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
282

pitiot's avatar
pitiot committed
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
	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);

299
		mo4= new MovingObstacle(this, i,vPos,goals, true, true);
pitiot's avatar
pitiot committed
300
301

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
302
	}
Thomas's avatar
Thomas committed
303

304
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
305
306
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
307
#endif
pitiot's avatar
merging    
pitiot committed
308
309

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

David Cazier's avatar
David Cazier committed
312
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
313
{
pitiot's avatar
merging    
pitiot committed
314
315
316
317
318
319
320
321
322
323
324
	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
325
326
327
328
329
330
331
332
333
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
	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
362

David Cazier's avatar
David Cazier committed
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
	// 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);
401
		mo4= new MovingObstacle(this, i,vPos,goals, true, false);
David Cazier's avatar
David Cazier committed
402
403


404
405
406
407
408
409
410
411
412
413
414
415
416
417
//		//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
418
419
420
421
422

		movingObstacles_.push_back(mo4);
	}
}

pitiot's avatar
merging    
pitiot committed
423
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
David Cazier's avatar
David Cazier committed
424
{
pitiot's avatar
merging    
pitiot committed
425
426
427
428
429
430
	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
431
432

	// Bordure à éviter autour de la scène (10% de sa taille)
pitiot's avatar
merging    
pitiot committed
433
434
	int xBorder = envMap_.geometry.size(0) / 5 ;
	int yBorder = envMap_.geometry.size(1) / 5 ;
David Cazier's avatar
David Cazier committed
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450

	// 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
451
	{
David Cazier's avatar
David Cazier committed
452
453
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;
454

David Cazier's avatar
David Cazier committed
455
		// Un agent sur 2 va de droite à gauche
pitiot's avatar
merging    
pitiot committed
456
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
457
		if (i % 2 == 1)
pitiot's avatar
merging    
pitiot committed
458
459
460
461
462
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
		}
pitiot's avatar
merge    
pitiot committed
463

pitiot's avatar
merging    
pitiot committed
464
		addAgent(start, goal) ;
pitiot's avatar
merge    
pitiot committed
465

Thomas's avatar
Thomas committed
466
467
	}

David Cazier's avatar
David Cazier committed
468
469
470
471
	// 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
472
	yStartDelta = envMap_.geometry.size(1) / 20 ;
David Cazier's avatar
David Cazier committed
473
474

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

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

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

480
	VEC3 xSide (5.0f,0.0f,0.0f);
481
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
482

pitiot's avatar
merging    
pitiot committed
483
484
485

	int sumObstacles=0;
	for(unsigned int j = 0; j<nbSnakes; j++)
David Cazier's avatar
David Cazier committed
486
	{
pitiot's avatar
merging    
pitiot committed
487
488
489
490
		std::vector<PFP::VEC3> positions [snakeSize] ;
		float x = xStartMin + rand() % xStartDelta;
		VEC3 goal;

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

pitiot's avatar
merging    
pitiot committed
493
494
495
496
497
498
499
500
501
502
503
504
505

		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
506
		{
pitiot's avatar
merging    
pitiot committed
507
508
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
509
510
			if ((goal-goals[r]).norm2()>1000)
			{
pitiot's avatar
merging    
pitiot committed
511
				goals.push_back(goal);
512
				r++;
pitiot's avatar
merging    
pitiot committed
513
514
			}

David Cazier's avatar
David Cazier committed
515
		}
pitiot's avatar
merging    
pitiot committed
516
517
518
519
520
521
522
523
524
525

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

David Cazier's avatar
David Cazier committed
526
527
528
529
			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
530
			positions[i]=vPos;
David Cazier's avatar
David Cazier committed
531
		}
pitiot's avatar
merging    
pitiot committed
532
533
534
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
535

pitiot's avatar
merging    
pitiot committed
536
537
		for (int i = 0 ; i < snakeSize ; i++)
			movingObstacles_.push_back(art->members[i]);
David Cazier's avatar
David Cazier committed
538
	}
Thomas's avatar
Thomas committed
539
540
}

David Cazier's avatar
David Cazier committed
541
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
542
{
David Cazier's avatar
David Cazier committed
543
544
545
546
547
	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
548

David Cazier's avatar
David Cazier committed
549
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
550
	{
David Cazier's avatar
David Cazier committed
551
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
552
		{
553
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
David Cazier's avatar
David Cazier committed
554
			addAgent(posagent, -1.0f * posagent) ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
555
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
556
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
557
558
}

559
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
560
561
{
	/*
David Cazier's avatar
David Cazier committed
562
	 * Add agents, specifying their start position, and store their goal on the
David Cazier's avatar
David Cazier committed
563
564
	 * opposite side of the environment.
	 */
565
566
	TraversorF<PFP::MAP> tF(envMap_.map);
	Dart d = tF.begin() ;
David Cazier's avatar
David Cazier committed
567

568
569
	unsigned int nbx = 2 ;
	unsigned int nby = 2 ;
David Cazier's avatar
David Cazier committed
570
571
572

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

David Cazier's avatar
David Cazier committed
573
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
574
	{
David Cazier's avatar
David Cazier committed
575
576
577
		bool found = false ;
		VEC3 pos ;
		Dart dCell ;
578
		while (!found && d != tF.end())
David Cazier's avatar
David Cazier committed
579
		{
580
			if (!envMap_.buildingMark.isMarked(d)
581
			    && envMap_.pedWayMark.isMarked(d)
582
			    )
David Cazier's avatar
David Cazier committed
583
			{
584
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
585
586
587
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
588
			}
589
			d = tF.next() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
590
591
		}

David Cazier's avatar
David Cazier committed
592
593
		if (found)
		{
594
			float ecart = 3.0f ;
595
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
596
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
David Cazier's avatar
David Cazier committed
597
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
598
			{
David Cazier's avatar
David Cazier committed
599
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
600
				{
601
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
602
					addAgent(posagent, -1.0f * posagent, dCell) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
603
604
605
606
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
607
608
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
609
}
610

611
612
613
614
615
616
617
618
619
620
621
622
623
624
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)
				)
			{
625
626
627
628
629
630
				float area = Algo::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
				if(area>1400)
				{
					dCell = d ;
					found = true ;
				}
631
632
633
634
635
			}
			d = tF.next() ;
		}

		if (found)
636
			addMovingObstacle(dCell,1) ;
637
638
639
640
641
642
643
	}
}

void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
{
	std::vector<VEC3> vPos;
	VEC3 start;
Thomas Jund's avatar
Thomas Jund committed
644
645
	MovingMesh * mm;
	float maxHeight=10.0f;
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660

	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;
661
662
		case 1 :
		{
Thomas Jund's avatar
Thomas Jund committed
663
			mm = new MovingMesh(envMap_, d, "meshRessources/Limace.ply");
664
			movingMeshes_.push_back(mm);
665
			vPos = mm->computeProjectedPointSet(maxHeight);
666
667
		}
			break;
668
669
670
671
672
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

Thomas Jund's avatar
Thomas Jund committed
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
	bool orientAccordingToFace=true;
	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);
	}

700
701
	std::vector<VEC3> goals;
	goals.push_back(start);
702
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), false);
703
704

	movingObstacles_.push_back(mo);
705
706
707
708
709

	if(obstType==1)
	{
		//PIERRE : calcul coord bary d som du movMesh en fction de la cage movingObstacle
	}
710
711
712
713
714
715
716
717
718
}

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;
719
720
721
722
723
724
725
726
727

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

728
729
730
731
732
		Dart dStop = dStart;

		for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop)
			|| j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStart)
733
			|| envMap_.pedWayMark.isMarked(dStop)
734
735
736
737
738
739
740
741
742
743
744
			; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
		}

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

745
746
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
David Cazier's avatar
David Cazier committed
747
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
748
749
																   envMap_.position, dStart,
																   dGoal,
750
																   envMap_.pedWayMark) ;
751
752
753

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
754
755
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;
756
757
758

		mo->goals_.push_back(dest) ;
	}
759
760
761
762
763

	path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
												envMap_.position,
												dGoal,
												dStart,
764
												envMap_.pedWayMark) ;
765
766
767

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

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

Thomas Jund's avatar
Thomas Jund committed
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
//	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;
//		}
//	}
791
792
}

Pierre Kraemer's avatar
Pierre Kraemer committed
793
#ifndef SPATIAL_HASHING
794
795
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
796
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
797
	{
David Cazier's avatar
David Cazier committed
798
799
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
800

David Cazier's avatar
David Cazier committed
801
802
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
803

David Cazier's avatar
David Cazier committed
804
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
805
806
807
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
808

David Cazier's avatar
David Cazier committed
809
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
810
		{
811
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
812
813
814
815
816

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

David Cazier's avatar
David Cazier committed
817
			agents_[i]->goals_.push_back(dest) ;
818
819
820
821
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
822
823
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
824
	//city
David Cazier's avatar
David Cazier committed
825
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
826
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
827
	{
David Cazier's avatar
David Cazier committed
828
829
		agents_[i]->goals_.clear() ;

830
831
		TraversorF<PFP::MAP> tF(envMap_.map);

David Cazier's avatar
David Cazier committed
832
833
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
834
835
836
837
838
839
840
		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
841
		{
David Cazier's avatar
David Cazier committed
842
			envMap_.map.next(dStop) ;
843
844
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
845
		}
846

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

David Cazier's avatar
David Cazier committed
849
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
850
851
852
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
853

David Cazier's avatar
David Cazier committed
854
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
855
		{
Thomas Jund's avatar
Thomas Jund committed
856
857
858
859
860
861
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

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

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

David Cazier's avatar
David Cazier committed
868
			agents_[i]->goals_.push_back(dest) ;
869
		}
870

David Cazier's avatar
David Cazier committed
871
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
872
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
873
874
875
876
877
878
		    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
879
		{
David Cazier's avatar
David Cazier committed
880
			envMap_.map.next(dStop2) ;
881
882
			if (dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
883
		}
884

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

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

David Cazier's avatar
David Cazier committed
890
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
891
		{
Thomas Jund's avatar
Thomas Jund committed
892
893
894
895
896
897
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

898
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
Thomas Jund's avatar
Thomas Jund committed
899
			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
David Cazier's avatar
David Cazier committed
900
901
			dest /= 2.0f ;
			dest[2] = 0 ;
902

David Cazier's avatar
David Cazier committed
903
			agents_[i]->goals_.push_back(dest) ;
904
		}
905

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

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

David Cazier's avatar
David Cazier committed
911
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
912
		{
913
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
914
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
915
			dest /= 2.0f ;
916

David Cazier's avatar
David Cazier committed
917
			dest[2] = 0 ;
918

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

Thomas's avatar
Thomas committed
924
925
926
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
927
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
928
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
929
	{
David Cazier's avatar
David Cazier committed
930
931
932
933
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
934
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
935
936
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
937
		{
David Cazier's avatar
David Cazier committed
938
939
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
940
941
		}

David Cazier's avatar
David Cazier committed
942
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
943
944
945
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
946

David Cazier's avatar
David Cazier committed
947
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
948
		{
949
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
950
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
951
952
		}

David Cazier's avatar
David Cazier committed
953
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
954
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
955
956
957
		    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
958
		{
David Cazier's avatar
David Cazier committed
959
960
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
961
962
		}

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

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

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

David Cazier's avatar
David Cazier committed
975
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
976
		{
977
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
978
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
979
980
981
		}
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
982
#endif
Thomas's avatar
Thomas committed
983

Pierre Kraemer's avatar
Pierre Kraemer committed
984
985
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
986
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
987

David Cazier's avatar
David Cazier committed
988
989
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
990
991
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
992
993
	}

David Cazier's avatar
David Cazier committed
994
995
	std::vector<VEC3> goals ;

David Cazier's avatar
David Cazier committed
996
997
998
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
999
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1000

David Cazier's avatar
David Cazier committed
1001
1002
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
1003
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
1004
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1005

David Cazier's avatar
David Cazier committed
1006
1007
1008
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1009

David Cazier's avatar
David Cazier committed
1010
1011
			VEC3 posagent(x, y, z) ;
			addAgent(posagent, -1.0f * posagent) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1012
1013
1014
		}
	}

David Cazier's avatar
David Cazier committed
1015
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1016

David Cazier's avatar
David Cazier committed
1017
1018
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1019
1020
1021
1022
}

bool Simulator::exportAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
1023
	std::ofstream out(filename.c_str(), std::ios::out) ;
David Cazier's avatar
David Cazier committed
1024
1025
	if (!out.good())
	{
David Cazier's avatar
David Cazier committed
1026
1027
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1028
1029
	}

David Cazier's avatar
David Cazier committed
1030
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
1031
		out << agents_[i]->getPosition() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1032

David Cazier's avatar
David Cazier committed
1033
1034
	out.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1035
1036
1037
1038
}

void Simulator::swapAgentsGoals()
{