simulator.cpp 29.7 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

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

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

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

Arash HABIBI's avatar
nickel    
Arash HABIBI committed
118
119
		// commente par Arash
		// 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
//		if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE))
165
			if (agents_[i]->part_.crossCell != CGoGN::Algo::Surface::MovingObjects::NO_CROSS)
David Cazier's avatar
David Cazier committed
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
//		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
		}

		vPos.clear();
Arash HABIBI's avatar
nickel    
Arash HABIBI committed
287

pitiot's avatar
pitiot committed
288
289
290
291
292
		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
	// 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);
		}
Thomas Jund's avatar
Thomas Jund committed
416
417
		else
		{
David Cazier's avatar
David Cazier committed
418
419
420
421
422
423
424
425
			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);
426
		mo4= new MovingObstacle(this, i,vPos,goals, true, false);
David Cazier's avatar
David Cazier committed
427
428


429
430
431
432
433
434
435
436
437
438
439
440
441
442
//		//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
443
444
445
446
447

		movingObstacles_.push_back(mo4);
	}
}

pitiot's avatar
merging    
pitiot committed
448
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
David Cazier's avatar
David Cazier committed
449
{
pitiot's avatar
merging    
pitiot committed
450
451
452
453
454
455
	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
456
457

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

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

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

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

Thomas's avatar
Thomas committed
491
492
	}

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

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

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

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

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

pitiot's avatar
merging    
pitiot committed
508
509
510

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

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

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

		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
531
		{
pitiot's avatar
merging    
pitiot committed
532
533
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
534
535
			if ((goal-goals[r]).norm2()>1000)
			{
pitiot's avatar
merging    
pitiot committed
536
				goals.push_back(goal);
537
				r++;
pitiot's avatar
merging    
pitiot committed
538
539
			}

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

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

David Cazier's avatar
David Cazier committed
551
552
553
554
			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
555
			positions[i]=vPos;
David Cazier's avatar
David Cazier committed
556
		}
pitiot's avatar
merging    
pitiot committed
557
558
559
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
560

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

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

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

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

Arash HABIBI's avatar
nickel    
Arash HABIBI committed
593
594
	unsigned int nbx = 4 ;
	unsigned int nby = 4 ;
David Cazier's avatar
David Cazier committed
595
596
597

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

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

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

636
637
638
639
640
641
642
643
644
645
646
647
648
649
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)
				)
			{
650
				float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
651
652
653
654
655
				if(area>1400)
				{
					dCell = d ;
					found = true ;
				}
656
657
658
659
660
			}
			d = tF.next() ;
		}

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

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

671
672
	MovingMesh* mm = NULL;

673
674
675
676
	switch(obstType)
	{
		case 0 :
		{
677
			start = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
678
679
680
681
682
683
			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);
Thomas Jund's avatar
Thomas Jund committed
684
			vPos.push_back(start-xSide+ySide);
685
686
		}
			break;
687
688
		case 1 :
		{
Thomas Jund's avatar
Thomas Jund committed
689
			mm = new MovingMesh(envMap_, d, "./meshRessources/Limace_2.obj");
690
			movingMeshes_.push_back(mm);
691
			vPos = mm->computeProjectedPointSet(maxHeight);
Thomas Jund's avatar
Thomas Jund committed
692
693
694
695
696
697
698
699
700
701

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

			float scale = 1.2f;
			for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
				*it = *it*scale - bary*(scale-1.0f);
702
703
		}
			break;
704
705
706
707
708
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

Thomas Jund's avatar
Thomas Jund committed
709
710
	bool orientAccordingToFace=true;
	if(mm!=NULL && orientAccordingToFace)
Thomas Jund's avatar
Thomas Jund committed
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
	{
		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);

Thomas Jund's avatar
Thomas Jund committed
728
		TraversorV<PFP2::MAP> tv(mm->map);
Thomas Jund's avatar
Thomas Jund committed
729
730
		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
		{
Thomas Jund's avatar
Thomas Jund committed
731
			mm->position[d] += rotate(mm->position[d], bary, angle);
Thomas Jund's avatar
Thomas Jund committed
732
		}
Thomas Jund's avatar
Thomas Jund committed
733
734
735

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

738
739
	std::vector<VEC3> goals;
	goals.push_back(start);
740
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), false);
741
742

	movingObstacles_.push_back(mo);
743

744
	if(mm != NULL)
745
	{
746
		mo->attachMesh(mm);
Thomas Jund's avatar
Thomas Jund committed
747
748
749
//		std::cout << "here you go" << std::endl;
//		addAgent(vPos[0], vPos[0], d);
//		mo->attachAgent(agents_.back());
750
	}
751
752
753
754
755
756
757
758
759
}

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;
760
761
762
763
764
765
766
767
768

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

769
770
771
772
773
		Dart dStop = dStart;

		for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop)
			|| j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStart)
774
			|| envMap_.pedWayMark.isMarked(dStop)
775
776
777
778
779
780
781
782
783
784
785
			; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
		}

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

786
787
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
David Cazier's avatar
David Cazier committed
788
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
789
790
																   envMap_.position, dStart,
																   dGoal,
791
																   envMap_.pedWayMark) ;
792
793
794

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

		mo->goals_.push_back(dest) ;
	}
800
801
802
803
804

	path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
												envMap_.position,
												dGoal,
												dStart,
805
												envMap_.pedWayMark) ;
806
807
808

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
809
810
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;
811
812
813

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

Thomas Jund's avatar
Thomas Jund committed
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
//	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;
//		}
//	}
832
833
}

Pierre Kraemer's avatar
Pierre Kraemer committed
834
#ifndef SPATIAL_HASHING
835
836
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
837
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
838
	{
David Cazier's avatar
David Cazier committed
839
840
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
841

David Cazier's avatar
David Cazier committed
842
843
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
844

David Cazier's avatar
David Cazier committed
845
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
846
847
848
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
849

David Cazier's avatar
David Cazier committed
850
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
851
		{
852
			VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
853
854
855
856
857

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

David Cazier's avatar
David Cazier committed
858
			agents_[i]->goals_.push_back(dest) ;
859
860
861
862
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
863
864
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
865
	//city
David Cazier's avatar
David Cazier committed
866
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
867
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
868
	{
David Cazier's avatar
David Cazier committed
869
870
		agents_[i]->goals_.clear() ;

871
872
		TraversorF<PFP::MAP> tF(envMap_.map);

David Cazier's avatar
David Cazier committed
873
874
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
875
876
877
878
879
880
881
		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
882
		{
David Cazier's avatar
David Cazier committed
883
			envMap_.map.next(dStop) ;
884
885
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
886
		}
887

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

David Cazier's avatar
David Cazier committed
890
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
891
892
893
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
894

David Cazier's avatar
David Cazier committed
895
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
896
		{
Thomas Jund's avatar
Thomas Jund committed
897
898
899
900
901
902
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

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

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

David Cazier's avatar
David Cazier committed
910
			agents_[i]->goals_.push_back(dest) ;
911
		}
912

David Cazier's avatar
David Cazier committed
913
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
914
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
915
916
917
918
919
920
		    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
921
		{
David Cazier's avatar
David Cazier committed
922
			envMap_.map.next(dStop2) ;
923
924
			if (dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
925
		}
926

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

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

David Cazier's avatar
David Cazier committed
932
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
933
		{
Thomas Jund's avatar
Thomas Jund committed
934
935
936
937
938
939
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

940
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
Thomas Jund's avatar
Thomas Jund committed
941
			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
David Cazier's avatar
David Cazier committed
942
			dest /= 2.0f ;
Thomas Jund's avatar
Thomas Jund committed
943
			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
David Cazier's avatar
David Cazier committed
944
			dest[2] = 0 ;
945

David Cazier's avatar
David Cazier committed
946
			agents_[i]->goals_.push_back(dest) ;
947
		}
948

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

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

David Cazier's avatar
David Cazier committed
954
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
955
		{
956
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
957
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
958
			dest /= 2.0f ;
Thomas Jund's avatar
Thomas Jund committed
959
			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
David Cazier's avatar
David Cazier committed
960
			dest[2] = 0 ;
961

David Cazier's avatar
David Cazier committed
962
			agents_[i]->goals_.push_back(dest) ;
963
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
964
965
	}
}
966

Thomas's avatar
Thomas committed
967
968
969
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
970
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
971
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
972
	{
David Cazier's avatar
David Cazier committed
973
974
975
976
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
977
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
978
979
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
980
		{
David Cazier's avatar
David Cazier committed
981
982
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
983
984
		}

David Cazier's avatar
David Cazier committed
985
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
986
987
988
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
989

David Cazier's avatar
David Cazier committed
990
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
991
		{
992
			VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
993
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
994
995
		}

David Cazier's avatar
David Cazier committed
996
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
997
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
998
999
1000
		    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
1001
		{
David Cazier's avatar
David Cazier committed
1002
1003
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
1004
1005
		}