simulator.cpp 40.6 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

pitiot's avatar
stats    
pitiot committed
4
5
6
7
8
9
10
11
12
13
14
15
16
timespec timespec_delta(const timespec& t1, const timespec& t2) {
    timespec result;

    if ((t2.tv_nsec - t1.tv_nsec) < 0 /* ns */) {
        result.tv_sec = t2.tv_sec - t1.tv_sec - 1;
        result.tv_nsec = 1000000000 /* ns */ + t2.tv_nsec - t1.tv_nsec;
    } else {
        result.tv_sec = t2.tv_sec - t1.tv_sec;
        result.tv_nsec = t2.tv_nsec - t1.tv_nsec;
    }

    return result;
}
Thomas Jund's avatar
merge    
Thomas Jund committed
17

pitiot's avatar
pitiot committed
18
Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgent, unsigned int nbObst, bool resolution) :
pitiot's avatar
merging    
pitiot committed
19
#ifdef TWO_AND_HALF_DIM
pitiot's avatar
pitiot committed
20
	timeStep_(0.1f),
pitiot's avatar
merging    
pitiot committed
21
#else
pitiot's avatar
pitiot committed
22
	timeStep_(config > 2 ? 0.1f : 0.25f),
pitiot's avatar
pitiot committed
23
//	timeStep_(0.1f),
pitiot's avatar
merging    
pitiot committed
24
#endif
pitiot's avatar
pitiot committed
25

David Cazier's avatar
David Cazier committed
26
27
28
29
30
31
32
	globalTime_(0.0f),
	nbSteps_(0),
	nbUpdates(0),
	nbSorts(0),
	nbRefineCandidate(0),
	nbCoarsenCandidate(0),
	nearNeighbors(0),
David Cazier's avatar
David Cazier committed
33
	totalNeighbors(0),
pitiot's avatar
maj    
pitiot committed
34
	avoidance(1),
35
36
	nb_dead(0),
	time_agent(0),
pitiot's avatar
stats    
pitiot committed
37
38
39
	time_obstacle(0),
	time_behave(0),
	time_multires(0)
Pierre Kraemer's avatar
Pierre Kraemer committed
40
{
pitiot's avatar
pitiot committed
41
	minSize=minS;
pitiot's avatar
pitiot committed
42
	multires=resolution;
pitiot's avatar
pitiot committed
43
	detect_agent_collision=false;
44
45
	srand(10) ;
	nbStepsPerUnit_ = 1 / timeStep_ ;
pitiot's avatar
merging    
pitiot committed
46
	this->config=config;
47
	init(2.0f, nbAgent, nbObst) ;
Thomas's avatar
Thomas committed
48
49
50
51
}

Simulator::~Simulator()
{
David Cazier's avatar
David Cazier committed
52
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
53
		delete agents_[i] ;
Thomas's avatar
Thomas committed
54
55
}

56
void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst, bool enablePathFinding)
Thomas's avatar
Thomas committed
57
{
58
	std::cout << "Setup scenario" << std::endl ;
Thomas's avatar
Thomas committed
59

David Cazier's avatar
David Cazier committed
60
61
	switch (config)
	{
David Cazier's avatar
David Cazier committed
62
		case 0 :
63
			setupCircleScenario(nbAgent,nbObst) ;
Thomas Jund's avatar
Thomas Jund committed
64
//			addPathsToAgents();
David Cazier's avatar
David Cazier committed
65
66
			break ;
		case 1 :
67
			setupCorridorScenario(nbAgent,nbObst) ;
David Cazier's avatar
David Cazier committed
68
69
			break ;
		case 2 :
70
			setupSnakeCorridorScenario(nbAgent,nbObst,10) ;
David Cazier's avatar
David Cazier committed
71
			break ;
Thomas Jund's avatar
Thomas Jund committed
72
		case 3 :
pitiot's avatar
pitiot committed
73
			envMap_.init(config, 500.0f, 500.0f, minSize, 50.0f) ; //grosses cases
pitiot's avatar
pitiot committed
74
			addMovingObstacles(nbObst, 1, 1000);
pitiot's avatar
pitiot committed
75
76
			setupScenario(nbAgent, false) ;

Thomas Jund's avatar
Thomas Jund committed
77
			addPathToObstacles(envMap_.buildingMark, true);
pitiot's avatar
pitiot committed
78
79
////			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
80
			break ;
pitiot's avatar
pitiot committed
81
82
		case 4 : //scenario foule dense + train
			setupCrowdedScenario(nbAgent,8,1) ;
pitiot's avatar
pitiot committed
83
			break ;
pitiot's avatar
pitiot committed
84
		case 5:     //strasbourg
85
			envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ; //svg import
Thomas Jund's avatar
Thomas Jund committed
86
			setupScenario(nbAgent, true) ;
87
//			SelectorCellNotMarked<PFP::MAP> scnm(envMap_.pedWayMark);
88
			addMovingObstacles(nbObst, 1);
Arash HABIBI's avatar
Arash HABIBI committed
89
			addPathToObstacles(envMap_.pedWayMark, false);
Thomas Jund's avatar
Thomas Jund committed
90
			addPathsToAgents();
Jund Thomas's avatar
Jund Thomas committed
91
			break;
92
#ifdef TWO_AND_HALF_DIM
pitiot's avatar
merging    
pitiot committed
93
94
		case 6:

95
			envMap_.init(config,200.0,200.0, minSize, 400.0f);
pitiot's avatar
ok    
pitiot committed
96

pitiot's avatar
pitiot committed
97
			addMovingObstacles(nbObst, 1, 500, 3);
pitiot's avatar
up    
pitiot committed
98
			setupPlanetScenario(nbAgent,nbObst);
Thomas Jund's avatar
Thomas Jund committed
99
			addPathToObstacles(envMap_.buildingMark, true);
pitiot's avatar
pitiot committed
100
			addPathsToAgents();
pitiot's avatar
merging    
pitiot committed
101
102
103

			break;
		case 7 : envMap_.init(config,200.0,200.0, minSize, 400.0f);
pitiot's avatar
ok    
pitiot committed
104

pitiot's avatar
u    
pitiot committed
105
			addMovingObstacles(nbObst, 1 , 300 , 1 );
pitiot's avatar
up    
pitiot committed
106
			setupPlanetScenario(nbAgent,nbObst, 1 , 1);
pitiot's avatar
merging    
pitiot committed
107
108
109
110
			addPathToObstacles(envMap_.buildingMark, true);
			addPathsToAgents();
			break;
		case 8 : envMap_.init(config,200.0,200.0, minSize, 400.0f);
pitiot's avatar
plop    
pitiot committed
111
			addMovingObstacles(nbObst, 1 , 300 , 15);
pitiot's avatar
up    
pitiot committed
112
			setupPlanetScenario(nbAgent,nbObst,1,1);
pitiot's avatar
merging    
pitiot committed
113
114
115
116
			addPathToObstacles(envMap_.buildingMark, true);
			addPathsToAgents();
			break;
		case 9 : envMap_.init(config,200.0,200.0, minSize, 400.0f);
Thomas Jund's avatar
Thomas Jund committed
117
118
			addMovingObstacles(nbObst, 1 , 400 , 6);
			setupPlanetScenario(nbAgent,nbObst, 1, 1, 300);
pitiot's avatar
merging    
pitiot committed
119
120
121
			addPathToObstacles(envMap_.buildingMark, true);
			addPathsToAgents();
			break;
pitiot's avatar
up    
pitiot committed
122

123
#endif
David Cazier's avatar
David Cazier committed
124
		default:
pitiot's avatar
up    
pitiot committed
125
			std::cout << "Unknown scenario ! or not in 2.5D mode" << std::endl ;
David Cazier's avatar
David Cazier committed
126
			exit(1) ;
Thomas's avatar
Thomas committed
127
128
	}

David Cazier's avatar
David Cazier committed
129
130
131
132
133
#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
134
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
135
136
137
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
David Cazier's avatar
David Cazier committed
138
			addPathsToAgents() ;
139
140
		else if (dimension == 2.5f)
			addPathsToAgents_height() ;
Thomas's avatar
Thomas committed
141
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
142
#endif
143

David Cazier's avatar
David Cazier committed
144
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
145
	{
David Cazier's avatar
David Cazier committed
146
147
		agents_[i]->updateObstacleNeighbors() ;
		agents_[i]->updateAgentNeighbors() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
148
	}
149

Pierre Kraemer's avatar
Pierre Kraemer committed
150
#ifndef SPATIAL_HASHING
151
152
	if (multires)
		envMap_.subdivideToProperLevel() ;
David Cazier's avatar
David Cazier committed
153
	//	envMap_.subdivideAllToMaxLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
154
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
155
156
}

David Cazier's avatar
David Cazier committed
157
void Simulator::doStep()
Pierre Kraemer's avatar
Pierre Kraemer committed
158
{
David Cazier's avatar
David Cazier committed
159
160
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
Jund Thomas's avatar
Jund Thomas committed
161
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
162
#endif
pitiot's avatar
stats    
pitiot committed
163
	struct timespec begTime, endTime ;
pitiot's avatar
pitiot committed
164
//	CGoGNout<<" valide dart 559 "<<envMap_.map.isDartValid(Dart(559))<<CGoGNendl;
pitiot's avatar
stats    
pitiot committed
165

David Cazier's avatar
David Cazier committed
166
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
pitiot's avatar
up    
pitiot committed
167

pitiot's avatar
pitiot committed
168
	{
pitiot's avatar
up    
pitiot committed
169
//		unsigned int i =12;
pitiot's avatar
stats    
pitiot committed
170
		clock_gettime(CLOCK_MONOTONIC, &begTime) ;
David Cazier's avatar
David Cazier committed
171
		movingObstacles_[i]->computePrefVelocity() ;
pitiot's avatar
pitiot committed
172
		movingObstacles_[i]->computeNewVelocity() ;
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
173
		movingObstacles_[i]->initForces();
pitiot's avatar
stats    
pitiot committed
174
175
		clock_gettime(CLOCK_MONOTONIC, &endTime) ;
		time_behave+= timespec_delta(begTime,endTime).tv_nsec;
pitiot's avatar
up    
pitiot committed
176
177
178
179
180
181
182
//		CGoGNout<<" Obstacle "<<i<<" : ";
//		for(unsigned int j=0;j<movingObstacles_[i]->nbVertices;j++)
//		{
//			VEC3 dist =movingObstacles_[i]->parts_[j]->getPosition()-movingObstacles_[i]->parts_[(j+1)%movingObstacles_[i]->nbVertices]->getPosition();
//			CGoGNout<<dist.norm()<<" || ";
//		}
//		CGoGNout<<CGoGNendl;
pitiot's avatar
merge    
pitiot committed
183

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
184
	}
185

pitiot's avatar
merge    
pitiot committed
186
	clock_gettime(CLOCK_MONOTONIC, &begTime) ;
187
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
pitiot's avatar
merge    
pitiot committed
188
	{
pitiot's avatar
up    
pitiot committed
189
//		unsigned int i =12;
Thomas Jund's avatar
Thomas Jund committed
190
		movingObstacles_[i]->updateAgentNeighbors();
191
		movingObstacles_[i]->updateObstacleNeighbors();
192
193
		movingObstacles_[i]->updateForces() ;

pitiot's avatar
merge    
pitiot committed
194
195
196
	}
	clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	time_obstacle+= timespec_delta(begTime,endTime).tv_nsec;
pitiot's avatar
up    
pitiot committed
197
//	CGoGNout<<"deplacement obstacles"<<CGoGNendl;
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
198
199
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
	{
pitiot's avatar
up    
pitiot committed
200
//		unsigned int i =12;
pitiot's avatar
merge    
pitiot committed
201
		clock_gettime(CLOCK_MONOTONIC, &begTime) ;
202
		movingObstacles_[i]->applyForces();
pitiot's avatar
merging    
pitiot committed
203
204
205
206
207
		clock_gettime(CLOCK_MONOTONIC, &endTime) ;
		time_behave+= timespec_delta(begTime,endTime).tv_nsec;

		clock_gettime(CLOCK_MONOTONIC, &begTime) ;

pitiot's avatar
pitiot committed
208
		movingObstacles_[i]->updateRegistration();
pitiot's avatar
stats    
pitiot committed
209
210
		clock_gettime(CLOCK_MONOTONIC, &endTime) ;
		time_obstacle+= timespec_delta(begTime,endTime).tv_nsec;
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
211
		movingObstacles_[i]->updateMesh() ;
pitiot's avatar
pitiot committed
212
	}
pitiot's avatar
pitiot committed
213
214
215



pitiot's avatar
up    
pitiot committed
216
//	CGoGNout<<"deplacement obstacles fin"<<CGoGNendl;
pitiot's avatar
stats    
pitiot committed
217

David Cazier's avatar
David Cazier committed
218
219
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
pitiot's avatar
pitiot committed
220
221
		if (agents_[i]->alive)
		{
pitiot's avatar
stats    
pitiot committed
222
			clock_gettime(CLOCK_MONOTONIC, &begTime) ;
pitiot's avatar
pitiot committed
223
			agents_[i]->updateObstacleNeighbors() ;
pitiot's avatar
maj    
pitiot committed
224
225

#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
226
			envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
227
#endif
pitiot's avatar
pitiot committed
228
			agents_[i]->updateAgentNeighbors() ;
pitiot's avatar
maj    
pitiot committed
229
#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
230
			envMap_.map.setCurrentLevel(0) ;
pitiot's avatar
maj    
pitiot committed
231
#endif
pitiot's avatar
stats    
pitiot committed
232
233
234
			clock_gettime(CLOCK_MONOTONIC, &endTime) ;
			time_agent+=timespec_delta(begTime,endTime).tv_nsec;
			clock_gettime(CLOCK_MONOTONIC, &begTime) ;
pitiot's avatar
pitiot committed
235
236
			agents_[i]->computePrefVelocity() ;
			agents_[i]->computeNewVelocity() ;
pitiot's avatar
stats    
pitiot committed
237
238
			clock_gettime(CLOCK_MONOTONIC, &endTime) ;
			time_behave+=timespec_delta(begTime,endTime).tv_nsec;
pitiot's avatar
pitiot committed
239
		}
David Cazier's avatar
David Cazier committed
240
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
241

Pierre Kraemer's avatar
Pierre Kraemer committed
242
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
243
244
245
246
247
248
249
250
251
252
253
	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
254
#else
Thomas Jund's avatar
Thomas Jund committed
255

256
257
	if (multires)
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
Thomas Jund's avatar
Thomas Jund committed
258

David Cazier's avatar
David Cazier committed
259
	nb_dead = 0 ;
pitiot's avatar
stats    
pitiot committed
260
	clock_gettime(CLOCK_MONOTONIC, &begTime) ;
David Cazier's avatar
David Cazier committed
261
262
263
264
265
266
	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
267

268
			if (agents_[i]->part_.crossCell != CGoGN::Algo::Surface::MovingObjects::NO_CROSS)
David Cazier's avatar
David Cazier committed
269
270
271
272
273
274
				envMap_.agentChangeFace(agents_[i], oldFace) ;
		}
		else
			nb_dead++ ;

	}
pitiot's avatar
stats    
pitiot committed
275
276
	clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	time_agent+= timespec_delta(begTime,endTime).tv_nsec;
David Cazier's avatar
David Cazier committed
277
278
279

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

Thomas Jund's avatar
Thomas Jund committed
281
	 if (multires)
pitiot's avatar
stats    
pitiot committed
282
283
	 {
		 clock_gettime(CLOCK_MONOTONIC, &begTime) ;
Thomas Jund's avatar
Thomas Jund committed
284
		 envMap_.updateMap() ;
pitiot's avatar
stats    
pitiot committed
285
286
287
		 clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	 	 time_multires+= timespec_delta(begTime,endTime).tv_nsec;
	 }
David Cazier's avatar
David Cazier committed
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
#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
304
void Simulator::addAgent(const VEC3& start, const VEC3& goal)
David Cazier's avatar
David Cazier committed
305
{
David Cazier's avatar
David Cazier committed
306
	agents_.push_back(new Agent(this, start, goal)) ;
David Cazier's avatar
David Cazier committed
307
308
}

309
310
311
312
313
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
314
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
David Cazier's avatar
David Cazier committed
315
{
pitiot's avatar
merging    
pitiot committed
316
317
318
	if (multires)
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
	else
pitiot's avatar
pitiot committed
319

pitiot's avatar
merging    
pitiot committed
320
321
	{

pitiot's avatar
stats    
pitiot committed
322
323
//		envMap_.init(config, 2000.0f, 2000.0f, minSize, 50.0f) ; //cases fines
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
pitiot's avatar
merging    
pitiot committed
324
325
326
	}


327
	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles"<< std::endl ;
David Cazier's avatar
David Cazier committed
328
329
330
331
332
333
334
335
336
337
338

	// 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
339
//	int radius = nbAgents/(Agent::radius_*Agent::radius_);
Thomas Jund's avatar
Thomas Jund committed
340
	std::cout << "radius " << radius << std::endl;
David Cazier's avatar
David Cazier committed
341
342
343
344
	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
345
		double angle = i * 2.0f * M_PI / float(nbAgents) ;
David Cazier's avatar
David Cazier committed
346
347
348
		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
349
		addAgent(start, goal) ;
pitiot's avatar
pitiot committed
350
//		CGoGNout<<"agent n°" << agents_[i]->agentNo << " : "<< agents_[i]<<CGoGNendl;
351
	}
pitiot's avatar
pitiot committed
352
353
354
355
356
	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
357
	int curGoal=0;
Thomas Jund's avatar
merge    
Thomas Jund committed
358

pitiot's avatar
pitiot committed
359
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
David Cazier's avatar
David Cazier committed
360
	{
Thomas Jund's avatar
merge    
Thomas Jund committed
361
362
		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
363
364
365
366
367
368
369
		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
370
			v=VEC3 (std::cos(angle) * (radius/2), std::sin(angle) * (radius/2), 0) ;
pitiot's avatar
pitiot committed
371
372
373
		}
		start = center + v ;
		positions.push_back(start);
pitiot's avatar
pitiot committed
374
	}
Thomas Jund's avatar
merge    
Thomas Jund committed
375

pitiot's avatar
pitiot committed
376
377
378
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
379
380

		unsigned int nbGoals=360;
pitiot's avatar
pitiot committed
381
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
382
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
383
		{
Thomas Jund's avatar
Thomas Jund committed
384
			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
385
386
387
388
		}

		vPos.clear();
		vPos.push_back(start+xSide+ySide);
pitiot's avatar
merging    
pitiot committed
389
		vPos.push_back(start+xSide-ySide);
pitiot's avatar
pitiot committed
390
		vPos.push_back(start-xSide-ySide);
pitiot's avatar
merging    
pitiot committed
391
392
		vPos.push_back(start-xSide+ySide);

pitiot's avatar
pitiot committed
393
		curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+1)%nbGoals;
394
		mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);
pitiot's avatar
pitiot committed
395

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

pitiot's avatar
pitiot committed
399
400
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
401

pitiot's avatar
pitiot committed
402
403
404
	for (unsigned int i = nbObstacles/2 ; i < nbObstacles ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
405
406
407
408
409
410
411
//		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
412
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
413
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
414
		{
Thomas Jund's avatar
Thomas Jund committed
415
			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
416
417
418
419
420
		}

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		vPos.push_back(start+xSide+ySide);
pitiot's avatar
merging    
pitiot committed
421
		vPos.push_back(start+xSide-ySide);
pitiot's avatar
pitiot committed
422
		vPos.push_back(start-xSide-ySide);
pitiot's avatar
merging    
pitiot committed
423
		vPos.push_back(start-xSide+ySide);
424
425
		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
426

427
428
//		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
429
430

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
431
	}
Thomas's avatar
Thomas committed
432

433
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
434
435
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
436
#endif
pitiot's avatar
merging    
pitiot committed
437
438

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

pitiot's avatar
pitiot committed
441
442
443
void Simulator::setupCrowdedScenario(unsigned int nbAgents , unsigned int nbObstacles, int snake)
{
	if (multires)
pitiot's avatar
pitiot committed
444
		envMap_.init(config, 750.0f, 750.0f, minSize, 400.0f) ; //grosses cases
pitiot's avatar
pitiot committed
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
	else

	{

//		envMap_.init(config, 2000.0f, 2000.0f, minSize, 50.0f) ; //cases fines
		envMap_.init(config, 1000.0f,1000.0f, minSize, 400.0f) ; //grosses cases
	}


	std::cout << " - Setup Crowded Scenario : " << nbAgents << " agents et 1 obstacle"<< std::endl ;



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


	VEC3 center = (envMap_.geometry.min() + envMap_.geometry.max()) / 2 ;


		Dart d =  envMap_.getBelongingCell(center);

pitiot's avatar
pitiot committed
466
467
		unsigned int nbx =  50;
		unsigned int nby = 30;
pitiot's avatar
pitiot committed
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483


	VEC3 pos=center;

	Dart dCell =d;



		float ecart = 5.0f*Agent::radius_;
		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 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
pitiot's avatar
pitiot committed
484
485
486
				VEC3 goal = posagent;
				goal[0]*=-1;
				addAgent(posagent, goal, dCell) ;
pitiot's avatar
pitiot committed
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
			}
		}



	VEC3 xSide (5.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,10.0f,0.0f);
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;

	int curGoal=0;
	VEC3 start;

	start = center;
	float mapSize=(envMap_.geometry.max()[1]-envMap_.geometry.min()[1]);
	start[1]-= mapSize/4;
pitiot's avatar
pop    
pitiot committed
503
	start[0]-=10.0f;
pitiot's avatar
pitiot committed
504
505
506
507
508
509
510
511
512
513
514
	std::vector<VEC3> goals;
	VEC3 goal ;


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

if(snake==1)
{
pitiot's avatar
pitiot committed
515
516
	CGoGNout<<"IT'S A SNAKE"<<CGoGNendl;
	int snakeSize=nbObstacles;
pitiot's avatar
pitiot committed
517
518
519
520
	std::vector<PFP::VEC3> positions [snakeSize] ;
	positions[0]=vPos;
	goal=center;
	goals.push_back(goal);
pitiot's avatar
pop    
pitiot committed
521
	goal=center+VEC3(mapSize/6,mapSize/6,0);
pitiot's avatar
pitiot committed
522
523
524
	goals.push_back(goal);
	goal=center;
	goals.push_back(goal);
pitiot's avatar
pop    
pitiot committed
525
	goal=center+VEC3(-mapSize/6,mapSize/6,0);
pitiot's avatar
pitiot committed
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
	goals.push_back(goal);
	goal=center;
	goals.push_back(goal);
	goal=start;
	goals.push_back(goal);
	for (int i = 1 ; i < snakeSize ; i++)
	{

		start=start-ySide-ySide;
		vPos.clear();
		vPos.push_back(start+xSide+ySide);
		vPos.push_back(start+xSide-ySide);
		vPos.push_back(start-xSide-ySide);
		vPos.push_back(start-xSide+ySide);
		positions[i]=vPos;
	}
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
	ArticulatedObstacle * art=new ArticulatedObstacle (this,0,0,positions,snakeSize,goals,curGoal);
	for (int i = 0 ; i < snakeSize ; i++)
		movingObstacles_.push_back(art->members[i]);
}
else
{
	goal=start;
	goal[1]+=(envMap_.geometry.max()[1]-envMap_.geometry.min()[1])/2;


	goals.push_back(goal);
	goals.push_back(start);
	curGoal = 0;
pitiot's avatar
pitiot committed
556
557
558
	if (nbObstacles !=0)
	{
		mo4= new MovingObstacle(this, 0,vPos,goals, true, true,curGoal);
pitiot's avatar
pitiot committed
559

pitiot's avatar
pitiot committed
560
561
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
562
563
564
565
566
567
568
569
570
571
572
573
}


#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
#endif

	std::cout << "nb agents : " << agents_.size() << std::endl ;
}


David Cazier's avatar
David Cazier committed
574
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
575
{
pitiot's avatar
pitiot committed
576
577
	VEC3 xSide (5.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,10.0f,0.0f);
pitiot's avatar
merging    
pitiot committed
578
579
	if (multires)
	{
pitiot's avatar
pitiot committed
580
//		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
pitiot's avatar
up    
pitiot committed
581
		envMap_.init(config, nbObstacles > 60 ? ((nbObstacles*20*xSide[0])/6) : 1000.0f , 960.0f, minSize, 320.0f) ; //grosses cases
pitiot's avatar
merging    
pitiot committed
582
583
584
585
	}
	else
	{

pitiot's avatar
merging    
pitiot committed
586
587
//		envMap_.init(config, nbObstacles > 60 ? ((nbObstacles*20*xSide[0])/6) : 1000.0f, 960.0f, minSize, 320.0f) ; //grosses cases
		envMap_.init(config,nbObstacles > 60 ? ((nbObstacles*20*xSide[0])/6) : 1000.0f, 960.0f, Agent::range_/5, Agent::range_/5) ; //cases fines
pitiot's avatar
merging    
pitiot committed
588
589
590
	}


David Cazier's avatar
David Cazier committed
591
592
593
594
595
596
597
598
599
600
	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 ;
pitiot's avatar
pitiot committed
601
	int xStartDelta = envMap_.geometry.size(0) / 10 ;
David Cazier's avatar
David Cazier committed
602
603
604
605
	int yStartMin = envMap_.geometry.min()[1] + yBorder ;
	int yStartDelta = envMap_.geometry.size(1) - 2 * yBorder ;

	// Arrivée des agents à l'opposée
pitiot's avatar
pitiot committed
606
	int xGoalDelta = envMap_.geometry.size(0) / 10 ;
David Cazier's avatar
David Cazier committed
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
	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
pitiot's avatar
pitiot committed
624
625
	xStartMin = envMap_.geometry.min()[0] + envMap_.geometry.size(0) / 5 ;
	xStartDelta = 6*envMap_.geometry.size(0) / 10 ;
David Cazier's avatar
David Cazier committed
626
627
	yStartMin = envMap_.geometry.min()[1] + yBorder ;
	yStartDelta = envMap_.geometry.size(1) / 5 ;
pitiot's avatar
pitiot committed
628

David Cazier's avatar
David Cazier committed
629
630
631
632
	// Arrivée des obstacles à l'opposée
	yGoalDelta = envMap_.geometry.size(1) / 5 ;
	yGoalMin = envMap_.geometry.max()[1] - yBorder - yGoalDelta ;

pitiot's avatar
pitiot committed
633

David Cazier's avatar
David Cazier committed
634
635
636
637
638
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
pitiot's avatar
pitiot committed
639
		float x = xStartMin + ((int)(i*2*xSide[0])) % xStartDelta;
David Cazier's avatar
David Cazier committed
640
641
642
643
644
645
646
647
648
649
650
651
652
//		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 ;
		}
pitiot's avatar
merging    
pitiot committed
653
654
655
656
		vPos.push_back(start+xSide+ySide);
		vPos.push_back(start+xSide-ySide);
		vPos.push_back(start-xSide-ySide);
		vPos.push_back(start-xSide+ySide);
David Cazier's avatar
David Cazier committed
657
658
659
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
Thomas Jund's avatar
merge    
Thomas Jund committed
660

pitiot's avatar
pitiot committed
661
		mo4= new MovingObstacle(this, i,vPos,goals, true, false,1);
David Cazier's avatar
David Cazier committed
662
663


664
665
666
667
668
669
670
671
672
673
674
675
676
677
//		//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
678
679
680
681
682

		movingObstacles_.push_back(mo4);
	}
}

pitiot's avatar
merging    
pitiot committed
683
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
David Cazier's avatar
David Cazier committed
684
{
pitiot's avatar
merging    
pitiot committed
685
686
687
688
689
690
	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
691
692

	// Bordure à éviter autour de la scène (10% de sa taille)
pitiot's avatar
merging    
pitiot committed
693
694
	int xBorder = envMap_.geometry.size(0) / 5 ;
	int yBorder = envMap_.geometry.size(1) / 5 ;
David Cazier's avatar
David Cazier committed
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710

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

David Cazier's avatar
David Cazier committed
715
		// Un agent sur 2 va de droite à gauche
pitiot's avatar
merging    
pitiot committed
716
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
717
		if (i % 2 == 1)
pitiot's avatar
merging    
pitiot committed
718
719
720
721
722
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
		}
pitiot's avatar
merge    
pitiot committed
723

pitiot's avatar
merging    
pitiot committed
724
		addAgent(start, goal) ;
pitiot's avatar
merge    
pitiot committed
725

Thomas's avatar
Thomas committed
726
727
	}

David Cazier's avatar
David Cazier committed
728
729
730
731
	// 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
732
	yStartDelta = envMap_.geometry.size(1) / 10 ;
David Cazier's avatar
David Cazier committed
733
734

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

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

739
	VEC3 xSide (5.0f,0.0f,0.0f);
740
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
741

Thomas Jund's avatar
Thomas Jund committed
742
	std::cout << "geom : " << envMap_.geometry << std::endl;
pitiot's avatar
merging    
pitiot committed
743
744
745

	int sumObstacles=0;
	for(unsigned int j = 0; j<nbSnakes; j++)
David Cazier's avatar
David Cazier committed
746
	{
pitiot's avatar
merging    
pitiot committed
747
748
749
750
		std::vector<PFP::VEC3> positions [snakeSize] ;
		float x = xStartMin + rand() % xStartDelta;
		VEC3 goal;

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

pitiot's avatar
merging    
pitiot committed
753
754
755
756
757

		std::vector<VEC3> vPos;


		vPos.push_back(start+xSide+ySide);
pitiot's avatar
merging    
pitiot committed
758
		vPos.push_back(start+xSide-ySide);
pitiot's avatar
merging    
pitiot committed
759
		vPos.push_back(start-xSide-ySide);
pitiot's avatar
merging    
pitiot committed
760
		vPos.push_back(start-xSide+ySide);
pitiot's avatar
merging    
pitiot committed
761
762
763
764

		std::vector<VEC3> goals;
		goals.push_back(start);
		int r=0;
Thomas Jund's avatar
Thomas Jund committed
765
		while (r<80)
David Cazier's avatar
David Cazier committed
766
		{
pitiot's avatar
merging    
pitiot committed
767
768
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
Thomas Jund's avatar
Thomas Jund committed
769
770
771


			if ((r==0 || angle(goal-goals[r],goals[r]-goals[r-1])<M_PI/2.0f) )
772
			{
Thomas Jund's avatar
Thomas Jund committed
773
774
775

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

pitiot's avatar
merging    
pitiot committed
776
				goals.push_back(goal);
777
				r++;
pitiot's avatar
merging    
pitiot committed
778
779
			}

David Cazier's avatar
David Cazier committed
780
		}
pitiot's avatar
merging    
pitiot committed
781
782
783
784
785
786
787
788
789
790

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

David Cazier's avatar
David Cazier committed
791
			vPos.push_back(start+xSide+ySide);
pitiot's avatar
merging    
pitiot committed
792
			vPos.push_back(start+xSide-ySide);
David Cazier's avatar
David Cazier committed
793
			vPos.push_back(start-xSide-ySide);
pitiot's avatar
merging    
pitiot committed
794
			vPos.push_back(start-xSide+ySide);
pitiot's avatar
merging    
pitiot committed
795
			positions[i]=vPos;
David Cazier's avatar
David Cazier committed
796
		}
pitiot's avatar
merging    
pitiot committed
797
798
799
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
800

pitiot's avatar
merging    
pitiot committed
801
802
		for (int i = 0 ; i < snakeSize ; i++)
			movingObstacles_.push_back(art->members[i]);
David Cazier's avatar
David Cazier committed
803
	}
Thomas's avatar
Thomas committed
804
805
}

David Cazier's avatar
David Cazier committed
806
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
807
{
David Cazier's avatar
David Cazier committed
808
809
810
811
812
	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
813

David Cazier's avatar
David Cazier committed
814
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
815
	{
David Cazier's avatar
David Cazier committed
816
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
817
		{
818
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
David Cazier's avatar
David Cazier committed
819
			addAgent(posagent, -1.0f * posagent) ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
820
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
821
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
822
823
}

Thomas Jund's avatar
Thomas Jund committed
824
void Simulator::setupScenario(unsigned int nbMaxAgent, bool pedWay)
Pierre Kraemer's avatar
Pierre Kraemer committed
825
826
{
	/*
David Cazier's avatar
David Cazier committed
827
	 * Add agents, specifying their start position, and store their goal on the
David Cazier's avatar
David Cazier committed
828
829
	 * opposite side of the environment.
	 */
pitiot's avatar
pitiot committed
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
	Dart d = envMap_.map.begin();
	CellMarker<FACE> to_fill(envMap_.map);
	int nb_cells=0;
	while (d!= envMap_.map.end())
	{
		if(!to_fill.isMarked(d))
			{
			to_fill.mark(d);
			nb_cells ++;
			}

		envMap_.map.next(d);
	}


845
	TraversorF<PFP::MAP> tF(envMap_.map);
pitiot's avatar
pitiot committed
846
	d = tF.begin() ;
David Cazier's avatar
David Cazier committed
847

pitiot's avatar
pitiot committed
848
	unsigned int nbx =  1;
pitiot's avatar
pitiot committed
849
	unsigned int nby = 5;
David Cazier's avatar
David Cazier committed
850
851
852
853


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

David Cazier's avatar
David Cazier committed
854
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
855
	{
Thomas Jund's avatar
Thomas Jund committed
856
		VEC3 pos;
David Cazier's avatar
David Cazier committed
857
858
		bool found = false ;
		Dart dCell ;
859
		while (!found && d != tF.end())
David Cazier's avatar
David Cazier committed
860
		{
861
			if (!envMap_.buildingMark.isMarked(d)
Thomas Jund's avatar
Thomas Jund committed
862
			    && (!pedWay || envMap_.pedWayMark.isMarked(d))
863
			    )
David Cazier's avatar
David Cazier committed
864
			{
865
				pos = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
866
867
868
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
869
			}
870
			d = tF.next() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
871
872
		}

David Cazier's avatar
David Cazier committed
873
874
		if (found)
		{
Thomas Jund's avatar
Thomas Jund committed
875
			float ecart = 2.0f*Agent::radius_;
876
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
877
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
David Cazier's avatar
David Cazier committed
878
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
879
			{
David Cazier's avatar
David Cazier committed
880
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
881
				{
882
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
883
					addAgent(posagent, -1.0f * posagent, dCell) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
884
885
886
887
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
888
889
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
890
}
891

pitiot's avatar
up    
pitiot committed
892
void Simulator::setupPlanetScenario(unsigned int nbAgents, unsigned int nbObstacles,unsigned int nbx,unsigned int nby, float areaMin)
893
894
895
896
897
898
{
	/*
	* Add agents, specifying their start position, and store their goals on the
	* opposite side of the environment.
	*/
	Dart d = envMap_.map.begin();
pitiot's avatar
pitiot committed
899
900
901
902
903
904
905
906
907
908
909
910
911
	CellMarker<FACE> to_fill(envMap_.map);
	int nb_cells=0;
	while (d!= envMap_.map.end())
	{
		if(!to_fill.isMarked(d))
			{
			to_fill.mark(d);
			nb_cells ++;
			}

		envMap_.map.next(d);
	}
	d=envMap_.map.begin();
912
913
914
	CellMarker<FACE> filled(envMap_.map);


pitiot's avatar
u    
pitiot committed
915
	unsigned int bMax = nbx * nby > 0 ? (nbx * nby < nbAgents ? nbAgents/ (nbx * nby) : nbAgents) : nbAgents ;
916
917
918
919
920
921
922
923

	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())
		{
pitiot's avatar
up    
pitiot committed
924
925
			float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
			if(!filled.isMarked(d) && !envMap_.buildingMark.isMarked(d) && envMap_.movingObstacleFree(d)&& area >areaMin )
926
927
928
929
930
931
932
933
934
935
936
			{
				filled.mark(d);
				pos = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position);
				dCell = d;
				found = true;
			}
			envMap_.map.next(d);
		}

		if(found)
		{
pitiot's avatar
test    
pitiot committed
937
			float ecart = 4.0f;
pitiot's avatar
pitiot committed
938
			VEC3 displ2(- (float(nbx)/2.0f * ecart), - (float(nby)/2.0f * ecart), 0);
939
			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, dCell, envMap_.position), 0.0f);
pitiot's avatar
pitiot committed
940
941
			Agent::rotate(Agent::xyPlane, pl.normal(), displ2);
			VEC3 posinit = pos + displ2;
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
			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();
}

pitiot's avatar
up    
pitiot committed
964
void Simulator::addMovingObstacles(unsigned int nb, unsigned int type, float areaMin, int randLimace)
965
966
967
968
969
970
971
972
973
{
	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())
		{
pitiot's avatar
ok    
pitiot committed
974
			std::vector<Obstacle*>& obst = envMap_.obstvect[d];