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

David Cazier's avatar
David Cazier committed
3
Simulator::Simulator(unsigned int config, unsigned int minS) :
David Cazier's avatar
David Cazier committed
4
5
6
7
8
9
10
11
	timeStep_(0.2f),
	globalTime_(0.0f),
	nbSteps_(0),
	nbUpdates(0),
	nbSorts(0),
	nbRefineCandidate(0),
	nbCoarsenCandidate(0),
	nearNeighbors(0),
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;
17
	multires=false;
pitiot's avatar
pitiot committed
18
	detect_agent_collision=false;
19
20
	srand(10) ;
	nbStepsPerUnit_ = 1 / timeStep_ ;
David Cazier's avatar
David Cazier committed
21
	init(config, minSize, 2.0f) ;
Thomas's avatar
Thomas committed
22
23
24
25
}

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

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

David Cazier's avatar
David Cazier committed
34
35
	switch (config)
	{
David Cazier's avatar
David Cazier committed
36
		case 0 :
David Cazier's avatar
David Cazier committed
37
38
			envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ;
			setupCircleScenario(1000, 35) ;
David Cazier's avatar
David Cazier committed
39
40
			break ;
		case 1 :
David Cazier's avatar
David Cazier committed
41
42
			envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ;
			setupCorridorScenario(1000, 40) ;
David Cazier's avatar
David Cazier committed
43
44
			break ;
		case 2 :
David Cazier's avatar
David Cazier committed
45
46
			envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ;
			setupCorridor2Scenario(200, 4) ;
David Cazier's avatar
David Cazier committed
47
			break ;
pitiot's avatar
pitiot committed
48
49
50
51
52
53
54
55
56
57
58
//		case 2 :
//			setupScenario(1000) ;
//			break ;
//		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
59
		case 5:
60
61
62
			envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ; //svg import
			setupScenario(10000) ;
			addPathsToAgents();
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
		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() ;
pitiot's avatar
pitiot committed
112
113
	}

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

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

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

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

David Cazier's avatar
David Cazier committed
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
//		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
174

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

198
199
200
201
202
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
203
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
David Cazier's avatar
David Cazier committed
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
{
	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
219
	double pi = 3.14159265358979323846f ;
David Cazier's avatar
David Cazier committed
220
221
222
223
224
225
226

	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
227
		addAgent(start, goal) ;
228
	}
pitiot's avatar
pitiot committed
229
230
231
232
233
	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
234
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
David Cazier's avatar
David Cazier committed
235
	{
pitiot's avatar
pitiot committed
236
237
238
239
240
241
242
243
244
245
246
247
		double angle = i * 2.0f * pi / float(nbObstacles/2) ;
		VEC3 v,start;
		if(i<nbObstacles/2)
		{
			v=VEC3 (std::cos(angle) * (2*radius/3), std::sin(angle) * (2*radius/3), 0) ;
		}
		else
		{
			v=VEC3 (std::cos(angle) * (radius/3), std::sin(angle) * (radius/3), 0) ;
		}
		start = center + v ;
		positions.push_back(start);
pitiot's avatar
pitiot committed
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
	}
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
		std::vector<VEC3> goals;
		for(unsigned int k = 0 ; k < nbObstacles/2 ; k++)
		{
			goals.push_back(positions[(i+k)%(nbObstacles/2)]);
		}

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

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

pitiot's avatar
pitiot committed
267
268
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
269

pitiot's avatar
pitiot committed
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
	for (unsigned int i = nbObstacles/2 ; i < nbObstacles ; i++)
	{
		VEC3 start = positions[i];
		std::vector<VEC3> goals;
		for(unsigned int k = (nbObstacles/2) ; k > 0 ; k--)
		{
			goals.push_back(positions[((i-(nbObstacles/2)+k)%(nbObstacles/2))+nbObstacles/2]);
		}

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

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

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
289
	}
Thomas's avatar
Thomas committed
290

291
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
292
293
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
294
#endif
Thomas's avatar
Thomas committed
295
296
}

David Cazier's avatar
David Cazier committed
297
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
298
{
David Cazier's avatar
David Cazier committed
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
	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
336

David Cazier's avatar
David Cazier committed
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
	// 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);
		mo4= new MovingObstacle(this, i,vPos,goals,false);


378
379
380
381
382
383
384
385
386
387
388
389
390
391
//		//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
392
393
394
395
396
397
398

		movingObstacles_.push_back(mo4);
	}
}

void Simulator::setupCorridor2Scenario(unsigned int nbAgents, unsigned int nbObstacles)
{
David Cazier's avatar
David Cazier committed
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
	std::cout << " - Setup Corridor Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles" << std::endl ;

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

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

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

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

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

David Cazier's avatar
David Cazier committed
424
		// Un agent sur 2 va de droite à gauche
David Cazier's avatar
David Cazier committed
425
		if (i % 2 == 1)
David Cazier's avatar
David Cazier committed
426
427
428
			addAgent(start, goal) ;
		else
			addAgent(goal, start) ;
Thomas's avatar
Thomas committed
429
430
	}

David Cazier's avatar
David Cazier committed
431
432
433
434
435
436
437
438
439
440
	// 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 ;

	// Arrivée des obstacles à l'opposée
	yGoalDelta = envMap_.geometry.size(1) / 5 ;
	yGoalMin = envMap_.geometry.max()[1] - yBorder - yGoalDelta ;

441
	VEC3 xSide (5.0f,0.0f,0.0f);
442
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
443
444
445
446
447
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
Jund Thomas's avatar
Jund Thomas committed
448
		float x = xStartMin + ((int)i*30) % xStartDelta;
449
//		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
David Cazier's avatar
David Cazier committed
450
451
452
		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(x, yGoalMin  + rand() % yGoalDelta,  0) ;

David Cazier's avatar
David Cazier committed
453
		vPos.clear();
David Cazier's avatar
David Cazier committed
454
455
456
457
458
459
460
		// Un obstacle sur deux va vers le haut
		VEC3 tmp ;
		if (i % 2 == 1)
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
David Cazier's avatar
David Cazier committed
461
462
463
464
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
David Cazier's avatar
David Cazier committed
465
		}
David Cazier's avatar
David Cazier committed
466
467
468
469
470
471
		else {
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
		}
pitiot's avatar
pitiot committed
472
473
474
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
pitiot's avatar
pitiot committed
475
		mo4= new MovingObstacle(this, i,vPos,goals,false);
476
477
478


		//for generating a random path
Jund Thomas's avatar
Jund Thomas committed
479
480
481
482
483
484
485
486
487
488
489
490
491
		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);
492

David Cazier's avatar
David Cazier committed
493
494
		movingObstacles_.push_back(mo4);
	}
Thomas's avatar
Thomas committed
495
496
}

David Cazier's avatar
David Cazier committed
497
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
498
{
David Cazier's avatar
David Cazier committed
499
500
501
502
503
	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
504

David Cazier's avatar
David Cazier committed
505
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
506
	{
David Cazier's avatar
David Cazier committed
507
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
508
		{
509
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
David Cazier's avatar
David Cazier committed
510
			addAgent(posagent, -1.0f * posagent) ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
511
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
512
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
513
514
}

515
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
516
517
{
	/*
David Cazier's avatar
David Cazier committed
518
	 * Add agents, specifying their start position, and store their goal on the
David Cazier's avatar
David Cazier committed
519
520
	 * opposite side of the environment.
	 */
521
522
	TraversorF<PFP::MAP> tF(envMap_.map);
	Dart d = tF.begin() ;
David Cazier's avatar
David Cazier committed
523

524
525
	unsigned int nbx = 1 ;
	unsigned int nby = 1 ;
David Cazier's avatar
David Cazier committed
526
527
528

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

David Cazier's avatar
David Cazier committed
529
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
530
	{
David Cazier's avatar
David Cazier committed
531
532
533
		bool found = false ;
		VEC3 pos ;
		Dart dCell ;
534
		while (!found && d != tF.end())
David Cazier's avatar
David Cazier committed
535
		{
536
537
538
			if (!envMap_.buildingMark.isMarked(d)
//			    && envMap_.pedWayMark.isMarked(d)
			    )
David Cazier's avatar
David Cazier committed
539
			{
540
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
541
542
543
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
544
			}
545
			d = tF.next() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
546
547
		}

David Cazier's avatar
David Cazier committed
548
549
		if (found)
		{
550
			float ecart = 0.0f ;
551
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
552
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
David Cazier's avatar
David Cazier committed
553
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
554
			{
David Cazier's avatar
David Cazier committed
555
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
556
				{
557
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
558
					addAgent(posagent, -1.0f * posagent, dCell) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
559
560
561
562
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
563
564
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
565
}
566

567
568
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
David Cazier's avatar
David Cazier committed
569
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
570
571
572
573
574
575
576
577
578
579
580
581
																   envMap_.position, dStart,
																   dGoal,
																   envMap_.buildingMark) ;

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
582
#ifndef SPATIAL_HASHING
583
584
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
585
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
586
	{
David Cazier's avatar
David Cazier committed
587
588
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
589

David Cazier's avatar
David Cazier committed
590
591
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
592

David Cazier's avatar
David Cazier committed
593
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
594
595
596
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
597

David Cazier's avatar
David Cazier committed
598
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
599
		{
600
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
601
602
603
604
605

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

David Cazier's avatar
David Cazier committed
606
			agents_[i]->goals_.push_back(dest) ;
607
608
609
610
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
611
612
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
613
	//city
David Cazier's avatar
David Cazier committed
614
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
615
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
616
	{
David Cazier's avatar
David Cazier committed
617
618
619
620
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
621
622
623
624
625
626
627
		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
628
		{
David Cazier's avatar
David Cazier committed
629
630
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
631
		}
632

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

David Cazier's avatar
David Cazier committed
635
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
636
637
638
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
639

David Cazier's avatar
David Cazier committed
640
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
641
		{
642
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
643
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
644
645
			dest /= 2.0f ;
			dest[2] = 0 ;
646

David Cazier's avatar
David Cazier committed
647
			agents_[i]->goals_.push_back(dest) ;
648
		}
649

David Cazier's avatar
David Cazier committed
650
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
651
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
652
653
654
655
656
657
		    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
658
		{
David Cazier's avatar
David Cazier committed
659
660
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
661
		}
662

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

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

David Cazier's avatar
David Cazier committed
668
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
669
		{
670
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
671
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
672
673
			dest /= 2.0f ;
			dest[2] = 0 ;
674

David Cazier's avatar
David Cazier committed
675
			agents_[i]->goals_.push_back(dest) ;
676
		}
677

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

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

David Cazier's avatar
David Cazier committed
683
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
684
		{
685
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
686
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
687
688
			dest /= 2.0f ;
			dest[2] = 0 ;
689

David Cazier's avatar
David Cazier committed
690
			agents_[i]->goals_.push_back(dest) ;
691
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
692
693
	}
}
694

Thomas's avatar
Thomas committed
695
696
697
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
698
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
699
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
700
	{
David Cazier's avatar
David Cazier committed
701
702
703
704
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
705
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
706
707
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
708
		{
David Cazier's avatar
David Cazier committed
709
710
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
711
712
		}

David Cazier's avatar
David Cazier committed
713
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
714
715
716
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
717

David Cazier's avatar
David Cazier committed
718
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
719
		{
720
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
721
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
722
723
		}

David Cazier's avatar
David Cazier committed
724
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
725
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
726
727
728
		    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
729
		{
David Cazier's avatar
David Cazier committed
730
731
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
732
733
		}

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

David Cazier's avatar
David Cazier committed
737
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
738
		{
739
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
740
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
741
742
		}

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

David Cazier's avatar
David Cazier committed
746
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
747
		{
748
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
749
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
750
751
752
		}
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
753
#endif
Thomas's avatar
Thomas committed
754

Pierre Kraemer's avatar
Pierre Kraemer committed
755
756
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
757
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
758

David Cazier's avatar
David Cazier committed
759
760
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
761
762
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
763
764
	}

David Cazier's avatar
David Cazier committed
765
766
	std::vector<VEC3> goals ;

David Cazier's avatar
David Cazier committed
767
768
769
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
770
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
771

David Cazier's avatar
David Cazier committed
772
773
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
774
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
775
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
776

David Cazier's avatar
David Cazier committed
777
778
779
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
780

David Cazier's avatar
David Cazier committed
781
782
			VEC3 posagent(x, y, z) ;
			addAgent(posagent, -1.0f * posagent) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
783
784
785
		}
	}

David Cazier's avatar
David Cazier committed
786
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
787

David Cazier's avatar
David Cazier committed
788
789
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
790
791
792
793
}

bool Simulator::exportAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
794
	std::ofstream out(filename.c_str(), std::ios::out) ;
David Cazier's avatar
David Cazier committed
795
796
	if (!out.good())
	{
David Cazier's avatar
David Cazier committed
797
798
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
799
800
	}

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

David Cazier's avatar
David Cazier committed
804
805
	out.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
806
807
808
809
}

void Simulator::swapAgentsGoals()
{
David Cazier's avatar
David Cazier committed
810
	unsigned int nbAgents = agents_.size() ;
David Cazier's avatar
David Cazier committed
811
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
812
	{
David Cazier's avatar
David Cazier committed
813
		unsigned int r = rand() % nbAgents ;
814
		std::swap(agents_[i]->goals_, agents_[r]->goals_) ;
Thomas's avatar
Thomas committed
815
816
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
817

Thomas's avatar
Thomas committed
818
819
Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
{
David Cazier's avatar
David Cazier committed
820
821
	Geom::BoundingBox<VEC3> bb ;
	unsigned int nbAgents = agents_.size() ;
David Cazier's avatar
David Cazier committed
822
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
823
	{
David Cazier's avatar
David Cazier committed
824
		bb.addPoint(agents_[i]->getPosition()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
825
	}
David Cazier's avatar
David Cazier committed
826
	return bb ;
Pierre Kraemer's avatar
Pierre Kraemer committed
827
}