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

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

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

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

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

David Cazier's avatar
David Cazier committed
53
54
55
56
57
#ifndef SPATIAL_HASHING
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
#endif

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
73
#ifndef SPATIAL_HASHING
74
	envMap_.subdivideToProperLevel() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
75
#endif
76

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

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

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

David Cazier's avatar
David Cazier committed
93
94
95
96
97

	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		agents_[i]->updateObstacleNeighbors() ;

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

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

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

David Cazier's avatar
David Cazier committed
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
//		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() ;
	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) ;
#else
	Dart dCell = envMap_.getBelongingCell(start) ;
	agents_.push_back(new Agent(this, start, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
177
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
178

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

	float 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

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

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

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

David Cazier's avatar
David Cazier committed
226
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
227
{
David Cazier's avatar
David Cazier committed
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
	std::cout << " - Setup Corridor Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles" << std::endl ;

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

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

	// Départ des agents du quart gauche sur toute la hauteur
	int xStartMin = envMap_.geometry.min()[0] + xBorder ;
	int xStartDelta = envMap_.geometry.size(0) / 5 ;
	int yStartMin = envMap_.geometry.min()[1] + yBorder ;
	int yStartDelta = envMap_.geometry.size(1) - 2 * yBorder ;

	// Arrivée des agents à l'opposée
	int xGoalDelta = envMap_.geometry.size(0) / 5 ;
	int xGoalMin = envMap_.geometry.max()[0] - xBorder - xGoalDelta ;
	int yGoalMin = yStartMin ;
	int yGoalDelta = yStartDelta ;

	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
249
	{
David Cazier's avatar
David Cazier committed
250
251
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;
252

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

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

David Cazier's avatar
David Cazier committed
265
	std::cout << "nombre d'agents : " << agents_.size() << std::endl ;
Thomas's avatar
Thomas committed
266

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

	VEC3 xSide (10.0f,0.0f,0.0f);
	VEC3 ySide (05.0f,0.0f,0.0f);
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
		float x = xStartMin + rand() % xStartDelta;
		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(x, yGoalMin  + rand() % yGoalDelta,  0) ;

		// Un obstacle sur deux va vers le haut
		VEC3 tmp ;
		if (i % 2 == 1)
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
		}

		vPos.clear();
		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(i,&envMap_, vPos,goal,0);
		movingObstacles_.push_back(mo4);
	}
	std::cout << "nombre d'obstacles : " << movingObstacles_.size() << std::endl ;
Thomas's avatar
Thomas committed
307
308
}

David Cazier's avatar
David Cazier committed
309
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
310
{
David Cazier's avatar
David Cazier committed
311
312
313
314
315
	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
316

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

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

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

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

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

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

// TODO Code Thomas à vérifier
//					VEC3 oppos = -1.0f * posagent;
//					Dart d1 =envMap_.getBelongingCell(oppos);
//
//					if (!envMap_.buildingMark.isMarked(d1))
//						agents_.back()->goals_.push_back(oppos);
//					else if (oppos[0]+50 <envMap_.max_for_obstacles && oppos[1]+50 <envMap_.max_for_obstacles)
//						agents_.back()->goals_.push_back(oppos + VEC3(50,50,0));
//					else
//						agents_.back()->goals_.push_back(oppos + VEC3(-50,-50,0));
434
					agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
435
436
437
438
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
439
440
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
441
}
442

Pierre Kraemer's avatar
Pierre Kraemer committed
443
#ifndef SPATIAL_HASHING
444
445
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
446
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
447
	{
David Cazier's avatar
David Cazier committed
448
449
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
450

David Cazier's avatar
David Cazier committed
451
452
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
453

454
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
455
456
457
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
458

David Cazier's avatar
David Cazier committed
459
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
460
		{
461
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
462
463
464
465
466

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

David Cazier's avatar
David Cazier committed
467
			agents_[i]->goals_.push_back(dest) ;
468
469
470
471
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
472
473
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
474
	//city
David Cazier's avatar
David Cazier committed
475
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
476
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
477
	{
David Cazier's avatar
David Cazier committed
478
479
480
481
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
482
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
483
484
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
485
		{
David Cazier's avatar
David Cazier committed
486
487
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
488
		}
489

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

492
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
493
494
495
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
496

David Cazier's avatar
David Cazier committed
497
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
498
		{
499
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
500
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
501
502
			dest /= 2.0f ;
			dest[2] = 0 ;
503

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

David Cazier's avatar
David Cazier committed
507
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
508
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
509
510
511
		    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
512
		{
David Cazier's avatar
David Cazier committed
513
514
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
515
		}
516

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

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

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

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

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

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

David Cazier's avatar
David Cazier committed
537
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
538
		{
539
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
540
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
541
542
			dest /= 2.0f ;
			dest[2] = 0 ;
543

David Cazier's avatar
David Cazier committed
544
			agents_[i]->goals_.push_back(dest) ;
545
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
546
547
	}
}
548

Thomas's avatar
Thomas committed
549
550
551
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
552
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier 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
557
558
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
559
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
560
561
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
562
		{
David Cazier's avatar
David Cazier committed
563
564
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
565
566
		}

567
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
568
569
570
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
571

David Cazier's avatar
David Cazier committed
572
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
573
		{
574
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
575
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
576
577
		}

David Cazier's avatar
David Cazier committed
578
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
579
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
580
581
582
		    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
583
		{
David Cazier's avatar
David Cazier committed
584
585
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
586
587
		}

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

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

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

David Cazier's avatar
David Cazier committed
600
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
601
		{
602
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
603
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
604
605
606
		}
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
607
#endif
Thomas's avatar
Thomas committed
608

Pierre Kraemer's avatar
Pierre Kraemer committed
609
610
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
611
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
612

David Cazier's avatar
David Cazier committed
613
614
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
615
616
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
617
618
	}

David Cazier's avatar
David Cazier committed
619
620
621
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
622
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
623

David Cazier's avatar
David Cazier committed
624
625
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
626
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
627
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
628

David Cazier's avatar
David Cazier committed
629
630
631
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
632

633
			VEC3 pos(x, y, z) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
634
635
636
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, pos) ;
			agents_.push_back(a) ;
David Cazier's avatar
David Cazier committed
637
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
638
#else
639
640
			Dart dCell = envMap_.getBelongingCell(pos) ;
			agents_.push_back(new Agent(this, pos, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
641
#endif
642
			agents_.back()->goals_.push_back(pos) ;
David Cazier's avatar
David Cazier committed
643
			agents_.back()->goals_.push_back(-1.0f * pos) ;
644
			agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
645
646
647
		}
	}

David Cazier's avatar
David Cazier committed
648
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
649

David Cazier's avatar
David Cazier committed
650
651
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
652
653
654
655
}

bool Simulator::exportAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
656
	std::ofstream out(filename.c_str(), std::ios::out) ;
David Cazier's avatar
David Cazier committed
657
658
	if (!out.good())
	{
David Cazier's avatar
David Cazier committed
659
660
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
661
662
	}

David Cazier's avatar
David Cazier committed
663
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
664
#ifdef SPATIAL_HASHING
665
		out << agents_[i]->pos << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
666
#else
David Cazier's avatar
David Cazier committed
667
		out << agents_[i]->part_.getPosition() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
668
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
669

David Cazier's avatar
David Cazier committed
670
671
	out.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
672
673
674
675
}

void Simulator::swapAgentsGoals()
{
David Cazier's avatar
David Cazier committed
676
	unsigned int nbAgents = agents_.size() ;
David Cazier's avatar
David Cazier committed
677
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
678
	{
David Cazier's avatar
David Cazier committed
679
		unsigned int r = rand() % nbAgents ;
Thomas's avatar
Thomas committed
680
681
682
683
684
685
686
//		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;
//		}

687
		std::swap(agents_[i]->goals_, agents_[r]->goals_) ;
Thomas's avatar
Thomas committed
688
689
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
690

Thomas's avatar
Thomas committed
691
692
Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
{
David Cazier's avatar
David Cazier committed
693
694
	Geom::BoundingBox<VEC3> bb ;
	unsigned int nbAgents = agents_.size() ;
David Cazier's avatar
David Cazier committed
695
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
696
	{
David Cazier's avatar
David Cazier committed
697
		bb.addPoint(agents_[i]->getPosition()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
698
	}
Thomas's avatar
Thomas committed
699

David Cazier's avatar
David Cazier committed
700
	return bb ;
Pierre Kraemer's avatar
Pierre Kraemer committed
701
}
702

703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
//void Simulator::setupMovingObstacle(unsigned int nb_obst)
//{
//	float xSize = 10.0f ;
//	float ySize = 45.0f ;
//	VEC3 goal(50.0f, 50.0f, 0) ;
//	VEC3 posInit(0, 0, 0) ;
//	VEC3 possiblepos(0, 0, 0) ;
//	std::vector<VEC3> vPos ;
//
//	MovingObstacle* mo4 ;
//	float xx, yy ;
//	float sx, sy ;
//
//	for (unsigned int i = 0 ; i < (nb_obst) ; i++)
//	{
//		sx = rand() % 2 + 1 ;
//		sy = rand() % 2 + 1 ;
//		xx = rand() % (int)envMap_.max_for_obstacles ;
//		if (sx - 1 == 1) xx *= -1 ;
//		yy = rand() % (int)envMap_.max_for_obstacles ;
//		if (sy - 1 == 1) yy *= -1 ;
//		VEC3 possiblepos(xx, yy, 0) ;
//		Dart d1 = envMap_.getBelongingCell(possiblepos) ;
//		if (envMap_.buildingMark.isMarked(d1))
//		{
//			if (((possiblepos[0] + 50) < envMap_.max_for_obstacles)
//			    && ((possiblepos[1] + 50) < envMap_.max_for_obstacles))
//				possiblepos += VEC3(50, 50, 0) ;
//			else
//				possiblepos += VEC3(-50, -50, 0) ;
//		}
//		xx = possiblepos[0] ;
//		yy = possiblepos[1] ;
//
//		goal = PFP::VEC3(xx, -yy, 0) ;
//
//		posInit = PFP::VEC3(0, 0, 0) ;
//		//posInit[0] -= 200.0f;
//		posInit[1] += yy ;
//		posInit[0] += xx ;
//		possiblepos = posInit ;
//		d1 = envMap_.getBelongingCell(possiblepos) ;
//
//		// TODO Check sideSide
//		if (envMap_.buildingMark.isMarked(d1))
//		{
//			if (((possiblepos[0] + envMap_.sideSize) < envMap_.max_for_obstacles)
//			    && ((possiblepos[1] + envMap_.sideSize) < envMap_.max_for_obstacles))
//				possiblepos += VEC3(envMap_.sideSize, envMap_.sideSize, 0) ;
//			else if (((possiblepos[0] - envMap_.sideSize) < envMap_.max_for_obstacles)
//			    && ((possiblepos[1] - envMap_.sideSize) < envMap_.max_for_obstacles))
//				possiblepos += VEC3(-envMap_.sideSize, -envMap_.sideSize, 0) ;
//			else if (((possiblepos[0] - envMap_.sideSize) < envMap_.max_for_obstacles)
//			    && ((possiblepos[1] + envMap_.sideSize) < envMap_.max_for_obstacles))
//				possiblepos += VEC3(-envMap_.sideSize, envMap_.sideSize, 0) ;
//			else
//				possiblepos += VEC3(envMap_.sideSize, -envMap_.sideSize, 0) ;
//		}
//		posInit = possiblepos ;
//		vPos.clear() ;
//		vPos.push_back(posInit) ;
//		//								posInit[0] -= xSize/2;
//		//								vPos.push_back(posInit);
//		posInit[0] -= xSize ;
//		posInit[1] -= ySize / 2 ;
//		vPos.push_back(posInit) ;
//		posInit[0] += xSize ;
//		posInit[1] -= ySize / 2 ;
//		vPos.push_back(posInit) ;
//		//								posInit[0] += xSize;
//		//	posInit[1] += ySize;
//		//								vPos.push_back(posInit);
//		posInit[0] += xSize ;
//		posInit[1] += ySize / 2 ;
//		vPos.push_back(posInit) ;
//		//								posInit[0] -= xSize;
//		//								posInit[1] += ySize/2;
//		//								vPos.push_back(posInit);
//
//		//	CGoGNout << "ligne "<< i << vPos[i] << CGoGNendl;
//
//		mo4 = new MovingObstacle(i, &envMap_, vPos, goal, 0) ;
//		movingObstacles_.push_back(mo4) ;
//	}
//}