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

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

pitiot's avatar
pitiot committed
3
Simulator::Simulator(int minS) :
David Cazier's avatar
David Cazier committed
4
5
6
7
8
9
10
11
	timeStep_(0.2f),
	globalTime_(0.0f),
	nbSteps_(0),
	nbUpdates(0),
	nbSorts(0),
	nbRefineCandidate(0),
	nbCoarsenCandidate(0),
	nearNeighbors(0),
pitiot's avatar
maj    
pitiot committed
12
13
14
	totalNeighbors(0),
	avoidance(1),
	nb_dead(0)
Pierre Kraemer's avatar
Pierre Kraemer committed
15
{
pitiot's avatar
pitiot committed
16
	minSize=minS;
pitiot's avatar
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
pitiot committed
21
	config=1;
pitiot's avatar
pitiot committed
22
	init( minSize, 2.0f) ;
Thomas's avatar
Thomas committed
23
24
25
26
}

Simulator::~Simulator()
{
pitiot's avatar
maj    
pitiot 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
}

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

David Cazier's avatar
David Cazier committed
35
36
	switch (config)
	{
pitiot's avatar
maj    
pitiot committed
37
		case 0 :
pitiot's avatar
pitiot committed
38
			setupCircleScenario(1000,40) ;
pitiot's avatar
maj    
pitiot committed
39
40
			break ;
		case 1 :
pitiot's avatar
pitiot committed
41
			setupCorridorScenario(1000,40) ;
pitiot's avatar
maj    
pitiot committed
42
			break ;
pitiot's avatar
pitiot committed
43
44
45
46
47
48
49
50
51
52
53
//		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 ;
Thomas's avatar
Thomas committed
54
55
	}

pitiot's avatar
maj    
pitiot committed
56
57
58
59
60
#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
61
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
62
63
64
65
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
			addPathsToAgents() ;
66
		else if (dimension == 2.5f) addPathsToAgents_height() ;
Thomas's avatar
Thomas committed
67
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
68
#endif
69

pitiot's avatar
maj    
pitiot committed
70
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
71
	{
David Cazier's avatar
David Cazier committed
72
73
		agents_[i]->updateObstacleNeighbors() ;
		agents_[i]->updateAgentNeighbors() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
74
	}
75

Pierre Kraemer's avatar
Pierre Kraemer committed
76
#ifndef SPATIAL_HASHING
77
	if (multires) envMap_.subdivideToProperLevel() ;
pitiot's avatar
maj    
pitiot committed
78
//	envMap_.subdivideAllToMaxLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
79
#endif
pitiot's avatar
maj    
pitiot committed
80
81

//	setupMovingObstacle(40);
Pierre Kraemer's avatar
Pierre Kraemer committed
82
83
}

pitiot's avatar
maj    
pitiot committed
84
void Simulator::doStep()
Pierre Kraemer's avatar
Pierre Kraemer committed
85
{
pitiot's avatar
maj    
pitiot committed
86
87
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
pitiot's avatar
pitiot committed
88
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
89
90
91
92
93
#endif

	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
	{
		movingObstacles_[i]->computePrefVelocity() ;
pitiot's avatar
pitiot committed
94
		movingObstacles_[i]->computeNewVelocity() ;
pitiot's avatar
pitiot committed
95
96
97
#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
pitiot's avatar
maj    
pitiot committed
98
99
		movingObstacles_[i]->update() ;
	}
pitiot's avatar
pitiot committed
100
101
102
#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(0) ;
#endif
pitiot's avatar
maj    
pitiot committed
103
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
104
	{
pitiot's avatar
pitiot committed
105
106
107
		if (agents_[i]->alive)
		{
			agents_[i]->updateObstacleNeighbors() ;
pitiot's avatar
maj    
pitiot committed
108
109

#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
110
			envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
111
#endif
pitiot's avatar
pitiot committed
112
			agents_[i]->updateAgentNeighbors() ;
pitiot's avatar
maj    
pitiot committed
113
#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
114
			envMap_.map.setCurrentLevel(0) ;
pitiot's avatar
maj    
pitiot committed
115
116
#endif

pitiot's avatar
pitiot committed
117
118
119
			agents_[i]->computePrefVelocity() ;
			agents_[i]->computeNewVelocity() ;
		}
pitiot's avatar
maj    
pitiot committed
120
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
121

Pierre Kraemer's avatar
Pierre Kraemer committed
122
#ifdef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
123
124
125
126
127
128
129
130
131
132
133
134
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		Geom::Vec2ui cold = envMap_.agentPositionCell(agents_[i]) ;
		agents_[i]->update() ;
		Geom::Vec2ui cnew = envMap_.agentPositionCell(agents_[i]) ;
		if (cnew != cold)
		{
			envMap_.removeAgentFromGrid(agents_[i], cold) ;
			envMap_.addAgentInGrid(agents_[i], cnew) ;
		}
	}
#else
135
	if (multires) envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
136
137
138
139
	nb_dead = 0 ;
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		if (agents_[i]->alive)
Pierre Kraemer's avatar
Pierre Kraemer committed
140
		{
pitiot's avatar
maj    
pitiot committed
141
142
143
144
145
146
			Dart oldFace = agents_[i]->part_.d ;
			agents_[i]->update() ;

//		if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE))
			if (agents_[i]->part_.crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//		switch(agents_[i]->part_.crossCell)
Pierre Kraemer's avatar
Pierre Kraemer committed
147
			{
pitiot's avatar
maj    
pitiot committed
148
149
150
151
152
153
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace) ;
//				break;
Pierre Kraemer's avatar
Pierre Kraemer committed
154
155
			}
		}
pitiot's avatar
maj    
pitiot committed
156
157
158
159
160
161
162
		else
			nb_dead++ ;

	}

	nbRefineCandidate += envMap_.refineCandidate.size() ;
	nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
163
	if (multires) envMap_.updateMap() ;
pitiot's avatar
maj    
pitiot committed
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
#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 ;
}

180
void Simulator::addAgent(const VEC3& start, const std::vector<VEC3>& goal)
pitiot's avatar
maj    
pitiot committed
181
182
183
184
185
{
#ifdef SPATIAL_HASHING
	Agent* a = new Agent(this, start) ;
	agents_.push_back(a) ;
	envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
186
#else
pitiot's avatar
pitiot committed
187

pitiot's avatar
maj    
pitiot committed
188
	Dart dCell = envMap_.getBelongingCell(start) ;
pitiot's avatar
pitiot committed
189
//	if (dCell==envMap_.map.begin()) CGoGNout<<"pbagents"<<CGoGNendl;
pitiot's avatar
maj    
pitiot committed
190
	agents_.push_back(new Agent(this, start, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
191
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
192

193
	agents_.back()->goals_=goal ;
pitiot's avatar
maj    
pitiot committed
194
195
196
	agents_.back()->curGoal_ = 0 ;
}

pitiot's avatar
pitiot committed
197
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
pitiot's avatar
maj    
pitiot committed
198
{
pitiot's avatar
pitiot committed
199
200
201
202
203
204
205
206
207
208
	if (multires)
	{
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
	}
	else
	{

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

pitiot's avatar
maj    
pitiot committed
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents" << std::endl ;

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

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

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

	double pi = 3.14159265358979323846f ;

	for (unsigned int i = 0 ; i < nbAgents ; ++i)
	{
		double angle = i * 2.0f * pi / float(nbAgents) ;
		VEC3 v(std::cos(angle) * radius, std::sin(angle) * radius, 0) ;
		VEC3 start = center + v ;
		VEC3 goal = center - v ;
231
232
233
234
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
235
	}
pitiot's avatar
pitiot committed
236
237
238
239
240
	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
241
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
pitiot's avatar
maj    
pitiot committed
242
	{
pitiot's avatar
pitiot committed
243
244
245
246
247
248
249
250
251
252
253
254
		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
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
	}
	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
272
		mo4= new MovingObstacle(this, i,vPos,goals,true);
pitiot's avatar
pitiot committed
273

pitiot's avatar
pitiot committed
274
275
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
276

pitiot's avatar
pitiot committed
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
	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
294
295

		movingObstacles_.push_back(mo4);
pitiot's avatar
maj    
pitiot committed
296
	}
297
#ifndef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
298
299
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
300
#endif
pitiot's avatar
maj    
pitiot committed
301
302

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

pitiot's avatar
maj    
pitiot committed
305
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
306
{
pitiot's avatar
pitiot committed
307
308
309
310
311
312
313
314
315
316
317
	if (multires)
	{
		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
	}
	else
	{

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


pitiot's avatar
maj    
pitiot committed
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
	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
339
	{
pitiot's avatar
maj    
pitiot committed
340
341
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;
342

pitiot's avatar
maj    
pitiot committed
343
344
		// Un agent sur 2 va de droite à gauche
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
345
346
		if (i % 2 == 1)
		{
pitiot's avatar
maj    
pitiot committed
347
348
349
			tmp = goal ;
			goal = start ;
			start = tmp ;
Pierre Kraemer's avatar
Pierre Kraemer committed
350
		}
351
352
353
354
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
Thomas's avatar
Thomas committed
355
356
	}

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

pitiot's avatar
maj    
pitiot committed
363
364
365
366
367
368
369
370
371
372
373
374
	// 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;
375
//		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
pitiot's avatar
maj    
pitiot committed
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(x, yGoalMin  + rand() % yGoalDelta,  0) ;

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		VEC3 tmp ;
		if (i % 2 == 1)
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
		}
		else {
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
		}
pitiot's avatar
pitiot committed
398
399
400
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
pitiot's avatar
pitiot committed
401
		mo4= new MovingObstacle(this, i,vPos,goals,false);
pitiot's avatar
pitiot committed
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418


		//for generating a random path
//		unsigned int dartDistForPath = 50 ;
//		mo4->goals_.clear() ;
//		Dart dStart = mo4->parts_[0]->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);

pitiot's avatar
maj    
pitiot committed
419
420
		movingObstacles_.push_back(mo4);
	}
Thomas's avatar
Thomas committed
421
422
}

pitiot's avatar
maj    
pitiot committed
423
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
424
{
pitiot's avatar
maj    
pitiot committed
425
426
427
428
429
	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
430

pitiot's avatar
maj    
pitiot committed
431
	VEC3 posagent(xCornerDown - xBorder / 2, yCornerDown - yBorder / 2, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
432
433
434
#ifdef SPATIAL_HASHING
	Agent* a = new Agent(this, posagent) ;
	agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
435
	envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
436
#else
437
438
	Dart dCell = envMap_.getBelongingCell(posagent) ;
	agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
439
#endif
David Cazier's avatar
David Cazier committed
440
	agents_.back()->goals_.push_back(-1.0f * posagent) ;
441
	agents_.back()->curGoal_ = 0 ;
442

pitiot's avatar
maj    
pitiot committed
443
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
444
	{
pitiot's avatar
maj    
pitiot committed
445
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
446
		{
447
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
448
449
450
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, posagent) ;
			agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
451
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
452
#else
453
454
			Dart dCell = envMap_.getBelongingCell(posagent) ;
			agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
455
#endif
David Cazier's avatar
David Cazier committed
456
			agents_.back()->goals_.push_back(-1.0f * posagent) ;
457
			agents_.back()->curGoal_ = 0 ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
458
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
459
	}
Thomas's avatar
Thomas committed
460

David Cazier's avatar
David Cazier committed
461
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
462
463
}

464
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
465
{
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
//	Geom::BoundingBox<PFP::VEC3> bb(envMap_.position.begin());
//	for(unsigned int v = envMap_.position.begin() ; v != envMap_.position.end() ; envMap_.position.next(v))
//		bb.addPoint(envMap_.position[v]);
//	VEC3 posinit = bb.center();
//	posinit[1] = bb.min()[1] + 35.0f;

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

Pierre Kraemer's avatar
Pierre Kraemer committed
487
	/*
David Cazier's avatar
David Cazier committed
488
489
490
491
	 * Add agents, specifying their start position, and store their goals on the
	 * opposite side of the environment.
	 */
	Dart d = envMap_.map.begin() ;
pitiot's avatar
maj    
pitiot committed
492
	CellMarker<FACE> filled(envMap_.map) ;
David Cazier's avatar
David Cazier committed
493
494
495
496
497
498

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

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

pitiot's avatar
maj    
pitiot committed
499
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
500
	{
David Cazier's avatar
David Cazier committed
501
502
503
		bool found = false ;
		VEC3 pos ;
		Dart dCell ;
David Cazier's avatar
David Cazier committed
504
505
506
507
508
		while (!found && d != envMap_.map.end())
		{
			if (!filled.isMarked(d)
			    && !envMap_.buildingMark.isMarked(d) /*&& envMap_.pedWayMark.isMarked(d)*/)
			{
David Cazier's avatar
David Cazier committed
509
				filled.mark(d) ;
510
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
511
512
513
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
514
			}
David Cazier's avatar
David Cazier committed
515
			envMap_.map.next(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
516
517
		}

David Cazier's avatar
David Cazier committed
518
519
		if (found)
		{
520
521
			float ecart = 3.0f ;
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
522
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
pitiot's avatar
maj    
pitiot committed
523
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
524
			{
pitiot's avatar
maj    
pitiot committed
525
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
526
				{
527
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
528
529
530
#ifdef SPATIAL_HASHING
					Agent* a = new Agent(this, posagent) ;
					agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
531
					envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
532
#else
533
					agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
534
#endif
535
					agents_.back()->goals_.push_back(posagent) ;
David Cazier's avatar
David Cazier committed
536
					agents_.back()->goals_.push_back(-1.0f * posagent) ;
pitiot's avatar
maj    
pitiot committed
537

538
					agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
539
540
541
542
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
543
544
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
545
}
546

pitiot's avatar
pitiot committed
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
																   envMap_.position, dStart,
																   dGoal,
																   envMap_.buildingMark) ;

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
562
#ifndef SPATIAL_HASHING
563
564
void Simulator::addPathToCorner()
{
pitiot's avatar
maj    
pitiot committed
565
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
566
	{
David Cazier's avatar
David Cazier committed
567
568
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
569

David Cazier's avatar
David Cazier committed
570
571
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
572

573
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
574
575
576
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
577

pitiot's avatar
maj    
pitiot committed
578
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
579
		{
580
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
581
582
583
584
585

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

David Cazier's avatar
David Cazier committed
586
			agents_[i]->goals_.push_back(dest) ;
587
588
589
590
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
591
592
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
593
	//city
David Cazier's avatar
David Cazier committed
594
	unsigned int dartDistForPath = 50 ;
pitiot's avatar
maj    
pitiot committed
595
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
596
	{
David Cazier's avatar
David Cazier committed
597
598
599
600
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
pitiot's avatar
maj    
pitiot committed
601
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
602
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
603
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
604
		{
David Cazier's avatar
David Cazier committed
605
606
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
607
		}
608

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

611
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
612
613
614
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
615

pitiot's avatar
maj    
pitiot committed
616
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
617
		{
618
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
619
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
620
621
			dest /= 2.0f ;
			dest[2] = 0 ;
622

David Cazier's avatar
David Cazier committed
623
			agents_[i]->goals_.push_back(dest) ;
624
		}
625

David Cazier's avatar
David Cazier committed
626
		Dart dStop2 = dStop ;
pitiot's avatar
maj    
pitiot committed
627
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
628
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
629
630
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
631
		{
David Cazier's avatar
David Cazier committed
632
633
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
634
		}
635

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

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

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

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

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

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

pitiot's avatar
maj    
pitiot committed
656
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
657
		{
658
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
659
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
660
661
			dest /= 2.0f ;
			dest[2] = 0 ;
662

David Cazier's avatar
David Cazier committed
663
			agents_[i]->goals_.push_back(dest) ;
664
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
665
666
	}
}
667

Thomas's avatar
Thomas committed
668
669
670
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
671
	unsigned int dartDistForPath = 50 ;
pitiot's avatar
maj    
pitiot committed
672
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
673
	{
David Cazier's avatar
David Cazier committed
674
675
676
677
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
pitiot's avatar
maj    
pitiot committed
678
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
679
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
680
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
681
		{
David Cazier's avatar
David Cazier committed
682
683
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
684
685
		}

686
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
687
688
689
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
690

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

David Cazier's avatar
David Cazier committed
697
		Dart dStop2 = dStop ;
pitiot's avatar
maj    
pitiot committed
698
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
699
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
700
701
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
702
		{
David Cazier's avatar
David Cazier committed
703
704
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
705
706
		}

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

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

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
728
729
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
730
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
731

David Cazier's avatar
David Cazier committed
732
733
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
734
735
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
736
737
	}

David Cazier's avatar
David Cazier committed
738
739
740
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
741
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
742

David Cazier's avatar
David Cazier committed
743
744
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
745
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
746
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
747

David Cazier's avatar
David Cazier committed
748
749
750
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
751

752
			VEC3 pos(x, y, z) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
753
754
755
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, pos) ;
			agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
756
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
757
#else
758
759
			Dart dCell = envMap_.getBelongingCell(pos) ;
			agents_.push_back(new Agent(this, pos, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
760
#endif
761
			agents_.back()->goals_.push_back(pos) ;
David Cazier's avatar
David Cazier committed
762
			agents_.back()->goals_.push_back(-1.0f * pos) ;
763
			agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
764
765
766
		}
	}

David Cazier's avatar
David Cazier committed
767
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
768

David Cazier's avatar
David Cazier committed
769
770
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
771
772
773
774
}

bool Simulator::exportAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
775
	std::ofstream out(filename.c_str(), std::ios::out) ;
David Cazier's avatar
David Cazier committed
776
777
	if (!out.good())
	{
David Cazier's avatar
David Cazier committed
778
779
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
780
781
	}

pitiot's avatar
maj    
pitiot committed
782
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
783
#ifdef SPATIAL_HASHING
784
		out << agents_[i]->pos << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
785
#else
pitiot's avatar
maj    
pitiot committed
786
		out << agents_[i]->part_.getPosition() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
787
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
788

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

void Simulator::swapAgentsGoals()
{
David Cazier's avatar
David Cazier committed
795
	unsigned int nbAgents = agents_.size() ;
pitiot's avatar
maj    
pitiot committed
796
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
797
	{
David Cazier's avatar
David Cazier committed
798
		unsigned int r = rand() % nbAgents ;
Thomas's avatar
Thomas committed
799
800
801
802
803
804
805
//		for(unsigned int nbg = 0; nbg < agents_[i]->goals_.size(); ++nbg)
//		{
//			VEC3 g = agents_[i]->goals_[nbg];
//			agents_[i]->goals_[nbg] = agents_[r]->goals_[nbg];
//			agents_[r]->goals_[nbg] = g;
//		}

806
		std::swap(agents_[i]->goals_, agents_[r]->goals_) ;
Thomas's avatar
Thomas committed
807
808
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
809

Thomas's avatar
Thomas committed
810
811
Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
{
David Cazier's avatar
David Cazier committed
812
813
	Geom::BoundingBox<VEC3> bb ;
	unsigned int nbAgents = agents_.size() ;
pitiot's avatar
maj    
pitiot committed
814
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
815
	{
David Cazier's avatar
David Cazier committed
816
		bb.addPoint(agents_[i]->getPosition()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
817
	}
Thomas's avatar
Thomas committed
818

David Cazier's avatar
David Cazier committed
819
	return bb ;
Pierre Kraemer's avatar
Pierre Kraemer committed
820
}