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

David Cazier's avatar
David Cazier committed
3
Simulator::Simulator(unsigned int config, unsigned 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),
David Cazier's avatar
David Cazier committed
12
	totalNeighbors(0),
pitiot's avatar
maj    
pitiot committed
13
	avoidance(1),
pitiot's avatar
pitiot committed
14
	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_ ;
David Cazier's avatar
David Cazier committed
21
	init(config, minSize, 2.0f) ;
Thomas's avatar
Thomas committed
22
23
24
25
}

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

David Cazier's avatar
David Cazier committed
30
void Simulator::init(unsigned int config, float dimension, bool enablePathFinding)
Thomas's avatar
Thomas committed
31
{
32
	std::cout << "Setup scenario" << std::endl ;
Thomas's avatar
Thomas committed
33

David Cazier's avatar
David Cazier committed
34
35
	switch (config)
	{
David Cazier's avatar
David Cazier committed
36
		case 0 :
David Cazier's avatar
David Cazier committed
37
			setupCircleScenario(0, 1000, 35) ;
David Cazier's avatar
David Cazier committed
38
39
			break ;
		case 1 :
David Cazier's avatar
David Cazier committed
40
41
42
43
			setupCorridorScenario(1, 1000,40) ;
			break ;
		case 2 :
//			setupCorridor2Scenario(1, 200,4) ;
David Cazier's avatar
David Cazier committed
44
			break ;
pitiot's avatar
pitiot committed
45
46
47
48
49
50
51
52
53
54
55
//		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 ;
Jund Thomas's avatar
Jund Thomas committed
56
		case 5:
David Cazier's avatar
David Cazier committed
57
			envMap_.init(5, 1600.0f, 1200.0f, minSize, 400.0f) ; //cases fines
Jund Thomas's avatar
Jund Thomas committed
58
			break;
David Cazier's avatar
David Cazier committed
59
60
61
		default:
			std::cout << "Unknown scenario !" << std::endl ;
			exit(1) ;
Thomas's avatar
Thomas committed
62
63
	}

David Cazier's avatar
David Cazier committed
64
65
66
67
68
#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
69
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
70
71
72
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
David Cazier's avatar
David Cazier committed
73
			addPathsToAgents() ;
74
		else if (dimension == 2.5f) addPathsToAgents_height() ;
Thomas's avatar
Thomas committed
75
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
76
#endif
77

David Cazier's avatar
David Cazier committed
78
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
79
	{
David Cazier's avatar
David Cazier committed
80
81
		agents_[i]->updateObstacleNeighbors() ;
		agents_[i]->updateAgentNeighbors() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
82
	}
83

84

Pierre Kraemer's avatar
Pierre Kraemer committed
85
#ifndef SPATIAL_HASHING
86
87
	if (multires)
		envMap_.subdivideToProperLevel() ;
David Cazier's avatar
David Cazier committed
88
	//	envMap_.subdivideAllToMaxLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
89
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
90
91
}

David Cazier's avatar
David Cazier committed
92
void Simulator::doStep()
Pierre Kraemer's avatar
Pierre Kraemer committed
93
{
David Cazier's avatar
David Cazier committed
94
95
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
Jund Thomas's avatar
Jund Thomas committed
96
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
97
#endif
David Cazier's avatar
David Cazier committed
98

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

David Cazier's avatar
David Cazier committed
109
110
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
pitiot's avatar
pitiot committed
111
112
113
		if (agents_[i]->alive)
		{
			agents_[i]->updateObstacleNeighbors() ;
pitiot's avatar
maj    
pitiot committed
114
115

#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
116
			envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
117
#endif
pitiot's avatar
pitiot committed
118
			agents_[i]->updateAgentNeighbors() ;
pitiot's avatar
maj    
pitiot committed
119
#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
120
			envMap_.map.setCurrentLevel(0) ;
pitiot's avatar
maj    
pitiot committed
121
122
#endif

pitiot's avatar
pitiot committed
123
124
125
			agents_[i]->computePrefVelocity() ;
			agents_[i]->computeNewVelocity() ;
		}
David Cazier's avatar
David Cazier committed
126
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
127

Pierre Kraemer's avatar
Pierre Kraemer committed
128
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
129
130
131
132
133
134
135
136
137
138
139
	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
140
#else
141
	if (multires) envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
142
143
144
145
146
147
148
	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
149

David Cazier's avatar
David Cazier committed
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
//		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() ;
Jund Thomas's avatar
Jund Thomas committed
169

170
171
	if (multires)
		envMap_.updateMap() ;
David Cazier's avatar
David Cazier committed
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
#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 ;
}

David Cazier's avatar
David Cazier committed
188
void Simulator::addAgent(const VEC3& start, const std::vector<VEC3>& goals)
David Cazier's avatar
David Cazier committed
189
{
David Cazier's avatar
David Cazier committed
190
	agents_.push_back(new Agent(this, start, goals)) ;
David Cazier's avatar
David Cazier committed
191
192
}

David Cazier's avatar
David Cazier committed
193
void Simulator::setupCircleScenario(unsigned int config, unsigned int nbAgents , unsigned int nbObstacles)
David Cazier's avatar
David Cazier committed
194
{
David Cazier's avatar
David Cazier committed
195
	envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ;
pitiot's avatar
pitiot committed
196

David Cazier's avatar
David Cazier committed
197
198
199
200
201
202
203
204
205
206
207
208
209
210
	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
211
	double pi = 3.14159265358979323846f ;
David Cazier's avatar
David Cazier committed
212
213
214
215
216
217
218

	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 ;
219
220
221
222
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
223
	}
pitiot's avatar
pitiot committed
224
225
226
227
228
	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;
pitiot's avatar
pitiot committed
229
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
David Cazier's avatar
David Cazier committed
230
	{
pitiot's avatar
pitiot committed
231
232
233
234
235
236
237
238
239
240
241
242
		double angle = i * 2.0f * pi / float(nbObstacles/2) ;
		VEC3 v,start;
		if(i<nbObstacles/2)
		{
			v=VEC3 (std::cos(angle) * (2*radius/3), std::sin(angle) * (2*radius/3), 0) ;
		}
		else
		{
			v=VEC3 (std::cos(angle) * (radius/3), std::sin(angle) * (radius/3), 0) ;
		}
		start = center + v ;
		positions.push_back(start);
pitiot's avatar
pitiot committed
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
	}
	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);

pitiot's avatar
pitiot committed
260
		mo4= new MovingObstacle(this, i,vPos,goals,true);
pitiot's avatar
pitiot committed
261

pitiot's avatar
pitiot committed
262
263
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
264

pitiot's avatar
pitiot committed
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
	for (unsigned int i = nbObstacles/2 ; i < nbObstacles ; i++)
	{
		VEC3 start = positions[i];
		std::vector<VEC3> goals;
		for(unsigned int k = (nbObstacles/2) ; k > 0 ; k--)
		{
			goals.push_back(positions[((i-(nbObstacles/2)+k)%(nbObstacles/2))+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,true);
pitiot's avatar
pitiot committed
282
283

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
284
	}
Thomas's avatar
Thomas committed
285

286
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
287
288
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
289
#endif
David Cazier's avatar
David Cazier committed
290
291

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

David Cazier's avatar
David Cazier committed
294
void Simulator::setupCorridorScenario(unsigned int config, unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
295
{
David Cazier's avatar
David Cazier committed
296
	envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ;
pitiot's avatar
pitiot committed
297

David Cazier's avatar
David Cazier committed
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
	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
319
	{
David Cazier's avatar
David Cazier committed
320
321
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;
322

David Cazier's avatar
David Cazier committed
323
324
		// Un agent sur 2 va de droite à gauche
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
325
326
		if (i % 2 == 1)
		{
David Cazier's avatar
David Cazier committed
327
328
329
			tmp = goal ;
			goal = start ;
			start = tmp ;
330
		}
331
332
333
334
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
Thomas's avatar
Thomas committed
335
336
	}

David Cazier's avatar
David Cazier committed
337
338
339
340
341
342
343
344
345
346
	// 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 ;

347
	VEC3 xSide (5.0f,0.0f,0.0f);
348
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
349
350
351
352
353
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
Jund Thomas's avatar
Jund Thomas committed
354
		float x = xStartMin + ((int)i*30) % xStartDelta;
355
//		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
David Cazier's avatar
David Cazier committed
356
357
358
		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(x, yGoalMin  + rand() % yGoalDelta,  0) ;

David Cazier's avatar
David Cazier committed
359
		vPos.clear();
David Cazier's avatar
David Cazier committed
360
361
362
363
364
365
366
		// 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
367
368
369
370
			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
371
		}
David Cazier's avatar
David Cazier committed
372
373
374
375
376
377
		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
378
379
380
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
pitiot's avatar
pitiot committed
381
		mo4= new MovingObstacle(this, i,vPos,goals,false);
382
383
384


		//for generating a random path
pitiot's avatar
pitiot committed
385
386
//		unsigned int dartDistForPath = 50 ;
//		mo4->goals_.clear() ;
387
//		Dart dStart = mo4->registering_part->d;
pitiot's avatar
pitiot committed
388
389
390
391
392
393
394
395
396
397
//		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);
398

David Cazier's avatar
David Cazier committed
399
400
		movingObstacles_.push_back(mo4);
	}
Thomas's avatar
Thomas committed
401
402
}

David Cazier's avatar
David Cazier committed
403
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
404
{
David Cazier's avatar
David Cazier committed
405
406
407
408
409
	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
410

David Cazier's avatar
David Cazier committed
411
	VEC3 posagent(xCornerDown - xBorder / 2, yCornerDown - yBorder / 2, 0.0f) ;
David Cazier's avatar
David Cazier committed
412
413
414
415

	std::vector<VEC3> goals ;
	goals.push_back(-1.0f * posagent) ;
	agents_.push_back(new Agent(this, posagent, goals)) ;
416

David Cazier's avatar
David Cazier committed
417
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
418
	{
David Cazier's avatar
David Cazier committed
419
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
420
		{
421
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
David Cazier's avatar
David Cazier committed
422
423
424
			goals.clear() ;
			goals.push_back(-1.0f * posagent) ;
			agents_.push_back(new Agent(this, posagent, goals)) ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
425
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
426
	}
Thomas's avatar
Thomas committed
427

David Cazier's avatar
David Cazier committed
428
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
429
430
}

431
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
432
433
{
	/*
David Cazier's avatar
David Cazier committed
434
435
436
437
	 * 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
438
	CellMarker<FACE> filled(envMap_.map) ;
David Cazier's avatar
David Cazier committed
439
440
441
442
443
444

	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
445
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
446
	{
David Cazier's avatar
David Cazier committed
447
448
449
		bool found = false ;
		VEC3 pos ;
		Dart dCell ;
David Cazier's avatar
David Cazier committed
450
451
452
453
454
		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
455
				filled.mark(d) ;
456
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
457
458
459
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
460
			}
David Cazier's avatar
David Cazier committed
461
			envMap_.map.next(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
462
463
		}

David Cazier's avatar
David Cazier committed
464
465
		if (found)
		{
466
467
			float ecart = 3.0f ;
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
468
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
David Cazier's avatar
David Cazier committed
469
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
470
			{
David Cazier's avatar
David Cazier committed
471
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
472
				{
473
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
David Cazier's avatar
David Cazier committed
474
475
476
477
					std::vector<VEC3> goals ;
					goals.push_back(-1.0f * posagent) ;
					goals.push_back(1.0f * posagent) ;
					agents_.push_back(new Agent(this, posagent, goals)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
478
479
480
481
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
482
483
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
484
}
485

486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
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
501
#ifndef SPATIAL_HASHING
502
503
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
504
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
505
	{
David Cazier's avatar
David Cazier committed
506
507
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
508

David Cazier's avatar
David Cazier committed
509
510
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
511

512
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
513
514
515
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
516

David Cazier's avatar
David Cazier committed
517
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
518
		{
519
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
520
521
522
523
524

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
530
531
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
532
	//city
David Cazier's avatar
David Cazier committed
533
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
534
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
535
	{
David Cazier's avatar
David Cazier committed
536
537
538
539
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
540
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
541
542
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
543
		{
David Cazier's avatar
David Cazier committed
544
545
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
546
		}
547

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

550
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
551
552
553
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
554

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

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

David Cazier's avatar
David Cazier committed
565
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
566
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
567
568
569
		    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
570
		{
David Cazier's avatar
David Cazier committed
571
572
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
573
		}
574

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

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

David Cazier's avatar
David Cazier committed
580
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
581
		{
582
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
583
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
584
585
			dest /= 2.0f ;
			dest[2] = 0 ;
586

David Cazier's avatar
David Cazier committed
587
			agents_[i]->goals_.push_back(dest) ;
588
		}
589

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

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

David Cazier's avatar
David Cazier committed
595
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
596
		{
597
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
598
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
599
600
			dest /= 2.0f ;
			dest[2] = 0 ;
601

David Cazier's avatar
David Cazier committed
602
			agents_[i]->goals_.push_back(dest) ;
603
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
604
605
	}
}
606

Thomas's avatar
Thomas committed
607
608
609
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
610
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
611
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
612
	{
David Cazier's avatar
David Cazier committed
613
614
615
616
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
617
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
618
619
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
620
		{
David Cazier's avatar
David Cazier committed
621
622
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
623
624
		}

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

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

David Cazier's avatar
David Cazier committed
636
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
637
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
638
639
640
		    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
641
		{
David Cazier's avatar
David Cazier committed
642
643
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
644
645
		}

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

David Cazier's avatar
David Cazier committed
649
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
650
		{
651
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
652
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
653
654
		}

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

David Cazier's avatar
David Cazier committed
658
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
659
		{
660
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
661
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
662
663
664
		}
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
665
#endif
Thomas's avatar
Thomas committed
666

Pierre Kraemer's avatar
Pierre Kraemer committed
667
668
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
669
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
670

David Cazier's avatar
David Cazier committed
671
672
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
673
674
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
675
676
	}

David Cazier's avatar
David Cazier committed
677
678
	std::vector<VEC3> goals ;

David Cazier's avatar
David Cazier committed
679
680
681
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
682
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
683

David Cazier's avatar
David Cazier committed
684
685
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
686
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
687
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
688

David Cazier's avatar
David Cazier committed
689
690
691
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
692

693
			VEC3 pos(x, y, z) ;
David Cazier's avatar
David Cazier committed
694
695
696
697
			goals.clear() ;
			goals.push_back(-1.0f * pos) ;
			goals.push_back(pos) ;
			agents_.push_back(new Agent(this, pos, goals)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
698
699
700
		}
	}

David Cazier's avatar
David Cazier committed
701
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
702

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

bool Simulator::exportAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
709
	std::ofstream out(filename.c_str(), std::ios::out) ;
David Cazier's avatar
David Cazier committed
710
711
	if (!out.good())
	{
David Cazier's avatar
David Cazier committed
712
713
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
714
715
	}

David Cazier's avatar
David Cazier committed
716
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
717
		out << agents_[i]->getPosition() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
718

David Cazier's avatar
David Cazier committed
719
720
	out.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
721
722
723
724
}

void Simulator::swapAgentsGoals()
{
David Cazier's avatar
David Cazier committed
725
	unsigned int nbAgents = agents_.size() ;
David Cazier's avatar
David Cazier committed
726
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
727
	{
David Cazier's avatar
David Cazier committed
728
		unsigned int r = rand() % nbAgents ;
729
		std::swap(agents_[i]->goals_, agents_[r]->goals_) ;
Thomas's avatar
Thomas committed
730
731
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
732

Thomas's avatar
Thomas committed
733
734
Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
{
David Cazier's avatar
David Cazier committed
735
736
	Geom::BoundingBox<VEC3> bb ;
	unsigned int nbAgents = agents_.size() ;
David Cazier's avatar
David Cazier committed
737
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
738
	{
David Cazier's avatar
David Cazier committed
739
		bb.addPoint(agents_[i]->getPosition()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
740
	}
David Cazier's avatar
David Cazier committed
741
	return bb ;
Pierre Kraemer's avatar
Pierre Kraemer committed
742
}