simulator.cpp 19.6 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
38
39
40
41
42
43
44
45
46
47
48
49
50
		case 0 :
			setupCircleScenario(100) ;
			break ;
		case 1 :
			setupCorridorScenario(500, 25) ;
			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
85
86
87
88
89
90
91
92
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
#endif

	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
	{
		movingObstacles_[i]->computePrefVelocity() ;
		movingObstacles_[i]->update() ;
	}
93

pitiot's avatar
maj    
pitiot committed
94
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
95
	{
pitiot's avatar
maj    
pitiot committed
96
97
98
99
100
101
102
103
104
105
106
107
108
		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
109

Pierre Kraemer's avatar
Pierre Kraemer committed
110
#ifdef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
	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
128
		{
pitiot's avatar
maj    
pitiot committed
129
130
131
132
133
134
			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
135
			{
pitiot's avatar
maj    
pitiot committed
136
137
138
139
140
141
//			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
142
143
			}
		}
pitiot's avatar
maj    
pitiot committed
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
		else
			nb_dead++ ;

	}

	nbRefineCandidate += envMap_.refineCandidate.size() ;
	nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
//	envMap_.updateMap() ;
#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
174
#else
pitiot's avatar
maj    
pitiot committed
175
176
	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

pitiot's avatar
maj    
pitiot committed
179
180
181
182
183
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
	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) ;
209
	}
Thomas's avatar
Thomas committed
210

pitiot's avatar
maj    
pitiot 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
pitiot's avatar
maj    
pitiot committed
219
220
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
221
#endif
pitiot's avatar
maj    
pitiot committed
222
223

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

pitiot's avatar
maj    
pitiot committed
226
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
227
{
pitiot's avatar
maj    
pitiot 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
	{
pitiot's avatar
maj    
pitiot committed
250
251
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;
252

pitiot's avatar
maj    
pitiot 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)
		{
pitiot's avatar
maj    
pitiot committed
257
258
259
			tmp = goal ;
			goal = start ;
			start = tmp ;
Pierre Kraemer's avatar
Pierre Kraemer committed
260
261
		}

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

pitiot's avatar
maj    
pitiot committed
265
266
267
268
269
	// 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
270

pitiot's avatar
maj    
pitiot committed
271
272
273
274
275
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
	// 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);
		movingObstacles_.push_back(mo4);
	}
Thomas's avatar
Thomas committed
309
310
}

pitiot's avatar
maj    
pitiot committed
311
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
312
{
pitiot's avatar
maj    
pitiot committed
313
314
315
316
317
	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
318

pitiot's avatar
maj    
pitiot committed
319
	VEC3 posagent(xCornerDown - xBorder / 2, yCornerDown - yBorder / 2, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
320
321
322
#ifdef SPATIAL_HASHING
	Agent* a = new Agent(this, posagent) ;
	agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
323
	envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
324
#else
325
326
	Dart dCell = envMap_.getBelongingCell(posagent) ;
	agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
327
#endif
David Cazier's avatar
David Cazier committed
328
	agents_.back()->goals_.push_back(-1.0f * posagent) ;
329
	agents_.back()->curGoal_ = 0 ;
330

pitiot's avatar
maj    
pitiot committed
331
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
332
	{
pitiot's avatar
maj    
pitiot committed
333
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
334
		{
335
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
336
337
338
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, posagent) ;
			agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
339
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
340
#else
341
342
			Dart dCell = envMap_.getBelongingCell(posagent) ;
			agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
343
#endif
David Cazier's avatar
David Cazier committed
344
			agents_.back()->goals_.push_back(-1.0f * posagent) ;
345
			agents_.back()->curGoal_ = 0 ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
346
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
347
	}
Thomas's avatar
Thomas committed
348

David Cazier's avatar
David Cazier committed
349
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
350
351
}

352
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
353
{
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
//	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
375
	/*
David Cazier's avatar
David Cazier committed
376
377
378
379
	 * 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
380
	CellMarker<FACE> filled(envMap_.map) ;
David Cazier's avatar
David Cazier committed
381
382
383
384
385
386

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

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

pitiot's avatar
maj    
pitiot committed
387
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
388
	{
David Cazier's avatar
David Cazier committed
389
390
391
		bool found = false ;
		VEC3 pos ;
		Dart dCell ;
David Cazier's avatar
David Cazier committed
392
393
394
395
396
		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
397
				filled.mark(d) ;
398
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
399
400
401
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
402
			}
David Cazier's avatar
David Cazier committed
403
			envMap_.map.next(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
404
405
		}

David Cazier's avatar
David Cazier committed
406
407
		if (found)
		{
408
409
			float ecart = 3.0f ;
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
410
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
pitiot's avatar
maj    
pitiot committed
411
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
412
			{
pitiot's avatar
maj    
pitiot committed
413
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
414
				{
415
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
416
417
418
#ifdef SPATIAL_HASHING
					Agent* a = new Agent(this, posagent) ;
					agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
419
					envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
420
#else
421
					agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
422
#endif
423
					agents_.back()->goals_.push_back(posagent) ;
David Cazier's avatar
David Cazier committed
424
					agents_.back()->goals_.push_back(-1.0f * posagent) ;
pitiot's avatar
maj    
pitiot committed
425

426
					agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
427
428
429
430
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
431
432
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
433
}
434

Pierre Kraemer's avatar
Pierre Kraemer committed
435
#ifndef SPATIAL_HASHING
436
437
void Simulator::addPathToCorner()
{
pitiot's avatar
maj    
pitiot committed
438
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
439
	{
David Cazier's avatar
David Cazier committed
440
441
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
442

David Cazier's avatar
David Cazier committed
443
444
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
445

446
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
447
448
449
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
450

pitiot's avatar
maj    
pitiot committed
451
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
452
		{
453
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
454
455
456
457
458

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

David Cazier's avatar
David Cazier committed
459
			agents_[i]->goals_.push_back(dest) ;
460
461
462
463
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
464
465
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
466
	//city
David Cazier's avatar
David Cazier committed
467
	unsigned int dartDistForPath = 50 ;
pitiot's avatar
maj    
pitiot committed
468
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
469
	{
David Cazier's avatar
David Cazier committed
470
471
472
473
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
pitiot's avatar
maj    
pitiot committed
474
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
475
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
476
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
477
		{
David Cazier's avatar
David Cazier committed
478
479
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
480
		}
481

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

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

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

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

David Cazier's avatar
David Cazier committed
499
		Dart dStop2 = dStop ;
pitiot's avatar
maj    
pitiot committed
500
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
501
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
502
503
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
504
		{
David Cazier's avatar
David Cazier committed
505
506
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
507
		}
508

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

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

pitiot's avatar
maj    
pitiot committed
514
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
515
		{
516
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
517
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
518
519
			dest /= 2.0f ;
			dest[2] = 0 ;
520

David Cazier's avatar
David Cazier committed
521
			agents_[i]->goals_.push_back(dest) ;
522
		}
523

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

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

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

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

Thomas's avatar
Thomas committed
541
542
543
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
544
	unsigned int dartDistForPath = 50 ;
pitiot's avatar
maj    
pitiot committed
545
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
546
	{
David Cazier's avatar
David Cazier committed
547
548
549
550
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
pitiot's avatar
maj    
pitiot committed
551
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
552
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
553
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
554
		{
David Cazier's avatar
David Cazier committed
555
556
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
557
558
		}

559
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
560
561
562
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
563

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

David Cazier's avatar
David Cazier committed
570
		Dart dStop2 = dStop ;
pitiot's avatar
maj    
pitiot committed
571
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
572
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
573
574
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
575
		{
David Cazier's avatar
David Cazier committed
576
577
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
578
579
		}

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

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

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
601
602
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
603
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
604

David Cazier's avatar
David Cazier committed
605
606
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
607
608
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
609
610
	}

David Cazier's avatar
David Cazier committed
611
612
613
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
614
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
615

David Cazier's avatar
David Cazier committed
616
617
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
618
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
619
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
620

David Cazier's avatar
David Cazier committed
621
622
623
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
624

625
			VEC3 pos(x, y, z) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
626
627
628
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, pos) ;
			agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
629
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
630
#else
631
632
			Dart dCell = envMap_.getBelongingCell(pos) ;
			agents_.push_back(new Agent(this, pos, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
633
#endif
634
			agents_.back()->goals_.push_back(pos) ;
David Cazier's avatar
David Cazier committed
635
			agents_.back()->goals_.push_back(-1.0f * pos) ;
636
			agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
637
638
639
		}
	}

David Cazier's avatar
David Cazier committed
640
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
641

David Cazier's avatar
David Cazier committed
642
643
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
644
645
646
647
}

bool Simulator::exportAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
648
	std::ofstream out(filename.c_str(), std::ios::out) ;
David Cazier's avatar
David Cazier committed
649
650
	if (!out.good())
	{
David Cazier's avatar
David Cazier committed
651
652
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
653
654
	}

pitiot's avatar
maj    
pitiot committed
655
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
656
#ifdef SPATIAL_HASHING
657
		out << agents_[i]->pos << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
658
#else
pitiot's avatar
maj    
pitiot committed
659
		out << agents_[i]->part_.getPosition() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
660
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
661

David Cazier's avatar
David Cazier committed
662
663
	out.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
664
665
666
667
}

void Simulator::swapAgentsGoals()
{
David Cazier's avatar
David Cazier committed
668
	unsigned int nbAgents = agents_.size() ;
pitiot's avatar
maj    
pitiot committed
669
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
670
	{
David Cazier's avatar
David Cazier committed
671
		unsigned int r = rand() % nbAgents ;
Thomas's avatar
Thomas committed
672
673
674
675
676
677
678
//		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;
//		}

679
		std::swap(agents_[i]->goals_, agents_[r]->goals_) ;
Thomas's avatar
Thomas committed
680
681
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
682

Thomas's avatar
Thomas committed
683
684
Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
{
David Cazier's avatar
David Cazier committed
685
686
	Geom::BoundingBox<VEC3> bb ;
	unsigned int nbAgents = agents_.size() ;
pitiot's avatar
maj    
pitiot committed
687
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
688
	{
David Cazier's avatar
David Cazier committed
689
		bb.addPoint(agents_[i]->getPosition()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
690
	}
Thomas's avatar
Thomas committed
691

David Cazier's avatar
David Cazier committed
692
	return bb ;
Pierre Kraemer's avatar
Pierre Kraemer committed
693
}