simulator.cpp 14.5 KB
Newer Older
Pierre Kraemer's avatar
Pierre Kraemer committed
1
2
#include "simulator.h"

Pierre Kraemer's avatar
Pierre Kraemer committed
3
Simulator::Simulator() : timeStep_(0.2f), globalTime_(0.0f), nbSteps_(0)
Pierre Kraemer's avatar
Pierre Kraemer committed
4
{
Pierre Kraemer's avatar
Pierre Kraemer committed
5
	srand(10);
Pierre Kraemer's avatar
Pierre Kraemer committed
6
7
	nbStepsPerUnit_ = 1 / timeStep_;

Pierre Kraemer's avatar
Pierre Kraemer committed
8
	init(0, 2.0f);
Thomas's avatar
Thomas committed
9
10
11
12
13
14
15
16
17
18
19
}

Simulator::~Simulator()
{
	for (unsigned int i = 0; i < agents_.size(); ++i)
		delete agents_[i];
}

void Simulator::init(unsigned int config, float dimension, bool enablePathFinding)
{
	envMap_.init(config);
Pierre Kraemer's avatar
Pierre Kraemer committed
20

Pierre Kraemer's avatar
Pierre Kraemer committed
21
	std::cout << "setup scenario" << std::endl;
Thomas's avatar
Thomas committed
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41

	switch(config)
	{
	case 0 : setupCircleScenario(1000);
			break;
	case 1 : importAgents("myAgents.pos");
			break;
	case 2 : setupScenario(700);
			break;
	case 3 : setupCityScenario(
				-1.0f * (12 * (70.0f / 2.0f) - 10),
				-1.0f * (12 * (70.0f / 2.0f) - 10),
				20,
				20
			);
			break;
	}

	if(enablePathFinding)
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
42
		if(dimension == 2.0f)
Thomas's avatar
Thomas committed
43
			addPathsToAgents();
Pierre Kraemer's avatar
Pierre Kraemer committed
44
		else if(dimension == 2.5f)
Thomas's avatar
Thomas committed
45
46
			addPathsToAgents_height();
	}
47

Pierre Kraemer's avatar
Pierre Kraemer committed
48
//	setupMovingObstacle(1);
49

50
51
	unsigned nbAgents = agents_.size();

52
53
//	addPathToCorner();

54
	for(unsigned int i = 0; i < nbAgents; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
55
56
57
58
	{
		agents_[i]->updateObstacleNeighbors();
		agents_[i]->updateAgentNeighbors();
	}
59

Pierre Kraemer's avatar
Pierre Kraemer committed
60
61
//	tc1 = new ThreadUpdateInfo(agents_, 0, nbAgents / 4);
//	tc2 = new ThreadUpdateInfo(agents_, nbAgents / 4, nbAgents / 2);
62
63
//	tc3 = new ThreadUpdateInfo(agents_, nbAgents / 2, nbAgents * 3 / 4);
//	tc4 = new ThreadUpdateInfo(agents_, nbAgents * 3 / 4, nbAgents);
Thomas's avatar
Thomas committed
64

Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
65
	envMap_.subdivideToProperLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
66
67
68
69
70
71
}

void Simulator::doStep()
{
	envMap_.clearUpdateCandidates();

Pierre Kraemer's avatar
Pierre Kraemer committed
72
	envMap_.map.setCurrentLevel(0);
73

Pierre Kraemer's avatar
Pierre Kraemer committed
74
75
76
77
78
79
80
81
//	for (unsigned int i = 0; i < movingObstacles_.size(); ++i)
//	{
//		movingObstacles_[i]->updateAgentNeighbors();
//		movingObstacles_[i]->computePrefVelocity();
//		movingObstacles_[i]->computeNewVelocity();
//
//		movingObstacles_[i]->update();
//	}
82

Thomas's avatar
Thomas committed
83
84
85
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->updateObstacleNeighbors();
Thomas's avatar
Thomas committed
86
87

		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
Thomas's avatar
Thomas committed
88
		agents_[i]->updateAgentNeighbors();
Thomas's avatar
Thomas committed
89
90
		envMap_.map.setCurrentLevel(0);

Thomas's avatar
Thomas committed
91
92
93
		agents_[i]->computePrefVelocity();
		agents_[i]->computeNewVelocity();
	}
94

Thomas's avatar
Thomas committed
95
96
//	boost::thread thread1(&ThreadUpdateInfo::run, tc1);
//	boost::thread thread2(&ThreadUpdateInfo::run, tc2);
Pierre Kraemer's avatar
Pierre Kraemer committed
97
98
99
//	boost::thread thread3(&ThreadUpdateInfo::run, tc3);
//	boost::thread thread4(&ThreadUpdateInfo::run, tc4);
//
Thomas's avatar
Thomas committed
100
101
//	thread1.join();
//	thread2.join();
Pierre Kraemer's avatar
Pierre Kraemer committed
102
103
//	thread3.join();
//	thread4.join();
104

Pierre Kraemer's avatar
Pierre Kraemer committed
105
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
Pierre Kraemer's avatar
Pierre Kraemer committed
106
107
108
109

	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
		Dart oldFace = agents_[i]->part_.d;
Pierre Kraemer's avatar
Pierre Kraemer committed
110

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

Pierre Kraemer's avatar
Pierre Kraemer committed
113
//		if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE))
Pierre Kraemer's avatar
Pierre Kraemer committed
114
115
116
		if(agents_[i]->part_.crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//		switch(agents_[i]->part_.crossCell)
		{
Pierre Kraemer's avatar
Pierre Kraemer committed
117
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
Pierre Kraemer's avatar
Pierre Kraemer committed
118
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
Pierre Kraemer's avatar
Pierre Kraemer committed
119
120
121
122
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace);
//				break;
Pierre Kraemer's avatar
Pierre Kraemer committed
123
124
		}

Pierre Kraemer's avatar
Pierre Kraemer committed
125
126
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
127
	envMap_.updateMap();
Pierre Kraemer's avatar
Pierre Kraemer committed
128
129

	globalTime_ += timeStep_;
Pierre Kraemer's avatar
Pierre Kraemer committed
130
	++nbSteps_;
Pierre Kraemer's avatar
Pierre Kraemer committed
131

Pierre Kraemer's avatar
Pierre Kraemer committed
132
//	if(nbSteps % 2000 == 0)
Thomas's avatar
Thomas committed
133
134
135
136
//	{
//		std::cout << "swap goals (globalTime -> " << globalTime_ << ")" << std::endl ;
//		swapAgentsGoals();
//	}
Thomas's avatar
Thomas committed
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
}

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

void Simulator::setupCircleScenario(unsigned int nbMaxAgent)
{
	float pi = 3.14159265358979323846f;

Pierre Kraemer's avatar
update    
Pierre Kraemer committed
153
	float r = 800.0f;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
154
155
	for (unsigned int i = 0; i < nbMaxAgent; ++i)
	{
Thomas's avatar
Thomas committed
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
		VEC3 posagent(std::cos(i * 2.0f * pi / float(nbMaxAgent))*r,std::sin(i * 2.0f * pi / float(nbMaxAgent))*r,0);
		Dart dCell = envMap_.getBelongingCell(posagent);
		agents_.push_back(new Agent(this, posagent, dCell));
		agents_.back()->goals_.push_back(-1.0f * posagent);
		agents_.back()->curGoal_ = 0;
	}

	std::cout << "nb agents : " << agents_.size() << std::endl;

	for(unsigned int i = 0; i < agents_.size(); ++i)
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
}

void Simulator::setupCityScenario(float startX, float startY, int nbLines , int nbRank)
{
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
171
172
	float xCornerDown = startX;
	float yCornerDown = startY;
Thomas's avatar
Thomas committed
173

174
175
176
177
178
179
	VEC3 posagent(xCornerDown-140,  yCornerDown-140, 0.0f);
	Dart dCell = envMap_.getBelongingCell(posagent);
	agents_.push_back(new Agent(this, posagent, dCell));
	agents_.back()->goals_.push_back(-1.0f * posagent);
	agents_.back()->curGoal_ = 0;

Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
180
	for (int i = 0; i < nbLines; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
181
	{
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
182
183
184
185
186
187
188
189
		for (int j = 0; j < nbRank; ++j)
		{
			VEC3 posagent(xCornerDown + i * 10.0f,  yCornerDown + j * 10.0f, 0.0f);
			Dart dCell = envMap_.getBelongingCell(posagent);
			agents_.push_back(new Agent(this, posagent, dCell));
			agents_.back()->goals_.push_back(-1.0f * posagent);
			agents_.back()->curGoal_ = 0;
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
190
	}
Thomas's avatar
Thomas committed
191
192
193
194
195

	std::cout << "nb agents : " << agents_.size() << std::endl;

	for(unsigned int i = 0; i < agents_.size(); ++i)
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
196
197
}

198
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
199
{
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
//	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
221
222
223
224
225
	/*
	* Add agents, specifying their start position, and store their goals on the
	* opposite side of the environment.
	*/
	Dart d = envMap_.map.begin();
Pierre Kraemer's avatar
Pierre Kraemer committed
226
	CellMarker filled(envMap_.map,FACE);
Pierre Kraemer's avatar
Pierre Kraemer committed
227

Pierre Kraemer's avatar
Pierre Kraemer committed
228
229
	unsigned int nbx = 5;
	unsigned int nby = 5;
230

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

	for (unsigned int i = 0; i < bMax && d != envMap_.map.end(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
234
235
236
237
238
239
	{
		bool found = false;
		VEC3 pos;
		Dart dCell;
		while(!found && d != envMap_.map.end())
		{
240
			if(!filled.isMarked(d) && !envMap_.buildingMark.isMarked(d) /*&& envMap_.pedWayMark.isMarked(d)*/)
Pierre Kraemer's avatar
Pierre Kraemer committed
241
			{
Thomas's avatar
Thomas committed
242
				filled.mark(d);
Pierre Kraemer's avatar
Pierre Kraemer committed
243
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position);
244
				pos[2] = 0;
Pierre Kraemer's avatar
Pierre Kraemer committed
245
246
247
248
249
250
251
252
253
254
255
				dCell = d;
				found = true;
			}
			envMap_.map.next(d);
		}

		if(found)
		{
			float ecart = 3.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)
256
			{
Pierre Kraemer's avatar
Pierre Kraemer committed
257
258
259
260
261
				for(unsigned int cury = 0; cury < nby; ++cury)
				{
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f);
					agents_.push_back(new Agent(this, posagent, dCell));
					agents_.back()->goals_.push_back(posagent);
262
//				agents_.back()->goals_.push_back(VEC3(0,0,0));
Pierre Kraemer's avatar
Pierre Kraemer committed
263
					agents_.back()->goals_.push_back(-1.0f * posagent);
Pierre Kraemer's avatar
Pierre Kraemer committed
264
					agents_.back()->curGoal_ = 1;
Pierre Kraemer's avatar
Pierre Kraemer committed
265
266
267
268
269
270
271
272
273
				}
			}
		}
	}
	std::cout << "nb agents : " << agents_.size() << std::endl;

	swapAgentsGoals();

	for(unsigned int i = 0; i < agents_.size(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
274
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
275
}
276

277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
void Simulator::addPathToCorner()
{
	for(unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->goals_.clear();
		agents_.back()->curGoal_ = 1;

		Dart dStart =  agents_[i]->part_.d;
		Dart dStop = agents_[i]->finalDart;

		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStart, dStop, envMap_.buildingMark);

		for(std::vector<Dart>::iterator it = path.begin(); it != path.end(); ++it)
		{
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);

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

			agents_[i]->goals_.push_back(dest);
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
302
303
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
304
	//city
Pierre Kraemer's avatar
Pierre Kraemer committed
305
	unsigned int dartDistForPath = 50;
Pierre Kraemer's avatar
Pierre Kraemer committed
306
307
308
	for(unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->goals_.clear();
309

Pierre Kraemer's avatar
Pierre Kraemer committed
310
311
		Dart dStart = agents_[i]->part_.d;
		Dart dStop = dStart;
312
		for(unsigned int j = 0; /*!envMap_.pedWayMark.isMarked(dStop) ||*/ envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath+rand()*20 || envMap_.map.sameFace(dStop,dStart); ++j)
Pierre Kraemer's avatar
Pierre Kraemer committed
313
		{
Thomas's avatar
Thomas committed
314
			envMap_.map.next(dStop);
Pierre Kraemer's avatar
Pierre Kraemer committed
315
316
317
			if(dStop == envMap_.map.end())
				dStop = envMap_.map.begin();
		}
318

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

321
322
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStart, dStop, envMap_.buildingMark);

Pierre Kraemer's avatar
Pierre Kraemer committed
323
		for(std::vector<Dart>::iterator it = path.begin(); it != path.end(); ++it)
324
		{
325
326
327
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)];
			dest /= 2.0f;
328
			dest[2]=0;
329

330
331
			agents_[i]->goals_.push_back(dest);
		}
332

Pierre Kraemer's avatar
Pierre Kraemer committed
333
		Dart dStop2 = dStop;
334
		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)
Pierre Kraemer's avatar
Pierre Kraemer committed
335
		{
Thomas's avatar
Thomas committed
336
			envMap_.map.next(dStop2);
Pierre Kraemer's avatar
Pierre Kraemer committed
337
338
339
			if(dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin();
		}
340

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

343
344
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop, dStop2, envMap_.buildingMark);

Pierre Kraemer's avatar
Pierre Kraemer committed
345
		for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
346
		{
347
348
349
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)];
			dest /= 2.0f;
350
			dest[2]=0;
351

352
353
			agents_[i]->goals_.push_back(dest);
		}
354
355
356

		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop2, dStart, envMap_.buildingMark);

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

Pierre Kraemer's avatar
Pierre Kraemer committed
359
		for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
360
		{
361
362
363
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)];
			dest /= 2.0f;
364
			dest[2]=0;
365

366
367
			agents_[i]->goals_.push_back(dest);
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
368
369
	}
}
370

Thomas's avatar
Thomas committed
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
void Simulator::addPathsToAgents_height()
{
	//city
	unsigned int dartDistForPath = 50;
	for(unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->goals_.clear();

		Dart dStart = agents_[i]->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();
		}

		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStart, dStop, envMap_.buildingMark);

		for(std::vector<Dart>::iterator it = path.begin(); it != path.end(); ++it)
		{
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
			agents_[i]->goals_.push_back(dest);
		}

		Dart dStop2 = dStop;
		for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath+rand()*20 || envMap_.map.sameFace(dStop,dStop2) || envMap_.map.sameFace(dStop2,dStart); ++j)
		{
			envMap_.map.next(dStop2);
			if(dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin();
		}

		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop, dStop2, envMap_.buildingMark);

		for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
		{
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
			agents_[i]->goals_.push_back(dest);
		}

		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop2, dStart, envMap_.buildingMark);

		for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
		{
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
			agents_[i]->goals_.push_back(dest);
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
422
423
424
425
426
427
428
429
430
431
432
bool Simulator::importAgents(std::string filename)
{
	std::ifstream myfile(filename.c_str(), std::ios::in);

	if (!myfile.good())
	{
		std::cerr << "(export) Unable to open file " << filename << std::endl;
		return false;
	}

	std::string line,token;
Pierre Kraemer's avatar
Pierre Kraemer committed
433
434
	while ( myfile.good() )
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
435
436
		getline(myfile,line);

Pierre Kraemer's avatar
Pierre Kraemer committed
437
438
		if(line.size() > 1)
		{
Pierre Kraemer's avatar
Pierre Kraemer committed
439
440
441
			std::istringstream iss(line);
			float x,y,z;

Pierre Kraemer's avatar
Pierre Kraemer committed
442
443
444
445
446
			iss >> x;
			iss >> y;
			iss >> z;

			VEC3 pos(x, y, z);
Pierre Kraemer's avatar
Pierre Kraemer committed
447
448
449
450
451
			Dart dCell = envMap_.getBelongingCell(pos);

			agents_.push_back(new Agent(this, pos, dCell));
			agents_.back()->goals_.push_back(pos);
			agents_.back()->goals_.push_back(-1.0f * pos);
Pierre Kraemer's avatar
Pierre Kraemer committed
452
			agents_.back()->curGoal_ = 1;
Pierre Kraemer's avatar
Pierre Kraemer committed
453
454
455
		}
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
456
457
	swapAgentsGoals();

Pierre Kraemer's avatar
Pierre Kraemer committed
458
	for (unsigned int i = 0; i < agents_.size(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
459
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
460
461
462
463
464
465
466
467

	myfile.close();
	return true;
}

bool Simulator::exportAgents(std::string filename)
{
	std::ofstream out(filename.c_str(), std::ios::out);
468
469
	if (!out.good())
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
470
471
472
473
		std::cerr << "(export) Unable to open file " << filename << std::endl;
		return false;
	}

474
	for (unsigned int i = 0; i < agents_.size(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
475
476
477
478
479
480
481
482
		out << agents_[i]->part_.m_position << std::endl;

	out.close();
	return true;
}

void Simulator::swapAgentsGoals()
{
Pierre Kraemer's avatar
Pierre Kraemer committed
483
484
485
486
	unsigned int nbAgents = agents_.size();
	for(unsigned int i = 0; i < nbAgents; ++i)
	{
		unsigned int r = rand() % nbAgents;
Thomas's avatar
Thomas committed
487
488
489
490
491
492
493
494
495
496
//		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;
//		}

		std::swap(agents_[i]->goals_,agents_[r]->goals_);
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
497

Thomas's avatar
Thomas committed
498
499
500
501
502
503
504
Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
{
	Geom::BoundingBox<VEC3> bb(agents_[0]->getPosition());
	unsigned int nbAgents = agents_.size();
	for(unsigned int i = 1; i < nbAgents; ++i)
	{
		bb.addPoint(agents_[i]->getPosition());
Pierre Kraemer's avatar
Pierre Kraemer committed
505
	}
Thomas's avatar
Thomas committed
506
507

	return bb;
Pierre Kraemer's avatar
Pierre Kraemer committed
508
}
509

510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
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);
}