simulator.cpp 35 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) :
Thomas Jund's avatar
Thomas Jund committed
19
	timeStep_(0.05f),
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
70
			setupScenario(nbAgent, false) ;
			addMovingObstacles(nbObst);
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);
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
90
		case 6:
#ifdef TWO_AND_HALF_DIM
			envMap_.init(config,200.0,200.0, minSize, 400.0f);
			setupPlanetScenario(nbAgent,nbObst);
			addMovingObstacles(nbObst);
Thomas Jund's avatar
Thomas Jund committed
91
			addPathToObstacles(envMap_.buildingMark, true);
92
93
94
95
#else
			std::cout << "Agents not in 2.5D mode" << std::endl;
#endif
			break;
David Cazier's avatar
David Cazier committed
96
97
98
		default:
			std::cout << "Unknown scenario !" << std::endl ;
			exit(1) ;
Thomas's avatar
Thomas committed
99
100
	}

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

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

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

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

David Cazier's avatar
David Cazier committed
137
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
pitiot's avatar
pitiot committed
138
	{
pitiot's avatar
stats    
pitiot committed
139
		clock_gettime(CLOCK_MONOTONIC, &begTime) ;
David Cazier's avatar
David Cazier committed
140
		movingObstacles_[i]->computePrefVelocity() ;
pitiot's avatar
pitiot committed
141
		movingObstacles_[i]->computeNewVelocity() ;
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
142
		movingObstacles_[i]->initForces();
pitiot's avatar
stats    
pitiot committed
143
144
		clock_gettime(CLOCK_MONOTONIC, &endTime) ;
		time_behave+= timespec_delta(begTime,endTime).tv_nsec;
pitiot's avatar
merge    
pitiot committed
145
146


Arash HABIBI's avatar
unidir    
Arash HABIBI committed
147
	}
148

pitiot's avatar
merge    
pitiot committed
149
	clock_gettime(CLOCK_MONOTONIC, &begTime) ;
150
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
pitiot's avatar
merge    
pitiot committed
151
	{
Thomas Jund's avatar
Thomas Jund committed
152
		movingObstacles_[i]->updateAgentNeighbors();
153
		movingObstacles_[i]->updateObstacleNeighbors();
154
155
		movingObstacles_[i]->updateForces() ;

pitiot's avatar
merge    
pitiot committed
156
157
158
159
	}
	clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	time_obstacle+= timespec_delta(begTime,endTime).tv_nsec;

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
160
161
	for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
	{
pitiot's avatar
merge    
pitiot committed
162
		clock_gettime(CLOCK_MONOTONIC, &begTime) ;
163
		movingObstacles_[i]->applyForces();
pitiot's avatar
stats    
pitiot committed
164
165
		clock_gettime(CLOCK_MONOTONIC, &endTime) ;
		time_obstacle+= timespec_delta(begTime,endTime).tv_nsec;
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
166
		movingObstacles_[i]->updateMesh() ;
pitiot's avatar
pitiot committed
167
	}
pitiot's avatar
stats    
pitiot committed
168
169


David Cazier's avatar
David Cazier committed
170
171
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
pitiot's avatar
pitiot committed
172
173
		if (agents_[i]->alive)
		{
pitiot's avatar
stats    
pitiot committed
174
			clock_gettime(CLOCK_MONOTONIC, &begTime) ;
pitiot's avatar
pitiot committed
175
			agents_[i]->updateObstacleNeighbors() ;
pitiot's avatar
maj    
pitiot committed
176
177

#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
178
			envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
179
#endif
pitiot's avatar
pitiot committed
180
			agents_[i]->updateAgentNeighbors() ;
pitiot's avatar
maj    
pitiot committed
181
#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
182
			envMap_.map.setCurrentLevel(0) ;
pitiot's avatar
maj    
pitiot committed
183
#endif
pitiot's avatar
stats    
pitiot committed
184
185
186
			clock_gettime(CLOCK_MONOTONIC, &endTime) ;
			time_agent+=timespec_delta(begTime,endTime).tv_nsec;
			clock_gettime(CLOCK_MONOTONIC, &begTime) ;
pitiot's avatar
pitiot committed
187
188
			agents_[i]->computePrefVelocity() ;
			agents_[i]->computeNewVelocity() ;
pitiot's avatar
stats    
pitiot committed
189
190
			clock_gettime(CLOCK_MONOTONIC, &endTime) ;
			time_behave+=timespec_delta(begTime,endTime).tv_nsec;
pitiot's avatar
pitiot committed
191
		}
David Cazier's avatar
David Cazier committed
192
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
193

Pierre Kraemer's avatar
Pierre Kraemer committed
194
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
195
196
197
198
199
200
201
202
203
204
205
	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
206
#else
Thomas Jund's avatar
Thomas Jund committed
207

208
209
	if (multires)
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
Thomas Jund's avatar
Thomas Jund committed
210

David Cazier's avatar
David Cazier committed
211
	nb_dead = 0 ;
pitiot's avatar
stats    
pitiot committed
212
	clock_gettime(CLOCK_MONOTONIC, &begTime) ;
David Cazier's avatar
David Cazier committed
213
214
215
216
217
218
	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
219

220
			if (agents_[i]->part_.crossCell != CGoGN::Algo::Surface::MovingObjects::NO_CROSS)
David Cazier's avatar
David Cazier committed
221
222
223
224
225
226
				envMap_.agentChangeFace(agents_[i], oldFace) ;
		}
		else
			nb_dead++ ;

	}
pitiot's avatar
stats    
pitiot committed
227
228
	clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	time_agent+= timespec_delta(begTime,endTime).tv_nsec;
David Cazier's avatar
David Cazier committed
229
230
231

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

Thomas Jund's avatar
Thomas Jund committed
233
	 if (multires)
pitiot's avatar
stats    
pitiot committed
234
235
	 {
		 clock_gettime(CLOCK_MONOTONIC, &begTime) ;
Thomas Jund's avatar
Thomas Jund committed
236
		 envMap_.updateMap() ;
pitiot's avatar
stats    
pitiot committed
237
238
239
		 clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	 	 time_multires+= timespec_delta(begTime,endTime).tv_nsec;
	 }
David Cazier's avatar
David Cazier committed
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
#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
256
void Simulator::addAgent(const VEC3& start, const VEC3& goal)
David Cazier's avatar
David Cazier committed
257
{
David Cazier's avatar
David Cazier committed
258
	agents_.push_back(new Agent(this, start, goal)) ;
David Cazier's avatar
David Cazier committed
259
260
}

261
262
263
264
265
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
266
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
David Cazier's avatar
David Cazier committed
267
{
pitiot's avatar
merging    
pitiot committed
268
269
270
	if (multires)
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
	else
pitiot's avatar
pitiot committed
271

pitiot's avatar
merging    
pitiot committed
272
273
	{

pitiot's avatar
stats    
pitiot committed
274
275
//		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
276
277
278
	}


279
	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles"<< std::endl ;
David Cazier's avatar
David Cazier committed
280
281
282
283
284
285
286
287
288
289
290

	// 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
291
//	int radius = nbAgents/(Agent::radius_*Agent::radius_);
Thomas Jund's avatar
Thomas Jund committed
292
	std::cout << "radius " << radius << std::endl;
David Cazier's avatar
David Cazier committed
293
294
295
296
	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
297
		double angle = i * 2.0f * M_PI / float(nbAgents) ;
David Cazier's avatar
David Cazier committed
298
299
300
		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
301
		addAgent(start, goal) ;
pitiot's avatar
pitiot committed
302
//		CGoGNout<<"agent n°" << agents_[i]->agentNo << " : "<< agents_[i]<<CGoGNendl;
303
	}
pitiot's avatar
pitiot committed
304
305
306
307
308
	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
309
	int curGoal=0;
Thomas Jund's avatar
merge    
Thomas Jund committed
310

pitiot's avatar
pitiot committed
311
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
David Cazier's avatar
David Cazier committed
312
	{
Thomas Jund's avatar
merge    
Thomas Jund committed
313
314
		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
315
316
317
318
319
320
321
		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
322
			v=VEC3 (std::cos(angle) * (radius/2), std::sin(angle) * (radius/2), 0) ;
pitiot's avatar
pitiot committed
323
324
325
		}
		start = center + v ;
		positions.push_back(start);
pitiot's avatar
pitiot committed
326
	}
Thomas Jund's avatar
merge    
Thomas Jund committed
327

pitiot's avatar
pitiot committed
328
329
330
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
331
332

		unsigned int nbGoals=360;
pitiot's avatar
pitiot committed
333
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
334
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
335
		{
Thomas Jund's avatar
Thomas Jund committed
336
			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
337
338
339
		}

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

pitiot's avatar
pitiot committed
341
342
343
344
		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
345
		curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+1)%nbGoals;
346
		mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);
pitiot's avatar
pitiot committed
347

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

pitiot's avatar
pitiot committed
351
352
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
353

pitiot's avatar
pitiot committed
354
355
356
	for (unsigned int i = nbObstacles/2 ; i < nbObstacles ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
357
358
359
360
361
362
363
//		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
364
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
365
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
366
		{
Thomas Jund's avatar
Thomas Jund committed
367
			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
368
369
370
371
372
373
374
375
		}

		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);
376
377
		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
378

379
380
//		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
381
382

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
383
	}
Thomas's avatar
Thomas committed
384

385
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
386
387
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
388
#endif
pitiot's avatar
merging    
pitiot committed
389
390

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

David Cazier's avatar
David Cazier committed
393
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
394
{
pitiot's avatar
pitiot committed
395
396
	VEC3 xSide (5.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,10.0f,0.0f);
pitiot's avatar
merging    
pitiot committed
397
398
	if (multires)
	{
pitiot's avatar
pitiot committed
399
400
//		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
		envMap_.init(config, nbObstacles > 10 ? ((nbObstacles*20*xSide[0])/6) : 500.0f , 960.0f, minSize, 320.0f) ; //grosses cases
pitiot's avatar
merging    
pitiot committed
401
402
403
404
	}
	else
	{

pitiot's avatar
stats    
pitiot committed
405
406
//		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
407
408
409
	}


David Cazier's avatar
David Cazier committed
410
411
412
413
414
415
416
417
418
419
	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
420
	int xStartDelta = envMap_.geometry.size(0) / 10 ;
David Cazier's avatar
David Cazier committed
421
422
423
424
	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
425
	int xGoalDelta = envMap_.geometry.size(0) / 10 ;
David Cazier's avatar
David Cazier committed
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
	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
443
444
	xStartMin = envMap_.geometry.min()[0] + envMap_.geometry.size(0) / 5 ;
	xStartDelta = 6*envMap_.geometry.size(0) / 10 ;
David Cazier's avatar
David Cazier committed
445
446
	yStartMin = envMap_.geometry.min()[1] + yBorder ;
	yStartDelta = envMap_.geometry.size(1) / 5 ;
pitiot's avatar
pitiot committed
447

David Cazier's avatar
David Cazier committed
448
449
450
451
	// 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
452

David Cazier's avatar
David Cazier committed
453
454
455
456
457
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
pitiot's avatar
pitiot committed
458
		float x = xStartMin + ((int)(i*2*xSide[0])) % xStartDelta;
David Cazier's avatar
David Cazier committed
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
//		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
476
477
		else
		{
David Cazier's avatar
David Cazier committed
478
479
480
481
482
483
484
485
			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
486

pitiot's avatar
pitiot committed
487
		mo4= new MovingObstacle(this, i,vPos,goals, true, false,1);
David Cazier's avatar
David Cazier committed
488
489


490
491
492
493
494
495
496
497
498
499
500
501
502
503
//		//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
504
505
506
507
508

		movingObstacles_.push_back(mo4);
	}
}

pitiot's avatar
merging    
pitiot committed
509
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
David Cazier's avatar
David Cazier committed
510
{
pitiot's avatar
merging    
pitiot committed
511
512
513
514
515
516
	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
517
518

	// Bordure à éviter autour de la scène (10% de sa taille)
pitiot's avatar
merging    
pitiot committed
519
520
	int xBorder = envMap_.geometry.size(0) / 5 ;
	int yBorder = envMap_.geometry.size(1) / 5 ;
David Cazier's avatar
David Cazier committed
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536

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

David Cazier's avatar
David Cazier committed
541
		// Un agent sur 2 va de droite à gauche
pitiot's avatar
merging    
pitiot committed
542
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
543
		if (i % 2 == 1)
pitiot's avatar
merging    
pitiot committed
544
545
546
547
548
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
		}
pitiot's avatar
merge    
pitiot committed
549

pitiot's avatar
merging    
pitiot committed
550
		addAgent(start, goal) ;
pitiot's avatar
merge    
pitiot committed
551

Thomas's avatar
Thomas committed
552
553
	}

David Cazier's avatar
David Cazier committed
554
555
556
557
	// 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
558
	yStartDelta = envMap_.geometry.size(1) / 10 ;
David Cazier's avatar
David Cazier committed
559
560

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

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

565
	VEC3 xSide (5.0f,0.0f,0.0f);
566
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
567

Thomas Jund's avatar
Thomas Jund committed
568
	std::cout << "geom : " << envMap_.geometry << std::endl;
pitiot's avatar
merging    
pitiot committed
569
570
571

	int sumObstacles=0;
	for(unsigned int j = 0; j<nbSnakes; j++)
David Cazier's avatar
David Cazier committed
572
	{
pitiot's avatar
merging    
pitiot committed
573
574
575
576
		std::vector<PFP::VEC3> positions [snakeSize] ;
		float x = xStartMin + rand() % xStartDelta;
		VEC3 goal;

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

pitiot's avatar
merging    
pitiot committed
579
580
581
582
583
584
585
586
587
588
589
590

		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
591
		while (r<80)
David Cazier's avatar
David Cazier committed
592
		{
pitiot's avatar
merging    
pitiot committed
593
594
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
Thomas Jund's avatar
Thomas Jund committed
595
596
597


			if ((r==0 || angle(goal-goals[r],goals[r]-goals[r-1])<M_PI/2.0f) )
598
			{
Thomas Jund's avatar
Thomas Jund committed
599
600
601

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

pitiot's avatar
merging    
pitiot committed
602
				goals.push_back(goal);
603
				r++;
pitiot's avatar
merging    
pitiot committed
604
605
			}

David Cazier's avatar
David Cazier committed
606
		}
pitiot's avatar
merging    
pitiot committed
607
608
609
610
611
612
613
614
615
616

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

David Cazier's avatar
David Cazier committed
617
618
619
620
			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
621
			positions[i]=vPos;
David Cazier's avatar
David Cazier committed
622
		}
pitiot's avatar
merging    
pitiot committed
623
624
625
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
626

pitiot's avatar
merging    
pitiot committed
627
628
		for (int i = 0 ; i < snakeSize ; i++)
			movingObstacles_.push_back(art->members[i]);
David Cazier's avatar
David Cazier committed
629
	}
Thomas's avatar
Thomas committed
630
631
}

David Cazier's avatar
David Cazier committed
632
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
633
{
David Cazier's avatar
David Cazier committed
634
635
636
637
638
	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
639

David Cazier's avatar
David Cazier committed
640
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
641
	{
David Cazier's avatar
David Cazier committed
642
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
643
		{
644
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
David Cazier's avatar
David Cazier committed
645
			addAgent(posagent, -1.0f * posagent) ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
646
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
647
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
648
649
}

Thomas Jund's avatar
Thomas Jund committed
650
void Simulator::setupScenario(unsigned int nbMaxAgent, bool pedWay)
Pierre Kraemer's avatar
Pierre Kraemer committed
651
652
{
	/*
David Cazier's avatar
David Cazier committed
653
	 * Add agents, specifying their start position, and store their goal on the
David Cazier's avatar
David Cazier committed
654
655
	 * opposite side of the environment.
	 */
656
657
	TraversorF<PFP::MAP> tF(envMap_.map);
	Dart d = tF.begin() ;
David Cazier's avatar
David Cazier committed
658

Thomas Jund's avatar
Thomas Jund committed
659
660
	unsigned int nbx = 1 ;
	unsigned int nby = 1 ;
David Cazier's avatar
David Cazier committed
661
662
663

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

David Cazier's avatar
David Cazier committed
664
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
665
	{
Thomas Jund's avatar
Thomas Jund committed
666
		VEC3 pos;
David Cazier's avatar
David Cazier committed
667
668
		bool found = false ;
		Dart dCell ;
669
		while (!found && d != tF.end())
David Cazier's avatar
David Cazier committed
670
		{
671
			if (!envMap_.buildingMark.isMarked(d)
Thomas Jund's avatar
Thomas Jund committed
672
			    && (!pedWay || envMap_.pedWayMark.isMarked(d))
673
			    )
David Cazier's avatar
David Cazier committed
674
			{
675
				pos = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
676
677
678
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
679
			}
680
			d = tF.next() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
681
682
		}

David Cazier's avatar
David Cazier committed
683
684
		if (found)
		{
Thomas Jund's avatar
Thomas Jund committed
685
			float ecart = 2.0f*Agent::radius_;
686
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
687
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
David Cazier's avatar
David Cazier committed
688
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
689
			{
David Cazier's avatar
David Cazier committed
690
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
691
				{
692
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
693
					addAgent(posagent, -1.0f * posagent, dCell) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
694
695
696
697
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
698
699
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
700
}
701

702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
void Simulator::setupPlanetScenario(unsigned int nbAgents, unsigned int nbObstacles)
{
	/*
	* Add agents, specifying their start position, and store their goals on the
	* opposite side of the environment.
	*/
	Dart d = envMap_.map.begin();
	CellMarker<FACE> filled(envMap_.map);

	unsigned int nbx = 1;
	unsigned int nby = 1;

	unsigned int bMax = nbAgents / (nbx*nby);

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

		if(found)
		{
			float ecart = 3.0f;
			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, dCell, envMap_.position), 0.0f);
			VEC3 posinit = VEC3(pos[0] - (float(nbx)/2.0f * ecart), pos[1] - (float(nby)/2.0f * ecart), pos[2]);
			for(unsigned int curx = 0; curx < nbx; ++curx)
			{
				for(unsigned int cury = 0; cury < nby; ++cury)
				{
					VEC3 displ(ecart * curx, ecart * cury, 0.0f);
					Agent::rotate(Agent::xyPlane, pl.normal(), displ);
					VEC3 posagent = posinit + displ;
//					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f);
					addAgent(posagent,-1.0f * posagent, dCell);
//					agents_.back()->goals_.push_back(posagent);
//					agents_.back()->goals_.push_back(VEC3(0,0,0));
//					agents_.back()->goals_.push_back(-1.0f * posagent);
					agents_.back()->curGoal_ = 1;
				}
			}
		}
	}
	std::cout << "nb agents : " << agents_.size() << std::endl;

	swapAgentsGoals();
}

760
761
762
763
764
765
766
767
768
769
770
771
772
773
void Simulator::addMovingObstacles(unsigned int nb)
{
	TraversorF<PFP::MAP> tF(envMap_.map);
	Dart d = tF.begin() ;
	for (unsigned int i = 0 ; i < nb && d != envMap_.map.end() ; ++i)
	{
		bool found = false ;
		Dart dCell ;
		while (!found && d != tF.end())
		{
			if (!envMap_.buildingMark.isMarked(d)
				&& !envMap_.pedWayMark.isMarked(d)
				)
			{
774
				float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
775
776
777
778
779
				if(area>1400)
				{
					dCell = d ;
					found = true ;
				}
780
781
782
783
784
			}
			d = tF.next() ;
		}

		if (found)
Arash HABIBI's avatar
Arash HABIBI committed
785
			addMovingObstacle(dCell,1) ;
786
787
788
789
790
791
792
	}
}

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

795
796
	MovingMesh* mm = NULL;

797
798
799
800
	switch(obstType)
	{
		case 0 :
		{
801
			start = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
802
803
804
			VEC3 xSide (2.0f,0.0f,0.0f);
			VEC3 ySide (0.0f,5.0f,0.0f);

805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
#ifdef TWO_AND_HALF_DIM
			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, d, envMap_.position), 0.0f);

			VEC3 displ = -xSide-ySide;
			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
			vPos.push_back(start + displ);

			displ = xSide-ySide;
			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
			vPos.push_back(start + displ);

			displ = xSide+ySide;
			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
			vPos.push_back(start + displ);

			displ = ySide-xSide;
			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
			vPos.push_back(start + displ);
#else
824
825
826
			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
827
			vPos.push_back(start-xSide+ySide);
828
829
#endif

830
831
		}
			break;
832
833
		case 1 :
		{
834
835
836
			mm = new MovingMesh(envMap_, d, "./meshRessources/LimaceLvl1.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/cylinder.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/simpleCyl.obj");
837
			movingMeshes_.push_back(mm);
838
			vPos = mm->computeProjectedPointSet(maxHeight);
Thomas Jund's avatar
Thomas Jund committed
839
840
841
842
843
844
845
846
847
848

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

			float scale = 1.2f;
			for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
				*it = *it*scale - bary*(scale-1.0f);
849
850
		}
			break;
851
852
853
854
855
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

856
	bool orientAccordingToFace=false;
Thomas Jund's avatar
Thomas Jund committed
857
	if(mm!=NULL && orientAccordingToFace)
Thomas Jund's avatar
Thomas Jund committed
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
	{
		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
875
		TraversorV<PFP2::MAP> tv(mm->map);
Thomas Jund's avatar
Thomas Jund committed
876
877
		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
		{
Thomas Jund's avatar
Thomas Jund committed
878
			mm->position[d] += rotate(mm->position[d], bary, angle);
Thomas Jund's avatar
Thomas Jund committed
879
		}
Thomas Jund's avatar
Thomas Jund committed
880
881
882

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

885
886
	std::vector<VEC3> goals;
	goals.push_back(start);
Thomas Jund's avatar
merge    
Thomas Jund committed
887

pitiot's avatar
pitiot committed
888
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), (obstType==0));
889
890

	movingObstacles_.push_back(mo);
891

892
	if(mm != NULL)
893
	{
894
		mo->attachMesh(mm);
Thomas Jund's avatar
Thomas Jund committed
895
896
897
//		std::cout << "here you go" << std::endl;
//		addAgent(vPos[0], vPos[0], d);
//		mo->attachAgent(agents_.back());
898
	}
899
900
}

Thomas Jund's avatar
Thomas Jund committed
901
void Simulator::addPathToObstacles(CellMarker<FACE>& markForbid, bool goalCenterCell)
902
903
904
905
906
{
	unsigned int dartDistForPath = 50 ;

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

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

Thomas Jund's avatar
Thomas Jund committed
911
		while(markForbid.isMarked(dStart))
912
		{
Thomas Jund's avatar
Thomas Jund committed
913
			if(!markForbid.isMarked(envMap_.map.phi2(dStart)))
914
915
916
917
918
				dStart = envMap_.map.phi2(dStart);
			else
				dStart = envMap_.map.phi1(dStart);
		}

919
920
921
922
923
		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
924
			|| markForbid.isMarked(dStop)
925
926
927
928
929
930
931
			; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
		}

Thomas Jund's avatar
Thomas Jund committed
932
		addPathToObstacle(*it, dStart, dStop, markForbid, goalCenterCell);
933
934
935
	}
}

Thomas Jund's avatar
Thomas Jund committed
936
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal, CellMarker<FACE>& markForbid, bool goalCenterCell)
937
{
David Cazier's avatar
David Cazier committed
938
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
939
940
																   envMap_.position, dStart,
																   dGoal,
Thomas Jund's avatar
Thomas Jund committed
941
																   markForbid) ;
942
943
944

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
Thomas Jund's avatar
Thomas Jund committed
945
946
947
948
949
950
951
952
		VEC3 dest;
		if(goalCenterCell)
			dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
		else
		{
			dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
			dest /= 2.0f ;
		}
953
954
955

		mo->goals_.push_back(dest) ;
	}
956
957
958
959
960

	path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
												envMap_.position,
												dGoal,
												dStart,
Thomas Jund's avatar
Thomas Jund committed
961
												markForbid) ;
962
963
964

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
Thomas Jund's avatar
Thomas Jund committed
965
966
967
968
969
970
971
972
		VEC3 dest;
		if(goalCenterCell)
			dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
		else
		{
			dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
			dest /= 2.0f ;
		}
973
974
975

		mo->goals_.push_back(dest) ;
	}
pitiot's avatar
pitiot committed
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993

//	if(((mo->front-mo->center)*(mo->goals_[mo->curGoal_]-mo->center))<0)
//	{
//		std::cout << __FILE__ << " " << __LINE__ << " rotate limace" << std::endl;
//
//		Geom::Matrix44f m;
//		m.identity();
//		rotateZ(180.0f,m);
//
//		TraversorV<PFP::MAP> tv(mo->map);
//		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
//		{
//			mo->position[d] -= mo->center;
//			mo->position[d] = Geom::transform(mo->position[d],m);
//			mo->position[d] += mo->center;
//			mo->position[d][2] += 10.0f;
//		}
//	}
994
995
}

Pierre Kraemer's avatar
Pierre Kraemer committed
996
#ifndef SPATIAL_HASHING
997
998
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
999
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
1000
	{
David Cazier's avatar
David Cazier committed
1001
1002
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
1003

David Cazier's avatar
David Cazier committed
1004
1005
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
1006

David Cazier's avatar
David Cazier committed
1007
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
1008
1009
1010
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
1011

David Cazier's avatar
David Cazier committed
1012
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
1013
		{
1014
			VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
1015
1016
1017
1018
1019

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

David Cazier's avatar
David Cazier committed
1020
			agents_[i]->goals_.push_back(dest) ;
1021
1022
1023
1024
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
1025
1026
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
1027
	//city