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

Simulator::~Simulator()
{
David Cazier's avatar
David Cazier committed
47
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
48
		delete agents_[i] ;
Thomas's avatar
Thomas committed
49
50
}

51
void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst, bool enablePathFinding)
Thomas's avatar
Thomas committed
52
{
53
	std::cout << "Setup scenario" << std::endl ;
Thomas's avatar
Thomas committed
54

David Cazier's avatar
David Cazier committed
55
56
	switch (config)
	{
David Cazier's avatar
David Cazier committed
57
		case 0 :
58
			setupCircleScenario(nbAgent,nbObst) ;
Thomas Jund's avatar
Thomas Jund committed
59
//			addPathsToAgents();
David Cazier's avatar
David Cazier committed
60
61
			break ;
		case 1 :
62
			setupCorridorScenario(nbAgent,nbObst) ;
David Cazier's avatar
David Cazier committed
63
64
			break ;
		case 2 :
65
			setupSnakeCorridorScenario(nbAgent,nbObst,10) ;
David Cazier's avatar
David Cazier committed
66
			break ;
Thomas Jund's avatar
Thomas Jund committed
67
		case 3 :
68
			envMap_.init(config, 1000.0f, 1000.0f, minSize, 100.0f) ; //grosses cases
Thomas Jund's avatar
Thomas Jund committed
69
			setupScenario(nbAgent, false) ;
70
			addMovingObstacles(nbObst, 1);
Thomas Jund's avatar
Thomas Jund committed
71
			addPathToObstacles(envMap_.buildingMark, true);
pitiot's avatar
pitiot committed
72
73
////			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
74
			break ;
pitiot's avatar
pitiot committed
75
76
77
//		case 4 :
//			importAgents("myAgents.pos") ;
//			break ;
Jund Thomas's avatar
Jund Thomas committed
78
		case 5:
79
			envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ; //svg import
Thomas Jund's avatar
Thomas Jund committed
80
			setupScenario(nbAgent, true) ;
81
//			SelectorCellNotMarked<PFP::MAP> scnm(envMap_.pedWayMark);
82
			addMovingObstacles(nbObst, 1);
Thomas Jund's avatar
Thomas Jund committed
83
//			addPathToObstacles(envMap_.pedWayMark, false);
Thomas Jund's avatar
Thomas Jund committed
84
			addPathsToAgents();
Jund Thomas's avatar
Jund Thomas committed
85
			break;
86
87
88
89
		case 6:
#ifdef TWO_AND_HALF_DIM
			envMap_.init(config,200.0,200.0, minSize, 400.0f);
			setupPlanetScenario(nbAgent,nbObst);
pitiot's avatar
pitiot committed
90
			addMovingObstacles(nbObst, 1);
Thomas Jund's avatar
Thomas Jund committed
91
			addPathToObstacles(envMap_.buildingMark, true);
pitiot's avatar
pitiot committed
92
			addPathsToAgents();
93
94
95
96
#else
			std::cout << "Agents not in 2.5D mode" << std::endl;
#endif
			break;
David Cazier's avatar
David Cazier committed
97
98
99
		default:
			std::cout << "Unknown scenario !" << std::endl ;
			exit(1) ;
Thomas's avatar
Thomas committed
100
101
	}

David Cazier's avatar
David Cazier committed
102
103
104
105
106
#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
107
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
108
109
110
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
David Cazier's avatar
David Cazier committed
111
			addPathsToAgents() ;
112
113
		else if (dimension == 2.5f)
			addPathsToAgents_height() ;
Thomas's avatar
Thomas committed
114
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
115
#endif
116

David Cazier's avatar
David Cazier committed
117
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
118
	{
David Cazier's avatar
David Cazier committed
119
120
		agents_[i]->updateObstacleNeighbors() ;
		agents_[i]->updateAgentNeighbors() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
121
	}
122

Pierre Kraemer's avatar
Pierre Kraemer committed
123
#ifndef SPATIAL_HASHING
124
125
	if (multires)
		envMap_.subdivideToProperLevel() ;
David Cazier's avatar
David Cazier committed
126
	//	envMap_.subdivideAllToMaxLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
127
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
128
129
}

David Cazier's avatar
David Cazier committed
130
void Simulator::doStep()
Pierre Kraemer's avatar
Pierre Kraemer committed
131
{
David Cazier's avatar
David Cazier committed
132
133
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
Jund Thomas's avatar
Jund Thomas committed
134
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
David Cazier's avatar
David Cazier committed
135
#endif
pitiot's avatar
stats    
pitiot committed
136
137
	struct timespec begTime, endTime ;

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

pitiot's avatar
pitiot committed
140
	{
pitiot's avatar
up    
pitiot committed
141
//		unsigned int i =12;
pitiot's avatar
stats    
pitiot committed
142
		clock_gettime(CLOCK_MONOTONIC, &begTime) ;
David Cazier's avatar
David Cazier committed
143
		movingObstacles_[i]->computePrefVelocity() ;
pitiot's avatar
pitiot committed
144
		movingObstacles_[i]->computeNewVelocity() ;
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
145
		movingObstacles_[i]->initForces();
pitiot's avatar
stats    
pitiot committed
146
147
		clock_gettime(CLOCK_MONOTONIC, &endTime) ;
		time_behave+= timespec_delta(begTime,endTime).tv_nsec;
pitiot's avatar
up    
pitiot committed
148
149
150
151
152
153
154
//		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
155

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
156
	}
157

pitiot's avatar
merge    
pitiot committed
158
	clock_gettime(CLOCK_MONOTONIC, &begTime) ;
159
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
pitiot's avatar
merge    
pitiot committed
160
	{
pitiot's avatar
up    
pitiot committed
161
//		unsigned int i =12;
Thomas Jund's avatar
Thomas Jund committed
162
		movingObstacles_[i]->updateAgentNeighbors();
163
		movingObstacles_[i]->updateObstacleNeighbors();
164
165
		movingObstacles_[i]->updateForces() ;

pitiot's avatar
merge    
pitiot committed
166
167
168
	}
	clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	time_obstacle+= timespec_delta(begTime,endTime).tv_nsec;
pitiot's avatar
up    
pitiot committed
169
//	CGoGNout<<"deplacement obstacles"<<CGoGNendl;
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
170
171
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
	{
pitiot's avatar
up    
pitiot committed
172
//		unsigned int i =12;
pitiot's avatar
merge    
pitiot committed
173
		clock_gettime(CLOCK_MONOTONIC, &begTime) ;
174
		movingObstacles_[i]->applyForces();
pitiot's avatar
pitiot committed
175
		movingObstacles_[i]->updateRegistration();
pitiot's avatar
stats    
pitiot committed
176
177
		clock_gettime(CLOCK_MONOTONIC, &endTime) ;
		time_obstacle+= timespec_delta(begTime,endTime).tv_nsec;
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
178
		movingObstacles_[i]->updateMesh() ;
pitiot's avatar
pitiot committed
179
	}
pitiot's avatar
pitiot committed
180
181
182



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

David Cazier's avatar
David Cazier committed
185
186
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
pitiot's avatar
pitiot committed
187
188
		if (agents_[i]->alive)
		{
pitiot's avatar
stats    
pitiot committed
189
			clock_gettime(CLOCK_MONOTONIC, &begTime) ;
pitiot's avatar
pitiot committed
190
			agents_[i]->updateObstacleNeighbors() ;
pitiot's avatar
maj    
pitiot committed
191
192

#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
193
			envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
194
#endif
pitiot's avatar
pitiot committed
195
			agents_[i]->updateAgentNeighbors() ;
pitiot's avatar
maj    
pitiot committed
196
#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
197
			envMap_.map.setCurrentLevel(0) ;
pitiot's avatar
maj    
pitiot committed
198
#endif
pitiot's avatar
stats    
pitiot committed
199
200
201
			clock_gettime(CLOCK_MONOTONIC, &endTime) ;
			time_agent+=timespec_delta(begTime,endTime).tv_nsec;
			clock_gettime(CLOCK_MONOTONIC, &begTime) ;
pitiot's avatar
pitiot committed
202
203
			agents_[i]->computePrefVelocity() ;
			agents_[i]->computeNewVelocity() ;
pitiot's avatar
stats    
pitiot committed
204
205
			clock_gettime(CLOCK_MONOTONIC, &endTime) ;
			time_behave+=timespec_delta(begTime,endTime).tv_nsec;
pitiot's avatar
pitiot committed
206
		}
David Cazier's avatar
David Cazier committed
207
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
208

Pierre Kraemer's avatar
Pierre Kraemer committed
209
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
210
211
212
213
214
215
216
217
218
219
220
	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
221
#else
Thomas Jund's avatar
Thomas Jund committed
222

223
224
	if (multires)
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
Thomas Jund's avatar
Thomas Jund committed
225

David Cazier's avatar
David Cazier committed
226
	nb_dead = 0 ;
pitiot's avatar
stats    
pitiot committed
227
	clock_gettime(CLOCK_MONOTONIC, &begTime) ;
David Cazier's avatar
David Cazier committed
228
229
230
231
232
233
	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
234

235
			if (agents_[i]->part_.crossCell != CGoGN::Algo::Surface::MovingObjects::NO_CROSS)
David Cazier's avatar
David Cazier committed
236
237
238
239
240
241
				envMap_.agentChangeFace(agents_[i], oldFace) ;
		}
		else
			nb_dead++ ;

	}
pitiot's avatar
stats    
pitiot committed
242
243
	clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	time_agent+= timespec_delta(begTime,endTime).tv_nsec;
David Cazier's avatar
David Cazier committed
244
245
246

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

Thomas Jund's avatar
Thomas Jund committed
248
	 if (multires)
pitiot's avatar
stats    
pitiot committed
249
250
	 {
		 clock_gettime(CLOCK_MONOTONIC, &begTime) ;
Thomas Jund's avatar
Thomas Jund committed
251
		 envMap_.updateMap() ;
pitiot's avatar
stats    
pitiot committed
252
253
254
		 clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	 	 time_multires+= timespec_delta(begTime,endTime).tv_nsec;
	 }
David Cazier's avatar
David Cazier committed
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
#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
271
void Simulator::addAgent(const VEC3& start, const VEC3& goal)
David Cazier's avatar
David Cazier committed
272
{
David Cazier's avatar
David Cazier committed
273
	agents_.push_back(new Agent(this, start, goal)) ;
David Cazier's avatar
David Cazier committed
274
275
}

276
277
278
279
280
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
281
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
David Cazier's avatar
David Cazier committed
282
{
pitiot's avatar
merging    
pitiot committed
283
284
285
	if (multires)
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
	else
pitiot's avatar
pitiot committed
286

pitiot's avatar
merging    
pitiot committed
287
288
	{

pitiot's avatar
stats    
pitiot committed
289
290
//		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
291
292
293
	}


294
	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles"<< std::endl ;
David Cazier's avatar
David Cazier committed
295
296
297
298
299
300
301
302
303
304
305

	// 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
306
//	int radius = nbAgents/(Agent::radius_*Agent::radius_);
Thomas Jund's avatar
Thomas Jund committed
307
	std::cout << "radius " << radius << std::endl;
David Cazier's avatar
David Cazier committed
308
309
310
311
	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
312
		double angle = i * 2.0f * M_PI / float(nbAgents) ;
David Cazier's avatar
David Cazier committed
313
314
315
		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
316
		addAgent(start, goal) ;
pitiot's avatar
pitiot committed
317
//		CGoGNout<<"agent n°" << agents_[i]->agentNo << " : "<< agents_[i]<<CGoGNendl;
318
	}
pitiot's avatar
pitiot committed
319
320
321
322
323
	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
324
	int curGoal=0;
Thomas Jund's avatar
merge    
Thomas Jund committed
325

pitiot's avatar
pitiot committed
326
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
David Cazier's avatar
David Cazier committed
327
	{
Thomas Jund's avatar
merge    
Thomas Jund committed
328
329
		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
330
331
332
333
334
335
336
		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
337
			v=VEC3 (std::cos(angle) * (radius/2), std::sin(angle) * (radius/2), 0) ;
pitiot's avatar
pitiot committed
338
339
340
		}
		start = center + v ;
		positions.push_back(start);
pitiot's avatar
pitiot committed
341
	}
Thomas Jund's avatar
merge    
Thomas Jund committed
342

pitiot's avatar
pitiot committed
343
344
345
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
346
347

		unsigned int nbGoals=360;
pitiot's avatar
pitiot committed
348
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
349
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
350
		{
Thomas Jund's avatar
Thomas Jund committed
351
			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
352
353
354
		}

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

pitiot's avatar
pitiot committed
356
357
358
359
		vPos.push_back(start+xSide-ySide);
		vPos.push_back(start+xSide+ySide);
		vPos.push_back(start-xSide+ySide);
		vPos.push_back(start-xSide-ySide);
pitiot's avatar
pitiot committed
360
		curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+1)%nbGoals;
361
		mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);
pitiot's avatar
pitiot committed
362

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

pitiot's avatar
pitiot committed
366
367
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
368

pitiot's avatar
pitiot committed
369
370
371
	for (unsigned int i = nbObstacles/2 ; i < nbObstacles ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
372
373
374
375
376
377
378
//		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
379
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
380
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
381
		{
Thomas Jund's avatar
Thomas Jund committed
382
			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
383
384
385
386
387
388
389
390
		}

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		vPos.push_back(start+xSide-ySide);
		vPos.push_back(start+xSide+ySide);
		vPos.push_back(start-xSide+ySide);
		vPos.push_back(start-xSide-ySide);
391
392
		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
393

394
395
//		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
396
397

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
398
	}
Thomas's avatar
Thomas committed
399

400
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
401
402
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
403
#endif
pitiot's avatar
merging    
pitiot committed
404
405

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

David Cazier's avatar
David Cazier committed
408
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
409
{
pitiot's avatar
pitiot committed
410
411
	VEC3 xSide (5.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,10.0f,0.0f);
pitiot's avatar
merging    
pitiot committed
412
413
	if (multires)
	{
pitiot's avatar
pitiot committed
414
//		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
pitiot's avatar
up    
pitiot committed
415
		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
416
417
418
419
	}
	else
	{

pitiot's avatar
stats    
pitiot committed
420
421
//		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
		envMap_.init(config, 1600.0f, 960.0f, 40.0f, 40.0f) ; //cases fines
pitiot's avatar
merging    
pitiot committed
422
423
424
	}


David Cazier's avatar
David Cazier committed
425
426
427
428
429
430
431
432
433
434
	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
435
	int xStartDelta = envMap_.geometry.size(0) / 10 ;
David Cazier's avatar
David Cazier committed
436
437
438
439
	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
440
	int xGoalDelta = envMap_.geometry.size(0) / 10 ;
David Cazier's avatar
David Cazier committed
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
	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
458
459
	xStartMin = envMap_.geometry.min()[0] + envMap_.geometry.size(0) / 5 ;
	xStartDelta = 6*envMap_.geometry.size(0) / 10 ;
David Cazier's avatar
David Cazier committed
460
461
	yStartMin = envMap_.geometry.min()[1] + yBorder ;
	yStartDelta = envMap_.geometry.size(1) / 5 ;
pitiot's avatar
pitiot committed
462

David Cazier's avatar
David Cazier committed
463
464
465
466
	// 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
467

David Cazier's avatar
David Cazier committed
468
469
470
471
472
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
pitiot's avatar
pitiot committed
473
		float x = xStartMin + ((int)(i*2*xSide[0])) % xStartDelta;
David Cazier's avatar
David Cazier committed
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
//		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(x, yGoalMin  + rand() % yGoalDelta,  0) ;

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		VEC3 tmp ;
		if (i % 2 == 1)
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
		}
Thomas Jund's avatar
Thomas Jund committed
491
492
		else
		{
David Cazier's avatar
David Cazier committed
493
494
495
496
497
498
499
500
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
		}
		std::vector<VEC3> goals;
		goals.push_back(start);
		goals.push_back(goal);
Thomas Jund's avatar
merge    
Thomas Jund committed
501

pitiot's avatar
pitiot committed
502
		mo4= new MovingObstacle(this, i,vPos,goals, true, false,1);
David Cazier's avatar
David Cazier committed
503
504


505
506
507
508
509
510
511
512
513
514
515
516
517
518
//		//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
519
520
521
522
523

		movingObstacles_.push_back(mo4);
	}
}

pitiot's avatar
merging    
pitiot committed
524
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
David Cazier's avatar
David Cazier committed
525
{
pitiot's avatar
merging    
pitiot committed
526
527
528
529
530
531
	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
532
533

	// Bordure à éviter autour de la scène (10% de sa taille)
pitiot's avatar
merging    
pitiot committed
534
535
	int xBorder = envMap_.geometry.size(0) / 5 ;
	int yBorder = envMap_.geometry.size(1) / 5 ;
David Cazier's avatar
David Cazier committed
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551

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

David Cazier's avatar
David Cazier committed
556
		// Un agent sur 2 va de droite à gauche
pitiot's avatar
merging    
pitiot committed
557
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
558
		if (i % 2 == 1)
pitiot's avatar
merging    
pitiot committed
559
560
561
562
563
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
		}
pitiot's avatar
merge    
pitiot committed
564

pitiot's avatar
merging    
pitiot committed
565
		addAgent(start, goal) ;
pitiot's avatar
merge    
pitiot committed
566

Thomas's avatar
Thomas committed
567
568
	}

David Cazier's avatar
David Cazier committed
569
570
571
572
	// 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
573
	yStartDelta = envMap_.geometry.size(1) / 10 ;
David Cazier's avatar
David Cazier committed
574
575

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

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

580
	VEC3 xSide (5.0f,0.0f,0.0f);
581
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
582

Thomas Jund's avatar
Thomas Jund committed
583
	std::cout << "geom : " << envMap_.geometry << std::endl;
pitiot's avatar
merging    
pitiot committed
584
585
586

	int sumObstacles=0;
	for(unsigned int j = 0; j<nbSnakes; j++)
David Cazier's avatar
David Cazier committed
587
	{
pitiot's avatar
merging    
pitiot committed
588
589
590
591
		std::vector<PFP::VEC3> positions [snakeSize] ;
		float x = xStartMin + rand() % xStartDelta;
		VEC3 goal;

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

pitiot's avatar
merging    
pitiot committed
594
595
596
597
598
599
600
601
602
603
604
605

		std::vector<VEC3> vPos;


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

		std::vector<VEC3> goals;
		goals.push_back(start);
		int r=0;
Thomas Jund's avatar
Thomas Jund committed
606
		while (r<80)
David Cazier's avatar
David Cazier committed
607
		{
pitiot's avatar
merging    
pitiot committed
608
609
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
Thomas Jund's avatar
Thomas Jund committed
610
611
612


			if ((r==0 || angle(goal-goals[r],goals[r]-goals[r-1])<M_PI/2.0f) )
613
			{
Thomas Jund's avatar
Thomas Jund committed
614
615
616

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

pitiot's avatar
merging    
pitiot committed
617
				goals.push_back(goal);
618
				r++;
pitiot's avatar
merging    
pitiot committed
619
620
			}

David Cazier's avatar
David Cazier committed
621
		}
pitiot's avatar
merging    
pitiot committed
622
623
624
625
626
627
628
629
630
631

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

David Cazier's avatar
David Cazier committed
632
633
634
635
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
pitiot's avatar
merging    
pitiot committed
636
			positions[i]=vPos;
David Cazier's avatar
David Cazier committed
637
		}
pitiot's avatar
merging    
pitiot committed
638
639
640
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
641

pitiot's avatar
merging    
pitiot committed
642
643
		for (int i = 0 ; i < snakeSize ; i++)
			movingObstacles_.push_back(art->members[i]);
David Cazier's avatar
David Cazier committed
644
	}
Thomas's avatar
Thomas committed
645
646
}

David Cazier's avatar
David Cazier committed
647
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
648
{
David Cazier's avatar
David Cazier committed
649
650
651
652
653
	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
654

David Cazier's avatar
David Cazier committed
655
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
656
	{
David Cazier's avatar
David Cazier committed
657
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
658
		{
659
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
David Cazier's avatar
David Cazier committed
660
			addAgent(posagent, -1.0f * posagent) ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
661
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
662
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
663
664
}

Thomas Jund's avatar
Thomas Jund committed
665
void Simulator::setupScenario(unsigned int nbMaxAgent, bool pedWay)
Pierre Kraemer's avatar
Pierre Kraemer committed
666
667
{
	/*
David Cazier's avatar
David Cazier committed
668
	 * Add agents, specifying their start position, and store their goal on the
David Cazier's avatar
David Cazier committed
669
670
	 * opposite side of the environment.
	 */
pitiot's avatar
pitiot committed
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
	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);
	}


686
	TraversorF<PFP::MAP> tF(envMap_.map);
pitiot's avatar
pitiot committed
687
	d = tF.begin() ;
David Cazier's avatar
David Cazier committed
688

pitiot's avatar
pitiot committed
689
	unsigned int nbx =  nbMaxAgent==0 ? 1 :  (nbMaxAgent / (nb_cells))/5;
pitiot's avatar
pitiot committed
690
	unsigned int nby = 5;
David Cazier's avatar
David Cazier committed
691
692
693

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

David Cazier's avatar
David Cazier committed
694
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
695
	{
Thomas Jund's avatar
Thomas Jund committed
696
		VEC3 pos;
David Cazier's avatar
David Cazier committed
697
698
		bool found = false ;
		Dart dCell ;
699
		while (!found && d != tF.end())
David Cazier's avatar
David Cazier committed
700
		{
701
			if (!envMap_.buildingMark.isMarked(d)
Thomas Jund's avatar
Thomas Jund committed
702
			    && (!pedWay || envMap_.pedWayMark.isMarked(d))
703
			    )
David Cazier's avatar
David Cazier committed
704
			{
705
				pos = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
706
707
708
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
709
			}
710
			d = tF.next() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
711
712
		}

David Cazier's avatar
David Cazier committed
713
714
		if (found)
		{
Thomas Jund's avatar
Thomas Jund committed
715
			float ecart = 2.0f*Agent::radius_;
716
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
717
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
David Cazier's avatar
David Cazier committed
718
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
719
			{
David Cazier's avatar
David Cazier committed
720
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
721
				{
722
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
723
					addAgent(posagent, -1.0f * posagent, dCell) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
724
725
726
727
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
728
729
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
730
}
731

732
733
734
735
736
737
738
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
739
740
741
742
743
744
745
746
747
748
749
750
751
	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();
752
753
	CellMarker<FACE> filled(envMap_.map);

pitiot's avatar
pitiot committed
754
	unsigned int nbx =1;
pitiot's avatar
pitiot committed
755
	unsigned int nby = 1;
756

pitiot's avatar
pitiot committed
757
	unsigned int bMax = nbx * nby > 0 ? nbAgents / (nbx * nby) : nbAgents ;
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777

	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
778
			float ecart = 4.0f;
pitiot's avatar
pitiot committed
779
			VEC3 displ2(- (float(nbx)/2.0f * ecart), - (float(nby)/2.0f * ecart), 0);
780
			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, dCell, envMap_.position), 0.0f);
pitiot's avatar
pitiot committed
781
782
			Agent::rotate(Agent::xyPlane, pl.normal(), displ2);
			VEC3 posinit = pos + displ2;
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
			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();
}

805
void Simulator::addMovingObstacles(unsigned int nb, unsigned int type)
806
807
808
809
810
811
812
813
814
815
816
817
818
{
	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)
				)
			{
819
				float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
820
821
822
823
824
				if(area>1400)
				{
					dCell = d ;
					found = true ;
				}
825
826
827
828
829
			}
			d = tF.next() ;
		}

		if (found)
pitiot's avatar
pitiot committed
830
		{
831
			addMovingObstacle(dCell,type) ;
pitiot's avatar
pitiot committed
832
833
//			CGoGNout<<" dCell initiale :"<< d <<CGoGNendl;
		}
834
835
836
837
838
839
840
	}
}

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

843
844
	MovingMesh* mm = NULL;

845
846
847
848
	switch(obstType)
	{
		case 0 :
		{
849
			start = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
850
851
852
			VEC3 xSide (2.0f,0.0f,0.0f);
			VEC3 ySide (0.0f,5.0f,0.0f);

853
#ifdef TWO_AND_HALF_DIM
pitiot's avatar
up    
pitiot committed
854
855
			float xSideF=2.0f;
			float ySideF=5.0f;
856
			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::facePlane<PFP>(envMap_.map, d, envMap_.position));
pitiot's avatar
up    
pitiot committed
857
858
			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]);
859

pitiot's avatar
up    
pitiot committed
860
861
			dir1.normalize();
			VEC3 dir2 = norm ^dir1;
862

pitiot's avatar
up    
pitiot committed
863
864
865
866
			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);
867
#else
868
869
870
			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
871
			vPos.push_back(start-xSide+ySide);
872
873
#endif

874
875
		}
			break;
876
877
		case 1 :
		{
878
879
880
			mm = new MovingMesh(envMap_, d, "./meshRessources/LimaceLvl1.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/cylinder.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/simpleCyl.obj");
881
			movingMeshes_.push_back(mm);
pitiot's avatar
pitiot committed
882
			vPos = mm->computeProjectedPointSet(maxHeight, d);
pitiot's avatar
pitiot committed
883
			std::reverse(vPos.begin(),vPos.end());
pitiot's avatar
pitiot committed
884
#ifndef TWO_AND_HALF_DIM
Thomas Jund's avatar
Thomas Jund committed
885
886
887
888
889
890
			//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
891
			float scale =1.2f;
Thomas Jund's avatar
Thomas Jund committed
892
893
			for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
				*it = *it*scale - bary*(scale-1.0f);
pitiot's avatar
pitiot committed
894
#endif
895
		}
pitiot's avatar
pitiot committed
896

897
			break;
898
899
900
901
902
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

903
	bool orientAccordingToFace=false;
Thomas Jund's avatar
Thomas Jund committed
904
	if(mm!=NULL && orientAccordingToFace)
Thomas Jund's avatar
Thomas Jund committed
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
	{
		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
922
		TraversorV<PFP2::MAP> tv(mm->map);
Thomas Jund's avatar
Thomas Jund committed
923
924
		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
		{
pitiot's avatar
pitiot committed
925
			mm->position[d] += rotate2D(mm->position[d], bary, angle);
Thomas Jund's avatar
Thomas Jund committed
926
		}
Thomas Jund's avatar
Thomas Jund committed
927

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

932
933
	std::vector<VEC3> goals;
	goals.push_back(start);
Thomas Jund's avatar
merge    
Thomas Jund committed
934

pitiot's avatar
pitiot committed
935
936
937
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), (obstType==0), 0 , d);


938
939

	movingObstacles_.push_back(mo);
940

941
	if(mm != NULL)
942
	{
943
		mo->attachMesh(mm);
Thomas Jund's avatar
Thomas Jund committed
944
945
946
//		std::cout << "here you go" << std::endl;
//		addAgent(vPos[0], vPos[0], d);
//		mo->attachAgent(agents_.back());
947
	}
948
949
}

Thomas Jund's avatar
Thomas Jund committed
950
void Simulator::addPathToObstacles(CellMarker<FACE>& markForbid, bool goalCenterCell)
951
952
953
954
955
{
	unsigned int dartDistForPath = 50 ;

	for (std::vector<MovingObstacle*>::iterator it = movingObstacles_.begin() ; it != movingObstacles_.end() ; ++it)
	{
Thomas Jund's avatar
Thomas Jund committed
956
957
		(*it)->goals_.clear();

pitiot's avatar
pitiot committed
958
		Dart dStart = (*it)->parts_[0]->d;
959

Thomas Jund's avatar
Thomas Jund committed
960
		while(markForbid.isMarked(dStart))
961
		{
Thomas Jund's avatar
Thomas Jund committed
962
			if(!markForbid.isMarked(envMap_.map.phi2(dStart)))
963
964
965
966
967
				dStart = envMap_.map.phi2(dStart);
			else
				dStart = envMap_.map.phi1(dStart);
		}

968
969
970
971
972
		Dart dStop = dStart;

		for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop)
			|| j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStart)
Thomas Jund's avatar
Thomas Jund committed
973
			|| markForbid.isMarked(dStop)
974
975
976
977
978
979
980
			; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
		}

Thomas Jund's avatar
Thomas Jund committed
981
		addPathToObstacle(*it, dStart, dStop, markForbid, goalCenterCell);
982
983
984
	}
}

Thomas Jund's avatar
Thomas Jund committed
985