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

Thomas Jund's avatar
merge    
Thomas Jund committed
4

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

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

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

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

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

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

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

David Cazier's avatar
David Cazier committed
104
void Simulator::doStep()
Pierre Kraemer's avatar
Pierre Kraemer committed
105
{
David Cazier's avatar
David Cazier committed
106
107
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
Jund Thomas's avatar
Jund Thomas committed
108
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
109
#endif
110
111
	unsigned long begTime, endTime;
	begTime=clock() ;
David Cazier's avatar
David Cazier committed
112
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
pitiot's avatar
pitiot committed
113
	{
David Cazier's avatar
David Cazier committed
114
		movingObstacles_[i]->computePrefVelocity() ;
pitiot's avatar
pitiot committed
115
		movingObstacles_[i]->computeNewVelocity() ;
116

Thomas Jund's avatar
Thomas Jund committed
117
		movingObstacles_[i]->updateAgentNeighbors();
118
		movingObstacles_[i]->updateObstacleNeighbors();
pitiot's avatar
pitiot committed
119
120
121
#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
David Cazier's avatar
David Cazier committed
122
		movingObstacles_[i]->update() ;
123

Arash HABIBI's avatar
nickel    
Arash HABIBI committed
124
		// commente par Arash
125
		 movingObstacles_[i]->updateMesh() ;
pitiot's avatar
pitiot committed
126
	}
127
128
129
	endTime=clock() ;
	time_obstacle+= endTime-begTime;
	begTime=clock() ;
David Cazier's avatar
David Cazier committed
130
131
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
pitiot's avatar
pitiot committed
132
133
		if (agents_[i]->alive)
		{
134

pitiot's avatar
pitiot committed
135
			agents_[i]->updateObstacleNeighbors() ;
pitiot's avatar
maj    
pitiot committed
136
137

#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
138
			envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
139
#endif
pitiot's avatar
pitiot committed
140
			agents_[i]->updateAgentNeighbors() ;
pitiot's avatar
maj    
pitiot committed
141
#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
142
			envMap_.map.setCurrentLevel(0) ;
pitiot's avatar
maj    
pitiot committed
143
144
#endif

pitiot's avatar
pitiot committed
145
146
147
			agents_[i]->computePrefVelocity() ;
			agents_[i]->computeNewVelocity() ;
		}
David Cazier's avatar
David Cazier committed
148
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
149

Pierre Kraemer's avatar
Pierre Kraemer committed
150
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
151
152
153
154
155
156
157
158
159
160
161
	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
162
#else
163
164
	if (multires)
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
165
166
167
168
169
170
171
	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
172

David Cazier's avatar
David Cazier committed
173
//		if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE))
174
			if (agents_[i]->part_.crossCell != CGoGN::Algo::Surface::MovingObjects::NO_CROSS)
David Cazier's avatar
David Cazier committed
175
176
177
178
179
180
181
182
183
184
185
186
187
188
//		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++ ;

	}
189
190
	endTime=clock() ;
	time_agent+= endTime-begTime;
David Cazier's avatar
David Cazier committed
191
192
193

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

Thomas Jund's avatar
Thomas Jund committed
195
196
	 if (multires)
		 envMap_.updateMap() ;
David Cazier's avatar
David Cazier committed
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
#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
213
void Simulator::addAgent(const VEC3& start, const VEC3& goal)
David Cazier's avatar
David Cazier committed
214
{
David Cazier's avatar
David Cazier committed
215
	agents_.push_back(new Agent(this, start, goal)) ;
David Cazier's avatar
David Cazier committed
216
217
}

218
219
220
221
222
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
223
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
David Cazier's avatar
David Cazier committed
224
{
pitiot's avatar
merging    
pitiot committed
225
226
227
228
229
230
231
	if (multires)
	{
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
	}
	else
	{

pitiot's avatar
blah    
pitiot committed
232
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //cases fines
pitiot's avatar
merging    
pitiot committed
233
234
	}

235
	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles"<< std::endl ;
David Cazier's avatar
David Cazier committed
236
237
238
239
240
241
242
243
244
245
246

	// 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 ;
pitiot's avatar
pitiot committed
247
//	int radius = nbAgents/(Agent::radius_*Agent::radius_);
Thomas Jund's avatar
Thomas Jund committed
248
	std::cout << "radius " << radius << std::endl;
David Cazier's avatar
David Cazier committed
249
250
251
252
	VEC3 center = (envMap_.geometry.min() + envMap_.geometry.max()) / 2 ;

	for (unsigned int i = 0 ; i < nbAgents ; ++i)
	{
Thomas Jund's avatar
merge    
Thomas Jund committed
253
		double angle = i * 2.0f * M_PI / float(nbAgents) ;
David Cazier's avatar
David Cazier committed
254
255
256
		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
257
		addAgent(start, goal) ;
pitiot's avatar
pitiot committed
258
//		CGoGNout<<"agent n°" << agents_[i]->agentNo << " : "<< agents_[i]<<CGoGNendl;
259
	}
pitiot's avatar
pitiot committed
260
261
262
263
264
	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
265
	int curGoal=0;
Thomas Jund's avatar
merge    
Thomas Jund committed
266

pitiot's avatar
pitiot committed
267
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
David Cazier's avatar
David Cazier committed
268
	{
Thomas Jund's avatar
merge    
Thomas Jund committed
269
270
		double angle = nbObstacles<2 ? 0 : i * 2.0f * M_PI / float((float)nbObstacles/2.0f) ;
		//v : position on the circle, start position on the circle relative to the scene
pitiot's avatar
pitiot committed
271
272
273
274
275
276
277
		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
278
			v=VEC3 (std::cos(angle) * (radius/2), std::sin(angle) * (radius/2), 0) ;
pitiot's avatar
pitiot committed
279
280
281
		}
		start = center + v ;
		positions.push_back(start);
pitiot's avatar
pitiot committed
282
	}
Thomas Jund's avatar
merge    
Thomas Jund committed
283

pitiot's avatar
pitiot committed
284
285
286
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
287
288

		unsigned int nbGoals=360;
pitiot's avatar
pitiot committed
289
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
290
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
291
		{
Thomas Jund's avatar
Thomas Jund committed
292
			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
293
294
295
		}

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

pitiot's avatar
pitiot committed
297
298
299
300
		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
pitiot committed
301
		curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+1)%nbGoals;
302
		mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);
pitiot's avatar
pitiot committed
303

304
305
//		mo4= new MovingObstacle(this, i,vPos,goals, true, true);
//		mo4->curGoal_ = i*float(nbGoals)/float(nbObstacles/2.0f);
pitiot's avatar
pitiot committed
306

pitiot's avatar
pitiot committed
307
308
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
309

pitiot's avatar
pitiot committed
310
311
312
	for (unsigned int i = nbObstacles/2 ; i < nbObstacles ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
313
314
315
316
317
318
319
//		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
320
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
321
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
322
		{
Thomas Jund's avatar
Thomas Jund committed
323
			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
324
325
326
327
328
329
330
331
		}

		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);
332
333
		curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+5)%nbGoals;
		mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);
pitiot's avatar
pitiot committed
334

335
336
//		mo4= new MovingObstacle(this, i,vPos,goals, true, true);
//		mo4->curGoal_ = (i-nbObstacles/2)*float(nbGoals)/float(nbObstacles/2.0f);
pitiot's avatar
pitiot committed
337
338

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
339
	}
Thomas's avatar
Thomas committed
340

341
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
342
343
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
344
#endif
pitiot's avatar
merging    
pitiot committed
345
346

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

David Cazier's avatar
David Cazier committed
349
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
350
{
pitiot's avatar
merging    
pitiot committed
351
352
353
354
355
356
357
358
359
360
361
	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
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
	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
399

David Cazier's avatar
David Cazier committed
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
425
426
427
428
	// 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
429
430
		else
		{
David Cazier's avatar
David Cazier committed
431
432
433
434
435
436
437
438
			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);
Thomas Jund's avatar
merge    
Thomas Jund committed
439

pitiot's avatar
pitiot committed
440
		mo4= new MovingObstacle(this, i,vPos,goals, true, false,1);
David Cazier's avatar
David Cazier committed
441
442


443
444
445
446
447
448
449
450
451
452
453
454
455
456
//		//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
457
458
459
460
461

		movingObstacles_.push_back(mo4);
	}
}

pitiot's avatar
merging    
pitiot committed
462
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
David Cazier's avatar
David Cazier committed
463
{
pitiot's avatar
merging    
pitiot committed
464
465
466
467
468
469
	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
470
471

	// Bordure à éviter autour de la scène (10% de sa taille)
pitiot's avatar
merging    
pitiot committed
472
473
	int xBorder = envMap_.geometry.size(0) / 5 ;
	int yBorder = envMap_.geometry.size(1) / 5 ;
David Cazier's avatar
David Cazier committed
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489

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

David Cazier's avatar
David Cazier committed
494
		// Un agent sur 2 va de droite à gauche
pitiot's avatar
merging    
pitiot committed
495
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
496
		if (i % 2 == 1)
pitiot's avatar
merging    
pitiot committed
497
498
499
500
501
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
		}
pitiot's avatar
merge    
pitiot committed
502

pitiot's avatar
merging    
pitiot committed
503
		addAgent(start, goal) ;
pitiot's avatar
merge    
pitiot committed
504

Thomas's avatar
Thomas committed
505
506
	}

David Cazier's avatar
David Cazier committed
507
508
509
510
	// 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
511
	yStartDelta = envMap_.geometry.size(1) / 20 ;
David Cazier's avatar
David Cazier committed
512
513

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

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

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

519
	VEC3 xSide (5.0f,0.0f,0.0f);
520
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
521

pitiot's avatar
merging    
pitiot committed
522
523
524

	int sumObstacles=0;
	for(unsigned int j = 0; j<nbSnakes; j++)
David Cazier's avatar
David Cazier committed
525
	{
pitiot's avatar
merging    
pitiot committed
526
527
528
529
		std::vector<PFP::VEC3> positions [snakeSize] ;
		float x = xStartMin + rand() % xStartDelta;
		VEC3 goal;

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

pitiot's avatar
merging    
pitiot committed
532
533
534
535
536
537
538
539
540
541
542
543
544

		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
545
		{
pitiot's avatar
merging    
pitiot committed
546
547
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
548
549
			if ((goal-goals[r]).norm2()>1000)
			{
pitiot's avatar
merging    
pitiot committed
550
				goals.push_back(goal);
551
				r++;
pitiot's avatar
merging    
pitiot committed
552
553
			}

David Cazier's avatar
David Cazier committed
554
		}
pitiot's avatar
merging    
pitiot committed
555
556
557
558
559
560
561
562
563
564

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

David Cazier's avatar
David Cazier committed
565
566
567
568
			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
569
			positions[i]=vPos;
David Cazier's avatar
David Cazier committed
570
		}
pitiot's avatar
merging    
pitiot committed
571
572
573
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
574

pitiot's avatar
merging    
pitiot committed
575
576
		for (int i = 0 ; i < snakeSize ; i++)
			movingObstacles_.push_back(art->members[i]);
David Cazier's avatar
David Cazier committed
577
	}
Thomas's avatar
Thomas committed
578
579
}

David Cazier's avatar
David Cazier committed
580
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
581
{
David Cazier's avatar
David Cazier committed
582
583
584
585
586
	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
587

David Cazier's avatar
David Cazier committed
588
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
589
	{
David Cazier's avatar
David Cazier committed
590
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
591
		{
592
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
David Cazier's avatar
David Cazier committed
593
			addAgent(posagent, -1.0f * posagent) ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
594
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
595
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
596
597
}

Thomas Jund's avatar
Thomas Jund committed
598
void Simulator::setupScenario(unsigned int nbMaxAgent, bool pedWay)
Pierre Kraemer's avatar
Pierre Kraemer committed
599
600
{
	/*
David Cazier's avatar
David Cazier committed
601
	 * Add agents, specifying their start position, and store their goal on the
David Cazier's avatar
David Cazier committed
602
603
	 * opposite side of the environment.
	 */
604
605
	TraversorF<PFP::MAP> tF(envMap_.map);
	Dart d = tF.begin() ;
David Cazier's avatar
David Cazier committed
606

pitiot's avatar
pitiot committed
607
608
	unsigned int nbx = 4 ;
	unsigned int nby = 4 ;
David Cazier's avatar
David Cazier committed
609
610
611

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

David Cazier's avatar
David Cazier committed
612
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
613
	{
Thomas Jund's avatar
Thomas Jund committed
614
		VEC3 pos;
David Cazier's avatar
David Cazier committed
615
616
		bool found = false ;
		Dart dCell ;
617
		while (!found && d != tF.end())
David Cazier's avatar
David Cazier committed
618
		{
619
			if (!envMap_.buildingMark.isMarked(d)
Thomas Jund's avatar
Thomas Jund committed
620
			    && (!pedWay || envMap_.pedWayMark.isMarked(d))
621
			    )
David Cazier's avatar
David Cazier committed
622
			{
623
				pos = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
624
625
626
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
627
			}
628
			d = tF.next() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
629
630
		}

David Cazier's avatar
David Cazier committed
631
632
		if (found)
		{
Thomas Jund's avatar
Thomas Jund committed
633
			float ecart = 2.0f*Agent::radius_;
634
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
635
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
David Cazier's avatar
David Cazier committed
636
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
637
			{
David Cazier's avatar
David Cazier committed
638
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
639
				{
640
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
641
					addAgent(posagent, -1.0f * posagent, dCell) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
642
643
644
645
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
646
647
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
648
}
649

650
651
652
653
654
655
656
657
658
659
660
661
662
663
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)
				)
			{
664
				float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
665
666
667
668
669
				if(area>1400)
				{
					dCell = d ;
					found = true ;
				}
670
671
672
673
674
			}
			d = tF.next() ;
		}

		if (found)
675
			addMovingObstacle(dCell,1) ;
676
677
678
679
680
681
682
	}
}

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

685
686
	MovingMesh* mm = NULL;

687
688
689
690
	switch(obstType)
	{
		case 0 :
		{
691
			start = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
692
693
694
695
696
697
			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
698
			vPos.push_back(start-xSide+ySide);
699
700
		}
			break;
701
702
		case 1 :
		{
703
704
705
			mm = new MovingMesh(envMap_, d, "./meshRessources/LimaceLvl1.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/cylinder.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/simpleCyl.obj");
706
			movingMeshes_.push_back(mm);
707
			vPos = mm->computeProjectedPointSet(maxHeight);
Thomas Jund's avatar
Thomas Jund committed
708
709
710
711
712
713
714
715
716
717

			//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);
718
719
		}
			break;
720
721
722
723
724
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

Thomas Jund's avatar
Thomas Jund committed
725
726
	bool orientAccordingToFace=true;
	if(mm!=NULL && orientAccordingToFace)
Thomas Jund's avatar
Thomas Jund committed
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
	{
		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
744
		TraversorV<PFP2::MAP> tv(mm->map);
Thomas Jund's avatar
Thomas Jund committed
745
746
		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
		{
Thomas Jund's avatar
Thomas Jund committed
747
			mm->position[d] += rotate(mm->position[d], bary, angle);
Thomas Jund's avatar
Thomas Jund committed
748
		}
Thomas Jund's avatar
Thomas Jund committed
749
750
751

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

754
755
	std::vector<VEC3> goals;
	goals.push_back(start);
Thomas Jund's avatar
merge    
Thomas Jund committed
756

pitiot's avatar
pitiot committed
757
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), (obstType==0));
758
759

	movingObstacles_.push_back(mo);
760

761
	if(mm != NULL)
762
	{
763
		mo->attachMesh(mm);
Thomas Jund's avatar
Thomas Jund committed
764
765
766
//		std::cout << "here you go" << std::endl;
//		addAgent(vPos[0], vPos[0], d);
//		mo->attachAgent(agents_.back());
767
	}
768
769
770
771
772
773
774
775
}

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

	for (std::vector<MovingObstacle*>::iterator it = movingObstacles_.begin() ; it != movingObstacles_.end() ; ++it)
	{
pitiot's avatar
pitiot committed
776
		Dart dStart = (*it)->parts_[0]->d;
777
778
779
780
781
782
783
784
785

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

786
787
788
789
790
		Dart dStop = dStart;

		for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop)
			|| j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStart)
791
			|| envMap_.pedWayMark.isMarked(dStop)
792
793
794
795
796
797
798
799
800
801
802
			; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
		}

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

803
804
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
David Cazier's avatar
David Cazier committed
805
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
806
807
																   envMap_.position, dStart,
																   dGoal,
808
																   envMap_.pedWayMark) ;
809
810
811

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

		mo->goals_.push_back(dest) ;
	}
817
818
819
820
821

	path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
												envMap_.position,
												dGoal,
												dStart,
822
												envMap_.pedWayMark) ;
823
824
825

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
826
827
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;
828
829
830

		mo->goals_.push_back(dest) ;
	}
pitiot's avatar
pitiot committed
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848

//	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;
//		}
//	}
849
850
}

Pierre Kraemer's avatar
Pierre Kraemer committed
851
#ifndef SPATIAL_HASHING
852
853
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
854
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
855
	{
David Cazier's avatar
David Cazier committed
856
857
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
858

David Cazier's avatar
David Cazier committed
859
860
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
861

David Cazier's avatar
David Cazier committed
862
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
863
864
865
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
866

David Cazier's avatar
David Cazier committed
867
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
868
		{
869
			VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
870
871
872
873
874

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

David Cazier's avatar
David Cazier committed
875
			agents_[i]->goals_.push_back(dest) ;
876
877
878
879
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
880
881
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
882
	//city
David Cazier's avatar
David Cazier committed
883
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
884
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
885
	{
David Cazier's avatar
David Cazier committed
886
887
		agents_[i]->goals_.clear() ;

888
889
		TraversorF<PFP::MAP> tF(envMap_.map);

David Cazier's avatar
David Cazier committed
890
891
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
892
893
894
895
896
897
898
		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
899
		{
David Cazier's avatar
David Cazier committed
900
			envMap_.map.next(dStop) ;
901
902
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
903
		}
904

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

David Cazier's avatar
David Cazier committed
907
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
908
909
910
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
911

David Cazier's avatar
David Cazier committed
912
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
913
		{
Thomas Jund's avatar
Thomas Jund committed
914
915
916
917
918
919
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

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

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

David Cazier's avatar
David Cazier committed
927
			agents_[i]->goals_.push_back(dest) ;
928
		}
929

David Cazier's avatar
David Cazier committed
930
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
931
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
932
933
934
935
936
937
		    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
938
		{
David Cazier's avatar
David Cazier committed
939
			envMap_.map.next(dStop2) ;
940
941
			if (dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
942
		}
943

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

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

David Cazier's avatar
David Cazier committed
949
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
950
		{
Thomas Jund's avatar
Thomas Jund committed
951
952
953
954
955
956
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

957
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
Thomas Jund's avatar
Thomas Jund committed
958
			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
David Cazier's avatar
David Cazier committed
959
			dest /= 2.0f ;
Thomas Jund's avatar
Thomas Jund committed
960
			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
David Cazier's avatar
David Cazier committed
961
			dest[2] = 0 ;
962

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

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

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

David Cazier's avatar
David Cazier committed
971
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
972
		{
973
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
974
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
975
			dest /= 2.0f ;
Thomas Jund's avatar
Thomas Jund committed
976
			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
David Cazier's avatar
David Cazier committed
977
			dest[2] = 0 ;
978

David Cazier's avatar
David Cazier committed
979
			agents_[i]->goals_.push_back(dest) ;
980
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
981
982
	}
}
983

Thomas's avatar
Thomas committed
984
985
986
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
987
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
988
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)