simulator.cpp 30.2 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),
pitiot's avatar
pitiot committed
17
	nb_dead(0)
Pierre Kraemer's avatar
Pierre Kraemer committed
18
{
pitiot's avatar
pitiot committed
19
	minSize=minS;
pitiot's avatar
pitiot committed
20
	multires=resolution;
pitiot's avatar
pitiot committed
21
	detect_agent_collision=false;
22
23
	srand(10) ;
	nbStepsPerUnit_ = 1 / timeStep_ ;
pitiot's avatar
merging    
pitiot committed
24
	this->config=config;
25
	init(2.0f, nbAgent, nbObst) ;
Thomas's avatar
Thomas committed
26
27
28
29
}

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

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

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

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

David Cazier's avatar
David Cazier committed
89
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
90
	{
David Cazier's avatar
David Cazier committed
91
92
		agents_[i]->updateObstacleNeighbors() ;
		agents_[i]->updateAgentNeighbors() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
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();
114
		movingObstacles_[i]->updateObstacleNeighbors();
pitiot's avatar
pitiot committed
115
116
117
#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
David Cazier's avatar
David Cazier committed
118
		movingObstacles_[i]->update() ;
119

Arash HABIBI's avatar
nickel    
Arash HABIBI committed
120
		// commente par Arash
121
		 movingObstacles_[i]->updateMesh() ;
pitiot's avatar
pitiot committed
122
123
	}

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

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

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

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

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

Thomas Jund's avatar
Thomas Jund committed
186
187
	 if (multires)
		 envMap_.updateMap() ;
David Cazier's avatar
David Cazier committed
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
#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
204
void Simulator::addAgent(const VEC3& start, const VEC3& goal)
David Cazier's avatar
David Cazier committed
205
{
David Cazier's avatar
David Cazier committed
206
	agents_.push_back(new Agent(this, start, goal)) ;
David Cazier's avatar
David Cazier committed
207
208
}

209
210
211
212
213
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
214
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
David Cazier's avatar
David Cazier committed
215
{
pitiot's avatar
merging    
pitiot committed
216
217
218
219
220
221
222
	if (multires)
	{
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
	}
	else
	{

pitiot's avatar
blah    
pitiot committed
223
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //cases fines
pitiot's avatar
merging    
pitiot committed
224
225
	}

David Cazier's avatar
David Cazier committed
226
227
228
229
230
231
232
233
234
235
236
237
	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents" << std::endl ;

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

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

	// Départ des agents sur un cercle centré
	int xSize = envMap_.geometry.size(0) - 2 * xBorder ;
	int ySize = envMap_.geometry.size(1) - 2 * yBorder ;
	int radius = min(xSize, ySize) / 2 ;
pitiot's avatar
pitiot committed
238
//	int radius = nbAgents/(Agent::radius_*Agent::radius_);
Thomas Jund's avatar
Thomas Jund committed
239
	std::cout << "radius " << radius << std::endl;
David Cazier's avatar
David Cazier committed
240
241
242
243
	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
244
		double angle = i * 2.0f * M_PI / float(nbAgents) ;
David Cazier's avatar
David Cazier committed
245
246
247
		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) ;
pitiot's avatar
pitiot committed
249
//		CGoGNout<<"agent n°" << agents_[i]->agentNo << " : "<< agents_[i]<<CGoGNendl;
250
	}
pitiot's avatar
pitiot committed
251
252
253
254
255
	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
256
	int curGoal=0;
Thomas Jund's avatar
merge    
Thomas Jund committed
257

pitiot's avatar
pitiot committed
258
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
David Cazier's avatar
David Cazier committed
259
	{
Thomas Jund's avatar
merge    
Thomas Jund committed
260
261
		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
262
263
264
265
266
267
268
		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
269
			v=VEC3 (std::cos(angle) * (radius/2), std::sin(angle) * (radius/2), 0) ;
pitiot's avatar
pitiot committed
270
271
272
		}
		start = center + v ;
		positions.push_back(start);
pitiot's avatar
pitiot committed
273
	}
Thomas Jund's avatar
merge    
Thomas Jund committed
274

pitiot's avatar
pitiot committed
275
276
277
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
278
279

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

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

pitiot's avatar
pitiot committed
298
299
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
300

pitiot's avatar
pitiot committed
301
302
303
	for (unsigned int i = nbObstacles/2 ; i < nbObstacles ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
304
305
306
307
308
309
310
//		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
311
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
312
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
313
		{
Thomas Jund's avatar
Thomas Jund committed
314
			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
315
316
317
318
319
320
321
322
		}

		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);
323
324
		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
325

326
327
//		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
328
329

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
330
	}
Thomas's avatar
Thomas committed
331

332
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
333
334
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
335
#endif
pitiot's avatar
merging    
pitiot committed
336
337

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

David Cazier's avatar
David Cazier committed
340
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
341
{
pitiot's avatar
merging    
pitiot committed
342
343
344
345
346
347
348
349
350
351
352
	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
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
386
387
388
389
	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
390

David Cazier's avatar
David Cazier committed
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
	// 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
420
421
		else
		{
David Cazier's avatar
David Cazier committed
422
423
424
425
426
427
428
429
			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
430

pitiot's avatar
pitiot committed
431
		mo4= new MovingObstacle(this, i,vPos,goals, true, false,1);
David Cazier's avatar
David Cazier committed
432
433


434
435
436
437
438
439
440
441
442
443
444
445
446
447
//		//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
448
449
450
451
452

		movingObstacles_.push_back(mo4);
	}
}

pitiot's avatar
merging    
pitiot committed
453
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
David Cazier's avatar
David Cazier committed
454
{
pitiot's avatar
merging    
pitiot committed
455
456
457
458
459
460
	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
461
462

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

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

David Cazier's avatar
David Cazier committed
485
		// Un agent sur 2 va de droite à gauche
pitiot's avatar
merging    
pitiot committed
486
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
487
		if (i % 2 == 1)
pitiot's avatar
merging    
pitiot committed
488
489
490
491
492
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
		}
pitiot's avatar
merge    
pitiot committed
493

pitiot's avatar
merging    
pitiot committed
494
		addAgent(start, goal) ;
pitiot's avatar
merge    
pitiot committed
495

Thomas's avatar
Thomas committed
496
497
	}

David Cazier's avatar
David Cazier committed
498
499
500
501
	// 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
502
	yStartDelta = envMap_.geometry.size(1) / 20 ;
David Cazier's avatar
David Cazier committed
503
504

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

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

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

510
	VEC3 xSide (5.0f,0.0f,0.0f);
511
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
512

pitiot's avatar
merging    
pitiot committed
513
514
515

	int sumObstacles=0;
	for(unsigned int j = 0; j<nbSnakes; j++)
David Cazier's avatar
David Cazier committed
516
	{
pitiot's avatar
merging    
pitiot committed
517
518
519
520
		std::vector<PFP::VEC3> positions [snakeSize] ;
		float x = xStartMin + rand() % xStartDelta;
		VEC3 goal;

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

pitiot's avatar
merging    
pitiot committed
523
524
525
526
527
528
529
530
531
532
533
534
535

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

David Cazier's avatar
David Cazier committed
545
		}
pitiot's avatar
merging    
pitiot committed
546
547
548
549
550
551
552
553
554
555

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

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

pitiot's avatar
merging    
pitiot committed
566
567
		for (int i = 0 ; i < snakeSize ; i++)
			movingObstacles_.push_back(art->members[i]);
David Cazier's avatar
David Cazier committed
568
	}
Thomas's avatar
Thomas committed
569
570
}

David Cazier's avatar
David Cazier committed
571
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
572
{
David Cazier's avatar
David Cazier committed
573
574
575
576
577
	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
578

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

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

pitiot's avatar
pitiot committed
598
599
	unsigned int nbx = 4 ;
	unsigned int nby = 4 ;
David Cazier's avatar
David Cazier committed
600
601
602

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

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

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

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

		if (found)
666
			addMovingObstacle(dCell,1) ;
667
668
669
670
671
672
673
	}
}

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

676
677
	MovingMesh* mm = NULL;

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

			//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);
709
710
		}
			break;
711
712
713
714
715
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

Thomas Jund's avatar
Thomas Jund committed
716
717
	bool orientAccordingToFace=true;
	if(mm!=NULL && orientAccordingToFace)
Thomas Jund's avatar
Thomas Jund committed
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
	{
		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
735
		TraversorV<PFP2::MAP> tv(mm->map);
Thomas Jund's avatar
Thomas Jund committed
736
737
		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
		{
Thomas Jund's avatar
Thomas Jund committed
738
			mm->position[d] += rotate(mm->position[d], bary, angle);
Thomas Jund's avatar
Thomas Jund committed
739
		}
Thomas Jund's avatar
Thomas Jund committed
740
741
742

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

745
746
	std::vector<VEC3> goals;
	goals.push_back(start);
Thomas Jund's avatar
merge    
Thomas Jund committed
747

pitiot's avatar
pitiot committed
748
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), (obstType==0));
749
750

	movingObstacles_.push_back(mo);
751

752
	if(mm != NULL)
753
	{
754
		mo->attachMesh(mm);
Thomas Jund's avatar
Thomas Jund committed
755
756
757
//		std::cout << "here you go" << std::endl;
//		addAgent(vPos[0], vPos[0], d);
//		mo->attachAgent(agents_.back());
758
	}
759
760
761
762
763
764
765
766
}

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

	for (std::vector<MovingObstacle*>::iterator it = movingObstacles_.begin() ; it != movingObstacles_.end() ; ++it)
	{
pitiot's avatar
pitiot committed
767
		Dart dStart = (*it)->parts_[0]->d;
768
769
770
771
772
773
774
775
776

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

777
778
779
780
781
		Dart dStop = dStart;

		for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop)
			|| j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStart)
782
			|| envMap_.pedWayMark.isMarked(dStop)
783
784
785
786
787
788
789
790
791
792
793
			; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
		}

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

794
795
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
David Cazier's avatar
David Cazier committed
796
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
797
798
																   envMap_.position, dStart,
																   dGoal,
799
																   envMap_.pedWayMark) ;
800
801
802

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
803
804
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;
805
806
807

		mo->goals_.push_back(dest) ;
	}
808
809
810
811
812

	path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
												envMap_.position,
												dGoal,
												dStart,
813
												envMap_.pedWayMark) ;
814
815
816

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
817
818
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;
819
820
821

		mo->goals_.push_back(dest) ;
	}
pitiot's avatar
pitiot committed
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839

//	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;
//		}
//	}
840
841
}

Pierre Kraemer's avatar
Pierre Kraemer committed
842
#ifndef SPATIAL_HASHING
843
844
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
845
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
846
	{
David Cazier's avatar
David Cazier committed
847
848
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
849

David Cazier's avatar
David Cazier committed
850
851
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
852

David Cazier's avatar
David Cazier committed
853
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
854
855
856
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
857

David Cazier's avatar
David Cazier committed
858
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
859
		{
860
			VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
861
862
863
864
865

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

David Cazier's avatar
David Cazier committed
866
			agents_[i]->goals_.push_back(dest) ;
867
868
869
870
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
871
872
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
873
	//city
David Cazier's avatar
David Cazier committed
874
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
875
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
876
	{
David Cazier's avatar
David Cazier committed
877
878
		agents_[i]->goals_.clear() ;

879
880
		TraversorF<PFP::MAP> tF(envMap_.map);

David Cazier's avatar
David Cazier committed
881
882
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
883
884
885
886
887
888
889
		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
890
		{
David Cazier's avatar
David Cazier committed
891
			envMap_.map.next(dStop) ;
892
893
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
894
		}
895

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

David Cazier's avatar
David Cazier committed
898
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
899
900
901
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
902

David Cazier's avatar
David Cazier committed
903
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
904
		{
Thomas Jund's avatar
Thomas Jund committed
905
906
907
908
909
910
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

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

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

David Cazier's avatar
David Cazier committed
918
			agents_[i]->goals_.push_back(dest) ;
919
		}
920

David Cazier's avatar
David Cazier committed
921
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
922
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
923
924
925
926
927
928
		    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
929
		{
David Cazier's avatar
David Cazier committed
930
			envMap_.map.next(dStop2) ;
931
932
			if (dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
933
		}
934

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

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

David Cazier's avatar
David Cazier committed
940
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
941
		{
Thomas Jund's avatar
Thomas Jund committed
942
943
944
945
946
947
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

948
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
Thomas Jund's avatar
Thomas Jund committed
949
			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
David Cazier's avatar
David Cazier committed
950
			dest /= 2.0f ;
Thomas Jund's avatar
Thomas Jund committed
951
			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
David Cazier's avatar
David Cazier committed
952
			dest[2] = 0 ;
953

David Cazier's avatar
David Cazier committed
954
			agents_[i]->goals_.push_back(dest) ;
955
		}
956

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

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

David Cazier's avatar
David Cazier committed
962
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
963
		{
964
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
965
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
966
			dest /= 2.0f ;
Thomas Jund's avatar
Thomas Jund committed
967
			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
David Cazier's avatar
David Cazier committed
968
			dest[2] = 0 ;
969

David Cazier's avatar
David Cazier committed
970
			agents_[i]->goals_.push_back(dest) ;
971
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
972
973
	}
}
974

Thomas's avatar
Thomas committed
975
976
977
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
978
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
979
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
980
	{
David Cazier's avatar
David Cazier committed
981
982
983
984
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
985
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
986
987
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
988
		{
David Cazier's avatar
David Cazier committed
989
990
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;