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 32.9 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

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
135
136
137
138
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
	{
		movingObstacles_[i]->update() ;
		movingObstacles_[i]->updateMesh() ;
pitiot's avatar
pitiot committed
139
	}
140
141
142
	endTime=clock() ;
	time_obstacle+= endTime-begTime;
	begTime=clock() ;
David Cazier's avatar
David Cazier committed
143
144
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
pitiot's avatar
pitiot committed
145
146
		if (agents_[i]->alive)
		{
147

pitiot's avatar
pitiot committed
148
			agents_[i]->updateObstacleNeighbors() ;
pitiot's avatar
maj    
pitiot committed
149
150

#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
151
			envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
152
#endif
pitiot's avatar
pitiot committed
153
			agents_[i]->updateAgentNeighbors() ;
pitiot's avatar
maj    
pitiot committed
154
#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
155
			envMap_.map.setCurrentLevel(0) ;
pitiot's avatar
maj    
pitiot committed
156
157
#endif

pitiot's avatar
pitiot committed
158
159
160
			agents_[i]->computePrefVelocity() ;
			agents_[i]->computeNewVelocity() ;
		}
David Cazier's avatar
David Cazier committed
161
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
162

Pierre Kraemer's avatar
Pierre Kraemer committed
163
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
164
165
166
167
168
169
170
171
172
173
174
	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
175
#else
Thomas Jund's avatar
Thomas Jund committed
176

177
178
	if (multires)
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
Thomas Jund's avatar
Thomas Jund committed
179

David Cazier's avatar
David Cazier committed
180
181
182
183
184
185
186
	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
187

188
			if (agents_[i]->part_.crossCell != CGoGN::Algo::Surface::MovingObjects::NO_CROSS)
David Cazier's avatar
David Cazier committed
189
190
191
192
193
194
				envMap_.agentChangeFace(agents_[i], oldFace) ;
		}
		else
			nb_dead++ ;

	}
195
196
	endTime=clock() ;
	time_agent+= endTime-begTime;
David Cazier's avatar
David Cazier committed
197
198
199

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

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

224
225
226
227
228
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
229
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
David Cazier's avatar
David Cazier committed
230
{
pitiot's avatar
merging    
pitiot committed
231
232
233
	if (multires)
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
	else
pitiot's avatar
blah    
pitiot committed
234
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //cases fines
pitiot's avatar
merging    
pitiot committed
235

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

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

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

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

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

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

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

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

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

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

		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);
333
334
		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
335

336
337
//		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
338
339

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

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

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

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

David Cazier's avatar
David Cazier committed
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
	// 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
430
431
		else
		{
David Cazier's avatar
David Cazier committed
432
433
434
435
436
437
438
439
			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
440

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


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

		movingObstacles_.push_back(mo4);
	}
}

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

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

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

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

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

Thomas's avatar
Thomas committed
506
507
	}

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

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

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

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

Thomas Jund's avatar
Thomas Jund committed
522
	std::cout << "geom : " << envMap_.geometry << std::endl;
pitiot's avatar
merging    
pitiot committed
523
524
525

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

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

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

		std::vector<VEC3> vPos;


		vPos.push_back(start+xSide-ySide);
		vPos.push_back(start+xSide+ySide);
		vPos.push_back(start-xSide+ySide);
		vPos.push_back(start-xSide-ySide);

		std::vector<VEC3> goals;
		goals.push_back(start);
		int r=0;
Thomas Jund's avatar
Thomas Jund committed
545
		while (r<80)
David Cazier's avatar
David Cazier committed
546
		{
pitiot's avatar
merging    
pitiot committed
547
548
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
Thomas Jund's avatar
Thomas Jund committed
549
550
551


			if ((r==0 || angle(goal-goals[r],goals[r]-goals[r-1])<M_PI/2.0f) )
552
			{
Thomas Jund's avatar
Thomas Jund committed
553
554
555

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

pitiot's avatar
merging    
pitiot committed
556
				goals.push_back(goal);
557
				r++;
pitiot's avatar
merging    
pitiot committed
558
559
			}

David Cazier's avatar
David Cazier committed
560
		}
pitiot's avatar
merging    
pitiot committed
561
562
563
564
565
566
567
568
569
570

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

David Cazier's avatar
David Cazier committed
571
572
573
574
			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
575
			positions[i]=vPos;
David Cazier's avatar
David Cazier committed
576
		}
pitiot's avatar
merging    
pitiot committed
577
578
579
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
580

pitiot's avatar
merging    
pitiot committed
581
582
		for (int i = 0 ; i < snakeSize ; i++)
			movingObstacles_.push_back(art->members[i]);
David Cazier's avatar
David Cazier committed
583
	}
Thomas's avatar
Thomas committed
584
585
}

David Cazier's avatar
David Cazier committed
586
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
587
{
David Cazier's avatar
David Cazier committed
588
589
590
591
592
	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
593

David Cazier's avatar
David Cazier committed
594
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
595
	{
David Cazier's avatar
David Cazier committed
596
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
597
		{
598
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
David Cazier's avatar
David Cazier committed
599
			addAgent(posagent, -1.0f * posagent) ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
600
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
601
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
602
603
}

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

Thomas Jund's avatar
Thomas Jund committed
613
614
	unsigned int nbx = 1 ;
	unsigned int nby = 1 ;
David Cazier's avatar
David Cazier committed
615
616
617

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

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

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

656
657
658
659
660
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
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();
}

714
715
716
717
718
719
720
721
722
723
724
725
726
727
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)
				)
			{
728
				float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
729
730
731
732
733
				if(area>1400)
				{
					dCell = d ;
					found = true ;
				}
734
735
736
737
738
			}
			d = tF.next() ;
		}

		if (found)
Arash HABIBI's avatar
Arash HABIBI committed
739
			addMovingObstacle(dCell,1) ;
740
741
742
743
744
745
746
	}
}

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

749
750
	MovingMesh* mm = NULL;

751
752
753
754
	switch(obstType)
	{
		case 0 :
		{
755
			start = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
756
757
758
			VEC3 xSide (2.0f,0.0f,0.0f);
			VEC3 ySide (0.0f,5.0f,0.0f);

759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
#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
778
779
780
			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
781
			vPos.push_back(start-xSide+ySide);
782
783
#endif

784
785
		}
			break;
786
787
		case 1 :
		{
788
789
790
			mm = new MovingMesh(envMap_, d, "./meshRessources/LimaceLvl1.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/cylinder.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/simpleCyl.obj");
791
			movingMeshes_.push_back(mm);
792
			vPos = mm->computeProjectedPointSet(maxHeight);
Thomas Jund's avatar
Thomas Jund committed
793
794
795
796
797
798
799
800
801
802

			//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);
803
804
		}
			break;
805
806
807
808
809
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

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

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

839
840
	std::vector<VEC3> goals;
	goals.push_back(start);
Thomas Jund's avatar
merge    
Thomas Jund committed
841

pitiot's avatar
pitiot committed
842
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), (obstType==0));
843
844

	movingObstacles_.push_back(mo);
845

846
	if(mm != NULL)
847
	{
848
		mo->attachMesh(mm);
Thomas Jund's avatar
Thomas Jund committed
849
850
851
//		std::cout << "here you go" << std::endl;
//		addAgent(vPos[0], vPos[0], d);
//		mo->attachAgent(agents_.back());
852
	}
853
854
855
856
857
858
859
860
}

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

	for (std::vector<MovingObstacle*>::iterator it = movingObstacles_.begin() ; it != movingObstacles_.end() ; ++it)
	{
pitiot's avatar
pitiot committed
861
		Dart dStart = (*it)->parts_[0]->d;
862
863
864
865
866
867
868
869
870

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

871
872
873
874
875
		Dart dStop = dStart;

		for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop)
			|| j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStart)
876
			|| envMap_.pedWayMark.isMarked(dStop)
877
878
879
880
881
882
883
884
885
886
887
			; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
		}

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

888
889
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
David Cazier's avatar
David Cazier committed
890
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
891
892
																   envMap_.position, dStart,
																   dGoal,
893
																   envMap_.pedWayMark) ;
894
895
896

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
897
898
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;
899
900
901

		mo->goals_.push_back(dest) ;
	}
902
903
904
905
906

	path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
												envMap_.position,
												dGoal,
												dStart,
907
												envMap_.pedWayMark) ;
908
909
910

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
911
912
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;
913
914
915

		mo->goals_.push_back(dest) ;
	}
pitiot's avatar
pitiot committed
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933

//	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;
//		}
//	}
934
935
}

Pierre Kraemer's avatar
Pierre Kraemer committed
936
#ifndef SPATIAL_HASHING
937
938
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
939
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
940
	{
David Cazier's avatar
David Cazier committed
941
942
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
943

David Cazier's avatar
David Cazier committed
944
945
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
946

David Cazier's avatar
David Cazier committed
947
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
948
949
950
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
951

David Cazier's avatar
David Cazier committed
952
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
953
		{
954
			VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
955
956
957
958
959

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

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

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

973
974
		TraversorF<PFP::MAP> tF(envMap_.map);

David Cazier's avatar
David Cazier committed
975
976
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
977
978
979
980
981
982
983
		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
984
		{
David Cazier's avatar
David Cazier committed
985
			envMap_.map.next(dStop) ;
986
987
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
988
		}
989

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

David Cazier's avatar
David Cazier committed
992
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
993
994
995
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
996

David Cazier's avatar
David Cazier committed
997
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
998
		{
Thomas Jund's avatar
Thomas Jund committed
999
1000
1001
1002
1003
1004
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

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

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

David Cazier's avatar
David Cazier committed
1012
			agents_[i]->goals_.push_back(dest) ;
1013
		}
1014

David Cazier's avatar
David Cazier committed
1015
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
1016
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
1017
1018
1019
1020
1021
1022
		    envMap_.buildingMark.isMarked(dStop2)
		    || j < dartDistForPath + rand() * 20