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

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

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

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

pitiot's avatar
pitiot committed
139
	{
pitiot's avatar
up    
pitiot committed
140
//		unsigned int i =12;
pitiot's avatar
stats    
pitiot committed
141
		clock_gettime(CLOCK_MONOTONIC, &begTime) ;
David Cazier's avatar
David Cazier committed
142
		movingObstacles_[i]->computePrefVelocity() ;
pitiot's avatar
pitiot committed
143
		movingObstacles_[i]->computeNewVelocity() ;
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
144
		movingObstacles_[i]->initForces();
pitiot's avatar
stats    
pitiot committed
145
146
		clock_gettime(CLOCK_MONOTONIC, &endTime) ;
		time_behave+= timespec_delta(begTime,endTime).tv_nsec;
pitiot's avatar
up    
pitiot committed
147
148
149
150
151
152
153
//		CGoGNout<<" Obstacle "<<i<<" : ";
//		for(unsigned int j=0;j<movingObstacles_[i]->nbVertices;j++)
//		{
//			VEC3 dist =movingObstacles_[i]->parts_[j]->getPosition()-movingObstacles_[i]->parts_[(j+1)%movingObstacles_[i]->nbVertices]->getPosition();
//			CGoGNout<<dist.norm()<<" || ";
//		}
//		CGoGNout<<CGoGNendl;
pitiot's avatar
merge    
pitiot committed
154

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

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

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

David Cazier's avatar
David Cazier committed
180
181
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	{
pitiot's avatar
pitiot committed
182
183
		if (agents_[i]->alive)
		{
pitiot's avatar
stats    
pitiot committed
184
			clock_gettime(CLOCK_MONOTONIC, &begTime) ;
pitiot's avatar
pitiot committed
185
			agents_[i]->updateObstacleNeighbors() ;
pitiot's avatar
maj    
pitiot committed
186
187

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

Pierre Kraemer's avatar
Pierre Kraemer committed
204
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
205
206
207
208
209
210
211
212
213
214
215
	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
216
#else
Thomas Jund's avatar
Thomas Jund committed
217

218
219
	if (multires)
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
Thomas Jund's avatar
Thomas Jund committed
220

David Cazier's avatar
David Cazier committed
221
	nb_dead = 0 ;
pitiot's avatar
stats    
pitiot committed
222
	clock_gettime(CLOCK_MONOTONIC, &begTime) ;
David Cazier's avatar
David Cazier committed
223
224
225
226
227
228
	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
229

230
			if (agents_[i]->part_.crossCell != CGoGN::Algo::Surface::MovingObjects::NO_CROSS)
David Cazier's avatar
David Cazier committed
231
232
233
234
235
236
				envMap_.agentChangeFace(agents_[i], oldFace) ;
		}
		else
			nb_dead++ ;

	}
pitiot's avatar
stats    
pitiot committed
237
238
	clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	time_agent+= timespec_delta(begTime,endTime).tv_nsec;
David Cazier's avatar
David Cazier committed
239
240
241

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

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

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

pitiot's avatar
merging    
pitiot committed
282
283
	{

pitiot's avatar
stats    
pitiot committed
284
285
//		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
286
287
288
	}


289
	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles"<< std::endl ;
David Cazier's avatar
David Cazier committed
290
291
292
293
294
295
296
297
298
299
300

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

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

pitiot's avatar
pitiot committed
338
339
340
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
341
342

		unsigned int nbGoals=360;
pitiot's avatar
pitiot committed
343
		std::vector<VEC3> goals;
Thomas Jund's avatar
Thomas Jund committed
344
		for(unsigned int k = 0 ; k < nbGoals ; k++)
pitiot's avatar
pitiot committed
345
		{
Thomas Jund's avatar
Thomas Jund committed
346
			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
347
348
349
		}

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

pitiot's avatar
pitiot committed
351
352
353
354
		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
355
		curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+1)%nbGoals;
356
		mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);
pitiot's avatar
pitiot committed
357

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

pitiot's avatar
pitiot committed
361
362
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
363

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

		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);
386
387
		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
388

389
390
//		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
391
392

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
393
	}
Thomas's avatar
Thomas committed
394

395
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
396
397
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
398
#endif
pitiot's avatar
merging    
pitiot committed
399
400

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

David Cazier's avatar
David Cazier committed
403
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
404
{
pitiot's avatar
pitiot committed
405
406
	VEC3 xSide (5.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,10.0f,0.0f);
pitiot's avatar
merging    
pitiot committed
407
408
	if (multires)
	{
pitiot's avatar
pitiot committed
409
//		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
pitiot's avatar
up    
pitiot committed
410
		envMap_.init(config, nbObstacles > 60 ? ((nbObstacles*20*xSide[0])/6) : 1000.0f , 960.0f, minSize, 320.0f) ; //grosses cases
pitiot's avatar
merging    
pitiot committed
411
412
413
414
	}
	else
	{

pitiot's avatar
stats    
pitiot committed
415
416
//		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
417
418
419
	}


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

David Cazier's avatar
David Cazier committed
458
459
460
461
	// 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
462

David Cazier's avatar
David Cazier committed
463
464
465
466
467
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;

	for (unsigned int i = 0 ; i < nbObstacles ; i++)
	{
pitiot's avatar
pitiot committed
468
		float x = xStartMin + ((int)(i*2*xSide[0])) % xStartDelta;
David Cazier's avatar
David Cazier committed
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
//		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
486
487
		else
		{
David Cazier's avatar
David Cazier committed
488
489
490
491
492
493
494
495
			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
496

pitiot's avatar
pitiot committed
497
		mo4= new MovingObstacle(this, i,vPos,goals, true, false,1);
David Cazier's avatar
David Cazier committed
498
499


500
501
502
503
504
505
506
507
508
509
510
511
512
513
//		//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
514
515
516
517
518

		movingObstacles_.push_back(mo4);
	}
}

pitiot's avatar
merging    
pitiot committed
519
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
David Cazier's avatar
David Cazier committed
520
{
pitiot's avatar
merging    
pitiot committed
521
522
523
524
525
526
	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
527
528

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

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

David Cazier's avatar
David Cazier committed
551
		// Un agent sur 2 va de droite à gauche
pitiot's avatar
merging    
pitiot committed
552
		VEC3 tmp ;
David Cazier's avatar
David Cazier committed
553
		if (i % 2 == 1)
pitiot's avatar
merging    
pitiot committed
554
555
556
557
558
		{
			tmp = goal ;
			goal = start ;
			start = tmp ;
		}
pitiot's avatar
merge    
pitiot committed
559

pitiot's avatar
merging    
pitiot committed
560
		addAgent(start, goal) ;
pitiot's avatar
merge    
pitiot committed
561

Thomas's avatar
Thomas committed
562
563
	}

David Cazier's avatar
David Cazier committed
564
565
566
567
	// 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
568
	yStartDelta = envMap_.geometry.size(1) / 10 ;
David Cazier's avatar
David Cazier committed
569
570

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

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

575
	VEC3 xSide (5.0f,0.0f,0.0f);
576
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
577

Thomas Jund's avatar
Thomas Jund committed
578
	std::cout << "geom : " << envMap_.geometry << std::endl;
pitiot's avatar
merging    
pitiot committed
579
580
581

	int sumObstacles=0;
	for(unsigned int j = 0; j<nbSnakes; j++)
David Cazier's avatar
David Cazier committed
582
	{
pitiot's avatar
merging    
pitiot committed
583
584
585
586
		std::vector<PFP::VEC3> positions [snakeSize] ;
		float x = xStartMin + rand() % xStartDelta;
		VEC3 goal;

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

pitiot's avatar
merging    
pitiot committed
589
590
591
592
593
594
595
596
597
598
599
600

		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
601
		while (r<80)
David Cazier's avatar
David Cazier committed
602
		{
pitiot's avatar
merging    
pitiot committed
603
604
			x = xStartMin + rand() % xStartDelta;
			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
Thomas Jund's avatar
Thomas Jund committed
605
606
607


			if ((r==0 || angle(goal-goals[r],goals[r]-goals[r-1])<M_PI/2.0f) )
608
			{
Thomas Jund's avatar
Thomas Jund committed
609
610
611

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

pitiot's avatar
merging    
pitiot committed
612
				goals.push_back(goal);
613
				r++;
pitiot's avatar
merging    
pitiot committed
614
615
			}

David Cazier's avatar
David Cazier committed
616
		}
pitiot's avatar
merging    
pitiot committed
617
618
619
620
621
622
623
624
625
626

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

David Cazier's avatar
David Cazier committed
627
628
629
630
			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
631
			positions[i]=vPos;
David Cazier's avatar
David Cazier committed
632
		}
pitiot's avatar
merging    
pitiot committed
633
634
635
//		CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
		ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
		sumObstacles += snakeSize;
636

pitiot's avatar
merging    
pitiot committed
637
638
		for (int i = 0 ; i < snakeSize ; i++)
			movingObstacles_.push_back(art->members[i]);
David Cazier's avatar
David Cazier committed
639
	}
Thomas's avatar
Thomas committed
640
641
}

David Cazier's avatar
David Cazier committed
642
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
643
{
David Cazier's avatar
David Cazier committed
644
645
646
647
648
	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
649

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

Thomas Jund's avatar
Thomas Jund committed
660
void Simulator::setupScenario(unsigned int nbMaxAgent, bool pedWay)
Pierre Kraemer's avatar
Pierre Kraemer committed
661
662
{
	/*
David Cazier's avatar
David Cazier committed
663
	 * Add agents, specifying their start position, and store their goal on the
David Cazier's avatar
David Cazier committed
664
665
	 * opposite side of the environment.
	 */
666
667
	TraversorF<PFP::MAP> tF(envMap_.map);
	Dart d = tF.begin() ;
Arash HABIBI's avatar
Arash HABIBI committed
668

Arash HABIBI's avatar
Arash HABIBI committed
669
//-----------------------
Arash HABIBI's avatar
Arash HABIBI committed
670
671
672
673
674
#define SCENARIO3
#if defined SCENARIO3
	unsigned int nbx = 5 ;
	unsigned int nby = 5 ;
#elif defined SCENARIO5
Arash HABIBI's avatar
Arash HABIBI committed
675
676
	unsigned int nbx = 2 ;
	unsigned int nby = 2 ;
Arash HABIBI's avatar
Arash HABIBI committed
677
678
679
680
681
#else
	unsigned int nbx = 1 ;
	unsigned int nby = 1 ;
#endif

Arash HABIBI's avatar
Arash HABIBI committed
682
//-----------------------
David Cazier's avatar
David Cazier committed
683
684
685

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

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

David Cazier's avatar
David Cazier committed
705
706
		if (found)
		{
Thomas Jund's avatar
Thomas Jund committed
707
			float ecart = 2.0f*Agent::radius_;
708
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
709
			                    pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
David Cazier's avatar
David Cazier committed
710
			for (unsigned int curx = 0 ; curx < nbx ; ++curx)
David Cazier's avatar
David Cazier committed
711
			{
David Cazier's avatar
David Cazier committed
712
				for (unsigned int cury = 0 ; cury < nby ; ++cury)
David Cazier's avatar
David Cazier committed
713
				{
714
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
715
					addAgent(posagent, -1.0f * posagent, dCell) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
716
717
718
719
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
720
721
	std::cout << "nb agents : " << agents_.size() << std::endl ;
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
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
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
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();
}

782
void Simulator::addMovingObstacles(unsigned int nb, unsigned int type)
783
784
785
786
787
788
789
790
791
792
793
794
795
{
	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)
				)
			{
796
				float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
797
798
799
800
801
				if(area>1400)
				{
					dCell = d ;
					found = true ;
				}
802
803
804
805
806
			}
			d = tF.next() ;
		}

		if (found)
807
			addMovingObstacle(dCell,type) ;
808
809
810
811
812
813
814
	}
}

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

817
818
	MovingMesh* mm = NULL;

819
820
821
822
	switch(obstType)
	{
		case 0 :
		{
823
			start = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
824
825
826
			VEC3 xSide (2.0f,0.0f,0.0f);
			VEC3 ySide (0.0f,5.0f,0.0f);

827
#ifdef TWO_AND_HALF_DIM
pitiot's avatar
up    
pitiot committed
828
829
			float xSideF=2.0f;
			float ySideF=5.0f;
830
			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::facePlane<PFP>(envMap_.map, d, envMap_.position));
pitiot's avatar
up    
pitiot committed
831
832
			VEC3 norm =Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, d, envMap_.position) ;
			VEC3 dir1 =VEC3(norm[1]+norm[2],norm[2]-norm[0],-norm[0]-norm[1]);
833

pitiot's avatar
up    
pitiot committed
834
835
			dir1.normalize();
			VEC3 dir2 = norm ^dir1;
836

pitiot's avatar
up    
pitiot committed
837
838
839
840
			vPos.push_back(start-xSideF*dir1-ySideF*dir2);
			vPos.push_back(start+xSideF*dir1-ySideF*dir2);
			vPos.push_back(start+xSideF*dir1+ySideF*dir2);
			vPos.push_back(start-xSideF*dir1+ySideF*dir2);
841
#else
842
843
844
			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
845
			vPos.push_back(start-xSide+ySide);
846
847
#endif

848
849
		}
			break;
850
851
		case 1 :
		{
Arash HABIBI's avatar
Arash HABIBI committed
852
853
			mm = new MovingMesh(envMap_, d, "./meshRessources/scenario3/limace_occ.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/LimaceLvl1.obj");
854
855
//			mm = new MovingMesh(envMap_, d, "./meshRessources/cylinder.obj");
//			mm = new MovingMesh(envMap_, d, "./meshRessources/simpleCyl.obj");
856
			movingMeshes_.push_back(mm);
857
			vPos = mm->computeProjectedPointSet(maxHeight);
Thomas Jund's avatar
Thomas Jund committed
858
859
860
861
862
863
864
865
866
867

			//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);
868
869
		}
			break;
870
871
872
873
874
		default:
			std::cout << "Unknown obstacle type" << std::endl;
			return;
	}

875
	bool orientAccordingToFace=false;
Thomas Jund's avatar
Thomas Jund committed
876
	if(mm!=NULL && orientAccordingToFace)
Thomas Jund's avatar
Thomas Jund committed
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
	{
		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
894
		TraversorV<PFP2::MAP> tv(mm->map);
Thomas Jund's avatar
Thomas Jund committed
895
896
		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
		{
pitiot's avatar
pitiot committed
897
			mm->position[d] += rotate2D(mm->position[d], bary, angle);
Thomas Jund's avatar
Thomas Jund committed
898
		}
Thomas Jund's avatar
Thomas Jund committed
899

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

904
905
	std::vector<VEC3> goals;
	goals.push_back(start);
Thomas Jund's avatar
merge    
Thomas Jund committed
906

pitiot's avatar
pitiot committed
907
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), (obstType==0));
908
909

	movingObstacles_.push_back(mo);
910

911
	if(mm != NULL)
912
	{
913
		mo->attachMesh(mm);
Thomas Jund's avatar
Thomas Jund committed
914
915
916
//		std::cout << "here you go" << std::endl;
//		addAgent(vPos[0], vPos[0], d);
//		mo->attachAgent(agents_.back());
917
	}
918
919
}

Thomas Jund's avatar
Thomas Jund committed
920
void Simulator::addPathToObstacles(CellMarker<FACE>& markForbid, bool goalCenterCell)
921
922
923
924
925
{
	unsigned int dartDistForPath = 50 ;

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

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

Thomas Jund's avatar
Thomas Jund committed
930
		while(markForbid.isMarked(dStart))
931
		{
Thomas Jund's avatar
Thomas Jund committed
932
			if(!markForbid.isMarked(envMap_.map.phi2(dStart)))
933
934
935
936
937
				dStart = envMap_.map.phi2(dStart);
			else
				dStart = envMap_.map.phi1(dStart);
		}

938
939
940
941
942
		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
943
			|| markForbid.isMarked(dStop)
944
945
946
947
948
949
950
			; ++j)
		{
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end())
				dStop = envMap_.map.begin() ;
		}

Thomas Jund's avatar
Thomas Jund committed
951
		addPathToObstacle(*it, dStart, dStop, markForbid, goalCenterCell);
952
953
954
	}
}

Thomas Jund's avatar
Thomas Jund committed
955
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal, CellMarker<FACE>& markForbid, bool goalCenterCell)
956
{
David Cazier's avatar
David Cazier committed
957
	std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
958
959
																   envMap_.position, dStart,
																   dGoal,
Thomas Jund's avatar
Thomas Jund committed
960
																   markForbid) ;
961
962
963

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
Thomas Jund's avatar
Thomas Jund committed
964
965
966
967
968
969
970
971
		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 ;
		}
972
973
974

		mo->goals_.push_back(dest) ;
	}
975
976
977
978
979

	path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
												envMap_.position,
												dGoal,
												dStart,
Thomas Jund's avatar
Thomas Jund committed
980
												markForbid) ;
981
982
983

	for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
	{
Thomas Jund's avatar
Thomas Jund committed
984
985
986
987
988
989
990
991
		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 ;
		}
992
993
994

		mo->goals_.push_back(dest) ;
	}
pitiot's avatar
pitiot committed
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012

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