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

Pierre Kraemer's avatar
Pierre Kraemer committed
3
4
5
6
Simulator::Simulator() :
	timeStep_(0.2f),
	globalTime_(0.0f),
	nbSteps_(0)
Pierre Kraemer's avatar
Pierre Kraemer committed
7
{
Pierre Kraemer's avatar
Pierre Kraemer committed
8
	srand(10);
Pierre Kraemer's avatar
Pierre Kraemer committed
9
	nbStepsPerUnit_ = 1 / timeStep_;
Pierre Kraemer's avatar
Pierre Kraemer committed
10
	init(2, 2.0f);
Thomas's avatar
Thomas committed
11
12
13
14
15
16
17
18
19
20
21
}

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
22

Pierre Kraemer's avatar
Pierre Kraemer committed
23
	std::cout << "setup scenario" << std::endl;
Thomas's avatar
Thomas committed
24
25
26

	switch(config)
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
27
	case 0 : setupCircleScenario(100);
Thomas's avatar
Thomas committed
28
29
30
			break;
	case 1 : importAgents("myAgents.pos");
			break;
Pierre Kraemer's avatar
Pierre Kraemer committed
31
	case 2 : setupScenario(10000);
Thomas's avatar
Thomas committed
32
33
34
35
36
37
38
39
40
41
			break;
	case 3 : setupCityScenario(
				-1.0f * (12 * (70.0f / 2.0f) - 10),
				-1.0f * (12 * (70.0f / 2.0f) - 10),
				20,
				20
			);
			break;
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
42
#ifndef SPATIAL_HASHING
Thomas's avatar
Thomas committed
43
44
	if(enablePathFinding)
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
45
		if(dimension == 2.0f)
Thomas's avatar
Thomas committed
46
			addPathsToAgents();
Pierre Kraemer's avatar
Pierre Kraemer committed
47
		else if(dimension == 2.5f)
Thomas's avatar
Thomas committed
48
49
			addPathsToAgents_height();
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
50
#endif
51

Pierre Kraemer's avatar
Pierre Kraemer committed
52
//	setupMovingObstacle(1);
53

54
55
	unsigned nbAgents = agents_.size();

56
57
//	addPathToCorner();

58
	for(unsigned int i = 0; i < nbAgents; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
59
60
61
62
	{
		agents_[i]->updateObstacleNeighbors();
		agents_[i]->updateAgentNeighbors();
	}
63

Pierre Kraemer's avatar
Pierre Kraemer committed
64
65
//	tc1 = new ThreadUpdateInfo(agents_, 0, nbAgents / 4);
//	tc2 = new ThreadUpdateInfo(agents_, nbAgents / 4, nbAgents / 2);
66
67
//	tc3 = new ThreadUpdateInfo(agents_, nbAgents / 2, nbAgents * 3 / 4);
//	tc4 = new ThreadUpdateInfo(agents_, nbAgents * 3 / 4, nbAgents);
Pierre Kraemer's avatar
Pierre Kraemer committed
68
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
69
	envMap_.subdivideToProperLevel();
Pierre Kraemer's avatar
Pierre Kraemer committed
70
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
71
72
73
74
}

void Simulator::doStep()
{
Pierre Kraemer's avatar
Pierre Kraemer committed
75
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
76
	envMap_.clearUpdateCandidates();
Pierre Kraemer's avatar
Pierre Kraemer committed
77
	envMap_.map.setCurrentLevel(0);
Pierre Kraemer's avatar
Pierre Kraemer committed
78
#endif
79

Pierre Kraemer's avatar
Pierre Kraemer committed
80
81
82
83
84
85
86
87
//	for (unsigned int i = 0; i < movingObstacles_.size(); ++i)
//	{
//		movingObstacles_[i]->updateAgentNeighbors();
//		movingObstacles_[i]->computePrefVelocity();
//		movingObstacles_[i]->computeNewVelocity();
//
//		movingObstacles_[i]->update();
//	}
88

Thomas's avatar
Thomas committed
89
90
91
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->updateObstacleNeighbors();
Thomas's avatar
Thomas committed
92

Pierre Kraemer's avatar
Pierre Kraemer committed
93
#ifndef SPATIAL_HASHING
Thomas's avatar
Thomas committed
94
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
Pierre Kraemer's avatar
Pierre Kraemer committed
95
#endif
Thomas's avatar
Thomas committed
96
		agents_[i]->updateAgentNeighbors();
Pierre Kraemer's avatar
Pierre Kraemer committed
97
#ifndef SPATIAL_HASHING
Thomas's avatar
Thomas committed
98
		envMap_.map.setCurrentLevel(0);
Pierre Kraemer's avatar
Pierre Kraemer committed
99
#endif
Thomas's avatar
Thomas committed
100

Thomas's avatar
Thomas committed
101
102
103
		agents_[i]->computePrefVelocity();
		agents_[i]->computeNewVelocity();
	}
104

Thomas's avatar
Thomas committed
105
106
//	boost::thread thread1(&ThreadUpdateInfo::run, tc1);
//	boost::thread thread2(&ThreadUpdateInfo::run, tc2);
Pierre Kraemer's avatar
Pierre Kraemer committed
107
108
109
//	boost::thread thread3(&ThreadUpdateInfo::run, tc3);
//	boost::thread thread4(&ThreadUpdateInfo::run, tc4);
//
Thomas's avatar
Thomas committed
110
111
//	thread1.join();
//	thread2.join();
Pierre Kraemer's avatar
Pierre Kraemer committed
112
113
//	thread3.join();
//	thread4.join();
114

Pierre Kraemer's avatar
Pierre Kraemer committed
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
#ifdef SPATIAL_HASHING
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
		Geom::Vec2ui cold = envMap_.agentPositionCell(agents_[i]->pos) ;
		agents_[i]->update();
		Geom::Vec2ui cnew = envMap_.agentPositionCell(agents_[i]->pos) ;
		if(cnew != cold)
		{
			std::vector<Agent*>& old_agents = envMap_.ht_agents[cold] ;
			removeElementFromVector<Agent*>(old_agents, agents_[i]) ;
			if(old_agents.empty())
				envMap_.ht_agents.erase(cold) ;

			for(int ii = -1; ii <= 1; ++ii)
			{
				for(int jj = -1; jj <= 1; ++jj)
				{
					if(!(ii == 0 && jj == 0))
					{
						Geom::Vec2ui cc = cold + Geom::Vec2ui(ii, jj) ;
						std::vector<Agent*>& v = envMap_.ht_neighbor_agents[cc] ;
						removeElementFromVector<Agent*>(v, agents_[i]) ;
						if(v.empty())
							envMap_.ht_neighbor_agents.erase(cc) ;
					}
				}
			}

			envMap_.ht_agents[cnew].push_back(agents_[i]) ;

			for(int ii = -1; ii <= 1; ++ii)
			{
				for(int jj = -1; jj <= 1; ++jj)
				{
					if(!(ii == 0 && jj == 0))
					{
						Geom::Vec2ui cc = cnew + Geom::Vec2ui(ii, jj) ;
						envMap_.ht_neighbor_agents[cc].push_back(agents_[i]) ;
					}
				}
			}
		}
	}
#else
Pierre Kraemer's avatar
Pierre Kraemer committed
159
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
Pierre Kraemer's avatar
Pierre Kraemer committed
160
161
162
163
164

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

Pierre Kraemer's avatar
Pierre Kraemer committed
166
//		if(envMap_.map.getEmbedding(oldFace, FACE) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE))
Pierre Kraemer's avatar
Pierre Kraemer committed
167
168
169
		if(agents_[i]->part_.crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//		switch(agents_[i]->part_.crossCell)
		{
Pierre Kraemer's avatar
Pierre Kraemer committed
170
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
Pierre Kraemer's avatar
Pierre Kraemer committed
171
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
Pierre Kraemer's avatar
Pierre Kraemer committed
172
173
174
175
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace);
//				break;
Pierre Kraemer's avatar
Pierre Kraemer committed
176
177
		}

Pierre Kraemer's avatar
Pierre Kraemer committed
178
179
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
180
	envMap_.updateMap();
Pierre Kraemer's avatar
Pierre Kraemer committed
181
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
182
183

	globalTime_ += timeStep_;
Pierre Kraemer's avatar
Pierre Kraemer committed
184
	++nbSteps_;
Pierre Kraemer's avatar
Pierre Kraemer committed
185

Pierre Kraemer's avatar
Pierre Kraemer committed
186
//	if(nbSteps % 2000 == 0)
Thomas's avatar
Thomas committed
187
188
189
190
//	{
//		std::cout << "swap goals (globalTime -> " << globalTime_ << ")" << std::endl ;
//		swapAgentsGoals();
//	}
Thomas's avatar
Thomas committed
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
}

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
Pierre Kraemer committed
207
	float r = 200.0f;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
208
209
	for (unsigned int i = 0; i < nbMaxAgent; ++i)
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
		VEC3 posagent(std::cos(i * 2.0f * pi / float(nbMaxAgent)) * r, std::sin(i * 2.0f * pi / float(nbMaxAgent)) * r, 0);

#ifdef SPATIAL_HASHING
		Geom::Vec2ui c = envMap_.agentPositionCell(posagent) ;
		Agent* a = new Agent(this, posagent) ;
		agents_.push_back(a) ;
		envMap_.ht_agents[c].push_back(a) ;
		for(int ii = -1; ii <= 1; ++ii)
		{
			for(int jj = -1; jj <= 1; ++jj)
			{
				if(!(ii == 0 && jj == 0))
				{
					Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
					envMap_.ht_neighbor_agents[cc].push_back(a) ;
				}
			}
		}
#else
Thomas's avatar
Thomas committed
229
230
		Dart dCell = envMap_.getBelongingCell(posagent);
		agents_.push_back(new Agent(this, posagent, dCell));
Pierre Kraemer's avatar
Pierre Kraemer committed
231
232
#endif

Thomas's avatar
Thomas committed
233
234
235
236
237
238
		agents_.back()->goals_.push_back(-1.0f * posagent);
		agents_.back()->curGoal_ = 0;
	}

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

Pierre Kraemer's avatar
Pierre Kraemer committed
239
#ifndef SPATIAL_HASHING
Thomas's avatar
Thomas committed
240
241
	for(unsigned int i = 0; i < agents_.size(); ++i)
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
242
#endif
Thomas's avatar
Thomas committed
243
244
}

Pierre Kraemer's avatar
Pierre Kraemer committed
245
void Simulator::setupCityScenario(float startX, float startY, int nbLines, int nbRank)
Thomas's avatar
Thomas committed
246
{
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
247
248
	float xCornerDown = startX;
	float yCornerDown = startY;
Thomas's avatar
Thomas committed
249

Pierre Kraemer's avatar
Pierre Kraemer committed
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
	VEC3 posagent(xCornerDown - 140,  yCornerDown - 140, 0.0f);
#ifdef SPATIAL_HASHING
	Geom::Vec2ui c = envMap_.agentPositionCell(posagent) ;
	Agent* a = new Agent(this, posagent) ;
	agents_.push_back(a) ;
	envMap_.ht_agents[c].push_back(a) ;
	for(int ii = -1; ii <= 1; ++ii)
	{
		for(int jj = -1; jj <= 1; ++jj)
		{
			if(!(ii == 0 && jj == 0))
			{
				Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
				envMap_.ht_neighbor_agents[cc].push_back(a) ;
			}
		}
	}
#else
268
269
	Dart dCell = envMap_.getBelongingCell(posagent);
	agents_.push_back(new Agent(this, posagent, dCell));
Pierre Kraemer's avatar
Pierre Kraemer committed
270
#endif
271
272
273
	agents_.back()->goals_.push_back(-1.0f * posagent);
	agents_.back()->curGoal_ = 0;

Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
274
	for (int i = 0; i < nbLines; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
275
	{
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
276
277
278
		for (int j = 0; j < nbRank; ++j)
		{
			VEC3 posagent(xCornerDown + i * 10.0f,  yCornerDown + j * 10.0f, 0.0f);
Pierre Kraemer's avatar
Pierre Kraemer committed
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
#ifdef SPATIAL_HASHING
			Geom::Vec2ui c = envMap_.agentPositionCell(posagent) ;
			Agent* a = new Agent(this, posagent) ;
			agents_.push_back(a) ;
			envMap_.ht_agents[c].push_back(a) ;
			for(int ii = -1; ii <= 1; ++ii)
			{
				for(int jj = -1; jj <= 1; ++jj)
				{
					if(!(ii == 0 && jj == 0))
					{
						Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
						envMap_.ht_neighbor_agents[cc].push_back(a) ;
					}
				}
			}
#else
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
296
297
			Dart dCell = envMap_.getBelongingCell(posagent);
			agents_.push_back(new Agent(this, posagent, dCell));
Pierre Kraemer's avatar
Pierre Kraemer committed
298
#endif
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
299
300
301
			agents_.back()->goals_.push_back(-1.0f * posagent);
			agents_.back()->curGoal_ = 0;
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
302
	}
Thomas's avatar
Thomas committed
303
304
305

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

Pierre Kraemer's avatar
Pierre Kraemer committed
306
#ifndef SPATIAL_HASHING
Thomas's avatar
Thomas committed
307
308
	for(unsigned int i = 0; i < agents_.size(); ++i)
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
309
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
310
311
}

312
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
313
{
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
//	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
335
336
337
338
339
	/*
	* 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
340
	CellMarker<FACE> filled(envMap_.map);
Pierre Kraemer's avatar
Pierre Kraemer committed
341

Pierre Kraemer's avatar
Pierre Kraemer committed
342
343
	unsigned int nbx = 5;
	unsigned int nby = 5;
344

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

	for (unsigned int i = 0; i < bMax && d != envMap_.map.end(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
348
349
350
351
352
353
	{
		bool found = false;
		VEC3 pos;
		Dart dCell;
		while(!found && d != envMap_.map.end())
		{
354
			if(!filled.isMarked(d) && !envMap_.buildingMark.isMarked(d) /*&& envMap_.pedWayMark.isMarked(d)*/)
Pierre Kraemer's avatar
Pierre Kraemer committed
355
			{
Thomas's avatar
Thomas committed
356
				filled.mark(d);
Pierre Kraemer's avatar
Pierre Kraemer committed
357
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position);
358
				pos[2] = 0;
Pierre Kraemer's avatar
Pierre Kraemer committed
359
360
361
362
363
364
365
366
367
368
369
				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)
370
			{
Pierre Kraemer's avatar
Pierre Kraemer committed
371
372
373
				for(unsigned int cury = 0; cury < nby; ++cury)
				{
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f);
Pierre Kraemer's avatar
Pierre Kraemer committed
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
#ifdef SPATIAL_HASHING
					Geom::Vec2ui c = envMap_.agentPositionCell(posagent) ;
					Agent* a = new Agent(this, posagent) ;
					agents_.push_back(a) ;
					envMap_.ht_agents[c].push_back(a) ;
					for(int ii = -1; ii <= 1; ++ii)
					{
						for(int jj = -1; jj <= 1; ++jj)
						{
							if(!(ii == 0 && jj == 0))
							{
								Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
								envMap_.ht_neighbor_agents[cc].push_back(a) ;
							}
						}
					}
#else
Pierre Kraemer's avatar
Pierre Kraemer committed
391
					agents_.push_back(new Agent(this, posagent, dCell));
Pierre Kraemer's avatar
Pierre Kraemer committed
392
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
393
					agents_.back()->goals_.push_back(posagent);
Pierre Kraemer's avatar
Pierre Kraemer committed
394
//					agents_.back()->goals_.push_back(VEC3(0,0,0));
Pierre Kraemer's avatar
Pierre Kraemer committed
395
					agents_.back()->goals_.push_back(-1.0f * posagent);
Pierre Kraemer's avatar
Pierre Kraemer committed
396
					agents_.back()->curGoal_ = 1;
Pierre Kraemer's avatar
Pierre Kraemer committed
397
398
399
400
401
402
403
404
				}
			}
		}
	}
	std::cout << "nb agents : " << agents_.size() << std::endl;

	swapAgentsGoals();

Pierre Kraemer's avatar
Pierre Kraemer committed
405
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
406
	for(unsigned int i = 0; i < agents_.size(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
407
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
408
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
409
}
410

Pierre Kraemer's avatar
Pierre Kraemer committed
411
#ifndef SPATIAL_HASHING
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
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
437
438
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
439
	//city
Pierre Kraemer's avatar
Pierre Kraemer committed
440
	unsigned int dartDistForPath = 50;
Pierre Kraemer's avatar
Pierre Kraemer committed
441
442
443
	for(unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->goals_.clear();
444

Pierre Kraemer's avatar
Pierre Kraemer committed
445
446
		Dart dStart = agents_[i]->part_.d;
		Dart dStop = dStart;
447
		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
448
		{
Thomas's avatar
Thomas committed
449
			envMap_.map.next(dStop);
Pierre Kraemer's avatar
Pierre Kraemer committed
450
451
452
			if(dStop == envMap_.map.end())
				dStop = envMap_.map.begin();
		}
453

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
458
		for(std::vector<Dart>::iterator it = path.begin(); it != path.end(); ++it)
459
		{
460
461
462
//			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;
463
			dest[2]=0;
464

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

Pierre Kraemer's avatar
Pierre Kraemer committed
468
		Dart dStop2 = dStop;
469
		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
470
		{
Thomas's avatar
Thomas committed
471
			envMap_.map.next(dStop2);
Pierre Kraemer's avatar
Pierre Kraemer committed
472
473
474
			if(dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin();
		}
475

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
480
		for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
481
		{
482
483
484
//			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;
485
			dest[2]=0;
486

487
488
			agents_[i]->goals_.push_back(dest);
		}
489
490
491

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

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

Pierre Kraemer's avatar
Pierre Kraemer committed
494
		for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
495
		{
496
497
498
//			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;
499
			dest[2]=0;
500

501
502
			agents_[i]->goals_.push_back(dest);
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
503
504
	}
}
505

Thomas's avatar
Thomas committed
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
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
556
#endif
Thomas's avatar
Thomas committed
557

Pierre Kraemer's avatar
Pierre Kraemer committed
558
559
560
561
562
563
564
565
566
567
568
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
569
570
	while ( myfile.good() )
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
571
572
		getline(myfile,line);

Pierre Kraemer's avatar
Pierre Kraemer committed
573
574
		if(line.size() > 1)
		{
Pierre Kraemer's avatar
Pierre Kraemer committed
575
576
577
			std::istringstream iss(line);
			float x,y,z;

Pierre Kraemer's avatar
Pierre Kraemer committed
578
579
580
581
582
			iss >> x;
			iss >> y;
			iss >> z;

			VEC3 pos(x, y, z);
Pierre Kraemer's avatar
Pierre Kraemer committed
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
#ifdef SPATIAL_HASHING
			Geom::Vec2ui c = envMap_.agentPositionCell(pos) ;
			Agent* a = new Agent(this, pos) ;
			agents_.push_back(a) ;
			envMap_.ht_agents[c].push_back(a) ;
			for(int ii = -1; ii <= 1; ++ii)
			{
				for(int jj = -1; jj <= 1; ++jj)
				{
					if(!(ii == 0 && jj == 0))
					{
						Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
						envMap_.ht_neighbor_agents[cc].push_back(a) ;
					}
				}
			}
#else
Pierre Kraemer's avatar
Pierre Kraemer committed
600
601
			Dart dCell = envMap_.getBelongingCell(pos);
			agents_.push_back(new Agent(this, pos, dCell));
Pierre Kraemer's avatar
Pierre Kraemer committed
602
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
603
604
			agents_.back()->goals_.push_back(pos);
			agents_.back()->goals_.push_back(-1.0f * pos);
Pierre Kraemer's avatar
Pierre Kraemer committed
605
			agents_.back()->curGoal_ = 1;
Pierre Kraemer's avatar
Pierre Kraemer committed
606
607
608
		}
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
609
610
	swapAgentsGoals();

Pierre Kraemer's avatar
Pierre Kraemer committed
611
#ifndef SPATIAL_HASHING
Pierre Kraemer's avatar
Pierre Kraemer committed
612
	for (unsigned int i = 0; i < agents_.size(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
613
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
Pierre Kraemer's avatar
Pierre Kraemer committed
614
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
615
616
617
618
619
620
621
622

	myfile.close();
	return true;
}

bool Simulator::exportAgents(std::string filename)
{
	std::ofstream out(filename.c_str(), std::ios::out);
623
624
	if (!out.good())
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
625
626
627
628
		std::cerr << "(export) Unable to open file " << filename << std::endl;
		return false;
	}

629
	for (unsigned int i = 0; i < agents_.size(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
630
631
632
#ifdef SPATIAL_HASHING
		out << agents_[i]->pos << std::endl;
#else
Pierre Kraemer's avatar
Pierre Kraemer committed
633
		out << agents_[i]->part_.m_position << std::endl;
Pierre Kraemer's avatar
Pierre Kraemer committed
634
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
635
636
637
638
639
640
641

	out.close();
	return true;
}

void Simulator::swapAgentsGoals()
{
Pierre Kraemer's avatar
Pierre Kraemer committed
642
643
644
645
	unsigned int nbAgents = agents_.size();
	for(unsigned int i = 0; i < nbAgents; ++i)
	{
		unsigned int r = rand() % nbAgents;
Thomas's avatar
Thomas committed
646
647
648
649
650
651
652
//		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;
//		}

Pierre Kraemer's avatar
Pierre Kraemer committed
653
		std::swap(agents_[i]->goals_, agents_[r]->goals_);
Thomas's avatar
Thomas committed
654
655
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
656

Thomas's avatar
Thomas committed
657
658
659
660
661
662
663
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
664
	}
Thomas's avatar
Thomas committed
665
666

	return bb;
Pierre Kraemer's avatar
Pierre Kraemer committed
667
}
668

Pierre Kraemer's avatar
Pierre Kraemer committed
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
//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);
//}