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

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
20
21
22
23
#ifdef TWO_AND_HALF_DIM
	timeStep_(0.01f),
#else
	timeStep_(0.25f),
#endif
pitiot's avatar
pitiot committed
24
//	timeStep_(0.2f),
David Cazier's avatar
David Cazier committed
25
26
27
28
29
30
31
	globalTime_(0.0f),
	nbSteps_(0),
	nbUpdates(0),
	nbSorts(0),
	nbRefineCandidate(0),
	nbCoarsenCandidate(0),
	nearNeighbors(0),
David Cazier's avatar
David Cazier committed
32
	totalNeighbors(0),
pitiot's avatar
maj    
pitiot committed
33
	avoidance(1),
34
35
	nb_dead(0),
	time_agent(0),
pitiot's avatar
stats    
pitiot committed
36
37
38
	time_obstacle(0),
	time_behave(0),
	time_multires(0)
Pierre Kraemer's avatar
Pierre Kraemer committed
39
{
pitiot's avatar
pitiot committed
40
	minSize=minS;
pitiot's avatar
pitiot committed
41
	multires=resolution;
pitiot's avatar
pitiot committed
42
	detect_agent_collision=false;
43
44
	srand(10) ;
	nbStepsPerUnit_ = 1 / timeStep_ ;
pitiot's avatar
merging    
pitiot committed
45
	this->config=config;
46
	init(2.0f, nbAgent, nbObst) ;
Thomas's avatar
Thomas committed
47
48
49
50
}

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

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

David Cazier's avatar
David Cazier committed
59
60
	switch (config)
	{
David Cazier's avatar
David Cazier committed
61
		case 0 :
62
			setupCircleScenario(nbAgent,nbObst) ;
Thomas Jund's avatar
Thomas Jund committed
63
//			addPathsToAgents();
David Cazier's avatar
David Cazier committed
64
65
			break ;
		case 1 :
66
			setupCorridorScenario(nbAgent,nbObst) ;
David Cazier's avatar
David Cazier committed
67
68
			break ;
		case 2 :
69
			setupSnakeCorridorScenario(nbAgent,nbObst,10) ;
David Cazier's avatar
David Cazier committed
70
			break ;
Thomas Jund's avatar
Thomas Jund committed
71
		case 3 :
72
			envMap_.init(config, 1000.0f, 1000.0f, minSize, 100.0f) ; //grosses cases
Thomas Jund's avatar
Thomas Jund committed
73
			setupScenario(nbAgent, false) ;
74
			addMovingObstacles(nbObst, 1);
Thomas Jund's avatar
Thomas Jund committed
75
			addPathToObstacles(envMap_.buildingMark, true);
pitiot's avatar
pitiot committed
76
77
////			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
78
			break ;
pitiot's avatar
pitiot committed
79
80
81
//		case 4 :
//			importAgents("myAgents.pos") ;
//			break ;
Jund Thomas's avatar
Jund Thomas committed
82
		case 5:
83
			envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ; //svg import
Thomas Jund's avatar
Thomas Jund committed
84
			setupScenario(nbAgent, true) ;
85
//			SelectorCellNotMarked<PFP::MAP> scnm(envMap_.pedWayMark);
86
			addMovingObstacles(nbObst, 1);
Arash HABIBI's avatar
Arash HABIBI committed
87
			addPathToObstacles(envMap_.pedWayMark, false);
Thomas Jund's avatar
Thomas Jund committed
88
			addPathsToAgents();
Jund Thomas's avatar
Jund Thomas committed
89
			break;
90
#ifdef TWO_AND_HALF_DIM
pitiot's avatar
merging    
pitiot committed
91
92
		case 6:

93
94
			envMap_.init(config,200.0,200.0, minSize, 400.0f);
			setupPlanetScenario(nbAgent,nbObst);
pitiot's avatar
pitiot committed
95
			addMovingObstacles(nbObst, 1);
Thomas Jund's avatar
Thomas Jund committed
96
			addPathToObstacles(envMap_.buildingMark, true);
pitiot's avatar
pitiot committed
97
			addPathsToAgents();
pitiot's avatar
merging    
pitiot committed
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117

			break;
		case 7 : envMap_.init(config,200.0,200.0, minSize, 400.0f);
			setupPlanetScenario(nbAgent,nbObst);
			addMovingObstacles(nbObst, 1 , 100 );
			addPathToObstacles(envMap_.buildingMark, true);
			addPathsToAgents();
			break;
		case 8 : envMap_.init(config,200.0,200.0, minSize, 400.0f);
			setupPlanetScenario(nbAgent,nbObst);
			addMovingObstacles(nbObst, 1 , 100 );
			addPathToObstacles(envMap_.buildingMark, true);
			addPathsToAgents();
			break;
		case 9 : envMap_.init(config,200.0,200.0, minSize, 400.0f);
			setupPlanetScenario(nbAgent,nbObst);
			addMovingObstacles(nbObst, 1 , 100 );
			addPathToObstacles(envMap_.buildingMark, true);
			addPathsToAgents();
			break;
118
119
120
#else
			std::cout << "Agents not in 2.5D mode" << std::endl;
#endif
David Cazier's avatar
David Cazier committed
121
122
123
		default:
			std::cout << "Unknown scenario !" << std::endl ;
			exit(1) ;
Thomas's avatar
Thomas committed
124
125
	}

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

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

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

David Cazier's avatar
David Cazier committed
154
void Simulator::doStep()
Pierre Kraemer's avatar
Pierre Kraemer committed
155
{
David Cazier's avatar
David Cazier committed
156
157
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
Jund Thomas's avatar
Jund Thomas committed
158
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
159
#endif
pitiot's avatar
stats    
pitiot committed
160
161
	struct timespec begTime, endTime ;

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

pitiot's avatar
pitiot committed
164
	{
pitiot's avatar
up    
pitiot committed
165
//		unsigned int i =12;
pitiot's avatar
stats    
pitiot committed
166
		clock_gettime(CLOCK_MONOTONIC, &begTime) ;
David Cazier's avatar
David Cazier committed
167
		movingObstacles_[i]->computePrefVelocity() ;
pitiot's avatar
pitiot committed
168
		movingObstacles_[i]->computeNewVelocity() ;
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
169
		movingObstacles_[i]->initForces();
pitiot's avatar
stats    
pitiot committed
170
171
		clock_gettime(CLOCK_MONOTONIC, &endTime) ;
		time_behave+= timespec_delta(begTime,endTime).tv_nsec;
pitiot's avatar
up    
pitiot committed
172
173
174
175
176
177
178
//		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
179

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
180
	}
181

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

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

		clock_gettime(CLOCK_MONOTONIC, &begTime) ;

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



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

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

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

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

252
253
	if (multires)
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
Thomas Jund's avatar
Thomas Jund committed
254

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

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

	}
pitiot's avatar
stats    
pitiot committed
271
272
	clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	time_agent+= timespec_delta(begTime,endTime).tv_nsec;
David Cazier's avatar
David Cazier committed
273
274
275

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

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

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

pitiot's avatar
merging    
pitiot committed
316
317
	{

pitiot's avatar
stats    
pitiot committed
318
319
//		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
320
321
322
	}


323
	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles"<< std::endl ;
David Cazier's avatar
David Cazier committed
324
325
326
327
328
329
330
331
332
333
334

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

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

pitiot's avatar
pitiot committed
372
373
374
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
375
376

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

		vPos.clear();
		vPos.push_back(start+xSide+ySide);
pitiot's avatar
merging    
pitiot committed
385
		vPos.push_back(start+xSide-ySide);
pitiot's avatar
pitiot committed
386
		vPos.push_back(start-xSide-ySide);
pitiot's avatar
merging    
pitiot committed
387
388
		vPos.push_back(start-xSide+ySide);

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

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

pitiot's avatar
pitiot committed
395
396
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
397

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

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

423
424
//		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
425
426

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
427
	}
Thomas's avatar
Thomas committed
428

429
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
430
431
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
432
#endif
pitiot's avatar
merging    
pitiot committed
433
434

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

David Cazier's avatar
David Cazier committed
437
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
438
{
pitiot's avatar
pitiot committed
439
440
	VEC3 xSide (5.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,10.0f,0.0f);
pitiot's avatar
merging    
pitiot committed
441
442
	if (multires)
	{
pitiot's avatar
pitiot committed
443
//		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
pitiot's avatar
up    
pitiot committed
444
		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
445
446
447
448
	}
	else
	{

pitiot's avatar
merging    
pitiot committed
449
450
//		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
451
452
453
	}


David Cazier's avatar
David Cazier committed
454
455
456
457
458
459
460
461
462
463
	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
464
	int xStartDelta = envMap_.geometry.size(0) / 10 ;
David Cazier's avatar
David Cazier committed
465
466
467
468
	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
469
	int xGoalDelta = envMap_.geometry.size(0) / 10 ;
David Cazier's avatar
David Cazier committed
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
	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
487
488
	xStartMin = envMap_.geometry.min()[0] + envMap_.geometry.size(0) / 5 ;
	xStartDelta = 6*envMap_.geometry.size(0) / 10 ;
David Cazier's avatar
David Cazier committed
489
490
	yStartMin = envMap_.geometry.min()[1] + yBorder ;
	yStartDelta = envMap_.geometry.size(1) / 5 ;
pitiot's avatar
pitiot committed
491

David Cazier's avatar
David Cazier committed
492
493
494
495
	// 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
496

David Cazier's avatar
David Cazier committed
497
498
499
500
501
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
pitiot's avatar
pitiot committed
502
		float x = xStartMin + ((int)(i*2*xSide[0])) % xStartDelta;
David Cazier's avatar
David Cazier committed
503
504
505
506
507
508
509
510
511
512
513
514
515
//		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
516
517
518
519
		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
520
521
522
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
Thomas Jund's avatar
merge    
Thomas Jund committed
523

pitiot's avatar
pitiot committed
524
		mo4= new MovingObstacle(this, i,vPos,goals, true, false,1);
David Cazier's avatar
David Cazier committed
525
526


527
528
529
530
531
532
533
534
535
536
537
538
539
540
//		//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
541
542
543
544
545

		movingObstacles_.push_back(mo4);
	}
}

pitiot's avatar
merging    
pitiot committed
546
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
David Cazier's avatar
David Cazier committed
547
{
pitiot's avatar
merging    
pitiot committed
548
549
550
551
552
553
	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
554
555

	// Bordure à éviter autour de la scène (10% de sa taille)
pitiot's avatar
merging    
pitiot committed
556
557
	int xBorder = envMap_.geometry.size(0) / 5 ;
	int yBorder = envMap_.geometry.size(1) / 5 ;
David Cazier's avatar
David Cazier committed
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573

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

David Cazier's avatar
David Cazier committed
578
		// Un agent sur 2 va de droite à gauche
pitiot's avatar
merging    
pitiot committed
579
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
580
		if (i % 2 == 1)
pitiot's avatar
merging    
pitiot committed
581
582
583
584
585
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
		}
pitiot's avatar
merge    
pitiot committed
586

pitiot's avatar
merging    
pitiot committed
587
		addAgent(start, goal) ;
pitiot's avatar
merge    
pitiot committed
588

Thomas's avatar
Thomas committed
589
590
	}

David Cazier's avatar
David Cazier committed
591
592
593
594
	// 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
595
	yStartDelta = envMap_.geometry.size(1) / 10 ;
David Cazier's avatar
David Cazier committed
596
597

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

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

602
	VEC3 xSide (5.0f,0.0f,0.0f);
603
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
604

Thomas Jund's avatar
Thomas Jund committed
605
	std::cout << "geom : " << envMap_.geometry << std::endl;
pitiot's avatar
merging    
pitiot committed
606
607
608

	int sumObstacles=0;
	for(unsigned int j = 0; j<nbSnakes; j++)
David Cazier's avatar
David Cazier committed
609
	{
pitiot's avatar
merging    
pitiot committed
610
611
612
613
		std::vector<PFP::VEC3> positions [snakeSize] ;
		float x = xStartMin + rand() % xStartDelta;
		VEC3 goal;

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

pitiot's avatar
merging    
pitiot committed
616
617
618
619
620

		std::vector<VEC3> vPos;


		vPos.push_back(start+xSide+ySide);
pitiot's avatar
merging    
pitiot committed
621
		vPos.push_back(start+xSide-ySide);
pitiot's avatar
merging    
pitiot committed
622
		vPos.push_back(start-xSide-ySide);
pitiot's avatar
merging    
pitiot committed
623
		vPos.push_back(start-xSide+ySide);
pitiot's avatar
merging    
pitiot committed
624
625
626
627

		std::vector<VEC3> goals;
		goals.push_back(start);
		int r=0;
Thomas Jund's avatar
Thomas Jund committed
628
		while (r<80)
David Cazier's avatar
David Cazier committed
629
		{
pitiot's avatar
merging    
pitiot committed
630
631
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
Thomas Jund's avatar
Thomas Jund committed
632
633
634


			if ((r==0 || angle(goal-goals[r],goals[r]-goals[r-1])<M_PI/2.0f) )
635
			{
Thomas Jund's avatar
Thomas Jund committed
636
637
638

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

pitiot's avatar
merging    
pitiot committed
639
				goals.push_back(goal);
640
				r++;
pitiot's avatar
merging    
pitiot committed
641
642
			}

David Cazier's avatar
David Cazier committed
643
		}
pitiot's avatar
merging    
pitiot committed
644
645
646
647
648
649
650
651
652
653

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

David Cazier's avatar
David Cazier committed
654
			vPos.push_back(start+xSide+ySide);
pitiot's avatar
merging    
pitiot committed
655
			vPos.push_back(start+xSide-ySide);
David Cazier's avatar
David Cazier committed
656
			vPos.push_back(start-xSide-ySide);
pitiot's avatar
merging    
pitiot committed
657
			vPos.push_back(start-xSide+ySide);
pitiot's avatar
merging    
pitiot committed
658
			positions[i]=vPos;
David Cazier's avatar
David Cazier committed
659
		}
pitiot's avatar
merging    
pitiot committed
660
661
662
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
663

pitiot's avatar
merging    
pitiot committed
664
665
		for (int i = 0 ; i < snakeSize ; i++)
			movingObstacles_.push_back(art->members[i]);
David Cazier's avatar
David Cazier committed
666
	}
Thomas's avatar
Thomas committed
667
668
}

David Cazier's avatar
David Cazier committed
669
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
670
{
David Cazier's avatar
David Cazier committed
671
672
673
674
675
	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
676

David Cazier's avatar
David Cazier committed
677
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
678
	{
David Cazier's avatar
David Cazier committed
679
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
680
		{
681
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
David Cazier's avatar
David Cazier committed
682
			addAgent(posagent, -1.0f * posagent) ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
683
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
684
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
685
686
}

Thomas Jund's avatar
Thomas Jund committed
687
void Simulator::setupScenario(unsigned int nbMaxAgent, bool pedWay)
Pierre Kraemer's avatar
Pierre Kraemer committed
688
689
{
	/*
David Cazier's avatar
David Cazier committed
690
	 * Add agents, specifying their start position, and store their goal on the
David Cazier's avatar
David Cazier committed
691
692
	 * opposite side of the environment.
	 */
pitiot's avatar
pitiot committed
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
	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);
	}


708
	TraversorF<PFP::MAP> tF(envMap_.map);
pitiot's avatar
pitiot committed
709
	d = tF.begin() ;
David Cazier's avatar
David Cazier committed
710

pitiot's avatar
pitiot committed
711
	unsigned int nbx =  nbMaxAgent==0 ? 1 :  (nbMaxAgent / (nb_cells))/5;
pitiot's avatar
pitiot committed
712
	unsigned int nby = 5;
David Cazier's avatar
David Cazier committed
713
714
715
716


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

David Cazier's avatar
David Cazier committed
717
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
718
	{
Thomas Jund's avatar
Thomas Jund committed
719
		VEC3 pos;
David Cazier's avatar
David Cazier committed
720
721
		bool found = false ;
		Dart dCell ;
722
		while (!found && d != tF.end())
David Cazier's avatar
David Cazier committed
723
		{
724
			if (!envMap_.buildingMark.isMarked(d)
Thomas Jund's avatar
Thomas Jund committed
725
			    && (!pedWay || envMap_.pedWayMark.isMarked(d))
726
			    )
David Cazier's avatar
David Cazier committed
727
			{
728
				pos = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
729
730
731
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
732
			}
733
			d = tF.next() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
734
735
		}

David Cazier's avatar
David Cazier committed
736
737
		if (found)
		{
Thomas Jund's avatar
Thomas Jund committed
738
			float ecart = 2.0f*Agent::radius_;
739
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
740
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
David Cazier's avatar
David Cazier committed
741
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
742
			{
David Cazier's avatar
David Cazier committed
743
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
744
				{
745
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
746
					addAgent(posagent, -1.0f * posagent, dCell) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
747
748
749
750
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
751
752
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
753
}
754

755
756
757
758
759
760
761
void Simulator::setupPlanetScenario(unsigned int nbAgents, unsigned int nbObstacles)
{
	/*
	* Add agents, specifying their start position, and store their goals on the
	* opposite side of the environment.
	*/
	Dart d = envMap_.map.begin();
pitiot's avatar
pitiot committed
762
763
764
765
766
767
768
769
770
771
772
773
774
	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();
775
776
	CellMarker<FACE> filled(envMap_.map);

pitiot's avatar
pitiot committed
777
	unsigned int nbx =1;
pitiot's avatar
pitiot committed
778
	unsigned int nby = 1;
779

pitiot's avatar
pitiot committed
780
	unsigned int bMax = nbx * nby > 0 ? nbAgents / (nbx * nby) : nbAgents ;
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800

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

		if(found)
		{
pitiot's avatar
test    
pitiot committed
801
			float ecart = 4.0f;
pitiot's avatar
pitiot committed
802
			VEC3 displ2(- (float(nbx)/2.0f * ecart), - (float(nby)/2.0f * ecart), 0);
803
			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, dCell, envMap_.position), 0.0f);
pitiot's avatar
pitiot committed
804
805
			Agent::rotate(Agent::xyPlane, pl.normal(), displ2);
			VEC3 posinit = pos + displ2;
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
			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
merging    
pitiot committed
828
void Simulator::addMovingObstacles(unsigned int nb, unsigned int type, float areaMin)
829
830
831
832
833
834
835
836
837
838
839
840
841
{
	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)
				)
			{
842
				float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
pitiot's avatar
merging    
pitiot committed
843
				if(area>areaMin)
844
845
846
847
				{
					dCell = d ;
					found = true ;
				}
848
849
850
851
852
			}
			d = tF.next() ;
		}

		if (found)
pitiot's avatar
pitiot committed
853
		{
854
			addMovingObstacle(dCell,type) ;
pitiot's avatar
pitiot committed
855
856
//			CGoGNout<<" dCell initiale :"<< d <<CGoGNendl;
		}
857
858
859
860
861
862
863
	}
}

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

866
867
	MovingMesh* mm = NULL;

868
869
870
871
	switch(obstType)
	{
		case 0 :
		{
872
			start = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
873
874
875
			VEC3 xSide (2.0f,0.0f,0.0f);
			VEC3 ySide (0.0f,5.0f,0.0f);

876
#ifdef TWO_AND_HALF_DIM
pitiot's avatar
up    
pitiot committed
877
878
			float xSideF=2.0f;
			float ySideF=5.0f;
879
			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::facePlane<PFP>(envMap_.map, d, envMap_.position));
pitiot's avatar
up    
pitiot committed
880
881
			VEC3 norm =Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, d, envMap_.position) ;
			VEC3 dir1 =VEC3(norm[1]+norm[2],norm[2]-norm[0],-norm[0]-norm[1]);
882

pitiot's avatar
up    
pitiot committed
883
884
			dir1.normalize();
			VEC3 dir2 = norm ^dir1;
885

pitiot's avatar
up    
pitiot committed
886
887
888
889
			vPos.push_back(start-xSideF*dir1-ySideF*dir2);
			vPos.push_back(start+xSideF*dir1-ySideF*dir2);
			vPos.push_back(start+xSideF*dir1+ySideF*dir2);
			vPos.push_back(start-xSideF*dir1+ySideF*dir2);
890
#else
891
892
893
			vPos.push_back(start-xSide-ySide);
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
Thomas Jund's avatar
Thomas Jund committed
894
			vPos.push_back(start-xSide+ySide);
895
896
#endif

897
898
		}
			break;
899
900
		case 1 :
		{
Arash HABIBI's avatar
planB    
Arash HABIBI committed
901
//			mm = new MovingMesh(envMap_, d, "./meshRessources/scenario3/limace_occ.obj");
902
903
904
			mm = new MovingMesh(envMap_, d, "./meshRessources/LimaceLvl1.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/cylinder.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/simpleCyl.obj");
905
			movingMeshes_.push_back(mm);
pitiot's avatar
pitiot committed
906
			vPos = mm->computeProjectedPointSet(maxHeight, d);
pitiot's avatar
pitiot committed
907
			std::reverse(vPos.begin(),vPos.end());
pitiot's avatar
pitiot committed
908
#ifndef TWO_AND_HALF_DIM
Thomas Jund's avatar
Thomas Jund committed
909
910
911
912
913
914
			//scale projected pointset
			VEC3 bary(0);
			for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
				bary += *it;
			bary /= vPos.size();

pitiot's avatar
pitiot committed
915
			float scale =1.2f;
Thomas Jund's avatar
Thomas Jund committed
916
917
			for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
				*it = *it*scale - bary*(scale-1.0f);
pitiot's avatar
pitiot committed
918
#endif
919
		}
pitiot's avatar
pitiot committed
920

921
			break;
922
923
924
925
926
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

927
	bool orientAccordingToFace=false;
Thomas Jund's avatar
Thomas Jund committed
928
	if(mm!=NULL && orientAccordingToFace)
Thomas Jund's avatar
Thomas Jund committed
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
	{
		while(envMap_.pedWayMark.isMarked(envMap_.map.phi2(d)))
		{
			d = envMap_.map.phi1(d);
		}

		VEC3 mid = (envMap_.position[d]+envMap_.position[envMap_.map.phi1(d)])*0.5f;
		VEC3 front = vPos[0];

		VEC3 bary(0);
		for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
			bary += *it;

		bary /= vPos.size();

		float angle = get_angle(mid - bary,front  - bary);

Thomas Jund's avatar
Thomas Jund committed
946
		TraversorV<PFP2::MAP> tv(mm->map);
Thomas Jund's avatar
Thomas Jund committed
947
948
		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
		{
pitiot's avatar
pitiot committed
949
			mm->position[d] += rotate2D(mm->position[d], bary, angle);
Thomas Jund's avatar
Thomas Jund committed
950
		}
Thomas Jund's avatar
Thomas Jund committed
951

pitiot's avatar
pitiot committed
952
953
//		for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
//			*it += rotate2D(*it , bary, angle);
Thomas Jund's avatar
Thomas Jund committed
954
955
	}

956
957
	std::vector<VEC3> goals;
	goals.push_back(start);
Thomas Jund's avatar
merge    
Thomas Jund committed
958

pitiot's avatar
pitiot committed
959
960
961
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), (obstType==0), 0 , d);


962
963

	movingObstacles_.push_back(mo);
964

965
	if(mm != NULL)
966
	{
967
		mo->attachMesh(mm);
Thomas Jund's avatar
Thomas Jund committed
968
969
970
//		std::cout << "here you go" << std::endl;
//		addAgent(vPos[0], vPos[0], d);
//		mo->attachAgent(agents_.back());
971
	}
972
973
}

Thomas Jund's avatar
Thomas Jund committed
974
void Simulator::addPathToObstacles(CellMarker<FACE>& markForbid