simulator.cpp 35.4 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);
Thomas Jund's avatar
Thomas Jund committed
83
//			addPathToObstacles(envMap_.pedWayMark, false);
Thomas Jund's avatar
Thomas Jund committed
84
			addPathsToAgents();
Jund Thomas's avatar
Jund Thomas committed
85
			break;
86
87
88
89
		case 6:
#ifdef TWO_AND_HALF_DIM
			envMap_.init(config,200.0,200.0, minSize, 400.0f);
			setupPlanetScenario(nbAgent,nbObst);
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
merge    
pitiot committed
147
148


Arash HABIBI's avatar
unidir    
Arash HABIBI committed
149
	}
150

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

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

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

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

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

212
213
	if (multires)
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
Thomas Jund's avatar
Thomas Jund committed
214

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

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

	}
pitiot's avatar
stats    
pitiot committed
231
232
	clock_gettime(CLOCK_MONOTONIC, &endTime) ;
	time_agent+= timespec_delta(begTime,endTime).tv_nsec;
David Cazier's avatar
David Cazier committed
233
234
235

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

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

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

pitiot's avatar
merging    
pitiot committed
276
277
	{

pitiot's avatar
stats    
pitiot committed
278
279
//		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
280
281
282
	}


283
	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles"<< std::endl ;
David Cazier's avatar
David Cazier committed
284
285
286
287
288
289
290
291
292
293
294

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

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

pitiot's avatar
pitiot committed
332
333
334
	for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
	{
		VEC3 start = positions[i];
Thomas Jund's avatar
Thomas Jund committed
335
336

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

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

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

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

pitiot's avatar
pitiot committed
355
356
		movingObstacles_.push_back(mo4);
	}
pitiot's avatar
pitiot committed
357

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

		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);
380
381
		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
382

383
384
//		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
385
386

		movingObstacles_.push_back(mo4);
David Cazier's avatar
David Cazier committed
387
	}
Thomas's avatar
Thomas committed
388

389
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
390
391
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
392
#endif
pitiot's avatar
merging    
pitiot committed
393
394

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

David Cazier's avatar
David Cazier committed
397
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
Thomas's avatar
Thomas committed
398
{
pitiot's avatar
pitiot committed
399
400
	VEC3 xSide (5.0f,0.0f,0.0f);
	VEC3 ySide (0.0f,10.0f,0.0f);
pitiot's avatar
merging    
pitiot committed
401
402
	if (multires)
	{
pitiot's avatar
pitiot committed
403
//		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
pitiot's avatar
up    
pitiot committed
404
		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
405
406
407
408
	}
	else
	{

pitiot's avatar
stats    
pitiot committed
409
410
//		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
411
412
413
	}


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

David Cazier's avatar
David Cazier committed
452
453
454
455
	// 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
456

David Cazier's avatar
David Cazier committed
457
458
459
460
461
	std::vector<VEC3> vPos;
	MovingObstacle* mo4;

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

pitiot's avatar
pitiot committed
491
		mo4= new MovingObstacle(this, i,vPos,goals, true, false,1);
David Cazier's avatar
David Cazier committed
492
493


494
495
496
497
498
499
500
501
502
503
504
505
506
507
//		//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
508
509
510
511
512

		movingObstacles_.push_back(mo4);
	}
}

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

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

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

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

pitiot's avatar
merging    
pitiot committed
554
		addAgent(start, goal) ;
pitiot's avatar
merge    
pitiot committed
555

Thomas's avatar
Thomas committed
556
557
	}

David Cazier's avatar
David Cazier committed
558
559
560
561
	// 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
562
	yStartDelta = envMap_.geometry.size(1) / 10 ;
David Cazier's avatar
David Cazier committed
563
564

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

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

569
	VEC3 xSide (5.0f,0.0f,0.0f);
570
	VEC3 ySide (0.0f,10.0f,0.0f);
David Cazier's avatar
David Cazier committed
571

Thomas Jund's avatar
Thomas Jund committed
572
	std::cout << "geom : " << envMap_.geometry << std::endl;
pitiot's avatar
merging    
pitiot committed
573
574
575

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

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

pitiot's avatar
merging    
pitiot committed
583
584
585
586
587
588
589
590
591
592
593
594

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


			if ((r==0 || angle(goal-goals[r],goals[r]-goals[r-1])<M_PI/2.0f) )
602
			{
Thomas Jund's avatar
Thomas Jund committed
603
604
605

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

pitiot's avatar
merging    
pitiot committed
606
				goals.push_back(goal);
607
				r++;
pitiot's avatar
merging    
pitiot committed
608
609
			}

David Cazier's avatar
David Cazier committed
610
		}
pitiot's avatar
merging    
pitiot committed
611
612
613
614
615
616
617
618
619
620

		positions[0]=vPos;

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

			start=start-ySide-ySide;

			vPos.clear();

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

pitiot's avatar
merging    
pitiot committed
631
632
		for (int i = 0 ; i < snakeSize ; i++)
			movingObstacles_.push_back(art->members[i]);
David Cazier's avatar
David Cazier committed
633
	}
Thomas's avatar
Thomas committed
634
635
}

David Cazier's avatar
David Cazier committed
636
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
637
{
David Cazier's avatar
David Cazier committed
638
639
640
641
642
	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
643

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

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

Thomas Jund's avatar
Thomas Jund committed
663
664
	unsigned int nbx = 1 ;
	unsigned int nby = 1 ;
David Cazier's avatar
David Cazier committed
665
666
667

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

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

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

706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
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();
}

764
void Simulator::addMovingObstacles(unsigned int nb, unsigned int type)
765
766
767
768
769
770
771
772
773
774
775
776
777
{
	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)
				)
			{
778
				float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
779
780
781
782
783
				if(area>1400)
				{
					dCell = d ;
					found = true ;
				}
784
785
786
787
788
			}
			d = tF.next() ;
		}

		if (found)
789
			addMovingObstacle(dCell,type) ;
790
791
792
793
794
795
796
	}
}

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

799
800
	MovingMesh* mm = NULL;

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

809
#ifdef TWO_AND_HALF_DIM
810
811
812
			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::facePlane<PFP>(envMap_.map, d, envMap_.position));

			VEC3 np;
813
814
815

			VEC3 displ = -xSide-ySide;
			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
816
817
818
819
820

			np = start+displ;
			pl.project(np); //<- without this line, the point is not on the plane
			pl.project(np); //<- without this line, the point is not on the plane (no doublon)
			vPos.push_back(np);
821
822
823

			displ = xSide-ySide;
			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
824
825
826
827
828

			np = start+displ;
			pl.project(np);

			vPos.push_back(np);
829
830
831

			displ = xSide+ySide;
			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
832
833
834
835
836

			np = start+displ;
			pl.project(np);

			vPos.push_back(np);
837
838
839

			displ = ySide-xSide;
			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
840
841
842
843
844

			np = start+displ;
			pl.project(np);

			vPos.push_back(np);
845
#else
846
847
848
			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
849
			vPos.push_back(start-xSide+ySide);
850
851
#endif

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

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

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

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

907
908
	std::vector<VEC3> goals;
	goals.push_back(start);
Thomas Jund's avatar
merge    
Thomas Jund committed
909

pitiot's avatar
pitiot committed
910
	MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), (obstType==0));
911
912

	movingObstacles_.push_back(mo);
913

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

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

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

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

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

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

Thomas Jund's avatar
Thomas Jund committed
954
		addPathToObstacle(*it, dStart, dStop, markForbid, goalCenterCell);
955
956
957
	}
}

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

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

		mo->goals_.push_back(dest) ;
	}
978
979
980
981
982

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

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

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
1018
#ifndef SPATIAL_HASHING
1019
1020
void Simulator::addPathToCorner()
{