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

pitiot's avatar
pitiot committed
3
Simulator::Simulator(int minS) :
David Cazier's avatar
David Cazier committed
4
5
6
7
8
9
10
11
	timeStep_(0.2f),
	globalTime_(0.0f),
	nbSteps_(0),
	nbUpdates(0),
	nbSorts(0),
	nbRefineCandidate(0),
	nbCoarsenCandidate(0),
	nearNeighbors(0),
pitiot's avatar
maj    
pitiot committed
12
13
14
	totalNeighbors(0),
	avoidance(1),
	nb_dead(0)
Pierre Kraemer's avatar
Pierre Kraemer committed
15
{
pitiot's avatar
pitiot committed
16
	minSize=minS;
pitiot's avatar
pitiot committed
17
	multires=true;
pitiot's avatar
pitiot committed
18
	detect_agent_collision=false;
19
20
	srand(10) ;
	nbStepsPerUnit_ = 1 / timeStep_ ;
pitiot's avatar
pitiot committed
21
22
	config=0;
	init( minSize, 2.0f) ;
Thomas's avatar
Thomas committed
23
24
25
26
}

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

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

David Cazier's avatar
David Cazier committed
35
36
	switch (config)
	{
pitiot's avatar
maj    
pitiot committed
37
		case 0 :
pitiot's avatar
pitiot committed
38
			setupCircleScenario(1000,40) ;
pitiot's avatar
maj    
pitiot committed
39
40
			break ;
		case 1 :
pitiot's avatar
pitiot committed
41
			setupCorridorScenario(1000,40) ;
pitiot's avatar
maj    
pitiot committed
42
			break ;
pitiot's avatar
pitiot committed
43
44
45
46
47
48
49
50
51
52
53
//		case 2 :
//			setupScenario(1000) ;
//			break ;
//		case 3 :
//			setupCityScenario(20, 20) ;
////			setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10),
////					-1.0f * (12 * (70.0f / 2.0f) - 10), 20, 20);
//			break ;
//		case 4 :
//			importAgents("myAgents.pos") ;
//			break ;
Thomas's avatar
Thomas committed
54
55
	}

pitiot's avatar
maj    
pitiot committed
56
57
58
59
60
#ifndef SPATIAL_HASHING
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
#endif

Pierre Kraemer's avatar
Pierre Kraemer committed
61
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
62
63
64
65
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
			addPathsToAgents() ;
66
		else if (dimension == 2.5f) addPathsToAgents_height() ;
Thomas's avatar
Thomas committed
67
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
68
#endif
69

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

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

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

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

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

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

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

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

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

	}

	nbRefineCandidate += envMap_.refineCandidate.size() ;
	nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
163
	if (multires) envMap_.updateMap() ;
pitiot's avatar
maj    
pitiot committed
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
#endif

	globalTime_ += timeStep_ ;
	++nbSteps_ ;
}

bool Simulator::reachedGoal()
{
	/* Check if all agents have reached their goals. */
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
		if ((agents_[i]->getPosition() - agents_[i]->goals_[agents_[i]->curGoal_]).norm2()
		    > agents_[i]->radius_ * agents_[i]->radius_) return false ;

	return true ;
}

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

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

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

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

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

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

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

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

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

	double pi = 3.14159265358979323846f ;

	for (unsigned int i = 0 ; i < nbAgents ; ++i)
	{
		double angle = i * 2.0f * pi / float(nbAgents) ;
		VEC3 v(std::cos(angle) * radius, std::sin(angle) * radius, 0) ;
		VEC3 start = center + v ;
		VEC3 goal = center - v ;
231
232
233
234
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
235
	}
pitiot's avatar
pitiot committed
236
237
238
239
240
241
	VEC3 xSide (5.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,10.0f,0.0f);
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;
	std::vector<VEC3> positions;
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
pitiot's avatar
maj    
pitiot committed
242
	{
pitiot's avatar
pitiot committed
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
			double angle = i * 2.0f * pi / float(nbObstacles/2) ;
			VEC3 v(std::cos(angle) * (2*radius/3), std::sin(angle) * (2*radius/3), 0) ;
			VEC3 start = center + v ;
			positions.push_back(start);
	}
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
		std::vector<VEC3> goals;
		for(unsigned int k = 0 ; k < nbObstacles/2 ; k++)
		{
			goals.push_back(positions[(i+k)%(nbObstacles/2)]);
		}

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		vPos.push_back(start+xSide-ySide);
		vPos.push_back(start+xSide+ySide);
		vPos.push_back(start-xSide+ySide);
		vPos.push_back(start-xSide-ySide);

		mo4= new MovingObstacle(this, i,vPos,goals,0);


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

		movingObstacles_.push_back(mo4);
pitiot's avatar
maj    
pitiot committed
283
	}
Thomas's avatar
Thomas committed
284

285
#ifndef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
286
287
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
288
#endif
pitiot's avatar
maj    
pitiot committed
289
290

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

pitiot's avatar
maj    
pitiot committed
293
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
294
{
pitiot's avatar
pitiot committed
295
296
297
298
299
300
301
302
303
304
305
	if (multires)
	{
		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
	}
	else
	{

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


pitiot's avatar
maj    
pitiot committed
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
	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
327
	{
pitiot's avatar
maj    
pitiot committed
328
329
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;
330

pitiot's avatar
maj    
pitiot committed
331
332
		// Un agent sur 2 va de droite à gauche
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
333
334
		if (i % 2 == 1)
		{
pitiot's avatar
maj    
pitiot committed
335
336
337
			tmp = goal ;
			goal = start ;
			start = tmp ;
Pierre Kraemer's avatar
Pierre Kraemer committed
338
		}
339
340
341
342
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
Thomas's avatar
Thomas committed
343
344
	}

pitiot's avatar
maj    
pitiot committed
345
346
347
348
349
	// 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
350

pitiot's avatar
maj    
pitiot committed
351
352
353
354
355
356
357
358
359
360
361
362
	// 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;
363
//		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
pitiot's avatar
maj    
pitiot committed
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(x, yGoalMin  + rand() % yGoalDelta,  0) ;

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		VEC3 tmp ;
		if (i % 2 == 1)
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
		}
		else {
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
		}
pitiot's avatar
pitiot committed
386
387
388
389
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		mo4= new MovingObstacle(this, i,vPos,goals,0);
pitiot's avatar
pitiot committed
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406


		//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
407
408
		movingObstacles_.push_back(mo4);
	}
Thomas's avatar
Thomas committed
409
410
}

pitiot's avatar
maj    
pitiot committed
411
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
412
{
pitiot's avatar
maj    
pitiot committed
413
414
415
416
417
	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
418

pitiot's avatar
maj    
pitiot committed
419
	VEC3 posagent(xCornerDown - xBorder / 2, yCornerDown - yBorder / 2, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
420
421
422
#ifdef SPATIAL_HASHING
	Agent* a = new Agent(this, posagent) ;
	agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
423
	envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
424
#else
425
426
	Dart dCell = envMap_.getBelongingCell(posagent) ;
	agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
427
#endif
David Cazier's avatar
David Cazier committed
428
	agents_.back()->goals_.push_back(-1.0f * posagent) ;
429
	agents_.back()->curGoal_ = 0 ;
430

pitiot's avatar
maj    
pitiot committed
431
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
432
	{
pitiot's avatar
maj    
pitiot committed
433
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
434
		{
435
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
436
437
438
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, posagent) ;
			agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
439
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
440
#else
441
442
			Dart dCell = envMap_.getBelongingCell(posagent) ;
			agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
443
#endif
David Cazier's avatar
David Cazier committed
444
			agents_.back()->goals_.push_back(-1.0f * posagent) ;
445
			agents_.back()->curGoal_ = 0 ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
446
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
447
	}
Thomas's avatar
Thomas committed
448

David Cazier's avatar
David Cazier committed
449
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
450
451
}

452
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
453
{
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
//	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
475
	/*
David Cazier's avatar
David Cazier committed
476
477
478
479
	 * 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
480
	CellMarker<FACE> filled(envMap_.map) ;
David Cazier's avatar
David Cazier committed
481
482
483
484
485
486

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

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

pitiot's avatar
maj    
pitiot committed
487
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
488
	{
David Cazier's avatar
David Cazier committed
489
490
491
		bool found = false ;
		VEC3 pos ;
		Dart dCell ;
David Cazier's avatar
David Cazier committed
492
493
494
495
496
		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
497
				filled.mark(d) ;
498
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
499
500
501
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
502
			}
David Cazier's avatar
David Cazier committed
503
			envMap_.map.next(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
504
505
		}

David Cazier's avatar
David Cazier committed
506
507
		if (found)
		{
508
509
			float ecart = 3.0f ;
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
510
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
pitiot's avatar
maj    
pitiot committed
511
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
512
			{
pitiot's avatar
maj    
pitiot committed
513
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
514
				{
515
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
516
517
518
#ifdef SPATIAL_HASHING
					Agent* a = new Agent(this, posagent) ;
					agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
519
					envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
520
#else
521
					agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
522
#endif
523
					agents_.back()->goals_.push_back(posagent) ;
David Cazier's avatar
David Cazier committed
524
					agents_.back()->goals_.push_back(-1.0f * posagent) ;
pitiot's avatar
maj    
pitiot committed
525

526
					agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
527
528
529
530
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
531
532
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
533
}
534

pitiot's avatar
pitiot committed
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
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
550
#ifndef SPATIAL_HASHING
551
552
void Simulator::addPathToCorner()
{
pitiot's avatar
maj    
pitiot committed
553
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
554
	{
David Cazier's avatar
David Cazier committed
555
556
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
557

David Cazier's avatar
David Cazier committed
558
559
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
560

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

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

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

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

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

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

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

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

pitiot's avatar
maj    
pitiot committed
604
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
605
		{
606
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
607
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
608
609
			dest /= 2.0f ;
			dest[2] = 0 ;
610

David Cazier's avatar
David Cazier committed
611
			agents_[i]->goals_.push_back(dest) ;
612
		}
613

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

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

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

pitiot's avatar
maj    
pitiot committed
629
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
630
		{
631
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
632
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
633
634
			dest /= 2.0f ;
			dest[2] = 0 ;
635

David Cazier's avatar
David Cazier committed
636
			agents_[i]->goals_.push_back(dest) ;
637
		}
638

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

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

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

David Cazier's avatar
David Cazier committed
651
			agents_[i]->goals_.push_back(dest) ;
652
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
653
654
	}
}
655

Thomas's avatar
Thomas committed
656
657
658
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
659
	unsigned int dartDistForPath = 50 ;
pitiot's avatar
maj    
pitiot committed
660
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
661
	{
David Cazier's avatar
David Cazier committed
662
663
664
665
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
pitiot's avatar
maj    
pitiot committed
666
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
667
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
668
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
669
		{
David Cazier's avatar
David Cazier committed
670
671
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
672
673
		}

674
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
675
676
677
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
678

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

David Cazier's avatar
David Cazier committed
685
		Dart dStop2 = dStop ;
pitiot's avatar
maj    
pitiot committed
686
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
687
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
688
689
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
690
		{
David Cazier's avatar
David Cazier committed
691
692
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
693
694
		}

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

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

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
716
717
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
718
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
719

David Cazier's avatar
David Cazier committed
720
721
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
722
723
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
724
725
	}

David Cazier's avatar
David Cazier committed
726
727
728
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
729
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
730

David Cazier's avatar
David Cazier committed
731
732
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
733
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
734
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
735

David Cazier's avatar
David Cazier committed
736
737
738
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
739

740
			VEC3 pos(x, y, z) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
741
742
743
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, pos) ;
			agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
744
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
745
#else
746
747
			Dart dCell = envMap_.getBelongingCell(pos) ;
			agents_.push_back(new Agent(this, pos, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
748
#endif
749
			agents_.back()->goals_.push_back(pos) ;
David Cazier's avatar
David Cazier committed
750
			agents_.back()->goals_.push_back(-1.0f * pos) ;
751
			agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
752
753
754
		}
	}

David Cazier's avatar
David Cazier committed
755
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
756

David Cazier's avatar
David Cazier committed
757
758
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
759
760
761
762
}

bool Simulator::exportAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
763
	std::ofstream out(filename.c_str(), std::ios::out) ;
David Cazier's avatar
David Cazier committed
764
765
	if (!out.good())
	{
David Cazier's avatar
David Cazier committed
766
767
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
768
769
	}

pitiot's avatar
maj    
pitiot committed
770
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
771
#ifdef SPATIAL_HASHING
772
		out << agents_[i]->pos << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
773
#else
pitiot's avatar
maj    
pitiot committed
774
		out << agents_[i]->part_.getPosition() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
775
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
776

David Cazier's avatar
David Cazier committed
777
778
	out.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
779
780
781
782
}

void Simulator::swapAgentsGoals()
{
David Cazier's avatar
David Cazier committed
783
	unsigned int nbAgents = agents_.size() ;
pitiot's avatar
maj    
pitiot committed
784
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
785
	{
David Cazier's avatar
David Cazier committed
786
		unsigned int r = rand() % nbAgents ;
Thomas's avatar
Thomas committed
787
788
789
790
791
792
793
//		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;
//		}

794
		std::swap(agents_[i]->goals_, agents_[r]->goals_) ;
Thomas's avatar
Thomas committed
795
796
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
797

Thomas's avatar
Thomas committed
798
799
Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
{
David Cazier's avatar
David Cazier committed
800
801
	Geom::BoundingBox<VEC3> bb ;
	unsigned int nbAgents = agents_.size() ;
pitiot's avatar
maj    
pitiot committed
802
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
803
	{
David Cazier's avatar
David Cazier committed
804
		bb.addPoint(agents_[i]->getPosition()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
805
	}
Thomas's avatar
Thomas committed
806

David Cazier's avatar
David Cazier committed
807
	return bb ;
Pierre Kraemer's avatar
Pierre Kraemer committed
808
}