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

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

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

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

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

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

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

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

David Cazier's avatar
David Cazier committed
101
void Simulator::doStep()
Pierre Kraemer's avatar
Pierre Kraemer committed
102
{
David Cazier's avatar
David Cazier committed
103
104
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
Jund Thomas's avatar
Jund Thomas committed
105
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
106
#endif
David Cazier's avatar
David Cazier committed
107

David Cazier's avatar
David Cazier committed
108
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
pitiot's avatar
pitiot committed
109
	{
David Cazier's avatar
David Cazier committed
110
		movingObstacles_[i]->computePrefVelocity() ;
pitiot's avatar
pitiot committed
111
		movingObstacles_[i]->computeNewVelocity() ;
pitiot's avatar
pitiot committed
112
		movingObstacles_[i]->updateAgentNeighbors();
pitiot's avatar
pitiot committed
113
114
115
#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
David Cazier's avatar
David Cazier committed
116
		movingObstacles_[i]->update() ;
pitiot's avatar
pitiot committed
117
118

		movingObstacles_[i]->updateMesh() ;
pitiot's avatar
pitiot committed
119
120
	}

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

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

pitiot's avatar
pitiot committed
135
136
137
			agents_[i]->computePrefVelocity() ;
			agents_[i]->computeNewVelocity() ;
		}
David Cazier's avatar
David Cazier committed
138
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
139

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

David Cazier's avatar
David Cazier committed
163
//		if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE))
pitiot's avatar
pitiot committed
164
			if (agents_[i]->part_.crossCell != CGoGN::Algo::Surface::MovingObjects::NO_CROSS)
David Cazier's avatar
David Cazier committed
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
//		switch(agents_[i]->part_.crossCell)
			{
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace) ;
//				break;
			}
		}
		else
			nb_dead++ ;

	}

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

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

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

pitiot's avatar
pitiot committed
220
		envMap_.init(config, 1600.0f, 960.0f, minSize, 400.0f) ; //cases fines
pitiot's avatar
merging    
pitiot committed
221
222
	}

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

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

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

	// Départ des agents sur un cercle centré
	int xSize = envMap_.geometry.size(0) - 2 * xBorder ;
	int ySize = envMap_.geometry.size(1) - 2 * yBorder ;
	int radius = min(xSize, ySize) / 2 ;
pitiot's avatar
pitiot committed
235
236
//	int radius = nbAgents/(Agent::radius_*Agent::radius_);
	std::cout << "radius " << radius << std::endl;
David Cazier's avatar
David Cazier committed
237
238
	VEC3 center = (envMap_.geometry.min() + envMap_.geometry.max()) / 2 ;

David Cazier's avatar
David Cazier committed
239
	double pi = 3.14159265358979323846f ;
David Cazier's avatar
David Cazier committed
240
241
242
243
244
245
246

	for (unsigned int i = 0 ; i < nbAgents ; ++i)
	{
		double angle = i * 2.0f * pi / float(nbAgents) ;
		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
247
		addAgent(start, goal) ;
pitiot's avatar
pitiot committed
248
//		CGoGNout<<"agent n°" << agents_[i]->agentNo << " : "<< agents_[i]<<CGoGNendl;
249
	}
pitiot's avatar
pitiot committed
250
251
252
253
254
	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
255
	int curGoal=0;
pitiot's avatar
pitiot committed
256
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
David Cazier's avatar
David Cazier committed
257
	{
pitiot's avatar
pitiot committed
258
		double angle = nbObstacles<2 ? 0 : i * 2.0f * pi / float((float)nbObstacles/2.0f) ;
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
271
272
273
	}
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
pitiot's avatar
pitiot committed
274
275
276
277
278
279
280
//		std::vector<VEC3> goals;
//		for(unsigned int k = 0 ; k < nbObstacles/2 ; k++)
//		{
//			goals.push_back(positions[(i+k)%(nbObstacles/2)]);
//		}

		unsigned int nbGoals=360;
pitiot's avatar
pitiot committed
281
		std::vector<VEC3> goals;
pitiot's avatar
pitiot committed
282
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
283
		{
pitiot's avatar
pitiot committed
284
			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
285
286
287
288
289
290
291
292
		}

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


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

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

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

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);
		}
pitiot's avatar
pitiot 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);
pitiot's avatar
pitiot committed
427
		mo4= new MovingObstacle(this, i,vPos,goals, true, false,1);
David Cazier's avatar
David Cazier committed
428
429


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

		movingObstacles_.push_back(mo4);
	}
}

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

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

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

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

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

Thomas's avatar
Thomas committed
492
493
	}

David Cazier's avatar
David Cazier committed
494
495
496
497
	// Départ des obstacles du quart haut sur toute une demi-largeur
	xStartMin = envMap_.geometry.min()[0] + envMap_.geometry.size(0) / 4 ;
	xStartDelta = envMap_.geometry.size(0) / 2 ;
	yStartMin = envMap_.geometry.min()[1] + yBorder ;
pitiot's avatar
merging    
pitiot committed
498
	yStartDelta = envMap_.geometry.size(1) / 20 ;
David Cazier's avatar
David Cazier committed
499
500

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

pitiot's avatar
merging    
pitiot committed
502
	yGoalDelta = 3* 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

pitiot's avatar
merging    
pitiot committed
509
510
511

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

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

pitiot's avatar
merging    
pitiot committed
519
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;
		while (r<40)
David Cazier's avatar
David Cazier committed
532
		{
pitiot's avatar
merging    
pitiot committed
533
534
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
pitiot's avatar
pitiot committed
535
536
			if ((goal-goals[r]).norm2()>1000)
			{
pitiot's avatar
merging    
pitiot committed
537
				goals.push_back(goal);
pitiot's avatar
pitiot committed
538
				r++;
pitiot's avatar
merging    
pitiot committed
539
540
			}

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

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

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

pitiot's avatar
merging    
pitiot committed
562
563
		for (int i = 0 ; i < snakeSize ; i++)
			movingObstacles_.push_back(art->members[i]);
David Cazier's avatar
David Cazier committed
564
	}
Thomas's avatar
Thomas committed
565
566
}

David Cazier's avatar
David Cazier committed
567
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
568
{
David Cazier's avatar
David Cazier committed
569
570
571
572
573
	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
574

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

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

594
595
	unsigned int nbx = 2 ;
	unsigned int nby = 2 ;
David Cazier's avatar
David Cazier committed
596
597
598

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

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

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

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

		if (found)
pitiot's avatar
pitiot committed
662
			addMovingObstacle(dCell,1) ;
663
664
665
666
667
668
669
	}
}

void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
{
	std::vector<VEC3> vPos;
	VEC3 start;
pitiot's avatar
pitiot committed
670
	float maxHeight=5.0f;
pitiot's avatar
pitiot committed
671
672

	MovingMesh* mm = NULL;
673
674
675
676
677

	switch(obstType)
	{
		case 0 :
		{
pitiot's avatar
pitiot committed
678
			start = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
679
680
681
682
683
684
685
686
687
			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);
			vPos.push_back(start+xSide+ySide);
		}
			break;
pitiot's avatar
pitiot committed
688
689
690
691
692
693
694
		case 1 :
		{
			mm = new MovingMesh(envMap_, d, "meshRessources/Limace.ply");
			movingMeshes_.push_back(mm);
			vPos = mm->computeProjectedPointSet(maxHeight);
		}
			break;
695
696
697
698
699
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

pitiot's avatar
pitiot committed
700
701
	bool orientAccordingToFace=true;
	if(mm!=NULL && orientAccordingToFace)
pitiot's avatar
pitiot committed
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
	{
		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);

		TraversorV<PFP::MAP> tv(mm->map);
		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
		{
pitiot's avatar
pitiot committed
722
			mm->position[d] += rotate(mm->position[d], bary, angle);
pitiot's avatar
pitiot committed
723
		}
pitiot's avatar
pitiot committed
724
725
726

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

729
730
	std::vector<VEC3> goals;
	goals.push_back(start);
pitiot's avatar
pitiot committed
731
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), (obstType==0));
732
733

	movingObstacles_.push_back(mo);
pitiot's avatar
pitiot committed
734
735
736
737

	if(mm != NULL)
	{
		mo->attachMesh(mm);
pitiot's avatar
pitiot committed
738
739
740
//		std::cout << "here you go" << std::endl;
//		addAgent(vPos[0], vPos[0], d);
//		mo->attachAgent(agents_.back());
pitiot's avatar
pitiot committed
741
	}
742
743
744
745
746
747
748
749
}

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

	for (std::vector<MovingObstacle*>::iterator it = movingObstacles_.begin() ; it != movingObstacles_.end() ; ++it)
	{
pitiot's avatar
pitiot committed
750
		Dart dStart = (*it)->parts_[0]->d;
pitiot's avatar
pitiot committed
751
752
753
754
755
756
757
758
759

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

760
761
762
763
764
		Dart dStop = dStart;

		for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop)
			|| j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStart)
pitiot's avatar
pitiot committed
765
			|| envMap_.pedWayMark.isMarked(dStop)
766
767
768
769
770
771
772
773
774
775
776
			; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
		}

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

777
778
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
David Cazier's avatar
David Cazier committed
779
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
780
781
																   envMap_.position, dStart,
																   dGoal,
pitiot's avatar
pitiot committed
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
																   envMap_.pedWayMark) ;

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

		mo->goals_.push_back(dest) ;
	}

	path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
												envMap_.position,
												dGoal,
												dStart,
												envMap_.pedWayMark) ;
797
798
799

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
pitiot's avatar
pitiot committed
800
801
		VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
		dest /= 2.0f ;
802
803
804

		mo->goals_.push_back(dest) ;
	}
pitiot's avatar
pitiot committed
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822

//	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;
//		}
//	}
823
824
}

Pierre Kraemer's avatar
Pierre Kraemer committed
825
#ifndef SPATIAL_HASHING
826
827
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
828
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
829
	{
David Cazier's avatar
David Cazier committed
830
831
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
832

David Cazier's avatar
David Cazier committed
833
834
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
835

David Cazier's avatar
David Cazier committed
836
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
837
838
839
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
840

David Cazier's avatar
David Cazier committed
841
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
842
		{
pitiot's avatar
pitiot committed
843
			VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
844
845
846
847
848

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

David Cazier's avatar
David Cazier committed
849
			agents_[i]->goals_.push_back(dest) ;
850
851
852
853
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
854
855
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
856
	//city
David Cazier's avatar
David Cazier committed
857
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
858
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
859
	{
David Cazier's avatar
David Cazier committed
860
861
		agents_[i]->goals_.clear() ;

862
863
		TraversorF<PFP::MAP> tF(envMap_.map);

David Cazier's avatar
David Cazier committed
864
865
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
866
867
868
869
870
871
872
		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
873
		{
David Cazier's avatar
David Cazier committed
874
			envMap_.map.next(dStop) ;
875
876
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
877
		}
878

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

David Cazier's avatar
David Cazier committed
881
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
882
883
884
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
885

David Cazier's avatar
David Cazier committed
886
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
887
		{
pitiot's avatar
pitiot committed
888
889
890
891
892
893
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

894
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
pitiot's avatar
pitiot committed
895
896

			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
David Cazier's avatar
David Cazier committed
897
			dest /= 2.0f ;
pitiot's avatar
pitiot committed
898
			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
David Cazier's avatar
David Cazier committed
899
			dest[2] = 0 ;
900

David Cazier's avatar
David Cazier committed
901
			agents_[i]->goals_.push_back(dest) ;
902
		}
903

David Cazier's avatar
David Cazier committed
904
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
905
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
906
907
908
909
910
911
		    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
912
		{
David Cazier's avatar
David Cazier committed
913
			envMap_.map.next(dStop2) ;
914
915
			if (dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
916
		}
917

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

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

David Cazier's avatar
David Cazier committed
923
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
924
		{
pitiot's avatar
pitiot committed
925
926
927
928
929
930
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

931
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
pitiot's avatar
pitiot committed
932
			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
David Cazier's avatar
David Cazier committed
933
			dest /= 2.0f ;
pitiot's avatar
pitiot committed
934
			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
David Cazier's avatar
David Cazier committed
935
			dest[2] = 0 ;
936

David Cazier's avatar
David Cazier committed
937
			agents_[i]->goals_.push_back(dest) ;
938
		}
939

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

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

David Cazier's avatar
David Cazier committed
945
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
946
		{
947
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
948
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
949
			dest /= 2.0f ;
pitiot's avatar
pitiot committed
950
			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
David Cazier's avatar
David Cazier committed
951
			dest[2] = 0 ;
952

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

Thomas's avatar
Thomas committed
958
959
960
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
961
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
962
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
963
	{
David Cazier's avatar
David Cazier committed
964
965
966
967
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
968
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
969
970
		    envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
		        || envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
971
		{
David Cazier's avatar
David Cazier committed
972
973
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
974
975
		}

David Cazier's avatar
David Cazier committed
976
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
977
978
979
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
980

David Cazier's avatar
David Cazier committed
981
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
982
		{
pitiot's avatar
pitiot committed
983
			VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
984
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
985
986
		}

David Cazier's avatar
David Cazier committed
987
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
988
		for (unsigned int j = 0 ;
David Cazier's avatar
David Cazier committed
989
990
991
		    envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
		        || envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ;
		    ++j)
David Cazier's avatar
David Cazier committed
992
		{
David Cazier's avatar
David Cazier committed
993
994
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
995
996
		}

David Cazier's avatar
David Cazier committed
997
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_, envMap_.position, dStop, dStop2,
David Cazier's avatar
David Cazier committed
998
		                                             envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
999

David Cazier's avatar
David Cazier committed
1000
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
1001
		{
pitiot's avatar
pitiot committed
1002
			VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
1003
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
1004
1005
		}

David Cazier's avatar
David Cazier committed
1006
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_, envMap_.position, dStop2, dStart,
David Cazier's avatar
David Cazier committed
1007
		                                             envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
1008

David Cazier's avatar
David Cazier committed
1009
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
1010
		{
pitiot's avatar
pitiot committed
1011
			VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
1012
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
1013
1014
1015
		}
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed