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.9 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
	multires=true;
17
18
	srand(10) ;
	nbStepsPerUnit_ = 1 / timeStep_ ;
David Cazier's avatar
David Cazier committed
19
	init(1, minSize, 2.0f) ;
Thomas's avatar
Thomas committed
20
21
22
23
}

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

David Cazier's avatar
David Cazier committed
28
void Simulator::init(unsigned int config, int minSize, float dimension, bool enablePathFinding)
Thomas's avatar
Thomas committed
29
{
30
	std::cout << "Setup scenario" << std::endl ;
31
	envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ;
Thomas's avatar
Thomas committed
32

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

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

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

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

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

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

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

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

	}

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

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

185
	agents_.back()->goals_=goal ;
pitiot's avatar
maj    
pitiot committed
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
	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 ;
213
214
215
216
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
217
	}
Thomas's avatar
Thomas committed
218

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

226
#ifndef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
227
228
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
229
#endif
pitiot's avatar
maj    
pitiot committed
230
231

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

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

pitiot's avatar
maj    
pitiot committed
261
262
		// Un agent sur 2 va de droite à gauche
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
263
264
		if (i % 2 == 1)
		{
pitiot's avatar
maj    
pitiot committed
265
266
267
			tmp = goal ;
			goal = start ;
			start = tmp ;
Pierre Kraemer's avatar
Pierre Kraemer committed
268
		}
269
270
271
272
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
Thomas's avatar
Thomas committed
273
274
	}

pitiot's avatar
maj    
pitiot committed
275
276
277
278
279
	// 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
280

pitiot's avatar
maj    
pitiot committed
281
282
283
284
285
286
287
288
289
290
291
292
	// 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;
293
//		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
pitiot's avatar
maj    
pitiot committed
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
		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
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333


		//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
334
335
		movingObstacles_.push_back(mo4);
	}
Thomas's avatar
Thomas committed
336
337
}

pitiot's avatar
maj    
pitiot committed
338
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
339
{
pitiot's avatar
maj    
pitiot committed
340
341
342
343
344
	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
345

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

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

David Cazier's avatar
David Cazier committed
376
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
377
378
}

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

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

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

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

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

453
					agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
454
455
456
457
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
458
459
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
460
}
461

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

David Cazier's avatar
David Cazier committed
485
486
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
487

488
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
489
490
491
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
492

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

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

David Cazier's avatar
David Cazier committed
501
			agents_[i]->goals_.push_back(dest) ;
502
503
504
505
		}
	}
}

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

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

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

526
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
527
528
529
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
530

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

David Cazier's avatar
David Cazier committed
538
			agents_[i]->goals_.push_back(dest) ;
539
		}
540

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

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

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

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

David Cazier's avatar
David Cazier committed
563
			agents_[i]->goals_.push_back(dest) ;
564
		}
565

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

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

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

David Cazier's avatar
David Cazier committed
578
			agents_[i]->goals_.push_back(dest) ;
579
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
580
581
	}
}
582

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

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

601
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
602
603
604
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
605

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

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

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

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

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
643
644
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
645
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
646

David Cazier's avatar
David Cazier committed
647
648
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
649
650
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
651
652
	}

David Cazier's avatar
David Cazier committed
653
654
655
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
656
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
657

David Cazier's avatar
David Cazier committed
658
659
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
660
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
661
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
662

David Cazier's avatar
David Cazier committed
663
664
665
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
666

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

David Cazier's avatar
David Cazier committed
682
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
683

David Cazier's avatar
David Cazier committed
684
685
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
686
687
688
689
}

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

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

David Cazier's avatar
David Cazier committed
704
705
	out.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
706
707
708
709
}

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

721
		std::swap(agents_[i]->goals_, agents_[r]->goals_) ;
Thomas's avatar
Thomas committed
722
723
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
724

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

David Cazier's avatar
David Cazier committed
734
	return bb ;
Pierre Kraemer's avatar
Pierre Kraemer committed
735
}