simulator.cpp 20.5 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),
David Cazier's avatar
David Cazier committed
12
	totalNeighbors(0),
pitiot's avatar
pitiot committed
13
14
	avoidance(0),
	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()
{
David Cazier's avatar
David Cazier 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 ;
David Cazier's avatar
David Cazier 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)
	{
David Cazier's avatar
David Cazier committed
34
		case 0 :
David Cazier's avatar
David Cazier committed
35
			setupCircleScenario(100) ;
David Cazier's avatar
David Cazier committed
36
37
			break ;
		case 1 :
38
			setupCorridorScenario(500, 25) ;
David Cazier's avatar
David Cazier committed
39
40
41
42
43
44
			break ;
		case 2 :
			setupScenario(1000) ;
			break ;
		case 3 :
			setupCityScenario(20, 20) ;
pitiot's avatar
pitiot committed
45
46
//			setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10),
//					-1.0f * (12 * (70.0f / 2.0f) - 10), 20, 20);
David Cazier's avatar
David Cazier committed
47
48
49
50
			break ;
		case 4 :
			importAgents("myAgents.pos") ;
			break ;
Thomas's avatar
Thomas committed
51
52
	}

David Cazier's avatar
David Cazier 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
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
David Cazier's avatar
David Cazier committed
62
			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

David Cazier's avatar
David Cazier 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
74
//	envMap_.subdivideToProperLevel() ;
75
//	envMap_.subdivideAllToMaxLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
76
#endif
77

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

David Cazier's avatar
David Cazier committed
81
void Simulator::doStep()
Pierre Kraemer's avatar
Pierre Kraemer committed
82
{
David Cazier's avatar
David Cazier committed
83
84
85
86
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
#endif
David Cazier's avatar
David Cazier committed
87

David Cazier's avatar
David Cazier committed
88
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
pitiot's avatar
pitiot committed
89
	{
David Cazier's avatar
David Cazier committed
90
91
		movingObstacles_[i]->computePrefVelocity() ;
		movingObstacles_[i]->update() ;
pitiot's avatar
pitiot committed
92
93
	}

David Cazier's avatar
David Cazier committed
94
95
96
97
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		agents_[i]->updateObstacleNeighbors() ;

Pierre Kraemer's avatar
Pierre Kraemer committed
98
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
99
100
101
102
103
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
		agents_[i]->updateAgentNeighbors() ;
#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(0) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
104
#endif
105

David Cazier's avatar
David Cazier committed
106
107
108
		agents_[i]->computePrefVelocity() ;
		agents_[i]->computeNewVelocity() ;
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
109

Pierre Kraemer's avatar
Pierre Kraemer committed
110
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
111
112
113
114
115
116
117
118
119
120
121
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		Geom::Vec2ui cold = envMap_.agentPositionCell(agents_[i]) ;
		agents_[i]->update() ;
		Geom::Vec2ui cnew = envMap_.agentPositionCell(agents_[i]) ;
		if (cnew != cold)
		{
			envMap_.removeAgentFromGrid(agents_[i], cold) ;
			envMap_.addAgentInGrid(agents_[i], cnew) ;
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
122
#else
David Cazier's avatar
David Cazier committed
123
124
125
126
127
128
129
130
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
	nb_dead = 0 ;
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		if (agents_[i]->alive)
		{
			Dart oldFace = agents_[i]->part_.d ;
			agents_[i]->update() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
131

David Cazier's avatar
David Cazier committed
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
//		if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE))
			if (agents_[i]->part_.crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//		switch(agents_[i]->part_.crossCell)
			{
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace) ;
//				break;
			}
		}
		else
			nb_dead++ ;

	}

	nbRefineCandidate += envMap_.refineCandidate.size() ;
	nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
151
//	envMap_.updateMap() ;
David Cazier's avatar
David Cazier committed
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
#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) ;
#else
	Dart dCell = envMap_.getBelongingCell(start) ;
	agents_.push_back(new Agent(this, start, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
177
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
178

David Cazier's avatar
David Cazier committed
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
	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 ;

David Cazier's avatar
David Cazier committed
199
	double pi = 3.14159265358979323846f ;
David Cazier's avatar
David Cazier committed
200
201
202
203
204
205
206
207
208

	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) ;
209
	}
Thomas's avatar
Thomas committed
210

David Cazier's avatar
David Cazier committed
211
212
213
214
215
216
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
	{
		movingObstacles_[i]->updateFixedObstNeighbors() ;
		movingObstacles_[i]->computePrefVelocity() ;
		movingObstacles_[i]->update() ;
	}
Thomas's avatar
Thomas committed
217

218
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
219
220
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
221
#endif
David Cazier's avatar
David Cazier committed
222
223

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

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

David Cazier's avatar
David Cazier committed
253
254
		// Un agent sur 2 va de droite à gauche
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
255
256
		if (i % 2 == 1)
		{
David Cazier's avatar
David Cazier committed
257
258
259
			tmp = goal ;
			goal = start ;
			start = tmp ;
260
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
261

David Cazier's avatar
David Cazier committed
262
		addAgent(start, goal) ;
Thomas's avatar
Thomas committed
263
264
	}

David Cazier's avatar
David Cazier committed
265
266
267
268
269
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 ;

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

275
	VEC3 xSide (5.0f,0.0f,0.0f);
276
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
277
278
279
280
281
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
Jund Thomas's avatar
Jund Thomas committed
282
283
		float x = xStartMin + ((int)i*30) % xStartDelta;
		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
David Cazier's avatar
David Cazier committed
284
285
286
		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(x, yGoalMin  + rand() % yGoalDelta,  0) ;

David Cazier's avatar
David Cazier committed
287
		vPos.clear();
David Cazier's avatar
David Cazier committed
288
289
290
291
292
293
294
		// Un obstacle sur deux va vers le haut
		VEC3 tmp ;
		if (i % 2 == 1)
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
David Cazier's avatar
David Cazier committed
295
296
297
298
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
David Cazier's avatar
David Cazier committed
299
		}
David Cazier's avatar
David Cazier committed
300
301
302
303
304
305
306
		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);
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323


		//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);

David Cazier's avatar
David Cazier committed
324
325
		movingObstacles_.push_back(mo4);
	}
Thomas's avatar
Thomas committed
326
327
}

David Cazier's avatar
David Cazier committed
328
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
329
{
David Cazier's avatar
David Cazier committed
330
331
332
333
334
	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
335

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

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

David Cazier's avatar
David Cazier committed
366
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
367
368
}

369
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
370
{
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
//	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
392
	/*
David Cazier's avatar
David Cazier committed
393
394
395
396
	 * Add agents, specifying their start position, and store their goals on the
	 * opposite side of the environment.
	 */
	Dart d = envMap_.map.begin() ;
David Cazier's avatar
David Cazier committed
397
	CellMarker<FACE> filled(envMap_.map) ;
David Cazier's avatar
David Cazier committed
398
399
400
401
402
403

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

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

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

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

443
					agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
444
445
446
447
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
448
449
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
450
}
451

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

David Cazier's avatar
David Cazier committed
475
476
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
477

478
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
479
480
481
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
482

David Cazier's avatar
David Cazier committed
483
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
484
		{
485
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
486
487
488
489
490

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

David Cazier's avatar
David Cazier committed
491
			agents_[i]->goals_.push_back(dest) ;
492
493
494
495
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
496
497
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
498
	//city
David Cazier's avatar
David Cazier committed
499
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
500
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
501
	{
David Cazier's avatar
David Cazier committed
502
503
504
505
		agents_[i]->goals_.clear() ;

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

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

516
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
517
518
519
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
520

David Cazier's avatar
David Cazier committed
521
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
522
		{
523
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
524
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
525
526
			dest /= 2.0f ;
			dest[2] = 0 ;
527

David Cazier's avatar
David Cazier committed
528
			agents_[i]->goals_.push_back(dest) ;
529
		}
530

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

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

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

David Cazier's avatar
David Cazier committed
546
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
547
		{
548
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
549
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
550
551
			dest /= 2.0f ;
			dest[2] = 0 ;
552

David Cazier's avatar
David Cazier committed
553
			agents_[i]->goals_.push_back(dest) ;
554
		}
555

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

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

David Cazier's avatar
David Cazier committed
561
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
562
		{
563
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
564
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
565
566
			dest /= 2.0f ;
			dest[2] = 0 ;
567

David Cazier's avatar
David Cazier committed
568
			agents_[i]->goals_.push_back(dest) ;
569
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
570
571
	}
}
572

Thomas's avatar
Thomas committed
573
574
575
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
576
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
577
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
578
	{
David Cazier's avatar
David Cazier committed
579
580
581
582
		agents_[i]->goals_.clear() ;

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

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

David Cazier's avatar
David Cazier committed
596
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
597
		{
598
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
599
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
600
601
		}

David Cazier's avatar
David Cazier committed
602
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
603
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
604
605
606
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
607
		{
David Cazier's avatar
David Cazier committed
608
609
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
610
611
		}

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

David Cazier's avatar
David Cazier committed
615
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
616
		{
617
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
618
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
619
620
		}

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
633
634
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
635
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
636

David Cazier's avatar
David Cazier committed
637
638
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
639
640
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
641
642
	}

David Cazier's avatar
David Cazier committed
643
644
645
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
646
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
647

David Cazier's avatar
David Cazier committed
648
649
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
650
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
651
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
652

David Cazier's avatar
David Cazier committed
653
654
655
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
656

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

David Cazier's avatar
David Cazier committed
672
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
673

David Cazier's avatar
David Cazier committed
674
675
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
676
677
678
679
}

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

David Cazier's avatar
David Cazier committed
687
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
688
#ifdef SPATIAL_HASHING
689
		out << agents_[i]->pos << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
690
#else
David Cazier's avatar
David Cazier committed
691
		out << agents_[i]->part_.getPosition() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
692
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
693

David Cazier's avatar
David Cazier committed
694
695
	out.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
696
697
698
699
}

void Simulator::swapAgentsGoals()
{
David Cazier's avatar
David Cazier committed
700
	unsigned int nbAgents = agents_.size() ;
David Cazier's avatar
David Cazier committed
701
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
702
	{
David Cazier's avatar
David Cazier committed
703
		unsigned int r = rand() % nbAgents ;
Thomas's avatar
Thomas committed
704
705
706
707
708
709
710
//		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;
//		}

711
		std::swap(agents_[i]->goals_, agents_[r]->goals_) ;
Thomas's avatar
Thomas committed
712
713
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
714

Thomas's avatar
Thomas committed
715
716
Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
{
David Cazier's avatar
David Cazier committed
717
718
	Geom::BoundingBox<VEC3> bb ;
	unsigned int nbAgents = agents_.size() ;
David Cazier's avatar
David Cazier committed
719
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
720
	{
David Cazier's avatar
David Cazier committed
721
		bb.addPoint(agents_[i]->getPosition()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
722
	}
Thomas's avatar
Thomas committed
723

David Cazier's avatar
David Cazier committed
724
	return bb ;
Pierre Kraemer's avatar
Pierre Kraemer committed
725
}