simulator.cpp 31.4 KB
Newer Older
Pierre Kraemer's avatar
Pierre Kraemer committed
1
#include "simulator.h"
2
#include <cstdlib>
Pierre Kraemer's avatar
Pierre Kraemer committed
3

David Cazier's avatar
David Cazier committed
4
5
6
7
Simulator::Simulator(int minSize) :
	timeStep_(0.2f),
	globalTime_(0.0f),
	nbSteps_(0),
8
<<<<<<< HEAD
David Cazier's avatar
David Cazier committed
9
10
11
12
13
14
	nbUpdates(0),
	nbSorts(0),
	nbRefineCandidate(0),
	nbCoarsenCandidate(0),
	nearNeighbors(0),
	totalNeighbors(0)
15
=======
pitiot's avatar
pitiot committed
16
17
	avoidance(0),
	nb_dead(0)
18
>>>>>>> 8159c58ab1233ab687d63f815280c094e1a91513
Pierre Kraemer's avatar
Pierre Kraemer committed
19
{
20
21
	srand(10) ;
	nbStepsPerUnit_ = 1 / timeStep_ ;
David Cazier's avatar
David Cazier committed
22
	init(1, minSize, 2.0f) ;
Thomas's avatar
Thomas committed
23
24
25
26
}

Simulator::~Simulator()
{
David Cazier's avatar
David Cazier committed
27
	for (unsigned int i = 0; i < agents_.size(); ++i)
David Cazier's avatar
David Cazier committed
28
		delete agents_[i] ;
Thomas's avatar
Thomas committed
29
30
}

David Cazier's avatar
David Cazier committed
31
void Simulator::init(unsigned int config, int minSize, float dimension, bool enablePathFinding)
Thomas's avatar
Thomas committed
32
{
33
<<<<<<< HEAD
34
	std::cout << "Setup scenario" << std::endl ;
35
=======
Thomas's avatar
Thomas committed
36
	envMap_.init(config);
pitiot's avatar
pitiot committed
37
	setupMovingObstacle(40);
38
>>>>>>> 8159c58ab1233ab687d63f815280c094e1a91513
Pierre Kraemer's avatar
Pierre Kraemer committed
39

David Cazier's avatar
David Cazier committed
40
	envMap_.init(config, 2500.0f, 2000.0f, minSize, 200.0f) ;
Thomas's avatar
Thomas committed
41

David Cazier's avatar
David Cazier committed
42
43
	switch (config)
	{
44
<<<<<<< HEAD
David Cazier's avatar
David Cazier committed
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
		case 0 :
			setupCircleScenario(1000) ;
			break ;
		case 1 :
			setupCorridorScenario(2000) ;
			break ;
		case 2 :
			setupScenario(1000) ;
			break ;
		case 3 :
			setupCityScenario(20, 20) ;
			break ;
		case 4 :
			importAgents("myAgents.pos") ;
			break ;
60
=======
pitiot's avatar
pitiot committed
61
	case 0 : setupCircleScenario(200);
Thomas's avatar
Thomas committed
62
63
64
			break;
	case 1 : importAgents("myAgents.pos");
			break;
pitiot's avatar
pitiot committed
65
	case 2 : setupScenario(1000);
Thomas's avatar
Thomas committed
66
67
68
69
70
71
72
73
			break;
	case 3 : setupCityScenario(
				-1.0f * (12 * (70.0f / 2.0f) - 10),
				-1.0f * (12 * (70.0f / 2.0f) - 10),
				20,
				20
			);
			break;
74
>>>>>>> 8159c58ab1233ab687d63f815280c094e1a91513
Thomas's avatar
Thomas committed
75
76
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
77
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
78
79
80
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
David Cazier's avatar
David Cazier committed
81
		addPathsToAgents() ;
82
		else if (dimension == 2.5f) addPathsToAgents_height() ;
Thomas's avatar
Thomas committed
83
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
84
#endif
85

Pierre Kraemer's avatar
Pierre Kraemer committed
86
//	setupMovingObstacle(1);
87

David Cazier's avatar
David Cazier committed
88
	unsigned nbAgents = agents_.size() ;
89

90
91
//	addPathToCorner();

David Cazier's avatar
David Cazier committed
92
93
	for (unsigned int i = 0; i < nbAgents; ++i)
	{
David Cazier's avatar
David Cazier committed
94
95
		agents_[i]->updateObstacleNeighbors() ;
		agents_[i]->updateAgentNeighbors() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
96
	}
97

Pierre Kraemer's avatar
Pierre Kraemer committed
98
99
//	tc1 = new ThreadUpdateInfo(agents_, 0, nbAgents / 4);
//	tc2 = new ThreadUpdateInfo(agents_, nbAgents / 4, nbAgents / 2);
100
101
//	tc3 = new ThreadUpdateInfo(agents_, nbAgents / 2, nbAgents * 3 / 4);
//	tc4 = new ThreadUpdateInfo(agents_, nbAgents * 3 / 4, nbAgents);
Thomas's avatar
Thomas committed
102

Pierre Kraemer's avatar
Pierre Kraemer committed
103
#ifndef SPATIAL_HASHING
104
<<<<<<< HEAD
105
	envMap_.subdivideToProperLevel() ;
106
=======
107
//	envMap_.subdivideToProperLevel();
108
>>>>>>> 8159c58ab1233ab687d63f815280c094e1a91513
Pierre Kraemer's avatar
Pierre Kraemer committed
109
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
110
111
}

112
void Simulator::setupCircleScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
113
{
114
<<<<<<< HEAD
David Cazier's avatar
David Cazier committed
115
	std::cout << " - Setup Circle Scenario : " << nbMaxAgent << " agents" << std::endl ;
116
	float pi = 3.14159265358979323846f ;
117

118
	float r = 800.0f ;
David Cazier's avatar
David Cazier committed
119
	for (unsigned int i = 0; i < nbMaxAgent; ++i)
120
=======
pitiot's avatar
pitiot committed
121
122
123
124
125
126
127
128
	for (unsigned int i = 0; i < movingObstacles_.size(); ++i)
	{
		movingObstacles_[i]->updateFixedObstNeighbors();
		movingObstacles_[i]->computePrefVelocity();
		movingObstacles_[i]->update();
//		CGoGNout<<"fin update obstacle au temps : "<<nbSteps_<<CGoGNendl;
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
129
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
130
	envMap_.clearUpdateCandidates();
Pierre Kraemer's avatar
Pierre Kraemer committed
131
	envMap_.map.setCurrentLevel(0);
Pierre Kraemer's avatar
Pierre Kraemer committed
132
#endif
133

Thomas's avatar
Thomas committed
134
	for (unsigned int i = 0; i < agents_.size(); ++i)
135
>>>>>>> 8159c58ab1233ab687d63f815280c094e1a91513
David Cazier's avatar
David Cazier committed
136
	{
137
		VEC3 posagent(std::cos(i * 2.0f * pi / float(nbMaxAgent)) * r,
David Cazier's avatar
David Cazier committed
138
		              std::sin(i * 2.0f * pi / float(nbMaxAgent)) * r, 0) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
139

Pierre Kraemer's avatar
Pierre Kraemer committed
140
#ifdef SPATIAL_HASHING
141
142
		Agent* a = new Agent(this, posagent) ;
		agents_.push_back(a) ;
David Cazier's avatar
David Cazier committed
143
		envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
144
#else
145
<<<<<<< HEAD
146
147
		Dart dCell = envMap_.getBelongingCell(posagent) ;
		agents_.push_back(new Agent(this, posagent, dCell)) ;
148
=======
Pierre Kraemer's avatar
Pierre Kraemer committed
149
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
pitiot's avatar
pitiot committed
150
	nb_dead=0;
Pierre Kraemer's avatar
Pierre Kraemer committed
151
152
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
pitiot's avatar
pitiot committed
153
154
155
156
		if (agents_[i]->alive)
			{
			Dart oldFace = agents_[i]->part_.d;

Pierre Kraemer's avatar
Pierre Kraemer committed
157
		agents_[i]->update();
Pierre Kraemer's avatar
Pierre Kraemer committed
158

Pierre Kraemer's avatar
Pierre Kraemer committed
159
//		if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE))
Pierre Kraemer's avatar
Pierre Kraemer committed
160
161
162
		if(agents_[i]->part_.crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//		switch(agents_[i]->part_.crossCell)
		{
Pierre Kraemer's avatar
Pierre Kraemer committed
163
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
Pierre Kraemer's avatar
Pierre Kraemer committed
164
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
Pierre Kraemer's avatar
Pierre Kraemer committed
165
166
167
168
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace);
//				break;
Pierre Kraemer's avatar
Pierre Kraemer committed
169
		}
pitiot's avatar
pitiot committed
170
171
			}
		else nb_dead++;
Pierre Kraemer's avatar
Pierre Kraemer committed
172

Pierre Kraemer's avatar
Pierre Kraemer committed
173
174
	}

175
//	envMap_.updateMap();
pitiot's avatar
pitiot committed
176
//	CGoGNout<<"fin update map temps : "<<nbSteps_<<CGoGNendl;
177
>>>>>>> 8159c58ab1233ab687d63f815280c094e1a91513
Pierre Kraemer's avatar
Pierre Kraemer committed
178
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
179

David Cazier's avatar
David Cazier committed
180
		agents_.back()->goals_.push_back(-1.0f * posagent) ;
181
182
		agents_.back()->curGoal_ = 0 ;
	}
Thomas's avatar
Thomas committed
183

184
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Thomas's avatar
Thomas committed
185

186
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
187
188
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
189
#endif
Thomas's avatar
Thomas committed
190
191
}

192
void Simulator::setupCorridorScenario(unsigned int nbMaxAgent)
Thomas's avatar
Thomas committed
193
{
David Cazier's avatar
David Cazier committed
194
	std::cout << " - Setup Corridor Scenario : " << nbMaxAgent << " agents" << std::endl ;
David Cazier's avatar
David Cazier committed
195
196
197
198
199
200
	int xBorder = 530 ;
	int variationX = 250 ;
	int left = envMap_.geometry.min()[0] + xBorder ;
	int right = envMap_.geometry.max()[0] - xBorder - variationX ;
	int variationY = envMap_.geometry.size(1) * 0.4f ;
	int middle = -variationY / 2 ;
201

202
203
<<<<<<< HEAD
=======
pitiot's avatar
pitiot committed
204
	float r = 220.0f;
205
>>>>>>> 8159c58ab1233ab687d63f815280c094e1a91513
David Cazier's avatar
David Cazier committed
206
207
	for (unsigned int i = 0; i < nbMaxAgent; ++i)
	{
David Cazier's avatar
David Cazier committed
208
209
		VEC3 posagent(left + rand() % variationX, middle + rand() % variationY, 0) ;
		VEC3 posgoal(right + rand() % variationX, middle + rand() % variationY, 0) ;
210
211
		VEC3 tmp ;

David Cazier's avatar
David Cazier committed
212
213
		if (i % 2 == 1)
		{
214
215
216
217
			tmp = posgoal ;
			posgoal = posagent ;
			posagent = tmp ;
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
218
219
220
221

#ifdef SPATIAL_HASHING
		Agent* a = new Agent(this, posagent) ;
		agents_.push_back(a) ;
David Cazier's avatar
David Cazier committed
222
		envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
223
#else
224
225
		Dart dCell = envMap_.getBelongingCell(posagent) ;
		agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
226
227
#endif

228
229
		agents_.back()->goals_.push_back(posgoal) ;
		agents_.back()->curGoal_ = 0 ;
Thomas's avatar
Thomas committed
230
231
	}

David Cazier's avatar
David Cazier committed
232
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Thomas's avatar
Thomas committed
233

Pierre Kraemer's avatar
Pierre Kraemer committed
234
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
235
236
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
237
#endif
Thomas's avatar
Thomas committed
238
239
}

David Cazier's avatar
David Cazier committed
240
void Simulator::setupCityScenario(int nbLines, int nbRank)
Thomas's avatar
Thomas committed
241
{
David Cazier's avatar
David Cazier committed
242
243
244
245
246
	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
247

David Cazier's avatar
David Cazier committed
248
	VEC3 posagent(xCornerDown - xBorder/2, yCornerDown - yBorder/2, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
249
250
251
#ifdef SPATIAL_HASHING
	Agent* a = new Agent(this, posagent) ;
	agents_.push_back(a) ;
David Cazier's avatar
David Cazier committed
252
	envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
253
#else
254
255
	Dart dCell = envMap_.getBelongingCell(posagent) ;
	agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
256
#endif
David Cazier's avatar
David Cazier committed
257
	agents_.back()->goals_.push_back(-1.0f * posagent) ;
258
	agents_.back()->curGoal_ = 0 ;
259

David Cazier's avatar
David Cazier committed
260
261
262
263
	for (int i = 0; i < nbLines; ++i)
	{
		for (int j = 0; j < nbRank; ++j)
		{
264
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
265
266
267
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, posagent) ;
			agents_.push_back(a) ;
David Cazier's avatar
David Cazier committed
268
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
269
#else
270
271
			Dart dCell = envMap_.getBelongingCell(posagent) ;
			agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
272
#endif
David Cazier's avatar
David Cazier committed
273
			agents_.back()->goals_.push_back(-1.0f * posagent) ;
274
			agents_.back()->curGoal_ = 0 ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
275
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
276
	}
Thomas's avatar
Thomas committed
277

David Cazier's avatar
David Cazier committed
278
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Thomas's avatar
Thomas committed
279

Pierre Kraemer's avatar
Pierre Kraemer committed
280
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
281
282
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
283
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
284
285
}

286
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
287
{
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
//	Geom::BoundingBox<PFP::VEC3> bb(envMap_.position.begin());
//	for(unsigned int v = envMap_.position.begin() ; v != envMap_.position.end() ; envMap_.position.next(v))
//		bb.addPoint(envMap_.position[v]);
//	VEC3 posinit = bb.center();
//	posinit[1] = bb.min()[1] + 35.0f;

//	float ecart = bb.size(0)/nbMaxAgent;
//	posinit[0] -= ecart * (nbMaxAgent/2 - 0.5f);
//	for(unsigned int i = 0; i < nbMaxAgent; ++i)
//	{
//		VEC3 posagent = posinit + VEC3(ecart * i, 0.0f, 0.0f);
//		agents_.push_back(new Agent(this, posagent, envMap_.getBelongingCell(posagent)));
//		agents_.back()->goals_.push_back(posagent);
//		VEC3 goal = posagent;
//		goal *= -1.0f;
//		agents_.back()->goals_.push_back(goal);
//		agents_.back()->curGoal_ = 1;
//		agents_.back()->finalGoal = goal;
//		agents_.back()->finalDart = envMap_.getBelongingCell(goal);
//	}

Pierre Kraemer's avatar
Pierre Kraemer committed
309
	/*
310
<<<<<<< HEAD
David Cazier's avatar
David Cazier committed
311
312
313
314
	 * Add agents, specifying their start position, and store their goals on the
	 * opposite side of the environment.
	 */
	Dart d = envMap_.map.begin() ;
David Cazier's avatar
David Cazier committed
315
	CellMarker<FACE> filled(envMap_.map) ;
David Cazier's avatar
David Cazier committed
316
317
318

	unsigned int nbx = 5 ;
	unsigned int nby = 5 ;
319
=======
Pierre Kraemer's avatar
Pierre Kraemer committed
320
321
322
323
	* Add agents, specifying their start position, and store their goals on the
	* opposite side of the environment.
	*/
	Dart d = envMap_.map.begin();
pitiot's avatar
pitiot committed
324
	CellMarker<FACE> filled(envMap_.map);
Pierre Kraemer's avatar
Pierre Kraemer committed
325

pitiot's avatar
pitiot committed
326
327
	unsigned int nbx = (int)envMap_.sideSize/40;
	unsigned int nby = (int)envMap_.sideSize/40;
328
>>>>>>> 8159c58ab1233ab687d63f815280c094e1a91513
David Cazier's avatar
David Cazier committed
329
330
331

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

David Cazier's avatar
David Cazier committed
332
333
	for (unsigned int i = 0; i < bMax && d != envMap_.map.end(); ++i)
	{
David Cazier's avatar
David Cazier committed
334
335
336
		bool found = false ;
		VEC3 pos ;
		Dart dCell ;
David Cazier's avatar
David Cazier committed
337
338
339
340
341
		while (!found && d != envMap_.map.end())
		{
			if (!filled.isMarked(d)
			    && !envMap_.buildingMark.isMarked(d) /*&& envMap_.pedWayMark.isMarked(d)*/)
			{
David Cazier's avatar
David Cazier committed
342
				filled.mark(d) ;
343
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
344
345
346
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
347
			}
David Cazier's avatar
David Cazier committed
348
			envMap_.map.next(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
349
350
		}

David Cazier's avatar
David Cazier committed
351
352
		if (found)
		{
353
354
			float ecart = 3.0f ;
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
355
356
357
358
359
			                    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)
				{
360
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
361
362
363
#ifdef SPATIAL_HASHING
					Agent* a = new Agent(this, posagent) ;
					agents_.push_back(a) ;
David Cazier's avatar
David Cazier committed
364
					envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
365
#else
366
					agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
367
#endif
368
<<<<<<< HEAD
369
					agents_.back()->goals_.push_back(posagent) ;
David Cazier's avatar
David Cazier committed
370
					agents_.back()->goals_.push_back(-1.0f * posagent) ;
371
					agents_.back()->curGoal_ = 1 ;
372
=======
Pierre Kraemer's avatar
Pierre Kraemer committed
373
					agents_.back()->goals_.push_back(posagent);
Pierre Kraemer's avatar
Pierre Kraemer committed
374
//					agents_.back()->goals_.push_back(VEC3(0,0,0));
375

pitiot's avatar
pitiot committed
376
377
					VEC3 oppos = -1.0f * posagent;
					Dart d1 =envMap_.getBelongingCell(oppos);
378
379
380
381
382
383
384
385

					if (!envMap_.buildingMark.isMarked(d1))
						agents_.back()->goals_.push_back(oppos);
					else if (oppos[0]+50 <envMap_.max_for_obstacles && oppos[1]+50 <envMap_.max_for_obstacles)
						agents_.back()->goals_.push_back(oppos + VEC3(50,50,0));
					else
						agents_.back()->goals_.push_back(oppos + VEC3(-50,-50,0));

pitiot's avatar
pitiot committed
386
//					agents_.back()->goals_.push_back( VEC3 (-1.0f*envMap_.max_for_obstacles,-300,0));
Pierre Kraemer's avatar
Pierre Kraemer committed
387
					agents_.back()->curGoal_ = 1;
388
>>>>>>> 8159c58ab1233ab687d63f815280c094e1a91513
Pierre Kraemer's avatar
Pierre Kraemer committed
389
390
391
392
				}
			}
		}
	}
393
<<<<<<< HEAD
David Cazier's avatar
David Cazier committed
394
	std::cout << "nb agents : " << agents_.size() << std::endl ;
395
=======
pitiot's avatar
pitiot committed
396
//	std::cout << "nb agents : " << agents_.size() << std::endl;
397
>>>>>>> 8159c58ab1233ab687d63f815280c094e1a91513
Pierre Kraemer's avatar
Pierre Kraemer committed
398

David Cazier's avatar
David Cazier committed
399
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
400

Pierre Kraemer's avatar
Pierre Kraemer committed
401
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
402
403
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
#endif
}

void Simulator::doStep()
{
#ifndef SPATIAL_HASHING
	envMap_.clearUpdateCandidates() ;
	envMap_.map.setCurrentLevel(0) ;
#endif

//	for (unsigned int i = 0; i < movingObstacles_.size(); ++i)
//	{
//		movingObstacles_[i]->updateAgentNeighbors();
//		movingObstacles_[i]->computePrefVelocity();
//		movingObstacles_[i]->computeNewVelocity();
//
//		movingObstacles_[i]->update();
//	}

David Cazier's avatar
David Cazier committed
423
424
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
		agents_[i]->updateObstacleNeighbors() ;

#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
		agents_[i]->updateAgentNeighbors() ;
#ifndef SPATIAL_HASHING
		envMap_.map.setCurrentLevel(0) ;
#endif

		agents_[i]->computePrefVelocity() ;
		agents_[i]->computeNewVelocity() ;
	}

//	boost::thread thread1(&ThreadUpdateInfo::run, tc1);
//	boost::thread thread2(&ThreadUpdateInfo::run, tc2);
//	boost::thread thread3(&ThreadUpdateInfo::run, tc3);
//	boost::thread thread4(&ThreadUpdateInfo::run, tc4);
//
//	thread1.join();
//	thread2.join();
//	thread3.join();
//	thread4.join();

#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
450
	for (unsigned int i = 0; i < agents_.size(); ++i)
451
	{
David Cazier's avatar
David Cazier committed
452
		Geom::Vec2ui cold = envMap_.agentPositionCell(agents_[i]) ;
453
		agents_[i]->update() ;
David Cazier's avatar
David Cazier committed
454
455
		Geom::Vec2ui cnew = envMap_.agentPositionCell(agents_[i]) ;
		if (cnew != cold)
456
		{
David Cazier's avatar
David Cazier committed
457
458
			envMap_.removeAgentFromGrid(agents_[i], cold) ;
			envMap_.addAgentInGrid(agents_[i], cnew) ;
459
460
461
462
463
		}
	}
#else
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;

David Cazier's avatar
David Cazier committed
464
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
465
	{
466
467
468
469
470
471
		Dart oldFace = agents_[i]->part_.d ;
		agents_[i]->update() ;

//		if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE))
		if (agents_[i]->part_.crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//		switch(agents_[i]->part_.crossCell)
David Cazier's avatar
David Cazier committed
472
		{
473
474
475
476
477
478
479
480
481
482
483
484
485
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
			envMap_.agentChangeFace(agents_[i], oldFace) ;
//				break;
		}

	}

	nbRefineCandidate += envMap_.refineCandidate.size() ;
	nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
	envMap_.updateMap() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
486
#endif
487
488
489
490
491
492
493
494
495
496
497
498
499
500

	globalTime_ += timeStep_ ;
	++nbSteps_ ;

//	if(nbSteps % 2000 == 0)
//	{
//		std::cout << "swap goals (globalTime -> " << globalTime_ << ")" << std::endl ;
//		swapAgentsGoals();
//	}
}

bool Simulator::reachedGoal()
{
	/* Check if all agents have reached their goals. */
David Cazier's avatar
David Cazier committed
501
	for (unsigned int i = 0; i < agents_.size(); ++i)
502
		if ((agents_[i]->getPosition() - agents_[i]->goals_[agents_[i]->curGoal_]).norm2()
David Cazier's avatar
David Cazier committed
503
		    > agents_[i]->radius_ * agents_[i]->radius_) return false ;
504
505

	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
506
}
507

Pierre Kraemer's avatar
Pierre Kraemer committed
508
#ifndef SPATIAL_HASHING
509
510
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
511
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
512
	{
David Cazier's avatar
David Cazier committed
513
514
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
515

David Cazier's avatar
David Cazier committed
516
517
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
518

519
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
520
521
522
			envMap_.position, dStart,
			dStop,
			envMap_.buildingMark) ;
523

David Cazier's avatar
David Cazier committed
524
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
525
		{
526
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
527
528
529
530
531

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

David Cazier's avatar
David Cazier committed
532
			agents_[i]->goals_.push_back(dest) ;
533
534
535
536
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
537
538
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
539
	//city
David Cazier's avatar
David Cazier committed
540
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
541
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
542
	{
David Cazier's avatar
David Cazier committed
543
544
545
546
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
547
548
549
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
			envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
550
		{
David Cazier's avatar
David Cazier committed
551
552
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
553
		}
554

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

557
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
558
559
560
			envMap_.position, dStart,
			dStop,
			envMap_.buildingMark) ;
561

David Cazier's avatar
David Cazier committed
562
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
563
		{
564
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
565
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
566
567
			dest /= 2.0f ;
			dest[2] = 0 ;
568

David Cazier's avatar
David Cazier committed
569
			agents_[i]->goals_.push_back(dest) ;
570
		}
571

David Cazier's avatar
David Cazier committed
572
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
573
574
575
		for (unsigned int j = 0 ; /*!envMap_.pedWayMark.isMarked(dStop) ||*/
			envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
576
		{
David Cazier's avatar
David Cazier committed
577
578
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
579
		}
580

581
582
//		std::cout << "dest2" << Algo::Geometry::faceCentroid<PFP>(envMap_.map, dStop2, envMap_.position) << std::endl;

David Cazier's avatar
David Cazier committed
583
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop, dStop2,
David Cazier's avatar
David Cazier committed
584
			envMap_.buildingMark) ;
585

David Cazier's avatar
David Cazier committed
586
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
587
		{
588
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
589
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
590
591
			dest /= 2.0f ;
			dest[2] = 0 ;
592

David Cazier's avatar
David Cazier committed
593
			agents_[i]->goals_.push_back(dest) ;
594
		}
595

David Cazier's avatar
David Cazier committed
596
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop2, dStart,
David Cazier's avatar
David Cazier committed
597
			envMap_.buildingMark) ;
598

599
600
//		std::cout << "destStart" << Algo::Geometry::faceCentroid<PFP>(envMap_.map, dStart, envMap_.position) << std::endl;

David Cazier's avatar
David Cazier committed
601
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
602
		{
603
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
604
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
605
606
			dest /= 2.0f ;
			dest[2] = 0 ;
607

David Cazier's avatar
David Cazier committed
608
			agents_[i]->goals_.push_back(dest) ;
609
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
610
611
	}
}
612

Thomas's avatar
Thomas committed
613
614
615
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
616
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
617
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
David Cazier's avatar
David Cazier committed
618
	{
David Cazier's avatar
David Cazier committed
619
620
621
622
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
623
624
625
		for (unsigned int j = 0 ;
			envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
626
		{
David Cazier's avatar
David Cazier committed
627
628
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
629
630
		}

631
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
632
633
634
			envMap_.position, dStart,
			dStop,
			envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
635

David Cazier's avatar
David Cazier committed
636
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
637
		{
638
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
639
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
640
641
		}

David Cazier's avatar
David Cazier committed
642
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
643
644
645
		for (unsigned int j = 0 ;
			envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath + rand() * 20
			|| envMap_.map.sameFace(dStop, dStop2) || envMap_.map.sameFace(dStop2, dStart) ; ++j)
David Cazier's avatar
David Cazier committed
646
		{
David Cazier's avatar
David Cazier committed
647
648
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
649
650
		}

David Cazier's avatar
David Cazier committed
651
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop, dStop2,
David Cazier's avatar
David Cazier committed
652
			envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
653

David Cazier's avatar
David Cazier committed
654
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
655
		{
656
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
657
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
658
659
		}

David Cazier's avatar
David Cazier committed
660
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop2, dStart,
David Cazier's avatar
David Cazier committed
661
			envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
662

David Cazier's avatar
David Cazier committed
663
		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
David Cazier's avatar
David Cazier committed
664
		{
665
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
666
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
667
668
669
		}
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
670
#endif
Thomas's avatar
Thomas committed
671

Pierre Kraemer's avatar
Pierre Kraemer committed
672
673
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
674
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
675

David Cazier's avatar
David Cazier committed
676
677
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
678
679
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
680
681
	}

David Cazier's avatar
David Cazier committed
682
683
684
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
685
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
686

David Cazier's avatar
David Cazier committed
687
688
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
689
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
690
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
691

David Cazier's avatar
David Cazier committed
692
693
694
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
695

696
			VEC3 pos(x, y, z) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
697
698
699
#ifdef SPATIAL_HASHING
			Agent* a = new Agent(this, pos) ;
			agents_.push_back(a) ;
David Cazier's avatar
David Cazier committed
700
			envMap_.addAgentInGrid(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
701
#else
702
703
			Dart dCell = envMap_.getBelongingCell(pos) ;
			agents_.push_back(new Agent(this, pos, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
704
#endif
705
			agents_.back()->goals_.push_back(pos) ;
David Cazier's avatar
David Cazier committed
706
			agents_.back()->goals_.push_back(-1.0f * pos) ;
707
			agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
708
709
710
		}
	}

David Cazier's avatar
David Cazier committed
711
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
712

Pierre Kraemer's avatar
Pierre Kraemer committed
713
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
714
715
	for (unsigned int i = 0 ; i < agents_.size() ; ++i)
	envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
716
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
717

David Cazier's avatar
David Cazier committed
718
719
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
720
721
722
723
}

bool Simulator::exportAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
724
	std::ofstream out(filename.c_str(), std::ios::out) ;
David Cazier's avatar
David Cazier committed
725
726
	if (!out.good())
	{
David Cazier's avatar
David Cazier committed
727
728
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
729
730
	}

David Cazier's avatar
David Cazier committed
731
	for (unsigned int i = 0; i < agents_.size(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
732
#ifdef SPATIAL_HASHING
733
		out << agents_[i]->pos << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
734
#else
David Cazier's avatar
David Cazier committed
735
	out << agents_[i]->part_.getPosition() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
736
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
737

David Cazier's avatar
David Cazier committed
738
739
	out.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
740
741
742
743
}

void Simulator::swapAgentsGoals()
{
David Cazier's avatar
David Cazier committed
744
	unsigned int nbAgents = agents_.size() ;
David Cazier's avatar
David Cazier committed
745
746
	for (unsigned int i = 0; i < nbAgents; ++i)
	{
David Cazier's avatar
David Cazier committed
747
		unsigned int r = rand() % nbAgents ;
Thomas's avatar
Thomas committed
748
749
750
751
752
753
754
//		for(unsigned int nbg = 0; nbg < agents_[i]->goals_.size(); ++nbg)
//		{
//			VEC3 g = agents_[i]->goals_[nbg];
//			agents_[i]->goals_[nbg] = agents_[r]->goals_[nbg];
//			agents_[r]->goals_[nbg] = g;
//		}

755
		std::swap(agents_[i]->goals_, agents_[r]->goals_) ;
Thomas's avatar
Thomas committed
756
757
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
758

Thomas's avatar
Thomas committed
759
760
Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
{
David Cazier's avatar
David Cazier committed
761
762
	Geom::BoundingBox<VEC3> bb ;
	unsigned int nbAgents = agents_.size() ;
David Cazier's avatar
David Cazier committed
763
764
	for (unsigned int i = 0; i < nbAgents; ++i)
	{
David Cazier's avatar
David Cazier committed
765
		bb.addPoint(agents_[i]->getPosition()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
766
	}
Thomas's avatar
Thomas committed
767

David Cazier's avatar
David Cazier committed
768
	return bb ;
Pierre Kraemer's avatar
Pierre Kraemer committed
769
}
770

771
<<<<<<< HEAD
Pierre Kraemer's avatar
Pierre Kraemer committed
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
//void Simulator::setupMovingObstacle( unsigned int nbMovingObstacles)
//{
//	float xSize = 10.0f;
//	float ySize = 20.0f;
//
//	VEC3 posInit(30,-420,0);
//
//	std::vector<VEC3> vPos;
//	vPos.push_back(posInit);
//	posInit[0] += xSize;
//	vPos.push_back(posInit);
//	posInit[1] += ySize;
//	vPos.push_back(posInit);
//	posInit[0] -= xSize;
//	vPos.push_back(posInit);
//
//	MovingObstacle* mo = new MovingObstacle(&envMap_, vPos);
//	movingObstacles_.push_back(mo);
//}
791
=======
pitiot's avatar
pitiot committed
792
793
794
795
796
797
798
void Simulator::setupMovingObstacle( unsigned int nb_obst)
{
	float xSize = 10.0f;
	float ySize = 45.0f;
	VEC3 goal (50.0f,50.0f,0);
	VEC3 posInit(0,0,0);
	VEC3 possiblepos(0,0,0);
799
800
801
802
	//	//posInit[0] -= 200.0f;
	//	posInit[1] -= 100.0f;
	//
	//
pitiot's avatar
pitiot committed
803
	std::vector<VEC3> vPos;
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
	//	vPos.push_back(posInit);
	//	posInit[0] -= xSize;
	////	vPos.push_back(posInit);
	//	posInit[1] -= ySize;
	//	vPos.push_back(posInit);
	//	posInit[0] += 2*xSize;
	////	posInit[1] += ySize;
	//	vPos.push_back(posInit);
	////	posInit[0] -= xSize;
	////	posInit[1] += ySize;
	////		vPos.push_back(posInit);
	//
	//
	////	CGoGNout << "ligne "<< i << vPos[i] << CGoGNendl;
	//
	//	MovingObstacle* mo = new MovingObstacle(1,&envMap_, vPos,goal,0);
	//	movingObstacles_.push_back(mo);
	//
	//
	//		goal =PFP::VEC3(-50.0f,50.0f,0);
	//		posInit=PFP::VEC3(0,0,0);
	//		//posInit[0] -= 200.0f;
	//		posInit[1] += 100.0f;
	//
	//
	//		vPos.clear();
	//		vPos.push_back(posInit);
	//		posInit[0] -= xSize;
	//	//	vPos.push_back(posInit);
	//		posInit[1] -= ySize;
	//		vPos.push_back(posInit);
	//		posInit[0] += 2*xSize;
	//	//	posInit[1] += ySize;
	//		vPos.push_back(posInit);
	//	//	posInit[0] -= xSize;
	//	//	posInit[1] += ySize;
	//	//		vPos.push_back(posInit);
	//
	//
	//	//	CGoGNout << "ligne "<< i << vPos[i] << CGoGNendl;
	//
	//		MovingObstacle* mo2 = new MovingObstacle(2,&envMap_, vPos,goal,0);
	//		movingObstacles_.push_back(mo2);

	MovingObstacle* mo4;
	float xx, yy;
	float sx, sy;

	for (unsigned int i =0; i <(nb_obst); i++)
	{
		sx=rand() % 2 +1;
		sy=rand() % 2 +1;
		xx=rand() % (int)envMap_.max_for_obstacles;
		if(sx-1==1)xx*=-1;
		yy=rand() % (int)envMap_.max_for_obstacles;
		if(sy-1==1)yy*=-1;
		VEC3 possiblepos(xx,yy,0);
		Dart d1 =envMap_.getBelongingCell(possiblepos);
		if (envMap_.buildingMark.isMarked(d1))
		{
			if (((possiblepos[0]+50) <envMap_.max_for_obstacles) && ((possiblepos[1]+50) <envMap_.max_for_obstacles)) possiblepos += VEC3(50,50,0);
			else possiblepos += VEC3(-50,-50,0);
		}
		xx=possiblepos[0];
		yy=possiblepos[1];

		goal =PFP::VEC3(xx,-yy,0);

		posInit=PFP::VEC3(0,0,0);
		//posInit[0] -= 200.0f;
		posInit[1] += yy;
		posInit[0] += xx;
		possiblepos=posInit;
		d1 =envMap_.getBelongingCell(possiblepos);
		if (envMap_.buildingMark.isMarked(d1))
		{
			if (((possiblepos[0]+envMap_.sideSize) <envMap_.max_for_obstacles) && ((possiblepos[1]+envMap_.sideSize) <envMap_.max_for_obstacles)) possiblepos += VEC3(envMap_.sideSize,envMap_.sideSize,0);
			else if (((possiblepos[0]-envMap_.sideSize) <envMap_.max_for_obstacles) && ((possiblepos[1]-envMap_.sideSize) <envMap_.max_for_obstacles)) possiblepos += VEC3(-envMap_.sideSize,-envMap_.sideSize,0);
			else if (((possiblepos[0]-envMap_.sideSize) <envMap_.max_for_obstacles) && ((possiblepos[1]+envMap_.sideSize) <envMap_.max_for_obstacles)) possiblepos += VEC3(-envMap_.sideSize,envMap_.sideSize,0);
			else possiblepos += VEC3(envMap_.sideSize,-envMap_.sideSize,0);
		}
		posInit=possiblepos;
		vPos.clear();
		vPos.push_back(posInit);
		//								posInit[0] -= xSize/2;
		//								vPos.push_back(posInit);
		posInit[0] -= xSize;
		posInit[1] -= ySize/2;
		vPos.push_back(posInit);
		posInit[0] += xSize;
		posInit[1] -= ySize/2;
		vPos.push_back(posInit);
		//								posInit[0] += xSize;
		//	posInit[1] += ySize;
		//								vPos.push_back(posInit);
		posInit[0] += xSize;
		posInit[1] += ySize/2;
		vPos.push_back(posInit);
		//								posInit[0] -= xSize;
		//								posInit[1] += ySize/2;
		//								vPos.push_back(posInit);


		//	CGoGNout << "ligne "<< i << vPos[i] << CGoGNendl;

		mo4= new MovingObstacle(i,&envMap_, vPos,goal,0);
		movingObstacles_.push_back(mo4);
	}
	//				xSize = 15.0f;
	//				ySize = 45.0f;
	//				for (unsigned int i =0; i <(2*nb_obst); i++)
	//									{
	//										sx=rand() % 2 +1;
	//										sy=rand() % 2 +1;
	//									xx=rand() % (int)envMap_.max_for_obstacles;
	//									yy=rand() % (int)envMap_.max_for_obstacles;
	//									if(sx-1==1)xx*=-1;
	//									if(sy-1==1)yy*=-1;
	//									goal =PFP::VEC3(-xx,yy,0);
	//
	//												posInit=PFP::VEC3(0,0,0);
	//												//posInit[0] -= 200.0f;
	//												posInit[1] += yy;
	//												posInit[0] += xx;
	//												possiblepos=posInit;
	//												Dart d1 =envMap_.getBelongingCell(possiblepos);
	//												if (envMap_.buildingMark.isMarked(d1))
	//												{
	//												if (((possiblepos[0]+envMap_.sideSize) <envMap_.max_for_obstacles) && ((possiblepos[1]+envMap_.sideSize) <envMap_.max_for_obstacles)) possiblepos += VEC3(envMap_.sideSize,envMap_.sideSize,0);
	//												else if (((possiblepos[0]-envMap_.sideSize) <envMap_.max_for_obstacles) && ((possiblepos[1]-envMap_.sideSize) <envMap_.max_for_obstacles)) possiblepos += VEC3(-envMap_.sideSize,-envMap_.sideSize,0);
	//												else if (((possiblepos[0]-envMap_.sideSize) <envMap_.max_for_obstacles) && ((possiblepos[1]+envMap_.sideSize) <envMap_.max_for_obstacles)) possiblepos += VEC3(-envMap_.sideSize,envMap_.sideSize,0);
	//												else possiblepos += VEC3(envMap_.sideSize,-envMap_.sideSize,0);
	//												}
	//												posInit=possiblepos;
	//
	//												vPos.clear();
	//												vPos.push_back(posInit);
	//												posInit[0] -= xSize/2;
	//											//	vPos.push_back(posInit);
	//												posInit[1] -= ySize/2;
	//												vPos.push_back(posInit);
	//												posInit[0] += xSize;
	//											//	posInit[1] += ySize;
	//												vPos.push_back(posInit);
	//											//	posInit[0] -= xSize;
	//											//	posInit[1] += ySize;
	//											//		vPos.push_back(posInit);
	//
	//
	//											//	CGoGNout << "ligne "<< i << vPos[i] << CGoGNendl;
	//
	//												 mo4= new MovingObstacle(i,&envMap_, vPos,goal,0);
	//												movingObstacles_.push_back(mo4);
	//									}


	//				xx=-45;
	//				yy=245;
	//				goal =PFP::VEC3(xx,-yy,0);
	//
	//							posInit=PFP::VEC3(0,0,0);
	//							//posInit[0] -= 200.0f;
	//							posInit[1] += yy;
	//							posInit[0] += xx;
	//
	//
	//							vPos.clear();
	//							vPos.push_back(posInit);
	//							posInit[0] -= xSize/2;
	//						//	vPos.push_back(posInit);
	//							posInit[1] -= ySize/2;
	//							vPos.push_back(posInit);
	//							posInit[0] += x