simulator.cpp 22.8 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 :
Jund Thomas's avatar
Jund Thomas committed
38
			setupCorridorScenario(500, 40) ;
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
//		if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE))
			if (agents_[i]->part_.crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//		switch(agents_[i]->part_.crossCell)
			{
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace) ;
//				break;
			}
		}
		else
			nb_dead++ ;

	}

	nbRefineCandidate += envMap_.refineCandidate.size() ;
	nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
151
//	envMap_.updateMap() ;
David Cazier's avatar
David Cazier committed
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
#endif

	globalTime_ += timeStep_ ;
	++nbSteps_ ;
}

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

	return true ;
}

void Simulator::addAgent(const VEC3& start, const VEC3& goal)
{
#ifdef SPATIAL_HASHING
	Agent* a = new Agent(this, start) ;
	agents_.push_back(a) ;
	envMap_.addAgentInGrid(a) ;
#else
	Dart dCell = envMap_.getBelongingCell(start) ;
	agents_.push_back(new Agent(this, start, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
177
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
178

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

Jund Thomas's avatar
Jund Thomas committed
277
278
	VEC3 xSide (10.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,15.0f,0.0f);
David Cazier's avatar
David Cazier committed
279
280
281
282
283
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
Jund Thomas's avatar
Jund Thomas committed
284
285
		float x = xStartMin + ((int)i*30) % xStartDelta;
		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
David Cazier's avatar
David Cazier committed
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
		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
308
309
}

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

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

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

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

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

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

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

// 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));
435
					agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
436
437
438
439
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
440
441
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
442
}
443

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Thomas's avatar
Thomas committed
550
551
552
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
553
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
554
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
555
	{
David Cazier's avatar
David Cazier committed
556
557
558
559
		agents_[i]->goals_.clear() ;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

David Cazier's avatar
David Cazier committed
701
	return bb ;
Pierre Kraemer's avatar
Pierre Kraemer committed
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
788
//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) ;
//	}
//}