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;
pitiot's avatar
pitiot committed
17
	detect_agent_collision=false;
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
{
pitiot's avatar
pitiot committed
38

39
	envMap_.init(config, 1600.0f, 960.0f, minSize, 20.0f) ; //cases fines
pitiot's avatar
pitiot committed
40
}
David Cazier's avatar
David Cazier committed
41
42
	switch (config)
	{
pitiot's avatar
maj    
pitiot committed
43
44
45
46
		case 0 :
			setupCircleScenario(100) ;
			break ;
		case 1 :
pitiot's avatar
pitiot committed
47
			setupCorridorScenario(1000,40) ;
pitiot's avatar
maj    
pitiot committed
48
49
50
51
52
53
54
55
56
57
58
59
			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
60
61
	}

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

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

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

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

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

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

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
128
#ifdef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
129
130
131
132
133
134
135
136
137
138
139
140
	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
141
	if (multires) envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
142
143
144
145
	nb_dead = 0 ;
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
		if (agents_[i]->alive)
Pierre Kraemer's avatar
Pierre Kraemer committed
146
		{
pitiot's avatar
maj    
pitiot committed
147
148
149
150
151
152
			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
153
			{
pitiot's avatar
maj    
pitiot committed
154
155
156
157
158
159
//			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
160
161
			}
		}
pitiot's avatar
maj    
pitiot committed
162
163
164
165
166
167
168
		else
			nb_dead++ ;

	}

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

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

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

199
	agents_.back()->goals_=goal ;
pitiot's avatar
maj    
pitiot committed
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
226
	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 ;
227
228
229
230
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
		addAgent(start, goals) ;
231
	}
Thomas's avatar
Thomas committed
232

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

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

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

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

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

pitiot's avatar
maj    
pitiot committed
289
290
291
292
293
	// 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
294

pitiot's avatar
maj    
pitiot committed
295
296
297
298
299
300
301
302
303
304
305
306
	// 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;
307
//		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
pitiot's avatar
maj    
pitiot committed
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
		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
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347


		//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
348
349
		movingObstacles_.push_back(mo4);
	}
Thomas's avatar
Thomas committed
350
351
}

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

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

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

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

393
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
394
{
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
//	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
416
	/*
David Cazier's avatar
David Cazier committed
417
418
419
420
	 * 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
421
	CellMarker<FACE> filled(envMap_.map) ;
David Cazier's avatar
David Cazier committed
422
423
424
425
426
427

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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