Création d'un compte pour un collaborateur extérieur au laboratoire depuis l'intranet ICube : https://intranet.icube.unistra.fr/fr/labs/member/profile

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

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

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

David Cazier's avatar
David Cazier committed
29
void Simulator::init(unsigned int config, int minSize, float dimension, bool enablePathFinding)
Thomas's avatar
Thomas committed
30
{
31
	std::cout << "Setup scenario" << std::endl ;
pitiot's avatar
pitiot committed
32
33
34
35
36
37
if (multires)
{
	envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
}
else
{
38
	envMap_.init(config, 1600.0f, 960.0f, minSize, 20.0f) ; //cases fines
pitiot's avatar
pitiot committed
39
}
David Cazier's avatar
David Cazier committed
40
41
	switch (config)
	{
pitiot's avatar
maj    
pitiot committed
42
43
44
45
		case 0 :
			setupCircleScenario(100) ;
			break ;
		case 1 :
pitiot's avatar
pitiot committed
46
			setupCorridorScenario(1000,40) ;
pitiot's avatar
maj    
pitiot committed
47
48
49
50
51
52
53
54
55
56
57
58
			break ;
		case 2 :
			setupScenario(1000) ;
			break ;
		case 3 :
			setupCityScenario(20, 20) ;
//			setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10),
//					-1.0f * (12 * (70.0f / 2.0f) - 10), 20, 20);
			break ;
		case 4 :
			importAgents("myAgents.pos") ;
			break ;
Thomas's avatar
Thomas committed
59
60
	}

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
81
#ifndef SPATIAL_HASHING
82
	if (multires) envMap_.subdivideToProperLevel() ;
pitiot's avatar
maj    
pitiot committed
83
//	envMap_.subdivideAllToMaxLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
84
#endif
pitiot's avatar
maj    
pitiot committed
85
86

//	setupMovingObstacle(40);
Pierre Kraemer's avatar
Pierre Kraemer committed
87
88
}

pitiot's avatar
maj    
pitiot committed
89
void Simulator::doStep()
Pierre Kraemer's avatar
Pierre Kraemer committed
90
{
pitiot's avatar
maj    
pitiot committed
91
92
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
pitiot's avatar
pitiot committed
93
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
94
95
96
97
98
#endif

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

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
127
#ifdef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
128
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) ;
		}
	}
#else
140
	if (multires) envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
141
142
143
144
	nb_dead = 0 ;
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		if (agents_[i]->alive)
Pierre Kraemer's avatar
Pierre Kraemer committed
145
		{
pitiot's avatar
maj    
pitiot committed
146
147
148
149
150
151
			Dart oldFace = agents_[i]->part_.d ;
			agents_[i]->update() ;

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

	}

	nbRefineCandidate += envMap_.refineCandidate.size() ;
	nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
168
	if (multires) envMap_.updateMap() ;
pitiot's avatar
maj    
pitiot committed
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
#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 ;
}

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

pitiot's avatar
maj    
pitiot committed
193
	Dart dCell = envMap_.getBelongingCell(start) ;
pitiot's avatar
pitiot committed
194
//	if (dCell==envMap_.map.begin()) CGoGNout<<"pbagents"<<CGoGNendl;
pitiot's avatar
maj    
pitiot committed
195
	agents_.push_back(new Agent(this, start, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
196
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
197

198
	agents_.back()->goals_=goal ;
pitiot's avatar
maj    
pitiot committed
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
	agents_.back()->curGoal_ = 0 ;
}

void Simulator::setupCircleScenario(unsigned int nbAgents)
{
	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents" << std::endl ;

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

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

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

	double pi = 3.14159265358979323846f ;

	for (unsigned int i = 0 ; i < nbAgents ; ++i)
	{
		double angle = i * 2.0f * pi / float(nbAgents) ;
		VEC3 v(std::cos(angle) * radius, std::sin(angle) * radius, 0) ;
		VEC3 start = center + v ;
		VEC3 goal = center - v ;
226
227
228
229
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
230
	}
Thomas's avatar
Thomas committed
231

pitiot's avatar
maj    
pitiot committed
232
233
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
	{
pitiot's avatar
pitiot committed
234
//		movingObstacles_[i]->updateFixedObstNeighbors() ;
pitiot's avatar
maj    
pitiot committed
235
236
237
		movingObstacles_[i]->computePrefVelocity() ;
		movingObstacles_[i]->update() ;
	}
Thomas's avatar
Thomas committed
238

239
#ifndef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
240
241
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
242
#endif
pitiot's avatar
maj    
pitiot committed
243
244

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

pitiot's avatar
maj    
pitiot committed
247
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
248
{
pitiot's avatar
maj    
pitiot committed
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
	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
270
	{
pitiot's avatar
maj    
pitiot committed
271
272
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;
273

pitiot's avatar
maj    
pitiot committed
274
275
		// Un agent sur 2 va de droite à gauche
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
276
277
		if (i % 2 == 1)
		{
pitiot's avatar
maj    
pitiot committed
278
279
280
			tmp = goal ;
			goal = start ;
			start = tmp ;
Pierre Kraemer's avatar
Pierre Kraemer committed
281
		}
282
283
284
285
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
Thomas's avatar
Thomas committed
286
287
	}

pitiot's avatar
maj    
pitiot committed
288
289
290
291
292
	// Départ des obstacles du quart haut sur toute une demi-largeur
	xStartMin = envMap_.geometry.min()[0] + envMap_.geometry.size(0) / 4 ;
	xStartDelta = envMap_.geometry.size(0) / 2 ;
	yStartMin = envMap_.geometry.min()[1] + yBorder ;
	yStartDelta = envMap_.geometry.size(1) / 5 ;
Thomas's avatar
Thomas committed
293

pitiot's avatar
maj    
pitiot committed
294
295
296
297
298
299
300
301
302
303
304
305
	// Arrivée des obstacles à l'opposée
	yGoalDelta = envMap_.geometry.size(1) / 5 ;
	yGoalMin = envMap_.geometry.max()[1] - yBorder - yGoalDelta ;

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

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
		float x = xStartMin + ((int)i*30) % xStartDelta;
306
//		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
pitiot's avatar
maj    
pitiot committed
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(x, yGoalMin  + rand() % yGoalDelta,  0) ;

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		VEC3 tmp ;
		if (i % 2 == 1)
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
		}
		else {
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
		}
		mo4= new MovingObstacle(this, i,vPos,goal,0);
pitiot's avatar
pitiot committed
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346


		//for generating a random path
//		unsigned int dartDistForPath = 50 ;
//		mo4->goals_.clear() ;
//		Dart dStart = mo4->parts_[0]->d;
//		Dart dStop = dStart ;
//		for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20 || envMap_.map.sameFace(dStop, dStart) ; ++j)
//		{
//			envMap_.map.next(dStop) ;
//			if (dStop == envMap_.map.end())
//				dStop = envMap_.map.begin() ;
//		}
//
//		addPathToObstacle(mo4, dStart, dStop);
//		addPathToObstacle(mo4, dStop, dStart);

pitiot's avatar
maj    
pitiot committed
347
348
		movingObstacles_.push_back(mo4);
	}
Thomas's avatar
Thomas committed
349
350
}

pitiot's avatar
maj    
pitiot committed
351
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
352
{
pitiot's avatar
maj    
pitiot committed
353
354
355
356
357
	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
358

pitiot's avatar
maj    
pitiot committed
359
	VEC3 posagent(xCornerDown - xBorder / 2, yCornerDown - yBorder / 2, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
360
361
362
#ifdef SPATIAL_HASHING
	Agent* a = new Agent(this, posagent) ;
	agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
363
	envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
364
#else
365
366
	Dart dCell = envMap_.getBelongingCell(posagent) ;
	agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
367
#endif
David Cazier's avatar
David Cazier committed
368
	agents_.back()->goals_.push_back(-1.0f * posagent) ;
369
	agents_.back()->curGoal_ = 0 ;
370

pitiot's avatar
maj    
pitiot committed
371
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
372
	{
pitiot's avatar
maj    
pitiot committed
373
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
374
		{
375
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
376
377
378
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, posagent) ;
			agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
379
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
380
#else
381
382
			Dart dCell = envMap_.getBelongingCell(posagent) ;
			agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
383
#endif
David Cazier's avatar
David Cazier committed
384
			agents_.back()->goals_.push_back(-1.0f * posagent) ;
385
			agents_.back()->curGoal_ = 0 ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
386
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
387
	}
Thomas's avatar
Thomas committed
388

David Cazier's avatar
David Cazier committed
389
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
390
391
}

392
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
393
{
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
//	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
415
	/*
David Cazier's avatar
David Cazier committed
416
417
418
419
	 * Add agents, specifying their start position, and store their goals on the
	 * opposite side of the environment.
	 */
	Dart d = envMap_.map.begin() ;
pitiot's avatar
maj    
pitiot committed
420
	CellMarker<FACE> filled(envMap_.map) ;
David Cazier's avatar
David Cazier committed
421
422
423
424
425
426

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

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

pitiot's avatar
maj    
pitiot committed
427
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
428
	{
David Cazier's avatar
David Cazier committed
429
430
431
		bool found = false ;
		VEC3 pos ;
		Dart dCell ;
David Cazier's avatar
David Cazier committed
432
433
434
435
436
		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
437
				filled.mark(d) ;
438
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
439
440
441
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
442
			}
David Cazier's avatar
David Cazier committed
443
			envMap_.map.next(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
444
445
		}

David Cazier's avatar
David Cazier committed
446
447
		if (found)
		{
448
449
			float ecart = 3.0f ;
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
450
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
pitiot's avatar
maj    
pitiot committed
451
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
452
			{
pitiot's avatar
maj    
pitiot committed
453
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
454
				{
455
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
456
457
458
#ifdef SPATIAL_HASHING
					Agent* a = new Agent(this, posagent) ;
					agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
459
					envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
460
#else
461
					agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
462
#endif
463
					agents_.back()->goals_.push_back(posagent) ;
David Cazier's avatar
David Cazier committed
464
					agents_.back()->goals_.push_back(-1.0f * posagent) ;
pitiot's avatar
maj    
pitiot committed
465

466
					agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
467
468
469
470
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
471
472
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
473
}
474

pitiot's avatar
pitiot committed
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
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
490
#ifndef SPATIAL_HASHING
491
492
void Simulator::addPathToCorner()
{
pitiot's avatar
maj    
pitiot committed
493
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
494
	{
David Cazier's avatar
David Cazier committed
495
496
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
497

David Cazier's avatar
David Cazier committed
498
499
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
500

501
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
502
503
504
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
505

pitiot's avatar
maj    
pitiot committed
506
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
507
		{
508
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
509
510
511
512
513

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

David Cazier's avatar
David Cazier committed
514
			agents_[i]->goals_.push_back(dest) ;
515
516
517
518
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
519
520
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
521
	//city
David Cazier's avatar
David Cazier committed
522
	unsigned int dartDistForPath = 50 ;
pitiot's avatar
maj    
pitiot committed
523
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
524
	{
David Cazier's avatar
David Cazier committed
525
526
527
528
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
pitiot's avatar
maj    
pitiot committed
529
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
530
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
531
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
532
		{
David Cazier's avatar
David Cazier committed
533
534
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
535
		}
536

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

539
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
540
541
542
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
543

pitiot's avatar
maj    
pitiot committed
544
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
545
		{
546
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
547
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
548
549
			dest /= 2.0f ;
			dest[2] = 0 ;
550

David Cazier's avatar
David Cazier committed
551
			agents_[i]->goals_.push_back(dest) ;
552
		}
553

David Cazier's avatar
David Cazier committed
554
		Dart dStop2 = dStop ;
pitiot's avatar
maj    
pitiot committed
555
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
David Cazier's avatar
David Cazier committed
556
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
557
558
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
559
		{
David Cazier's avatar
David Cazier committed
560
561
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
562
		}
563

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

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

pitiot's avatar
maj    
pitiot committed
569
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
570
		{
571
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
572
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
573
574
			dest /= 2.0f ;
			dest[2] = 0 ;
575

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

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

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

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

David Cazier's avatar
David Cazier committed
591
			agents_[i]->goals_.push_back(dest) ;
592
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
593
594
	}
}
595

Thomas's avatar
Thomas committed
596
597
598
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
599
	unsigned int dartDistForPath = 50 ;
pitiot's avatar
maj    
pitiot committed
600
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
601
	{
David Cazier's avatar
David Cazier committed
602
603
604
605
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
pitiot's avatar
maj    
pitiot committed
606
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
607
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
608
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
609
		{
David Cazier's avatar
David Cazier committed
610
611
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
612
613
		}

614
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
615
616
617
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
618

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

David Cazier's avatar
David Cazier committed
625
		Dart dStop2 = dStop ;
pitiot's avatar
maj    
pitiot committed
626
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
627
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
pitiot's avatar
maj    
pitiot committed
628
629
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
630
		{
David Cazier's avatar
David Cazier committed
631
632
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
633
634
		}

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

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

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
656
657
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
658
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
659

David Cazier's avatar
David Cazier committed
660
661
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
662
663
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
664
665
	}

David Cazier's avatar
David Cazier committed
666
667
668
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
669
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
670

David Cazier's avatar
David Cazier committed
671
672
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
673
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
674
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
675

David Cazier's avatar
David Cazier committed
676
677
678
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
679

680
			VEC3 pos(x, y, z) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
681
682
683
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, pos) ;
			agents_.push_back(a) ;
pitiot's avatar
maj    
pitiot committed
684
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
685
#else
686
687
			Dart dCell = envMap_.getBelongingCell(pos) ;
			agents_.push_back(new Agent(this, pos, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
688
#endif
689
			agents_.back()->goals_.push_back(pos) ;
David Cazier's avatar
David Cazier committed
690
			agents_.back()->goals_.push_back(-1.0f * pos) ;
691
			agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
692
693
694
		}
	}

David Cazier's avatar
David Cazier committed
695
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
696

David Cazier's avatar
David Cazier committed
697
698
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
699
700
701
702
}

bool Simulator::exportAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
703
	std::ofstream out(filename.c_str(), std::ios::out) ;
David Cazier's avatar
David Cazier committed
704
705
	if (!out.good())
	{
David Cazier's avatar
David Cazier committed
706
707
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
708
709
	}

pitiot's avatar
maj    
pitiot committed
710
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
711
#ifdef SPATIAL_HASHING
712
		out << agents_[i]->pos << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
713
#else
pitiot's avatar
maj    
pitiot committed
714
		out << agents_[i]->part_.getPosition() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
715
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
716

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

void Simulator::swapAgentsGoals()
{
David Cazier's avatar
David Cazier committed
723
	unsigned int nbAgents = agents_.size() ;
pitiot's avatar
maj    
pitiot committed
724
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
725
	{
David Cazier's avatar
David Cazier committed
726
		unsigned int r = rand() % nbAgents ;
Thomas's avatar
Thomas committed
727
728
729
730
731
732
733
//		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;
//		}

734
		std::swap(agents_[i]->goals_, agents_[r]->goals_) ;
Thomas's avatar
Thomas committed
735
736
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
737

Thomas's avatar
Thomas committed
738
739
Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
{
David Cazier's avatar
David Cazier committed
740
741
	Geom::BoundingBox<VEC3> bb ;
	unsigned int nbAgents = agents_.size() ;
pitiot's avatar
maj    
pitiot committed
742
	for (unsigned int i = 0 ; i < nbAgents ; ++i)
David Cazier's avatar
David Cazier committed
743
	{
David Cazier's avatar
David Cazier committed
744
		bb.addPoint(agents_[i]->getPosition()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
745
	}
Thomas's avatar
Thomas committed
746

David Cazier's avatar
David Cazier committed
747
	return bb ;
Pierre Kraemer's avatar
Pierre Kraemer committed
748
}