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

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
Thomas Jund's avatar
Thomas Jund committed
163

164
165
	if (multires)
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
Thomas Jund's avatar
Thomas Jund committed
166

David Cazier's avatar
David Cazier committed
167
168
169
170
171
172
173
	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
174

175
			if (agents_[i]->part_.crossCell != CGoGN::Algo::Surface::MovingObjects::NO_CROSS)
David Cazier's avatar
David Cazier committed
176
177
178
179
180
181
				envMap_.agentChangeFace(agents_[i], oldFace) ;
		}
		else
			nb_dead++ ;

	}
182
183
	endTime=clock() ;
	time_agent+= endTime-begTime;
David Cazier's avatar
David Cazier committed
184
185
186

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

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

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

223
	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles"<< std::endl ;
David Cazier's avatar
David Cazier committed
224
225
226
227
228
229
230
231
232
233
234

	// 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
235
//	int radius = nbAgents/(Agent::radius_*Agent::radius_);
Thomas Jund's avatar
Thomas Jund committed
236
	std::cout << "radius " << radius << std::endl;
David Cazier's avatar
David Cazier committed
237
238
239
240
	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
241
		double angle = i * 2.0f * M_PI / float(nbAgents) ;
David Cazier's avatar
David Cazier committed
242
243
244
		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
245
		addAgent(start, goal) ;
pitiot's avatar
pitiot committed
246
//		CGoGNout<<"agent n°" << agents_[i]->agentNo << " : "<< agents_[i]<<CGoGNendl;
247
	}
pitiot's avatar
pitiot committed
248
249
250
251
252
	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
253
	int curGoal=0;
Thomas Jund's avatar
merge    
Thomas Jund committed
254

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

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

		unsigned int nbGoals=360;
pitiot's avatar
pitiot committed
277
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
278
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
279
		{
Thomas Jund's avatar
Thomas Jund committed
280
			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
281
282
283
		}

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

pitiot's avatar
pitiot committed
285
286
287
288
		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
289
		curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+1)%nbGoals;
290
		mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);
pitiot's avatar
pitiot committed
291

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

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

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

		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);
320
321
		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
322

323
324
//		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
325
326

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

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

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

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

David Cazier's avatar
David Cazier committed
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
	// 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
417
418
		else
		{
David Cazier's avatar
David Cazier committed
419
420
421
422
423
424
425
426
			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
427

pitiot's avatar
pitiot committed
428
		mo4= new MovingObstacle(this, i,vPos,goals, true, false,1);
David Cazier's avatar
David Cazier committed
429
430


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

		movingObstacles_.push_back(mo4);
	}
}

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

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

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

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

pitiot's avatar
merging    
pitiot committed
491
		addAgent(start, goal) ;
pitiot's avatar
merge    
pitiot committed
492

Thomas's avatar
Thomas committed
493
494
	}

David Cazier's avatar
David Cazier committed
495
496
497
498
	// 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 ;
Thomas Jund's avatar
Thomas Jund committed
499
	yStartDelta = envMap_.geometry.size(1) / 10 ;
David Cazier's avatar
David Cazier committed
500
501

	// Arrivée des obstacles à l'opposée
Thomas Jund's avatar
Thomas Jund committed
502
	yGoalDelta = envMap_.geometry.size(1) / 5 ;
pitiot's avatar
merge    
pitiot committed
503

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

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

Thomas Jund's avatar
Thomas Jund committed
509
	std::cout << "geom : " << envMap_.geometry << std::endl;
pitiot's avatar
merging    
pitiot committed
510
511
512

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

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

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

		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;
Thomas Jund's avatar
Thomas Jund committed
532
		while (r<80)
David Cazier's avatar
David Cazier committed
533
		{
pitiot's avatar
merging    
pitiot committed
534
535
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
Thomas Jund's avatar
Thomas Jund committed
536
537
538


			if ((r==0 || angle(goal-goals[r],goals[r]-goals[r-1])<M_PI/2.0f) )
539
			{
Thomas Jund's avatar
Thomas Jund committed
540
541
542

				std::cout << "new goal " << goal << std::endl;

pitiot's avatar
merging    
pitiot committed
543
				goals.push_back(goal);
544
				r++;
pitiot's avatar
merging    
pitiot committed
545
546
			}

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

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

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

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

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

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

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

Thomas Jund's avatar
Thomas Jund committed
600
601
	unsigned int nbx = 1 ;
	unsigned int nby = 1 ;
David Cazier's avatar
David Cazier committed
602
603
604

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

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

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

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

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

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

678
679
	MovingMesh* mm = NULL;

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

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

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

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

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

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

	movingObstacles_.push_back(mo);
753

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

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

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

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

779
780
781
782
783
		Dart dStop = dStart;

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

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

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

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

		mo->goals_.push_back(dest) ;
	}
810
811
812
813
814

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

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

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

//	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;
//		}
//	}
842
843
}

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

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

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

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

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

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

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

881
882
		TraversorF<PFP::MAP> tF(envMap_.map);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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