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

David Cazier's avatar
David Cazier committed
3
4
5
6
7
8
9
10
11
Simulator::Simulator(int minSize) :
	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
{
16
17
	srand(10) ;
	nbStepsPerUnit_ = 1 / timeStep_ ;
David Cazier's avatar
David Cazier committed
18
	init(1, minSize, 2.0f) ;
Thomas's avatar
Thomas committed
19
20
21
22
}

Simulator::~Simulator()
{
pitiot's avatar
maj    
pitiot committed
23
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
24
		delete agents_[i] ;
Thomas's avatar
Thomas committed
25
26
}

David Cazier's avatar
David Cazier committed
27
void Simulator::init(unsigned int config, int minSize, float dimension, bool enablePathFinding)
Thomas's avatar
Thomas committed
28
{
29
	std::cout << "Setup scenario" << std::endl ;
pitiot's avatar
maj    
pitiot committed
30
	envMap_.init(config, 1500.0f, 1000.0f, minSize, 50.0f) ;
Thomas's avatar
Thomas committed
31

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

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
73
#ifndef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
74
75
//	envMap_.subdivideToProperLevel() ;
//	envMap_.subdivideAllToMaxLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
76
#endif
pitiot's avatar
maj    
pitiot committed
77
78

//	setupMovingObstacle(40);
Pierre Kraemer's avatar
Pierre Kraemer committed
79
80
}

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

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

#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
		agents_[i]->updateAgentNeighbors() ;
#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(0) ;
#endif

		agents_[i]->computePrefVelocity() ;
		agents_[i]->computeNewVelocity() ;
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
114

Pierre Kraemer's avatar
Pierre Kraemer committed
115
#ifdef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
	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
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
	nb_dead = 0 ;
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		if (agents_[i]->alive)
Pierre Kraemer's avatar
Pierre Kraemer committed
133
		{
pitiot's avatar
maj    
pitiot committed
134
135
136
137
138
139
			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
140
			{
pitiot's avatar
maj    
pitiot committed
141
142
143
144
145
146
//			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
147
148
			}
		}
pitiot's avatar
maj    
pitiot committed
149
150
151
152
153
154
155
		else
			nb_dead++ ;

	}

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

void Simulator::addAgent(const VEC3& start, const VEC3& goal)
{
#ifdef SPATIAL_HASHING
	Agent* a = new Agent(this, start) ;
	agents_.push_back(a) ;
	envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
179
#else
pitiot's avatar
maj    
pitiot committed
180
181
	Dart dCell = envMap_.getBelongingCell(start) ;
	agents_.push_back(new Agent(this, start, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
182
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
183

pitiot's avatar
maj    
pitiot committed
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
	agents_.back()->goals_.push_back(goal) ;
	agents_.back()->curGoal_ = 0 ;
}

void Simulator::setupCircleScenario(unsigned int nbAgents)
{
	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 ;

		addAgent(start, goal) ;
214
	}
Thomas's avatar
Thomas committed
215

pitiot's avatar
maj    
pitiot committed
216
217
218
219
220
221
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
	{
		movingObstacles_[i]->updateFixedObstNeighbors() ;
		movingObstacles_[i]->computePrefVelocity() ;
		movingObstacles_[i]->update() ;
	}
Thomas's avatar
Thomas committed
222

223
#ifndef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
224
225
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
226
#endif
pitiot's avatar
maj    
pitiot committed
227
228

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

pitiot's avatar
maj    
pitiot committed
231
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
232
{
pitiot's avatar
maj    
pitiot committed
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
	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
254
	{
pitiot's avatar
maj    
pitiot committed
255
256
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;
257

pitiot's avatar
maj    
pitiot committed
258
259
		// Un agent sur 2 va de droite à gauche
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
260
261
		if (i % 2 == 1)
		{
pitiot's avatar
maj    
pitiot committed
262
263
264
			tmp = goal ;
			goal = start ;
			start = tmp ;
Pierre Kraemer's avatar
Pierre Kraemer committed
265
266
		}

pitiot's avatar
maj    
pitiot committed
267
		addAgent(start, goal) ;
Thomas's avatar
Thomas committed
268
269
	}

pitiot's avatar
maj    
pitiot committed
270
271
272
273
274
	// 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
275

pitiot's avatar
maj    
pitiot committed
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
	// 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);
		}
		mo4= new MovingObstacle(this, i,vPos,goal,0);
pitiot's avatar
pitiot committed
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328


		//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
329
330
		movingObstacles_.push_back(mo4);
	}
Thomas's avatar
Thomas committed
331
332
}

pitiot's avatar
maj    
pitiot committed
333
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
334
{
pitiot's avatar
maj    
pitiot committed
335
336
337
338
339
	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
340

pitiot's avatar
maj    
pitiot committed
341
	VEC3 posagent(xCornerDown - xBorder / 2, yCornerDown - yBorder / 2, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
342
343
344
#ifdef SPATIAL_HASHING
	Agent* a = new Agent(this, posagent) ;
	agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
345
	envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
346
#else
347
348
	Dart dCell = envMap_.getBelongingCell(posagent) ;
	agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
349
#endif
David Cazier's avatar
David Cazier committed
350
	agents_.back()->goals_.push_back(-1.0f * posagent) ;
351
	agents_.back()->curGoal_ = 0 ;
352

pitiot's avatar
maj    
pitiot committed
353
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
354
	{
pitiot's avatar
maj    
pitiot committed
355
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
356
		{
357
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
358
359
360
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, posagent) ;
			agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
361
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
362
#else
363
364
			Dart dCell = envMap_.getBelongingCell(posagent) ;
			agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
365
#endif
David Cazier's avatar
David Cazier committed
366
			agents_.back()->goals_.push_back(-1.0f * posagent) ;
367
			agents_.back()->curGoal_ = 0 ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
368
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
369
	}
Thomas's avatar
Thomas committed
370

David Cazier's avatar
David Cazier committed
371
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
372
373
}

374
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
375
{
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
//	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
397
	/*
David Cazier's avatar
David Cazier committed
398
399
400
401
	 * 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
402
	CellMarker<FACE> filled(envMap_.map) ;
David Cazier's avatar
David Cazier committed
403
404
405
406
407
408

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

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

pitiot's avatar
maj    
pitiot committed
409
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
410
	{
David Cazier's avatar
David Cazier committed
411
412
413
		bool found = false ;
		VEC3 pos ;
		Dart dCell ;
David Cazier's avatar
David Cazier committed
414
415
416
417
418
		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
419
				filled.mark(d) ;
420
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
421
422
423
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
424
			}
David Cazier's avatar
David Cazier committed
425
			envMap_.map.next(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
426
427
		}

David Cazier's avatar
David Cazier committed
428
429
		if (found)
		{
430
431
			float ecart = 3.0f ;
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
432
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
pitiot's avatar
maj    
pitiot committed
433
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
434
			{
pitiot's avatar
maj    
pitiot committed
435
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
436
				{
437
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
438
439
440
#ifdef SPATIAL_HASHING
					Agent* a = new Agent(this, posagent) ;
					agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
441
					envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
442
#else
443
					agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
444
#endif
445
					agents_.back()->goals_.push_back(posagent) ;
David Cazier's avatar
David Cazier committed
446
					agents_.back()->goals_.push_back(-1.0f * posagent) ;
pitiot's avatar
maj    
pitiot committed
447

448
					agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
449
450
451
452
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
453
454
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
455
}
456

pitiot's avatar
pitiot committed
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
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
472
#ifndef SPATIAL_HASHING
473
474
void Simulator::addPathToCorner()
{
pitiot's avatar
maj    
pitiot committed
475
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
476
	{
David Cazier's avatar
David Cazier committed
477
478
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
479

David Cazier's avatar
David Cazier committed
480
481
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
482

483
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
484
485
486
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
487

pitiot's avatar
maj    
pitiot committed
488
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
489
		{
490
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
491
492
493
494
495

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

David Cazier's avatar
David Cazier committed
496
			agents_[i]->goals_.push_back(dest) ;
497
498
499
500
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
501
502
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
503
	//city
David Cazier's avatar
David Cazier committed
504
	unsigned int dartDistForPath = 50 ;
pitiot's avatar
maj    
pitiot committed
505
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
506
	{
David Cazier's avatar
David Cazier committed
507
508
509
510
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
pitiot's avatar
maj    
pitiot committed
511
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
512
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
513
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
514
		{
David Cazier's avatar
David Cazier committed
515
516
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
517
		}
518

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

521
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
522
523
524
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
525

pitiot's avatar
maj    
pitiot committed
526
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
527
		{
528
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
529
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
530
531
			dest /= 2.0f ;
			dest[2] = 0 ;
532

David Cazier's avatar
David Cazier committed
533
			agents_[i]->goals_.push_back(dest) ;
534
		}
535

David Cazier's avatar
David Cazier committed
536
		Dart dStop2 = dStop ;
pitiot's avatar
maj    
pitiot committed
537
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
538
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
539
540
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
541
		{
David Cazier's avatar
David Cazier committed
542
543
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
544
		}
545

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

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

pitiot's avatar
maj    
pitiot committed
551
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
552
		{
553
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
554
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
555
556
			dest /= 2.0f ;
			dest[2] = 0 ;
557

David Cazier's avatar
David Cazier committed
558
			agents_[i]->goals_.push_back(dest) ;
559
		}
560

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

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

pitiot's avatar
maj    
pitiot committed
566
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
567
		{
568
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
569
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
570
571
			dest /= 2.0f ;
			dest[2] = 0 ;
572

David Cazier's avatar
David Cazier committed
573
			agents_[i]->goals_.push_back(dest) ;
574
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
575
576
	}
}
577

Thomas's avatar
Thomas committed
578
579
580
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
581
	unsigned int dartDistForPath = 50 ;
pitiot's avatar
maj    
pitiot committed
582
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
583
	{
David Cazier's avatar
David Cazier committed
584
585
586
587
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
pitiot's avatar
maj    
pitiot committed
588
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
589
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
590
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
591
		{
David Cazier's avatar
David Cazier committed
592
593
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
594
595
		}

596
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
597
598
599
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
600

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

David Cazier's avatar
David Cazier committed
607
		Dart dStop2 = dStop ;
pitiot's avatar
maj    
pitiot committed
608
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
609
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
610
611
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
612
		{
David Cazier's avatar
David Cazier committed
613
614
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
615
616
		}

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

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

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

pitiot's avatar
maj    
pitiot committed
629
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
630
		{
631
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
632
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
633
634
635
		}
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
636
#endif
Thomas's avatar
Thomas committed
637

Pierre Kraemer's avatar
Pierre Kraemer committed
638
639
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
640
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
641

David Cazier's avatar
David Cazier committed
642
643
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
644
645
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
646
647
	}

David Cazier's avatar
David Cazier committed
648
649
650
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
651
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
652

David Cazier's avatar
David Cazier committed
653
654
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
655
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
656
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
657

David Cazier's avatar
David Cazier committed
658
659
660
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
661

662
			VEC3 pos(x, y, z) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
663
664
665
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, pos) ;
			agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
666
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
667
#else
668
669
			Dart dCell = envMap_.getBelongingCell(pos) ;
			agents_.push_back(new Agent(this, pos, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
670
#endif
671
			agents_.back()->goals_.push_back(pos) ;
David Cazier's avatar
David Cazier committed
672
			agents_.back()->goals_.push_back(-1.0f * pos) ;
673
			agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
674
675
676
		}
	}

David Cazier's avatar
David Cazier committed
677
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
678

David Cazier's avatar
David Cazier committed
679
680
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
681
682
683
684
}

bool Simulator::exportAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
685
	std::ofstream out(filename.c_str(), std::ios::out) ;
David Cazier's avatar
David Cazier committed
686
687
	if (!out.good())
	{
David Cazier's avatar
David Cazier committed
688
689
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
690
691
	}

pitiot's avatar
maj    
pitiot committed
692
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
693
#ifdef SPATIAL_HASHING
694
		out << agents_[i]->pos << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
695
#else
pitiot's avatar
maj    
pitiot committed
696
		out << agents_[i]->part_.getPosition() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
697
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
698

David Cazier's avatar
David Cazier committed
699
700
	out.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
701
702
703
704
}

void Simulator::swapAgentsGoals()
{
David Cazier's avatar
David Cazier committed
705
	unsigned int nbAgents = agents_.size() ;
pitiot's avatar
maj    
pitiot committed
706
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
707
	{
David Cazier's avatar
David Cazier committed
708
		unsigned int r = rand() % nbAgents ;
Thomas's avatar
Thomas committed
709
710
711
712
713
714
715
//		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;
//		}

716
		std::swap(agents_[i]->goals_, agents_[r]->goals_) ;
Thomas's avatar
Thomas committed
717
718
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
719

Thomas's avatar
Thomas committed
720
721
Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
{
David Cazier's avatar
David Cazier committed
722
723
	Geom::BoundingBox<VEC3> bb ;
	unsigned int nbAgents = agents_.size() ;
pitiot's avatar
maj    
pitiot committed
724
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
725
	{
David Cazier's avatar
David Cazier committed
726
		bb.addPoint(agents_[i]->getPosition()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
727
	}
Thomas's avatar
Thomas committed
728

David Cazier's avatar
David Cazier committed
729
	return bb ;
Pierre Kraemer's avatar
Pierre Kraemer committed
730
}