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

pitiot's avatar
pitiot committed
3
Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgent, unsigned int nbObst) :
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),
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
	multires=true;
pitiot's avatar
pitiot committed
18
	detect_agent_collision=false;
19
20
	srand(10) ;
	nbStepsPerUnit_ = 1 / timeStep_ ;
pitiot's avatar
merge    
pitiot committed
21

pitiot's avatar
merging    
pitiot committed
22
	this->config=config;
pitiot's avatar
pitiot committed
23
	init(2.0f, nbAgent, nbObst) ;
Thomas's avatar
Thomas committed
24
25
26
27
}

Simulator::~Simulator()
{
David Cazier's avatar
David Cazier 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, unsigned int nbAgent, unsigned int nbObst, bool enablePathFinding)
Thomas's avatar
Thomas committed
33
{
34
	std::cout << "Setup scenario" << std::endl ;
Thomas's avatar
Thomas committed
35

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

David Cazier's avatar
David Cazier committed
69
70
71
72
73
#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
74
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
75
76
77
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
David Cazier's avatar
David Cazier committed
78
			addPathsToAgents() ;
79
80
		else if (dimension == 2.5f)
			addPathsToAgents_height() ;
Thomas's avatar
Thomas committed
81
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
82
#endif
83

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

90

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

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

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

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

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

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
136
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
137
138
139
140
141
142
143
144
145
146
147
	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
148
#else
149
150
	if (multires)
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
151
152
153
154
155
156
157
	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
158

David Cazier's avatar
David Cazier committed
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
//		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
178

pitiot's avatar
pitiot committed
179
180
	 if (multires)
		 envMap_.updateMap() ;
David Cazier's avatar
David Cazier committed
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
#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
197
void Simulator::addAgent(const VEC3& start, const VEC3& goal)
David Cazier's avatar
David Cazier committed
198
{
David Cazier's avatar
David Cazier committed
199
	agents_.push_back(new Agent(this, start, goal)) ;
David Cazier's avatar
David Cazier committed
200
201
}

202
203
204
205
206
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
207
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
David Cazier's avatar
David Cazier committed
208
{
pitiot's avatar
merging    
pitiot committed
209
210
211
212
213
214
215
216
217
218
	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
219
220
221
222
223
224
225
226
227
228
229
230
231
232
	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
233
	double pi = 3.14159265358979323846f ;
David Cazier's avatar
David Cazier committed
234
235
236
237
238
239
240

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

		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
286
287
		curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+1)%nbGoals;
		mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);
pitiot's avatar
pitiot committed
288
289


pitiot's avatar
pitiot committed
290
291
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
292

pitiot's avatar
pitiot committed
293
294
295
	for (unsigned int i = nbObstacles/2 ; i < nbObstacles ; i++)
	{
		VEC3 start = positions[i];
pitiot's avatar
pitiot committed
296
297
298
299
300
301
302
//		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
303
		std::vector<VEC3> goals;
pitiot's avatar
pitiot committed
304
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
305
		{
pitiot's avatar
pitiot committed
306
			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
307
308
309
310
311
312
313
314
		}

		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
315
316
		curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+1)%nbGoals;
		mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);
pitiot's avatar
pitiot committed
317

pitiot's avatar
pitiot committed
318
319

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
320
	}
Thomas's avatar
Thomas committed
321

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

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

David Cazier's avatar
David Cazier committed
330
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
331
{
pitiot's avatar
merging    
pitiot committed
332
333
334
335
336
337
338
339
340
341
342
	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
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
	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
380

David Cazier's avatar
David Cazier committed
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
417
418
	// 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);
pitiot's avatar
pitiot committed
419
		mo4= new MovingObstacle(this, i,vPos,goals, true, false,1);
David Cazier's avatar
David Cazier committed
420
421


422
423
424
425
426
427
428
429
430
431
432
433
434
435
//		//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
436
437
438
439
440

		movingObstacles_.push_back(mo4);
	}
}

pitiot's avatar
merging    
pitiot committed
441
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
David Cazier's avatar
David Cazier committed
442
{
pitiot's avatar
merging    
pitiot committed
443
444
445
446
447
448
	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
449
450

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

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

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

pitiot's avatar
merging    
pitiot committed
482
		addAgent(start, goal) ;
pitiot's avatar
merge    
pitiot committed
483

Thomas's avatar
Thomas committed
484
485
	}

David Cazier's avatar
David Cazier committed
486
487
488
489
	// 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
490
	yStartDelta = envMap_.geometry.size(1) / 20 ;
David Cazier's avatar
David Cazier committed
491
492

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

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

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

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

pitiot's avatar
merging    
pitiot committed
501
502
503

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

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

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

		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
524
		{
pitiot's avatar
merging    
pitiot committed
525
526
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
pitiot's avatar
pitiot committed
527
528
			if ((goal-goals[r]).norm2()>1000)
			{
pitiot's avatar
merging    
pitiot committed
529
				goals.push_back(goal);
pitiot's avatar
pitiot committed
530
				r++;
pitiot's avatar
merging    
pitiot committed
531
532
			}

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

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

David Cazier's avatar
David Cazier committed
544
545
546
547
			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
548
			positions[i]=vPos;
David Cazier's avatar
David Cazier committed
549
		}
pitiot's avatar
merging    
pitiot committed
550
551
552
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
pitiot's avatar
pitiot committed
553

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

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

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

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

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

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

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

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

629
630
631
632
633
634
635
636
637
638
639
640
641
642
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)
				)
			{
pitiot's avatar
pitiot committed
643
644
645
646
647
648
				float area = Algo::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
				if(area>1400)
				{
					dCell = d ;
					found = true ;
				}
649
650
651
652
653
			}
			d = tF.next() ;
		}

		if (found)
pitiot's avatar
pitiot committed
654
			addMovingObstacle(dCell,1) ;
655
656
657
658
659
660
661
	}
}

void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
{
	std::vector<VEC3> vPos;
	VEC3 start;
pitiot's avatar
pitiot committed
662
663
664
	float maxHeight=10.0f;

	MovingMesh* mm = NULL;
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679

	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;
pitiot's avatar
pitiot committed
680
681
682
683
684
685
686
		case 1 :
		{
			mm = new MovingMesh(envMap_, d, "meshRessources/Limace.ply");
			movingMeshes_.push_back(mm);
			vPos = mm->computeProjectedPointSet(maxHeight);
		}
			break;
687
688
689
690
691
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

pitiot's avatar
pitiot committed
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
717
718
	bool orientAccordingToFace=false;
	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);
	}

719
720
	std::vector<VEC3> goals;
	goals.push_back(start);
pitiot's avatar
pitiot committed
721
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), (obstType==0));
722
723

	movingObstacles_.push_back(mo);
pitiot's avatar
pitiot committed
724
725
726
727
728

	if(mm != NULL)
	{
		mo->attachMesh(mm);
	}
729
730
731
732
733
734
735
736
}

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

	for (std::vector<MovingObstacle*>::iterator it = movingObstacles_.begin() ; it != movingObstacles_.end() ; ++it)
	{
pitiot's avatar
pitiot committed
737
		Dart dStart = (*it)->parts_[0]->d;
pitiot's avatar
pitiot committed
738
739
740
741
742
743
744
745
746

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

747
748
749
750
751
		Dart dStop = dStart;

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

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

764
765
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
David Cazier's avatar
David Cazier committed
766
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
767
768
																   envMap_.position, dStart,
																   dGoal,
pitiot's avatar
pitiot committed
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
																   envMap_.pedWayMark) ;

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

		mo->goals_.push_back(dest) ;
	}

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

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
pitiot's avatar
pitiot committed
787
788
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;
789
790
791

		mo->goals_.push_back(dest) ;
	}
pitiot's avatar
pitiot committed
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809

//	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;
//		}
//	}
810
811
}

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

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

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

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

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

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

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

849
850
		TraversorF<PFP::MAP> tF(envMap_.map);

David Cazier's avatar
David Cazier committed
851
852
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
853
854
855
856
857
858
859
		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
860
		{
David Cazier's avatar
David Cazier committed
861
			envMap_.map.next(dStop) ;
862
863
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
864
		}
865

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

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

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

881
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
pitiot's avatar
pitiot committed
882
883

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

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

David Cazier's avatar
David Cazier committed
890
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
891
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
892
893
894
895
896
897
		    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
898
		{
David Cazier's avatar
David Cazier committed
899
			envMap_.map.next(dStop2) ;
900
901
			if (dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
902
		}
903

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

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

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

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

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

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

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

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

David Cazier's avatar
David Cazier committed
936
			dest[2] = 0 ;
937

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

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

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

David Cazier's avatar
David Cazier committed
961
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
962
963
964
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               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
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
973
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
974
975
976
		    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
977
		{
David Cazier's avatar
David Cazier committed
978
979
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
980
981
		}

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

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

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

David Cazier's avatar
David Cazier committed
994
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
995
		{
996
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
997
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
998
999
1000
		}
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
1001
#endif
Thomas's avatar
Thomas committed
1002

Pierre Kraemer's avatar
Pierre Kraemer committed
1003
1004
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
1005
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1006

David Cazier's avatar
David Cazier committed
1007
1008
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
1009
1010
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1011
1012
	}

David Cazier's avatar
David Cazier committed
1013
1014
	std::vector<VEC3> goals ;

David Cazier's avatar
David Cazier committed
1015
1016
1017
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
1018
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1019

David Cazier's avatar
David Cazier committed
1020
1021
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
1022
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
1023
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1024

David Cazier's avatar
David Cazier committed
1025
1026
1027
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1028

David Cazier's avatar
David Cazier committed
1029
1030
			VEC3 posagent(x, y, z) ;
			addAgent(posagent, -1.0f * posagent) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1031
1032
1033
		}
	}

David Cazier's avatar
David Cazier committed
1034
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1035

David Cazier's avatar
David Cazier committed
1036
1037
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1038
1039
1040
1041
}

bool Simulator::exportAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
1042
	std::ofstream out(filename.c_str(), std::ios::out) ;
David Cazier's avatar
David Cazier committed
1043
1044
	if (!out.good())
	{
David Cazier's avatar
David Cazier committed
1045
1046
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;