simulator.cpp 34.1 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
71
			setupScenario(nbAgent, false) ;
			addMovingObstacles(nbObst);
			addPathToObstacles();
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();
Thomas Jund's avatar
Thomas Jund committed
84
			addPathsToAgents();
Jund Thomas's avatar
Jund Thomas committed
85
			break;
86
87
88
89
90
91
92
93
94
95
		case 6:
#ifdef TWO_AND_HALF_DIM
			envMap_.init(config,200.0,200.0, minSize, 400.0f);
			setupPlanetScenario(nbAgent,nbObst);
			addMovingObstacles(nbObst);
			addPathToObstacles();
#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() ;
pitiot's avatar
stats    
pitiot committed
142
143
144
		clock_gettime(CLOCK_MONOTONIC, &endTime) ;
		time_behave+= timespec_delta(begTime,endTime).tv_nsec;
		clock_gettime(CLOCK_MONOTONIC, &begTime) ;
Thomas Jund's avatar
Thomas Jund committed
145
		movingObstacles_[i]->updateAgentNeighbors();
146
		movingObstacles_[i]->updateObstacleNeighbors();
pitiot's avatar
pitiot committed
147
148
149
#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
David Cazier's avatar
David Cazier committed
150
		movingObstacles_[i]->update() ;
pitiot's avatar
stats    
pitiot committed
151
152
		clock_gettime(CLOCK_MONOTONIC, &endTime) ;
		time_obstacle+= timespec_delta(begTime,endTime).tv_nsec;
Arash HABIBI's avatar
nickel    
Arash HABIBI committed
153
		// commente par Arash
154
		 movingObstacles_[i]->updateMesh() ;
pitiot's avatar
pitiot committed
155
	}
pitiot's avatar
stats    
pitiot committed
156
157


David Cazier's avatar
David Cazier committed
158
159
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
pitiot's avatar
pitiot committed
160
161
		if (agents_[i]->alive)
		{
pitiot's avatar
stats    
pitiot committed
162
			clock_gettime(CLOCK_MONOTONIC, &begTime) ;
pitiot's avatar
pitiot committed
163
			agents_[i]->updateObstacleNeighbors() ;
pitiot's avatar
maj    
pitiot committed
164
165

#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
166
			envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
pitiot's avatar
maj    
pitiot committed
167
#endif
pitiot's avatar
pitiot committed
168
			agents_[i]->updateAgentNeighbors() ;
pitiot's avatar
maj    
pitiot committed
169
#ifndef SPATIAL_HASHING
pitiot's avatar
pitiot committed
170
			envMap_.map.setCurrentLevel(0) ;
pitiot's avatar
maj    
pitiot committed
171
#endif
pitiot's avatar
stats    
pitiot committed
172
173
174
			clock_gettime(CLOCK_MONOTONIC, &endTime) ;
			time_agent+=timespec_delta(begTime,endTime).tv_nsec;
			clock_gettime(CLOCK_MONOTONIC, &begTime) ;
pitiot's avatar
pitiot committed
175
176
			agents_[i]->computePrefVelocity() ;
			agents_[i]->computeNewVelocity() ;
pitiot's avatar
stats    
pitiot committed
177
178
			clock_gettime(CLOCK_MONOTONIC, &endTime) ;
			time_behave+=timespec_delta(begTime,endTime).tv_nsec;
pitiot's avatar
pitiot committed
179
		}
David Cazier's avatar
David Cazier committed
180
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
181

Pierre Kraemer's avatar
Pierre Kraemer committed
182
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
183
184
185
186
187
188
189
190
191
192
193
	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
194
#else
Thomas Jund's avatar
Thomas Jund committed
195

196
197
	if (multires)
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
Thomas Jund's avatar
Thomas Jund committed
198

David Cazier's avatar
David Cazier committed
199
	nb_dead = 0 ;
pitiot's avatar
stats    
pitiot committed
200
	clock_gettime(CLOCK_MONOTONIC, &begTime) ;
David Cazier's avatar
David Cazier committed
201
202
203
204
205
206
	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
207

208
			if (agents_[i]->part_.crossCell != CGoGN::Algo::Surface::MovingObjects::NO_CROSS)
David Cazier's avatar
David Cazier committed
209
210
211
212
213
214
				envMap_.agentChangeFace(agents_[i], oldFace) ;
		}
		else
			nb_dead++ ;

	}
pitiot's avatar
stats    
pitiot committed
215
216
	clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	time_agent+= timespec_delta(begTime,endTime).tv_nsec;
David Cazier's avatar
David Cazier committed
217
218
219

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

Thomas Jund's avatar
Thomas Jund committed
221
	 if (multires)
pitiot's avatar
stats    
pitiot committed
222
223
	 {
		 clock_gettime(CLOCK_MONOTONIC, &begTime) ;
Thomas Jund's avatar
Thomas Jund committed
224
		 envMap_.updateMap() ;
pitiot's avatar
stats    
pitiot committed
225
226
227
		 clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	 	 time_multires+= timespec_delta(begTime,endTime).tv_nsec;
	 }
David Cazier's avatar
David Cazier committed
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
#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
244
void Simulator::addAgent(const VEC3& start, const VEC3& goal)
David Cazier's avatar
David Cazier committed
245
{
David Cazier's avatar
David Cazier committed
246
	agents_.push_back(new Agent(this, start, goal)) ;
David Cazier's avatar
David Cazier committed
247
248
}

249
250
251
252
253
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
254
void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
David Cazier's avatar
David Cazier committed
255
{
pitiot's avatar
merging    
pitiot committed
256
257
258
	if (multires)
		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
	else
pitiot's avatar
pitiot committed
259

pitiot's avatar
merging    
pitiot committed
260
261
	{

pitiot's avatar
stats    
pitiot committed
262
263
//		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
264
265
266
	}


267
	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles"<< std::endl ;
David Cazier's avatar
David Cazier committed
268
269
270
271
272
273
274
275
276
277
278

	// 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
279
//	int radius = nbAgents/(Agent::radius_*Agent::radius_);
Thomas Jund's avatar
Thomas Jund committed
280
	std::cout << "radius " << radius << std::endl;
David Cazier's avatar
David Cazier committed
281
282
283
284
	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
285
		double angle = i * 2.0f * M_PI / float(nbAgents) ;
David Cazier's avatar
David Cazier committed
286
287
288
		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
289
		addAgent(start, goal) ;
pitiot's avatar
pitiot committed
290
//		CGoGNout<<"agent n°" << agents_[i]->agentNo << " : "<< agents_[i]<<CGoGNendl;
291
	}
pitiot's avatar
pitiot committed
292
293
294
295
296
	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
297
	int curGoal=0;
Thomas Jund's avatar
merge    
Thomas Jund committed
298

pitiot's avatar
pitiot committed
299
	for (unsigned int i = 0 ; i < nbObstacles ; i++)
David Cazier's avatar
David Cazier committed
300
	{
Thomas Jund's avatar
merge    
Thomas Jund committed
301
302
		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
303
304
305
306
307
308
309
		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
310
			v=VEC3 (std::cos(angle) * (radius/2), std::sin(angle) * (radius/2), 0) ;
pitiot's avatar
pitiot committed
311
312
313
		}
		start = center + v ;
		positions.push_back(start);
pitiot's avatar
pitiot committed
314
	}
Thomas Jund's avatar
merge    
Thomas Jund committed
315

pitiot's avatar
pitiot committed
316
317
318
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
319
320

		unsigned int nbGoals=360;
pitiot's avatar
pitiot committed
321
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
322
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
323
		{
Thomas Jund's avatar
Thomas Jund committed
324
			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
325
326
327
		}

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

pitiot's avatar
pitiot committed
329
330
331
332
		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
333
		curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+1)%nbGoals;
334
		mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);
pitiot's avatar
pitiot committed
335

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

pitiot's avatar
pitiot committed
339
340
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
341

pitiot's avatar
pitiot committed
342
343
344
	for (unsigned int i = nbObstacles/2 ; i < nbObstacles ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
345
346
347
348
349
350
351
//		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
352
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
353
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
354
		{
Thomas Jund's avatar
Thomas Jund committed
355
			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
356
357
358
359
360
361
362
363
		}

		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);
364
365
		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
366

367
368
//		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
369
370

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
371
	}
Thomas's avatar
Thomas committed
372

373
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
374
375
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
376
#endif
pitiot's avatar
merging    
pitiot committed
377
378

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

David Cazier's avatar
David Cazier committed
381
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
382
{
pitiot's avatar
merging    
pitiot committed
383
384
385
386
387
388
389
	if (multires)
	{
		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
	}
	else
	{

pitiot's avatar
stats    
pitiot committed
390
391
//		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
392
393
394
	}


David Cazier's avatar
David Cazier committed
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
	std::cout << " - Setup Corridor Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles" << std::endl ;

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

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

	// Départ des agents du quart gauche sur toute la hauteur
	int xStartMin = envMap_.geometry.min()[0] + xBorder ;
	int xStartDelta = envMap_.geometry.size(0) / 5 ;
	int yStartMin = envMap_.geometry.min()[1] + yBorder ;
	int yStartDelta = envMap_.geometry.size(1) - 2 * yBorder ;

	// Arrivée des agents à l'opposée
	int xGoalDelta = envMap_.geometry.size(0) / 5 ;
	int xGoalMin = envMap_.geometry.max()[0] - xBorder - xGoalDelta ;
	int yGoalMin = yStartMin ;
	int yGoalDelta = yStartDelta ;

	for (unsigned int i = 0 ; i < nbAgents ; ++i)
	{
		VEC3 start(xStartMin + rand() % xStartDelta, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;

		// Un agent sur 2 va de droite à gauche
		if (i % 2 == 1)
			addAgent(start, goal) ;
		else
			addAgent(goal, start) ;
	}

	// Départ des obstacles du quart haut sur toute une demi-largeur
	xStartMin = envMap_.geometry.min()[0] + envMap_.geometry.size(0) / 4 ;
	xStartDelta = envMap_.geometry.size(0) / 2 ;
	yStartMin = envMap_.geometry.min()[1] + yBorder ;
	yStartDelta = envMap_.geometry.size(1) / 5 ;
pitiot's avatar
pitiot committed
432

David Cazier's avatar
David Cazier committed
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
	// Arrivée des obstacles à l'opposée
	yGoalDelta = envMap_.geometry.size(1) / 5 ;
	yGoalMin = envMap_.geometry.max()[1] - yBorder - yGoalDelta ;

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

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
		float x = xStartMin + ((int)i*30) % xStartDelta;
//		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;
		VEC3  goal(x, yGoalMin  + rand() % yGoalDelta,  0) ;

		vPos.clear();
		// Un obstacle sur deux va vers le haut
		VEC3 tmp ;
		if (i % 2 == 1)
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
			vPos.push_back(start-xSide+ySide);
			vPos.push_back(start-xSide-ySide);
			vPos.push_back(start+xSide-ySide);
			vPos.push_back(start+xSide+ySide);
		}
Thomas Jund's avatar
Thomas Jund committed
462
463
		else
		{
David Cazier's avatar
David Cazier committed
464
465
466
467
468
469
470
471
			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
472

pitiot's avatar
pitiot committed
473
		mo4= new MovingObstacle(this, i,vPos,goals, true, false,1);
David Cazier's avatar
David Cazier committed
474
475


476
477
478
479
480
481
482
483
484
485
486
487
488
489
//		//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
490
491
492
493
494

		movingObstacles_.push_back(mo4);
	}
}

pitiot's avatar
merging    
pitiot committed
495
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
David Cazier's avatar
David Cazier committed
496
{
pitiot's avatar
merging    
pitiot committed
497
498
499
500
501
502
	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
503
504

	// Bordure à éviter autour de la scène (10% de sa taille)
pitiot's avatar
merging    
pitiot committed
505
506
	int xBorder = envMap_.geometry.size(0) / 5 ;
	int yBorder = envMap_.geometry.size(1) / 5 ;
David Cazier's avatar
David Cazier committed
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522

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

David Cazier's avatar
David Cazier committed
527
		// Un agent sur 2 va de droite à gauche
pitiot's avatar
merging    
pitiot committed
528
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
529
		if (i % 2 == 1)
pitiot's avatar
merging    
pitiot committed
530
531
532
533
534
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
		}
pitiot's avatar
merge    
pitiot committed
535

pitiot's avatar
merging    
pitiot committed
536
		addAgent(start, goal) ;
pitiot's avatar
merge    
pitiot committed
537

Thomas's avatar
Thomas committed
538
539
	}

David Cazier's avatar
David Cazier committed
540
541
542
543
	// 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
544
	yStartDelta = envMap_.geometry.size(1) / 10 ;
David Cazier's avatar
David Cazier committed
545
546

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

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

551
	VEC3 xSide (5.0f,0.0f,0.0f);
552
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
553

Thomas Jund's avatar
Thomas Jund committed
554
	std::cout << "geom : " << envMap_.geometry << std::endl;
pitiot's avatar
merging    
pitiot committed
555
556
557

	int sumObstacles=0;
	for(unsigned int j = 0; j<nbSnakes; j++)
David Cazier's avatar
David Cazier committed
558
	{
pitiot's avatar
merging    
pitiot committed
559
560
561
562
		std::vector<PFP::VEC3> positions [snakeSize] ;
		float x = xStartMin + rand() % xStartDelta;
		VEC3 goal;

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

pitiot's avatar
merging    
pitiot committed
565
566
567
568
569
570
571
572
573
574
575
576

		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
577
		while (r<80)
David Cazier's avatar
David Cazier committed
578
		{
pitiot's avatar
merging    
pitiot committed
579
580
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
Thomas Jund's avatar
Thomas Jund committed
581
582
583


			if ((r==0 || angle(goal-goals[r],goals[r]-goals[r-1])<M_PI/2.0f) )
584
			{
Thomas Jund's avatar
Thomas Jund committed
585
586
587

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

pitiot's avatar
merging    
pitiot committed
588
				goals.push_back(goal);
589
				r++;
pitiot's avatar
merging    
pitiot committed
590
591
			}

David Cazier's avatar
David Cazier committed
592
		}
pitiot's avatar
merging    
pitiot committed
593
594
595
596
597
598
599
600
601
602

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

David Cazier's avatar
David Cazier committed
603
604
605
606
			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
607
			positions[i]=vPos;
David Cazier's avatar
David Cazier committed
608
		}
pitiot's avatar
merging    
pitiot committed
609
610
611
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
612

pitiot's avatar
merging    
pitiot committed
613
614
		for (int i = 0 ; i < snakeSize ; i++)
			movingObstacles_.push_back(art->members[i]);
David Cazier's avatar
David Cazier committed
615
	}
Thomas's avatar
Thomas committed
616
617
}

David Cazier's avatar
David Cazier committed
618
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
619
{
David Cazier's avatar
David Cazier committed
620
621
622
623
624
	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
625

David Cazier's avatar
David Cazier committed
626
	for (int i = 0 ; i < nbLines ; ++i)
David Cazier's avatar
David Cazier committed
627
	{
David Cazier's avatar
David Cazier committed
628
		for (int j = 0 ; j < nbRank ; ++j)
David Cazier's avatar
David Cazier committed
629
		{
630
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
David Cazier's avatar
David Cazier committed
631
			addAgent(posagent, -1.0f * posagent) ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
632
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
633
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
634
635
}

Thomas Jund's avatar
Thomas Jund committed
636
void Simulator::setupScenario(unsigned int nbMaxAgent, bool pedWay)
Pierre Kraemer's avatar
Pierre Kraemer committed
637
638
{
	/*
David Cazier's avatar
David Cazier committed
639
	 * Add agents, specifying their start position, and store their goal on the
David Cazier's avatar
David Cazier committed
640
641
	 * opposite side of the environment.
	 */
642
643
	TraversorF<PFP::MAP> tF(envMap_.map);
	Dart d = tF.begin() ;
David Cazier's avatar
David Cazier committed
644

Thomas Jund's avatar
Thomas Jund committed
645
646
	unsigned int nbx = 1 ;
	unsigned int nby = 1 ;
David Cazier's avatar
David Cazier committed
647
648
649

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

David Cazier's avatar
David Cazier committed
650
	for (unsigned int i = 0 ; i < bMax && d != envMap_.map.end() ; ++i)
David Cazier's avatar
David Cazier committed
651
	{
Thomas Jund's avatar
Thomas Jund committed
652
		VEC3 pos;
David Cazier's avatar
David Cazier committed
653
654
		bool found = false ;
		Dart dCell ;
655
		while (!found && d != tF.end())
David Cazier's avatar
David Cazier committed
656
		{
657
			if (!envMap_.buildingMark.isMarked(d)
Thomas Jund's avatar
Thomas Jund committed
658
			    && (!pedWay || envMap_.pedWayMark.isMarked(d))
659
			    )
David Cazier's avatar
David Cazier committed
660
			{
661
				pos = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
662
663
664
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
665
			}
666
			d = tF.next() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
667
668
		}

David Cazier's avatar
David Cazier committed
669
670
		if (found)
		{
Thomas Jund's avatar
Thomas Jund committed
671
			float ecart = 2.0f*Agent::radius_;
672
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
673
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
David Cazier's avatar
David Cazier committed
674
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
675
			{
David Cazier's avatar
David Cazier committed
676
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
677
				{
678
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
679
					addAgent(posagent, -1.0f * posagent, dCell) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
680
681
682
683
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
684
685
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
686
}
687

688
689
690
691
692
693
694
695
696
697
698
699
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
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();
}

746
747
748
749
750
751
752
753
754
755
756
757
758
759
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)
				)
			{
760
				float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
761
762
763
764
765
				if(area>1400)
				{
					dCell = d ;
					found = true ;
				}
766
767
768
769
770
			}
			d = tF.next() ;
		}

		if (found)
771
			addMovingObstacle(dCell,0) ;
772
773
774
775
776
777
778
	}
}

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

781
782
	MovingMesh* mm = NULL;

783
784
785
786
	switch(obstType)
	{
		case 0 :
		{
787
			start = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
788
789
790
			VEC3 xSide (2.0f,0.0f,0.0f);
			VEC3 ySide (0.0f,5.0f,0.0f);

791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
#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
810
811
812
			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
813
			vPos.push_back(start-xSide+ySide);
814
815
#endif

816
817
		}
			break;
818
819
		case 1 :
		{
820
821
822
			mm = new MovingMesh(envMap_, d, "./meshRessources/LimaceLvl1.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/cylinder.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/simpleCyl.obj");
823
			movingMeshes_.push_back(mm);
824
			vPos = mm->computeProjectedPointSet(maxHeight);
Thomas Jund's avatar
Thomas Jund committed
825
826
827
828
829
830
831
832
833
834

			//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);
835
836
		}
			break;
837
838
839
840
841
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

842
	bool orientAccordingToFace=false;
Thomas Jund's avatar
Thomas Jund committed
843
	if(mm!=NULL && orientAccordingToFace)
Thomas Jund's avatar
Thomas Jund committed
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
	{
		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
861
		TraversorV<PFP2::MAP> tv(mm->map);
Thomas Jund's avatar
Thomas Jund committed
862
863
		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
		{
Thomas Jund's avatar
Thomas Jund committed
864
			mm->position[d] += rotate(mm->position[d], bary, angle);
Thomas Jund's avatar
Thomas Jund committed
865
		}
Thomas Jund's avatar
Thomas Jund committed
866
867
868

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

871
872
	std::vector<VEC3> goals;
	goals.push_back(start);
Thomas Jund's avatar
merge    
Thomas Jund committed
873

pitiot's avatar
pitiot committed
874
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), (obstType==0));
875
876

	movingObstacles_.push_back(mo);
877

878
	if(mm != NULL)
879
	{
880
		mo->attachMesh(mm);
Thomas Jund's avatar
Thomas Jund committed
881
882
883
//		std::cout << "here you go" << std::endl;
//		addAgent(vPos[0], vPos[0], d);
//		mo->attachAgent(agents_.back());
884
	}
885
886
887
888
889
890
891
892
}

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

	for (std::vector<MovingObstacle*>::iterator it = movingObstacles_.begin() ; it != movingObstacles_.end() ; ++it)
	{
pitiot's avatar
pitiot committed
893
		Dart dStart = (*it)->parts_[0]->d;
894
895
896
897
898
899
900
901
902

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

903
904
905
906
907
		Dart dStop = dStart;

		for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop)
			|| j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStart)
908
			|| envMap_.pedWayMark.isMarked(dStop)
909
910
911
912
913
914
915
916
917
918
919
			; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
		}

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

920
921
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
David Cazier's avatar
David Cazier committed
922
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
923
924
																   envMap_.position, dStart,
																   dGoal,
925
																   envMap_.pedWayMark) ;
926
927
928

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

		mo->goals_.push_back(dest) ;
	}
934
935
936
937
938

	path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
												envMap_.position,
												dGoal,
												dStart,
939
												envMap_.pedWayMark) ;
940
941
942

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

		mo->goals_.push_back(dest) ;
	}
pitiot's avatar
pitiot committed
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965

//	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;
//		}
//	}
966
967
}

Pierre Kraemer's avatar
Pierre Kraemer committed
968
#ifndef SPATIAL_HASHING
969
970
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
971
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
972
	{
David Cazier's avatar
David Cazier committed
973
974
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
975

David Cazier's avatar
David Cazier committed
976
977
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
978

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

David Cazier's avatar
David Cazier committed
984
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
985
		{
986
			VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
987
988
989
990
991

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

David Cazier's avatar
David Cazier committed
992
			agents_[i]->goals_.push_back(dest) ;
993
994
995
996
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
997
998
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
999
	//city
David Cazier's avatar
David Cazier committed
1000
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
1001
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
1002
	{
David Cazier's avatar
David Cazier committed
1003
1004
		agents_[i]->goals_.clear() ;

1005
1006
		TraversorF<PFP::MAP> tF(envMap_.map);

David Cazier's avatar
David Cazier committed
1007
1008
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
1009
1010
1011
1012
1013
1014
1015
		for (unsigned int j = 0 ;
			/*!envMap_.pedWayMark.isMarked(dStop) || */
			envMap_.buildingMark.isMarked(dStop)
		    || j < dartDistForPath + rand() * 20
		    || envMap_.map.sameFace(dStop, dStart)
		    || envMap_.obstacleMark.isMarked(dStop)
		    ; ++j)
David Cazier's avatar
David Cazier committed
1016
		{
David Cazier's avatar
David Cazier committed
1017
			envMap_.map.next(dStop) ;
1018
1019
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
1020
		}
1021

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

David Cazier's avatar
David Cazier committed
1024
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
David Cazier's avatar
David Cazier committed
1025
1026
1027
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
1028

David Cazier's avatar
David Cazier committed
1029
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
1030
		{
Thomas Jund's avatar
Thomas Jund committed
1031
1032
1033
1034
1035
1036
			Dart d = *it;
			while(envMap_.map.isBoundaryEdge(d))
			{
				d = envMap_.map.phi1(d);
			}

1037
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
Thomas Jund's avatar
Thomas Jund committed
1038
1039

			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
David Cazier's avatar
David Cazier committed
1040
			dest /= 2.0f ;
Thomas Jund's avatar
Thomas Jund committed
1041
			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
David Cazier's avatar
David Cazier committed
1042
			dest[2] = 0 ;
1043

David Cazier's avatar
David Cazier committed
1044
			agents_[i]->goals_.push_back(dest) ;
1045
		}
1046

David Cazier's avatar
David Cazier committed
1047
		Dart dStop2 = dStop ;