simulator.cpp 21.2 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
8
9
10
11
12
13
Simulator::Simulator(int minSize) :
	timeStep_(0.2f),
	globalTime_(0.0f),
	nbSteps_(0),
	nbUpdates(0),
	nbSorts(0),
	nbRefineCandidate(0),
	nbCoarsenCandidate(0),
	nearNeighbors(0),
	totalNeighbors(0)
Pierre Kraemer's avatar
Pierre Kraemer committed
14
{
15
16
	srand(10) ;
	nbStepsPerUnit_ = 1 / timeStep_ ;
David Cazier's avatar
David Cazier committed
17
	init(1, minSize, 2.0f) ;
Thomas's avatar
Thomas committed
18
19
20
21
}

Simulator::~Simulator()
{
David Cazier's avatar
David Cazier committed
22
	for (unsigned int i = 0; i < agents_.size(); ++i)
David Cazier's avatar
David Cazier committed
23
		delete agents_[i] ;
Thomas's avatar
Thomas committed
24
25
}

David Cazier's avatar
David Cazier committed
26
void Simulator::init(unsigned int config, int minSize, float dimension, bool enablePathFinding)
Thomas's avatar
Thomas committed
27
{
28
	std::cout << "Setup scenario" << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
29

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

David Cazier's avatar
David Cazier committed
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
	switch (config)
	{
	case 0 :
		setupCircleScenario(1000) ;
		break ;
	case 1 :
		setupCorridorScenario(5000) ;
		break ;
	case 2 :
		setupScenario(1000) ;
		break ;
	case 3 :
		setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10), -1.0f * (12 * (70.0f / 2.0f) - 10),
		                  20, 20) ;
		break ;
	case 4 :
		importAgents("myAgents.pos") ;
		break ;
Thomas's avatar
Thomas committed
50
51
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
52
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
53
54
55
56
	if (enablePathFinding)
	{
		if (dimension == 2.0f)
			addPathsToAgents() ;
57
		else if (dimension == 2.5f) addPathsToAgents_height() ;
Thomas's avatar
Thomas committed
58
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
59
#endif
60

Pierre Kraemer's avatar
Pierre Kraemer committed
61
//	setupMovingObstacle(1);
62

David Cazier's avatar
David Cazier committed
63
	unsigned nbAgents = agents_.size() ;
64

65
66
//	addPathToCorner();

David Cazier's avatar
David Cazier committed
67
68
	for (unsigned int i = 0; i < nbAgents; ++i)
	{
David Cazier's avatar
David Cazier committed
69
70
		agents_[i]->updateObstacleNeighbors() ;
		agents_[i]->updateAgentNeighbors() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
71
	}
72

Pierre Kraemer's avatar
Pierre Kraemer committed
73
74
//	tc1 = new ThreadUpdateInfo(agents_, 0, nbAgents / 4);
//	tc2 = new ThreadUpdateInfo(agents_, nbAgents / 4, nbAgents / 2);
75
76
//	tc3 = new ThreadUpdateInfo(agents_, nbAgents / 2, nbAgents * 3 / 4);
//	tc4 = new ThreadUpdateInfo(agents_, nbAgents * 3 / 4, nbAgents);
Thomas's avatar
Thomas committed
77

Pierre Kraemer's avatar
Pierre Kraemer committed
78
#ifndef SPATIAL_HASHING
79
	envMap_.subdivideToProperLevel() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
80
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
81
82
}

83
void Simulator::setupCircleScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
84
{
David Cazier's avatar
David Cazier committed
85
	std::cout << " - Setup Circle Scenario : " << nbMaxAgent << " agents" << std::endl ;
86
	float pi = 3.14159265358979323846f ;
87

88
	float r = 800.0f ;
David Cazier's avatar
David Cazier committed
89
90
	for (unsigned int i = 0; i < nbMaxAgent; ++i)
	{
91
		VEC3 posagent(std::cos(i * 2.0f * pi / float(nbMaxAgent)) * r,
David Cazier's avatar
David Cazier committed
92
		              std::sin(i * 2.0f * pi / float(nbMaxAgent)) * r, 0) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
93

Pierre Kraemer's avatar
Pierre Kraemer committed
94
#ifdef SPATIAL_HASHING
95
96
97
98
99
		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)
Pierre Kraemer's avatar
Pierre Kraemer committed
100
		{
101
			for(int jj = -1 ; jj <= 1 ; ++jj)
Pierre Kraemer's avatar
Pierre Kraemer committed
102
			{
103
				if(!(ii == 0 && jj == 0))
Pierre Kraemer's avatar
Pierre Kraemer committed
104
				{
105
106
					Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
					envMap_.ht_neighbor_agents[cc].push_back(a) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
107
108
109
110
				}
			}
		}
#else
111
112
		Dart dCell = envMap_.getBelongingCell(posagent) ;
		agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
113
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
114

David Cazier's avatar
David Cazier committed
115
		agents_.back()->goals_.push_back(-1.0f * posagent) ;
116
117
		agents_.back()->curGoal_ = 0 ;
	}
Thomas's avatar
Thomas committed
118

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

121
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
122
	for (unsigned int i = 0; i < agents_.size(); ++i)
123
124
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
#endif
Thomas's avatar
Thomas committed
125
126
}

127
void Simulator::setupCorridorScenario(unsigned int nbMaxAgent)
Thomas's avatar
Thomas committed
128
{
David Cazier's avatar
David Cazier committed
129
	std::cout << " - Setup Corridor Scenario : " << nbMaxAgent << " agents" << std::endl ;
130
131
132
133
134
135
	float left = -600.0f ;
	float variationX = 120.0f ;
	float right = 600.0f ;
	float middle = 0.0f ;
	float variationY = 400.0f ;

David Cazier's avatar
David Cazier committed
136
137
138
139
140
141
	for (unsigned int i = 0; i < nbMaxAgent; ++i)
	{
		VEC3 posagent(left + std::cos(rand()) * variationX, middle + std::sin(rand()) * variationY,
		              0) ;
		VEC3 posgoal(right + std::cos(rand()) * variationX, middle + std::sin(rand()) * variationY,
		             0) ;
142
143
		VEC3 tmp ;

David Cazier's avatar
David Cazier committed
144
145
		if (i % 2 == 1)
		{
146
147
148
149
			tmp = posgoal ;
			posgoal = posagent ;
			posagent = tmp ;
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
150
151
152
153
154
155

#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) ;
156
		for(int ii = -1 ; ii <= 1 ; ++ii)
Pierre Kraemer's avatar
Pierre Kraemer committed
157
		{
158
			for(int jj = -1 ; jj <= 1 ; ++jj)
Pierre Kraemer's avatar
Pierre Kraemer committed
159
160
161
162
163
164
165
166
167
			{
				if(!(ii == 0 && jj == 0))
				{
					Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
					envMap_.ht_neighbor_agents[cc].push_back(a) ;
				}
			}
		}
#else
168
169
		Dart dCell = envMap_.getBelongingCell(posagent) ;
		agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
170
171
#endif

172
173
		agents_.back()->goals_.push_back(posgoal) ;
		agents_.back()->curGoal_ = 0 ;
Thomas's avatar
Thomas committed
174
175
	}

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

Pierre Kraemer's avatar
Pierre Kraemer committed
178
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
179
	for (unsigned int i = 0; i < agents_.size(); ++i)
180
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
181
#endif
Thomas's avatar
Thomas committed
182
183
}

David Cazier's avatar
David Cazier committed
184
void Simulator::setupCityScenario(float startX, float startY, int nbLines, int nbRank)
Thomas's avatar
Thomas committed
185
{
186
187
	float xCornerDown = startX ;
	float yCornerDown = startY ;
Thomas's avatar
Thomas committed
188

189
	VEC3 posagent(xCornerDown - 140, yCornerDown - 140, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
190
191
192
193
194
#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) ;
195
	for(int ii = -1 ; ii <= 1 ; ++ii)
Pierre Kraemer's avatar
Pierre Kraemer committed
196
	{
197
		for(int jj = -1 ; jj <= 1 ; ++jj)
Pierre Kraemer's avatar
Pierre Kraemer committed
198
199
200
201
202
203
204
205
206
		{
			if(!(ii == 0 && jj == 0))
			{
				Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
				envMap_.ht_neighbor_agents[cc].push_back(a) ;
			}
		}
	}
#else
207
208
	Dart dCell = envMap_.getBelongingCell(posagent) ;
	agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
209
#endif
David Cazier's avatar
David Cazier committed
210
	agents_.back()->goals_.push_back(-1.0f * posagent) ;
211
	agents_.back()->curGoal_ = 0 ;
212

David Cazier's avatar
David Cazier committed
213
214
215
216
	for (int i = 0; i < nbLines; ++i)
	{
		for (int j = 0; j < nbRank; ++j)
		{
217
			VEC3 posagent(xCornerDown + i * 10.0f, yCornerDown + j * 10.0f, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
218
219
220
221
222
#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) ;
223
			for(int ii = -1 ; ii <= 1 ; ++ii)
Pierre Kraemer's avatar
Pierre Kraemer committed
224
			{
225
				for(int jj = -1 ; jj <= 1 ; ++jj)
Pierre Kraemer's avatar
Pierre Kraemer committed
226
227
228
229
230
231
232
233
234
				{
					if(!(ii == 0 && jj == 0))
					{
						Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
						envMap_.ht_neighbor_agents[cc].push_back(a) ;
					}
				}
			}
#else
235
236
			Dart dCell = envMap_.getBelongingCell(posagent) ;
			agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
237
#endif
David Cazier's avatar
David Cazier committed
238
			agents_.back()->goals_.push_back(-1.0f * posagent) ;
239
			agents_.back()->curGoal_ = 0 ;
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
240
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
241
	}
Thomas's avatar
Thomas committed
242

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

Pierre Kraemer's avatar
Pierre Kraemer committed
245
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
246
	for (unsigned int i = 0; i < agents_.size(); ++i)
247
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
248
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
249
250
}

251
void Simulator::setupScenario(unsigned int nbMaxAgent)
Pierre Kraemer's avatar
Pierre Kraemer committed
252
{
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
//	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
274
	/*
David Cazier's avatar
David Cazier committed
275
276
277
278
279
280
281
282
283
284
285
	 * Add agents, specifying their start position, and store their goals on the
	 * opposite side of the environment.
	 */
	Dart d = envMap_.map.begin() ;
	CellMarker filled(envMap_.map, FACE) ;

	unsigned int nbx = 5 ;
	unsigned int nby = 5 ;

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

David Cazier's avatar
David Cazier committed
286
287
	for (unsigned int i = 0; i < bMax && d != envMap_.map.end(); ++i)
	{
David Cazier's avatar
David Cazier committed
288
289
290
		bool found = false ;
		VEC3 pos ;
		Dart dCell ;
David Cazier's avatar
David Cazier committed
291
292
293
294
295
		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
296
				filled.mark(d) ;
297
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
David Cazier's avatar
David Cazier committed
298
299
300
				pos[2] = 0 ;
				dCell = d ;
				found = true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
301
			}
David Cazier's avatar
David Cazier committed
302
			envMap_.map.next(d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
303
304
		}

David Cazier's avatar
David Cazier committed
305
306
		if (found)
		{
307
308
			float ecart = 3.0f ;
			VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
David Cazier's avatar
David Cazier committed
309
310
311
312
313
			                    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)
				{
314
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
315
316
317
318
319
#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) ;
320
					for(int ii = -1 ; ii <= 1 ; ++ii)
Pierre Kraemer's avatar
Pierre Kraemer committed
321
					{
322
						for(int jj = -1 ; jj <= 1 ; ++jj)
Pierre Kraemer's avatar
Pierre Kraemer committed
323
324
325
326
327
328
329
330
331
						{
							if(!(ii == 0 && jj == 0))
							{
								Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
								envMap_.ht_neighbor_agents[cc].push_back(a) ;
							}
						}
					}
#else
332
					agents_.push_back(new Agent(this, posagent, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
333
#endif
334
					agents_.back()->goals_.push_back(posagent) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
335
//					agents_.back()->goals_.push_back(VEC3(0,0,0));
David Cazier's avatar
David Cazier committed
336
					agents_.back()->goals_.push_back(-1.0f * posagent) ;
337
					agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
338
339
340
341
				}
			}
		}
	}
David Cazier's avatar
David Cazier committed
342
	std::cout << "nb agents : " << agents_.size() << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
343

David Cazier's avatar
David Cazier committed
344
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
345

Pierre Kraemer's avatar
Pierre Kraemer committed
346
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
347
	for (unsigned int i = 0; i < agents_.size(); ++i)
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
#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
368
369
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
370
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
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
		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
	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
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;

David Cazier's avatar
David Cazier committed
440
441
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
442
443
444
445
446
447
		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
448
		{
449
450
451
452
453
454
455
456
457
458
459
460
461
//			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
462
#endif
463
464
465
466
467
468
469
470
471
472
473
474
475
476

	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
477
	for (unsigned int i = 0; i < agents_.size(); ++i)
478
		if ((agents_[i]->getPosition() - agents_[i]->goals_[agents_[i]->curGoal_]).norm2()
David Cazier's avatar
David Cazier committed
479
		    > agents_[i]->radius_ * agents_[i]->radius_) return false ;
480
481

	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
482
}
483

Pierre Kraemer's avatar
Pierre Kraemer committed
484
#ifndef SPATIAL_HASHING
485
486
void Simulator::addPathToCorner()
{
David Cazier's avatar
David Cazier committed
487
488
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
David Cazier's avatar
David Cazier committed
489
490
		agents_[i]->goals_.clear() ;
		agents_.back()->curGoal_ = 1 ;
491

David Cazier's avatar
David Cazier committed
492
493
		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = agents_[i]->finalDart ;
494

495
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
496
497
498
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
499

David Cazier's avatar
David Cazier committed
500
501
		for (std::vector<Dart>::iterator it = path.begin(); it != path.end(); ++it)
		{
502
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
503
504
505
506
507

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

David Cazier's avatar
David Cazier committed
508
			agents_[i]->goals_.push_back(dest) ;
509
510
511
512
		}
	}
}

Pierre Kraemer's avatar
Pierre Kraemer committed
513
514
void Simulator::addPathsToAgents()
{
Thomas's avatar
Thomas committed
515
	//city
David Cazier's avatar
David Cazier committed
516
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
517
518
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
David Cazier's avatar
David Cazier committed
519
520
521
522
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
523
524
525
526
		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
527
528
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
529
		}
530

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

533
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
534
535
536
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
537

David Cazier's avatar
David Cazier committed
538
539
		for (std::vector<Dart>::iterator it = path.begin(); it != path.end(); ++it)
		{
540
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
541
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
542
543
			dest /= 2.0f ;
			dest[2] = 0 ;
544

David Cazier's avatar
David Cazier committed
545
			agents_[i]->goals_.push_back(dest) ;
546
		}
547

David Cazier's avatar
David Cazier committed
548
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
549
550
551
552
		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
553
554
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
555
		}
556

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

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

David Cazier's avatar
David Cazier committed
562
563
		for (std::vector<Dart>::iterator it = path.begin(); it != path.end(); ++it)
		{
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
573
		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop2, dStart,
		                                             envMap_.buildingMark) ;
574

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

David Cazier's avatar
David Cazier committed
577
578
		for (std::vector<Dart>::iterator it = path.begin(); it != path.end(); ++it)
		{
579
//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
David Cazier's avatar
David Cazier committed
580
			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
David Cazier's avatar
David Cazier committed
581
582
			dest /= 2.0f ;
			dest[2] = 0 ;
583

David Cazier's avatar
David Cazier committed
584
			agents_[i]->goals_.push_back(dest) ;
585
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
586
587
	}
}
588

Thomas's avatar
Thomas committed
589
590
591
void Simulator::addPathsToAgents_height()
{
	//city
David Cazier's avatar
David Cazier committed
592
	unsigned int dartDistForPath = 50 ;
David Cazier's avatar
David Cazier committed
593
594
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
David Cazier's avatar
David Cazier committed
595
596
597
598
		agents_[i]->goals_.clear() ;

		Dart dStart = agents_[i]->part_.d ;
		Dart dStop = dStart ;
David Cazier's avatar
David Cazier committed
599
600
601
602
		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
603
604
			envMap_.map.next(dStop) ;
			if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
605
606
		}

607
		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
David Cazier's avatar
David Cazier committed
608
609
610
		                                                               envMap_.position, dStart,
		                                                               dStop,
		                                                               envMap_.buildingMark) ;
Thomas's avatar
Thomas committed
611

David Cazier's avatar
David Cazier committed
612
613
		for (std::vector<Dart>::iterator it = path.begin(); it != path.end(); ++it)
		{
614
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
615
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
616
617
		}

David Cazier's avatar
David Cazier committed
618
		Dart dStop2 = dStop ;
David Cazier's avatar
David Cazier committed
619
620
621
622
		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
623
624
			envMap_.map.next(dStop2) ;
			if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
Thomas's avatar
Thomas committed
625
626
		}

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

David Cazier's avatar
David Cazier committed
630
631
		for (std::vector<Dart>::iterator it = path.begin(); it != path.end(); ++it)
		{
632
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
633
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
634
635
		}

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

David Cazier's avatar
David Cazier committed
639
640
		for (std::vector<Dart>::iterator it = path.begin(); it != path.end(); ++it)
		{
641
			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
David Cazier's avatar
David Cazier committed
642
			agents_[i]->goals_.push_back(dest) ;
Thomas's avatar
Thomas committed
643
644
645
		}
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
646
#endif
Thomas's avatar
Thomas committed
647

Pierre Kraemer's avatar
Pierre Kraemer committed
648
649
bool Simulator::importAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
650
	std::ifstream myfile(filename.c_str(), std::ios::in) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
651

David Cazier's avatar
David Cazier committed
652
653
	if (!myfile.good())
	{
David Cazier's avatar
David Cazier committed
654
655
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
656
657
	}

David Cazier's avatar
David Cazier committed
658
659
660
	std::string line, token ;
	while (myfile.good())
	{
David Cazier's avatar
David Cazier committed
661
		getline(myfile, line) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
662

David Cazier's avatar
David Cazier committed
663
664
		if (line.size() > 1)
		{
David Cazier's avatar
David Cazier committed
665
			std::istringstream iss(line) ;
David Cazier's avatar
David Cazier committed
666
			float x, y, z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
667

David Cazier's avatar
David Cazier committed
668
669
670
			iss >> x ;
			iss >> y ;
			iss >> z ;
Pierre Kraemer's avatar
Pierre Kraemer committed
671

672
			VEC3 pos(x, y, z) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
673
674
675
676
677
#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) ;
678
			for(int ii = -1 ; ii <= 1 ; ++ii)
Pierre Kraemer's avatar
Pierre Kraemer committed
679
			{
680
				for(int jj = -1 ; jj <= 1 ; ++jj)
Pierre Kraemer's avatar
Pierre Kraemer committed
681
682
683
684
685
686
687
688
689
				{
					if(!(ii == 0 && jj == 0))
					{
						Geom::Vec2ui cc = c + Geom::Vec2ui(ii, jj) ;
						envMap_.ht_neighbor_agents[cc].push_back(a) ;
					}
				}
			}
#else
690
691
			Dart dCell = envMap_.getBelongingCell(pos) ;
			agents_.push_back(new Agent(this, pos, dCell)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
692
#endif
693
			agents_.back()->goals_.push_back(pos) ;
David Cazier's avatar
David Cazier committed
694
			agents_.back()->goals_.push_back(-1.0f * pos) ;
695
			agents_.back()->curGoal_ = 1 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
696
697
698
		}
	}

David Cazier's avatar
David Cazier committed
699
	swapAgentsGoals() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
700

Pierre Kraemer's avatar
Pierre Kraemer committed
701
#ifndef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
702
	for (unsigned int i = 0; i < agents_.size(); ++i)
703
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
704
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
705

David Cazier's avatar
David Cazier committed
706
707
	myfile.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
708
709
710
711
}

bool Simulator::exportAgents(std::string filename)
{
David Cazier's avatar
David Cazier committed
712
	std::ofstream out(filename.c_str(), std::ios::out) ;
David Cazier's avatar
David Cazier committed
713
714
	if (!out.good())
	{
David Cazier's avatar
David Cazier committed
715
716
		std::cerr << "(export) Unable to open file " << filename << std::endl ;
		return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
717
718
	}

David Cazier's avatar
David Cazier committed
719
	for (unsigned int i = 0; i < agents_.size(); ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
720
#ifdef SPATIAL_HASHING
721
		out << agents_[i]->pos << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
722
#else
723
		out << agents_[i]->part_.m_position << std::endl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
724
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
725

David Cazier's avatar
David Cazier committed
726
727
	out.close() ;
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
728
729
730
731
}

void Simulator::swapAgentsGoals()
{
David Cazier's avatar
David Cazier committed
732
	unsigned int nbAgents = agents_.size() ;
David Cazier's avatar
David Cazier committed
733
734
	for (unsigned int i = 0; i < nbAgents; ++i)
	{
David Cazier's avatar
David Cazier committed
735
		unsigned int r = rand() % nbAgents ;
Thomas's avatar
Thomas committed
736
737
738
739
740
741
742
//		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;
//		}

743
		std::swap(agents_[i]->goals_, agents_[r]->goals_) ;
Thomas's avatar
Thomas committed
744
745
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
746

Thomas's avatar
Thomas committed
747
748
Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
{
David Cazier's avatar
David Cazier committed
749
750
	Geom::BoundingBox<VEC3> bb ;
	unsigned int nbAgents = agents_.size() ;
David Cazier's avatar
David Cazier committed
751
752
	for (unsigned int i = 0; i < nbAgents; ++i)
	{
David Cazier's avatar
David Cazier committed
753
		bb.addPoint(agents_[i]->getPosition()) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
754
	}
Thomas's avatar
Thomas committed
755

David Cazier's avatar
David Cazier committed
756
	return bb ;
Pierre Kraemer's avatar
Pierre Kraemer committed
757
}
758

Pierre Kraemer's avatar
Pierre Kraemer committed
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
//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);
//}
778