Création d'un compte pour un collaborateur extérieur au laboratoire depuis l'intranet ICube : https://intranet.icube.unistra.fr/fr/labs/member/profile

simulator.cpp 33 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;
71
72
73
74
75
76
77
78
79
80
		case 6:
#ifdef TWO_AND_HALF_DIM
			envMap_.init(config,200.0,200.0, minSize, 400.0f);
			setupPlanetScenario(nbAgent,nbObst);
			addMovingObstacles(nbObst);
			addPathToObstacles();
#else
			std::cout << "Agents not in 2.5D mode" << std::endl;
#endif
			break;
David Cazier's avatar
David Cazier committed
81
82
83
		default:
			std::cout << "Unknown scenario !" << std::endl ;
			exit(1) ;
Thomas's avatar
Thomas committed
84
85
	}

David Cazier's avatar
David Cazier committed
86
87
88
89
90
#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
91
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
92
93
94
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
David Cazier's avatar
David Cazier committed
95
			addPathsToAgents() ;
96
97
		else if (dimension == 2.5f)
			addPathsToAgents_height() ;
Thomas's avatar
Thomas committed
98
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
99
#endif
100

David Cazier's avatar
David Cazier committed
101
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
102
	{
David Cazier's avatar
David Cazier committed
103
104
		agents_[i]->updateObstacleNeighbors() ;
		agents_[i]->updateAgentNeighbors() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
105
	}
106

Pierre Kraemer's avatar
Pierre Kraemer committed
107
#ifndef SPATIAL_HASHING
108
109
	if (multires)
		envMap_.subdivideToProperLevel() ;
David Cazier's avatar
David Cazier committed
110
	//	envMap_.subdivideAllToMaxLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
111
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
112
113
}

David Cazier's avatar
David Cazier committed
114
void Simulator::doStep()
Pierre Kraemer's avatar
Pierre Kraemer committed
115
{
David Cazier's avatar
David Cazier committed
116
117
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
Jund Thomas's avatar
Jund Thomas committed
118
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
119
#endif
120
121
	unsigned long begTime, endTime;
	begTime=clock() ;
David Cazier's avatar
David Cazier committed
122
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
pitiot's avatar
pitiot committed
123
	{
David Cazier's avatar
David Cazier committed
124
		movingObstacles_[i]->computePrefVelocity() ;
pitiot's avatar
pitiot committed
125
		movingObstacles_[i]->computeNewVelocity() ;
126

Thomas Jund's avatar
Thomas Jund committed
127
		movingObstacles_[i]->updateAgentNeighbors();
128
		movingObstacles_[i]->updateObstacleNeighbors();
pitiot's avatar
pitiot committed
129
130
131
#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
132
133
		movingObstacles_[i]->initForces();
	}
134

135
136
137
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
		movingObstacles_[i]->updateForces() ;

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
138
139
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
	{
140
		movingObstacles_[i]->applyForces();
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
141
		movingObstacles_[i]->updateMesh() ;
pitiot's avatar
pitiot committed
142
	}
143
144
145
146




147
148
149
	endTime=clock() ;
	time_obstacle+= endTime-begTime;
	begTime=clock() ;
David Cazier's avatar
David Cazier committed
150
151
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
pitiot's avatar
pitiot committed
152
153
		if (agents_[i]->alive)
		{
154

pitiot's avatar
pitiot committed
155
			agents_[i]->updateObstacleNeighbors() ;
pitiot's avatar
maj    
pitiot committed
156
157

#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
158
			envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
159
#endif
pitiot's avatar
pitiot committed
160
			agents_[i]->updateAgentNeighbors() ;
pitiot's avatar
maj    
pitiot committed
161
#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
162
			envMap_.map.setCurrentLevel(0) ;
pitiot's avatar
maj    
pitiot committed
163
164
#endif

pitiot's avatar
pitiot committed
165
166
167
			agents_[i]->computePrefVelocity() ;
			agents_[i]->computeNewVelocity() ;
		}
David Cazier's avatar
David Cazier committed
168
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
169

Pierre Kraemer's avatar
Pierre Kraemer committed
170
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
171
172
173
174
175
176
177
178
179
180
181
	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
182
#else
Thomas Jund's avatar
Thomas Jund committed
183

184
185
	if (multires)
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
Thomas Jund's avatar
Thomas Jund committed
186

David Cazier's avatar
David Cazier committed
187
188
189
190
191
192
193
	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
194

195
			if (agents_[i]->part_.crossCell != CGoGN::Algo::Surface::MovingObjects::NO_CROSS)
David Cazier's avatar
David Cazier committed
196
197
198
199
200
201
				envMap_.agentChangeFace(agents_[i], oldFace) ;
		}
		else
			nb_dead++ ;

	}
202
203
	endTime=clock() ;
	time_agent+= endTime-begTime;
David Cazier's avatar
David Cazier committed
204
205
206

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

Thomas Jund's avatar
Thomas Jund committed
208
209
	 if (multires)
		 envMap_.updateMap() ;
David Cazier's avatar
David Cazier committed
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
#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
226
void Simulator::addAgent(const VEC3& start, const VEC3& goal)
David Cazier's avatar
David Cazier committed
227
{
David Cazier's avatar
David Cazier committed
228
	agents_.push_back(new Agent(this, start, goal)) ;
David Cazier's avatar
David Cazier committed
229
230
}

231
232
233
234
235
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
236
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
David Cazier's avatar
David Cazier committed
237
{
pitiot's avatar
merging    
pitiot committed
238
239
240
	if (multires)
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
	else
pitiot's avatar
blah    
pitiot committed
241
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //cases fines
pitiot's avatar
merging    
pitiot committed
242

243
	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles"<< std::endl ;
David Cazier's avatar
David Cazier committed
244
245
246
247
248
249
250
251
252
253
254

	// 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
255
//	int radius = nbAgents/(Agent::radius_*Agent::radius_);
Thomas Jund's avatar
Thomas Jund committed
256
	std::cout << "radius " << radius << std::endl;
David Cazier's avatar
David Cazier committed
257
258
259
260
	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
261
		double angle = i * 2.0f * M_PI / float(nbAgents) ;
David Cazier's avatar
David Cazier committed
262
263
264
		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
265
		addAgent(start, goal) ;
pitiot's avatar
pitiot committed
266
//		CGoGNout<<"agent n°" << agents_[i]->agentNo << " : "<< agents_[i]<<CGoGNendl;
267
	}
pitiot's avatar
pitiot committed
268
269
270
271
272
	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
273
	int curGoal=0;
Thomas Jund's avatar
merge    
Thomas Jund committed
274

pitiot's avatar
pitiot committed
275
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
David Cazier's avatar
David Cazier committed
276
	{
Thomas Jund's avatar
merge    
Thomas Jund committed
277
278
		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
279
280
281
282
283
284
285
		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
286
			v=VEC3 (std::cos(angle) * (radius/2), std::sin(angle) * (radius/2), 0) ;
pitiot's avatar
pitiot committed
287
288
289
		}
		start = center + v ;
		positions.push_back(start);
pitiot's avatar
pitiot committed
290
	}
Thomas Jund's avatar
merge    
Thomas Jund committed
291

pitiot's avatar
pitiot committed
292
293
294
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
295
296

		unsigned int nbGoals=360;
pitiot's avatar
pitiot committed
297
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
298
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
299
		{
Thomas Jund's avatar
Thomas Jund committed
300
			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
301
302
303
		}

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

pitiot's avatar
pitiot committed
305
306
307
308
		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
309
		curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+1)%nbGoals;
310
		mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);
pitiot's avatar
pitiot committed
311

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

pitiot's avatar
pitiot committed
315
316
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
317

pitiot's avatar
pitiot committed
318
319
320
	for (unsigned int i = nbObstacles/2 ; i < nbObstacles ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
321
322
323
324
325
326
327
//		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
328
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
329
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
330
		{
Thomas Jund's avatar
Thomas Jund committed
331
			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
332
333
334
335
336
337
338
339
		}

		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);
340
341
		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
342

343
344
//		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
345
346

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
347
	}
Thomas's avatar
Thomas committed
348

349
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
350
351
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
352
#endif
pitiot's avatar
merging    
pitiot committed
353
354

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

David Cazier's avatar
David Cazier committed
357
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
358
{
pitiot's avatar
merging    
pitiot committed
359
360
361
362
363
364
365
366
367
368
369
	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
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
399
400
401
402
403
404
405
406
	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
407

David Cazier's avatar
David Cazier committed
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
	// 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
437
438
		else
		{
David Cazier's avatar
David Cazier committed
439
440
441
442
443
444
445
446
			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
447

pitiot's avatar
pitiot committed
448
		mo4= new MovingObstacle(this, i,vPos,goals, true, false,1);
David Cazier's avatar
David Cazier committed
449
450


451
452
453
454
455
456
457
458
459
460
461
462
463
464
//		//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
465
466
467
468
469

		movingObstacles_.push_back(mo4);
	}
}

pitiot's avatar
merging    
pitiot committed
470
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
David Cazier's avatar
David Cazier committed
471
{
pitiot's avatar
merging    
pitiot committed
472
473
474
475
476
477
	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
478
479

	// Bordure à éviter autour de la scène (10% de sa taille)
pitiot's avatar
merging    
pitiot committed
480
481
	int xBorder = envMap_.geometry.size(0) / 5 ;
	int yBorder = envMap_.geometry.size(1) / 5 ;
David Cazier's avatar
David Cazier committed
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497

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

David Cazier's avatar
David Cazier committed
502
		// Un agent sur 2 va de droite à gauche
pitiot's avatar
merging    
pitiot committed
503
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
504
		if (i % 2 == 1)
pitiot's avatar
merging    
pitiot committed
505
506
507
508
509
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
		}
pitiot's avatar
merge    
pitiot committed
510

pitiot's avatar
merging    
pitiot committed
511
		addAgent(start, goal) ;
pitiot's avatar
merge    
pitiot committed
512

Thomas's avatar
Thomas committed
513
514
	}

David Cazier's avatar
David Cazier committed
515
516
517
518
	// 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
519
	yStartDelta = envMap_.geometry.size(1) / 10 ;
David Cazier's avatar
David Cazier committed
520
521

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

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

526
	VEC3 xSide (5.0f,0.0f,0.0f);
527
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
528

Thomas Jund's avatar
Thomas Jund committed
529
	std::cout << "geom : " << envMap_.geometry << std::endl;
pitiot's avatar
merging    
pitiot committed
530
531
532

	int sumObstacles=0;
	for(unsigned int j = 0; j<nbSnakes; j++)
David Cazier's avatar
David Cazier committed
533
	{
pitiot's avatar
merging    
pitiot committed
534
535
536
537
		std::vector<PFP::VEC3> positions [snakeSize] ;
		float x = xStartMin + rand() % xStartDelta;
		VEC3 goal;

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

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

		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
552
		while (r<80)
David Cazier's avatar
David Cazier committed
553
		{
pitiot's avatar
merging    
pitiot committed
554
555
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
Thomas Jund's avatar
Thomas Jund committed
556
557
558


			if ((r==0 || angle(goal-goals[r],goals[r]-goals[r-1])<M_PI/2.0f) )
559
			{
Thomas Jund's avatar
Thomas Jund committed
560
561
562

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

pitiot's avatar
merging    
pitiot committed
563
				goals.push_back(goal);
564
				r++;
pitiot's avatar
merging    
pitiot committed
565
566
			}

David Cazier's avatar
David Cazier committed
567
		}
pitiot's avatar
merging    
pitiot committed
568
569
570
571
572
573
574
575
576
577

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

David Cazier's avatar
David Cazier committed
578
579
580
581
			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
582
			positions[i]=vPos;
David Cazier's avatar
David Cazier committed
583
		}
pitiot's avatar
merging    
pitiot committed
584
585
586
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
587

pitiot's avatar
merging    
pitiot committed
588
589
		for (int i = 0 ; i < snakeSize ; i++)
			movingObstacles_.push_back(art->members[i]);
David Cazier's avatar
David Cazier committed
590
	}
Thomas's avatar
Thomas committed
591
592
}

David Cazier's avatar
David Cazier committed
593
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
594
{
David Cazier's avatar
David Cazier committed
595
596
597
598
599
	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
600

David Cazier's avatar
David Cazier committed
601
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
602
	{
David Cazier's avatar
David Cazier committed
603
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
604
		{
605
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
David Cazier's avatar
David Cazier committed
606
			addAgent(posagent, -1.0f * posagent) ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
607
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
608
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
609
610
}

Thomas Jund's avatar
Thomas Jund committed
611
void Simulator::setupScenario(unsigned int nbMaxAgent, bool pedWay)
Pierre Kraemer's avatar
Pierre Kraemer committed
612
613
{
	/*
David Cazier's avatar
David Cazier committed
614
	 * Add agents, specifying their start position, and store their goal on the
David Cazier's avatar
David Cazier committed
615
616
	 * opposite side of the environment.
	 */
617
618
	TraversorF<PFP::MAP> tF(envMap_.map);
	Dart d = tF.begin() ;
David Cazier's avatar
David Cazier committed
619

Thomas Jund's avatar
Thomas Jund committed
620
621
	unsigned int nbx = 1 ;
	unsigned int nby = 1 ;
David Cazier's avatar
David Cazier committed
622
623
624

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

David Cazier's avatar
David Cazier committed
625
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
626
	{
Thomas Jund's avatar
Thomas Jund committed
627
		VEC3 pos;
David Cazier's avatar
David Cazier committed
628
629
		bool found = false ;
		Dart dCell ;
630
		while (!found && d != tF.end())
David Cazier's avatar
David Cazier committed
631
		{
632
			if (!envMap_.buildingMark.isMarked(d)
Thomas Jund's avatar
Thomas Jund committed
633
			    && (!pedWay || envMap_.pedWayMark.isMarked(d))
634
			    )
David Cazier's avatar
David Cazier committed
635
			{
636
				pos = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
637
638
639
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
640
			}
641
			d = tF.next() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
642
643
		}

David Cazier's avatar
David Cazier committed
644
645
		if (found)
		{
Thomas Jund's avatar
Thomas Jund committed
646
			float ecart = 2.0f*Agent::radius_;
647
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
648
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
David Cazier's avatar
David Cazier committed
649
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
650
			{
David Cazier's avatar
David Cazier committed
651
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
652
				{
653
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
654
					addAgent(posagent, -1.0f * posagent, dCell) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
655
656
657
658
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
659
660
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
661
}
662

663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
void Simulator::setupPlanetScenario(unsigned int nbAgents, unsigned int nbObstacles)
{
	/*
	* Add agents, specifying their start position, and store their goals on the
	* opposite side of the environment.
	*/
	Dart d = envMap_.map.begin();
	CellMarker<FACE> filled(envMap_.map);

	unsigned int nbx = 1;
	unsigned int nby = 1;

	unsigned int bMax = nbAgents / (nbx*nby);

	for (unsigned int i = 0; i < bMax && d != envMap_.map.end(); ++i)
	{
		bool found = false;
		VEC3 pos;
		Dart dCell;
		while(!found && d != envMap_.map.end())
		{
			if(!filled.isMarked(d) && !envMap_.buildingMark.isMarked(d))
			{
				filled.mark(d);
				pos = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position);
				dCell = d;
				found = true;
			}
			envMap_.map.next(d);
		}

		if(found)
		{
			float ecart = 3.0f;
			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, dCell, envMap_.position), 0.0f);
			VEC3 posinit = VEC3(pos[0] - (float(nbx)/2.0f * ecart), pos[1] - (float(nby)/2.0f * ecart), pos[2]);
			for(unsigned int curx = 0; curx < nbx; ++curx)
			{
				for(unsigned int cury = 0; cury < nby; ++cury)
				{
					VEC3 displ(ecart * curx, ecart * cury, 0.0f);
					Agent::rotate(Agent::xyPlane, pl.normal(), displ);
					VEC3 posagent = posinit + displ;
//					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f);
					addAgent(posagent,-1.0f * posagent, dCell);
//					agents_.back()->goals_.push_back(posagent);
//					agents_.back()->goals_.push_back(VEC3(0,0,0));
//					agents_.back()->goals_.push_back(-1.0f * posagent);
					agents_.back()->curGoal_ = 1;
				}
			}
		}
	}
	std::cout << "nb agents : " << agents_.size() << std::endl;

	swapAgentsGoals();
}

721
722
723
724
725
726
727
728
729
730
731
732
733
734
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)
				)
			{
735
				float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
736
737
738
739
740
				if(area>1400)
				{
					dCell = d ;
					found = true ;
				}
741
742
743
744
745
			}
			d = tF.next() ;
		}

		if (found)
Arash HABIBI's avatar
Arash HABIBI committed
746
			addMovingObstacle(dCell,1) ;
747
748
749
750
751
752
753
	}
}

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

756
757
	MovingMesh* mm = NULL;

758
759
760
761
	switch(obstType)
	{
		case 0 :
		{
762
			start = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
763
764
765
			VEC3 xSide (2.0f,0.0f,0.0f);
			VEC3 ySide (0.0f,5.0f,0.0f);

766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
#ifdef TWO_AND_HALF_DIM
			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, d, envMap_.position), 0.0f);

			VEC3 displ = -xSide-ySide;
			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
			vPos.push_back(start + displ);

			displ = xSide-ySide;
			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
			vPos.push_back(start + displ);

			displ = xSide+ySide;
			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
			vPos.push_back(start + displ);

			displ = ySide-xSide;
			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
			vPos.push_back(start + displ);
#else
785
786
787
			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
788
			vPos.push_back(start-xSide+ySide);
789
790
#endif

791
792
		}
			break;
793
794
		case 1 :
		{
795
796
797
			mm = new MovingMesh(envMap_, d, "./meshRessources/LimaceLvl1.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/cylinder.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/simpleCyl.obj");
798
			movingMeshes_.push_back(mm);
799
			vPos = mm->computeProjectedPointSet(maxHeight);
Thomas Jund's avatar
Thomas Jund committed
800
801
802
803
804
805
806
807
808
809

			//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);
810
811
		}
			break;
812
813
814
815
816
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

817
	bool orientAccordingToFace=false;
Thomas Jund's avatar
Thomas Jund committed
818
	if(mm!=NULL && orientAccordingToFace)
Thomas Jund's avatar
Thomas Jund committed
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
	{
		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
836
		TraversorV<PFP2::MAP> tv(mm->map);
Thomas Jund's avatar
Thomas Jund committed
837
838
		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
		{
Thomas Jund's avatar
Thomas Jund committed
839
			mm->position[d] += rotate(mm->position[d], bary, angle);
Thomas Jund's avatar
Thomas Jund committed
840
		}
Thomas Jund's avatar
Thomas Jund committed
841
842
843

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

846
847
	std::vector<VEC3> goals;
	goals.push_back(start);
Thomas Jund's avatar
merge    
Thomas Jund committed
848

pitiot's avatar
pitiot committed
849
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), (obstType==0));
850
851

	movingObstacles_.push_back(mo);
852

853
	if(mm != NULL)
854
	{
855
		mo->attachMesh(mm);
Thomas Jund's avatar
Thomas Jund committed
856
857
858
//		std::cout << "here you go" << std::endl;
//		addAgent(vPos[0], vPos[0], d);
//		mo->attachAgent(agents_.back());
859
	}
860
861
862
863
864
865
866
867
}

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

	for (std::vector<MovingObstacle*>::iterator it = movingObstacles_.begin() ; it != movingObstacles_.end() ; ++it)
	{
pitiot's avatar
pitiot committed
868
		Dart dStart = (*it)->parts_[0]->d;
869
870
871
872
873
874
875
876
877

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

878
879
880
881
882
		Dart dStop = dStart;

		for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop)
			|| j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStart)
883
			|| envMap_.pedWayMark.isMarked(dStop)
884
885
886
887
888
889
890
891
892
893
894
			; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
		}

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

895
896
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
David Cazier's avatar
David Cazier committed
897
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
898
899
																   envMap_.position, dStart,
																   dGoal,
900
																   envMap_.pedWayMark) ;
901
902
903

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
904
905
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;
906
907
908

		mo->goals_.push_back(dest) ;
	}
909
910
911
912
913

	path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
												envMap_.position,
												dGoal,
												dStart,
914
												envMap_.pedWayMark) ;
915
916
917

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
918
919
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;
920
921
922

		mo->goals_.push_back(dest) ;
	}
pitiot's avatar
pitiot committed
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940

//	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;
//		}
//	}
941
942
}

Pierre Kraemer's avatar
Pierre Kraemer committed
943
#ifndef SPATIAL_HASHING
944
945
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
946
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
947
	{
David Cazier's avatar
David Cazier committed
948
949
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
950

David Cazier's avatar
David Cazier committed
951
952
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
953

David Cazier's avatar
David Cazier committed
954
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
955
956
957
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
958

David Cazier's avatar
David Cazier committed
959
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
960
		{
961
			VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
962
963
964
965
966

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

David Cazier's avatar
David Cazier committed
967
			agents_[i]->goals_.push_back(dest) ;
968
969
970
971
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
972
973
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
974
	//city
David Cazier's avatar
David Cazier committed
975
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
976
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
977
	{
David Cazier's avatar
David Cazier committed
978
979
		agents_[i]->goals_.clear() ;

980
981
		TraversorF<PFP::MAP> tF(envMap_.map);

David Cazier's avatar
David Cazier committed
982
983
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
984
985
986
987
988
989
990
		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
991
		{
David Cazier's avatar
David Cazier committed
992
			envMap_.map.next(dStop) ;
993
994
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
995
		}
996

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

David Cazier's avatar
David Cazier committed
999
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
1000
1001
1002
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
1003

David Cazier's avatar
David Cazier committed
1004
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
1005
		{
Thomas Jund's avatar
Thomas Jund committed
1006
1007
1008
1009
1010
1011
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

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

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

David Cazier's avatar
David Cazier committed
1019
			agents_[i]->goals_.push_back(dest) ;
1020
		}
1021

David Cazier's avatar
David Cazier committed
1022
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
1023
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
1024
1025
1026
1027
1028
1029
		    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
1030
		{
David Cazier's avatar
David Cazier committed
1031
			envMap_.map.next(dStop2) ;
1032
1033
			if (dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1034
		}
1035

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

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

David Cazier's avatar
David Cazier committed
1041
		for (std::vector<Dart>::iterator