simulator.cpp 29.3 KB
Newer Older
Pierre Kraemer's avatar
Pierre Kraemer committed
1
#include "simulator.h"
Thomas Jund's avatar
Thomas Jund committed
2
#include "env_generator.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
3

4
Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgent, unsigned int nbObst) :
Thomas Jund's avatar
Thomas Jund committed
5
	timeStep_(0.05f),
Thomas Jund's avatar
Thomas Jund committed
6
//	timeStep_(0.1f),
David Cazier's avatar
David Cazier committed
7
8
9
10
11
12
13
	globalTime_(0.0f),
	nbSteps_(0),
	nbUpdates(0),
	nbSorts(0),
	nbRefineCandidate(0),
	nbCoarsenCandidate(0),
	nearNeighbors(0),
David Cazier's avatar
David Cazier committed
14
	totalNeighbors(0),
pitiot's avatar
maj    
pitiot committed
15
	avoidance(1),
pitiot's avatar
pitiot committed
16
	nb_dead(0)
Pierre Kraemer's avatar
Pierre Kraemer committed
17
{
pitiot's avatar
pitiot committed
18
	minSize=minS;
pitiot's avatar
merging    
pitiot committed
19
20
	multires=true;
	detect_agent_collision=true;
21
22
	srand(10) ;
	nbStepsPerUnit_ = 1 / timeStep_ ;
pitiot's avatar
merging    
pitiot committed
23
	this->config=config;
24
	init(2.0f, nbAgent, nbObst) ;
Thomas's avatar
Thomas committed
25
26
27
28
}

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

33
void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst, bool enablePathFinding)
Thomas's avatar
Thomas committed
34
{
35
	std::cout << "Setup scenario" << std::endl ;
Thomas's avatar
Thomas committed
36

David Cazier's avatar
David Cazier committed
37
38
	switch (config)
	{
David Cazier's avatar
David Cazier committed
39
		case 0 :
40
			setupCircleScenario(nbAgent,nbObst) ;
Thomas Jund's avatar
Thomas Jund committed
41
//			addPathsToAgents();
David Cazier's avatar
David Cazier committed
42
43
			break ;
		case 1 :
44
			setupCorridorScenario(nbAgent,nbObst) ;
David Cazier's avatar
David Cazier committed
45
46
			break ;
		case 2 :
47
			setupSnakeCorridorScenario(nbAgent,nbObst,10) ;
David Cazier's avatar
David Cazier committed
48
			break ;
Thomas Jund's avatar
Thomas Jund committed
49
50
51
52
53
		case 3 :
			envMap_.init(config, 500.0f, 500.0f, minSize, 100.0f) ; //grosses cases
			setupScenario(nbAgent, false) ;
			addMovingObstacles(nbObst);
			addPathToObstacles();
pitiot's avatar
pitiot committed
54
55
////			setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10),
////					-1.0f * (12 * (70.0f / 2.0f) - 10), 20, 20);
Thomas Jund's avatar
Thomas Jund committed
56
			break ;
pitiot's avatar
pitiot committed
57
58
59
//		case 4 :
//			importAgents("myAgents.pos") ;
//			break ;
Jund Thomas's avatar
Jund Thomas committed
60
		case 5:
61
			envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ; //svg import
Thomas Jund's avatar
Thomas Jund committed
62
			setupScenario(nbAgent, true) ;
63
//			SelectorCellNotMarked<PFP::MAP> scnm(envMap_.pedWayMark);
64
			addMovingObstacles(nbObst);
Thomas Jund's avatar
Thomas Jund committed
65
//			addPathToObstacles();
Thomas Jund's avatar
Thomas Jund committed
66
			addPathsToAgents();
Jund Thomas's avatar
Jund Thomas committed
67
			break;
David Cazier's avatar
David Cazier committed
68
69
70
		default:
			std::cout << "Unknown scenario !" << std::endl ;
			exit(1) ;
Thomas's avatar
Thomas committed
71
72
	}

David Cazier's avatar
David Cazier committed
73
74
75
76
77
#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
78
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
79
80
81
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
David Cazier's avatar
David Cazier committed
82
			addPathsToAgents() ;
83
84
		else if (dimension == 2.5f)
			addPathsToAgents_height() ;
Thomas's avatar
Thomas committed
85
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
86
#endif
87

David Cazier's avatar
David Cazier committed
88
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
89
	{
David Cazier's avatar
David Cazier committed
90
91
		agents_[i]->updateObstacleNeighbors() ;
		agents_[i]->updateAgentNeighbors() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
92
	}
93

94

Pierre Kraemer's avatar
Pierre Kraemer committed
95
#ifndef SPATIAL_HASHING
96
97
	if (multires)
		envMap_.subdivideToProperLevel() ;
David Cazier's avatar
David Cazier committed
98
	//	envMap_.subdivideAllToMaxLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
99
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
100
101
}

David Cazier's avatar
David Cazier committed
102
void Simulator::doStep()
Pierre Kraemer's avatar
Pierre Kraemer committed
103
{
David Cazier's avatar
David Cazier committed
104
105
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
Jund Thomas's avatar
Jund Thomas committed
106
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
107
#endif
David Cazier's avatar
David Cazier committed
108

David Cazier's avatar
David Cazier committed
109
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
pitiot's avatar
pitiot committed
110
	{
David Cazier's avatar
David Cazier committed
111
		movingObstacles_[i]->computePrefVelocity() ;
pitiot's avatar
pitiot committed
112
		movingObstacles_[i]->computeNewVelocity() ;
Thomas Jund's avatar
Thomas Jund committed
113
		movingObstacles_[i]->updateAgentNeighbors();
pitiot's avatar
pitiot committed
114
115
116
#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
David Cazier's avatar
David Cazier committed
117
		movingObstacles_[i]->update() ;
118

119
		movingObstacles_[i]->updateMesh() ;
pitiot's avatar
pitiot committed
120
121
	}

David Cazier's avatar
David Cazier committed
122
123
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
pitiot's avatar
pitiot committed
124
125
126
		if (agents_[i]->alive)
		{
			agents_[i]->updateObstacleNeighbors() ;
pitiot's avatar
maj    
pitiot committed
127
128

#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
129
			envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
130
#endif
pitiot's avatar
pitiot committed
131
			agents_[i]->updateAgentNeighbors() ;
pitiot's avatar
maj    
pitiot committed
132
#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
133
			envMap_.map.setCurrentLevel(0) ;
pitiot's avatar
maj    
pitiot committed
134
135
#endif

pitiot's avatar
pitiot committed
136
137
138
			agents_[i]->computePrefVelocity() ;
			agents_[i]->computeNewVelocity() ;
		}
David Cazier's avatar
David Cazier committed
139
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
140

Pierre Kraemer's avatar
Pierre Kraemer committed
141
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
142
143
144
145
146
147
148
149
150
151
152
	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
153
#else
154
155
	if (multires)
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
156
157
158
159
160
161
162
	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
163

David Cazier's avatar
David Cazier committed
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
//		if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE))
			if (agents_[i]->part_.crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//		switch(agents_[i]->part_.crossCell)
			{
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace) ;
//				break;
			}
		}
		else
			nb_dead++ ;

	}

	nbRefineCandidate += envMap_.refineCandidate.size() ;
	nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
Jund Thomas's avatar
Jund Thomas committed
183

Thomas Jund's avatar
Thomas Jund committed
184
185
	 if (multires)
		 envMap_.updateMap() ;
David Cazier's avatar
David Cazier committed
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
#endif

	globalTime_ += timeStep_ ;
	++nbSteps_ ;
}

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

	return true ;
}

David Cazier's avatar
David Cazier committed
202
void Simulator::addAgent(const VEC3& start, const VEC3& goal)
David Cazier's avatar
David Cazier committed
203
{
David Cazier's avatar
David Cazier committed
204
	agents_.push_back(new Agent(this, start, goal)) ;
David Cazier's avatar
David Cazier committed
205
206
}

207
208
209
210
211
void Simulator::addAgent(const VEC3& start, const VEC3& goal, Dart d)
{
	agents_.push_back(new Agent(this, start, goal, d)) ;
}

David Cazier's avatar
David Cazier committed
212
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
David Cazier's avatar
David Cazier committed
213
{
pitiot's avatar
merging    
pitiot committed
214
215
216
217
218
219
220
221
222
223
	if (multires)
	{
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
	}
	else
	{

		envMap_.init(config, 1600.0f, 960.0f, minSize, 400.0f) ; //cases fines
	}

David Cazier's avatar
David Cazier committed
224
225
226
227
228
229
230
231
232
	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é
Thomas Jund's avatar
Thomas Jund committed
233
234
235
236
237
//	int xSize = envMap_.geometry.size(0) - 2 * xBorder ;
//	int ySize = envMap_.geometry.size(1) - 2 * yBorder ;
//	int radius = min(xSize, ySize) / 2 ;
	int radius = nbAgents/(Agent::radius_*Agent::radius_);
	std::cout << "radius " << radius << std::endl;
David Cazier's avatar
David Cazier committed
238
239
	VEC3 center = (envMap_.geometry.min() + envMap_.geometry.max()) / 2 ;

David Cazier's avatar
David Cazier committed
240
	double pi = 3.14159265358979323846f ;
David Cazier's avatar
David Cazier committed
241
242
243
244
245
246
247

	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 ;
David Cazier's avatar
David Cazier committed
248
		addAgent(start, goal) ;
249
	}
pitiot's avatar
pitiot committed
250
251
252
253
254
	VEC3 xSide (5.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,10.0f,0.0f);
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;
	std::vector<VEC3> positions;
pitiot's avatar
pitiot committed
255
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
David Cazier's avatar
David Cazier committed
256
	{
pitiot's avatar
pitiot committed
257
258
259
260
261
262
263
264
		double angle = i * 2.0f * pi / float(nbObstacles/2) ;
		VEC3 v,start;
		if(i<nbObstacles/2)
		{
			v=VEC3 (std::cos(angle) * (2*radius/3), std::sin(angle) * (2*radius/3), 0) ;
		}
		else
		{
pitiot's avatar
merging    
pitiot committed
265
			v=VEC3 (std::cos(angle) * (radius/2), std::sin(angle) * (radius/2), 0) ;
pitiot's avatar
pitiot committed
266
267
268
		}
		start = center + v ;
		positions.push_back(start);
pitiot's avatar
pitiot committed
269
270
271
272
	}
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
273
274
275
276
277
278
279
//		std::vector<VEC3> goals;
//		for(unsigned int k = 0 ; k < nbObstacles/2 ; k++)
//		{
//			goals.push_back(positions[(i+k)%(nbObstacles/2)]);
//		}

		unsigned int nbGoals=360;
pitiot's avatar
pitiot committed
280
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
281
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
282
		{
Thomas Jund's avatar
Thomas Jund committed
283
			goals.push_back(VEC3(std::cos(2.0f*M_PI*k/nbGoals) * (2*radius/3), std::sin(2.0f*M_PI*k/nbGoals) * (2*radius/3), 0)) ;
pitiot's avatar
pitiot committed
284
285
286
287
288
289
290
291
292
		}

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		vPos.push_back(start+xSide-ySide);
		vPos.push_back(start+xSide+ySide);
		vPos.push_back(start-xSide+ySide);
		vPos.push_back(start-xSide-ySide);

293
		mo4= new MovingObstacle(this, i,vPos,goals, true, true);
Thomas Jund's avatar
Thomas Jund committed
294
		mo4->curGoal_ = i*float(nbGoals)/float(nbObstacles/2.0f);
pitiot's avatar
pitiot committed
295

pitiot's avatar
pitiot committed
296
297
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
298

pitiot's avatar
pitiot committed
299
300
301
	for (unsigned int i = nbObstacles/2 ; i < nbObstacles ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
302
303
304
305
306
307
308
//		std::vector<VEC3> goals;
//		for(unsigned int k = (nbObstacles/2) ; k > 0 ; k--)
//		{
//			goals.push_back(positions[((i-(nbObstacles/2)+k)%(nbObstacles/2))+nbObstacles/2]);
//		}

		unsigned int nbGoals=360;
pitiot's avatar
pitiot committed
309
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
310
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
311
		{
Thomas Jund's avatar
Thomas Jund committed
312
			goals.push_back(VEC3(std::cos(2.0f*M_PI*k/nbGoals) * (radius/2), std::sin(2.0f*M_PI*k/nbGoals) * (radius/2), 0)) ;
pitiot's avatar
pitiot committed
313
314
315
316
317
318
319
320
321
		}

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		vPos.push_back(start+xSide-ySide);
		vPos.push_back(start+xSide+ySide);
		vPos.push_back(start-xSide+ySide);
		vPos.push_back(start-xSide-ySide);

322
		mo4= new MovingObstacle(this, i,vPos,goals, true, true);
Thomas Jund's avatar
Thomas Jund committed
323
		mo4->curGoal_ = (i-nbObstacles/2)*float(nbGoals)/float(nbObstacles/2.0f);
pitiot's avatar
pitiot committed
324
325

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
326
	}
Thomas's avatar
Thomas committed
327

328
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
329
330
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
331
#endif
pitiot's avatar
merging    
pitiot committed
332
333

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

David Cazier's avatar
David Cazier committed
336
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
337
{
pitiot's avatar
merging    
pitiot committed
338
339
340
341
342
343
344
345
346
347
348
	if (multires)
	{
		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
	}
	else
	{

		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //cases fines
	}


David Cazier's avatar
David Cazier committed
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
	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)
	{
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;

		// Un agent sur 2 va de droite à gauche
		if (i % 2 == 1)
			addAgent(start, goal) ;
		else
			addAgent(goal, start) ;
	}

	// 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 ;
pitiot's avatar
pitiot committed
386

David Cazier's avatar
David Cazier committed
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
	// 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;
//		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
		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);
		}
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
425
		mo4= new MovingObstacle(this, i,vPos,goals, true, false);
David Cazier's avatar
David Cazier committed
426
427


428
429
430
431
432
433
434
435
436
437
438
439
440
441
//		//for generating a random path
//		unsigned int dartDistForPath = 50 ;
//		mo4->goals_.clear() ;
//		Dart dStart = mo4->registering_part->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);
David Cazier's avatar
David Cazier committed
442
443
444
445
446

		movingObstacles_.push_back(mo4);
	}
}

pitiot's avatar
merging    
pitiot committed
447
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
David Cazier's avatar
David Cazier committed
448
{
pitiot's avatar
merging    
pitiot committed
449
450
451
452
453
454
	if (multires)
		envMap_.init(config, 1600.0f, 960.0f, minSize, 160.0f) ; //grosses cases
	else
		envMap_.init(config, 1600.0f, 960.0f, minSize, 160.0f) ; //cases fines

	std::cout << " - Setup Snake Corridor Scenario : " << nbAgents << " agents et " << nbSnakes*snakeSize << " obstacles" << std::endl ;
David Cazier's avatar
David Cazier committed
455
456

	// Bordure à éviter autour de la scène (10% de sa taille)
pitiot's avatar
merging    
pitiot committed
457
458
	int xBorder = envMap_.geometry.size(0) / 5 ;
	int yBorder = envMap_.geometry.size(1) / 5 ;
David Cazier's avatar
David Cazier committed
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474

	// 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
475
	{
David Cazier's avatar
David Cazier committed
476
477
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;
478

David Cazier's avatar
David Cazier committed
479
		// Un agent sur 2 va de droite à gauche
pitiot's avatar
merging    
pitiot committed
480
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
481
		if (i % 2 == 1)
pitiot's avatar
merging    
pitiot committed
482
483
484
485
486
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
		}
pitiot's avatar
merge    
pitiot committed
487

pitiot's avatar
merging    
pitiot committed
488
		addAgent(start, goal) ;
pitiot's avatar
merge    
pitiot committed
489

Thomas's avatar
Thomas committed
490
491
	}

David Cazier's avatar
David Cazier committed
492
493
494
495
	// 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 ;
pitiot's avatar
merging    
pitiot committed
496
	yStartDelta = envMap_.geometry.size(1) / 20 ;
David Cazier's avatar
David Cazier committed
497
498

	// Arrivée des obstacles à l'opposée
pitiot's avatar
merge    
pitiot committed
499

pitiot's avatar
merging    
pitiot committed
500
	yGoalDelta = 3* envMap_.geometry.size(1) / 5 ;
pitiot's avatar
merge    
pitiot committed
501

David Cazier's avatar
David Cazier committed
502
503
	yGoalMin = envMap_.geometry.max()[1] - yBorder - yGoalDelta ;

504
	VEC3 xSide (5.0f,0.0f,0.0f);
505
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
506

pitiot's avatar
merging    
pitiot committed
507
508
509

	int sumObstacles=0;
	for(unsigned int j = 0; j<nbSnakes; j++)
David Cazier's avatar
David Cazier committed
510
	{
pitiot's avatar
merging    
pitiot committed
511
512
513
514
		std::vector<PFP::VEC3> positions [snakeSize] ;
		float x = xStartMin + rand() % xStartDelta;
		VEC3 goal;

David Cazier's avatar
David Cazier committed
515
516
		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;

pitiot's avatar
merging    
pitiot committed
517
518
519
520
521
522
523
524
525
526
527
528
529

		std::vector<VEC3> vPos;


		vPos.push_back(start+xSide-ySide);
		vPos.push_back(start+xSide+ySide);
		vPos.push_back(start-xSide+ySide);
		vPos.push_back(start-xSide-ySide);

		std::vector<VEC3> goals;
		goals.push_back(start);
		int r=0;
		while (r<40)
David Cazier's avatar
David Cazier committed
530
		{
pitiot's avatar
merging    
pitiot committed
531
532
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
533
534
			if ((goal-goals[r]).norm2()>1000)
			{
pitiot's avatar
merging    
pitiot committed
535
				goals.push_back(goal);
536
				r++;
pitiot's avatar
merging    
pitiot committed
537
538
			}

David Cazier's avatar
David Cazier committed
539
		}
pitiot's avatar
merging    
pitiot committed
540
541
542
543
544
545
546
547
548
549

		positions[0]=vPos;

		for (int i = 1 ; i < snakeSize ; i++)
		{

			start=start-ySide-ySide;

			vPos.clear();

David Cazier's avatar
David Cazier committed
550
551
552
553
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
pitiot's avatar
merging    
pitiot committed
554
			positions[i]=vPos;
David Cazier's avatar
David Cazier committed
555
		}
pitiot's avatar
merging    
pitiot committed
556
557
558
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
559

pitiot's avatar
merging    
pitiot committed
560
561
		for (int i = 0 ; i < snakeSize ; i++)
			movingObstacles_.push_back(art->members[i]);
David Cazier's avatar
David Cazier committed
562
	}
Thomas's avatar
Thomas committed
563
564
}

David Cazier's avatar
David Cazier committed
565
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
566
{
David Cazier's avatar
David Cazier committed
567
568
569
570
571
	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
572

David Cazier's avatar
David Cazier committed
573
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
574
	{
David Cazier's avatar
David Cazier committed
575
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
576
		{
577
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
David Cazier's avatar
David Cazier committed
578
			addAgent(posagent, -1.0f * posagent) ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
579
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
580
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
581
582
}

Thomas Jund's avatar
Thomas Jund committed
583
void Simulator::setupScenario(unsigned int nbMaxAgent, bool pedWay)
Pierre Kraemer's avatar
Pierre Kraemer committed
584
585
{
	/*
David Cazier's avatar
David Cazier committed
586
	 * Add agents, specifying their start position, and store their goal on the
David Cazier's avatar
David Cazier committed
587
588
	 * opposite side of the environment.
	 */
589
590
	TraversorF<PFP::MAP> tF(envMap_.map);
	Dart d = tF.begin() ;
David Cazier's avatar
David Cazier committed
591

592
593
	unsigned int nbx = 2 ;
	unsigned int nby = 2 ;
David Cazier's avatar
David Cazier committed
594
595
596

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

David Cazier's avatar
David Cazier committed
597
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
598
	{
David Cazier's avatar
David Cazier committed
599
600
601
		bool found = false ;
		VEC3 pos ;
		Dart dCell ;
602
		while (!found && d != tF.end())
David Cazier's avatar
David Cazier committed
603
		{
604
			if (!envMap_.buildingMark.isMarked(d)
Thomas Jund's avatar
Thomas Jund committed
605
			    && (!pedWay || envMap_.pedWayMark.isMarked(d))
606
			    )
David Cazier's avatar
David Cazier committed
607
			{
608
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
609
610
611
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
612
			}
613
			d = tF.next() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
614
615
		}

David Cazier's avatar
David Cazier committed
616
617
		if (found)
		{
618
			float ecart = 3.0f ;
619
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
620
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
David Cazier's avatar
David Cazier committed
621
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
622
			{
David Cazier's avatar
David Cazier committed
623
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
624
				{
625
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
626
					addAgent(posagent, -1.0f * posagent, dCell) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
627
628
629
630
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
631
632
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
633
}
634

635
636
637
638
639
640
641
642
643
644
645
646
647
648
void Simulator::addMovingObstacles(unsigned int nb)
{
	TraversorF<PFP::MAP> tF(envMap_.map);
	Dart d = tF.begin() ;
	for (unsigned int i = 0 ; i < nb && d != envMap_.map.end() ; ++i)
	{
		bool found = false ;
		Dart dCell ;
		while (!found && d != tF.end())
		{
			if (!envMap_.buildingMark.isMarked(d)
				&& !envMap_.pedWayMark.isMarked(d)
				)
			{
649
650
651
652
653
654
				float area = Algo::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
				if(area>1400)
				{
					dCell = d ;
					found = true ;
				}
655
656
657
658
659
			}
			d = tF.next() ;
		}

		if (found)
660
			addMovingObstacle(dCell,1) ;
661
662
663
664
665
666
667
	}
}

void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
{
	std::vector<VEC3> vPos;
	VEC3 start;
Thomas Jund's avatar
Thomas Jund committed
668
	float maxHeight=5.0f;
669

670
671
	MovingMesh* mm = NULL;

672
673
674
675
676
677
678
679
680
681
682
683
684
685
	switch(obstType)
	{
		case 0 :
		{
			start = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
			VEC3 xSide (2.0f,0.0f,0.0f);
			VEC3 ySide (0.0f,5.0f,0.0f);

			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
		}
			break;
686
687
		case 1 :
		{
Thomas Jund's avatar
Thomas Jund committed
688
			mm = new MovingMesh(envMap_, d, "meshRessources/Limace.ply");
689
			movingMeshes_.push_back(mm);
690
			vPos = mm->computeProjectedPointSet(maxHeight);
691
692
		}
			break;
693
694
695
696
697
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

Thomas Jund's avatar
Thomas Jund committed
698
699
	bool orientAccordingToFace=true;
	if(mm!=NULL && orientAccordingToFace)
Thomas Jund's avatar
Thomas Jund committed
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
	{
		while(envMap_.pedWayMark.isMarked(envMap_.map.phi2(d)))
		{
			d = envMap_.map.phi1(d);
		}

		VEC3 mid = (envMap_.position[d]+envMap_.position[envMap_.map.phi1(d)])*0.5f;
		VEC3 front = vPos[0];

		VEC3 bary(0);
		for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
			bary += *it;

		bary /= vPos.size();

		float angle = get_angle(mid - bary,front  - bary);

		TraversorV<PFP::MAP> tv(mm->map);
		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
		{
Thomas Jund's avatar
Thomas Jund committed
720
			mm->position[d] += rotate(mm->position[d], bary, angle);
Thomas Jund's avatar
Thomas Jund committed
721
		}
Thomas Jund's avatar
Thomas Jund committed
722
723
724

		for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
			*it += rotate(*it , bary, angle);
Thomas Jund's avatar
Thomas Jund committed
725
726
	}

727
728
	std::vector<VEC3> goals;
	goals.push_back(start);
729
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), false);
730
731

	movingObstacles_.push_back(mo);
732

733
	if(mm != NULL)
734
	{
735
		mo->attachMesh(mm);
Thomas Jund's avatar
Thomas Jund committed
736
737
738
//		std::cout << "here you go" << std::endl;
//		addAgent(vPos[0], vPos[0], d);
//		mo->attachAgent(agents_.back());
739
	}
740
741
742
743
744
745
746
747
748
}

void Simulator::addPathToObstacles()
{
	unsigned int dartDistForPath = 50 ;

	for (std::vector<MovingObstacle*>::iterator it = movingObstacles_.begin() ; it != movingObstacles_.end() ; ++it)
	{
		Dart dStart = (*it)->registering_part->d;
749
750
751
752
753
754
755
756
757

		while(envMap_.pedWayMark.isMarked(dStart))
		{
			if(!envMap_.pedWayMark.isMarked(envMap_.map.phi2(dStart)))
				dStart = envMap_.map.phi2(dStart);
			else
				dStart = envMap_.map.phi1(dStart);
		}

758
759
760
761
762
		Dart dStop = dStart;

		for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop)
			|| j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStart)
763
			|| envMap_.pedWayMark.isMarked(dStop)
764
765
766
767
768
769
770
771
772
773
774
			; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
		}

		addPathToObstacle(*it, dStart, dStop);
	}
}

775
776
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
David Cazier's avatar
David Cazier committed
777
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
778
779
																   envMap_.position, dStart,
																   dGoal,
780
																   envMap_.pedWayMark) ;
781
782
783

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
784
785
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;
786
787
788

		mo->goals_.push_back(dest) ;
	}
789
790
791
792
793

	path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
												envMap_.position,
												dGoal,
												dStart,
794
												envMap_.pedWayMark) ;
795
796
797

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
798
799
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;
800
801
802

		mo->goals_.push_back(dest) ;
	}
803

Thomas Jund's avatar
Thomas Jund committed
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
//	if(((mo->front-mo->center)*(mo->goals_[mo->curGoal_]-mo->center))<0)
//	{
//		std::cout << __FILE__ << " " << __LINE__ << " rotate limace" << std::endl;
//
//		Geom::Matrix44f m;
//		m.identity();
//		rotateZ(180.0f,m);
//
//		TraversorV<PFP::MAP> tv(mo->map);
//		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
//		{
//			mo->position[d] -= mo->center;
//			mo->position[d] = Geom::transform(mo->position[d],m);
//			mo->position[d] += mo->center;
//			mo->position[d][2] += 10.0f;
//		}
//	}
821
822
}

Pierre Kraemer's avatar
Pierre Kraemer committed
823
#ifndef SPATIAL_HASHING
824
825
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
826
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
827
	{
David Cazier's avatar
David Cazier committed
828
829
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
830

David Cazier's avatar
David Cazier committed
831
832
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
833

David Cazier's avatar
David Cazier committed
834
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
835
836
837
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
838

David Cazier's avatar
David Cazier committed
839
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
840
		{
841
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
842
843
844
845
846

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

David Cazier's avatar
David Cazier committed
847
			agents_[i]->goals_.push_back(dest) ;
848
849
850
851
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
852
853
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
854
	//city
David Cazier's avatar
David Cazier committed
855
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
856
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
857
	{
David Cazier's avatar
David Cazier committed
858
859
		agents_[i]->goals_.clear() ;

860
861
		TraversorF<PFP::MAP> tF(envMap_.map);

David Cazier's avatar
David Cazier committed
862
863
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
864
865
866
867
868
869
870
		for (unsigned int j = 0 ;
			/*!envMap_.pedWayMark.isMarked(dStop) || */
			envMap_.buildingMark.isMarked(dStop)
		    || j < dartDistForPath + rand() * 20
		    || envMap_.map.sameFace(dStop, dStart)
		    || envMap_.obstacleMark.isMarked(dStop)
		    ; ++j)
David Cazier's avatar
David Cazier committed
871
		{
David Cazier's avatar
David Cazier committed
872
			envMap_.map.next(dStop) ;
873
874
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
875
		}
876

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

David Cazier's avatar
David Cazier committed
879
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
880
881
882
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
883

David Cazier's avatar
David Cazier committed
884
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
885
		{
Thomas Jund's avatar
Thomas Jund committed
886
887
888
889
890
891
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

892
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
Thomas Jund's avatar
Thomas Jund committed
893
894

			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
David Cazier's avatar
David Cazier committed
895
			dest /= 2.0f ;
Thomas Jund's avatar
Thomas Jund committed
896
			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
David Cazier's avatar
David Cazier committed
897
			dest[2] = 0 ;
898

David Cazier's avatar
David Cazier committed
899
			agents_[i]->goals_.push_back(dest) ;
900
		}
901

David Cazier's avatar
David Cazier committed
902
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
903
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
904
905
906
907
908
909
		    envMap_.buildingMark.isMarked(dStop2)
		    || j < dartDistForPath + rand() * 20
		    || envMap_.map.sameFace(dStop, dStop2)
		    || envMap_.map.sameFace(dStop2, dStart)
		    || envMap_.obstacleMark.isMarked(dStop)
		    ; ++j)
David Cazier's avatar
David Cazier committed
910
		{
David Cazier's avatar
David Cazier committed
911
			envMap_.map.next(dStop2) ;
912
913
			if (dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
914
		}
915

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

David Cazier's avatar
David Cazier committed
918
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_, envMap_.position, dStop, dStop2,
David Cazier's avatar
David Cazier committed
919
		                                             envMap_.buildingMark) ;
920

David Cazier's avatar
David Cazier committed
921
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
922
		{
Thomas Jund's avatar
Thomas Jund committed
923
924
925
926
927
928
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

929
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
Thomas Jund's avatar
Thomas Jund committed
930
			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
David Cazier's avatar
David Cazier committed
931
			dest /= 2.0f ;
Thomas Jund's avatar
Thomas Jund committed
932
			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
David Cazier's avatar
David Cazier committed
933
			dest[2] = 0 ;
934

David Cazier's avatar
David Cazier committed
935
			agents_[i]->goals_.push_back(dest) ;
936
		}
937

David Cazier's avatar
David Cazier committed
938
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_, envMap_.position, dStop2, dStart,
David Cazier's avatar
David Cazier committed
939
		                                             envMap_.buildingMark) ;
940

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

David Cazier's avatar
David Cazier committed
943
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
944
		{
945
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
946
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
947
			dest /= 2.0f ;
Thomas Jund's avatar
Thomas Jund committed
948
			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
David Cazier's avatar
David Cazier committed
949
			dest[2] = 0 ;
950

David Cazier's avatar
David Cazier committed
951
			agents_[i]->goals_.push_back(dest) ;
952
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
953
954
	}
}
955

Thomas's avatar
Thomas committed
956
957
958
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
959
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
960
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
961
	{
David Cazier's avatar
David Cazier committed
962
963
964
965
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
966
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
967
968
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
969
		{
David Cazier's avatar
David Cazier committed
970
971
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
972
973
		}

David Cazier's avatar
David Cazier committed
974
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
975
976
977
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
978

David Cazier's avatar
David Cazier committed
979
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
980
		{
981
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
982
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
983
984
		}

David Cazier's avatar
David Cazier committed
985
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
986
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
987
988
989
		    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
990
		{
David Cazier's avatar
David Cazier committed
991
992
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
993
994
		}

David Cazier's avatar
David Cazier committed
995
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_, envMap_.position, dStop, dStop2,
David Cazier's avatar
David Cazier committed
996
		                                             envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
997

David Cazier's avatar
David Cazier committed
998
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
999
		{
1000
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
1001
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
1002
1003
		}

David Cazier's avatar
David Cazier committed
1004
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_, envMap_.position, dStop2, dStart,
David Cazier's avatar
David Cazier committed
1005
		                                             envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
1006

David Cazier's avatar
David Cazier committed
1007
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)